5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 23:48:31 -04:00

AP_Compass: fix the milligauss handling

the previous approach assumed a 1:1 mapping between compass backends
and compass instances, which isn't true on PX4.

It also only setup milligauss offsets on a set_and_save call, which is
not the only way offsets change

this adds a milligauss_ratio per instance, which is considerably
simpler
This commit is contained in:
Andrew Tridgell 2015-09-09 14:52:14 +10:00
parent 3699932417
commit c59bdc12df
12 changed files with 21 additions and 44 deletions

View File

@ -178,6 +178,8 @@ bool AP_Compass_AK8963::init()
set_dev_id(_compass_instance, _bus->get_dev_id());
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Compass_AK8963::_update, void));
set_milligauss_ratio(_compass_instance, 10.0f);
_bus_sem->give();
hal.scheduler->resume_timer_procs();
@ -216,12 +218,6 @@ 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);

View File

@ -46,7 +46,6 @@ 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);

View File

@ -41,11 +41,6 @@ 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;
@ -146,3 +141,9 @@ void AP_Compass_Backend::set_external(uint8_t instance, bool external)
{
_compass._state[instance].external.set(external);
}
// set ratio to convert to milligauss
void AP_Compass_Backend::set_milligauss_ratio(uint8_t instance, float ratio)
{
_compass._state[instance].milligauss_ratio = ratio;
}

View File

@ -43,8 +43,6 @@ public:
// backends
virtual void accumulate(void) {};
virtual float get_conversion_ratio(void) = 0;
protected:
/*
@ -75,6 +73,9 @@ protected:
// set external state for an instance
void set_external(uint8_t instance, bool external);
// set ratio to convert to milligauss
void set_milligauss_ratio(uint8_t instance, float ratio);
// access to frontend
Compass &_compass;

View File

@ -53,6 +53,7 @@ AP_Compass_HIL::init(void)
// register two compass instances
for (uint8_t i=0; i<HIL_NUM_COMPASSES; i++) {
_compass_instance[i] = register_compass();
set_milligauss_ratio(_compass_instance[i], 1.0f);
}
return true;
}
@ -70,8 +71,3 @@ void AP_Compass_HIL::read()
}
}
}
float AP_Compass_HIL::get_conversion_ratio(void)
{
return 1.0f;
}

View File

@ -16,7 +16,6 @@ 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);

View File

@ -331,6 +331,8 @@ AP_Compass_HMC5843::init()
_compass_instance = register_compass();
set_dev_id(_compass_instance, _product_id);
set_milligauss_ratio(_compass_instance, 1.0f / _gain_multiple);
return true;
errout:
@ -481,14 +483,6 @@ 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)

View File

@ -61,7 +61,6 @@ public:
bool init(void);
void read(void);
void accumulate(void);
float get_conversion_ratio(void) override;
};
class AP_HMC5843_SerialBus

View File

@ -95,6 +95,8 @@ bool AP_Compass_PX4::init(void)
set_external(_instance[i], ioctl(_mag_fd[i], MAGIOCGEXTERNAL, 0) > 0);
_count[i] = 0;
_sum[i].zero();
set_milligauss_ratio(_instance[i], 1.0f);
}
// give the driver a chance to run, and gather one sample
@ -159,9 +161,4 @@ void AP_Compass_PX4::accumulate(void)
}
}
float AP_Compass_PX4::get_conversion_ratio(void)
{
return 1.0f;
}
#endif // CONFIG_HAL_BOARD

View File

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

View File

@ -517,9 +517,6 @@ Compass::set_and_save_offsets(uint8_t i, const Vector3f &offsets)
if (i < COMPASS_MAX_INSTANCES) {
_state[i].offset.set(offsets);
save_offsets(i);
/* Ugly hack to update offsets in milligauss that are going to be used across all the codebase in the future */
_state[i].offset_milligauss = offsets * _backends[i]->get_conversion_ratio();
}
}

View File

@ -113,8 +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()); }
const Vector3f get_field_milligauss(uint8_t i) const { return get_field(i) * _state[i].milligauss_ratio; }
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; }
@ -173,8 +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()); }
const Vector3f get_offsets_milligauss(uint8_t i) const { return get_offsets(i) * _state[i].milligauss_ratio; }
const Vector3f get_offsets_milligauss(void) const { return get_offsets_milligauss(get_primary()); }
/// Sets the initial location used to get declination
///
@ -364,7 +364,6 @@ private:
AP_Vector3f offset;
AP_Vector3f diagonals;
AP_Vector3f offdiagonals;
Vector3f offset_milligauss;
#if COMPASS_MAX_INSTANCES > 1
// device id detected at init.
@ -385,7 +384,7 @@ private:
// corrected magnetic field strength
Vector3f field;
Vector3f field_milligauss;
float milligauss_ratio;
// when we last got data
uint32_t last_update_ms;