Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-05 23:49:00 +08:00
Code Issues Actions 24 Packages Projects Releases Wiki Activity

[rotorcraft] fix MODE_STARTUP

Browse Source 
- init nav, guidance and stabilization from autopilot_init
- after init of the above, set the autopilot_mode so it propagates to the guidance submodes.
- also call autopilot_init after the ahrs_init for ahrs_is_aligned check...
This commit is contained in:
Felix Ruess
2013-07-12 00:05:42 +02:00
parent d17f8f4669
commit 30fb6211eb
2 changed files with 15 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/autopilot.c
+13 -2
View File
@@ -60,6 +60,7 @@ static inline int ahrs_is_aligned(void) {
return (ahrs.status == AHRS_RUNNING); return (ahrs.status == AHRS_RUNNING);
} }
#else #else
PRINT_CONFIG_MSG("Using AUTOPILOT_DISABLE_AHRS_KILL")
static inline int ahrs_is_aligned(void) { static inline int ahrs_is_aligned(void) {
return TRUE; return TRUE;
} }
@@ -75,11 +76,12 @@ static inline int ahrs_is_aligned(void) {
#ifndef MODE_STARTUP #ifndef MODE_STARTUP
#define MODE_STARTUP AP_MODE_KILL #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 #endif
void autopilot_init(void) { 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; autopilot_motors_on = FALSE;
kill_throttle = ! autopilot_motors_on; kill_throttle = ! autopilot_motors_on;
autopilot_in_flight = FALSE; autopilot_in_flight = FALSE;
@@ -93,7 +95,16 @@ void autopilot_init(void) {
#ifdef POWER_SWITCH_LED #ifdef POWER_SWITCH_LED
LED_ON(POWER_SWITCH_LED); // POWER OFF LED_ON(POWER_SWITCH_LED); // POWER OFF
#endif #endif
autopilot_arming_init(); 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
+2 -5
View File
@@ -138,11 +138,6 @@ STATIC_INLINE void main_init( void ) {
baro_init(); baro_init();
imu_init(); imu_init();
autopilot_init();
nav_init();
guidance_h_init();
guidance_v_init();
stabilization_init();
ahrs_aligner_init(); ahrs_aligner_init();
ahrs_init(); ahrs_init();
@@ -153,6 +148,8 @@ STATIC_INLINE void main_init( void ) {
gps_init(); gps_init();
#endif #endif
autopilot_init();
modules_init(); modules_init();
settings_init(); settings_init();