From fcd00f65e948958aceb6e05b50f45c6cf210d2d2 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 20 Sep 2023 18:34:33 +1000 Subject: [PATCH] SITL: fix balancebot yaw reset to be kinimatically consistent ... or at least closer to it. We were hard-resetting the yaw to zero when the vehicle was upright. That makes for huge simulated gyro rates, and that means the differences between the gyros can be huge sample-to-sample, so we can get gyros-inconsistent errors. Fix things so we don't reset yaw at the same time as pitch, and also twist the vehicle to point North again when disarmed. Backport of PR https://github.com/ArduPilot/ardupilot/pull/25046 --- libraries/SITL/SIM_BalanceBot.cpp | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/libraries/SITL/SIM_BalanceBot.cpp b/libraries/SITL/SIM_BalanceBot.cpp index 56b02f6205..23b52e742f 100644 --- a/libraries/SITL/SIM_BalanceBot.cpp +++ b/libraries/SITL/SIM_BalanceBot.cpp @@ -87,7 +87,7 @@ void BalanceBot::update(const struct sitl_input &input) const float delta_time = frame_time_us * 1.0e-6f; // yaw rate in degrees/s - const float yaw_rate = calc_yaw_rate(steering); + float yaw_rate = calc_yaw_rate(steering); // obtain roll, pitch, yaw from dcm float r, p, y; @@ -100,6 +100,11 @@ void BalanceBot::update(const struct sitl_input &input) const float p_gain = 200; const float pitch_response = -sin(p) * p_gain * delta_time; ang_vel += pitch_response; + + // simulated fingers rotating the vehicle + const float y_gain = 100000; + const float yaw_response = -sin(wrap_180(y)) * y_gain * delta_time; + yaw_rate += yaw_response; } // t1,t2,t3 are terms in the equation to find vehicle frame x acceleration @@ -145,11 +150,15 @@ void BalanceBot::update(const struct sitl_input &input) // reset to vertical when not armed for faster testing accel_earth.zero(); velocity_ef.zero(); - dcm.identity(); - gyro.zero(); velocity_vf_x =0; + gyro[1] = 0; // no pitch rate + if (y < radians(2)) { + // no rates at all: + dcm.identity(); + gyro.zero(); + } } - + // work out acceleration as seen by the accelerometers. It sees the kinematic // acceleration (ie. real movement), plus gravity accel_body += dcm.transposed() * (Vector3f(0, 0, -GRAVITY_MSS));