acceleration added when hitting the ground

src/modules/sih/sih.cpp

@@ -348,6 +348,8 @@ 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,9 +361,14 @@ void Sih::equations_of_motion()
     // 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
+            // for the accelerometer, compute the acceleration that will stop the vehicle in one time step
+            _v_I_dot=-_v_I/_dt;
+        else
+            _v_I_dot.setZero();
         _v_I.setZero();
         _w_B.setZero();
-        _v_I_dot.setZero();
+        grounded=true;
     }
     else
     {
@@ -371,7 +378,7 @@ void Sih::equations_of_motion()
         _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;
     }
 }