Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
Peter Barker 2023-07-15 14:37:14 +10:00 committed by Randy Mackay
parent dafebe481e
commit 9d70db6df1
1 changed files with 9 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/SITL/SIM_BalanceBot.cpp
View File

@ -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();