Merge pull request #473 from flixr/rotorcraft_startup_mode

[rotorcraft] fix MODE_STARTUP

sw/airborne/firmwares/rotorcraft/autopilot.c

@@ -60,6 +60,7 @@ static inline int ahrs_is_aligned(void) {
   return (ahrs.status == AHRS_RUNNING);
 }
 #else
+PRINT_CONFIG_MSG("Using AUTOPILOT_DISABLE_AHRS_KILL")
 static inline int ahrs_is_aligned(void) {
   return TRUE;
 }
@@ -75,11 +76,12 @@ static inline int ahrs_is_aligned(void) {
 
 #ifndef MODE_STARTUP
 #define MODE_STARTUP AP_MODE_KILL
-PRINT_CONFIG_MSG("Using AP_MODE_KILL as MODE_STARTUP")
+PRINT_CONFIG_MSG("Using default AP_MODE_KILL as MODE_STARTUP")
 #endif
 
 void autopilot_init(void) {
-  autopilot_mode = MODE_STARTUP;
+  /* mode is finally set at end of init if MODE_STARTUP is not KILL */
+  autopilot_mode = AP_MODE_KILL;
   autopilot_motors_on = FALSE;
   kill_throttle = ! autopilot_motors_on;
   autopilot_in_flight = FALSE;
@@ -93,7 +95,16 @@ void autopilot_init(void) {
 #ifdef POWER_SWITCH_LED
   LED_ON(POWER_SWITCH_LED); // POWER OFF
 #endif
+
   autopilot_arming_init();
+
+  nav_init();
+  guidance_h_init();
+  guidance_v_init();
+  stabilization_init();
+
+  /* set startup mode, propagats through to guidance h/v */
+  autopilot_set_mode(MODE_STARTUP);
 }
 
 

sw/airborne/firmwares/rotorcraft/main.c

@@ -138,11 +138,6 @@ STATIC_INLINE void main_init( void ) {
 
   baro_init();
   imu_init();
-  autopilot_init();
-  nav_init();
-  guidance_h_init();
-  guidance_v_init();
-  stabilization_init();
 
   ahrs_aligner_init();
   ahrs_init();
@@ -153,6 +148,8 @@ STATIC_INLINE void main_init( void ) {
   gps_init();
 #endif
 
+  autopilot_init();
+
   modules_init();
 
   settings_init();
@@ -225,6 +222,7 @@ STATIC_INLINE void failsafe_check( void ) {
 
 #if USE_GPS
   if (autopilot_mode == AP_MODE_NAV &&
+      autopilot_motors_on &&
 #if NO_GPS_LOST_WITH_RC_VALID
       radio_control.status != RC_OK &&
 #endif