diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index 153354ca1c..9583896e6b 100644 --- a/APMrover2/Parameters.cpp +++ b/APMrover2/Parameters.cpp @@ -694,6 +694,15 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { AP_SUBGROUPINFO(gripper, "GRIP_", 39, ParametersG2, AP_Gripper), #endif + // @Param: BAL_PITCH_TRIM + // @DisplayName: Balance Bot pitch trim angle + // @Description: Balance Bot pitch trim for balancing. This offsets the tilt of the center of mass. + // @Units: deg + // @Range: -2 2 + // @Increment: 0.1 + // @User: Standard + AP_GROUPINFO("BAL_PITCH_TRIM", 40, ParametersG2, bal_pitch_trim, 0), + AP_GROUPEND }; diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index 5e39363c87..9e6881cc97 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -396,6 +396,9 @@ public: // mission behave AP_Int8 mis_done_behave; + + // balance both pitch trim + AP_Float bal_pitch_trim; }; extern const AP_Param::Info var_info[]; diff --git a/APMrover2/balance_bot.cpp b/APMrover2/balance_bot.cpp index 7cd9fbdb06..cf35149794 100644 --- a/APMrover2/balance_bot.cpp +++ b/APMrover2/balance_bot.cpp @@ -5,7 +5,7 @@ void Rover::balancebot_pitch_control(float &throttle) { // calculate desired pitch angle - const float demanded_pitch = radians(-throttle * 0.01f * g2.bal_pitch_max); + const float demanded_pitch = radians(-throttle * 0.01f * g2.bal_pitch_max) + radians(g2.bal_pitch_trim); // calculate speed from wheel encoders float veh_speed_pct = 0.0f;