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

Copter Motors: replace limit bitmask with structure

Browse Source
This commit is contained in:
Randy Mackay 2013-07-21 13:58:24 +09:00
parent ac0886334d
commit 43379f20c3
3 changed files with 37 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduCopter/Attitude.pde
View File

@ -583,7 +583,7 @@ get_rate_roll(int32_t target_rate)
i = g.pid_rate_roll.get_integrator();
// update i term as long as we haven't breached the limits or the I term will certainly reduce
if (!motors.reached_limit(AP_MOTOR_ROLLPITCH_LIMIT) || ((i>0&&rate_error<0)||(i<0&&rate_error>0))) {
if (!motors.limit.roll_pitch || ((i>0&&rate_error<0)||(i<0&&rate_error>0))) {
i = g.pid_rate_roll.get_i(rate_error, G_Dt);
}
@ -627,7 +627,7 @@ get_rate_pitch(int32_t target_rate)
i = g.pid_rate_pitch.get_integrator();
// update i term as long as we haven't breached the limits or the I term will certainly reduce
if (!motors.reached_limit(AP_MOTOR_ROLLPITCH_LIMIT) || ((i>0&&rate_error<0)||(i<0&&rate_error>0))) {
if (!motors.limit.roll_pitch || ((i>0&&rate_error<0)||(i<0&&rate_error>0))) {
i = g.pid_rate_pitch.get_i(rate_error, G_Dt);
}
@ -667,7 +667,7 @@ get_rate_yaw(int32_t target_rate)
i = g.pid_rate_yaw.get_integrator();
// update i term as long as we haven't breached the limits or the I term will certainly reduce
if (!motors.reached_limit(AP_MOTOR_YAW_LIMIT) || ((i>0&&rate_error<0)||(i<0&&rate_error>0))) {
if (!motors.limit.yaw || ((i>0&&rate_error<0)||(i<0&&rate_error>0))) {
i = g.pid_rate_yaw.get_i(rate_error, G_Dt);
}
@ -972,7 +972,7 @@ get_throttle_accel(int16_t z_target_accel)
// separately calculate p, i, d values for logging
p = g.pid_throttle_accel.get_p(z_accel_error);
// freeze I term if we've breached throttle limits
if( motors.reached_limit(AP_MOTOR_THROTTLE_LIMIT) ) {
if (motors.limit.throttle) {
i = g.pid_throttle_accel.get_integrator();
}else{
i = g.pid_throttle_accel.get_i(z_accel_error, .01f);
@ -1121,7 +1121,7 @@ get_throttle_rate(float z_target_speed)
p = g.pid_throttle_rate.get_p(z_rate_error);
// freeze I term if we've breached throttle limits
if(motors.reached_limit(AP_MOTOR_THROTTLE_LIMIT)) {
if(motors.limit.throttle) {
i = g.pid_throttle_rate.get_integrator();
}else{
i = g.pid_throttle_rate.get_i(z_rate_error, .02);

40
libraries/AP_Motors/AP_MotorsMatrix.cpp
View File

@ -108,8 +108,10 @@ void AP_MotorsMatrix::output_armed()
int16_t yaw_allowed; // amount of yaw we can fit in
int16_t thr_adj; // how far we move the throttle point from out_max_range
// initialize reached_limit flag
_reached_limit = AP_MOTOR_NO_LIMITS_REACHED;
// initialize limits flag
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle = false;
// Throttle is 0 to 1000 only
// To-Do: we should not really be limiting this here because we don't "own" this _rc_throttle object
@ -138,13 +140,15 @@ void AP_MotorsMatrix::output_armed()
}
// Every thing is limited
_reached_limit |= AP_MOTOR_ROLLPITCH_LIMIT | AP_MOTOR_YAW_LIMIT | AP_MOTOR_THROTTLE_LIMIT;
limit.roll_pitch = true;
limit.yaw = true;
limit.throttle = true;
} else {
// check if throttle is below limit
if (_rc_throttle->radio_out < out_min) {
_reached_limit |= AP_MOTOR_THROTTLE_LIMIT;
limit.throttle = true;
}
// calculate roll and pitch for each motor
@ -181,7 +185,7 @@ void AP_MotorsMatrix::output_armed()
if (yaw_allowed > _rc_yaw->pwm_out) {
yaw_allowed = _rc_yaw->pwm_out; // to-do: this is bad form for yaw_allows to change meaning to become the amount that we are going to output
}else{
_reached_limit |= AP_MOTOR_YAW_LIMIT;
limit.yaw = true;
}
}else{
// if yawing left
@ -189,7 +193,7 @@ void AP_MotorsMatrix::output_armed()
if( yaw_allowed < _rc_yaw->pwm_out ) {
yaw_allowed = _rc_yaw->pwm_out; // to-do: this is bad form for yaw_allows to change meaning to become the amount that we are going to output
}else{
_reached_limit |= AP_MOTOR_YAW_LIMIT;
limit.yaw = true;
}
}
@ -219,7 +223,7 @@ void AP_MotorsMatrix::output_armed()
if (thr_adj > out_max-(rpy_high+out_max_range)){
thr_adj = out_max-(rpy_high+out_max_range);
// we haven't even been able to apply full throttle command
_reached_limit |= AP_MOTOR_THROTTLE_LIMIT;
limit.throttle = true;
}
}else if(thr_adj < 0){
// decrease throttle as close as possible to requested throttle
@ -233,11 +237,13 @@ void AP_MotorsMatrix::output_armed()
if ((rpy_low+out_max_range)+thr_adj < out_min){
rpy_scale = (float)(out_min-thr_adj-out_max_range)/rpy_low;
// we haven't even been able to apply full roll, pitch and minimal yaw without scaling
_reached_limit |= AP_MOTOR_ROLLPITCH_LIMIT | AP_MOTOR_YAW_LIMIT;
limit.roll_pitch = true;
limit.yaw = true;
}else if((rpy_high+out_max_range)+thr_adj > out_max){
rpy_scale = (float)(out_max-thr_adj-out_max_range)/rpy_high;
// we haven't even been able to apply full roll, pitch and minimal yaw without scaling
_reached_limit |= AP_MOTOR_ROLLPITCH_LIMIT | AP_MOTOR_YAW_LIMIT;
limit.roll_pitch = true;
limit.yaw = true;
}
// add scaled roll, pitch, constrained yaw and throttle for each motor
@ -284,8 +290,10 @@ 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;
// 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);
@ -305,10 +313,10 @@ void AP_MotorsMatrix::output_armed()
}
// 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;
limit.roll_pitch = true;
}
if( _rc_yaw->pwm_out != 0 ) {
_reached_limit |= AP_MOTOR_YAW_LIMIT;
limit.yaw = true;
}
} else { // non-zero throttle
@ -375,7 +383,9 @@ void AP_MotorsMatrix::output_armed()
}
// 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;
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
@ -424,7 +434,7 @@ 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;
limit.yaw = true;
}
}

13
libraries/AP_Motors/AP_Motors_Class.h
View File

@ -104,11 +104,6 @@ public:
// output_min - sends minimum values out to the motors
virtual void output_min() = 0;
// reached_limits - return whether we hit the limits of the motors
uint8_t reached_limit( uint8_t which_limit = AP_MOTOR_ANY_LIMIT ) {
return _reached_limit & which_limit;
}
// motor test
virtual void output_test() = 0;
@ -125,6 +120,13 @@ public:
// final output values sent to the motors. public (for now) so that they can be access for logging
int16_t motor_out[AP_MOTORS_MAX_NUM_MOTORS];
// structure for holding motor limit flags
struct AP_Motors_limit {
uint8_t roll_pitch : 1; // we have reached roll or pitch limit
uint8_t yaw : 1; // we have reached yaw limit
uint8_t throttle : 1; // we have reached throttle limit
} limit;
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];
@ -147,7 +149,6 @@ 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
// for new stability patch
int16_t _hover_out; // the estimated hover throttle in pwm (i.e. 1000 ~ 2000). calculated from the THR_MID parameter