From 9d70db6df15b71fcf65c25dc49c47edf49f74d41 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 15 Jul 2023 14:37:14 +1000 Subject: [PATCH] SITL: reset balancebot to vertical in a kinmetically consistent manner The hard-reset of all state variables means we feed the EKF inconsistent gyro data. Attempt to upright the vehicle with a simple p-gain when it is disarmed, as if someone is pushing the thing back upright Backport of PR https://github.com/ArduPilot/ardupilot/pull/24314 --- libraries/SITL/SIM_BalanceBot.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/libraries/SITL/SIM_BalanceBot.cpp b/libraries/SITL/SIM_BalanceBot.cpp index f3b11d0c90..56b02f6205 100644 --- a/libraries/SITL/SIM_BalanceBot.cpp +++ b/libraries/SITL/SIM_BalanceBot.cpp @@ -95,7 +95,13 @@ void BalanceBot::update(const struct sitl_input &input) float theta = p; //radians float ang_vel = gyro.y; //radians/s - + if (!hal.util->get_soft_armed()) { + // simulated fingers uprighting the vehicle + const float p_gain = 200; + const float pitch_response = -sin(p) * p_gain * delta_time; + ang_vel += pitch_response; + } + // t1,t2,t3 are terms in the equation to find vehicle frame x acceleration const float t1 = ((2.0f*gear_ratio*k_t*v/(R*r_w)) - (2.0f*gear_ratio*k_t*k_e*velocity_vf_x/(R*r_w*r_w)) - (m_p*l*ang_vel*ang_vel*sin(theta))) * (i_p + m_p*l*l); const float t2 = -m_p*l*cos(theta)*((2.0f*gear_ratio*k_t*k_e*velocity_vf_x/(R*r_w)) - (2.0f*gear_ratio*k_t*v/(R)) + (m_p*GRAVITY_MSS*l*sin(theta))); @@ -134,7 +140,8 @@ void BalanceBot::update(const struct sitl_input &input) // we are on the ground, so our vertical accel is zero accel_earth.z = 0; - if (!hal.util->get_soft_armed()) { + if (!hal.util->get_soft_armed() && + p < radians(2)) { // reset to vertical when not armed for faster testing accel_earth.zero(); velocity_ef.zero();