diff --git a/src/modules/sih/sih.cpp b/src/modules/sih/sih.cpp index 76e0bde5d6..16f6029e6f 100644 --- a/src/modules/sih/sih.cpp +++ b/src/modules/sih/sih.cpp @@ -348,8 +348,6 @@ void Sih::generate_force_and_torques() // apply the equations of motion of a rigid body and integrate one step void Sih::equations_of_motion() { - static bool grounded=true; // the simulation starts with the vehicle on the ground - _C_IB=_q.to_dcm(); // body to inertial transformation // Equations of motion of a rigid body @@ -359,26 +357,24 @@ void Sih::equations_of_motion() _w_B_dot=_Im1*(_Mt_B+_Ma_B-_w_B.cross(_I*_w_B)); // conservation of angular momentum // fake ground, avoid free fall - if(_p_I(2)>0.0f && (_v_I_dot(2)>0.0f || _v_I(2)>0.0f)) - { - if (grounded==false) // if we just hit the floor + if(_p_I(2)>0.0f && (_v_I_dot(2)>0.0f || _v_I(2)>0.0f)) { + if (!_grounded) { // if we just hit the floor // for the accelerometer, compute the acceleration that will stop the vehicle in one time step _v_I_dot=-_v_I/_dt; - else + } else { _v_I_dot.setZero(); + } _v_I.setZero(); _w_B.setZero(); - grounded=true; - } - else - { + _grounded = true; + } else { // integration: Euler forward _p_I = _p_I + _p_I_dot*_dt; _v_I = _v_I + _v_I_dot*_dt; _q = _q+_q_dot*_dt; // as given in attitude_estimator_q_main.cpp _q.normalize(); _w_B = _w_B + _w_B_dot*_dt; - grounded=false; + _grounded = false; } } diff --git a/src/modules/sih/sih.hpp b/src/modules/sih/sih.hpp index 6132e88f9e..0dde73782c 100644 --- a/src/modules/sih/sih.hpp +++ b/src/modules/sih/sih.hpp @@ -146,13 +146,13 @@ private: px4_sem_t _data_semaphore; - int32_t _counter = 0; hrt_call _timer_call; hrt_abstime _last_run; hrt_abstime _gps_time; hrt_abstime _serial_time; hrt_abstime _now; float _dt; // sampling time [s] + bool _grounded{true};// whether the vehicle is on the ground matrix::Vector3f _T_B; // thrust force in body frame [N] matrix::Vector3f _Fa_I; // aerodynamic force in inertial frame [N]