Browse Source

DCM: update for new Matrix3f interface

master
Andrew Tridgell 13 years ago
parent
commit
e8f1c5742b
  1. 10
      libraries/AP_DCM/AP_DCM.cpp

10
libraries/AP_DCM/AP_DCM.cpp

@ -176,10 +176,10 @@ AP_DCM::matrix_reset(bool recover_eulers) @@ -176,10 +176,10 @@ AP_DCM::matrix_reset(bool recover_eulers)
// attitude then calculate the dcm matrix from the current
// roll/pitch/yaw values
if (recover_eulers && !isnan(roll) && !isnan(pitch) && !isnan(yaw)) {
rotation_matrix_from_euler(_dcm_matrix, roll, pitch, yaw);
_dcm_matrix.from_euler(roll, pitch, yaw);
} else {
// otherwise make it flat
rotation_matrix_from_euler(_dcm_matrix, 0, 0, 0);
_dcm_matrix.from_euler(0, 0, 0);
}
if (_compass != NULL) {
@ -426,7 +426,7 @@ AP_DCM::drift_correction(float deltat) @@ -426,7 +426,7 @@ AP_DCM::drift_correction(float deltat)
// now construct a new DCM matrix
_compass->null_offsets_disable();
rotation_matrix_from_euler(_dcm_matrix, roll, pitch, _compass->heading);
_dcm_matrix.from_euler(roll, pitch, _compass->heading);
_compass->null_offsets_enable();
_have_initial_yaw = true;
_compass_last_update = _compass->last_update;
@ -452,7 +452,7 @@ AP_DCM::drift_correction(float deltat) @@ -452,7 +452,7 @@ AP_DCM::drift_correction(float deltat)
if (_compass) {
_compass->null_offsets_disable();
}
rotation_matrix_from_euler(_dcm_matrix, roll, pitch, ToRad(_gps->ground_course));
_dcm_matrix.from_euler(roll, pitch, ToRad(_gps->ground_course));
if (_compass) {
_compass->null_offsets_enable();
}
@ -532,7 +532,7 @@ AP_DCM::drift_correction(float deltat) @@ -532,7 +532,7 @@ AP_DCM::drift_correction(float deltat)
void
AP_DCM::euler_angles(void)
{
calculate_euler_angles(_dcm_matrix, &roll, &pitch, &yaw);
_dcm_matrix.to_euler(&roll, &pitch, &yaw);
roll_sensor = degrees(roll) * 100;
pitch_sensor = degrees(pitch) * 100;

Loading…
Cancel
Save