Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 12:38:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: fixed ground accel for helis in FlightAxis

Browse Source 
fixes landing detection
This commit is contained in:
Andrew Tridgell 2016-06-04 15:52:00 +10:00
parent 7c227ac96c
commit 79ffc28f68
2 changed files with 10 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/SITL/SIM_FlightAxis.cpp
View File

@ -295,6 +295,13 @@ void FlightAxis::update(const struct sitl_input &input)
accel_body(state.m_accelerationBodyAX_MPS2, accel_body(state.m_accelerationBodyAX_MPS2,
state.m_accelerationBodyAY_MPS2, state.m_accelerationBodyAY_MPS2,
state.m_accelerationBodyAZ_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 // limit to 16G to match pixhawk
float a_limit = GRAVITY_MSS*16; float a_limit = GRAVITY_MSS*16;
accel_body.x = constrain_float(accel_body.x, -a_limit, a_limit); 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_time_s = state.m_currentPhysicsTime_SEC;
last_velocity_ef = velocity_ef;
} }
} // namespace SITL } // namespace SITL

1
libraries/SITL/SIM_FlightAxis.h
View File

@ -166,6 +166,7 @@ private:
uint64_t activation_frame_counter = 0; uint64_t activation_frame_counter = 0;
double last_frame_count_s = 0; double last_frame_count_s = 0;
Vector3f position_offset; Vector3f position_offset;
Vector3f last_velocity_ef;
const char *controller_ip = "127.0.0.1"; const char *controller_ip = "127.0.0.1";
uint16_t controller_port = 18083; uint16_t controller_port = 18083;