Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Notify: add EKF failure warning

Browse Source
This commit is contained in:
Randy Mackay 2014-07-21 19:12:53 +09:00
parent 7bda6cbadf
commit f24960f4c0
4 changed files with 29 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AP_Notify/AP_Notify.h
View File

@ -44,6 +44,7 @@ public:
uint16_t failsafe_gps : 1; // 1 if gps failsafe uint16_t failsafe_gps : 1; // 1 if gps failsafe
uint16_t arming_failed : 1; // 1 if copter failed to arm after user input uint16_t arming_failed : 1; // 1 if copter failed to arm after user input
uint16_t parachute_release : 1; // 1 if parachute is being released uint16_t parachute_release : 1; // 1 if parachute is being released
uint16_t ekf_bad : 1; // 1 if ekf is reporting problems
// additional flags // additional flags
uint16_t external_leds : 1; // 1 if external LEDs are enabled (normally only used for copter) uint16_t external_leds : 1; // 1 if external LEDs are enabled (normally only used for copter)

9
libraries/AP_Notify/ToneAlarm_PX4.cpp
View File

@ -120,6 +120,15 @@ void ToneAlarm_PX4::update()
play_tune(TONE_PARACHUTE_RELEASE_TUNE); play_tune(TONE_PARACHUTE_RELEASE_TUNE);
} }
} }
// check ekf has gone bad
if (flags.ekf_bad != AP_Notify::flags.ekf_bad) {
flags.ekf_bad = AP_Notify::flags.ekf_bad;
if (flags.ekf_bad) {
// ekf warning tune
play_tune(TONE_EKF_WARNING_TUNE);
}
}
} }
#endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4 #endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4

1
libraries/AP_Notify/ToneAlarm_PX4.h
View File

@ -43,6 +43,7 @@ private:
uint8_t failsafe_gps : 1; // 1 if gps failsafe uint8_t failsafe_gps : 1; // 1 if gps failsafe
uint8_t arming_failed : 1; // 0 = failing checks, 1 = passed uint8_t arming_failed : 1; // 0 = failing checks, 1 = passed
uint8_t parachute_release : 1; // 1 if parachute is being released uint8_t parachute_release : 1; // 1 if parachute is being released
uint8_t ekf_bad : 1; // 1 if ekf position has gone bad
} flags; } flags;
}; };

25
libraries/AP_Notify/ToshibaLED.cpp
View File

@ -132,9 +132,12 @@ void ToshibaLED::update_colours(void)
return; return;
} }
// failsafe patterns for radio and battery - single flash yellow // radio and battery failsafe patter: flash yellow
// failsafe pattern for gps - flashing blue and yellow // gps failsafe pattern : flashing yellow and blue
if (AP_Notify::flags.failsafe_radio || AP_Notify::flags.failsafe_battery || AP_Notify::flags.failsafe_gps || AP_Notify::flags.gps_glitching) { // ekf_bad pattern : flashing yellow and red
if (AP_Notify::flags.failsafe_radio || AP_Notify::flags.failsafe_battery ||
AP_Notify::flags.failsafe_gps || AP_Notify::flags.gps_glitching ||
AP_Notify::flags.ekf_bad) {
switch(step) { switch(step) {
case 0: case 0:
case 1: case 1:
@ -151,14 +154,22 @@ void ToshibaLED::update_colours(void)
case 7: case 7:
case 8: case 8:
case 9: case 9:
// all off of radio or battery, blue on for gps
_red_des = TOSHIBA_LED_OFF;
if (AP_Notify::flags.failsafe_gps || AP_Notify::flags.gps_glitching) { if (AP_Notify::flags.failsafe_gps || AP_Notify::flags.gps_glitching) {
// blue on for gps failsafe or glitching
_red_des = TOSHIBA_LED_OFF;
_blue_des = brightness; _blue_des = brightness;
}else{ _green_des = TOSHIBA_LED_OFF;
} else if (AP_Notify::flags.ekf_bad) {
// red on if ekf bad
_red_des = brightness;
_blue_des = TOSHIBA_LED_OFF; _blue_des = TOSHIBA_LED_OFF;
_green_des = TOSHIBA_LED_OFF;
}else{
// all off for radio or battery failsafe
_red_des = TOSHIBA_LED_OFF;
_blue_des = TOSHIBA_LED_OFF;
_green_des = TOSHIBA_LED_OFF;
} }
_green_des = TOSHIBA_LED_OFF;
break; break;
} }
// exit so no other status modify this pattern // exit so no other status modify this pattern