Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Compass: added COMPASS_AUTODEC option 

when this is 1 (which is the default), we will get the declination
automatically via the AP_Declination library

when it is 0 we will use the value configured by the user
This commit is contained in:
Andrew Tridgell 2012-03-30 14:18:06 +11:00
parent a98a092a59
commit 825340d53d
2 changed files with 11 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_Compass/Compass.cpp
View File

@ -7,6 +7,7 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = {
AP_GROUPINFO("DEC", 2, Compass, _declination), AP_GROUPINFO("DEC", 2, Compass, _declination),
AP_GROUPINFO("LEARN", 3, Compass, _learn), // true if learning calibration AP_GROUPINFO("LEARN", 3, Compass, _learn), // true if learning calibration
AP_GROUPINFO("USE", 4, Compass, _use_for_yaw), // true if used for DCM yaw AP_GROUPINFO("USE", 4, Compass, _use_for_yaw), // true if used for DCM yaw
AP_GROUPINFO("AUTODEC",5, Compass, _auto_declination),
AP_GROUPEND AP_GROUPEND
}; };
@ -21,6 +22,7 @@ Compass::Compass(void) :
_use_for_yaw(1), _use_for_yaw(1),
_null_enable(false), _null_enable(false),
_null_init_done(false), _null_init_done(false),
_auto_declination(1),
_orientation(ROTATION_NONE) _orientation(ROTATION_NONE)
{ {
} }
@ -57,23 +59,17 @@ Compass::get_offsets()
return _offset; return _offset;
} }
bool void
Compass::set_initial_location(long latitude, long longitude, bool force) Compass::set_initial_location(long latitude, long longitude)
{ {
// If the user has choosen to use auto-declination regardless of the planner value // if automatic declination is configured, then compute
// OR // the declination based on the initial GPS fix
// If the declination failed to load from the EEPROM (ie. not set by user) if (_auto_declination) {
if(force || !_declination.load())
{
// Set the declination based on the lat/lng from GPS // Set the declination based on the lat/lng from GPS
_declination.set(radians(AP_Declination::get_declination((float)latitude / 10000000, (float)longitude / 10000000)));
// Reset null offsets
null_offsets_disable(); null_offsets_disable();
_declination.set(radians(AP_Declination::get_declination((float)latitude / 10000000, (float)longitude / 10000000)));
null_offsets_enable(); null_offsets_enable();
return true;
} }
return false;
} }
void void

6
libraries/AP_Compass/Compass.h
View File

@ -81,13 +81,12 @@ public:
/// ///
virtual Vector3f &get_offsets(); virtual Vector3f &get_offsets();
/// Sets the initial location used to get declination - Returns true if declination set /// Sets the initial location used to get declination
/// ///
/// @param latitude GPS Latitude. /// @param latitude GPS Latitude.
/// @param longitude GPS Longitude. /// @param longitude GPS Longitude.
/// @param force Force the compass declination update.
/// ///
bool set_initial_location(long latitude, long longitude, bool force); void set_initial_location(long latitude, long longitude);
/// Program new offset values. /// Program new offset values.
/// ///
@ -129,6 +128,7 @@ protected:
AP_Float _declination; AP_Float _declination;
AP_Int8 _learn; ///<enable calibration learning AP_Int8 _learn; ///<enable calibration learning
AP_Int8 _use_for_yaw; ///<enable use for yaw calculation AP_Int8 _use_for_yaw; ///<enable use for yaw calculation
AP_Int8 _auto_declination; ///<enable automatic declination code
bool _null_enable; ///< enabled flag for offset nulling bool _null_enable; ///< enabled flag for offset nulling
bool _null_init_done; ///< first-time-around flag used by offset nulling bool _null_init_done; ///< first-time-around flag used by offset nulling