aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions 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

31
libraries/AP_Compass/AP_Compass.cpp
View File

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

15
libraries/AP_Compass/AP_Compass.h
View File

@ -173,13 +173,6 @@ public:
const Vector3f &get_offdiagonals(uint8_t i) const { return _state[i].offdiagonals; }
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
bool learn_offsets_enabled() const { return _learn == LEARN_INFLIGHT; }
@ -482,6 +475,14 @@ private:
CompassLearn *learn;
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 {