|
|
|
@ -1,17 +1,17 @@
@@ -1,17 +1,17 @@
|
|
|
|
|
/*
|
|
|
|
|
APM_AHRS_DCM.cpp |
|
|
|
|
|
|
|
|
|
AHRS system using DCM matrices |
|
|
|
|
|
|
|
|
|
Based on DCM code by Doug Weibel, Jordi Muñoz and Jose Julio. DIYDrones.com |
|
|
|
|
|
|
|
|
|
Adapted for the general ArduPilot AHRS interface by Andrew Tridgell |
|
|
|
|
|
|
|
|
|
This library is free software; you can redistribute it and/or |
|
|
|
|
modify it under the terms of the GNU Lesser General Public License |
|
|
|
|
as published by the Free Software Foundation; either version 2.1 |
|
|
|
|
of the License, or (at your option) any later version. |
|
|
|
|
*/ |
|
|
|
|
* APM_AHRS_DCM.cpp |
|
|
|
|
* |
|
|
|
|
* AHRS system using DCM matrices |
|
|
|
|
* |
|
|
|
|
* Based on DCM code by Doug Weibel, Jordi Muñoz and Jose Julio. DIYDrones.com |
|
|
|
|
* |
|
|
|
|
* Adapted for the general ArduPilot AHRS interface by Andrew Tridgell |
|
|
|
|
* |
|
|
|
|
* This library is free software; you can redistribute it and/or |
|
|
|
|
* modify it under the terms of the GNU Lesser General Public License |
|
|
|
|
* as published by the Free Software Foundation; either version 2.1 |
|
|
|
|
* of the License, or (at your option) any later version. |
|
|
|
|
*/ |
|
|
|
|
#include <FastSerial.h> |
|
|
|
|
#include <AP_AHRS.h> |
|
|
|
|
|
|
|
|
@ -78,8 +78,8 @@ AP_AHRS_DCM::matrix_update(float _G_Dt)
@@ -78,8 +78,8 @@ AP_AHRS_DCM::matrix_update(float _G_Dt)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
|
reset the DCM matrix and omega. Used on ground start, and on |
|
|
|
|
extreme errors in the matrix |
|
|
|
|
* reset the DCM matrix and omega. Used on ground start, and on |
|
|
|
|
* extreme errors in the matrix |
|
|
|
|
*/ |
|
|
|
|
void |
|
|
|
|
AP_AHRS_DCM::reset(bool recover_eulers) |
|
|
|
@ -102,7 +102,7 @@ AP_AHRS_DCM::reset(bool recover_eulers)
@@ -102,7 +102,7 @@ AP_AHRS_DCM::reset(bool recover_eulers)
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
|
check the DCM matrix for pathological values |
|
|
|
|
* check the DCM matrix for pathological values |
|
|
|
|
*/ |
|
|
|
|
void |
|
|
|
|
AP_AHRS_DCM::check_matrix(void) |
|
|
|
@ -191,15 +191,15 @@ AP_AHRS_DCM::renorm(Vector3f const &a, Vector3f &result)
@@ -191,15 +191,15 @@ AP_AHRS_DCM::renorm(Vector3f const &a, Vector3f &result)
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/*************************************************
|
|
|
|
|
Direction Cosine Matrix IMU: Theory |
|
|
|
|
William Premerlani and Paul Bizard |
|
|
|
|
|
|
|
|
|
Numerical errors will gradually reduce the orthogonality conditions expressed by equation 5 |
|
|
|
|
to approximations rather than identities. In effect, the axes in the two frames of reference no |
|
|
|
|
longer describe a rigid body. Fortunately, numerical error accumulates very slowly, so it is a |
|
|
|
|
simple matter to stay ahead of it. |
|
|
|
|
We call the process of enforcing the orthogonality conditions ÒrenormalizationÓ. |
|
|
|
|
*/ |
|
|
|
|
* Direction Cosine Matrix IMU: Theory |
|
|
|
|
* William Premerlani and Paul Bizard |
|
|
|
|
* |
|
|
|
|
* Numerical errors will gradually reduce the orthogonality conditions expressed by equation 5 |
|
|
|
|
* to approximations rather than identities. In effect, the axes in the two frames of reference no |
|
|
|
|
* longer describe a rigid body. Fortunately, numerical error accumulates very slowly, so it is a |
|
|
|
|
* simple matter to stay ahead of it. |
|
|
|
|
* We call the process of enforcing the orthogonality conditions ÒrenormalizationÓ. |
|
|
|
|
*/ |
|
|
|
|
void |
|
|
|
|
AP_AHRS_DCM::normalize(void) |
|
|
|
|
{ |
|
|
|
@ -449,13 +449,13 @@ AP_AHRS_DCM::drift_correction(float deltat)
@@ -449,13 +449,13 @@ AP_AHRS_DCM::drift_correction(float deltat)
|
|
|
|
|
#define USE_BAROMETER_FOR_VERTICAL_VELOCITY 1 |
|
|
|
|
#if USE_BAROMETER_FOR_VERTICAL_VELOCITY |
|
|
|
|
/*
|
|
|
|
|
The barometer for vertical velocity is only enabled if we got |
|
|
|
|
at least 5 pressure samples for the reading. This ensures we |
|
|
|
|
don't use very noisy climb rate data |
|
|
|
|
* The barometer for vertical velocity is only enabled if we got |
|
|
|
|
* at least 5 pressure samples for the reading. This ensures we |
|
|
|
|
* don't use very noisy climb rate data |
|
|
|
|
*/ |
|
|
|
|
if (_barometer != NULL && _barometer->get_pressure_samples() >= 5) { |
|
|
|
|
// Z velocity is down
|
|
|
|
|
velocity.z = - _barometer->get_climb_rate(); |
|
|
|
|
velocity.z = -_barometer->get_climb_rate(); |
|
|
|
|
} |
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|