Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: fix balancebot yaw reset to be kinimatically consistent

Browse Source 
... 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
This commit is contained in:
Peter Barker 2023-09-20 18:34:33 +10:00 committed by Andrew Tridgell
parent 2b211dcfaf
commit fcd00f65e9
1 changed files with 13 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/SITL/SIM_BalanceBot.cpp
View File

@ -87,7 +87,7 @@ void BalanceBot::update(const struct sitl_input &input)
const float delta_time = frame_time_us * 1.0e-6f; const float delta_time = frame_time_us * 1.0e-6f;
// yaw rate in degrees/s // 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 // obtain roll, pitch, yaw from dcm
float r, p, y; float r, p, y;
@ -100,6 +100,11 @@ void BalanceBot::update(const struct sitl_input &input)
const float p_gain = 200; const float p_gain = 200;
const float pitch_response = -sin(p) * p_gain * delta_time; const float pitch_response = -sin(p) * p_gain * delta_time;
ang_vel += pitch_response; 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 // 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 // reset to vertical when not armed for faster testing
accel_earth.zero(); accel_earth.zero();
velocity_ef.zero(); velocity_ef.zero();
dcm.identity();
gyro.zero();
velocity_vf_x =0; 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 // work out acceleration as seen by the accelerometers. It sees the kinematic
// acceleration (ie. real movement), plus gravity // acceleration (ie. real movement), plus gravity
accel_body += dcm.transposed() * (Vector3f(0, 0, -GRAVITY_MSS)); accel_body += dcm.transposed() * (Vector3f(0, 0, -GRAVITY_MSS));