Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-19 14:23:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: use a common function for updating the CD values

Browse Source 
this ensures the wrapping of yaw is consistent between the 3 use cases
This commit is contained in:
Andrew Tridgell 2014-10-15 13:18:08 +11:00
parent eec5cd5add
commit 4ad643b233
4 changed files with 19 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -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;
}

3
libraries/AP_AHRS/AP_AHRS.h
View File

@ -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;

7
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -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 */

11
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -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()