From ca30f6aec887b4448701de7aefefa60c0ca0318a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 5 Jan 2018 21:08:04 +1100 Subject: [PATCH] AP_Compass: added per-motor compass calibration this allows for a motor calibration vector per motor --- libraries/AP_Compass/AP_Compass.cpp | 4 + libraries/AP_Compass/AP_Compass.h | 27 +++ libraries/AP_Compass/AP_Compass_Backend.cpp | 34 ++- libraries/AP_Compass/Compass_PerMotor.cpp | 230 ++++++++++++++++++++ libraries/AP_Compass/Compass_PerMotor.h | 69 ++++++ 5 files changed, 354 insertions(+), 10 deletions(-) create mode 100644 libraries/AP_Compass/Compass_PerMotor.cpp create mode 100644 libraries/AP_Compass/Compass_PerMotor.h diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index daaf7425d4..0458e54abb 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -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 diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h index 0f605f7b77..7d8f957758 100644 --- a/libraries/AP_Compass/AP_Compass.h +++ b/libraries/AP_Compass/AP_Compass.h @@ -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; diff --git a/libraries/AP_Compass/AP_Compass_Backend.cpp b/libraries/AP_Compass/AP_Compass_Backend.cpp index 8d52916109..6f5ecac6d6 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.cpp +++ b/libraries/AP_Compass/AP_Compass_Backend.cpp @@ -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; } /* diff --git a/libraries/AP_Compass/Compass_PerMotor.cpp b/libraries/AP_Compass/Compass_PerMotor.cpp new file mode 100644 index 0000000000..bd0985a020 --- /dev/null +++ b/libraries/AP_Compass/Compass_PerMotor.cpp @@ -0,0 +1,230 @@ +/* + per-motor compass compensation + */ + +#include "AP_Compass.h" +#include + +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; + } +} diff --git a/libraries/AP_Compass/Compass_PerMotor.h b/libraries/AP_Compass/Compass_PerMotor.h new file mode 100644 index 0000000000..384828e9cd --- /dev/null +++ b/libraries/AP_Compass/Compass_PerMotor.h @@ -0,0 +1,69 @@ +/* + per-motor compass compensation + */ + +#include + + +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]; +}; +