From 4ad643b2333d45a1ce4fabc10973872dca46af8c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 15 Oct 2014 13:18:08 +1100 Subject: [PATCH] AP_AHRS: use a common function for updating the CD values this ensures the wrapping of yaw is consistent between the 3 use cases --- libraries/AP_AHRS/AP_AHRS.cpp | 12 ++++++++++++ libraries/AP_AHRS/AP_AHRS.h | 3 +++ libraries/AP_AHRS/AP_AHRS_DCM.cpp | 7 +------ libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 11 +++-------- 4 files changed, 19 insertions(+), 14 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 033d5db14c..bcffeca808 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -247,3 +247,15 @@ void AP_AHRS::update_trig(void) _sin_pitch = -temp.c.x; _sin_roll = temp.c.y / _cos_pitch; } + +/* + update the centi-degree values + */ +void AP_AHRS::update_cd_values(void) +{ + roll_sensor = degrees(roll) * 100; + pitch_sensor = degrees(pitch) * 100; + yaw_sensor = degrees(yaw) * 100; + if (yaw_sensor < 0) + yaw_sensor += 36000; +} diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 3a950cdbb2..3e6841452f 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -370,6 +370,9 @@ protected: // should be called after _dcm_matrix is updated void update_trig(void); + // update roll_sensor, pitch_sensor and yaw_sensor + void update_cd_values(void); + // pointer to compass object, if available Compass * _compass; diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index e925aa85a7..fd015cdd07 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -837,12 +837,7 @@ AP_AHRS_DCM::euler_angles(void) _body_dcm_matrix.rotateXYinv(_trim); _body_dcm_matrix.to_euler(&roll, &pitch, &yaw); - roll_sensor = degrees(roll) * 100; - pitch_sensor = degrees(pitch) * 100; - yaw_sensor = degrees(yaw) * 100; - - if (yaw_sensor < 0) - yaw_sensor += 36000; + update_cd_values(); } /* reporting of DCM state for MAVLink */ diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index 6f872f59c2..224e7a9ba7 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -58,9 +58,7 @@ void AP_AHRS_NavEKF::update(void) roll = _dcm_attitude.x; pitch = _dcm_attitude.y; yaw = _dcm_attitude.z; - roll_sensor = degrees(roll)*100; - pitch_sensor = degrees(pitch)*100; - yaw_sensor = degrees(yaw)*100; + update_cd_values(); AP_AHRS_DCM::update(); @@ -88,11 +86,8 @@ void AP_AHRS_NavEKF::update(void) roll = eulers.x; pitch = eulers.y; yaw = eulers.z; - roll_sensor = degrees(roll) * 100; - pitch_sensor = degrees(pitch) * 100; - yaw_sensor = degrees(yaw) * 100; - if (yaw_sensor < 0) - yaw_sensor += 36000; + + update_cd_values(); update_trig(); // keep _gyro_bias for get_gyro_drift()