#include <ao_log.h>
#endif
+#if HAS_MPU6000
+#include <ao_quaternion.h>
+#endif
+
#ifndef HAS_ACCEL
#error Please define HAS_ACCEL
#endif
if (ao_config.accel_plus_g == 0 ||
ao_config.accel_minus_g == 0 ||
ao_ground_accel < ao_config.accel_plus_g - ACCEL_NOSE_UP ||
- ao_ground_accel > ao_config.accel_minus_g + ACCEL_NOSE_UP)
+ ao_ground_accel > ao_config.accel_minus_g + ACCEL_NOSE_UP ||
+ ao_ground_height < -1000 ||
+ ao_ground_height > 7000)
{
/* Detected an accel value outside -1.5g to 1.5g
* (or uncalibrated values), so we go into invalid mode
{
/* Set pad mode - we can fly! */
ao_flight_state = ao_flight_pad;
-#if HAS_USB && HAS_RADIO && !HAS_FLIGHT_DEBUG && !HAS_SAMPLE_PROFILE
+#if HAS_USB && !HAS_FLIGHT_DEBUG && !HAS_SAMPLE_PROFILE
/* Disable the USB controller in flight mode
* to save power
*/
ao_usb_disable();
#endif
-#if !HAS_ACCEL
+#if !HAS_ACCEL && PACKET_HAS_SLAVE
/* Disable packet mode in pad state on TeleMini */
ao_packet_slave_stop();
#endif
ao_rdf_set(1);
ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_PAD);
#endif
+#if HAS_LED
/* signal successful initialization by turning off the LED */
ao_led_off(AO_LED_RED);
+#endif
} else {
/* Set idle mode */
ao_flight_state = ao_flight_idle;
ao_packet_slave_start();
#endif
+#if HAS_LED
/* signal successful initialization by turning off the LED */
ao_led_off(AO_LED_RED);
+#endif
}
/* wakeup threads due to state change */
ao_wakeup(DATA_TO_XDATA(&ao_flight_state));
#if HAS_GPS
/* Record current GPS position by waking up GPS log tasks */
- ao_wakeup(&ao_gps_data);
- ao_wakeup(&ao_gps_tracking_data);
+ ao_gps_new = AO_GPS_NEW_DATA | AO_GPS_NEW_TRACKING;
+ ao_wakeup(&ao_gps_new);
#endif
ao_wakeup(DATA_TO_XDATA(&ao_flight_state));