diff --git a/AntennaTracker/AntennaTracker.cpp b/AntennaTracker/AntennaTracker.cpp index 21490bb96a..7d7c60e447 100644 --- a/AntennaTracker/AntennaTracker.cpp +++ b/AntennaTracker/AntennaTracker.cpp @@ -97,9 +97,6 @@ void Tracker::one_second_loop() // updated armed/disarmed status LEDs update_armed_disarmed(); - // init compass location for declination - init_compass_location(); - if (!ahrs.home_is_set()) { // set home to current location Location temp_loc; diff --git a/AntennaTracker/Tracker.h b/AntennaTracker/Tracker.h index 6f51a9901a..fc61d78014 100644 --- a/AntennaTracker/Tracker.h +++ b/AntennaTracker/Tracker.h @@ -248,7 +248,6 @@ private: // sensors.cpp void update_ahrs(); void compass_save(); - void init_compass_location(); void update_compass(void); void accel_cal_update(void); void update_GPS(void); diff --git a/AntennaTracker/sensors.cpp b/AntennaTracker/sensors.cpp index 4b5126ae7a..5e7acef335 100644 --- a/AntennaTracker/sensors.cpp +++ b/AntennaTracker/sensors.cpp @@ -8,21 +8,6 @@ void Tracker::update_ahrs() ahrs.update(); } -/* - initialise compass's location used for declination - */ -void Tracker::init_compass_location(void) -{ - // update initial location used for declination - if (!compass_init_location) { - Location loc; - if (ahrs.get_position(loc)) { - compass.set_initial_location(loc.lat, loc.lng); - compass_init_location = true; - } - } -} - /* read and update compass */ @@ -85,11 +70,6 @@ void Tracker::update_GPS(void) if (!set_home(current_loc)) { // silently ignored } - - if (AP::compass().enabled()) { - // Set compass declination automatically - compass.set_initial_location(gps.location().lat, gps.location().lng); - } ground_start_count = 0; } }