From 54df7ad88d0ad6b805738f4a8302603cdbe09582 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 2 Aug 2018 12:20:17 +0900 Subject: [PATCH] Sub: remove compass accumulate --- ArduSub/ArduSub.cpp | 4 +++- ArduSub/Sub.h | 2 +- ArduSub/sensors.cpp | 11 ++--------- 3 files changed, 6 insertions(+), 11 deletions(-) diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index c8fcb8427f3a5..431da775360ce 100644 --- a/ArduSub/ArduSub.cpp +++ b/ArduSub/ArduSub.cpp @@ -35,7 +35,6 @@ const AP_Scheduler::Task Sub::scheduler_tasks[] = { SCHED_TASK(update_altitude, 10, 100), SCHED_TASK(three_hz_loop, 3, 75), SCHED_TASK(update_turn_counter, 10, 50), - SCHED_TASK(compass_accumulate, 100, 100), SCHED_TASK_CLASS(AP_Baro, &sub.barometer, accumulate, 50, 90), SCHED_TASK_CLASS(AP_Notify, &sub.notify, update, 50, 90), SCHED_TASK(one_hz_loop, 1, 100), @@ -289,6 +288,9 @@ void Sub::one_hz_loop() // log terrain data terrain_logging(); + + // init compass location for declination + init_compass_location(); } // called at 50hz diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index f0cc5d643cc3a..88b52e2887fa9 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -456,7 +456,7 @@ class Sub : public AP_HAL::HAL::Callbacks { static const AP_Param::Info var_info[]; static const struct LogStructure log_structure[]; - void compass_accumulate(void); + void init_compass_location(); void compass_cal_update(void); void fast_loop(); void fifty_hz_loop(); diff --git a/ArduSub/sensors.cpp b/ArduSub/sensors.cpp index b627ee751df23..75543e54fed21 100644 --- a/ArduSub/sensors.cpp +++ b/ArduSub/sensors.cpp @@ -93,17 +93,10 @@ void Sub::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 Sub::compass_accumulate() +void Sub::init_compass_location() { - if (!g.compass_enabled) { - return; - } - - compass.accumulate(); - // update initial location used for declination if (!ap.compass_init_location) { Location loc;