|
|
|
@ -8,7 +8,7 @@ void Rover::balancebot_pitch_control(float &throttle, bool armed)
@@ -8,7 +8,7 @@ void Rover::balancebot_pitch_control(float &throttle, bool armed)
|
|
|
|
|
const float demanded_pitch = radians(-throttle * 0.01f * g2.bal_pitch_max); |
|
|
|
|
|
|
|
|
|
// calculate required throttle using PID controller
|
|
|
|
|
const float balance_throttle = g2.attitude_control.get_throttle_out_from_pitch(demanded_pitch, armed, G_Dt) * 100.0f; |
|
|
|
|
const float balance_throttle = g2.attitude_control.get_throttle_out_from_pitch(demanded_pitch, g2.motors.limit.throttle_lower, g2.motors.limit.throttle_upper, G_Dt) * 100.0f; |
|
|
|
|
|
|
|
|
|
// constrain throttle between -100 and 100
|
|
|
|
|
throttle = constrain_float(balance_throttle, -100.0f, 100.0f); |
|
|
|
|