Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: fixed tailsitters in RealFlight

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