Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 12:38:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: move automatic declination setting into AP_Compass itself

Browse Source
This commit is contained in:
Peter Barker 2019-03-27 18:01:18 +11:00 committed by Andrew Tridgell
parent c339d18ee6
commit 1adda1ccf0
2 changed files with 30 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/AP_Compass/AP_Compass.cpp
View File

@ -959,6 +959,9 @@ void Compass::_detect_backends(void)
bool bool
Compass::read(void) Compass::read(void)
{ {
if (!_initial_location_set) {
try_set_initial_location();
}
for (uint8_t i=0; i< _backend_count; i++) { for (uint8_t i=0; i< _backend_count; i++) {
// call read on each of the backend. This call updates field[i] // call read on each of the backend. This call updates field[i]
_backends[i]->read(); _backends[i]->read();
@ -1060,18 +1063,28 @@ Compass::save_motor_compensation()
} }
} }
void void Compass::try_set_initial_location()
Compass::set_initial_location(int32_t latitude, int32_t longitude)
{ {
if (!_auto_declination) {
return;
}
if (!_enabled) {
return;
}
Location loc;
if (!AP::ahrs().get_position(loc)) {
return;
}
_initial_location_set = true;
// if automatic declination is configured, then compute // if automatic declination is configured, then compute
// the declination based on the initial GPS fix // the declination based on the initial GPS fix
if (_auto_declination) {
// Set the declination based on the lat/lng from GPS // Set the declination based on the lat/lng from GPS
_declination.set(radians( _declination.set(radians(
AP_Declination::get_declination( AP_Declination::get_declination(
(float)latitude / 10000000, (float)loc.lat / 10000000,
(float)longitude / 10000000))); (float)loc.lng / 10000000)));
}
} }
/// return true if the compass should be used for yaw calculations /// return true if the compass should be used for yaw calculations

15
libraries/AP_Compass/AP_Compass.h
View File

@ -174,13 +174,6 @@ public:
const Vector3f &get_offdiagonals(uint8_t i) const { return _state[i].offdiagonals; } const Vector3f &get_offdiagonals(uint8_t i) const { return _state[i].offdiagonals; }
const Vector3f &get_offdiagonals(void) const { return get_offdiagonals(get_primary()); } const Vector3f &get_offdiagonals(void) const { return get_offdiagonals(get_primary()); }
/// Sets the initial location used to get declination
///
/// @param latitude GPS Latitude.
/// @param longitude GPS Longitude.
///
void set_initial_location(int32_t latitude, int32_t longitude);
// learn offsets accessor // learn offsets accessor
bool learn_offsets_enabled() const { return _learn == LEARN_INFLIGHT; } bool learn_offsets_enabled() const { return _learn == LEARN_INFLIGHT; }
@ -482,6 +475,14 @@ private:
CompassLearn *learn; CompassLearn *learn;
bool learn_allocated; bool learn_allocated;
/// Sets the initial location used to get declination
///
/// @param latitude GPS Latitude.
/// @param longitude GPS Longitude.
///
void try_set_initial_location();
bool _initial_location_set;
}; };
namespace AP { namespace AP {