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

41
libraries/AP_Motors/AP_MotorsMatrix.cpp
View File

@ -109,18 +109,36 @@ 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();
// 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 ) {
_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 ) {
@ -180,6 +198,9 @@ void AP_MotorsMatrix::output_armed()
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
@ -226,6 +247,10 @@ void AP_MotorsMatrix::output_armed()
}
}
}
// 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
@ -243,17 +268,7 @@ void AP_MotorsMatrix::output_armed()
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++ ) {
if( motor_enabled[i] ) {
motor_out[i] = _rc_throttle->radio_min;
}
}
}
#endif
// send output to each motor
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {