From 936dca75314e63d02debb78260d2e3c6770a0156 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 2 Aug 2018 12:19:20 +0900 Subject: [PATCH] Rover: remove compass accumulate --- APMrover2/APMrover2.cpp | 4 +++- APMrover2/Rover.h | 2 +- APMrover2/sensors.cpp | 11 ++--------- 3 files changed, 6 insertions(+), 11 deletions(-) diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp index 790a977205..ece8f9f75c 100644 --- a/APMrover2/APMrover2.cpp +++ b/APMrover2/APMrover2.cpp @@ -74,7 +74,6 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = { #endif SCHED_TASK(gcs_failsafe_check, 10, 200), SCHED_TASK(fence_check, 10, 200), - SCHED_TASK(compass_accumulate, 50, 200), SCHED_TASK_CLASS(ModeSmartRTL, &rover.mode_smartrtl, save_position, 3, 200), SCHED_TASK_CLASS(AP_Notify, &rover.notify, update, 50, 300), SCHED_TASK(one_second_loop, 1, 1500), @@ -291,6 +290,9 @@ void Rover::one_second_loop(void) update_home(); } + // init compass location for declination + init_compass_location(); + // update error mask of sensors and subsystems. The mask uses the // MAV_SYS_STATUS_* values from mavlink. If a bit is set then it // indicates that the sensor or subsystem is present but not diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index e179359f56..6b8786f76f 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -492,7 +492,7 @@ private: // sensors.cpp void init_compass(void); - void compass_accumulate(void); + void init_compass_location(void); void init_beacon(); void init_visual_odom(); void update_visual_odom(); diff --git a/APMrover2/sensors.cpp b/APMrover2/sensors.cpp index 9ac8b47435..fc953f1ac5 100644 --- a/APMrover2/sensors.cpp +++ b/APMrover2/sensors.cpp @@ -18,17 +18,10 @@ void Rover::init_compass() } /* - if the compass is enabled then try to accumulate a reading - also update initial location used for declination + initialise compass's location used for declination */ -void Rover::compass_accumulate(void) +void Rover::init_compass_location(void) { - if (!g.compass_enabled) { - return; - } - - compass.accumulate(); - // update initial location used for declination if (!compass_init_location) { Location loc;