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

AP_Compass: added per-motor compass calibration

Browse Source 
this allows for a motor calibration vector per motor
This commit is contained in:
Andrew Tridgell 2018-01-05 21:08:04 +11:00
parent b1ccf575f7
commit ca30f6aec8
5 changed files with 354 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_Compass/AP_Compass.cpp
View File

@ -423,6 +423,10 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @User: Advanced
AP_GROUPINFO("OFFS_MAX", 31, Compass, _offset_max, AP_COMPASS_OFFSETS_MAX_DEFAULT),
// @Group: PMOT
// @Path: Compass_PerMotor.cpp
AP_SUBGROUPINFO(_per_motor, "PMOT", 32, Compass, Compass_PerMotor),
// @Param: TYPEMASK
// @DisplayName: Compass disable driver type mask
// @Description: This is a bitmask of driver types to disable. If a driver type is set in this mask then that driver will not try to find a sensor at startup

27
libraries/AP_Compass/AP_Compass.h
View File

@ -11,11 +11,13 @@
#include "CompassCalibrator.h"
#include "AP_Compass_Backend.h"
#include "Compass_PerMotor.h"
// motor compensation types (for use with motor_comp_enabled)
#define AP_COMPASS_MOT_COMP_DISABLED 0x00
#define AP_COMPASS_MOT_COMP_THROTTLE 0x01
#define AP_COMPASS_MOT_COMP_CURRENT 0x02
#define AP_COMPASS_MOT_COMP_PER_MOTOR 0x03
// setup default mag orientation for some board types
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX && CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
@ -112,6 +114,17 @@ public:
// compass calibrator interface
void compass_cal_update();
// per-motor calibration access
void per_motor_calibration_start(void) {
_per_motor.calibration_start();
}
void per_motor_calibration_update(void) {
_per_motor.calibration_update();
}
void per_motor_calibration_end(void) {
_per_motor.calibration_end();
}
void start_calibration_all(bool retry=false, bool autosave=false, float delay_sec=0.0f, bool autoreboot = false);
void cancel_calibration_all();
@ -142,6 +155,12 @@ 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_diagonals(uint8_t i) const { return _state[i].diagonals; }
const Vector3f &get_diagonals(void) const { return get_diagonals(get_primary()); }
const Vector3f &get_offdiagonals(uint8_t i) const { return _state[i].offdiagonals; }
const Vector3f &get_offdiagonals(void) const { return get_offdiagonals(get_primary()); }
/// Sets the initial location used to get declination
///
/// @param latitude GPS Latitude.
@ -235,6 +254,11 @@ public:
}
}
/// Set the battery voltage for per-motor compensation
void set_voltage(float voltage) {
_per_motor.set_voltage(voltage);
}
/// Returns True if the compasses have been configured (i.e. offsets saved)
///
/// @returns True if compass has been configured
@ -418,6 +442,9 @@ private:
CompassCalibrator _calibrator[COMPASS_MAX_INSTANCES];
// per-motor compass compensation
Compass_PerMotor _per_motor{*this};
// if we want HIL only
bool _hil_mode:1;

34
libraries/AP_Compass/AP_Compass_Backend.cpp
View File

@ -51,18 +51,10 @@ void AP_Compass_Backend::correct_field(Vector3f &mag, uint8_t i)
const Vector3f &offdiagonals = state.offdiagonals.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
*/
// add in the basic offsets
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();
}
// apply eliptical correction
Matrix3f mat(
diagonals.x, offdiagonals.x, offdiagonals.y,
offdiagonals.x, diagonals.y, offdiagonals.z,
@ -70,6 +62,28 @@ void AP_Compass_Backend::correct_field(Vector3f &mag, uint8_t i)
);
mag = mat * mag;
/*
calculate motor-power based compensation
note that _motor_offset[] is kept even if compensation is not
being applied so it can be logged correctly
*/
state.motor_offset.zero();
if (_compass._per_motor.enabled() && i == 0) {
// per-motor correction is only valid for first compass
_compass._per_motor.compensate(state.motor_offset);
} else if (_compass._motor_comp_type == AP_COMPASS_MOT_COMP_THROTTLE ||
_compass._motor_comp_type == AP_COMPASS_MOT_COMP_CURRENT) {
state.motor_offset = mot * _compass._thr_or_curr;
}
/*
we apply the motor offsets after we apply the eliptical
correction. This is needed to match the way that the motor
compensation values are calculated, as they are calculated based
on final field outputs, not on the raw outputs
*/
mag += state.motor_offset;
}
/*

230
libraries/AP_Compass/Compass_PerMotor.cpp Normal file
View File

@ -0,0 +1,230 @@
/*
per-motor compass compensation
*/
#include "AP_Compass.h"
#include <GCS_MAVLink/GCS.h>
extern const AP_HAL::HAL &hal;
const AP_Param::GroupInfo Compass_PerMotor::var_info[] = {
// @Param: _EN
// @DisplayName: per-motor compass correction enable
// @Description: This enables per-motor compass corrections
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
AP_GROUPINFO_FLAGS("_EN", 1, Compass_PerMotor, enable, 0, AP_PARAM_FLAG_ENABLE),
// @Param: _EXP
// @DisplayName: per-motor exponential correction
// @Description: This is the exponential correction for the power output of the motor for per-motor compass correction
// @Range: 0 2
// @Increment: 0.01
// @User: Advanced
AP_GROUPINFO("_EXP", 2, Compass_PerMotor, expo, 0.65),
// @Param: 1_X
// @DisplayName: Compass per-motor1 X
// @Description: Compensation for X axis of motor1
// @User: Advanced
// @Param: 1_Y
// @DisplayName: Compass per-motor1 Y
// @Description: Compensation for Y axis of motor1
// @User: Advanced
// @Param: 1_Z
// @DisplayName: Compass per-motor1 Z
// @Description: Compensation for Z axis of motor1
// @User: Advanced
AP_GROUPINFO("1", 3, Compass_PerMotor, compensation[0], 0),
// @Param: 2_X
// @DisplayName: Compass per-motor2 X
// @Description: Compensation for X axis of motor2
// @User: Advanced
// @Param: 2_Y
// @DisplayName: Compass per-motor2 Y
// @Description: Compensation for Y axis of motor2
// @User: Advanced
// @Param: 2_Z
// @DisplayName: Compass per-motor2 Z
// @Description: Compensation for Z axis of motor2
// @User: Advanced
AP_GROUPINFO("2", 4, Compass_PerMotor, compensation[1], 0),
// @Param: 3_X
// @DisplayName: Compass per-motor3 X
// @Description: Compensation for X axis of motor3
// @User: Advanced
// @Param: 3_Y
// @DisplayName: Compass per-motor3 Y
// @Description: Compensation for Y axis of motor3
// @User: Advanced
// @Param: 3_Z
// @DisplayName: Compass per-motor3 Z
// @Description: Compensation for Z axis of motor3
// @User: Advanced
AP_GROUPINFO("3", 5, Compass_PerMotor, compensation[2], 0),
// @Param: 4_X
// @DisplayName: Compass per-motor4 X
// @Description: Compensation for X axis of motor4
// @User: Advanced
// @Param: 4_Y
// @DisplayName: Compass per-motor4 Y
// @Description: Compensation for Y axis of motor4
// @User: Advanced
// @Param: 4_Z
// @DisplayName: Compass per-motor4 Z
// @Description: Compensation for Z axis of motor4
// @User: Advanced
AP_GROUPINFO("4", 6, Compass_PerMotor, compensation[3], 0),
AP_GROUPEND
};
// constructor
Compass_PerMotor::Compass_PerMotor(Compass &_compass) :
compass(_compass)
{
AP_Param::setup_object_defaults(this, var_info);
}
// return current scaled motor output
float Compass_PerMotor::scaled_output(uint8_t motor)
{
if (!have_motor_map) {
if (SRV_Channels::find_channel(SRV_Channel::k_motor1, motor_map[0]) &&
SRV_Channels::find_channel(SRV_Channel::k_motor2, motor_map[1]) &&
SRV_Channels::find_channel(SRV_Channel::k_motor3, motor_map[2]) &&
SRV_Channels::find_channel(SRV_Channel::k_motor4, motor_map[3])) {
have_motor_map = true;
}
}
if (!have_motor_map) {
return 0;
}
// this currently assumes first 4 channels.
uint16_t pwm = hal.rcout->read_last_sent(motor_map[motor]);
// get 0 to 1 motor demand
float output = (hal.rcout->scale_esc_to_unity(pwm)+1) * 0.5;
if (output <= 0) {
return 0;
}
// scale for voltage
output *= voltage;
// apply expo correction
output = powf(output, expo);
return output;
}
// per-motor calibration update
void Compass_PerMotor::calibration_start(void)
{
for (uint8_t i=0; i<4; i++) {
field_sum[i].zero();
output_sum[i] = 0;
count[i] = 0;
start_ms[i] = 0;
}
// we need to ensure we get current data by throwing away several
// samples. The offsets may have just changed from an offset
// calibration
for (uint8_t i=0; i<4; i++) {
compass.read();
hal.scheduler->delay(50);
}
base_field = compass.get_field(0);
running = true;
}
// per-motor calibration update
void Compass_PerMotor::calibration_update(void)
{
uint32_t now = AP_HAL::millis();
// accumulate per-motor sums
for (uint8_t i=0; i<4; i++) {
float output = scaled_output(i);
if (output <= 0) {
// motor is off
start_ms[i] = 0;
continue;
}
if (start_ms[i] == 0) {
start_ms[i] = now;
}
if (now - start_ms[i] < 500) {
// motor must run for 0.5s to settle
continue;
}
// accumulate a sample
field_sum[i] += compass.get_field(0);
output_sum[i] += output;
count[i]++;
}
}
// calculate per-motor calibration values
void Compass_PerMotor::calibration_end(void)
{
for (uint8_t i=0; i<4; i++) {
if (count[i] == 0) {
continue;
}
// calculate effective output
float output = output_sum[i] / count[i];
// calculate amount that field changed from base field
Vector3f field_change = base_field - (field_sum[i] / count[i]);
if (output <= 0) {
continue;
}
Vector3f c = field_change / output;
compensation[i].set_and_save(c);
}
// enable per-motor compensation
enable.set_and_save(1);
running = false;
}
/*
calculate total offset for per-motor compensation
*/
void Compass_PerMotor::compensate(Vector3f &offset)
{
offset.zero();
if (running) {
// don't compensate while calibrating
return;
}
for (uint8_t i=0; i<4; i++) {
float output = scaled_output(i);
const Vector3f &c = compensation[i].get();
offset += c * output;
}
}

