Rover: balance bot stands in acro with no position estimate

This commit is contained in:
Randy Mackay 2022-03-15 20:07:13 +09:00
parent a1856c5c22
commit a79ad5489c
1 changed files with 6 additions and 0 deletions

View File

@ -9,6 +9,12 @@ void ModeAcro::update()
float desired_throttle;
// convert pilot stick input into desired steering and throttle
get_pilot_desired_steering_and_throttle(desired_steering, desired_throttle);
// if vehicle is balance bot, calculate actual throttle required for balancing
if (rover.is_balancebot()) {
rover.balancebot_pitch_control(desired_throttle);
}
// no valid speed, just use the provided throttle
g2.motors.set_throttle(desired_throttle);
} else {