ardupilot/libraries/AP_Motors/AP_Motors_Class.cpp

403 lines
14 KiB
C++

// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
* AP_Motors.cpp - ArduCopter motors library
* Code by RandyMackay. DIYDrones.com
*
*/
#include "AP_Motors_Class.h"
#include <AP_HAL.h>
extern const AP_HAL::HAL& hal;
// initialise motor map
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
const uint8_t AP_Motors::_motor_to_channel_map[AP_MOTORS_MAX_NUM_MOTORS] PROGMEM = {APM1_MOTOR_TO_CHANNEL_MAP};
#else
const uint8_t AP_Motors::_motor_to_channel_map[AP_MOTORS_MAX_NUM_MOTORS] PROGMEM = {APM2_MOTOR_TO_CHANNEL_MAP};
#endif
// parameters for the motor class
const AP_Param::GroupInfo AP_Motors::var_info[] PROGMEM = {
// 0 was used by TB_RATIO
// 1,2,3 were used by throttle curve
// @Param: SPIN_ARMED
// @DisplayName: Motors always spin when armed
// @Description: Controls whether motors always spin when armed (must be below THR_MIN)
// @Values: 0:Do Not Spin,70:VerySlow,100:Slow,130:Medium,150:Fast
// @User: Standard
AP_GROUPINFO("SPIN_ARMED", 5, AP_Motors, _spin_when_armed, AP_MOTORS_SPIN_WHEN_ARMED),
// @Param: YAW_HEADROOM
// @DisplayName: Matrix Yaw Min
// @Description: Yaw control is given at least this pwm range
// @Range: 0 500
// @Units: pwm
// @User: Advanced
AP_GROUPINFO("YAW_HEADROOM", 6, AP_Motors, _yaw_headroom, AP_MOTORS_YAW_HEADROOM_DEFAULT),
// 7 was THR_LOW_CMP
// @Param: THST_EXPO
// @DisplayName: Thrust Curve Expo
// @Description: Motor thrust curve exponent (from 0 for linear to 1.0 for second order curve)
// @Range: 0.25 0.8
// @User: Advanced
AP_GROUPINFO("THST_EXPO", 8, AP_Motors, _thrust_curve_expo, AP_MOTORS_THST_EXPO_DEFAULT),
// @Param: THST_MAX
// @DisplayName: Thrust Curve Max
// @Description: Point at which the thrust saturates
// @Values: 0.9:Low, 1.0:High
// @User: Advanced
AP_GROUPINFO("THST_MAX", 9, AP_Motors, _thrust_curve_max, AP_MOTORS_THST_MAX_DEFAULT),
// @Param: THST_BAT_MAX
// @DisplayName: Battery voltage compensation maximum voltage
// @Description: Battery voltage compensation maximum voltage (voltage above this will have no additional scaling effect on thrust). Recommend 4.4 * cell count, 0 = Disabled
// @Range: 6 35
// @Units: Volts
// @User: Advanced
AP_GROUPINFO("THST_BAT_MAX", 10, AP_Motors, _batt_voltage_max, AP_MOTORS_THST_BAT_MAX_DEFAULT),
// @Param: THST_BAT_MIN
// @DisplayName: Battery voltage compensation minimum voltage
// @Description: Battery voltage compensation minimum voltage (voltage below this will have no additional scaling effect on thrust). Recommend 3.5 * cell count, 0 = Disabled
// @Range: 6 35
// @Units: Volts
// @User: Advanced
AP_GROUPINFO("THST_BAT_MIN", 11, AP_Motors, _batt_voltage_min, AP_MOTORS_THST_BAT_MIN_DEFAULT),
// @Param: CURR_MAX
// @DisplayName: Motor Current Max
// @Description: Maximum current over which maximum throttle is limited (0 = Disabled)
// @Range: 0 200
// @Units: Amps
// @User: Advanced
AP_GROUPINFO("CURR_MAX", 12, AP_Motors, _batt_current_max, AP_MOTORS_CURR_MAX_DEFAULT),
// @Param: THR_MIX_MIN
// @DisplayName: Throttle Mix Minimum
// @Description: Minimum ratio that the average throttle can increase above the desired throttle after roll, pitch and yaw are mixed
// @Range: 0.1 0.25
// @User: Advanced
AP_GROUPINFO("THR_MIX_MIN", 13, AP_Motors, _thr_mix_min, AP_MOTORS_THR_MIX_MIN_DEFAULT),
AP_GROUPEND
};
// Constructor
AP_Motors::AP_Motors(RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_throttle, RC_Channel& rc_yaw, uint16_t loop_rate, uint16_t speed_hz) :
_rc_roll(rc_roll),
_rc_pitch(rc_pitch),
_rc_throttle(rc_throttle),
_rc_yaw(rc_yaw),
_loop_rate(loop_rate),
_speed_hz(speed_hz),
_min_throttle(AP_MOTORS_DEFAULT_MIN_THROTTLE),
_max_throttle(AP_MOTORS_DEFAULT_MAX_THROTTLE),
_hover_out(AP_MOTORS_DEFAULT_MID_THROTTLE),
_spin_when_armed_ramped(0),
_throttle_thr_mix(AP_MOTORS_THR_LOW_CMP_DEFAULT),
_throttle_thr_mix_desired(AP_MOTORS_THR_LOW_CMP_DEFAULT),
_batt_voltage(0.0f),
_batt_voltage_resting(0.0f),
_batt_current(0.0f),
_batt_current_resting(0.0f),
_batt_resistance(0.0f),
_batt_timer(0),
_air_density_ratio(1.0f),
_lift_max(1.0f),
_throttle_limit(1.0f),
_throttle_in(0.0f),
_throttle_filter()
{
AP_Param::setup_object_defaults(this, var_info);
// slow start motors from zero to min throttle
_flags.slow_start_low_end = true;
// setup battery voltage filtering
_batt_voltage_filt.set_cutoff_frequency(AP_MOTORS_BATT_VOLT_FILT_HZ);
_batt_voltage_filt.reset(1.0f);
// setup throttle filtering
_throttle_filter.set_cutoff_frequency(0.0f);
_throttle_filter.reset(0.0f);
};
void AP_Motors::armed(bool arm)
{
_flags.armed = arm;
if (!_flags.armed) {
_flags.slow_start_low_end = true;
}
AP_Notify::flags.armed = arm;
};
// set_min_throttle - sets the minimum throttle that will be sent to the engines when they're not off (i.e. to prevents issues with some motors spinning and some not at very low throttle)
void AP_Motors::set_min_throttle(uint16_t min_throttle)
{
_min_throttle = (float)min_throttle * (_rc_throttle.radio_max - _rc_throttle.radio_min) / 1000.0f;
}
// get_hover_throttle_as_pwm - converts hover throttle to pwm (i.e. range 1000 ~ 2000)
int16_t AP_Motors::get_hover_throttle_as_pwm() const
{
return (_rc_throttle.radio_min + (float)(_rc_throttle.radio_max - _rc_throttle.radio_min) * _hover_out / 1000.0f);
}
// throttle_pass_through - passes provided pwm directly to all motors - dangerous but used for initialising ESCs
// pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
void AP_Motors::throttle_pass_through(int16_t pwm)
{
if (armed()) {
// send the pilot's input directly to each enabled motor
for (int16_t i=0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[i]), pwm);
}
}
}
}
// output - sends commands to the motors
void AP_Motors::output()
{
// update throttle filter
update_throttle_filter();
// update max throttle
update_max_throttle();
// update battery resistance
update_battery_resistance();
// calc filtered battery voltage and lift_max
update_lift_max_from_batt_voltage();
// move throttle_low_comp towards desired throttle low comp
update_throttle_thr_mix();
if (_flags.armed) {
if (!_flags.interlock) {
output_armed_zero_throttle();
} else if (_flags.stabilizing) {
output_armed_stabilizing();
} else {
output_armed_not_stabilizing();
}
} else {
output_disarmed();
}
};
// slow_start - set to true to slew motors from current speed to maximum
// Note: this must be set immediately before a step up in throttle
void AP_Motors::slow_start(bool true_false)
{
// set slow_start flag
_flags.slow_start = true;
// initialise maximum throttle to current throttle
_max_throttle = constrain_int16(_rc_throttle.servo_out, 0, AP_MOTORS_DEFAULT_MAX_THROTTLE);
}
// update the throttle input filter
void AP_Motors::update_throttle_filter()
{
if (armed()) {
_throttle_filter.apply(_throttle_in, 1.0f/_loop_rate);
} else {
_throttle_filter.reset(0.0f);
}
// prevent _rc_throttle.servo_out from wrapping at int16 max or min
_rc_throttle.servo_out = constrain_float(_throttle_filter.get(),-32000,32000);
}
// update_max_throttle - updates the limits on _max_throttle if necessary taking into account slow_start_throttle flag
void AP_Motors::update_max_throttle()
{
// ramp up minimum spin speed if necessary
if (_flags.slow_start_low_end) {
_spin_when_armed_ramped += AP_MOTOR_SLOW_START_LOW_END_INCREMENT;
if (_spin_when_armed_ramped >= _spin_when_armed) {
_spin_when_armed_ramped = _spin_when_armed;
_flags.slow_start_low_end = false;
}
}
// implement slow start
if (_flags.slow_start) {
// increase slow start throttle
_max_throttle += AP_MOTOR_SLOW_START_INCREMENT;
// turn off slow start if we've reached max throttle
if (_max_throttle >= _rc_throttle.servo_out) {
_max_throttle = AP_MOTORS_DEFAULT_MAX_THROTTLE;
_flags.slow_start = false;
}
return;
}
// current limit throttle
current_limit_max_throttle();
}
// current_limit_max_throttle - limits maximum throttle based on current
void AP_Motors::current_limit_max_throttle()
{
// return maximum if current limiting is disabled
if (_batt_current_max <= 0) {
_throttle_limit = 1.0f;
_max_throttle = AP_MOTORS_DEFAULT_MAX_THROTTLE;
return;
}
// remove throttle limit if throttle is at zero or disarmed
if(_rc_throttle.servo_out <= 0 || !_flags.armed) {
_throttle_limit = 1.0f;
}
// limit throttle if over current
if (_batt_current > _batt_current_max*1.25f) {
// Fast drop for extreme over current (1 second)
_throttle_limit -= 1.0f/_loop_rate;
} else if(_batt_current > _batt_current_max) {
// Slow drop for extreme over current (5 second)
_throttle_limit -= 0.2f/_loop_rate;
} else {
// Increase throttle limit (2 second)
_throttle_limit += 0.5f/_loop_rate;
}
// throttle limit drops to 20% between hover and full throttle
_throttle_limit = constrain_float(_throttle_limit, 0.2f, 1.0f);
// limit max throttle
_max_throttle = _hover_out + ((1000-_hover_out)*_throttle_limit);
}
// apply_thrust_curve_and_volt_scaling - returns throttle curve adjusted pwm value (i.e. 1000 ~ 2000)
int16_t AP_Motors::apply_thrust_curve_and_volt_scaling(int16_t pwm_out, int16_t pwm_min, int16_t pwm_max) const
{
// convert to 0.0 to 1.0 ratio
float throttle_ratio = ((float)(pwm_out-pwm_min))/((float)(pwm_max-pwm_min));
// apply thrust curve - domain 0.0 to 1.0, range 0.0 to 1.0
if (_thrust_curve_expo > 0.0f){
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*throttle_ratio))/(2.0f*_thrust_curve_expo*_batt_voltage_filt.get());
}
// scale to maximum thrust point
throttle_ratio *= _thrust_curve_max;
// convert back to pwm range, constrain and return
return (int16_t)constrain_float(throttle_ratio*(pwm_max-pwm_min)+pwm_min, pwm_min, (pwm_max-pwm_min)*_thrust_curve_max+pwm_min);
}
// update_lift_max from battery voltage - used for voltage compensation
void AP_Motors::update_lift_max_from_batt_voltage()
{
// sanity check battery_voltage_min is not too small
// if disabled or misconfigured exit immediately
if((_batt_voltage_max <= 0) || (_batt_voltage_min >= _batt_voltage_max) || (_batt_voltage < 0.25f*_batt_voltage_min)) {
_batt_voltage_filt.reset(1.0f);
_lift_max = 1.0f;
return;
}
_batt_voltage_min = max(_batt_voltage_min, _batt_voltage_max * 0.6f);
// add current based voltage sag to battery voltage
float batt_voltage = _batt_voltage + _batt_current * _batt_resistance;
batt_voltage = constrain_float(batt_voltage, _batt_voltage_min, _batt_voltage_max);
// filter at 0.5 Hz
float bvf = _batt_voltage_filt.apply(batt_voltage/_batt_voltage_max, 1.0f/_loop_rate);
// calculate lift max
_lift_max = bvf*(1-_thrust_curve_expo) + _thrust_curve_expo*bvf*bvf;
}
// update_battery_resistance - calculate battery resistance when throttle is above hover_out
void AP_Motors::update_battery_resistance()
{
// if motors are stopped, reset resting voltage and current
if (_rc_throttle.servo_out <= 0 || !_flags.armed) {
_batt_voltage_resting = _batt_voltage;
_batt_current_resting = _batt_current;
_batt_timer = 0;
} else {
// update battery resistance when throttle is over hover throttle
if ((_batt_timer < 400) && ((_batt_current_resting*2.0f) < _batt_current)) {
if (_rc_throttle.servo_out >= _hover_out) {
// filter reaches 90% in 1/4 the test time
_batt_resistance += 0.05f*(( (_batt_voltage_resting-_batt_voltage)/(_batt_current-_batt_current_resting) ) - _batt_resistance);
_batt_timer += 1;
} else {
// initialize battery resistance to prevent change in resting voltage estimate
_batt_resistance = ((_batt_voltage_resting-_batt_voltage)/(_batt_current-_batt_current_resting));
}
}
}
}
// update_throttle_thr_mix - slew set_throttle_thr_mix to requested value
void AP_Motors::update_throttle_thr_mix()
{
// slew _throttle_thr_mix to _throttle_thr_mix_desired
if (_throttle_thr_mix < _throttle_thr_mix_desired) {
// increase quickly (i.e. from 0.1 to 0.9 in 0.4 seconds)
_throttle_thr_mix += min(2.0f/_loop_rate, _throttle_thr_mix_desired-_throttle_thr_mix);
} else if (_throttle_thr_mix > _throttle_thr_mix_desired) {
// reduce more slowly (from 0.9 to 0.1 in 1.6 seconds)
_throttle_thr_mix -= min(0.5f/_loop_rate, _throttle_thr_mix-_throttle_thr_mix_desired);
}
_throttle_thr_mix = constrain_float(_throttle_thr_mix, 0.1f, 1.0f);
}
float AP_Motors::get_compensation_gain() const
{
// avoid divide by zero
if (_lift_max <= 0.0f) {
return 1.0f;
}
float ret = 1.0f / _lift_max;
// 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);
}
return ret;
}
float AP_Motors::rel_pwm_to_thr_range(float pwm) const
{
return 1000.0f*pwm/(_rc_throttle.radio_max-_rc_throttle.radio_min);
}
float AP_Motors::thr_range_to_rel_pwm(float thr) const
{
return (_rc_throttle.radio_max-_rc_throttle.radio_min)*thr/1000.0f;
}