Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: don't store raw and unfiltered fields 

These are not used. The only place where raw fields are used are in the
compass calibrator and we don't need to store them.

Additionally remove duplicated documentation about the meaning of the
functions to avoid them getting out of sync.
This commit is contained in:
Lucas De Marchi 2016-03-10 22:07:52 -03:00
parent f258b66627
commit 20a4a42cb3
9 changed files with 2 additions and 86 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/AP_Compass/AP_Compass.h
View File

@ -122,24 +122,6 @@ public:
const Vector3f &get_field(uint8_t i) const { return _state[i].field; }
const Vector3f &get_field(void) const { return get_field(get_primary()); }
// raw/unfiltered measurement interface
uint32_t raw_meas_time_us(uint8_t i) const { return _state[i].raw_meas_time_us; }
uint32_t raw_meas_time_us() const { return _state[get_primary()].raw_meas_time_us; }
uint32_t unfiltered_meas_time_us(uint8_t i) const { return _state[i].raw_meas_time_us; }
uint32_t unfiltered_meas_time_us() const { return _state[get_primary()].raw_meas_time_us; }
bool has_raw_field(uint8_t i) const { return _state[i].has_raw_field; }
bool has_raw_field() const { return has_raw_field(get_primary()); }
bool has_unfiltered_field(uint8_t i) const { return _state[i].has_unfiltered_field; }
bool has_unfiltered_field() const { return has_unfiltered_field(get_primary()); }
const Vector3f &get_raw_field(uint8_t i) const { return _state[i].raw_field; }
const Vector3f &get_raw_field(void) const { return get_raw_field(get_primary()); }
const Vector3f &get_unfiltered_field(uint8_t i) const { return _state[i].unfiltered_field; }
const Vector3f &get_unfiltered_field(void) const { return get_unfiltered_field(get_primary()); }
// compass calibrator interface
void compass_cal_update();
@ -395,14 +377,6 @@ private:
// when we last got data
uint32_t last_update_ms;
uint32_t last_update_usec;
uint32_t raw_meas_time_us;
bool has_raw_field;
bool has_unfiltered_field;
bool updated_raw_field;
bool updated_unfiltered_field;
Vector3f raw_field;
Vector3f unfiltered_field;
} _state[COMPASS_MAX_INSTANCES];
CompassCalibrator _calibrator[COMPASS_MAX_INSTANCES];

3
libraries/AP_Compass/AP_Compass_AK8963.cpp
View File

@ -264,9 +264,6 @@ void AP_Compass_AK8963::_update()
// correct raw_field for known errors
correct_field(raw_field, _compass_instance);
// publish raw_field (corrected point sample) for EKF use
publish_unfiltered_field(raw_field, time_us, _compass_instance);
_mag_x_accum += raw_field.x;
_mag_y_accum += raw_field.y;
_mag_z_accum += raw_field.z;

39
libraries/AP_Compass/AP_Compass_Backend.cpp
View File

