diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index a71262e856..25455d103b 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -178,16 +178,6 @@ void Copter::setup() fast_loopTimer = AP_HAL::micros(); } -/* - if the compass is enabled then try to accumulate a reading - */ -void Copter::compass_accumulate(void) -{ - if (g.compass_enabled) { - compass.accumulate(); - } -} - /* try to accumulate a baro reading */ diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 595b1fce98..e25c50901e 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -285,6 +285,7 @@ private: uint8_t motor_interlock_switch : 1; // 23 // true if pilot is requesting motor interlock enable uint8_t in_arming_delay : 1; // 24 // true while we are armed but waiting to spin motors uint8_t initialised_params : 1; // 25 // true when the all parameters have been initialised. we cannot send parameters to the GCS until this is done + uint8_t compass_init_location : 1; // 26 // true when the compass's initial location has been set }; uint32_t value; } ap; diff --git a/ArduCopter/commands.cpp b/ArduCopter/commands.cpp index 23eb135bad..b20fdeb0ce 100644 --- a/ArduCopter/commands.cpp +++ b/ArduCopter/commands.cpp @@ -72,10 +72,6 @@ bool Copter::set_home(const Location& loc, bool lock) // init inav and compass declination if (ap.home_state == HOME_UNSET) { - // Set compass declination automatically - if (g.compass_enabled) { - compass.set_initial_location(gps.location().lat, gps.location().lng); - } // update navigation scalers. used to offset the shrinking longitude as we go towards the poles scaleLongDown = longitude_scale(loc); // record home is set diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index f0316142a2..f2668dbddc 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -113,6 +113,28 @@ void Copter::init_compass() ahrs.set_compass(&compass); } +/* + if the compass is enabled then try to accumulate a reading + also update initial location used for declination + */ +void Copter::compass_accumulate(void) +{ + if (!g.compass_enabled) { + return; + } + + compass.accumulate(); + + // 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() {