diff --git a/APMrover2/BalanceBot.cpp b/APMrover2/BalanceBot.cpp index 6a3a40fde4..03168da374 100644 --- a/APMrover2/BalanceBot.cpp +++ b/APMrover2/BalanceBot.cpp @@ -2,21 +2,22 @@ #include "Rover.h" // Function to set a desired pitch angle according to throttle -void Rover::balance_pitch(float &throttle, bool armed) +void Rover::balancebot_pitch_control(float &throttle, bool armed) { // calculate desired pitch angle - float demanded_pitch = radians(-(throttle/100) * g2.bal_pitch_max); + const float demanded_pitch = radians(-throttle * 0.01f * g2.bal_pitch_max); // calculate required throttle using PID controller - float balance_throttle = g2.attitude_control.get_throttle_out_from_pitch(demanded_pitch, armed)*100; + const float balance_throttle = g2.attitude_control.get_throttle_out_from_pitch(demanded_pitch, armed) * 100.0f; - throttle = constrain_float(balance_throttle, -100, 100); + // constrain throttle between -100 and 100 + throttle = constrain_float(balance_throttle, -100.0f, 100.0f); } // returns true if vehicle is a balance bot // called in AP_MotorsUGV::output() // this affects whether the vehicle tries to control its pitch with throttle output -bool Rover::is_BalanceBot() const +bool Rover::is_balancebot() const { return ((enum frame_class)g2.frame_class.get() == FRAME_BALANCEBOT); } diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index d9abcb183c..591ecf6493 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -559,8 +559,8 @@ public: void failsafe_check(); // BalanceBot.cpp - void balance_pitch(float &, bool); - bool is_BalanceBot() const; + void balancebot_pitch_control(float &, bool); + bool is_balancebot() const; void update_soft_armed(); // Motor test diff --git a/APMrover2/mode.cpp b/APMrover2/mode.cpp index 2c4549e90f..d6fd83393a 100644 --- a/APMrover2/mode.cpp +++ b/APMrover2/mode.cpp @@ -252,6 +252,11 @@ void Mode::calc_throttle(float target_speed, bool nudge_allowed, bool avoidance_ 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); } + // if vehicle is balance bot, calculate actual throttle required for balancing + if (rover.is_balancebot()) { + rover.balancebot_pitch_control(throttle_out, rover.arming.is_armed()); + } + // send to motor g2.motors.set_throttle(throttle_out); } diff --git a/APMrover2/mode_hold.cpp b/APMrover2/mode_hold.cpp index 4f7aaed6ad..7d950d57ca 100644 --- a/APMrover2/mode_hold.cpp +++ b/APMrover2/mode_hold.cpp @@ -3,7 +3,14 @@ void ModeHold::update() { + float throttle = 0.0f; + + // if vehicle is balance bot, calculate actual throttle required for balancing + if (rover.is_balancebot()) { + rover.balancebot_pitch_control(throttle, rover.arming.is_armed()); + } + // hold position - stop motors and center steering - g2.motors.set_throttle(0.0f); + g2.motors.set_throttle(throttle); g2.motors.set_steering(0.0f); } diff --git a/APMrover2/mode_manual.cpp b/APMrover2/mode_manual.cpp index 992c0f2e1c..55c4528140 100644 --- a/APMrover2/mode_manual.cpp +++ b/APMrover2/mode_manual.cpp @@ -14,8 +14,8 @@ void ModeManual::update() get_pilot_desired_lateral(desired_lateral); // if vehicle is balance bot, calculate actual throttle required for balancing - if (rover.is_BalanceBot()) { - rover.balance_pitch(desired_throttle, rover.arming.is_armed()); + if (rover.is_balancebot()) { + rover.balancebot_pitch_control(desired_throttle, rover.arming.is_armed()); } // copy RC scaled inputs to outputs