aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_AHRS: apply AHRS trim in SITL

Browse Source
This commit is contained in:
Mark Whitehorn 2019-02-03 11:22:57 -07:00 committed by Andrew Tridgell
parent 0bf2fc004e
commit 8c84fa5c64
1 changed files with 2 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -309,11 +309,9 @@ void AP_AHRS_NavEKF::update_SITL(void)
const AP_InertialSensor &_ins = AP::ins(); const AP_InertialSensor &_ins = AP::ins();
if (active_EKF_type() == EKF_TYPE_SITL) { if (active_EKF_type() == EKF_TYPE_SITL) {
roll = radians(fdm.rollDeg);
pitch = radians(fdm.pitchDeg);
yaw = radians(fdm.yawDeg);
fdm.quaternion.rotation_matrix(_dcm_matrix); fdm.quaternion.rotation_matrix(_dcm_matrix);
_dcm_matrix = _dcm_matrix * get_rotation_vehicle_body_to_autopilot_body();
_dcm_matrix.to_euler(&roll, &pitch, &yaw);
update_cd_values(); update_cd_values();
update_trig(); update_trig();