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

AP_Motors: move thrust linerization to its own class

This commit is contained in:
Iampete1 2023-01-31 01:54:00 +00:00 committed by Andrew Tridgell
parent 5d68f44694
commit ab4777de14
13 changed files with 225 additions and 179 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_Motors/AP_MotorsCoax.cpp
View File

@ -91,8 +91,8 @@ void AP_MotorsCoax::output_to_motors()
for (uint8_t i = 0; i < NUM_ACTUATORS; i++) {
rc_write_angle(AP_MOTORS_MOT_1 + i, _actuator_out[i] * AP_MOTORS_COAX_SERVO_INPUT_RANGE);
}
set_actuator_with_slew(_actuator[AP_MOTORS_MOT_5], thrust_to_actuator(_thrust_yt_ccw));
set_actuator_with_slew(_actuator[AP_MOTORS_MOT_6], thrust_to_actuator(_thrust_yt_cw));
set_actuator_with_slew(_actuator[AP_MOTORS_MOT_5], thr_lin.thrust_to_actuator(_thrust_yt_ccw));
set_actuator_with_slew(_actuator[AP_MOTORS_MOT_6], thr_lin.thrust_to_actuator(_thrust_yt_cw));
rc_write(AP_MOTORS_MOT_5, output_to_pwm(_actuator[AP_MOTORS_MOT_5]));
rc_write(AP_MOTORS_MOT_6, output_to_pwm(_actuator[AP_MOTORS_MOT_6]));
break;
@ -129,7 +129,7 @@ void AP_MotorsCoax::output_armed_stabilizing()
float actuator_allowed = 0.0f; // amount of yaw we can fit in
// apply voltage and air pressure compensation
const float compensation_gain = get_compensation_gain();
const float compensation_gain = thr_lin.get_compensation_gain();
roll_thrust = (_roll_in + _roll_in_ff) * compensation_gain;
pitch_thrust = (_pitch_in + _pitch_in_ff) * compensation_gain;
yaw_thrust = (_yaw_in + _yaw_in_ff) * compensation_gain;

6
libraries/AP_Motors/AP_MotorsMatrix.cpp
View File

@ -168,7 +168,7 @@ void AP_MotorsMatrix::output_to_motors()
// set motor output based on thrust requests
for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
set_actuator_with_slew(_actuator[i], thrust_to_actuator(_thrust_rpyt_out[i]));
set_actuator_with_slew(_actuator[i], thr_lin.thrust_to_actuator(_thrust_rpyt_out[i]));
}
}
break;
@ -213,7 +213,7 @@ float AP_MotorsMatrix::boost_ratio(float boost_value, float normal_value) const
void AP_MotorsMatrix::output_armed_stabilizing()
{
// apply voltage and air pressure compensation
const float compensation_gain = get_compensation_gain(); // compensation for battery voltage and altitude
const float compensation_gain = thr_lin.get_compensation_gain(); // compensation for battery voltage and altitude
// pitch thrust input value, +/- 1.0
const float roll_thrust = (_roll_in + _roll_in_ff) * compensation_gain;
@ -454,7 +454,7 @@ void AP_MotorsMatrix::check_for_failed_motor(float throttle_thrust_best_plus_adj
}
// check to see if thrust boost is using more throttle than _throttle_thrust_max
if ((_throttle_thrust_max * get_compensation_gain() > throttle_thrust_best_plus_adj) && (rpyt_high < 0.9f) && _thrust_balanced) {
if ((_throttle_thrust_max * thr_lin.get_compensation_gain() > throttle_thrust_best_plus_adj) && (rpyt_high < 0.9f) && _thrust_balanced) {
_thrust_boost = false;
}
}

8
libraries/AP_Motors/AP_MotorsMatrix_6DoF_Scripting.cpp
View File

@ -46,17 +46,17 @@ void AP_MotorsMatrix_6DoF_Scripting::output_to_motors()
if (_reversible[i]) {
// revesible motor can provide both positive and negative thrust, +- spin max, spin min does not apply
if (is_positive(_thrust_rpyt_out[i])) {
_actuator[i] = apply_thrust_curve_and_volt_scaling(_thrust_rpyt_out[i]) * _spin_max;
_actuator[i] = thr_lin.apply_thrust_curve_and_volt_scaling(_thrust_rpyt_out[i]) * thr_lin.get_spin_max();
} else if (is_negative(_thrust_rpyt_out[i])) {
_actuator[i] = -apply_thrust_curve_and_volt_scaling(-_thrust_rpyt_out[i]) * _spin_max;
_actuator[i] = -thr_lin.apply_thrust_curve_and_volt_scaling(-_thrust_rpyt_out[i]) * thr_lin.get_spin_max();
} else {
_actuator[i] = 0.0f;
}
} else {
// motor can only provide trust in a single direction, spin min to spin max as 'normal' copter
_actuator[i] = thrust_to_actuator(_thrust_rpyt_out[i]);
_actuator[i] = thr_lin.thrust_to_actuator(_thrust_rpyt_out[i]);
}
}
}
@ -85,7 +85,7 @@ void AP_MotorsMatrix_6DoF_Scripting::output_armed_stabilizing()
// note that the throttle, forwards and right inputs are not in bodyframe, they are in the frame of the 'normal' 4DoF copter were pretending to be
// apply voltage and air pressure compensation
const float compensation_gain = get_compensation_gain(); // compensation for battery voltage and altitude
const float compensation_gain = thr_lin.get_compensation_gain(); // compensation for battery voltage and altitude
roll_thrust = (_roll_in + _roll_in_ff) * compensation_gain;
pitch_thrust = (_pitch_in + _pitch_in_ff) * compensation_gain;
yaw_thrust = (_yaw_in + _yaw_in_ff) * compensation_gain;

