Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: fixed tailsitters in RealFlight 

we need to use the rotated accel for the earth-frame accel
This commit is contained in:
Andrew Tridgell 2018-07-19 18:09:17 +10:00
parent cbd6f4cc21
commit 9a6113e5d8
1 changed files with 5 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -298,6 +298,7 @@ void AP_AHRS_NavEKF::update_SITL(void)
}
const struct SITL::sitl_fdm &fdm = _sitl->state;
const AP_InertialSensor &_ins = AP::ins();
if (active_EKF_type() == EKF_TYPE_SITL) {
roll = radians(fdm.rollDeg);
@ -311,14 +312,10 @@ void AP_AHRS_NavEKF::update_SITL(void)
_gyro_drift.zero();
_gyro_estimate = Vector3f(radians(fdm.rollRate),
radians(fdm.pitchRate),
radians(fdm.yawRate));
_gyro_estimate = _ins.get_gyro();
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
const Vector3f accel(fdm.xAccel,
fdm.yAccel,
fdm.zAccel);
const Vector3f &accel = _ins.get_accel(i);
_accel_ef_ekf[i] = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * accel;
}
_accel_ef_ekf_blended = _accel_ef_ekf[0];
@ -334,9 +331,8 @@ void AP_AHRS_NavEKF::update_SITL(void)
const float delTime = 0.001f * (timeStamp_ms - _last_body_odm_update_ms);
_last_body_odm_update_ms = timeStamp_ms;
timeStamp_ms -= (timeStamp_ms - _last_body_odm_update_ms)/2; // correct for first order hold average delay
Vector3f delAng(radians(fdm.rollRate),
radians(fdm.pitchRate),
radians(fdm.yawRate));
Vector3f delAng = _ins.get_gyro();
delAng *= delTime;
// rotate earth velocity into body frame and calculate delta position
Matrix3f Tbn;