aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Compass: add milligauss counterparts to get_field() and get_offsets()

Browse Source 
From now on there's a pair get_field_milligauss() and
get_offsets_milligauss() that can make the transition to the common
units across all compasses easier.
This commit is contained in:
Staroselskii Georgii 2015-08-24 17:21:34 +03:00 committed by Andrew Tridgell
parent 4431b33686
commit c207d8c6a8
11 changed files with 51 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_Compass/AP_Compass_AK8963.cpp
View File

@ -214,6 +214,12 @@ void AP_Compass_AK8963::read()
publish_filtered_field(field, _compass_instance);
}
float AP_Compass_AK8963::get_conversion_ratio(void)
{
/* Convert from microTesla to milliGauss */
return 10.0f;
}
Vector3f AP_Compass_AK8963::_get_filtered_field() const
{
Vector3f field(_mag_x_accum, _mag_y_accum, _mag_z_accum);

1
libraries/AP_Compass/AP_Compass_AK8963.h
View File

@ -46,6 +46,7 @@ public:
bool init(void);
void read(void);
void accumulate(void);
float get_conversion_ratio(void) override;
private:
static AP_Compass_Backend *_detect(Compass &compass, AP_AK8963_SerialBus *bus);

5
libraries/AP_Compass/AP_Compass_Backend.cpp
View File

@ -41,6 +41,11 @@ void AP_Compass_Backend::publish_raw_field(const Vector3f &mag, uint32_t time_us
{
Compass::mag_state &state = _compass._state[instance];
/* Update field in milligauss. Later this will be used throughout all codebase.
* We need this trick in order not to make users recalibrate their compasses.
* */
state.field_milligauss = state.field * get_conversion_ratio();
state.last_update_ms = hal.scheduler->millis();
state.last_update_usec = hal.scheduler->micros();
state.raw_field = mag;

2
libraries/AP_Compass/AP_Compass_Backend.h
View File

@ -43,6 +43,8 @@ public:
// backends
virtual void accumulate(void) {};
virtual float get_conversion_ratio(void) = 0;
protected:
/*

5
libraries/AP_Compass/AP_Compass_HIL.cpp
View File

@ -69,3 +69,8 @@ void AP_Compass_HIL::read()
}
}
}
float AP_Compass_HIL::get_conversion_ratio(void)
{
return 1.0f;
}

1
libraries/AP_Compass/AP_Compass_HIL.h
View File

@ -16,6 +16,7 @@ public:
AP_Compass_HIL(Compass &compass);
void read(void);
bool init(void);
float get_conversion_ratio(void) override;
// detect the sensor
static AP_Compass_Backend *detect(Compass &compass);

25
libraries/AP_Compass/AP_Compass_HMC5843.cpp
View File

@ -269,7 +269,7 @@ AP_Compass_HMC5843::init()
uint8_t calibration_gain = 0x20;
uint16_t expected_x = 715;
uint16_t expected_yz = 715;
float gain_multiple = 1.0;
_gain_multiple = 1.0;
_bus_sem = _bus->get_semaphore();
hal.scheduler->suspend_timer_procs();
@ -298,10 +298,10 @@ AP_Compass_HMC5843::init()
*/
expected_x = 766;
expected_yz = 713;
gain_multiple = 660.0f / 1090; // adjustment for runtime vs calibration gain
_gain_multiple = 660.0f / 1090; // adjustment for runtime vs calibration gain
}
if (!_calibrate(calibration_gain, expected_x, expected_yz, gain_multiple)) {
if (!_calibrate(calibration_gain, expected_x, expected_yz)) {
hal.console->printf_P(PSTR("HMC5843: Could not calibrate sensor\n"));
goto errout;
}
@ -342,8 +342,7 @@ fail_sem:
bool AP_Compass_HMC5843::_calibrate(uint8_t calibration_gain,
uint16_t expected_x,
uint16_t expected_yz,
float gain_multiple)
uint16_t expected_yz)
{
int numAttempts = 0, good_count = 0;
bool success = false;
@ -413,7 +412,7 @@ bool AP_Compass_HMC5843::_calibrate(uint8_t calibration_gain,
if (good_count >= 5) {
/*
The use of gain_multiple below is incorrect, as the gain
The use of _gain_multiple below is incorrect, as the gain
difference between 2.5Ga mode and 1Ga mode is already taken
into account by the expected_x and expected_yz values. We
are not going to fix it however as it would mean all
@ -423,9 +422,9 @@ bool AP_Compass_HMC5843::_calibrate(uint8_t calibration_gain,
doesn't have any impact other than the learned compass
offsets
*/
_scaling[0] = _scaling[0] * gain_multiple / good_count;
_scaling[1] = _scaling[1] * gain_multiple / good_count;
_scaling[2] = _scaling[2] * gain_multiple / good_count;
_scaling[0] = _scaling[0] * _gain_multiple / good_count;
_scaling[1] = _scaling[1] * _gain_multiple / good_count;
_scaling[2] = _scaling[2] * _gain_multiple / good_count;
success = true;
} else {
/* best guess */
@ -482,6 +481,14 @@ void AP_Compass_HMC5843::read()
_retry_time = 0;
}
float AP_Compass_HMC5843::get_conversion_ratio(void)
{
/* This value converts from hmc-units to milligauss. It looks strange for a reason.
* It's meant to cancel the unneccassary division in the read() method.
*/
return 1.0f / _gain_multiple;
}
/* I2C implementation of the HMC5843 */
AP_HMC5843_SerialBus_I2C::AP_HMC5843_SerialBus_I2C(AP_HAL::I2CDriver *i2c, uint8_t addr)
: _i2c(i2c)

4
libraries/AP_Compass/AP_Compass_HMC5843.h
View File

@ -31,7 +31,7 @@ private:
bool read_register(uint8_t address, uint8_t *value);
bool write_register(uint8_t address, uint8_t value);
bool _calibrate(uint8_t calibration_gain, uint16_t expected_x, uint16_t expected_yz, float gain_multiple);
bool _calibrate(uint8_t calibration_gain, uint16_t expected_x, uint16_t expected_yz);
bool _detect_version();
uint32_t _retry_time; // when unhealthy the millis() value to retry at
@ -48,6 +48,7 @@ private:
uint8_t _compass_instance;
uint8_t _product_id;
float _gain_multiple;
public:
// detect the sensor
static AP_Compass_Backend *detect_i2c(Compass &compass,
@ -60,6 +61,7 @@ public:
bool init(void);
void read(void);
void accumulate(void);
float get_conversion_ratio(void) override;
};
class AP_HMC5843_SerialBus

5
libraries/AP_Compass/AP_Compass_PX4.cpp
View File

@ -159,4 +159,9 @@ void AP_Compass_PX4::accumulate(void)
}
}
float AP_Compass_PX4::get_conversion_ratio(void)
{
return 1.0f;
}
#endif // CONFIG_HAL_BOARD

1
libraries/AP_Compass/AP_Compass_PX4.h
View File

@ -12,6 +12,7 @@ public:
bool init(void);
void read(void);
void accumulate(void);
float get_conversion_ratio(void) override;
AP_Compass_PX4(Compass &compass);
// detect the sensor

6
libraries/AP_Compass/Compass.h
View File

@ -113,6 +113,8 @@ public:
/// Return the current field as a Vector3f
const Vector3f &get_field(uint8_t i) const { return _state[i].field; }
const Vector3f &get_field(void) const { return get_field(get_primary()); }
const Vector3f &get_field_milligauss(uint8_t i) const { return _state[i].field_milligauss; }
const Vector3f &get_field_milligauss(void) const { return get_field_milligauss(get_primary()); }
// raw/unfiltered measurement interface
uint32_t raw_meas_time_us(uint8_t i) const { return _state[i].raw_meas_time_us; }
@ -171,6 +173,8 @@ public:
///
const Vector3f &get_offsets(uint8_t i) const { return _state[i].offset; }
const Vector3f &get_offsets(void) const { return get_offsets(get_primary()); }
const Vector3f &get_offsets_milligauss(uint8_t i) const { return _state[i].offset_milligauss; }
const Vector3f &get_offsets_milligauss(void) const { return get_offsets_milligauss(get_primary()); }
/// Sets the initial location used to get declination
///
@ -360,6 +364,7 @@ private:
AP_Vector3f offset;
AP_Vector3f diagonals;
AP_Vector3f offdiagonals;
Vector3f offset_milligauss;
#if COMPASS_MAX_INSTANCES > 1
// device id detected at init.
@ -380,6 +385,7 @@ private:
// corrected magnetic field strength
Vector3f field;
Vector3f field_milligauss;
// when we last got data
uint32_t last_update_ms;