5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 23:48:31 -04:00

AP_MotorsMulticopter: multicopter features moved in from parent

Also rename from multirotor to multicopter
This commit is contained in:
Randy Mackay 2015-07-15 20:21:13 +09:00
parent 7355ac9cb8
commit 4d1dfd94f5
2 changed files with 98 additions and 48 deletions

View File

@ -15,17 +15,17 @@
*/
/*
* AP_Motors_Multirotor.cpp - ArduCopter multirotor motors library
* AP_MotorsMulticopter.cpp - ArduCopter multicopter motors library
* Code by Randy Mackay and Robert Lefebvre. DIYDrones.com
*
*/
#include "AP_Motors_Multirotor.h"
#include "AP_MotorsMulticopter.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 = {
const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] PROGMEM = {
// 0 was used by TB_RATIO
// 1,2,3 were used by throttle curve
@ -34,7 +34,7 @@ const AP_Param::GroupInfo AP_Motors_Multirotor::var_info[] PROGMEM = {
// @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),
AP_GROUPINFO("SPIN_ARMED", 5, AP_MotorsMulticopter, _spin_when_armed, AP_MOTORS_SPIN_WHEN_ARMED),
// @Param: YAW_HEADROOM
// @DisplayName: Matrix Yaw Min
@ -42,7 +42,7 @@ const AP_Param::GroupInfo AP_Motors_Multirotor::var_info[] PROGMEM = {
// @Range: 0 500
// @Units: pwm
// @User: Advanced
AP_GROUPINFO("YAW_HEADROOM", 6, AP_Motors_Multirotor, _yaw_headroom, AP_MOTORS_YAW_HEADROOM_DEFAULT),
AP_GROUPINFO("YAW_HEADROOM", 6, AP_MotorsMulticopter, _yaw_headroom, AP_MOTORS_YAW_HEADROOM_DEFAULT),
// 7 was THR_LOW_CMP
@ -51,14 +51,14 @@ const AP_Param::GroupInfo AP_Motors_Multirotor::var_info[] PROGMEM = {
// @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),
AP_GROUPINFO("THST_EXPO", 8, AP_MotorsMulticopter, _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),
AP_GROUPINFO("THST_MAX", 9, AP_MotorsMulticopter, _thrust_curve_max, AP_MOTORS_THST_MAX_DEFAULT),
// @Param: THST_BAT_MAX
// @DisplayName: Battery voltage compensation maximum voltage
@ -66,7 +66,7 @@ const AP_Param::GroupInfo AP_Motors_Multirotor::var_info[] PROGMEM = {
// @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),
AP_GROUPINFO("THST_BAT_MAX", 10, AP_MotorsMulticopter, _batt_voltage_max, AP_MOTORS_THST_BAT_MAX_DEFAULT),
// @Param: THST_BAT_MIN
// @DisplayName: Battery voltage compensation minimum voltage
@ -74,7 +74,7 @@ const AP_Param::GroupInfo AP_Motors_Multirotor::var_info[] PROGMEM = {
// @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),
AP_GROUPINFO("THST_BAT_MIN", 11, AP_MotorsMulticopter, _batt_voltage_min, AP_MOTORS_THST_BAT_MIN_DEFAULT),
// @Param: CURR_MAX
// @DisplayName: Motor Current Max
@ -82,43 +82,52 @@ const AP_Param::GroupInfo AP_Motors_Multirotor::var_info[] PROGMEM = {
// @Range: 0 200
// @Units: Amps
// @User: Advanced
AP_GROUPINFO("CURR_MAX", 12, AP_Motors_Multirotor, _batt_current_max, AP_MOTORS_CURR_MAX_DEFAULT),
AP_GROUPINFO("CURR_MAX", 12, AP_MotorsMulticopter, _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_GROUPINFO("THR_MIX_MIN", 13, AP_MotorsMulticopter, _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_MotorsMulticopter::AP_MotorsMulticopter(uint16_t loop_rate, uint16_t speed_hz) :
AP_Motors(loop_rate, speed_hz),
_throttle_thr_mix(AP_MOTORS_THR_LOW_CMP_DEFAULT),
_spin_when_armed_ramped(0),
_throttle_thr_mix_desired(AP_MOTORS_THR_LOW_CMP_DEFAULT),
_throttle_thr_mix(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)
_hover_out(AP_MOTORS_DEFAULT_MID_THROTTLE),
_batt_voltage_resting(0.0f),
_batt_current_resting(0.0f),
_batt_resistance(0.0f),
_batt_timer(0),
_lift_max(1.0f),
_throttle_limit(1.0f)
{
AP_Param::setup_object_defaults(this, var_info);
// disable all motors by default
memset(motor_enabled, false, sizeof(motor_enabled));
// 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
int16_t AP_MotorsMulticopter::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()
void AP_MotorsMulticopter::output()
{
// update throttle filter
update_throttle_filter();
@ -144,12 +153,13 @@ void AP_Motors_Multirotor::output()
output_armed_not_stabilizing();
}
} else {
_multicopter_flags.slow_start_low_end = true;
output_disarmed();
}
};
// update the throttle input filter
void AP_Motors_Multirotor::update_throttle_filter()
void AP_MotorsMulticopter::update_throttle_filter()
{
if (armed()) {
_throttle_filter.apply(_throttle_in, 1.0f/_loop_rate);
@ -162,26 +172,26 @@ void AP_Motors_Multirotor::update_throttle_filter()
}
// 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()
void AP_MotorsMulticopter::update_max_throttle()
{
// ramp up minimum spin speed if necessary
if (_flags.slow_start_low_end) {
if (_multicopter_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;
_multicopter_flags.slow_start_low_end = false;
}
}
// implement slow start
if (_flags.slow_start) {
if (_multicopter_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;
_multicopter_flags.slow_start = false;
}
return;
}
@ -191,7 +201,7 @@ void AP_Motors_Multirotor::update_max_throttle()
}
// current_limit_max_throttle - limits maximum throttle based on current
void AP_Motors_Multirotor::current_limit_max_throttle()
void AP_MotorsMulticopter::current_limit_max_throttle()
{
// return maximum if current limiting is disabled
if (_batt_current_max <= 0) {
@ -225,7 +235,7 @@ void AP_Motors_Multirotor::current_limit_max_throttle()
}
// 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
int16_t AP_MotorsMulticopter::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));
@ -243,7 +253,7 @@ int16_t AP_Motors_Multirotor::apply_thrust_curve_and_volt_scaling(int16_t pwm_ou
}
// update_lift_max from battery voltage - used for voltage compensation
void AP_Motors_Multirotor::update_lift_max_from_batt_voltage()
void AP_MotorsMulticopter::update_lift_max_from_batt_voltage()
{
// sanity check battery_voltage_min is not too small
// if disabled or misconfigured exit immediately
@ -267,7 +277,7 @@ void AP_Motors_Multirotor::update_lift_max_from_batt_voltage()
}
// update_battery_resistance - calculate battery resistance when throttle is above hover_out
void AP_Motors_Multirotor::update_battery_resistance()
void AP_MotorsMulticopter::update_battery_resistance()
{
// if motors are stopped, reset resting voltage and current
if (_throttle_control_input <= 0 || !_flags.armed) {
@ -290,7 +300,7 @@ void AP_Motors_Multirotor::update_battery_resistance()
}
// update_throttle_thr_mix - slew set_throttle_thr_mix to requested value
void AP_Motors_Multirotor::update_throttle_thr_mix()
void AP_MotorsMulticopter::update_throttle_thr_mix()
{
// slew _throttle_thr_mix to _throttle_thr_mix_desired
if (_throttle_thr_mix < _throttle_thr_mix_desired) {
@ -303,7 +313,7 @@ void AP_Motors_Multirotor::update_throttle_thr_mix()
_throttle_thr_mix = constrain_float(_throttle_thr_mix, 0.1f, 1.0f);
}
float AP_Motors_Multirotor::get_compensation_gain() const
float AP_MotorsMulticopter::get_compensation_gain() const
{
// avoid divide by zero
if (_lift_max <= 0.0f) {
@ -319,19 +329,19 @@ float AP_Motors_Multirotor::get_compensation_gain() const
return ret;
}
float AP_Motors_Multirotor::rel_pwm_to_thr_range(float pwm) const
float AP_MotorsMulticopter::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
float AP_MotorsMulticopter::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)
void AP_MotorsMulticopter::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;
@ -341,10 +351,10 @@ void AP_Motors_Multirotor::set_throttle_range(uint16_t min_throttle, int16_t rad
// 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)
void AP_MotorsMulticopter::slow_start(bool true_false)
{
// set slow_start flag
_flags.slow_start = true;
_multicopter_flags.slow_start = true;
// initialise maximum throttle to current throttle
_max_throttle = constrain_int16(_throttle_control_input, 0, AP_MOTORS_DEFAULT_MAX_THROTTLE);
@ -352,7 +362,7 @@ void AP_Motors_Multirotor::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 AP_Motors_Multirotor::throttle_pass_through(int16_t pwm)
void AP_MotorsMulticopter::throttle_pass_through(int16_t pwm)
{
if (armed()) {
// send the pilot's input directly to each enabled motor
@ -362,4 +372,4 @@ void AP_Motors_Multirotor::throttle_pass_through(int16_t pwm)
}
}
}
}
}

View File

@ -1,13 +1,17 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/// @file AP_Motors_Multirotor.h
/// @brief Motor control class for Multirotors
/// @file AP_MotorsMulticopter.h
/// @brief Motor control class for Multicopters
#ifndef __AP_MOTORS_MULTIROTOR_H__
#define __AP_MOTORS_MULTIROTOR_H__
#ifndef __AP_MOTORS_MULTICOPTER_H__
#define __AP_MOTORS_MULTICOPTER_H__
#include "AP_Motors_Class.h"
#define AP_MOTORS_DEFAULT_MIN_THROTTLE 130
#define AP_MOTORS_DEFAULT_MID_THROTTLE 500
#define AP_MOTORS_DEFAULT_MAX_THROTTLE 1000
#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
@ -32,12 +36,12 @@
#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 {
/// @class AP_MotorsMulticopter
class AP_MotorsMulticopter : public AP_Motors {
public:
// Constructor
AP_Motors_Multirotor(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT);
AP_MotorsMulticopter(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT);
// output - sends commands to the motors
virtual void output();
@ -56,13 +60,12 @@ public:
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); }
bool is_throttle_mix_min() const { 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); }
float get_throttle_warn() const { 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;}
int16_t throttle_min() const { return rel_pwm_to_thr_range(_min_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
@ -80,6 +83,18 @@ public:
// 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);
// get_lift_max - get maximum lift ratio - for logging purposes only
float get_lift_max() { return _lift_max; }
// get_batt_voltage_filt - get battery voltage ratio - for logging purposes only
float get_batt_voltage_filt() const { return _batt_voltage_filt.get(); }
// get_batt_resistance - get battery resistance approximation - for logging purposes only
float get_batt_resistance() const { return _batt_resistance; }
// get_throttle_limit - throttle limit ratio - for logging purposes only
float get_throttle_limit() const { return _throttle_limit; }
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];
@ -112,6 +127,21 @@ protected:
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
// RPY channels typically +/-45 degrees servo travel between +/-400 PWM
// Throttle channel typically 0-1000 range converts to 1100-1900 PWM for final output signal to motors
// ToDo: this should all be handled as floats +/- 1.0 instead of PWM and fake angle ranges
int16_t calc_roll_pwm() { return (_roll_control_input / 11.25f);}
int16_t calc_pitch_pwm() { return (_pitch_control_input / 11.25f);}
int16_t calc_yaw_pwm() { return (_yaw_control_input / 11.25f);}
int16_t calc_throttle_radio_output() { return (_throttle_control_input * _throttle_pwm_scalar) + _throttle_radio_min;}
// flag bitmask
struct {
uint8_t slow_start : 1; // 1 if slow start is active
uint8_t slow_start_low_end : 1; // 1 just after arming so we can ramp up the spin_when_armed value
} _multicopter_flags;
// parameters
AP_Int16 _spin_when_armed; // used to control whether the motors always spin when armed. pwm value above radio_min
@ -124,11 +154,21 @@ protected:
AP_Float _thr_mix_min; // current over which maximum throttle is limited
// internal variables
bool motor_enabled[AP_MOTORS_MAX_NUM_MOTORS]; // true if motor is enabled
int16_t _spin_when_armed_ramped; // equal to _spin_when_armed parameter but slowly ramped up from zero
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)
// battery voltage, current and air pressure compensation variables
float _batt_voltage_resting; // battery voltage reading at minimum throttle
LowPassFilterFloat _batt_voltage_filt; // filtered battery voltage expressed as a percentage (0 ~ 1.0) of batt_voltage_max
float _batt_current_resting; // battery's current when motors at minimum
float _batt_resistance; // battery's resistance calculated by comparing resting voltage vs in flight voltage
int16_t _batt_timer; // timer used in battery resistance calcs
float _lift_max; // maximum lift ratio from battery voltage
float _throttle_limit; // ratio of throttle limit between hover and maximum
};
#endif // __AP_MOTORS_MULTIROTOR_H__
#endif // __AP_MOTORS_MULTICOPTER_H__