From f74d22135cc23b36f142c955314408fb3adc566d Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 10 Nov 2022 12:01:20 +0900 Subject: [PATCH] Rover: integrate balancebot pitch limit protection --- Rover/balance_bot.cpp | 2 +- Rover/mode.cpp | 4 +++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/Rover/balance_bot.cpp b/Rover/balance_bot.cpp index 2e55174cce..9b50625b10 100644 --- a/Rover/balance_bot.cpp +++ b/Rover/balance_bot.cpp @@ -8,7 +8,7 @@ void Rover::balancebot_pitch_control(float &throttle) const float demanded_pitch = radians(-throttle * 0.01f * g2.bal_pitch_max) + radians(g2.bal_pitch_trim); // calculate required throttle using PID controller - 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; + throttle = g2.attitude_control.get_throttle_out_from_pitch(demanded_pitch, radians(g2.bal_pitch_max), g2.motors.limit.throttle_lower || g2.motors.limit.throttle_upper, G_Dt) * 100.0f; } // returns true if vehicle is a balance bot diff --git a/Rover/mode.cpp b/Rover/mode.cpp index 76898d9bf6..1509b72436 100644 --- a/Rover/mode.cpp +++ b/Rover/mode.cpp @@ -317,7 +317,9 @@ void Mode::calc_throttle(float target_speed, bool avoidance_enabled) bool stopped; throttle_out = 100.0f * attitude_control.get_throttle_out_stop(g2.motors.limit.throttle_lower, g2.motors.limit.throttle_upper, g.speed_cruise, g.throttle_cruise * 0.01f, rover.G_Dt, stopped); } else { - throttle_out = 100.0f * attitude_control.get_throttle_out_speed(target_speed, g2.motors.limit.throttle_lower, g2.motors.limit.throttle_upper, g.speed_cruise, g.throttle_cruise * 0.01f, rover.G_Dt); + bool motor_lim_low = g2.motors.limit.throttle_lower || attitude_control.pitch_limited(); + bool motor_lim_high = g2.motors.limit.throttle_upper || attitude_control.pitch_limited(); + throttle_out = 100.0f * attitude_control.get_throttle_out_speed(target_speed, motor_lim_low, motor_lim_high, g.speed_cruise, g.throttle_cruise * 0.01f, rover.G_Dt); } // if vehicle is balance bot, calculate actual throttle required for balancing