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

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
This commit is contained in:
Peter Barker 2024-06-25 15:18:58 +10:00 committed by Andrew Tridgell
parent 3439ced236
commit 2425023331
2 changed files with 29 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
libraries/AP_Notify/RGBLed.cpp
View File

@ -20,6 +20,7 @@
#include <AP_GPS/AP_GPS.h> #include <AP_GPS/AP_GPS.h>
#include "RGBLed.h" #include "RGBLed.h"
#include "AP_Notify.h" #include "AP_Notify.h"
#include <AP_AHRS/AP_AHRS.h>
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
@ -136,33 +137,48 @@ uint32_t RGBLed::get_colour_sequence(void) const
return sequence_failsafe_radio_or_battery; 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 // solid green or blue if armed
if (AP_Notify::flags.armed) { if (AP_Notify::flags.armed) {
#if AP_GPS_ENABLED #if AP_GPS_ENABLED
// solid green if armed with GPS 3d lock // solid green if armed with GPS 3d lock and good location
if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D) { if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D &&
good_ahrs_location) {
return sequence_armed; return sequence_armed;
} }
#endif #endif
// solid blue if armed with no GPS lock // 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 // double flash yellow if failing pre-arm checks
if (!AP_Notify::flags.pre_arm_check) { if (!AP_Notify::flags.pre_arm_check) {
return sequence_prearm_failing; return sequence_prearm_failing;
} }
#if AP_GPS_ENABLED if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS &&
if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS && AP_Notify::flags.pre_arm_gps_check) { AP_Notify::flags.pre_arm_gps_check &&
return sequence_disarmed_good_dgps; 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) { if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D &&
return sequence_disarmed_good_gps; AP_Notify::flags.pre_arm_gps_check &&
good_ahrs_location) {
return sequence_disarmed_good_gps_and_location;
} }
#endif #endif
return sequence_disarmed_bad_gps; return sequence_disarmed_bad_gps_or_no_location;
} }
uint32_t RGBLed::get_colour_sequence_traffic_light(void) const uint32_t RGBLed::get_colour_sequence_traffic_light(void) const

8
libraries/AP_Notify/RGBLed.h
View File

@ -98,11 +98,11 @@ private:
const uint32_t sequence_failsafe_radio_or_battery = DEFINE_COLOUR_SEQUENCE_FAILSAFE(BLACK); 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 = 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_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_dgps_and_location = DEFINE_COLOUR_SEQUENCE_ALTERNATE(GREEN,BLACK);
const uint32_t sequence_disarmed_good_gps = DEFINE_COLOUR_SEQUENCE_SLOW(GREEN); const uint32_t sequence_disarmed_good_gps_and_location = DEFINE_COLOUR_SEQUENCE_SLOW(GREEN);
const uint32_t sequence_disarmed_bad_gps = DEFINE_COLOUR_SEQUENCE_SLOW(BLUE); const uint32_t sequence_disarmed_bad_gps_or_no_location = DEFINE_COLOUR_SEQUENCE_SLOW(BLUE);
uint8_t last_step; uint8_t last_step;
enum class Source { enum class Source {