diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 1041276291..7a17a0c5c7 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -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); diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index 6a57910458..c3be8cffa6 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -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; } } diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index 9af5f839c3..b622cf28a1 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -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