From 48706f44cfb4b92588883bb6541714d9c124ccee Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 25 Jun 2024 15:18:58 +1000 Subject: [PATCH] AP_Notify: flash green lights based off location not GPS Your Copter in stabilize mode can flash rapid-green indicating a good DGPS lock or better, and yet your vehicle still doesn't have a good idea of where it is. That means that your vehicle may end up RTL'ing onto a point somewhere along your flight path, when you were given an all-green indication by teh vehicle before you armed in stabilize. Past this PR, we require the same GPS as before, but in addition we must have been happy enough with our location to set the navigation origin, *and* currently be able to get a location. A user will receive slow-flashing blue lights if they can't currently get a location, or the navigation origin isn't set, even if they've got a "good" fix. We also require a good location to get a solid green light - you will get a solid blue light if you can't get a location or don't have a navigation origin, even if you have a good GPS lock --- libraries/AP_Notify/RGBLed.cpp | 34 +++++++++++++++++++++++++--------- libraries/AP_Notify/RGBLed.h | 8 ++++---- 2 files changed, 29 insertions(+), 13 deletions(-) diff --git a/libraries/AP_Notify/RGBLed.cpp b/libraries/AP_Notify/RGBLed.cpp index 29d15ba043c10..944fb1d0e82ac 100644 --- a/libraries/AP_Notify/RGBLed.cpp +++ b/libraries/AP_Notify/RGBLed.cpp @@ -20,6 +20,7 @@ #include #include "RGBLed.h" #include "AP_Notify.h" +#include extern const AP_HAL::HAL& hal; @@ -136,33 +137,48 @@ uint32_t RGBLed::get_colour_sequence(void) const return sequence_failsafe_radio_or_battery; } +#if AP_GPS_ENABLED + Location loc; +#if AP_AHRS_ENABLED + // the AHRS can return "true" for get_location and still not be + // happy enough with the location to set its origin from that + // location: + const bool good_ahrs_location = AP::ahrs().get_location(loc) && AP::ahrs().get_origin(loc); +#else + const bool good_ahrs_location = true; +#endif + // solid green or blue if armed if (AP_Notify::flags.armed) { #if AP_GPS_ENABLED - // solid green if armed with GPS 3d lock - if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D) { + // solid green if armed with GPS 3d lock and good location + if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D && + good_ahrs_location) { return sequence_armed; } #endif // solid blue if armed with no GPS lock - return sequence_armed_nogps; + return sequence_armed_no_gps_or_no_location; } // double flash yellow if failing pre-arm checks if (!AP_Notify::flags.pre_arm_check) { return sequence_prearm_failing; } -#if AP_GPS_ENABLED - if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS && AP_Notify::flags.pre_arm_gps_check) { - return sequence_disarmed_good_dgps; + if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS && + AP_Notify::flags.pre_arm_gps_check && + good_ahrs_location) { + return sequence_disarmed_good_dgps_and_location; } - if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D && AP_Notify::flags.pre_arm_gps_check) { - return sequence_disarmed_good_gps; + if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D && + AP_Notify::flags.pre_arm_gps_check && + good_ahrs_location) { + return sequence_disarmed_good_gps_and_location; } #endif - return sequence_disarmed_bad_gps; + return sequence_disarmed_bad_gps_or_no_location; } uint32_t RGBLed::get_colour_sequence_traffic_light(void) const diff --git a/libraries/AP_Notify/RGBLed.h b/libraries/AP_Notify/RGBLed.h index 7366a6e2b12d7..3978f04806c67 100644 --- a/libraries/AP_Notify/RGBLed.h +++ b/libraries/AP_Notify/RGBLed.h @@ -98,11 +98,11 @@ class RGBLed: public NotifyDevice { const uint32_t sequence_failsafe_radio_or_battery = DEFINE_COLOUR_SEQUENCE_FAILSAFE(BLACK); const uint32_t sequence_armed = DEFINE_COLOUR_SEQUENCE_SOLID(GREEN); - const uint32_t sequence_armed_nogps = DEFINE_COLOUR_SEQUENCE_SOLID(BLUE); + const uint32_t sequence_armed_no_gps_or_no_location = DEFINE_COLOUR_SEQUENCE_SOLID(BLUE); const uint32_t sequence_prearm_failing = DEFINE_COLOUR_SEQUENCE(YELLOW,YELLOW,BLACK,BLACK,YELLOW,YELLOW,BLACK,BLACK,BLACK,BLACK); - const uint32_t sequence_disarmed_good_dgps = DEFINE_COLOUR_SEQUENCE_ALTERNATE(GREEN,BLACK); - const uint32_t sequence_disarmed_good_gps = DEFINE_COLOUR_SEQUENCE_SLOW(GREEN); - const uint32_t sequence_disarmed_bad_gps = DEFINE_COLOUR_SEQUENCE_SLOW(BLUE); + const uint32_t sequence_disarmed_good_dgps_and_location = DEFINE_COLOUR_SEQUENCE_ALTERNATE(GREEN,BLACK); + const uint32_t sequence_disarmed_good_gps_and_location = DEFINE_COLOUR_SEQUENCE_SLOW(GREEN); + const uint32_t sequence_disarmed_bad_gps_or_no_location = DEFINE_COLOUR_SEQUENCE_SLOW(BLUE); uint8_t last_step; enum class Source {