mirror of https://github.com/ArduPilot/ardupilot
Copter: log GPS messages even when no fix
This commit is contained in:
parent
667c3ecbf8
commit
fe6ad3579c
|
@ -794,8 +794,6 @@ static uint32_t fast_loopTimer;
|
||||||
static uint16_t mainLoop_count;
|
static uint16_t mainLoop_count;
|
||||||
// Loiter timer - Records how long we have been in loiter
|
// Loiter timer - Records how long we have been in loiter
|
||||||
static uint32_t rtl_loiter_start_time;
|
static uint32_t rtl_loiter_start_time;
|
||||||
// prevents duplicate GPS messages from entering system
|
|
||||||
static uint32_t last_gps_time;
|
|
||||||
|
|
||||||
// Used to exit the roll and pitch auto trim function
|
// Used to exit the roll and pitch auto trim function
|
||||||
static uint8_t auto_trim_counter;
|
static uint8_t auto_trim_counter;
|
||||||
|
@ -1282,37 +1280,42 @@ static void update_optical_flow(void)
|
||||||
// called at 50hz
|
// called at 50hz
|
||||||
static void update_GPS(void)
|
static void update_GPS(void)
|
||||||
{
|
{
|
||||||
// A counter that is used to grab at least 10 reads before commiting the Home location
|
static uint32_t last_gps_reading; // time of last gps message
|
||||||
static uint8_t ground_start_count = 10;
|
static uint8_t ground_start_count = 10; // counter used to grab at least 10 reads before commiting the Home location
|
||||||
|
|
||||||
g_gps->update();
|
g_gps->update();
|
||||||
|
|
||||||
if (g_gps->new_data && last_gps_time != g_gps->last_fix_time && g_gps->status() >= GPS::GPS_OK_FIX_2D) {
|
// logging and glitch protection run after every gps message
|
||||||
// clear new data flag
|
if ((g_gps->last_message_time_ms() != last_gps_reading) && motors.armed()) {
|
||||||
g_gps->new_data = false;
|
last_gps_reading = g_gps->last_message_time_ms();
|
||||||
|
|
||||||
// save GPS time so we don't get duplicate reads
|
// log GPS message
|
||||||
last_gps_time = g_gps->last_fix_time;
|
if (g.log_bitmask & MASK_LOG_GPS) {
|
||||||
|
|
||||||
// log location if we have at least a 2D fix
|
|
||||||
if (g.log_bitmask & MASK_LOG_GPS && motors.armed()) {
|
|
||||||
DataFlash.Log_Write_GPS(g_gps, current_loc.alt);
|
DataFlash.Log_Write_GPS(g_gps, current_loc.alt);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// run glitch protection and update AP_Notify if home has been initialised
|
||||||
|
if (ap.home_is_set) {
|
||||||
|
gps_glitch.check_position();
|
||||||
|
if (AP_Notify::flags.gps_glitching != gps_glitch.glitching()) {
|
||||||
|
if (gps_glitch.glitching()) {
|
||||||
|
Log_Write_Error(ERROR_SUBSYSTEM_GPS, ERROR_CODE_GPS_GLITCH);
|
||||||
|
}else{
|
||||||
|
Log_Write_Error(ERROR_SUBSYSTEM_GPS, ERROR_CODE_ERROR_RESOLVED);
|
||||||
|
}
|
||||||
|
AP_Notify::flags.gps_glitching = gps_glitch.glitching();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// checks to initialise home and take location based pictures
|
||||||
|
if (g_gps->new_data && g_gps->status() >= GPS::GPS_OK_FIX_3D) {
|
||||||
|
// clear new data flag
|
||||||
|
g_gps->new_data = false;
|
||||||
|
|
||||||
// for performance monitoring
|
// for performance monitoring
|
||||||
gps_fix_count++;
|
gps_fix_count++;
|
||||||
|
|
||||||
// run glitch protection and update AP_Notify
|
|
||||||
gps_glitch.check_position();
|
|
||||||
if (AP_Notify::flags.gps_glitching != gps_glitch.glitching()) {
|
|
||||||
if (gps_glitch.glitching()) {
|
|
||||||
Log_Write_Error(ERROR_SUBSYSTEM_GPS, ERROR_CODE_GPS_GLITCH);
|
|
||||||
}else{
|
|
||||||
Log_Write_Error(ERROR_SUBSYSTEM_GPS, ERROR_CODE_ERROR_RESOLVED);
|
|
||||||
}
|
|
||||||
AP_Notify::flags.gps_glitching = gps_glitch.glitching();
|
|
||||||
}
|
|
||||||
|
|
||||||
// check if we can initialise home yet
|
// check if we can initialise home yet
|
||||||
if (!ap.home_is_set) {
|
if (!ap.home_is_set) {
|
||||||
// if we have a 3d lock and valid location
|
// if we have a 3d lock and valid location
|
||||||
|
|
Loading…
Reference in New Issue