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

AP_Motors: learned THST_HOVER replace THR_MID 

This holds the estimated thrust required for the vehicle to hover
expressed as a number from 0 to 1.  0 will be equivalent to outputting
SPIN_MIN (which is a number from 0~1 but in the full output range) and 1 is equivalent to SPIN_MAX (also 0~1 but in the full output range)
This commit is contained in:
Leonard Hall 2016-03-23 00:27:26 +10:30 committed by Randy Mackay
parent 5a8fc6283e
commit 4f0db2bc36
4 changed files with 24 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_Motors/AP_MotorsHeli.h
View File

@ -125,6 +125,8 @@ public:
// supports_yaw_passthrough
virtual bool supports_yaw_passthrough() const { return false; }
float get_throttle_hover() const { return 0.5f; };
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];

24
libraries/AP_Motors/AP_MotorsMulticopter.cpp
View File

@ -108,13 +108,19 @@ const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = {
// @User: Advanced
AP_GROUPINFO("PWM_MAX", 17, AP_MotorsMulticopter, _pwm_max, 0),
// @Param: THST_HOVER
// @DisplayName: Thrust Hover Value
// @Description: Motor thrust needed to hover expressed as a number from 0 to 1
// @Range: 0.25 0.8
// @User: Advanced
AP_GROUPINFO("THST_HOVER", 21, AP_MotorsMulticopter, _throttle_hover, AP_MOTORS_THST_HOVER_DEFAULT),
AP_GROUPEND
};
// Constructor
AP_MotorsMulticopter::AP_MotorsMulticopter(uint16_t loop_rate, uint16_t speed_hz) :
AP_Motors(loop_rate, speed_hz),
_hover_out(AP_MOTORS_DEFAULT_MID_THROTTLE),
_batt_voltage_resting(0.0f),
_batt_current_resting(0.0f),
_batt_resistance(0.0f),
@ -209,8 +215,7 @@ float AP_MotorsMulticopter::get_current_limit_max_throttle()
_throttle_limit = constrain_float(_throttle_limit, 0.2f, 1.0f);
// limit max throttle
float throttle_thrust_hover = get_hover_throttle_as_high_end_pct();
return throttle_thrust_hover + ((1.0-throttle_thrust_hover)*_throttle_limit);
return get_throttle_hover() + ((1.0-get_throttle_hover())*_throttle_limit);
}
// apply_thrust_curve_and_volt_scaling - returns throttle in the range 0 ~ 1
@ -263,7 +268,7 @@ void AP_MotorsMulticopter::update_battery_resistance()
} else {
// update battery resistance when throttle is over hover throttle
if ((_batt_timer < 400) && ((_batt_current_resting*2.0f) < _batt_current)) {
if (get_throttle() >= _hover_out) {
if (get_throttle() >= get_throttle_hover()) {
// 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;
@ -275,11 +280,6 @@ void AP_MotorsMulticopter::update_battery_resistance()
}
}
float AP_MotorsMulticopter::get_hover_throttle_as_high_end_pct() const
{
return (MAX(0,(float)_hover_out-_min_throttle) / (float)(1000-_min_throttle));
}
float AP_MotorsMulticopter::get_compensation_gain() const
{
// avoid divide by zero
@ -338,6 +338,12 @@ void AP_MotorsMulticopter::set_throttle_range(uint16_t min_throttle, int16_t rad
_min_throttle = (float)min_throttle * ((get_pwm_output_max() - get_pwm_output_min()) / 1000.0f);
}
// update the throttle input filter. should be called at 100hz
void AP_MotorsMulticopter::update_throttle_hover(float dt)
{
_throttle_hover = _throttle_hover + (dt/(dt+AP_MOTORS_THST_HOVER_TC))*(_throttle_in-_throttle_hover);
}
void AP_MotorsMulticopter::output_logic()
{
// force desired and current spool mode if disarmed or not interlocked

13
libraries/AP_Motors/AP_MotorsMulticopter.h
View File

@ -16,6 +16,8 @@
#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
#define AP_MOTORS_SPIN_MAX_DEFAULT 0.95f // throttle which produces the maximum thrust. (i.e. 0 ~ 1 ) of the full throttle range
#define AP_MOTORS_THST_HOVER_DEFAULT 0.5f // the estimated hover throttle, 0 ~ 1
#define AP_MOTORS_THST_HOVER_TC 10.0f // time constant used to update estimated hover throttle, 0 ~ 1
#define AP_MOTORS_BAT_VOLT_MAX_DEFAULT 0.0f // voltage limiting max default
#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
@ -48,9 +50,9 @@ public:
// also sets minimum and maximum pwm values that will be sent to the motors
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; }
// update estimated throttle required to hover
void update_throttle_hover(float dt);
virtual float get_throttle_hover() const { return _throttle_hover; };
// spool up states
enum spool_up_down_mode {
@ -123,9 +125,6 @@ protected:
// return gain scheduling gain based on voltage and air density
float get_compensation_gain() const;
// get_hover_throttle_as_high_end_pct - return hover throttle in the 0 to 1 range
float get_hover_throttle_as_high_end_pct() const;
// convert thrust (0~1) range back to pwm range
int16_t calc_thrust_to_pwm(float thrust_in) const;
@ -148,11 +147,11 @@ protected:
AP_Float _batt_current_max; // current over which maximum throttle is limited
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
// internal variables
bool motor_enabled[AP_MOTORS_MAX_NUM_MOTORS]; // true if motor is enabled
int16_t _min_throttle; // the minimum throttle to be sent to the motors when they're on (prevents motors stalling while flying)
int16_t _hover_out; // the estimated hover throttle as pct * 10 (i.e. 0 ~ 1000)
int16_t _throttle_radio_min; // minimum PWM from RC input's throttle channel (i.e. minimum PWM input from receiver, RC3_MIN)
int16_t _throttle_radio_max; // maximum PWM from RC input's throttle channel (i.e. maximum PWM input from receiver, RC3_MAX)
float _throttle_thrust_max; // the maximum allowed throttle thrust 0.0 to 1.0 in the range throttle_min to throttle_max

1
libraries/AP_Motors/AP_Motors_Class.h
View File

@ -71,6 +71,7 @@ public:
float get_pitch() const { return _pitch_in; }
float get_yaw() const { return _yaw_in; }
float get_throttle() const { return constrain_float(_throttle_filter.get(),0.0f,1.0f); }
virtual float get_throttle_hover() const = 0;
// spool up states
enum spool_up_down_desired {