Browse Source

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

limit.throttle_lower flag becomes true when the throttle passed into the
motors lib (which is in the 0 ~ 1000 range) is below _min throttle.
This makes the interpretation of the THR_MIN parameter consistent
between the main code (which uses 0 ~ 1000 range) and the motors lib
(which previously used the RC3_MIN ~ RC3_MAX range).
The remaining problem however is that the output of the motors continues
to use THR_MIN as if it were a pwm.  I don't believe this is a dangerous
problem however.
mission-4.1.18
Randy Mackay 10 years ago
parent
commit
91e5201439
  1. 14
      libraries/AP_Motors/AP_MotorsTri.cpp

14
libraries/AP_Motors/AP_MotorsTri.cpp

@ -99,7 +99,14 @@ void AP_MotorsTri::output_armed() @@ -99,7 +99,14 @@ void AP_MotorsTri::output_armed()
limit.throttle_lower = false;
// Throttle is 0 to 1000 only
_rc_throttle.servo_out = constrain_int16(_rc_throttle.servo_out, 0, _max_throttle);
if (_rc_throttle.servo_out <= 0) {
_rc_throttle.servo_out = 0;
limit.throttle_lower = true;
}
if (_rc_throttle.servo_out >= _max_throttle) {
_rc_throttle.servo_out = _max_throttle;
limit.throttle_upper = true;
}
// capture desired roll, pitch, yaw and throttle from receiver
_rc_roll.calc_pwm();
@ -120,15 +127,12 @@ void AP_MotorsTri::output_armed() @@ -120,15 +127,12 @@ void AP_MotorsTri::output_armed()
motor_out[AP_MOTORS_MOT_2] = _rc_throttle.radio_min + _spin_when_armed_ramped;
motor_out[AP_MOTORS_MOT_4] = _rc_throttle.radio_min + _spin_when_armed_ramped;
// Every thing is limited
limit.throttle_lower = true;
}else{
int16_t roll_out = (float)_rc_roll.pwm_out * 0.866f;
int16_t pitch_out = _rc_pitch.pwm_out / 2;
// check if throttle is below limit
if (_rc_throttle.radio_out <= out_min) {
if (_rc_throttle.servo_out <= _min_throttle) {
limit.throttle_lower = true;
}
//left front

Loading…
Cancel
Save