aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: break up GPS and logging, allow GPS update at 50Hz

Browse Source 
this prevents mismatches in GPS message arrival and accel message
arrival from causing small DCM errors
This commit is contained in:
Andrew Tridgell 2013-12-28 11:57:50 +11:00
parent a1d5c0a02a
commit 6856cc6e4b
1 changed files with 38 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
ArduPlane/ArduPlane.pde
View File

@ -730,8 +730,9 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
{ set_servos, 1, 1600 },
{ read_control_switch, 7, 1000 },
{ gcs_retry_deferred, 1, 1000 },
{ update_GPS, 5, 3700 },
{ navigate, 5, 3000 }, // 10
{ update_GPS_50Hz, 1, 2500 },
{ update_GPS_10Hz, 5, 2500 }, // 10
{ navigate, 5, 3000 },
{ update_compass, 5, 1200 },
{ read_airspeed, 5, 1200 },
{ update_alt, 5, 3400 },
@ -740,8 +741,8 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
{ obc_fs_check, 5, 1000 },
{ gcs_update, 1, 1700 },
{ gcs_data_stream_send, 1, 3000 },
{ update_events, 15, 1500 },
{ check_usb_mux, 5, 300 }, // 20
{ update_events, 15, 1500 }, // 20
{ check_usb_mux, 5, 300 },
{ read_battery, 5, 1000 },
{ compass_accumulate, 1, 1500 },
{ barometer_accumulate, 1, 900 },
@ -750,11 +751,12 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
{ one_second_loop, 50, 1000 },
{ check_long_failsafe, 15, 1000 },
{ read_receiver_rssi, 5, 1000 },
{ airspeed_ratio_update, 50, 1000 },
{ update_mount, 1, 1500 }, // 30
{ airspeed_ratio_update, 50, 1000 }, // 30
{ update_mount, 1, 1500 },
{ log_perf_info, 500, 1000 },
{ compass_save, 3000, 2500 },
{ update_logging, 5, 1200 },
{ update_logging1, 5, 1700 },
{ update_logging2, 5, 1700 },
};
// setup the var_info table
@ -908,7 +910,28 @@ static void barometer_accumulate(void)
/*
do 10Hz logging
*/
static void update_logging(void)
static void update_logging1(void)
{
if ((g.log_bitmask & MASK_LOG_ATTITUDE_MED) && !(g.log_bitmask & MASK_LOG_ATTITUDE_FAST))
Log_Write_Attitude();
if ((g.log_bitmask & MASK_LOG_ATTITUDE_MED) && !(g.log_bitmask & MASK_LOG_IMU))
Log_Write_IMU();
if (g.log_bitmask & MASK_LOG_CTUN)
Log_Write_Control_Tuning();
if (g.log_bitmask & MASK_LOG_NTUN)
Log_Write_Nav_Tuning();
if (g.log_bitmask & MASK_LOG_RC)
Log_Write_RC();
}
/*
do 10Hz logging - part2
*/
static void update_logging2(void)
{
if ((g.log_bitmask & MASK_LOG_ATTITUDE_MED) && !(g.log_bitmask & MASK_LOG_ATTITUDE_FAST))
Log_Write_Attitude();
@ -1028,18 +1051,23 @@ static void airspeed_ratio_update(void)
/*
read the GPS and update position
*/
static void update_GPS(void)
static void update_GPS_50Hz(void)
{
static uint32_t last_gps_reading;
g_gps->update();
if (g_gps->last_message_time_ms() != last_gps_reading) {
last_gps_reading = g_gps->last_message_time_ms();
if (g.log_bitmask & MASK_LOG_GPS) {
Log_Write_GPS();
}
}
}
/*
read update GPS position - 10Hz update
*/
static void update_GPS_10Hz(void)
{
// get position from AHRS
have_position = ahrs.get_projected_position(current_loc);