|
|
|
@ -346,7 +346,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
@@ -346,7 +346,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
|
|
|
|
|
// @Increment: 0.1
|
|
|
|
|
// @User: Advanced
|
|
|
|
|
AP_GROUPINFO("CAL_FIT", 30, Compass, _calibration_threshold, 8.0f), |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
AP_GROUPEND |
|
|
|
|
}; |
|
|
|
|
|
|
|
|
@ -473,22 +473,22 @@ void Compass::_detect_backends(void)
@@ -473,22 +473,22 @@ void Compass::_detect_backends(void)
|
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void
|
|
|
|
|
void |
|
|
|
|
Compass::accumulate(void) |
|
|
|
|
{
|
|
|
|
|
{ |
|
|
|
|
for (uint8_t i=0; i< _backend_count; i++) { |
|
|
|
|
// call accumulate on each of the backend
|
|
|
|
|
_backends[i]->accumulate(); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
bool
|
|
|
|
|
bool |
|
|
|
|
Compass::read(void) |
|
|
|
|
{ |
|
|
|
|
for (uint8_t i=0; i< _backend_count; i++) { |
|
|
|
|
// call read on each of the backend. This call updates field[i]
|
|
|
|
|
_backends[i]->read(); |
|
|
|
|
}
|
|
|
|
|
} |
|
|
|
|
for (uint8_t i=0; i < COMPASS_MAX_INSTANCES; i++) { |
|
|
|
|
_state[i].healthy = (AP_HAL::millis() - _state[i].last_update_ms < 500); |
|
|
|
|
} |
|
|
|
@ -741,7 +741,7 @@ void Compass::setHIL(uint8_t instance, const Vector3f &mag)
@@ -741,7 +741,7 @@ void Compass::setHIL(uint8_t instance, const Vector3f &mag)
|
|
|
|
|
_state[instance].last_update_usec = AP_HAL::micros(); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
const Vector3f& Compass::getHIL(uint8_t instance) const
|
|
|
|
|
const Vector3f& Compass::getHIL(uint8_t instance) const |
|
|
|
|
{ |
|
|
|
|
return _hil.field[instance]; |
|
|
|
|
} |
|
|
|
@ -751,7 +751,7 @@ void Compass::_setup_earth_field(void)
@@ -751,7 +751,7 @@ void Compass::_setup_earth_field(void)
|
|
|
|
|
{ |
|
|
|
|
// assume a earth field strength of 400
|
|
|
|
|
_hil.Bearth(400, 0, 0); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// rotate _Bearth for inclination and declination. -66 degrees
|
|
|
|
|
// is the inclination in Canberra, Australia
|
|
|
|
|
Matrix3f R; |
|
|
|
|