Browse Source

MotorsMatrix: _min_throttle interpreted as 0 ~ 1000 range for throttle_lower flag

Also trigger throttle_upper flag when throttle in reaches 1000
mission-4.1.18
Randy Mackay 11 years ago
parent
commit
85fb4b122a
  1. 7
      libraries/AP_Motors/AP_MotorsMatrix.cpp

7
libraries/AP_Motors/AP_MotorsMatrix.cpp

@ -145,11 +145,11 @@ void AP_MotorsMatrix::output_armed() @@ -145,11 +145,11 @@ void AP_MotorsMatrix::output_armed()
// 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
if (_rc_throttle.servo_out < 0) {
if (_rc_throttle.servo_out <= 0) {
_rc_throttle.servo_out = 0;
limit.throttle_lower = true;
}
if (_rc_throttle.servo_out > _max_throttle) {
if (_rc_throttle.servo_out >= _max_throttle) {
_rc_throttle.servo_out = _max_throttle;
limit.throttle_upper = true;
}
@ -179,12 +179,11 @@ void AP_MotorsMatrix::output_armed() @@ -179,12 +179,11 @@ void AP_MotorsMatrix::output_armed()
// Every thing is limited
limit.roll_pitch = true;
limit.yaw = true;
limit.throttle_lower = true;
} else {
// check if throttle is below limit
if (_rc_throttle.radio_out <= out_min_pwm) { // perhaps being at min throttle itself is not a problem, only being under is
if (_rc_throttle.servo_out <= _min_throttle) { // perhaps being at min throttle itself is not a problem, only being under is
limit.throttle_lower = true;
}

Loading…
Cancel
Save