From 4fa83ed40deed8dbaa1c738406e226c0d268add9 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 27 Mar 2019 18:02:01 +1100 Subject: [PATCH] Copter: move automatic declination setting into AP_Compass itself --- ArduCopter/Copter.cpp | 3 --- ArduCopter/Copter.h | 1 - ArduCopter/sensors.cpp | 15 --------------- 3 files changed, 19 deletions(-) diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 6e804c8dce..f73dfad45c 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -454,9 +454,6 @@ void Copter::one_hz_loop() // indicates that the sensor or subsystem is present but not // functioning correctly gcs().update_sensor_status_flags(); - - // init compass location for declination - init_compass_location(); } // called at 50hz diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 42216d16cc..681931c380 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -825,7 +825,6 @@ private: void read_rangefinder(void); bool rangefinder_alt_ok(); void rpm_update(); - void init_compass_location(); void init_optflow(); void update_optical_flow(void); void compass_cal_update(void); diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index 07e899320d..11a756b861 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -85,21 +85,6 @@ void Copter::rpm_update(void) #endif } -/* - initialise compass's location used for declination - */ -void Copter::init_compass_location() -{ - // update initial location used for declination - if (!ap.compass_init_location) { - Location loc; - if (ahrs.get_position(loc)) { - compass.set_initial_location(loc.lat, loc.lng); - ap.compass_init_location = true; - } - } -} - // initialise optical flow sensor void Copter::init_optflow() {