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

Copter: log GPS messages even when no fix

This commit is contained in:
Randy Mackay 2013-11-22 11:51:48 +09:00
parent 667c3ecbf8
commit fe6ad3579c
1 changed files with 26 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
ArduCopter/ArduCopter.pde
View File

@ -794,8 +794,6 @@ static uint32_t fast_loopTimer;
static uint16_t mainLoop_count;
// Loiter timer - Records how long we have been in loiter
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
static uint8_t auto_trim_counter;
@ -1282,37 +1280,42 @@ static void update_optical_flow(void)
// called at 50hz
static void update_GPS(void)
{
// A counter that is used to grab at least 10 reads before commiting the Home location
static uint8_t ground_start_count = 10;
static uint32_t last_gps_reading; // time of last gps message
static uint8_t ground_start_count = 10; // counter used to grab at least 10 reads before commiting the Home location
g_gps->update();
if (g_gps->new_data && last_gps_time != g_gps->last_fix_time && g_gps->status() >= GPS::GPS_OK_FIX_2D) {
// clear new data flag
g_gps->new_data = false;
// logging and glitch protection run after every gps message
if ((g_gps->last_message_time_ms() != last_gps_reading) && motors.armed()) {
last_gps_reading = g_gps->last_message_time_ms();
// save GPS time so we don't get duplicate reads
last_gps_time = g_gps->last_fix_time;
// log location if we have at least a 2D fix
if (g.log_bitmask & MASK_LOG_GPS && motors.armed()) {
// log GPS message
if (g.log_bitmask & MASK_LOG_GPS) {
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
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
if (!ap.home_is_set) {
// if we have a 3d lock and valid location