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:
parent
3699932417
commit
c59bdc12df
@ -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);
|
||||
|
@ -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);
|
||||
|
@ -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;
|
||||
}
|
||||
|
@ -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;
|
||||
|
||||
|
@ -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;
|
||||
}
|
||||
|
@ -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);
|
||||
|
@ -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)
|
||||
|
@ -61,7 +61,6 @@ public:
|
||||
bool init(void);
|
||||
void read(void);
|
||||
void accumulate(void);
|
||||
float get_conversion_ratio(void) override;
|
||||
};
|
||||
|
||||
class AP_HMC5843_SerialBus
|
||||
|
@ -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
|
||||
|
@ -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
|
||||
|
@ -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();
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -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;
|
||||
|
Loading…
Reference in New Issue
Block a user