mirror of https://github.com/ArduPilot/ardupilot
DCM: update for new Matrix3f interface
This commit is contained in:
parent
4d3789d11c
commit
e8f1c5742b
|
@ -176,10 +176,10 @@ AP_DCM::matrix_reset(bool recover_eulers)
|
||||||
// attitude then calculate the dcm matrix from the current
|
// attitude then calculate the dcm matrix from the current
|
||||||
// roll/pitch/yaw values
|
// roll/pitch/yaw values
|
||||||
if (recover_eulers && !isnan(roll) && !isnan(pitch) && !isnan(yaw)) {
|
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 {
|
} else {
|
||||||
// otherwise make it flat
|
// otherwise make it flat
|
||||||
rotation_matrix_from_euler(_dcm_matrix, 0, 0, 0);
|
_dcm_matrix.from_euler(0, 0, 0);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (_compass != NULL) {
|
if (_compass != NULL) {
|
||||||
|
@ -426,7 +426,7 @@ AP_DCM::drift_correction(float deltat)
|
||||||
|
|
||||||
// now construct a new DCM matrix
|
// now construct a new DCM matrix
|
||||||
_compass->null_offsets_disable();
|
_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();
|
_compass->null_offsets_enable();
|
||||||
_have_initial_yaw = true;
|
_have_initial_yaw = true;
|
||||||
_compass_last_update = _compass->last_update;
|
_compass_last_update = _compass->last_update;
|
||||||
|
@ -452,7 +452,7 @@ AP_DCM::drift_correction(float deltat)
|
||||||
if (_compass) {
|
if (_compass) {
|
||||||
_compass->null_offsets_disable();
|
_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) {
|
if (_compass) {
|
||||||
_compass->null_offsets_enable();
|
_compass->null_offsets_enable();
|
||||||
}
|
}
|
||||||
|
@ -532,7 +532,7 @@ AP_DCM::drift_correction(float deltat)
|
||||||
void
|
void
|
||||||
AP_DCM::euler_angles(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;
|
roll_sensor = degrees(roll) * 100;
|
||||||
pitch_sensor = degrees(pitch) * 100;
|
pitch_sensor = degrees(pitch) * 100;
|
||||||
|
|
Loading…
Reference in New Issue