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

AP_Motors: Creation of AP_Motors_Multirotor class

Browse Source
This commit is contained in:
Robert Lefebvre 2015-07-02 17:15:09 -04:00 committed by Randy Mackay
parent 0ecebbd55f
commit 61d6c5aa1d
12 changed files with 572 additions and 482 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_Motors/AP_MotorsCoax.cpp
View File

@ -29,9 +29,9 @@ extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AP_MotorsCoax::var_info[] PROGMEM = { const AP_Param::GroupInfo AP_MotorsCoax::var_info[] PROGMEM = {
// variables from parent vehicle // variables from parent vehicle
AP_NESTEDGROUPINFO(AP_Motors, 0), AP_NESTEDGROUPINFO(AP_Motors_Multirotor, 0),
// parameters 1 ~ 29 reserved for tradheli // parameters 1 ~ 29 were reserved for tradheli
// parameters 30 ~ 39 reserved for tricopter // parameters 30 ~ 39 reserved for tricopter
// parameters 40 ~ 49 for single copter and coax copter (these have identical parameter files) // parameters 40 ~ 49 for single copter and coax copter (these have identical parameter files)

6
libraries/AP_Motors/AP_MotorsCoax.h
View File

@ -9,7 +9,7 @@
#include <AP_Common.h> #include <AP_Common.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library #include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel.h> // RC Channel Library #include <RC_Channel.h> // RC Channel Library
#include "AP_Motors.h" #include "AP_Motors_Multirotor.h"
// feedback direction // feedback direction
#define AP_MOTORS_COAX_POSITIVE 1 #define AP_MOTORS_COAX_POSITIVE 1
@ -21,12 +21,12 @@
#define AP_MOTORS_COAX_SERVO_INPUT_RANGE 4500 // roll or pitch input of -4500 will cause servos to their minimum (i.e. radio_min), +4500 will move them to their maximum (i.e. radio_max) #define AP_MOTORS_COAX_SERVO_INPUT_RANGE 4500 // roll or pitch input of -4500 will cause servos to their minimum (i.e. radio_min), +4500 will move them to their maximum (i.e. radio_max)
/// @class AP_MotorsSingle /// @class AP_MotorsSingle
class AP_MotorsCoax : public AP_Motors { class AP_MotorsCoax : public AP_Motors_Multirotor {
public: public:
/// Constructor /// Constructor
AP_MotorsCoax(RC_Channel& servo1, RC_Channel& servo2, uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsCoax(RC_Channel& servo1, RC_Channel& servo2, uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
AP_Motors(loop_rate, speed_hz), AP_Motors_Multirotor(loop_rate, speed_hz),
_servo1(servo1), _servo1(servo1),
_servo2(servo2) _servo2(servo2)
{ {

2
libraries/AP_Motors/AP_MotorsHeli.cpp
View File

@ -26,8 +26,6 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = { const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = {
// variables from parent vehicle
AP_NESTEDGROUPINFO(AP_Motors, 0),
// @Param: SV1_POS // @Param: SV1_POS
// @DisplayName: Servo 1 Position // @DisplayName: Servo 1 Position

6
libraries/AP_Motors/AP_MotorsMatrix.h
View File

@ -9,18 +9,18 @@
#include <AP_Common.h> #include <AP_Common.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library #include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel.h> // RC Channel Library #include <RC_Channel.h> // RC Channel Library
#include "AP_Motors_Class.h" #include "AP_Motors_Multirotor.h"
#define AP_MOTORS_MATRIX_YAW_FACTOR_CW -1 #define AP_MOTORS_MATRIX_YAW_FACTOR_CW -1
#define AP_MOTORS_MATRIX_YAW_FACTOR_CCW 1 #define AP_MOTORS_MATRIX_YAW_FACTOR_CCW 1
/// @class AP_MotorsMatrix /// @class AP_MotorsMatrix
class AP_MotorsMatrix : public AP_Motors { class AP_MotorsMatrix : public AP_Motors_Multirotor {
public: public:
/// Constructor /// Constructor
AP_MotorsMatrix(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
AP_Motors(loop_rate, speed_hz) AP_Motors_Multirotor(loop_rate, speed_hz)
{}; {};
// init // init

4
libraries/AP_Motors/AP_MotorsSingle.cpp
View File

@ -29,9 +29,9 @@ extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AP_MotorsSingle::var_info[] PROGMEM = { const AP_Param::GroupInfo AP_MotorsSingle::var_info[] PROGMEM = {
// variables from parent vehicle // variables from parent vehicle
AP_NESTEDGROUPINFO(AP_Motors, 0), AP_NESTEDGROUPINFO(AP_Motors_Multirotor, 0),
// parameters 1 ~ 29 reserved for tradheli // parameters 1 ~ 29 were reserved for tradheli
// parameters 30 ~ 39 reserved for tricopter // parameters 30 ~ 39 reserved for tricopter
// parameters 40 ~ 49 for single copter and coax copter (these have identical parameter files) // parameters 40 ~ 49 for single copter and coax copter (these have identical parameter files)

6
libraries/AP_Motors/AP_MotorsSingle.h
View File

@ -9,7 +9,7 @@
#include <AP_Common.h> #include <AP_Common.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library #include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel.h> // RC Channel Library #include <RC_Channel.h> // RC Channel Library
#include "AP_Motors.h" #include "AP_Motors_Multirotor.h"
// feedback direction // feedback direction
#define AP_MOTORS_SING_POSITIVE 1 #define AP_MOTORS_SING_POSITIVE 1
@ -21,12 +21,12 @@
#define AP_MOTORS_SINGLE_SERVO_INPUT_RANGE 4500 // roll or pitch input of -4500 will cause servos to their minimum (i.e. radio_min), +4500 will move them to their maximum (i.e. radio_max) #define AP_MOTORS_SINGLE_SERVO_INPUT_RANGE 4500 // roll or pitch input of -4500 will cause servos to their minimum (i.e. radio_min), +4500 will move them to their maximum (i.e. radio_max)
/// @class AP_MotorsSingle /// @class AP_MotorsSingle
class AP_MotorsSingle : public AP_Motors { class AP_MotorsSingle : public AP_Motors_Multirotor {
public: public:
/// Constructor /// Constructor
AP_MotorsSingle(RC_Channel& servo1, RC_Channel& servo2, RC_Channel& servo3, RC_Channel& servo4, uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsSingle(RC_Channel& servo1, RC_Channel& servo2, RC_Channel& servo3, RC_Channel& servo4, uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
AP_Motors(loop_rate, speed_hz), AP_Motors_Multirotor(loop_rate, speed_hz),
_servo1(servo1), _servo1(servo1),
_servo2(servo2), _servo2(servo2),
_servo3(servo3), _servo3(servo3),

4
libraries/AP_Motors/AP_MotorsTri.cpp
View File

@ -27,9 +27,9 @@ extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AP_MotorsTri::var_info[] PROGMEM = { const AP_Param::GroupInfo AP_MotorsTri::var_info[] PROGMEM = {
// variables from parent vehicle // variables from parent vehicle
AP_NESTEDGROUPINFO(AP_Motors, 0), AP_NESTEDGROUPINFO(AP_Motors_Multirotor, 0),
// parameters 1 ~ 29 reserved for tradheli // parameters 1 ~ 29 were reserved for tradheli
// parameters 30 ~ 39 reserved for tricopter // parameters 30 ~ 39 reserved for tricopter
// parameters 40 ~ 49 for single copter and coax copter (these have identical parameter files) // parameters 40 ~ 49 for single copter and coax copter (these have identical parameter files)

6
libraries/AP_Motors/AP_MotorsTri.h
View File

@ -9,18 +9,18 @@
#include <AP_Common.h> #include <AP_Common.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library #include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel.h> // RC Channel Library #include <RC_Channel.h> // RC Channel Library
#include "AP_Motors.h" #include "AP_Motors_Multirotor.h"
// tail servo uses channel 7 // tail servo uses channel 7
#define AP_MOTORS_CH_TRI_YAW CH_7 #define AP_MOTORS_CH_TRI_YAW CH_7
/// @class AP_MotorsTri /// @class AP_MotorsTri
class AP_MotorsTri : public AP_Motors { class AP_MotorsTri : public AP_Motors_Multirotor {
public: public:
/// Constructor /// Constructor
AP_MotorsTri(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsTri(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
AP_Motors(loop_rate, speed_hz) AP_Motors_Multirotor(loop_rate, speed_hz)
{ {
AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info);
}; };

342
libraries/AP_Motors/AP_Motors_Class.cpp
View File

@ -24,84 +24,8 @@
#include <AP_HAL.h> #include <AP_HAL.h>
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
// initialise motor map // 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 = {MOTOR_TO_CHANNEL_MAP};
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 // Constructor
AP_Motors::AP_Motors(uint16_t loop_rate, uint16_t speed_hz) : AP_Motors::AP_Motors(uint16_t loop_rate, uint16_t speed_hz) :
@ -112,14 +36,9 @@ AP_Motors::AP_Motors(uint16_t loop_rate, uint16_t speed_hz) :
_throttle_pwm_scalar(1.0f), _throttle_pwm_scalar(1.0f),
_loop_rate(loop_rate), _loop_rate(loop_rate),
_speed_hz(speed_hz), _speed_hz(speed_hz),
_min_throttle(AP_MOTORS_DEFAULT_MIN_THROTTLE),
_max_throttle(AP_MOTORS_DEFAULT_MAX_THROTTLE),
_throttle_radio_min(1100), _throttle_radio_min(1100),
_throttle_radio_max(1900), _throttle_radio_max(1900),
_hover_out(AP_MOTORS_DEFAULT_MID_THROTTLE),
_spin_when_armed_ramped(0), _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(0.0f),
_batt_voltage_resting(0.0f), _batt_voltage_resting(0.0f),
_batt_current(0.0f), _batt_current(0.0f),
@ -132,8 +51,6 @@ AP_Motors::AP_Motors(uint16_t loop_rate, uint16_t speed_hz) :
_throttle_in(0.0f), _throttle_in(0.0f),
_throttle_filter() _throttle_filter()
{ {
AP_Param::setup_object_defaults(this, var_info);
// slow start motors from zero to min throttle // slow start motors from zero to min throttle
_flags.slow_start = true; _flags.slow_start = true;
_flags.slow_start_low_end = true; _flags.slow_start_low_end = true;
@ -144,10 +61,6 @@ AP_Motors::AP_Motors(uint16_t loop_rate, uint16_t speed_hz) :
_flags.frame_orientation = AP_MOTORS_X_FRAME; _flags.frame_orientation = AP_MOTORS_X_FRAME;
_flags.interlock = false; _flags.interlock = false;
// 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 // setup throttle filtering
_throttle_filter.set_cutoff_frequency(0.0f); _throttle_filter.set_cutoff_frequency(0.0f);
_throttle_filter.reset(0.0f); _throttle_filter.reset(0.0f);
@ -167,256 +80,3 @@ void AP_Motors::armed(bool arm)
} }
AP_Notify::flags.armed = arm; AP_Notify::flags.armed = arm;
}; };
// set_throttle_range - 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)
// also sets throttle channel minimum and maximum pwm
void AP_Motors::set_throttle_range(uint16_t min_throttle, int16_t radio_min, int16_t radio_max)
{
_throttle_radio_min = radio_min;
_throttle_radio_max = radio_max;
_throttle_pwm_scalar = (_throttle_radio_max - _throttle_radio_min) / 1000.0f;
_min_throttle = (float)min_throttle * _throttle_pwm_scalar;
}
// 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 (_throttle_radio_min + (float)(_throttle_radio_max - _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(_throttle_control_input, 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);
}
// constrain throttle signal to 0-1000
_throttle_control_input = constrain_float(_throttle_filter.get(),0.0f,1000.0f);
}
// 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 >= _throttle_control_input) {
_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(_throttle_control_input <= 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 (_throttle_control_input <= 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 (_throttle_control_input >= _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 pwm/_throttle_pwm_scalar;
}
float AP_Motors::thr_range_to_rel_pwm(float thr) const
{
return _throttle_pwm_scalar*thr;
}

173
libraries/AP_Motors/AP_Motors_Class.h
View File

@ -20,8 +20,7 @@
#define AP_MOTORS_MOT_7 6 #define AP_MOTORS_MOT_7 6
#define AP_MOTORS_MOT_8 7 #define AP_MOTORS_MOT_8 7
#define APM1_MOTOR_TO_CHANNEL_MAP CH_1,CH_2,CH_3,CH_4,CH_7,CH_8,CH_10,CH_11 #define MOTOR_TO_CHANNEL_MAP CH_1,CH_2,CH_3,CH_4,CH_5,CH_6,CH_7,CH_8
#define APM2_MOTOR_TO_CHANNEL_MAP CH_1,CH_2,CH_3,CH_4,CH_5,CH_6,CH_7,CH_8
#define AP_MOTORS_MAX_NUM_MOTORS 8 #define AP_MOTORS_MAX_NUM_MOTORS 8
@ -29,10 +28,6 @@
#define AP_MOTORS_DEFAULT_MID_THROTTLE 500 #define AP_MOTORS_DEFAULT_MID_THROTTLE 500
#define AP_MOTORS_DEFAULT_MAX_THROTTLE 1000 #define AP_MOTORS_DEFAULT_MAX_THROTTLE 1000
// APM board definitions
#define AP_MOTORS_APM1 1
#define AP_MOTORS_APM2 2
// frame definitions // frame definitions
#define AP_MOTORS_PLUS_FRAME 0 #define AP_MOTORS_PLUS_FRAME 0
#define AP_MOTORS_X_FRAME 1 #define AP_MOTORS_X_FRAME 1
@ -52,21 +47,6 @@
#define THROTTLE_CURVE_MID_THRUST 52 // throttle which produces 1/2 the maximum thrust. expressed as a percentage of the full throttle range (i.e 0 ~ 100) #define THROTTLE_CURVE_MID_THRUST 52 // throttle which produces 1/2 the maximum thrust. expressed as a percentage of the full throttle range (i.e 0 ~ 100)
#define THROTTLE_CURVE_MAX_THRUST 93 // throttle which produces the maximum thrust. expressed as a percentage of the full throttle range (i.e 0 ~ 100) #define THROTTLE_CURVE_MAX_THRUST 93 // throttle which produces the maximum thrust. expressed as a percentage of the full throttle range (i.e 0 ~ 100)
#define AP_MOTORS_SPIN_WHEN_ARMED 70 // spin motors at this PWM value when armed
#define AP_MOTORS_YAW_HEADROOM_DEFAULT 200
#define AP_MOTORS_THR_LOW_CMP_DEFAULT 0.5f // ratio controlling the max throttle output during competing requests of low throttle from the pilot (or autopilot) and higher throttle for attitude control. Higher favours Attitude over pilot input
#define AP_MOTORS_THST_EXPO_DEFAULT 0.5f // set to 0 for linear and 1 for second order approximation
#define AP_MOTORS_THST_MAX_DEFAULT 0.95f // throttle which produces the maximum thrust. (i.e. 0 ~ 1 ) of the full throttle range
#define AP_MOTORS_THST_BAT_MAX_DEFAULT 0.0f
#define AP_MOTORS_THST_BAT_MIN_DEFAULT 0.0f
#define AP_MOTORS_CURR_MAX_DEFAULT 0.0f // current limiting max default
#define AP_MOTORS_BATT_VOLT_FILT_HZ 0.5f // battery voltage filtered at 0.5hz
#define AP_MOTORS_THR_MIX_MIN_DEFAULT 0.1f // minimum throttle mix
#define AP_MOTORS_THR_MIX_MID_DEFAULT 0.5f // manual throttle mix
#define AP_MOTORS_THR_MIX_MAX_DEFAULT 0.9f // maximum throttle mix
// bit mask for recording which limits we have reached when outputting to motors // bit mask for recording which limits we have reached when outputting to motors
#define AP_MOTOR_NO_LIMITS_REACHED 0x00 #define AP_MOTOR_NO_LIMITS_REACHED 0x00
#define AP_MOTOR_ROLLPITCH_LIMIT 0x01 #define AP_MOTOR_ROLLPITCH_LIMIT 0x01
@ -74,16 +54,6 @@
#define AP_MOTOR_THROTTLE_LIMIT 0x04 #define AP_MOTOR_THROTTLE_LIMIT 0x04
#define AP_MOTOR_ANY_LIMIT 0xFF #define AP_MOTOR_ANY_LIMIT 0xFF
// To-Do: replace this hard coded counter with a timer
#if HAL_CPU_CLASS < HAL_CPU_CLASS_75 || CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX
// slow start increments - throttle increase per (100hz) iteration. i.e. 5 = full speed in 2 seconds
#define AP_MOTOR_SLOW_START_INCREMENT 10 // max throttle ramp speed (i.e. motors can reach full throttle in 1 second)
#define AP_MOTOR_SLOW_START_LOW_END_INCREMENT 2 // min throttle ramp speed (i.e. motors will speed up from zero to _spin_when_armed speed in about 1 second)
#else
// slow start increments - throttle increase per (400hz) iteration. i.e. 1 = full speed in 2.5 seconds
#define AP_MOTOR_SLOW_START_INCREMENT 3 // max throttle ramp speed (i.e. motors can reach full throttle in 0.8 seconds)
#define AP_MOTOR_SLOW_START_LOW_END_INCREMENT 1 // min throttle ramp speed (i.e. motors will speed up from zero to _spin_when_armed speed in about 0.3 second)
#endif
/// @class AP_Motors /// @class AP_Motors
class AP_Motors { class AP_Motors {
public: public:
@ -91,18 +61,12 @@ public:
// Constructor // Constructor
AP_Motors(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT); AP_Motors(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT);
// init
virtual void Init() {}
// set update rate to motors - a value in hertz // set update rate to motors - a value in hertz
virtual void set_update_rate( uint16_t speed_hz ) { _speed_hz = speed_hz; }; virtual void set_update_rate( uint16_t speed_hz ) { _speed_hz = speed_hz; };
// set frame orientation (normally + or X) // set frame orientation (normally + or X)
virtual void set_frame_orientation( uint8_t new_orientation ) { _flags.frame_orientation = new_orientation; }; virtual void set_frame_orientation( uint8_t new_orientation ) { _flags.frame_orientation = new_orientation; };
// enable - starts allowing signals to be sent to motors
virtual void enable() = 0;
// arm, disarm or check status status of motors // arm, disarm or check status status of motors
bool armed() const { return _flags.armed; }; bool armed() const { return _flags.armed; };
void armed(bool arm); void armed(bool arm);
@ -115,7 +79,7 @@ public:
// set_throttle_range - 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) // set_throttle_range - 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)
// also sets throttle channel minimum and maximum pwm // also sets throttle channel minimum and maximum pwm
void set_throttle_range(uint16_t min_throttle, int16_t radio_min, int16_t radio_max); virtual void set_throttle_range(uint16_t min_throttle, int16_t radio_min, int16_t radio_max) {}
// set_hover_throttle - sets the mid throttle which is close to the hover throttle of the copter // set_hover_throttle - sets the mid throttle which is close to the hover throttle of the copter
// this is used to limit the amount that the stability patch will increase the throttle to give more room for roll, pitch and yaw control // this is used to limit the amount that the stability patch will increase the throttle to give more room for roll, pitch and yaw control
@ -142,24 +106,6 @@ public:
void set_throttle_filter_cutoff(float filt_hz) { _throttle_filter.set_cutoff_frequency(filt_hz); } void set_throttle_filter_cutoff(float filt_hz) { _throttle_filter.set_cutoff_frequency(filt_hz); }
// output - sends commands to the motors
virtual void output();
// output_min - sends minimum values out to the motors
virtual void output_min() = 0;
// output_test - spin a motor at the pwm value specified
// motor_seq is the motor's sequence number from 1 to the number of motors on the frame
// pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
virtual void output_test(uint8_t motor_seq, int16_t pwm) = 0;
// 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
virtual void throttle_pass_through(int16_t pwm);
// set_yaw_headroom - set yaw headroom (yaw is given at least this amount of pwm)
virtual void set_yaw_headroom(int16_t pwm) { _yaw_headroom = pwm; }
// set_voltage - set voltage to be used for output scaling // set_voltage - set voltage to be used for output scaling
virtual void set_voltage(float volts){ _batt_voltage = volts; } virtual void set_voltage(float volts){ _batt_voltage = volts; }
@ -169,16 +115,6 @@ public:
// set_density_ratio - sets air density as a proportion of sea level density // set_density_ratio - sets air density as a proportion of sea level density
void set_air_density_ratio(float ratio) { _air_density_ratio = ratio; } void set_air_density_ratio(float ratio) { _air_density_ratio = ratio; }
// set_throttle_thr_mix - set desired throttle_thr_mix (actual throttle_thr_mix is slewed towards this value over 1~2 seconds)
// low values favour pilot/autopilot throttle over attitude control, high values favour attitude control over throttle
// has no effect when throttle is above hover throttle
void set_throttle_mix_min() { _throttle_thr_mix_desired = _thr_mix_min; }
void set_throttle_mix_mid() { _throttle_thr_mix_desired = AP_MOTORS_THR_MIX_MID_DEFAULT; }
void set_throttle_mix_max() { _throttle_thr_mix_desired = AP_MOTORS_THR_MIX_MAX_DEFAULT; }
// get_throttle_thr_mix - get low throttle compensation value
bool is_throttle_mix_min() { return (_throttle_thr_mix < 1.25f*_thr_mix_min); }
// get_lift_max - get maximum lift ratio // get_lift_max - get maximum lift ratio
float get_lift_max() { return _lift_max; } float get_lift_max() { return _lift_max; }
@ -191,20 +127,9 @@ public:
// get_throttle_limit - throttle limit ratio // get_throttle_limit - throttle limit ratio
float get_throttle_limit() { return _throttle_limit; } float get_throttle_limit() { return _throttle_limit; }
// returns warning throttle
float get_throttle_warn() { return rel_pwm_to_thr_range(_spin_when_armed); }
// 1 if motor is enabled, 0 otherwise // 1 if motor is enabled, 0 otherwise
bool motor_enabled[AP_MOTORS_MAX_NUM_MOTORS]; bool motor_enabled[AP_MOTORS_MAX_NUM_MOTORS];
// 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 slow_start(bool true_false);
// get_motor_mask - returns a bitmask of which outputs are being used for motors (1 means being used)
// this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
virtual uint16_t get_motor_mask() = 0;
// structure for holding motor limit flags // structure for holding motor limit flags
struct AP_Motors_limit { struct AP_Motors_limit {
uint8_t roll_pitch : 1; // we have reached roll or pitch limit uint8_t roll_pitch : 1; // we have reached roll or pitch limit
@ -213,8 +138,56 @@ public:
uint8_t throttle_upper : 1; // we have reached throttle's upper limit uint8_t throttle_upper : 1; // we have reached throttle's upper limit
} limit; } limit;
// var_info for holding Parameter information ////////////////////////////////////////////////////////////////////////
static const struct AP_Param::GroupInfo var_info[]; // Virtual Functions to be overloaded by child classes
// in most cases this is done because of global usage of multirotor specific
// functions which do not apply to helicopters
// init
virtual void Init() {}
// enable - starts allowing signals to be sent to motors
virtual void enable() = 0;
// output - sends commands to the motors
virtual void output() = 0;
// output_min - sends minimum values out to the motors
virtual void output_min() = 0;
// output_test - spin a motor at the pwm value specified
// motor_seq is the motor's sequence number from 1 to the number of motors on the frame
// pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
virtual void output_test(uint8_t motor_seq, int16_t pwm) = 0;
virtual int16_t throttle_min() { return AP_MOTORS_DEFAULT_MIN_THROTTLE;};
virtual int16_t throttle_max() { return AP_MOTORS_DEFAULT_MAX_THROTTLE;};
// set_hover_throttle - sets the mid throttle which is close to the hover throttle of the copter
// this is used to limit the amount that the stability patch will increase the throttle to give more room for roll, pitch and yaw control
virtual void set_hover_throttle(uint16_t hov_thr) {}
// 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
virtual void throttle_pass_through(int16_t pwm) {}
// returns warning throttle
virtual float get_throttle_warn() { return 0; }
// set_throttle_thr_mix - set desired throttle_thr_mix (actual throttle_thr_mix is slewed towards this value over 1~2 seconds)
// low values favour pilot/autopilot throttle over attitude control, high values favour attitude control over throttle
// has no effect when throttle is above hover throttle
virtual void set_throttle_mix_min() {}
virtual void set_throttle_mix_mid() {}
virtual void set_throttle_mix_max() {}
// 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
virtual void slow_start(bool true_false) {};
// get_motor_mask - returns a bitmask of which outputs are being used for motors (1 means being used)
// this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
virtual uint16_t get_motor_mask() = 0;
protected: protected:
// output functions that should be overloaded by child classes // output functions that should be overloaded by child classes
@ -224,31 +197,7 @@ protected:
virtual void output_disarmed()=0; virtual void output_disarmed()=0;
// update the throttle input filter // update the throttle input filter
virtual void update_throttle_filter(); virtual void update_throttle_filter() = 0;
// update_max_throttle - updates the limits on _max_throttle for slow_start and current limiting flag
void update_max_throttle();
// current_limit_max_throttle - current limit maximum throttle (called from update_max_throttle)
void current_limit_max_throttle();
// apply_thrust_curve_and_volt_scaling - thrust curve and voltage adjusted pwm value (i.e. 1000 ~ 2000)
int16_t apply_thrust_curve_and_volt_scaling(int16_t pwm_out, int16_t pwm_min, int16_t pwm_max) const;
// update_lift_max_from_batt_voltage - used for voltage compensation
void update_lift_max_from_batt_voltage();
// update_battery_resistance - calculate battery resistance when throttle is above hover_out
void update_battery_resistance();
// update_throttle_thr_mix - updates thr_low_comp value towards the target
void update_throttle_thr_mix();
// return gain scheduling gain based on voltage and air density
float get_compensation_gain() const;
float rel_pwm_to_thr_range(float pwm) const;
float thr_range_to_rel_pwm(float thr) const;
// convert RPY and Throttle servo ranges from legacy controller scheme back into PWM values // convert RPY and Throttle servo ranges from legacy controller scheme back into PWM values
// RPY channels typically +/-45 degrees servo travel between +/-400 PWM // RPY channels typically +/-45 degrees servo travel between +/-400 PWM
@ -272,17 +221,6 @@ protected:
// mapping of motor number (as received from upper APM code) to RC channel output - used to account for differences between APM1 and APM2 // mapping of motor number (as received from upper APM code) to RC channel output - used to account for differences between APM1 and APM2
static const uint8_t _motor_to_channel_map[AP_MOTORS_MAX_NUM_MOTORS] PROGMEM; static const uint8_t _motor_to_channel_map[AP_MOTORS_MAX_NUM_MOTORS] PROGMEM;
// parameters
AP_Int16 _spin_when_armed; // used to control whether the motors always spin when armed. pwm value above radio_min
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 _thrust_curve_max; // throttle which produces the maximum thrust. (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 _thr_mix_min; // current over which maximum throttle is limited
// internal variables // internal variables
float _roll_control_input; // desired roll control from attitude controllers, +/- 4500 float _roll_control_input; // desired roll control from attitude controllers, +/- 4500
float _pitch_control_input; // desired pitch control from attitude controller, +/- 4500 float _pitch_control_input; // desired pitch control from attitude controller, +/- 4500
@ -291,14 +229,9 @@ protected:
float _throttle_pwm_scalar; // scalar used to convert throttle channel pwm range into 0-1000 range, ~0.8 - 1.0 float _throttle_pwm_scalar; // scalar used to convert throttle channel pwm range into 0-1000 range, ~0.8 - 1.0
uint16_t _loop_rate; // rate at which output() function is called (normally 400hz) uint16_t _loop_rate; // rate at which output() function is called (normally 400hz)
uint16_t _speed_hz; // speed in hz to send updates to motors uint16_t _speed_hz; // speed in hz to send updates to motors
int16_t _min_throttle; // the minimum throttle to be sent to the motors when they're on (prevents motors stalling while flying)
int16_t _max_throttle; // the maximum throttle to be sent to the motors (sometimes limited by slow start)
int16_t _throttle_radio_min; // minimum radio channel pwm int16_t _throttle_radio_min; // minimum radio channel pwm
int16_t _throttle_radio_max; // maximum radio channel pwm int16_t _throttle_radio_max; // maximum radio channel pwm
int16_t _hover_out; // the estimated hover throttle as pct * 10 (i.e. 0 ~ 1000)
int16_t _spin_when_armed_ramped; // equal to _spin_when_armed parameter but slowly ramped up from zero int16_t _spin_when_armed_ramped; // equal to _spin_when_armed parameter but slowly ramped up from zero
float _throttle_thr_mix; // mix between throttle and hover throttle for 0 to 1 and ratio above hover throttle for >1
float _throttle_thr_mix_desired; // desired throttle_low_comp value, actual throttle_low_comp is slewed towards this value over 1~2 seconds
// battery voltage compensation variables // battery voltage compensation variables
float _batt_voltage; // latest battery voltage reading float _batt_voltage; // latest battery voltage reading

365
libraries/AP_Motors/AP_Motors_Multirotor.cpp Normal file
View File

@ -0,0 +1,365 @@
// -*- 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_Multirotor.cpp - ArduCopter multirotor motors library
* Code by Randy Mackay and Robert Lefebvre. DIYDrones.com
*
*/
#include "AP_Motors_Multirotor.h"
#include <AP_HAL.h>
extern const AP_HAL::HAL& hal;
// parameters for the motor class
const AP_Param::GroupInfo AP_Motors_Multirotor::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_Multirotor, _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_Multirotor, _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_Multirotor, _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_Multirotor, _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_Multirotor, _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_Multirotor, _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_Multirotor, _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_Multirotor, _thr_mix_min, AP_MOTORS_THR_MIX_MIN_DEFAULT),
AP_GROUPEND
};
// Constructor
AP_Motors_Multirotor::AP_Motors_Multirotor(uint16_t loop_rate, uint16_t speed_hz) :
AP_Motors(loop_rate, speed_hz),
_throttle_thr_mix(AP_MOTORS_THR_LOW_CMP_DEFAULT),
_throttle_thr_mix_desired(AP_MOTORS_THR_LOW_CMP_DEFAULT),
_min_throttle(AP_MOTORS_DEFAULT_MIN_THROTTLE),
_max_throttle(AP_MOTORS_DEFAULT_MAX_THROTTLE),
_hover_out(AP_MOTORS_DEFAULT_MID_THROTTLE)
{
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);
};
// get_hover_throttle_as_pwm - converts hover throttle to pwm (i.e. range 1000 ~ 2000)
int16_t AP_Motors_Multirotor::get_hover_throttle_as_pwm() const
{
return (_throttle_radio_min + (float)(_throttle_radio_max - _throttle_radio_min) * _hover_out / 1000.0f);
}
// output - sends commands to the motors
void AP_Motors_Multirotor::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();
}
};
// update the throttle input filter
void AP_Motors_Multirotor::update_throttle_filter()
{
if (armed()) {
_throttle_filter.apply(_throttle_in, 1.0f/_loop_rate);
} else {
_throttle_filter.reset(0.0f);
}
// constrain throttle signal to 0-1000
_throttle_control_input = constrain_float(_throttle_filter.get(),0.0f,1000.0f);
}
// update_max_throttle - updates the limits on _max_throttle if necessary taking into account slow_start_throttle flag
void AP_Motors_Multirotor::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 >= _throttle_control_input) {
_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_Multirotor::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(_throttle_control_input <= 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_Multirotor::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_Multirotor::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_Multirotor::update_battery_resistance()
{
// if motors are stopped, reset resting voltage and current
if (_throttle_control_input <= 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 (_throttle_control_input >= _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_Multirotor::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_Multirotor::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_Multirotor::rel_pwm_to_thr_range(float pwm) const
{
return pwm/_throttle_pwm_scalar;
}
float AP_Motors_Multirotor::thr_range_to_rel_pwm(float thr) const
{
return _throttle_pwm_scalar*thr;
}
// set_throttle_range - 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)
// also sets throttle channel minimum and maximum pwm
void AP_Motors_Multirotor::set_throttle_range(uint16_t min_throttle, int16_t radio_min, int16_t radio_max)
{
_throttle_radio_min = radio_min;
_throttle_radio_max = radio_max;
_throttle_pwm_scalar = (_throttle_radio_max - _throttle_radio_min) / 1000.0f;
_min_throttle = (float)min_throttle * _throttle_pwm_scalar;
}
// 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_Multirotor::slow_start(bool true_false)
{
// set slow_start flag
_flags.slow_start = true;
// initialise maximum throttle to current throttle
_max_throttle = constrain_int16(_throttle_control_input, 0, AP_MOTORS_DEFAULT_MAX_THROTTLE);
}
// 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_Multirotor::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);
}
}
}
}

134
libraries/AP_Motors/AP_Motors_Multirotor.h Normal file
View File

@ -0,0 +1,134 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/// @file AP_Motors_Multirotor.h
/// @brief Motor control class for Multirotors
#ifndef __AP_MOTORS_MULTIROTOR_H__
#define __AP_MOTORS_MULTIROTOR_H__
#include "AP_Motors_Class.h"
#define AP_MOTORS_SPIN_WHEN_ARMED 70 // spin motors at this PWM value when armed
#define AP_MOTORS_YAW_HEADROOM_DEFAULT 200
#define AP_MOTORS_THR_LOW_CMP_DEFAULT 0.5f // ratio controlling the max throttle output during competing requests of low throttle from the pilot (or autopilot) and higher throttle for attitude control. Higher favours Attitude over pilot input
#define AP_MOTORS_THST_EXPO_DEFAULT 0.5f // set to 0 for linear and 1 for second order approximation
#define AP_MOTORS_THST_MAX_DEFAULT 0.95f // throttle which produces the maximum thrust. (i.e. 0 ~ 1 ) of the full throttle range
#define AP_MOTORS_THST_BAT_MAX_DEFAULT 0.0f
#define AP_MOTORS_THST_BAT_MIN_DEFAULT 0.0f
#define AP_MOTORS_CURR_MAX_DEFAULT 0.0f // current limiting max default
#define AP_MOTORS_BATT_VOLT_FILT_HZ 0.5f // battery voltage filtered at 0.5hz
#define AP_MOTORS_THR_MIX_MIN_DEFAULT 0.1f // minimum throttle mix
#define AP_MOTORS_THR_MIX_MID_DEFAULT 0.5f // manual throttle mix
#define AP_MOTORS_THR_MIX_MAX_DEFAULT 0.9f // maximum throttle mix
// To-Do: replace this hard coded counter with a timer
#if HAL_CPU_CLASS < HAL_CPU_CLASS_75 || CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX
// slow start increments - throttle increase per (100hz) iteration. i.e. 5 = full speed in 2 seconds
#define AP_MOTOR_SLOW_START_INCREMENT 10 // max throttle ramp speed (i.e. motors can reach full throttle in 1 second)
#define AP_MOTOR_SLOW_START_LOW_END_INCREMENT 2 // min throttle ramp speed (i.e. motors will speed up from zero to _spin_when_armed speed in about 1 second)
#else
// slow start increments - throttle increase per (400hz) iteration. i.e. 1 = full speed in 2.5 seconds
#define AP_MOTOR_SLOW_START_INCREMENT 3 // max throttle ramp speed (i.e. motors can reach full throttle in 0.8 seconds)
#define AP_MOTOR_SLOW_START_LOW_END_INCREMENT 1 // min throttle ramp speed (i.e. motors will speed up from zero to _spin_when_armed speed in about 0.3 second)
#endif
/// @class AP_Motors_Multirotor
class AP_Motors_Multirotor :public AP_Motors {
public:
// Constructor
AP_Motors_Multirotor(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT);
// output - sends commands to the motors
virtual void output();
// get_hover_throttle_as_pwm - converts hover throttle to pwm (i.e. range 1000 ~ 2000)
int16_t get_hover_throttle_as_pwm() const;
// set_yaw_headroom - set yaw headroom (yaw is given at least this amount of pwm)
virtual void set_yaw_headroom(int16_t pwm) { _yaw_headroom = pwm; }
// set_throttle_thr_mix - set desired throttle_thr_mix (actual throttle_thr_mix is slewed towards this value over 1~2 seconds)
// low values favour pilot/autopilot throttle over attitude control, high values favour attitude control over throttle
// has no effect when throttle is above hover throttle
void set_throttle_mix_min() { _throttle_thr_mix_desired = _thr_mix_min; }
void set_throttle_mix_mid() { _throttle_thr_mix_desired = AP_MOTORS_THR_MIX_MID_DEFAULT; }
void set_throttle_mix_max() { _throttle_thr_mix_desired = AP_MOTORS_THR_MIX_MAX_DEFAULT; }
// get_throttle_thr_mix - get low throttle compensation value
bool is_throttle_mix_min() { return (_throttle_thr_mix < 1.25f*_thr_mix_min); }
// returns warning throttle
float get_throttle_warn() { return rel_pwm_to_thr_range(_spin_when_armed); }
int16_t throttle_min() const { return _min_throttle;}
int16_t throttle_max() const { return _max_throttle;}
// set_throttle_range - 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)
// also sets throttle channel minimum and maximum pwm
void set_throttle_range(uint16_t min_throttle, int16_t radio_min, int16_t radio_max);
// set_hover_throttle - sets the mid throttle which is close to the hover throttle of the copter
// this is used to limit the amount that the stability patch will increase the throttle to give more room for roll, pitch and yaw control
void set_hover_throttle(uint16_t hov_thr) { _hover_out = hov_thr; }
// 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 slow_start(bool true_false);
// 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 throttle_pass_through(int16_t pwm);
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];
protected:
// update the throttle input filter
virtual void update_throttle_filter();
// update_max_throttle - updates the limits on _max_throttle for slow_start and current limiting flag
void update_max_throttle();
// current_limit_max_throttle - current limit maximum throttle (called from update_max_throttle)
void current_limit_max_throttle();
// apply_thrust_curve_and_volt_scaling - thrust curve and voltage adjusted pwm value (i.e. 1000 ~ 2000)
int16_t apply_thrust_curve_and_volt_scaling(int16_t pwm_out, int16_t pwm_min, int16_t pwm_max) const;
// update_lift_max_from_batt_voltage - used for voltage compensation
void update_lift_max_from_batt_voltage();
// update_battery_resistance - calculate battery resistance when throttle is above hover_out
void update_battery_resistance();
// update_throttle_thr_mix - updates thr_low_comp value towards the target
void update_throttle_thr_mix();
// return gain scheduling gain based on voltage and air density
float get_compensation_gain() const;
float rel_pwm_to_thr_range(float pwm) const;
float thr_range_to_rel_pwm(float thr) const;
// parameters
AP_Int16 _spin_when_armed; // used to control whether the motors always spin when armed. pwm value above radio_min
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 _thrust_curve_max; // throttle which produces the maximum thrust. (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 _thr_mix_min; // current over which maximum throttle is limited
// internal variables
float _throttle_thr_mix_desired; // desired throttle_low_comp value, actual throttle_low_comp is slewed towards this value over 1~2 seconds
float _throttle_thr_mix; // mix between throttle and hover throttle for 0 to 1 and ratio above hover throttle for >1
int16_t _min_throttle; // the minimum throttle to be sent to the motors when they're on (prevents motors stalling while flying)
int16_t _max_throttle; // the maximum throttle to be sent to the motors (sometimes limited by slow start)
int16_t _hover_out; // the estimated hover throttle as pct * 10 (i.e. 0 ~ 1000)
};
#endif // __AP_MOTORS_MULTIROTOR_H__