zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Copter Motors: remove old style stability patch

mission-4.1.18
Randy Mackay 12 years ago
parent
c5851d2ddf
commit
f3a2db195e
2 changed files with 0 additions and 190 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 187
      libraries/AP_Motors/AP_MotorsMatrix.cpp
  2. 3
      libraries/AP_Motors/AP_MotorsMatrix.h

187
libraries/AP_Motors/AP_MotorsMatrix.cpp
Unescape View File

@ -89,7 +89,6 @@ void AP_MotorsMatrix::output_min() @@ -89,7 +89,6 @@ void AP_MotorsMatrix::output_min()
}
}
#ifdef AP_MOTORS_MATRIX_SCALING_STABILITY_PATCH
// output_armed - sends commands to the motors
// includes new scaling stability patch
void AP_MotorsMatrix::output_armed()
@ -277,192 +276,6 @@ void AP_MotorsMatrix::output_armed() @@ -277,192 +276,6 @@ void AP_MotorsMatrix::output_armed()
}
}
}
#else
// output_armed - sends commands to the motors
void AP_MotorsMatrix::output_armed()
{
int8_t i;
int16_t out_min = _rc_throttle->radio_min;
int16_t out_max = _rc_throttle->radio_max;
int16_t rc_yaw_constrained_pwm;
int16_t rc_yaw_excess;
int16_t upper_margin, lower_margin;
int16_t motor_adjustment = 0;
int16_t yaw_to_execute = 0;
// initialize limits flag
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle = false;
// Throttle is 0 to 1000 only
_rc_throttle->servo_out = constrain_int16(_rc_throttle->servo_out, 0, _max_throttle);
// capture desired roll, pitch, yaw and throttle from receiver
_rc_roll->calc_pwm();
_rc_pitch->calc_pwm();
_rc_throttle->calc_pwm();
_rc_yaw->calc_pwm();
// if we are not sending a throttle output, we cut the motors
if(_rc_throttle->servo_out == 0) {
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] ) {
motor_out[i] = _rc_throttle->radio_min;
}
}
// if we have any roll, pitch or yaw input then it's breaching the limit
if( _rc_roll->pwm_out != 0 || _rc_pitch->pwm_out != 0 ) {
limit.roll_pitch = true;
}
if( _rc_yaw->pwm_out != 0 ) {
limit.yaw = true;
}
} else { // non-zero throttle
out_min = _rc_throttle->radio_min + _min_throttle;
// initialise rc_yaw_contrained_pwm that we will certainly output and rc_yaw_excess that we will do on best-efforts basis.
// Note: these calculations and many others below depend upon _yaw_factors always being 0, -1 or 1.
if( _rc_yaw->pwm_out < -AP_MOTORS_MATRIX_YAW_LOWER_LIMIT_PWM ) {
rc_yaw_constrained_pwm = -AP_MOTORS_MATRIX_YAW_LOWER_LIMIT_PWM;
rc_yaw_excess = _rc_yaw->pwm_out+AP_MOTORS_MATRIX_YAW_LOWER_LIMIT_PWM;
}else if( _rc_yaw->pwm_out > AP_MOTORS_MATRIX_YAW_LOWER_LIMIT_PWM ) {
rc_yaw_constrained_pwm = AP_MOTORS_MATRIX_YAW_LOWER_LIMIT_PWM;
rc_yaw_excess = _rc_yaw->pwm_out-AP_MOTORS_MATRIX_YAW_LOWER_LIMIT_PWM;
}else{
rc_yaw_constrained_pwm = _rc_yaw->pwm_out;
rc_yaw_excess = 0;
}
// initialise upper and lower margins
upper_margin = lower_margin = out_max - out_min;
// add roll, pitch, throttle and constrained yaw for each motor
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] ) {
motor_out[i] = _rc_throttle->radio_out +
_rc_roll->pwm_out * _roll_factor[i] +
_rc_pitch->pwm_out * _pitch_factor[i] +
rc_yaw_constrained_pwm * _yaw_factor[i];
// calculate remaining room between fastest running motor and top of pwm range
if( out_max - motor_out[i] < upper_margin) {
upper_margin = out_max - motor_out[i];
}
// calculate remaining room between slowest running motor and bottom of pwm range
if( motor_out[i] - out_min < lower_margin ) {
lower_margin = motor_out[i] - out_min;
}
}
}
// if motors are running too fast and we have enough room below, lower overall throttle
if( upper_margin < 0 || lower_margin < 0 ) {
// calculate throttle adjustment that equalizes upper and lower margins. We will never push the throttle beyond this point
motor_adjustment = (upper_margin - lower_margin) / 2; // i.e. if overflowed by 20 on top, 30 on bottom, upper_margin = -20, lower_margin = -30. will adjust motors -5.
// if we have overflowed on the top, reduce but no more than to the mid point
if( upper_margin < 0 ) {
motor_adjustment = max(upper_margin, motor_adjustment);
}
// if we have underflowed on the bottom, increase throttle but no more than to the mid point
if( lower_margin < 0 ) {
motor_adjustment = min(-lower_margin, motor_adjustment);
}
}
// move throttle up or down to to pull within tolerance
if( motor_adjustment != 0 ) {
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] ) {
motor_out[i] += motor_adjustment;
}
}
// we haven't even been able to apply roll, pitch and minimal yaw without adjusting throttle so mark all limits as breached
limit.roll_pitch = true;
limit.yaw = true;
limit.throttle = true;
}
// if we didn't give all the yaw requested, calculate how much additional yaw we can add
if( rc_yaw_excess != 0 ) {
// try for everything
yaw_to_execute = rc_yaw_excess;
// loop through motors and reduce as necessary
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] && _yaw_factor[i] != 0 ) {
// calculate upper and lower margins for this motor
upper_margin = max(0,out_max - motor_out[i]);
lower_margin = max(0,motor_out[i] - out_min);
// motor is increasing, check upper limit
if( rc_yaw_excess > 0 && _yaw_factor[i] > 0 ) {
yaw_to_execute = min(yaw_to_execute, upper_margin);
}
// motor is decreasing, check lower limit
if( rc_yaw_excess > 0 && _yaw_factor[i] < 0 ) {
yaw_to_execute = min(yaw_to_execute, lower_margin);
}
// motor is decreasing, check lower limit
if( rc_yaw_excess < 0 && _yaw_factor[i] > 0 ) {
yaw_to_execute = max(yaw_to_execute, -lower_margin);
}
// motor is increasing, check upper limit
if( rc_yaw_excess < 0 && _yaw_factor[i] < 0 ) {
yaw_to_execute = max(yaw_to_execute, -upper_margin);
}
}
}
// check yaw_to_execute is reasonable
if( yaw_to_execute != 0 && ((yaw_to_execute>0 && rc_yaw_excess>0) || (yaw_to_execute<0 && rc_yaw_excess<0)) ) {
// add the additional yaw
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] ) {
motor_out[i] += _yaw_factor[i] * yaw_to_execute;
}
}
}
// mark yaw limit reached if we didn't get everything we asked for
if( yaw_to_execute != rc_yaw_excess ) {
limit.yaw = true;
}
}
// adjust for throttle curve
if( _throttle_curve_enabled ) {
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] ) {
motor_out[i] = _throttle_curve.get_y(motor_out[i]);
}
}
}
// clip motor output if required (shouldn't be)
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] ) {
motor_out[i] = constrain_int16(motor_out[i], out_min, out_max);
}
}
}
// send output to each motor
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] ) {
hal.rcout->write(_motor_to_channel_map[i], motor_out[i]);
}
}
}
#endif // AP_MOTORS_MATRIX_SCALING_STABILITY_PATCH
// output_disarmed - sends commands to the motors
void AP_MotorsMatrix::output_disarmed()

3
libraries/AP_Motors/AP_MotorsMatrix.h
Unescape View File

@ -11,9 +11,6 @@ @@ -11,9 +11,6 @@
#include <RC_Channel.h> // RC Channel Library
#include "AP_Motors_Class.h"
// comment out the line below to return to stability patch used in ArduCopter ver 2.8.1 ~ 2.9.1b
#define AP_MOTORS_MATRIX_SCALING_STABILITY_PATCH
#define AP_MOTORS_MATRIX_YAW_FACTOR_CW -1
#define AP_MOTORS_MATRIX_YAW_FACTOR_CCW 1

Write Preview
Loading…
Cancel
Save