137
libraries/AP_Motors/AP_MotorsMulticopter.cpp
View File

@ -49,14 +49,14 @@ const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = {
// @Description: Motor thrust curve exponent (0.0 for linear to 1.0 for second order curve)
// @Range: -1.0 1.0
// @User: Advanced
AP_GROUPINFO("THST_EXPO", 8, AP_MotorsMulticopter, _thrust_curve_expo, AP_MOTORS_THST_EXPO_DEFAULT),
AP_GROUPINFO("THST_EXPO", 8, AP_MotorsMulticopter, thr_lin.curve_expo, AP_MOTORS_THST_EXPO_DEFAULT),
// @Param: SPIN_MAX
// @DisplayName: Motor Spin maximum
// @Description: Point at which the thrust saturates expressed as a number from 0 to 1 in the entire output range
// @Values: 0.9:Low, 0.95:Default, 1.0:High
// @User: Advanced
AP_GROUPINFO("SPIN_MAX", 9, AP_MotorsMulticopter, _spin_max, AP_MOTORS_SPIN_MAX_DEFAULT),
AP_GROUPINFO("SPIN_MAX", 9, AP_MotorsMulticopter, thr_lin.spin_max, AP_MOTORS_SPIN_MAX_DEFAULT),
// @Param: BAT_VOLT_MAX
// @DisplayName: Battery voltage compensation maximum voltage
@ -64,7 +64,7 @@ const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = {
// @Range: 6 53
// @Units: V
// @User: Advanced
AP_GROUPINFO("BAT_VOLT_MAX", 10, AP_MotorsMulticopter, _batt_voltage_max, AP_MOTORS_BAT_VOLT_MAX_DEFAULT),
AP_GROUPINFO("BAT_VOLT_MAX", 10, AP_MotorsMulticopter, thr_lin.batt_voltage_max, AP_MOTORS_BAT_VOLT_MAX_DEFAULT),
// @Param: BAT_VOLT_MIN
// @DisplayName: Battery voltage compensation minimum voltage
@ -72,7 +72,7 @@ const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = {
// @Range: 6 42
// @Units: V
// @User: Advanced
AP_GROUPINFO("BAT_VOLT_MIN", 11, AP_MotorsMulticopter, _batt_voltage_min, AP_MOTORS_BAT_VOLT_MIN_DEFAULT),
AP_GROUPINFO("BAT_VOLT_MIN", 11, AP_MotorsMulticopter, thr_lin.batt_voltage_min, AP_MOTORS_BAT_VOLT_MIN_DEFAULT),
// @Param: BAT_CURR_MAX
// @DisplayName: Motor Current Max
@ -113,7 +113,7 @@ const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = {
// @Description: Point at which the thrust starts expressed as a number from 0 to 1 in the entire output range. Should be higher than MOT_SPIN_ARM.
// @Values: 0.0:Low, 0.15:Default, 0.25:High
// @User: Advanced
AP_GROUPINFO("SPIN_MIN", 18, AP_MotorsMulticopter, _spin_min, AP_MOTORS_SPIN_MIN_DEFAULT),
AP_GROUPINFO("SPIN_MIN", 18, AP_MotorsMulticopter, thr_lin.spin_min, AP_MOTORS_SPIN_MIN_DEFAULT),
// @Param: SPIN_ARM
// @DisplayName: Motor Spin armed
@ -186,7 +186,7 @@ const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = {
// @Description: Which battery monitor should be used for doing compensation
// @Values: 0:First battery, 1:Second battery
// @User: Advanced
AP_GROUPINFO("BAT_IDX", 39, AP_MotorsMulticopter, _batt_idx, 0),
AP_GROUPINFO("BAT_IDX", 39, AP_MotorsMulticopter, thr_lin.batt_idx, 0),
// @Param: SLEW_UP_TIME
// @DisplayName: Output slew time for increasing throttle
@ -221,15 +221,9 @@ const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = {
// Constructor
AP_MotorsMulticopter::AP_MotorsMulticopter(uint16_t speed_hz) :
AP_Motors(speed_hz),
_lift_max(1.0f),
_throttle_limit(1.0f)
{
AP_Param::setup_object_defaults(this, var_info);
// setup battery voltage filtering
_batt_voltage_filt.set_cutoff_frequency(AP_MOTORS_BATT_VOLT_FILT_HZ);
_batt_voltage_filt.reset(1.0f);
};
// output - sends commands to the motors
@ -239,7 +233,7 @@ void AP_MotorsMulticopter::output()
update_throttle_filter();
// calc filtered battery voltage and lift_max
update_lift_max_from_batt_voltage();
thr_lin.update_lift_max_from_batt_voltage();
// run spool logic
output_logic();
@ -337,16 +331,17 @@ float AP_MotorsMulticopter::get_current_limit_max_throttle()
{
AP_BattMonitor &battery = AP::battery();
const uint8_t batt_idx = thr_lin.get_battery_index();
float _batt_current;
if (_batt_current_max <= 0 || // return maximum if current limiting is disabled
!armed() || // remove throttle limit if disarmed
!battery.current_amps(_batt_current, _batt_idx)) { // no current monitoring is available
!battery.current_amps(_batt_current, batt_idx)) { // no current monitoring is available
_throttle_limit = 1.0f;
return 1.0f;
}
float _batt_resistance = battery.get_resistance(_batt_idx);
float _batt_resistance = battery.get_resistance(batt_idx);
if (is_zero(_batt_resistance)) {
_throttle_limit = 1.0f;
@ -354,7 +349,7 @@ float AP_MotorsMulticopter::get_current_limit_max_throttle()
}
// calculate the maximum current to prevent voltage sag below _batt_voltage_min
float batt_current_max = MIN(_batt_current_max, _batt_current + (battery.voltage(_batt_idx) - _batt_voltage_min) / _batt_resistance);
float batt_current_max = MIN(_batt_current_max, _batt_current + (battery.voltage(batt_idx) - thr_lin.get_battery_min_voltage()) / _batt_resistance);
float batt_current_ratio = _batt_current / batt_current_max;
@ -367,67 +362,6 @@ float AP_MotorsMulticopter::get_current_limit_max_throttle()
return get_throttle_hover() + ((1.0 - get_throttle_hover()) * _throttle_limit);
}
// apply_thrust_curve_and_volt_scaling - returns throttle in the range 0 ~ 1
float AP_MotorsMulticopter::apply_thrust_curve_and_volt_scaling(float thrust) const
{
float battery_scale = 1.0;
if (is_positive(_batt_voltage_filt.get())) {
battery_scale = 1.0 / _batt_voltage_filt.get();
}
// apply thrust curve - domain -1.0 to 1.0, range -1.0 to 1.0
float thrust_curve_expo = constrain_float(_thrust_curve_expo, -1.0f, 1.0f);
if (is_zero(thrust_curve_expo)) {
// zero expo means linear, avoid floating point exception for small values
return _lift_max * thrust * battery_scale;
}
float throttle_ratio = ((thrust_curve_expo - 1.0f) + safe_sqrt((1.0f - thrust_curve_expo) * (1.0f - thrust_curve_expo) + 4.0f * thrust_curve_expo * _lift_max * thrust)) / (2.0f * thrust_curve_expo);
return constrain_float(throttle_ratio * battery_scale, 0.0f, 1.0f);
}
// inverse of above, tested with AP_Motors/examples/expo_inverse_test
// used to calculate equivelent motor throttle level to direct ouput, used in tailsitter transtions
float AP_MotorsMulticopter::remove_thrust_curve_and_volt_scaling(float throttle) const
{
float battery_scale = 1.0;
if (is_positive(_batt_voltage_filt.get())) {
battery_scale = 1.0 / _batt_voltage_filt.get();
}
// apply thrust curve - domain -1.0 to 1.0, range -1.0 to 1.0
float thrust_curve_expo = constrain_float(_thrust_curve_expo, -1.0f, 1.0f);
if (is_zero(thrust_curve_expo)) {
// zero expo means linear, avoid floating point exception for small values
return throttle / (_lift_max * battery_scale);
}
float thrust = ((throttle / battery_scale) * (2.0f * thrust_curve_expo)) - (thrust_curve_expo - 1.0f);
thrust = (thrust * thrust) - ((1.0f - thrust_curve_expo) * (1.0f - thrust_curve_expo));
thrust /= 4.0f * thrust_curve_expo * _lift_max;
return constrain_float(thrust, 0.0f, 1.0f);
}
// update_lift_max from battery voltage - used for voltage compensation
void AP_MotorsMulticopter::update_lift_max_from_batt_voltage()
{
// sanity check battery_voltage_min is not too small
// if disabled or misconfigured exit immediately
float _batt_voltage_resting_estimate = AP::battery().voltage_resting_estimate(_batt_idx);
if ((_batt_voltage_max <= 0) || (_batt_voltage_min >= _batt_voltage_max) || (_batt_voltage_resting_estimate < 0.25f * _batt_voltage_min)) {
_batt_voltage_filt.reset(1.0f);
_lift_max = 1.0f;
return;
}
_batt_voltage_min.set(MAX(_batt_voltage_min, _batt_voltage_max * 0.6f));
// contrain resting voltage estimate (resting voltage is actual voltage with sag removed based on current draw and resistance)
_batt_voltage_resting_estimate = constrain_float(_batt_voltage_resting_estimate, _batt_voltage_min, _batt_voltage_max);
// filter at 0.5 Hz
float batt_voltage_filt = _batt_voltage_filt.apply(_batt_voltage_resting_estimate / _batt_voltage_max, _dt);
// calculate lift max
float thrust_curve_expo = constrain_float(_thrust_curve_expo, -1.0f, 1.0f);
_lift_max = batt_voltage_filt * (1 - thrust_curve_expo) + thrust_curve_expo * batt_voltage_filt * batt_voltage_filt;
}
// 10hz logging of voltage scaling and max trust
void AP_MotorsMulticopter::Log_Write()
@ -435,8 +369,8 @@ void AP_MotorsMulticopter::Log_Write()
const struct log_MotBatt pkt_mot {
LOG_PACKET_HEADER_INIT(LOG_MOTBATT_MSG),
time_us : AP_HAL::micros64(),
lift_max : _lift_max,
bat_volt : _batt_voltage_filt.get(),
lift_max : thr_lin.get_lift_max(),
bat_volt : thr_lin.batt_voltage_filt.get(),
th_limit : _throttle_limit,
th_average_max : _throttle_avg_max,
th_out : _throttle_out,
@ -445,24 +379,6 @@ void AP_MotorsMulticopter::Log_Write()
AP::logger().WriteBlock(&pkt_mot, sizeof(pkt_mot));
}
float AP_MotorsMulticopter::get_compensation_gain() const
{
// avoid divide by zero
if (_lift_max <= 0.0f) {
return 1.0f;
}
float ret = 1.0f / _lift_max;
#if AP_MOTORS_DENSITY_COMP == 1
// air density ratio is increasing in density / decreasing in altitude
if (_air_density_ratio > 0.3f && _air_density_ratio < 1.5f) {
ret *= 1.0f / constrain_float(_air_density_ratio, 0.5f, 1.25f);
}
#endif
return ret;
}
// convert actuator output (0~1) range to pwm range
int16_t AP_MotorsMulticopter::output_to_pwm(float actuator)
{
@ -482,21 +398,6 @@ int16_t AP_MotorsMulticopter::output_to_pwm(float actuator)
return pwm_output;
}
// converts desired thrust to linearized actuator output in a range of 0~1
float AP_MotorsMulticopter::thrust_to_actuator(float thrust_in) const
{
thrust_in = constrain_float(thrust_in, 0.0f, 1.0f);
return _spin_min + (_spin_max - _spin_min) * apply_thrust_curve_and_volt_scaling(thrust_in);
}
// inverse of above, tested with AP_Motors/examples/expo_inverse_test
// used to calculate equivelent motor throttle level to direct ouput, used in tailsitter transtions
float AP_MotorsMulticopter::actuator_to_thrust(float actuator) const
{
actuator = (actuator - _spin_min) / (_spin_max - _spin_min);
return constrain_float(remove_thrust_curve_and_volt_scaling(actuator), 0.0f, 1.0f);
}
// adds slew rate limiting to actuator output
void AP_MotorsMulticopter::set_actuator_with_slew(float& actuator_output, float input)
{
@ -530,7 +431,7 @@ void AP_MotorsMulticopter::set_actuator_with_slew(float& actuator_output, float
// gradually increase actuator output to spin_min
float AP_MotorsMulticopter::actuator_spin_up_to_ground_idle() const
{
return constrain_float(_spin_up_ratio, 0.0f, 1.0f) * _spin_min;
return constrain_float(_spin_up_ratio, 0.0f, 1.0f) * thr_lin.get_spin_min();
}
// parameter checks for MOT_PWM_MIN/MAX, returns true if parameters are valid
@ -655,8 +556,8 @@ void AP_MotorsMulticopter::output_logic()
case DesiredSpoolState::GROUND_IDLE:
float spin_up_armed_ratio = 0.0f;
if (_spin_min > 0.0f) {
spin_up_armed_ratio = _spin_arm / _spin_min;
if (thr_lin.get_spin_min() > 0.0f) {
spin_up_armed_ratio = _spin_arm / thr_lin.get_spin_min();
}
_spin_up_ratio += constrain_float(spin_up_armed_ratio - _spin_up_ratio, -spool_step, spool_step);
break;
@ -858,11 +759,11 @@ bool AP_MotorsMulticopter::arming_checks(size_t buflen, char *buffer) const
}
// Check param config
if (_spin_min > 0.3) {
hal.util->snprintf(buffer, buflen, "%sSPIN_MIN too high %.2f > 0.3", AP_MOTORS_PARAM_PREFIX, _spin_min.get());
if (thr_lin.get_spin_min() > 0.3) {
hal.util->snprintf(buffer, buflen, "%sSPIN_MIN too high %.2f > 0.3", AP_MOTORS_PARAM_PREFIX, thr_lin.get_spin_min());
return false;
}
if (_spin_arm > _spin_min) {
if (_spin_arm > thr_lin.get_spin_min()) {
hal.util->snprintf(buffer, buflen, "%sSPIN_ARM > %sSPIN_MIN", AP_MOTORS_PARAM_PREFIX, AP_MOTORS_PARAM_PREFIX);
return false;
}

34
libraries/AP_Motors/AP_MotorsMulticopter.h
View File

@ -3,10 +3,7 @@
#pragma once
#include "AP_Motors_Class.h"
#ifndef AP_MOTORS_DENSITY_COMP
#define AP_MOTORS_DENSITY_COMP 1
#endif
#include "AP_Motors_Thrust_Linearization.h"
#define AP_MOTORS_YAW_HEADROOM_DEFAULT 200
#define AP_MOTORS_THST_EXPO_DEFAULT 0.65f // set to 0 for linear and 1 for second order approximation
@ -21,7 +18,6 @@
#define AP_MOTORS_BAT_VOLT_MIN_DEFAULT 0.0f // voltage limiting min default (voltage dropping below this level will have no effect)
#define AP_MOTORS_BAT_CURR_MAX_DEFAULT 0.0f // current limiting max default
#define AP_MOTORS_BAT_CURR_TC_DEFAULT 5.0f // Time constant used to limit the maximum current
#define AP_MOTORS_BATT_VOLT_FILT_HZ 0.5f // battery voltage filtered at 0.5hz
#define AP_MOTORS_SLEW_TIME_DEFAULT 0.0f // slew rate limit for thrust output
#define AP_MOTORS_SAFE_TIME_DEFAULT 1.0f // Time for the esc when transitioning between zero pwm to minimum
@ -77,12 +73,6 @@ public:
// parameter check for MOT_PWM_MIN/MAX, returns true if parameters are valid
bool check_mot_pwm_params() const;
// converts desired thrust to linearized actuator output in a range of 0~1
float thrust_to_actuator(float thrust_in) const;
// inverse of above
float actuator_to_thrust(float actuator) const;
// set thrust compensation callback
FUNCTOR_TYPEDEF(thrust_compensation_fn_t, void, float *, uint8_t);
void set_thrust_compensation_callback(thrust_compensation_fn_t callback) {
@ -109,6 +99,9 @@ public:
float get_throttle_avg_max() const;
int16_t get_yaw_headroom() const;
// Thrust Linearization handling
Thrust_Linearization thr_lin {*this};
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];
@ -126,17 +119,6 @@ protected:
// return current_limit as a number from 0 ~ 1 in the range throttle_min to throttle_max
virtual float get_current_limit_max_throttle();
// apply_thrust_curve_and_volt_scaling - returns throttle in the range 0 ~ 1
float apply_thrust_curve_and_volt_scaling(float thrust) const;
// inverse of above
float remove_thrust_curve_and_volt_scaling(float throttle) const;
// update_lift_max_from_batt_voltage - used for voltage compensation
void update_lift_max_from_batt_voltage();
// return gain scheduling gain based on voltage and air density
float get_compensation_gain() const;
// convert actuator output (0~1) range to pwm range
int16_t output_to_pwm(float _actuator_output);
@ -170,18 +152,12 @@ protected:
// parameters
AP_Int16 _yaw_headroom; // yaw control is given at least this pwm range
AP_Float _thrust_curve_expo; // curve used to linearize pwm to thrust conversion. set to 0 for linear and 1 for second order approximation
AP_Float _slew_up_time; // throttle increase slew limitting
AP_Float _slew_dn_time; // throttle decrease slew limitting
AP_Float _safe_time; // Time for the esc when transitioning between zero pwm to minimum
AP_Float _spin_min; // throttle out ratio which produces the minimum thrust. (i.e. 0 ~ 1 ) of the full throttle range
AP_Float _spin_max; // throttle out ratio which produces the maximum thrust. (i.e. 0 ~ 1 ) of the full throttle range
AP_Float _spin_arm; // throttle out ratio which produces the armed spin rate. (i.e. 0 ~ 1 ) of the full throttle range
AP_Float _batt_voltage_max; // maximum voltage used to scale lift
AP_Float _batt_voltage_min; // minimum voltage used to scale lift
AP_Float _batt_current_max; // current over which maximum throttle is limited
AP_Float _batt_current_time_constant; // Time constant used to limit the maximum current
AP_Int8 _batt_idx; // battery index used for compensation
AP_Int16 _pwm_min; // minimum PWM value that will ever be output to the motors (if 0, vehicle's throttle input channel's min pwm used)
AP_Int16 _pwm_max; // maximum PWM value that will ever be output to the motors (if 0, vehicle's throttle input channel's max pwm used)
AP_Float _throttle_hover; // estimated throttle required to hover throttle in the range 0 ~ 1
@ -204,8 +180,6 @@ protected:
float _spin_up_ratio; // throttle percentage (0 ~ 1) between zero and throttle_min
// battery voltage, current and air pressure compensation variables
LowPassFilterFloat _batt_voltage_filt; // filtered battery voltage expressed as a percentage (0 ~ 1.0) of batt_voltage_max
float _lift_max; // maximum lift ratio from battery voltage
float _throttle_limit; // ratio of throttle limit between hover and maximum
float _throttle_thrust_max; // the maximum allowed throttle thrust 0.0 to 1.0 in the range throttle_min to throttle_max
float _disarm_safe_timer; // Timer for the esc when transitioning between zero pwm to minimum

6
libraries/AP_Motors/AP_MotorsSingle.cpp
View File

@ -94,8 +94,8 @@ void AP_MotorsSingle::output_to_motors()
for (uint8_t i = 0; i < NUM_ACTUATORS; i++) {
rc_write_angle(AP_MOTORS_MOT_1 + i, _actuator_out[i] * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
}
set_actuator_with_slew(_actuator[AP_MOTORS_MOT_5], thrust_to_actuator(_thrust_out));
set_actuator_with_slew(_actuator[AP_MOTORS_MOT_6], thrust_to_actuator(_thrust_out));
set_actuator_with_slew(_actuator[AP_MOTORS_MOT_5], thr_lin.thrust_to_actuator(_thrust_out));
set_actuator_with_slew(_actuator[AP_MOTORS_MOT_6], thr_lin.thrust_to_actuator(_thrust_out));
rc_write(AP_MOTORS_MOT_5, output_to_pwm(_actuator[AP_MOTORS_MOT_5]));
rc_write(AP_MOTORS_MOT_6, output_to_pwm(_actuator[AP_MOTORS_MOT_6]));
break;
@ -134,7 +134,7 @@ void AP_MotorsSingle::output_armed_stabilizing()
float actuator_max = 0.0f; // maximum actuator value
// apply voltage and air pressure compensation
const float compensation_gain = get_compensation_gain();
const float compensation_gain = thr_lin.get_compensation_gain();
roll_thrust = (_roll_in + _roll_in_ff) * compensation_gain;
pitch_thrust = (_pitch_in + _pitch_in_ff) * compensation_gain;
yaw_thrust = (_yaw_in + _yaw_in_ff) * compensation_gain;

8
libraries/AP_Motors/AP_MotorsTailsitter.cpp
View File

@ -95,9 +95,9 @@ void AP_MotorsTailsitter::output_to_motors()
case SpoolState::SPOOLING_UP:
case SpoolState::THROTTLE_UNLIMITED:
case SpoolState::SPOOLING_DOWN:
set_actuator_with_slew(_actuator[0], thrust_to_actuator(_thrust_left));
set_actuator_with_slew(_actuator[1], thrust_to_actuator(_thrust_right));
set_actuator_with_slew(_actuator[2], thrust_to_actuator(_throttle));
set_actuator_with_slew(_actuator[0], thr_lin.thrust_to_actuator(_thrust_left));
set_actuator_with_slew(_actuator[1], thr_lin.thrust_to_actuator(_thrust_right));
set_actuator_with_slew(_actuator[2], thr_lin.thrust_to_actuator(_throttle));
break;
}
@ -143,7 +143,7 @@ void AP_MotorsTailsitter::output_armed_stabilizing()
float thr_adj = 0.0f; // the difference between the pilot's desired throttle and throttle_thrust_best_rpy
// apply voltage and air pressure compensation
const float compensation_gain = get_compensation_gain();
const float compensation_gain = thr_lin.get_compensation_gain();
roll_thrust = (_roll_in + _roll_in_ff) * compensation_gain;
pitch_thrust = _pitch_in + _pitch_in_ff;
yaw_thrust = _yaw_in + _yaw_in_ff;

8
libraries/AP_Motors/AP_MotorsTri.cpp
View File

@ -109,9 +109,9 @@ void AP_MotorsTri::output_to_motors()
case SpoolState::THROTTLE_UNLIMITED:
case SpoolState::SPOOLING_DOWN:
// set motor output based on thrust requests
set_actuator_with_slew(_actuator[1], thrust_to_actuator(_thrust_right));
set_actuator_with_slew(_actuator[2], thrust_to_actuator(_thrust_left));
set_actuator_with_slew(_actuator[4], thrust_to_actuator(_thrust_rear));
set_actuator_with_slew(_actuator[1], thr_lin.thrust_to_actuator(_thrust_right));
set_actuator_with_slew(_actuator[2], thr_lin.thrust_to_actuator(_thrust_left));
set_actuator_with_slew(_actuator[4], thr_lin.thrust_to_actuator(_thrust_rear));
rc_write(AP_MOTORS_MOT_1, output_to_pwm(_actuator[1]));
rc_write(AP_MOTORS_MOT_2, output_to_pwm(_actuator[2]));
rc_write(AP_MOTORS_MOT_4, output_to_pwm(_actuator[4]));
@ -157,7 +157,7 @@ void AP_MotorsTri::output_armed_stabilizing()
_yaw_servo_angle_max_deg.set(constrain_float(_yaw_servo_angle_max_deg, AP_MOTORS_TRI_SERVO_RANGE_DEG_MIN, AP_MOTORS_TRI_SERVO_RANGE_DEG_MAX));
// apply voltage and air pressure compensation
const float compensation_gain = get_compensation_gain();
const float compensation_gain = thr_lin.get_compensation_gain();
roll_thrust = (_roll_in + _roll_in_ff) * compensation_gain;
pitch_thrust = (_pitch_in + _pitch_in_ff) * compensation_gain;
yaw_thrust = (_yaw_in + _yaw_in_ff) * compensation_gain * sinf(radians(_yaw_servo_angle_max_deg)); // we scale this so a thrust request of 1.0f will ask for full servo deflection at full rear throttle

3
libraries/AP_Motors/AP_Motors_Class.cpp
View File

@ -33,8 +33,7 @@ AP_Motors::AP_Motors(uint16_t speed_hz) :
_throttle_slew(),
_throttle_slew_filter(),
_spool_desired(DesiredSpoolState::SHUT_DOWN),
_spool_state(SpoolState::SHUT_DOWN),
_air_density_ratio(1.0f)
_spool_state(SpoolState::SHUT_DOWN)
{
_singleton = this;

6
libraries/AP_Motors/AP_Motors_Class.h
View File

@ -188,9 +188,6 @@ public:
// get_spool_state - get current spool state
enum SpoolState get_spool_state(void) const { return _spool_state; }
// set_density_ratio - sets air density as a proportion of sea level density
void set_air_density_ratio(float ratio) { _air_density_ratio = ratio; }
// set_dt / get_dt - dt is the time since the last time the motor mixers were updated
// _dt should be set based on the time of the last IMU read used by these controllers
// the motor mixers should run on each loop to ensure normal operation
@ -324,9 +321,6 @@ protected:
DesiredSpoolState _spool_desired; // desired spool state
SpoolState _spool_state; // current spool mode
// air pressure compensation variables
float _air_density_ratio; // air density / sea level density - decreases in altitude
// mask of what channels need fast output
uint32_t _motor_fast_mask;

118
libraries/AP_Motors/AP_Motors_Thrust_Linearization.cpp Normal file
View File

@ -0,0 +1,118 @@
#include "AP_Motors_Thrust_Linearization.h"
#include "AP_Motors_Class.h"
#include <AP_BattMonitor/AP_BattMonitor.h>
#include <AP_Baro/AP_Baro.h>
#define AP_MOTORS_BATT_VOLT_FILT_HZ 0.5 // battery voltage filtered at 0.5hz
#ifndef AP_MOTORS_DENSITY_COMP
#define AP_MOTORS_DENSITY_COMP 1
#endif
Thrust_Linearization::Thrust_Linearization(AP_Motors& _motors) :
motors(_motors),
lift_max(1.0)
{
// setup battery voltage filtering
batt_voltage_filt.set_cutoff_frequency(AP_MOTORS_BATT_VOLT_FILT_HZ);
batt_voltage_filt.reset(1.0);
}
// converts desired thrust to linearized actuator output in a range of 0~1
float Thrust_Linearization::thrust_to_actuator(float thrust_in) const
{
thrust_in = constrain_float(thrust_in, 0.0, 1.0);
return spin_min + (spin_max - spin_min) * apply_thrust_curve_and_volt_scaling(thrust_in);
}
// inverse of above, tested with AP_Motors/examples/expo_inverse_test
// used to calculate equivelent motor throttle level to direct ouput, used in tailsitter transtions
float Thrust_Linearization::actuator_to_thrust(float actuator) const
{
actuator = (actuator - spin_min) / (spin_max - spin_min);
return constrain_float(remove_thrust_curve_and_volt_scaling(actuator), 0.0, 1.0);
}
// apply_thrust_curve_and_volt_scaling - returns throttle in the range 0 ~ 1
float Thrust_Linearization::apply_thrust_curve_and_volt_scaling(float thrust) const
{
float battery_scale = 1.0;
if (is_positive(batt_voltage_filt.get())) {
battery_scale = 1.0 / batt_voltage_filt.get();
}
// apply thrust curve - domain -1.0 to 1.0, range -1.0 to 1.0
float thrust_curve_expo = constrain_float(curve_expo, -1.0, 1.0);
if (is_zero(thrust_curve_expo)) {
// zero expo means linear, avoid floating point exception for small values
return lift_max * thrust * battery_scale;
}
float throttle_ratio = ((thrust_curve_expo - 1.0) + safe_sqrt((1.0 - thrust_curve_expo) * (1.0 - thrust_curve_expo) + 4.0 * thrust_curve_expo * lift_max * thrust)) / (2.0 * thrust_curve_expo);
return constrain_float(throttle_ratio * battery_scale, 0.0, 1.0);
}
// inverse of above, tested with AP_Motors/examples/expo_inverse_test
// used to calculate equivelent motor throttle level to direct ouput, used in tailsitter transtions
float Thrust_Linearization::remove_thrust_curve_and_volt_scaling(float throttle) const
{
float battery_scale = 1.0;
if (is_positive(batt_voltage_filt.get())) {
battery_scale = 1.0 / batt_voltage_filt.get();
}
// apply thrust curve - domain -1.0 to 1.0, range -1.0 to 1.0
float thrust_curve_expo = constrain_float(curve_expo, -1.0, 1.0);
if (is_zero(thrust_curve_expo)) {
// zero expo means linear, avoid floating point exception for small values
return throttle / (lift_max * battery_scale);
}
float thrust = ((throttle / battery_scale) * (2.0 * thrust_curve_expo)) - (thrust_curve_expo - 1.0);
thrust = (thrust * thrust) - ((1.0 - thrust_curve_expo) * (1.0 - thrust_curve_expo));
thrust /= 4.0 * thrust_curve_expo * lift_max;
return constrain_float(thrust, 0.0, 1.0);
}
// update_lift_max from battery voltage - used for voltage compensation
void Thrust_Linearization::update_lift_max_from_batt_voltage()
{
// sanity check battery_voltage_min is not too small
// if disabled or misconfigured exit immediately
float _batt_voltage_resting_estimate = AP::battery().voltage_resting_estimate(batt_idx);
if ((batt_voltage_max <= 0) || (batt_voltage_min >= batt_voltage_max) || (_batt_voltage_resting_estimate < 0.25 * batt_voltage_min)) {
batt_voltage_filt.reset(1.0);
lift_max = 1.0;
return;
}
batt_voltage_min.set(MAX(batt_voltage_min, batt_voltage_max * 0.6));
// contrain resting voltage estimate (resting voltage is actual voltage with sag removed based on current draw and resistance)
_batt_voltage_resting_estimate = constrain_float(_batt_voltage_resting_estimate, batt_voltage_min, batt_voltage_max);
// filter at 0.5 Hz
float batt_voltage_flittered = batt_voltage_filt.apply(_batt_voltage_resting_estimate / batt_voltage_max, motors.get_dt());
// calculate lift max
float thrust_curve_expo = constrain_float(curve_expo, -1.0, 1.0);
lift_max = batt_voltage_flittered * (1 - thrust_curve_expo) + thrust_curve_expo * batt_voltage_flittered * batt_voltage_flittered;
}
// return gain scheduling gain based on voltage and air density
float Thrust_Linearization::get_compensation_gain() const
{
// avoid divide by zero
if (get_lift_max() <= 0.0) {
return 1.0;
}
float ret = 1.0 / get_lift_max();
#if AP_MOTORS_DENSITY_COMP == 1
// air density ratio is increasing in density / decreasing in altitude
const float air_density_ratio = AP::baro().get_air_density_ratio();
if (air_density_ratio > 0.3 && air_density_ratio < 1.5) {
ret *= 1.0 / constrain_float(air_density_ratio, 0.5, 1.25);
}
#endif
return ret;
}

60
libraries/AP_Motors/AP_Motors_Thrust_Linearization.h Normal file
View File

@ -0,0 +1,60 @@
#pragma once
#include <AP_Param/AP_Param.h>
#include <Filter/LowPassFilter.h>
class AP_Motors;
class Thrust_Linearization {
friend class AP_MotorsMulticopter;
friend class AP_MotorsMulticopter_test;
public:
Thrust_Linearization(AP_Motors& _motors);
// Apply_thrust_curve_and_volt_scaling - returns throttle in the range 0 ~ 1
float apply_thrust_curve_and_volt_scaling(float thrust) const;
// Inverse of above
float remove_thrust_curve_and_volt_scaling(float throttle) const;
// Converts desired thrust to linearized actuator output in a range of 0~1
float thrust_to_actuator(float thrust_in) const;
// Inverse of above
float actuator_to_thrust(float actuator) const;
// Update_lift_max_from_batt_voltage - used for voltage compensation
void update_lift_max_from_batt_voltage();
// return gain scheduling gain based on voltage and air density
float get_compensation_gain() const;
// Get spin min parameter value
float get_spin_min() const { return spin_min.get(); }
// Get spin max parameter value
float get_spin_max() const { return spin_max.get(); }
// Get spin max parameter value
int8_t get_battery_index() const { return batt_idx.get(); }
// Get min battery voltage
float get_battery_min_voltage() const { return batt_voltage_min.get(); }
// Get lift max
float get_lift_max() const { return lift_max; }
protected:
AP_Float curve_expo; // curve used to linearize pwm to thrust conversion. set to 0 for linear and 1 for second order approximation
AP_Float spin_min; // throttle out ratio which produces the minimum thrust. (i.e. 0 ~ 1 ) of the full throttle range
AP_Float spin_max; // throttle out ratio which produces the maximum thrust. (i.e. 0 ~ 1 ) of the full throttle range
AP_Int8 batt_idx; // battery index used for compensation
AP_Float batt_voltage_max; // maximum voltage used to scale lift
AP_Float batt_voltage_min; // minimum voltage used to scale lift
private:
float lift_max; // maximum lift ratio from battery voltage
float throttle_limit; // ratio of throttle limit between hover and maximum
LowPassFilterFloat batt_voltage_filt; // filtered battery voltage expressed as a percentage (0 ~ 1.0) of batt_voltage_max
AP_Motors& motors;
};

4
libraries/AP_Motors/examples/expo_inverse_test/expo_inverse_test.cpp
View File

@ -44,7 +44,7 @@ public:
void output_to_motors() override {};
// helper function to allow setting of expo
void set_expo(float v) { _thrust_curve_expo.set(v); }
void set_expo(float v) { thr_lin.curve_expo.set(v); }
};
@ -74,7 +74,7 @@ void setup(void)
float throttle = 0.0;
while (throttle < 1.0+throttle_step*0.5) {
const float throttle_out = motors.actuator_to_thrust(motors.thrust_to_actuator(throttle));
const float throttle_out = motors.thr_lin.actuator_to_thrust(motors.thr_lin.thrust_to_actuator(throttle));
const double diff = fabsf(throttle_out - throttle);
if (diff > max_diff) {
max_diff_throttle = throttle;