diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index 100ff80b9d..b4b02ddc9f 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -388,6 +388,10 @@ private: void update_GPS(void); void update_current_mode(void); + // balance_bot.cpp + void balancebot_pitch_control(float &throttle); + bool is_balancebot() const; + // capabilities.cpp void init_capabilities(void); @@ -552,11 +556,6 @@ private: public: void mavlink_delay_cb(); void failsafe_check(); - - // BalanceBot.cpp - void balancebot_pitch_control(float &, bool); - bool is_balancebot() const; - void update_soft_armed(); // Motor test void motor_test_output(); diff --git a/APMrover2/balance_bot.cpp b/APMrover2/balance_bot.cpp index 87747f6a45..063e79726a 100644 --- a/APMrover2/balance_bot.cpp +++ b/APMrover2/balance_bot.cpp @@ -2,16 +2,13 @@ #include "Rover.h" // Function to set a desired pitch angle according to throttle -void Rover::balancebot_pitch_control(float &throttle, bool armed) +void Rover::balancebot_pitch_control(float &throttle) { // calculate desired pitch angle 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, 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); + 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; } // returns true if vehicle is a balance bot diff --git a/APMrover2/mode.cpp b/APMrover2/mode.cpp index 3acd589ee0..00b93af73f 100644 --- a/APMrover2/mode.cpp +++ b/APMrover2/mode.cpp @@ -267,7 +267,7 @@ void Mode::calc_throttle(float target_speed, bool nudge_allowed, bool avoidance_ // 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()); + rover.balancebot_pitch_control(throttle_out); } // send to motor @@ -283,7 +283,7 @@ bool Mode::stop_vehicle() // 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()); + rover.balancebot_pitch_control(throttle_out); } // send to motor diff --git a/APMrover2/mode_hold.cpp b/APMrover2/mode_hold.cpp index 7d950d57ca..86619d0854 100644 --- a/APMrover2/mode_hold.cpp +++ b/APMrover2/mode_hold.cpp @@ -7,7 +7,7 @@ void ModeHold::update() // if vehicle is balance bot, calculate actual throttle required for balancing if (rover.is_balancebot()) { - rover.balancebot_pitch_control(throttle, rover.arming.is_armed()); + rover.balancebot_pitch_control(throttle); } // hold position - stop motors and center steering diff --git a/APMrover2/mode_manual.cpp b/APMrover2/mode_manual.cpp index 55c4528140..f2e9663bdd 100644 --- a/APMrover2/mode_manual.cpp +++ b/APMrover2/mode_manual.cpp @@ -15,7 +15,7 @@ void ModeManual::update() // if vehicle is balance bot, calculate actual throttle required for balancing if (rover.is_balancebot()) { - rover.balancebot_pitch_control(desired_throttle, rover.arming.is_armed()); + rover.balancebot_pitch_control(desired_throttle); } // copy RC scaled inputs to outputs