aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions 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);
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);