69
libraries/AP_Compass/Compass_PerMotor.h Normal file
View File

@ -0,0 +1,69 @@
/*
per-motor compass compensation
*/
#include <AP_Math/AP_Math.h>
class Compass;
// per-motor compass compensation class. Currently tied to quadcopters
// only, and single magnetometer
class Compass_PerMotor {
public:
static const struct AP_Param::GroupInfo var_info[];
Compass_PerMotor(Compass &_compass);
bool enabled(void) const {
return enable.get() != 0;
}
void set_voltage(float _voltage) {
// simple low-pass on voltage
voltage = 0.9 * voltage + 0.1 * _voltage;
}
void calibration_start(void);
void calibration_update(void);
void calibration_end(void);
void compensate(Vector3f &offset);
private:
Compass &compass;
AP_Int8 enable;
AP_Float expo;
AP_Vector3f compensation[4];
// base field on test start
Vector3f base_field;
// sum of calibration field samples
Vector3f field_sum[4];
// sum of output (voltage*scaledpwm) in calibration
float output_sum[4];
// count of calibration accumulation
uint16_t count[4];
// time a motor started in milliseconds
uint32_t start_ms[4];
// battery voltage
float voltage;
// what rcout channel is being calibrated
uint8_t channel;
// is calibration running?
bool running;
// get scaled motor ouput
float scaled_output(uint8_t motor);
// map of motors
bool have_motor_map;
uint8_t motor_map[4];
};