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

AP_MotorsCoax: stability patch in 0 to 1 range 

Also removes setting of limits in stability patch
sets outputs for additional 2 servos
This commit is contained in:
Leonard Hall 2016-01-20 12:56:58 +09:00 committed by Randy Mackay
parent 7df9b2eb8c
commit b85c20bb65
2 changed files with 117 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

153
libraries/AP_Motors/AP_MotorsCoax.cpp
View File

@ -147,60 +147,111 @@ uint16_t AP_MotorsCoax::get_motor_mask()
// sends commands to the motors
void AP_MotorsCoax::output_armed_stabilizing()
{
int16_t yaw_pwm; // yaw pwm value, initially calculated by calc_yaw_pwm() but may be modified after, +/- 400
int16_t throttle_radio_output; // total throttle pwm value, summed onto throttle channel minimum, typically ~1100-1900
int16_t out_min = _throttle_radio_min + _min_throttle;
int16_t motor_out[4];
float roll_thrust; // roll thrust input value, +/- 1.0
float pitch_thrust; // pitch thrust input value, +/- 1.0
float yaw_thrust; // yaw thrust input value, +/- 1.0
float throttle_thrust; // throttle thrust input value, 0.0 - 1.0
float thrust_min_rp; // the minimum throttle setting that will not limit the roll and pitch output
float thr_adj; // the difference between the pilot's desired throttle and throttle_thrust_best_rpy
float throttle_thrust_hover = get_hover_throttle_as_high_end_pct(); // throttle hover thrust value, 0.0 - 1.0
float throttle_thrust_best_rpy; // throttle providing maximum roll, pitch and yaw range without climbing
float throttle_thrust_rpy_mix; // partial calculation of throttle_thrust_best_rpy
float y_scale; // this is used to scale the yaw to fit within the motor limits
// initialize limits flags
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
// apply voltage and air pressure compensation
// todo: we shouldn't need input reversing with servo reversing
roll_thrust = _roll_reverse * get_roll_thrust() * get_compensation_gain();
pitch_thrust = _pitch_reverse * get_pitch_thrust() * get_compensation_gain();
yaw_thrust = _yaw_reverse * get_yaw_thrust() * get_compensation_gain();
throttle_thrust = get_throttle() * get_compensation_gain();
int16_t thr_in_min = rel_pwm_to_thr_range(_min_throttle);
if (_throttle_control_input <= thr_in_min) {
_throttle_control_input = thr_in_min;
// assuming maximum actuator defection the maximum roll and pitch torque is approximately proportional to thrust
thrust_min_rp = MAX(fabsf(roll_thrust), fabsf(pitch_thrust));
// sanity check throttle is above zero and below current limited throttle
if (throttle_thrust <= 0.0f) {
throttle_thrust = 0.0f;
limit.throttle_lower = true;
}
if (_throttle_control_input >= _max_throttle) {
_throttle_control_input = _max_throttle;
if (throttle_thrust >= _throttle_thrust_max) {
throttle_thrust = _throttle_thrust_max;
limit.throttle_upper = true;
}
throttle_thrust_rpy_mix = MAX(throttle_thrust, throttle_thrust*MAX(0.0f,1.0f-_throttle_rpy_mix)+throttle_thrust_hover*_throttle_rpy_mix);
// calculate throttle and yaw PWM
throttle_radio_output = calc_throttle_radio_output();
yaw_pwm = calc_yaw_pwm();
// check everything fits
throttle_thrust_best_rpy = MIN(0.5f, throttle_thrust_rpy_mix);
if (is_zero(yaw_thrust)) {
y_scale = 1.0f;
} else {
y_scale = constrain_float(throttle_thrust_best_rpy/fabsf(0.5f * yaw_thrust), 0.0f, 1.0f);
}
// motors
motor_out[AP_MOTORS_MOT_3] = _rev_yaw*yaw_pwm + throttle_radio_output;
motor_out[AP_MOTORS_MOT_4] = -_rev_yaw*yaw_pwm + throttle_radio_output;
thr_adj = throttle_thrust - throttle_thrust_best_rpy;
if(y_scale < 1.0f){
// Full range is being used yaw.
limit.yaw = true;
if(thr_adj < 0.0f){
limit.throttle_lower = true;
}else if(thr_adj > 0.0f){
limit.throttle_upper = true;
}
thr_adj = 0.0f;
}else{
if(thr_adj < MIN(-(throttle_thrust_best_rpy - fabsf(0.5f * yaw_thrust)), -(throttle_thrust_best_rpy - thrust_min_rp))){
// Throttle can't be reduced to the desired level for one of two reasons:
// 1. This would result in yaw control deviation causing the throttle output to be out of range.
// 2. This would roll or pitch control would not be able to reach the desired level because of lack of thrust.
thr_adj = MIN(-(throttle_thrust_best_rpy - fabsf(0.5f * yaw_thrust)), -(throttle_thrust_best_rpy - thrust_min_rp));
limit.throttle_lower = true;
if(thrust_min_rp > throttle_thrust_best_rpy + thr_adj){
// todo: add limits for roll and pitch separately
limit.roll_pitch = true;
}
}else if(thr_adj > 1.0f - (throttle_thrust_best_rpy + fabsf(0.5f * yaw_thrust))){
// Throttle can't be increased to desired value
thr_adj = 1.0f - (throttle_thrust_best_rpy + fabsf(0.5f * yaw_thrust));
limit.throttle_upper = true;
}
}
// TODO: set limits.roll_pitch and limits.yaw
_thrust_yt_ccw = throttle_thrust_best_rpy + thr_adj + 0.5f * y_scale *_thrust_yt_ccw;
_thrust_yt_cw = throttle_thrust_best_rpy + thr_adj - 0.5f * y_scale *_thrust_yt_cw;
// front
_servo1.servo_out = _rev_roll*_roll_control_input;
// right
_servo2.servo_out = _rev_pitch*_pitch_control_input;
_servo1.calc_pwm();
_servo2.calc_pwm();
// adjust for thrust curve and voltage scaling
motor_out[AP_MOTORS_MOT_3] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_3], out_min, _throttle_radio_max);
motor_out[AP_MOTORS_MOT_4] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_4], out_min, _throttle_radio_max);
// ensure motors don't drop below a minimum value and stop
motor_out[AP_MOTORS_MOT_3] = MAX(motor_out[AP_MOTORS_MOT_3], out_min);
motor_out[AP_MOTORS_MOT_4] = MAX(motor_out[AP_MOTORS_MOT_4], out_min);
// send output to each motor
hal.rcout->cork();
rc_write(AP_MOTORS_MOT_1, _servo1.radio_out);
rc_write(AP_MOTORS_MOT_2, _servo2.radio_out);
rc_write(AP_MOTORS_MOT_3, motor_out[AP_MOTORS_MOT_3]);
rc_write(AP_MOTORS_MOT_4, motor_out[AP_MOTORS_MOT_4]);
hal.rcout->push();
if(is_zero((throttle_thrust_best_rpy + thr_adj))){
limit.roll_pitch = true;
if(roll_thrust < 0.0f){
_actuator_out[0] = -1.0f;
}else if(roll_thrust > 0.0f){
_actuator_out[0] = 1.0f;
}else{
_actuator_out[0] = 0.0f;
}
if(roll_thrust < 0.0f){
_actuator_out[1] = -1.0f;
}else if(roll_thrust > 0.0f){
_actuator_out[1] = 1.0f;
}else{
_actuator_out[1] = 0.0f;
}
}else{
// force of a lifting surface is approximately equal to the angle of attack times the airflow velocity squared
// static thrust is proportional to the airflow velocity squared
// therefore the torque of the roll and pitch actuators should be approximately proportional to
// the angle of attack multiplied by the static thrust.
_actuator_out[0] = roll_thrust/(throttle_thrust_best_rpy + thr_adj);
_actuator_out[1] = pitch_thrust/(throttle_thrust_best_rpy + thr_adj);
if(fabsf(_actuator_out[0]) > 1.0f){
limit.roll_pitch = true;
_actuator_out[0] = constrain_float(_actuator_out[0], -1.0f, 1.0f);
}
if(fabsf(_actuator_out[1]) > 1.0f){
limit.roll_pitch = true;
_actuator_out[1] = constrain_float(_actuator_out[1], -1.0f, 1.0f);
}
}
_actuator_out[2] = _actuator_out[0];
_actuator_out[3] = _actuator_out[1];
}
// output_test - spin a motor at the pwm value specified
@ -244,3 +295,17 @@ void AP_MotorsCoax::output_test(uint8_t motor_seq, int16_t pwm)
break;
}
}
// calc_yaw_radio_output - calculate final radio output for yaw channel
int16_t AP_MotorsCoax::calc_pivot_radio_output(float yaw_input, int16_t servo_min, int16_t servo_trim, int16_t servo_max)
{
int16_t ret;
if (yaw_input >= 0){
ret = ((yaw_input * (servo_max - servo_trim)) + servo_trim);
} else {
ret = ((yaw_input * (servo_trim - servo_min)) + servo_trim);
}
return ret;
}

