Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Notify: add EKF failure warning

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 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 ekf_bad : 1; // 1 if ekf is reporting problems
// additional flags
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);
}
}
// 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

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 arming_failed : 1; // 0 = failing checks, 1 = passed
uint8_t parachute_release : 1; // 1 if parachute is being released
uint8_t ekf_bad : 1; // 1 if ekf position has gone bad
} flags;
};

25
libraries/AP_Notify/ToshibaLED.cpp
View File

@ -132,9 +132,12 @@ void ToshibaLED::update_colours(void)
return;
}
// failsafe patterns for radio and battery - single flash yellow
// failsafe pattern for gps - flashing blue and yellow
if (AP_Notify::flags.failsafe_radio || AP_Notify::flags.failsafe_battery || AP_Notify::flags.failsafe_gps || AP_Notify::flags.gps_glitching) {
// radio and battery failsafe patter: flash yellow
// gps failsafe pattern : flashing yellow and blue
// 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) {
case 0:
case 1:
@ -151,14 +154,22 @@ void ToshibaLED::update_colours(void)
case 7:
case 8:
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) {
// blue on for gps failsafe or glitching
_red_des = TOSHIBA_LED_OFF;
_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;
_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;
}
// exit so no other status modify this pattern