Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 10:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Motors: added reached_limit method which returns bit mask indicating which control inputs could not be achieved

Browse Source
This commit is contained in:
rmackay9 2012-10-10 15:54:13 +09:00
parent fc1774d77f
commit 1df891e2ce
2 changed files with 155 additions and 128 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_Motors/AP_Motors.h
View File

@ -50,6 +50,12 @@
#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)
// bit mask for recording which limits we have reached when outputting to motors
#define AP_MOTOR_NO_LIMITS_REACHED 0x00
#define AP_MOTOR_ROLLPITCH_LIMIT 0x01
#define AP_MOTOR_YAW_LIMIT 0x02
#define AP_MOTOR_THROTTLE_LIMIT 0x04
/// @class AP_Motors
class AP_Motors {
public:
@ -120,6 +126,11 @@ public:
virtual void output_min() {
};
// reached_limits - return whether we hit the limits of the motors
virtual uint8_t reached_limit( uint8_t which_limit = 0x00 ) {
return _reached_limit & which_limit;
}
// get basic information about the platform
virtual uint8_t get_num_motors() {
return 0;
@ -169,6 +180,7 @@ protected:
AP_Int8 _throttle_curve_enabled; // enable throttle curve
AP_Int8 _throttle_curve_mid; // throttle which produces 1/2 the maximum thrust. expressed as a percentage (i.e. 0 ~ 100 ) of the full throttle range
AP_Int8 _throttle_curve_max; // throttle which produces the maximum thrust. expressed as a percentage (i.e. 0 ~ 100 ) of the full throttle range
uint8_t _reached_limit; // bit mask to record which motor limits we hit (if any) during most recent output. Used to provide feedback to attitude controllers
};
#endif // AP_MOTORS

271
libraries/AP_Motors/AP_MotorsMatrix.cpp
View File

@ -109,142 +109,18 @@ void AP_MotorsMatrix::output_armed()
int16_t motor_adjustment = 0;
int16_t yaw_to_execute = 0;
// initialize reached_limit flag
_reached_limit = AP_MOTOR_NO_LIMITS_REACHED;
// Throttle is 0 to 1000 only
_rc_throttle->servo_out = constrain(_rc_throttle->servo_out, 0, _max_throttle);
if(_rc_throttle->servo_out > 0)
out_min = _rc_throttle->radio_min + _min_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();
// 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;
}
}
}
// 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;
}
}
}
}
// 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(motor_out[i], out_min, out_max);
}
}
#if CUT_MOTORS == ENABLED
// 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++ ) {
@ -252,8 +128,147 @@ void AP_MotorsMatrix::output_armed()
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 ) {
_reached_limit |= AP_MOTOR_ROLLPITCH_LIMIT;
}
if( _rc_yaw->pwm_out != 0 ) {
_reached_limit |= AP_MOTOR_YAW_LIMIT;
}
} 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
_reached_limit |= AP_MOTOR_ROLLPITCH_LIMIT | AP_MOTOR_YAW_LIMIT | AP_MOTOR_THROTTLE_LIMIT;
}
// 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 ) {
_reached_limit |= AP_MOTOR_YAW_LIMIT;
}
}
// 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(motor_out[i], out_min, out_max);
}
}
}
#endif
// send output to each motor
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {