mirror of https://github.com/ArduPilot/ardupilot
AP_Motors: move constraints to set_throttle
This commit is contained in:
parent
89345bad7a
commit
11214b3414
|
@ -838,8 +838,8 @@ void AP_MotorsHeli::update_throttle_filter()
|
|||
{
|
||||
_throttle_filter.apply(_throttle_in, 1.0f/_loop_rate);
|
||||
|
||||
// prevent _rc_throttle.servo_out from wrapping at int16 max or min
|
||||
_throttle_control_input = constrain_float(_throttle_filter.get(),-32000,32000);
|
||||
// constrain throttle signal to 0-1000
|
||||
_throttle_control_input = constrain_float(_throttle_filter.get(),0.0f,1000.0f);
|
||||
}
|
||||
|
||||
// set_radio_passthrough used to pass radio inputs directly to outputs
|
||||
|
|
|
@ -244,13 +244,13 @@ void AP_Motors::slow_start(bool true_false)
|
|||
void AP_Motors::update_throttle_filter()
|
||||
{
|
||||
if (armed()) {
|
||||
_throttle_filter.apply(constrain_float(_throttle_in,-100,1100), 1.0f/_loop_rate);
|
||||
_throttle_filter.apply(_throttle_in, 1.0f/_loop_rate);
|
||||
} else {
|
||||
_throttle_filter.reset(0.0f);
|
||||
}
|
||||
|
||||
// prevent _throttle_control_input from wrapping at int16 max or min
|
||||
_throttle_control_input = constrain_float(_throttle_filter.get(),-32000,32000);
|
||||
// constrain throttle signal to 0-1000
|
||||
_throttle_control_input = constrain_float(_throttle_filter.get(),0.0f,1000.0f);
|
||||
}
|
||||
|
||||
// update_max_throttle - updates the limits on _max_throttle if necessary taking into account slow_start_throttle flag
|
||||
|
|
|
@ -131,7 +131,7 @@ public:
|
|||
void set_roll(int16_t roll_in) { _roll_control_input = roll_in; }; // range -4500 ~ 4500
|
||||
void set_pitch(int16_t pitch_in) { _pitch_control_input = pitch_in; }; // range -4500 ~ 4500
|
||||
void set_yaw(int16_t yaw_in) { _yaw_control_input = yaw_in; }; // range -4500 ~ 4500
|
||||
void set_throttle(float throttle_in) { _throttle_in = throttle_in; }; // range 0 ~ 1000
|
||||
void set_throttle(float throttle_in) { _throttle_in = constrain_float(throttle_in,-100.0f,1100.0f); }; // range 0 ~ 1000
|
||||
void set_stabilizing(bool stabilizing) { _flags.stabilizing = stabilizing; }
|
||||
|
||||
// accessors for roll, pitch, yaw and throttle inputs to motors
|
||||
|
|
Loading…
Reference in New Issue