|
|
|
@ -8,7 +8,9 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = {
@@ -8,7 +8,9 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = {
|
|
|
|
|
AP_GROUPINFO("DEC", 2, Compass, _declination, 0), |
|
|
|
|
AP_GROUPINFO("LEARN", 3, Compass, _learn, 1), // true if learning calibration
|
|
|
|
|
AP_GROUPINFO("USE", 4, Compass, _use_for_yaw, 1), // true if used for DCM yaw
|
|
|
|
|
#if !defined( __AVR_ATmega1280__ ) |
|
|
|
|
AP_GROUPINFO("AUTODEC",5, Compass, _auto_declination, 1), |
|
|
|
|
#endif |
|
|
|
|
AP_GROUPEND |
|
|
|
|
}; |
|
|
|
|
|
|
|
|
@ -61,6 +63,7 @@ Compass::set_initial_location(int32_t latitude, int32_t longitude)
@@ -61,6 +63,7 @@ Compass::set_initial_location(int32_t latitude, int32_t longitude)
|
|
|
|
|
{ |
|
|
|
|
// if automatic declination is configured, then compute
|
|
|
|
|
// the declination based on the initial GPS fix
|
|
|
|
|
#if !defined( __AVR_ATmega1280__ ) |
|
|
|
|
if (_auto_declination) { |
|
|
|
|
// Set the declination based on the lat/lng from GPS
|
|
|
|
|
_declination.set(radians( |
|
|
|
@ -68,6 +71,7 @@ Compass::set_initial_location(int32_t latitude, int32_t longitude)
@@ -68,6 +71,7 @@ Compass::set_initial_location(int32_t latitude, int32_t longitude)
|
|
|
|
|
(float)latitude / 10000000, |
|
|
|
|
(float)longitude / 10000000))); |
|
|
|
|
} |
|
|
|
|
#endif |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void |
|
|
|
|