Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: modify backend interface to push raw samples to frontend

Browse Source
This commit is contained in:
Jonathan Challinger 2015-03-18 15:15:22 -07:00 committed by Andrew Tridgell
parent b990eaed68
commit add1b8c257
2 changed files with 95 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

113
libraries/AP_Compass/AP_Compass_Backend.cpp
View File

@ -10,31 +10,92 @@ 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
*/
void AP_Compass_Backend::rotate_field(Vector3f &mag, uint8_t instance)
{
Compass::mag_state &state = _compass._state[instance];
mag.rotate(MAG_BOARD_ORIENTATION);
if (!state.external) {
// and add in AHRS_ORIENTATION setting if not an external compass
mag.rotate(_compass._board_orientation);
} else {
// add user selectable orientation
mag.rotate((enum Rotation)state.orientation.get());
}
}
void AP_Compass_Backend::publish_raw_field(const Vector3f &mag, uint32_t time_us, uint8_t instance)
{
Compass::mag_state &state = _compass._state[instance];
state.last_update_ms = hal.scheduler->millis();
state.last_update_usec = hal.scheduler->micros();
state.raw_field = mag;
state.raw_meas_time_us = time_us;
state.updated_raw_field = true;
state.has_raw_field = true;
}
void AP_Compass_Backend::correct_field(Vector3f &mag, uint8_t i)
{
Compass::mag_state &state = _compass._state[i];
const Vector3f &offsets = state.offset.get();
const Vector3f &mot = state.motor_compensation.get();
/*
* note that _motor_offset[] is kept even if compensation is not
* being applied so it can be logged correctly
*/
mag += offsets;
if(_compass._motor_comp_type != AP_COMPASS_MOT_COMP_DISABLED && !is_zero(_compass._thr_or_curr)) {
state.motor_offset = mot * _compass._thr_or_curr;
mag += state.motor_offset;
} else {
state.motor_offset.zero();
}
}
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
*/
void AP_Compass_Backend::publish_field(const Vector3f &mag, uint8_t instance)
void AP_Compass_Backend::publish_filtered_field(const Vector3f &mag, uint8_t instance)
{
Compass::mag_state &state = _compass._state[instance];
state.field = mag;
// apply default board orientation for this compass type. This is
// a noop on most boards
state.field.rotate(MAG_BOARD_ORIENTATION);
if (!state.external) {
// and add in AHRS_ORIENTATION setting if not an external compass
state.field.rotate(_compass._board_orientation);
} else {
// add user selectable orientation
state.field.rotate((enum Rotation)state.orientation.get());
}
apply_corrections(state.field, instance);
state.last_update_ms = hal.scheduler->millis();
state.last_update_usec = hal.scheduler->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;
}
/*
@ -44,28 +105,6 @@ void AP_Compass_Backend::publish_field(const Vector3f &mag, uint8_t instance)
uint8_t AP_Compass_Backend::register_compass(void) const
{
return _compass.register_compass();
}
/*
apply offset and motor compensation corrections
*/
void AP_Compass_Backend::apply_corrections(Vector3f &mag, uint8_t i)
{
Compass::mag_state &state = _compass._state[i];
const Vector3f &offsets = state.offset.get();
const Vector3f &mot = state.motor_compensation.get();
/*
note that _motor_offset[] is kept even if compensation is not
being applied so it can be logged correctly
*/
mag += offsets;
if(_compass._motor_comp_type != AP_COMPASS_MOT_COMP_DISABLED && !is_zero(_compass._thr_or_curr)) {
state.motor_offset = mot * _compass._thr_or_curr;
mag += state.motor_offset;
} else {
state.motor_offset.zero();
}
}

21
libraries/AP_Compass/AP_Compass_Backend.h
View File

@ -44,8 +44,25 @@ public:
virtual void accumulate(void) {};
protected:
// publish a magnetic field vector to the frontend
void publish_field(const Vector3f &mag, uint8_t instance);
/*
* 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
*/
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
uint8_t register_compass(void) const;