Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 09:43:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: use FlightAxis accel when not touching the ground

Browse Source
This commit is contained in:
Andrew Tridgell 2016-06-04 11:34:57 +10:00
parent 77b7852ff0
commit 9c13ac997e
1 changed files with 11 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/SITL/SIM_FlightAxis.cpp
View File

@ -256,7 +256,7 @@ void FlightAxis::update(const struct sitl_input &input)
exchange_data(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) { if (dt_seconds <= 0) {
return; return;
} }
@ -290,12 +290,18 @@ void FlightAxis::update(const struct sitl_input &input)
} }
position -= position_offset; position -= position_offset;
// the accel values given in the state are very strange. Calculate Vector3f accel_ef(state.m_accelerationWorldAX_MPS2,
// it from delta-velocity instead, although this does introduce state.m_accelerationWorldAY_MPS2,
// noise state.m_accelerationWorldAZ_MPS2);
Vector3f accel_ef = (velocity_ef - last_velocity_ef) / dt_seconds; // 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_ef.z -= GRAVITY_MSS;
accel_body = dcm.transposed() * accel_ef; 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.x = constrain_float(accel_body.x, -16, 16);
accel_body.y = constrain_float(accel_body.y, -16, 16); accel_body.y = constrain_float(accel_body.y, -16, 16);
accel_body.z = constrain_float(accel_body.z, -16, 16); accel_body.z = constrain_float(accel_body.z, -16, 16);