mirror of https://github.com/ArduPilot/ardupilot
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
This commit is contained in:
parent
b5d22ce47c
commit
84b3c9e267
|
@ -93,7 +93,13 @@ void BalanceBot::update(const struct sitl_input &input)
|
||||||
float theta = p; //radians
|
float theta = p; //radians
|
||||||
|
|
||||||
float ang_vel = gyro.y; //radians/s
|
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
|
// 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 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)));
|
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)));
|
||||||
|
@ -132,7 +138,8 @@ void BalanceBot::update(const struct sitl_input &input)
|
||||||
// we are on the ground, so our vertical accel is zero
|
// we are on the ground, so our vertical accel is zero
|
||||||
accel_earth.z = 0;
|
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
|
// reset to vertical when not armed for faster testing
|
||||||
accel_earth.zero();
|
accel_earth.zero();
|
||||||
velocity_ef.zero();
|
velocity_ef.zero();
|
||||||
|
|
Loading…
Reference in New Issue