From 79ffc28f6829d63cfa23ae3cc80772e7ee21e9e9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 4 Jun 2016 15:52:00 +1000 Subject: [PATCH] SITL: fixed ground accel for helis in FlightAxis fixes landing detection --- libraries/SITL/SIM_FlightAxis.cpp | 9 +++++++++ libraries/SITL/SIM_FlightAxis.h | 1 + 2 files changed, 10 insertions(+) diff --git a/libraries/SITL/SIM_FlightAxis.cpp b/libraries/SITL/SIM_FlightAxis.cpp index a72b3deb95..bea968571c 100644 --- a/libraries/SITL/SIM_FlightAxis.cpp +++ b/libraries/SITL/SIM_FlightAxis.cpp @@ -295,6 +295,13 @@ void FlightAxis::update(const struct sitl_input &input) accel_body(state.m_accelerationBodyAX_MPS2, state.m_accelerationBodyAY_MPS2, state.m_accelerationBodyAZ_MPS2); + // accel on the ground is nasty in realflight, and prevents helicopter disarm + if (state.m_isTouchingGround) { + Vector3f accel_ef = (velocity_ef - last_velocity_ef) / dt_seconds; + accel_ef.z -= GRAVITY_MSS; + accel_body = dcm.transposed() * accel_ef; + } + // limit to 16G to match pixhawk float a_limit = GRAVITY_MSS*16; accel_body.x = constrain_float(accel_body.x, -a_limit, a_limit); @@ -337,6 +344,8 @@ void FlightAxis::update(const struct sitl_input &input) } last_time_s = state.m_currentPhysicsTime_SEC; + + last_velocity_ef = velocity_ef; } } // namespace SITL diff --git a/libraries/SITL/SIM_FlightAxis.h b/libraries/SITL/SIM_FlightAxis.h index 81df6bf655..a8f796d4af 100644 --- a/libraries/SITL/SIM_FlightAxis.h +++ b/libraries/SITL/SIM_FlightAxis.h @@ -166,6 +166,7 @@ private: uint64_t activation_frame_counter = 0; double last_frame_count_s = 0; Vector3f position_offset; + Vector3f last_velocity_ef; const char *controller_ip = "127.0.0.1"; uint16_t controller_port = 18083;