SITL: use FlightAxis accel when not touching the ground

libraries/SITL/SIM_FlightAxis.cpp

@@ -256,7 +256,7 @@ void FlightAxis::update(const struct sitl_input &input)
     
     exchange_data(input);
 
-    float dt_seconds = state.m_currentPhysicsTime_SEC - last_time_s;
+    double dt_seconds = state.m_currentPhysicsTime_SEC - last_time_s;
     if (dt_seconds <= 0) {
         return;
     }
@@ -290,12 +290,18 @@ void FlightAxis::update(const struct sitl_input &input)
     }
     position -= position_offset;
 
-    // the accel values given in the state are very strange. Calculate
-    // it from delta-velocity instead, although this does introduce
-    // noise
-    Vector3f accel_ef = (velocity_ef - last_velocity_ef) / dt_seconds;
+    Vector3f accel_ef(state.m_accelerationWorldAX_MPS2,
+                      state.m_accelerationWorldAY_MPS2,
+                      state.m_accelerationWorldAZ_MPS2);
+    // the accel values given in the state are very strange when on
+    // the ground. Calculate it from delta-velocity instead, although
+    // this does introduce noise
+    if (state.m_isTouchingGround) {
+        accel_ef = (velocity_ef - last_velocity_ef) / dt_seconds;
+    }
     accel_ef.z -= GRAVITY_MSS;
     accel_body = dcm.transposed() * accel_ef;
+    // limit accel range to what a pixhawk could see
     accel_body.x = constrain_float(accel_body.x, -16, 16);
     accel_body.y = constrain_float(accel_body.y, -16, 16);
     accel_body.z = constrain_float(accel_body.z, -16, 16);