8
libraries/AP_Motors/AP_MotorsCoax.h
View File

@ -13,6 +13,8 @@
#define AP_MOTORS_COAX_POSITIVE 1
#define AP_MOTORS_COAX_NEGATIVE -1
#define NUM_ACTUATORS 4
#define AP_MOTORS_SINGLE_SPEED_DIGITAL_SERVOS 250 // update rate for digital servos
#define AP_MOTORS_SINGLE_SPEED_ANALOG_SERVOS 125 // update rate for analog servos
@ -61,6 +63,9 @@ protected:
// output - sends commands to the motors
void output_armed_stabilizing();
// calc_yaw_radio_output - calculate final radio output for yaw channel
int16_t calc_pivot_radio_output(float yaw_input, int16_t servo_min, int16_t servo_trim, int16_t servo_max); // calculate radio output for yaw servo, typically in range of 1100-1900
// We shouldn't need roll, pitch, and yaw reversing with servo reversing.
AP_Int8 _roll_reverse; // Reverse roll output
AP_Int8 _pitch_reverse; // Reverse pitch output
@ -89,4 +94,7 @@ protected:
AP_Int16 _servo_4_min; // Minimum angle limit of pitch servo
AP_Int16 _servo_4_max; // Maximum angle limit of pitch servo
float _actuator_out[NUM_ACTUATORS]; // combined roll, pitch, yaw and throttle outputs to motors in 0~1 range
float _thrust_yt_ccw;
float _thrust_yt_cw;
};