@ -11,21 +11,6 @@ AP_Compass_Backend::AP_Compass_Backend(Compass &compass) :
_compass(compass)
{}
/*
* A compass measurement is expected to pass through the following functions:
* 1. rotate_field - this rotates the measurement in-place from sensor frame
* to body frame
* 2. publish_raw_field - this provides an uncorrected point-sample for
* calibration libraries
* 3. correct_field - this corrects the measurement in-place for hard iron,
* soft iron, motor interference, and non-orthagonality errors
* 4. publish_unfiltered_field - this (optionally) provides a corrected
* point sample for fusion into the EKF
* 5. publish_filtered_field - legacy filtered magnetic field
*
* All those functions expect the mag field to be in milligauss.
*/
void AP_Compass_Backend::rotate_field(Vector3f &mag, uint8_t instance)
{
Compass::mag_state &state = _compass._state[instance];
@ -44,16 +29,10 @@ void AP_Compass_Backend::publish_raw_field(const Vector3f &mag, uint32_t time_us
{
Compass::mag_state &state = _compass._state[instance];
state.last_update_ms = AP_HAL::millis();
// note that we do not set last_update_usec here as otherwise the
// EKF and DCM would end up consuming compass data at the full
// sensor rate. We want them to consume only the filtered fields
state.raw_field = mag;
state.raw_meas_time_us = time_us;
state.updated_raw_field = true;
state.has_raw_field = true;
state.last_update_ms = AP_HAL::millis();
_compass._calibrator[instance].new_sample(mag);
}
@ -92,16 +71,6 @@ void AP_Compass_Backend::correct_field(Vector3f &mag, uint8_t i)
mag = mat * mag;
}
void AP_Compass_Backend::publish_unfiltered_field(const Vector3f &mag, uint32_t time_us, uint8_t instance)
{
Compass::mag_state &state = _compass._state[instance];
state.unfiltered_field = mag;
state.raw_meas_time_us = time_us;
state.updated_unfiltered_field = true;
state.has_unfiltered_field = true;
}
/*
copy latest data to the frontend from a backend
*/
@ -113,12 +82,6 @@ void AP_Compass_Backend::publish_filtered_field(const Vector3f &mag, uint8_t ins
state.last_update_ms = AP_HAL::millis();
state.last_update_usec = AP_HAL::micros();
state.has_raw_field = state.updated_raw_field;
state.updated_raw_field = false;
state.has_unfiltered_field = state.updated_unfiltered_field;
state.updated_unfiltered_field = false;
}
/*

5
libraries/AP_Compass/AP_Compass_Backend.h
View File

@ -52,9 +52,7 @@ protected:
* calibration libraries
* 3. correct_field - this corrects the measurement in-place for hard iron,
* soft iron, motor interference, and non-orthagonality errors
* 4. publish_unfiltered_field - this (optionally) provides a corrected
* point sample for fusion into the EKF
* 5. publish_filtered_field - legacy filtered magnetic field
* 4. publish_filtered_field - legacy filtered magnetic field
*
* All those functions expect the mag field to be in milligauss.
*/
@ -62,7 +60,6 @@ protected:
void rotate_field(Vector3f &mag, uint8_t instance);
void publish_raw_field(const Vector3f &mag, uint32_t time_us, uint8_t instance);
void correct_field(Vector3f &mag, uint8_t i);
void publish_unfiltered_field(const Vector3f &mag, uint32_t time_us, uint8_t instance);
void publish_filtered_field(const Vector3f &mag, uint8_t instance);
// register a new compass instance with the frontend

3
libraries/AP_Compass/AP_Compass_HMC5843.cpp
View File

@ -248,9 +248,6 @@ void AP_Compass_HMC5843::accumulate()
// correct raw_field for known errors
correct_field(raw_field, _compass_instance);
// publish raw_field (corrected point sample) for EKF use
publish_unfiltered_field(raw_field, tnow, _compass_instance);
_mag_x_accum += raw_field.x;
_mag_y_accum += raw_field.y;
_mag_z_accum += raw_field.z;

3
libraries/AP_Compass/AP_Compass_LSM303D.cpp
View File

@ -417,9 +417,6 @@ void AP_Compass_LSM303D::_collect_samples()
// correct raw_field for known errors
correct_field(raw_field, _compass_instance);
// publish raw_field (corrected point sample) for EKF use
publish_unfiltered_field(raw_field, time_us, _compass_instance);
_mag_x_accum += raw_field.x;
_mag_y_accum += raw_field.y;
_mag_z_accum += raw_field.z;

3
libraries/AP_Compass/AP_Compass_PX4.cpp
View File

@ -148,9 +148,6 @@ void AP_Compass_PX4::accumulate(void)
// correct raw_field for known errors
correct_field(raw_field, frontend_instance);
// publish raw_field (corrected point sample) for EKF use
publish_unfiltered_field(raw_field, time_us, frontend_instance);
// accumulate into averaging filter
_sum[i] += raw_field;
_count[i]++;

3
libraries/AP_Compass/AP_Compass_QURT.cpp
View File

@ -88,9 +88,6 @@ void AP_Compass_QURT::timer_update(void)
// correct raw_field for known errors
correct_field(raw_field, instance);
// publish raw_field (corrected point sample) for EKF use
publish_unfiltered_field(raw_field, data.timestamp, instance);
// accumulate into averaging filter
sum += raw_field;
count++;

3
libraries/AP_Compass/AP_Compass_qflight.cpp
View File

@ -99,9 +99,6 @@ void AP_Compass_QFLIGHT::timer_update(void)
// correct raw_field for known errors
correct_field(raw_field, instance);
// publish raw_field (corrected point sample) for EKF use
publish_unfiltered_field(raw_field, time_us, instance);
// accumulate into averaging filter
sum += raw_field;
count++;