diff options
author | Darryl Taylor <darryl.c.taylor@gmail.com> | 2014-06-18 13:43:03 +0800 |
---|---|---|
committer | Darryl Taylor <darryl.c.taylor@gmail.com> | 2014-06-18 13:43:03 +0800 |
commit | 0f41d8b5077856cf8d00f30acf9f4bc52f6e41a6 (patch) | |
tree | 3c776d92227cf31c81f5aafca21ed808436a90fd /src/modules/attitude_estimator_ekf | |
parent | 251760679a4bed86fdb10746062fde2cc23e460f (diff) | |
download | px4-firmware-0f41d8b5077856cf8d00f30acf9f4bc52f6e41a6.tar.gz px4-firmware-0f41d8b5077856cf8d00f30acf9f4bc52f6e41a6.tar.bz2 px4-firmware-0f41d8b5077856cf8d00f30acf9f4bc52f6e41a6.zip |
Added pitch, roll, and yaw offsets to compensate for imperfect fmu placement. These were removed in 61a3177979838649af2a6e8e50bea7d15e2765f4
Diffstat (limited to 'src/modules/attitude_estimator_ekf')
-rwxr-xr-x | src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 9 |
1 files changed, 5 insertions, 4 deletions
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 66a949af1..95685b407 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -512,10 +512,11 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* send out */ att.timestamp = raw.timestamp; - - att.roll = euler[0]; - att.pitch = euler[1]; - att.yaw = euler[2] + ekf_params.mag_decl; + + /* Compensate for fmu placement offsets and magnetic declination */ + att.roll = euler[0] - ekf_params.roll_off; + att.pitch = euler[1] - ekf_params.pitch_off; + att.yaw = euler[2] + ekf_params.mag_decl - ekf_params.yaw_off; att.rollspeed = x_aposteriori[0]; att.pitchspeed = x_aposteriori[1]; |