AP_Notify: Add GCS failsafe notify tones and lights

This commit is contained in:
Matt Lawrence 2019-10-31 18:10:36 -04:00 committed by Randy Mackay
parent b9ff846162
commit 3af99115fb
8 changed files with 41 additions and 2 deletions

View File

@ -171,7 +171,7 @@ void AP_BoardLED2::update(void)
if ((counter2 & 0x7) == 0) { if ((counter2 & 0x7) == 0) {
hal.gpio->toggle(HAL_GPIO_A_LED_PIN); hal.gpio->toggle(HAL_GPIO_A_LED_PIN);
} }
}else if(AP_Notify::flags.failsafe_radio){// blink fast (around 4Hz) }else if(AP_Notify::flags.failsafe_radio || AP_Notify::flags.failsafe_gcs){// blink fast (around 4Hz)
if ((counter2 & 0x3) == 0) { if ((counter2 & 0x3) == 0) {
hal.gpio->toggle(HAL_GPIO_A_LED_PIN); hal.gpio->toggle(HAL_GPIO_A_LED_PIN);
} }

View File

@ -87,6 +87,7 @@ public:
bool esc_calibration; // true if calibrating escs bool esc_calibration; // true if calibrating escs
bool failsafe_radio; // true if radio failsafe bool failsafe_radio; // true if radio failsafe
bool failsafe_battery; // true if battery failsafe bool failsafe_battery; // true if battery failsafe
bool failsafe_gcs; // true if GCS failsafe
bool parachute_release; // true if parachute is being released bool parachute_release; // true if parachute is being released
bool ekf_bad; // true if ekf is reporting problems bool ekf_bad; // true if ekf is reporting problems
bool autopilot_mode; // true if vehicle is in an autopilot flight mode (only used by OreoLEDs) bool autopilot_mode; // true if vehicle is in an autopilot flight mode (only used by OreoLEDs)

View File

@ -185,7 +185,7 @@ void ExternalLED::update(void)
break; break;
} }
}else{ }else{
if (AP_Notify::flags.failsafe_battery || AP_Notify::flags.failsafe_radio) { if (AP_Notify::flags.failsafe_battery || AP_Notify::flags.failsafe_radio || AP_Notify::flags.failsafe_gcs) {
// radio or battery failsafe indicated by fast flashing // radio or battery failsafe indicated by fast flashing
set_pattern(FAST_FLASH); set_pattern(FAST_FLASH);
} else { } else {

View File

@ -87,6 +87,10 @@ void OreoLED_I2C::update()
return; // don't go any further if the Pixhawk is is in radio failsafe return; // don't go any further if the Pixhawk is is in radio failsafe
} }
if (mode_failsafe_gcs()) {
return; // don't go any further if the Pixhawk is is in gcs failsafe
}
set_standard_colors(); set_standard_colors();
if (mode_failsafe_batt()) { if (mode_failsafe_batt()) {
@ -159,6 +163,20 @@ bool OreoLED_I2C::mode_failsafe_radio()
} }
// Procedure for when Pixhawk is in GCS failsafe
// LEDs perform alternating yellow X pattern
bool OreoLED_I2C::mode_failsafe_gcs()
{
if (AP_Notify::flags.failsafe_gcs) {
set_rgb(OREOLED_FRONTLEFT, OREOLED_PATTERN_STROBE, 255, 50, 0,0,0,0,PERIOD_SLOW,0);
set_rgb(OREOLED_FRONTRIGHT, OREOLED_PATTERN_STROBE, 255, 50, 0,0,0,0,PERIOD_SLOW,PO_ALTERNATE);
set_rgb(OREOLED_BACKLEFT, OREOLED_PATTERN_STROBE, 255, 50, 0,0,0,0,PERIOD_SLOW,PO_ALTERNATE);
set_rgb(OREOLED_BACKRIGHT, OREOLED_PATTERN_STROBE, 255, 50, 0,0,0,0,PERIOD_SLOW,0);
}
return AP_Notify::flags.failsafe_gcs;
}
// Procedure to set standard rear LED colors in aviation theme // Procedure to set standard rear LED colors in aviation theme
// Back LEDS White for normal, yellow for GPS not usable, purple for EKF bad] // Back LEDS White for normal, yellow for GPS not usable, purple for EKF bad]
// Returns true GPS or EKF problem, returns false if all ok // Returns true GPS or EKF problem, returns false if all ok

View File

@ -113,6 +113,7 @@ private:
bool mode_firmware_update(void); bool mode_firmware_update(void);
bool mode_init(void); bool mode_init(void);
bool mode_failsafe_radio(void); bool mode_failsafe_radio(void);
bool mode_failsafe_gcs(void);
bool set_standard_colors(void); bool set_standard_colors(void);
bool mode_failsafe_batt(void); bool mode_failsafe_batt(void);
bool mode_auto_flight(void); bool mode_auto_flight(void);

View File

@ -118,6 +118,7 @@ uint32_t RGBLed::get_colour_sequence(void) const
// gps failsafe pattern : flashing yellow and blue // gps failsafe pattern : flashing yellow and blue
// ekf_bad pattern : flashing yellow and red // ekf_bad pattern : flashing yellow and red
if (AP_Notify::flags.failsafe_radio || if (AP_Notify::flags.failsafe_radio ||
AP_Notify::flags.failsafe_gcs ||
AP_Notify::flags.failsafe_battery || AP_Notify::flags.failsafe_battery ||
AP_Notify::flags.ekf_bad || AP_Notify::flags.ekf_bad ||
AP_Notify::flags.gps_glitching || AP_Notify::flags.gps_glitching ||

View File

@ -299,6 +299,23 @@ void AP_ToneAlarm::update()
} }
} }
// notify the user when GCS failsafe is triggered
if (flags.failsafe_gcs != AP_Notify::flags.failsafe_gcs) {
flags.failsafe_gcs = AP_Notify::flags.failsafe_gcs;
if (flags.failsafe_gcs) {
// armed case handled by events.failsafe_mode_change
if (!AP_Notify::flags.armed) {
play_tone(AP_NOTIFY_TONE_QUIET_NEG_FEEDBACK);
}
} else {
if (AP_Notify::flags.armed) {
play_tone(AP_NOTIFY_TONE_LOUD_POS_FEEDBACK);
} else {
play_tone(AP_NOTIFY_TONE_QUIET_POS_FEEDBACK);
}
}
}
// notify the user when pre_arm checks are passing // notify the user when pre_arm checks are passing
if (flags.pre_arm_check != AP_Notify::flags.pre_arm_check) { if (flags.pre_arm_check != AP_Notify::flags.pre_arm_check) {
flags.pre_arm_check = AP_Notify::flags.pre_arm_check; flags.pre_arm_check = AP_Notify::flags.pre_arm_check;

View File

@ -58,6 +58,7 @@ private:
uint16_t parachute_release : 1; // 1 if parachute is being released uint16_t parachute_release : 1; // 1 if parachute is being released
uint16_t pre_arm_check : 1; // 0 = failing checks, 1 = passed uint16_t pre_arm_check : 1; // 0 = failing checks, 1 = passed
uint16_t failsafe_radio : 1; // 1 if radio failsafe uint16_t failsafe_radio : 1; // 1 if radio failsafe
uint16_t failsafe_gcs : 1; // 1 if gcs failsafe
uint16_t vehicle_lost : 1; // 1 if lost copter tone requested uint16_t vehicle_lost : 1; // 1 if lost copter tone requested
uint16_t compass_cal_running : 1; // 1 if compass calibration is running uint16_t compass_cal_running : 1; // 1 if compass calibration is running
uint16_t waiting_for_throw : 1; // 1 if waiting for copter throw launch uint16_t waiting_for_throw : 1; // 1 if waiting for copter throw launch