From 6dcab4e87b59dea799b8b11e1c2fff6b1b76f0d6 Mon Sep 17 00:00:00 2001 From: Ebin Date: Mon, 23 Jul 2018 11:50:46 +0530 Subject: [PATCH] Rover: support balance bot in AUTO mdoe --- APMrover2/Parameters.cpp | 2 +- APMrover2/mode.cpp | 5 +++++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index cbf1a96cff..c59be72eb6 100644 --- a/APMrover2/Parameters.cpp +++ b/APMrover2/Parameters.cpp @@ -554,7 +554,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Range: 0 5 // @Increment: 0.1 // @User: Standard - AP_GROUPINFO("BAL_PITCH_MAX", 21, ParametersG2, bal_pitch_max, 5), + AP_GROUPINFO("BAL_PITCH_MAX", 21, ParametersG2, bal_pitch_max, 2), // @Param: CRASH_ANGLE // @DisplayName: Crash Angle diff --git a/APMrover2/mode.cpp b/APMrover2/mode.cpp index 94b1b54939..bba8492f3a 100644 --- a/APMrover2/mode.cpp +++ b/APMrover2/mode.cpp @@ -268,6 +268,11 @@ bool Mode::stop_vehicle() bool stopped = false; float 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); + // 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);