Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

uncrustify libraries/AP_AHRS/AP_AHRS_DCM.cpp

Browse Source
This commit is contained in:
uncrustify 2012-08-21 19:19:51 -07:00 committed by Pat Hickey
parent 56b7f1b031
commit 7dc3f26ce0
1 changed files with 386 additions and 386 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

772
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -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>
@ -34,108 +34,108 @@
void
AP_AHRS_DCM::update(void)
{
float delta_t;
float delta_t;
// tell the IMU to grab some data
_imu->update();
// tell the IMU to grab some data
_imu->update();
// ask the IMU how much time this sensor reading represents
delta_t = _imu->get_delta_time();
// ask the IMU how much time this sensor reading represents
delta_t = _imu->get_delta_time();
// Get current values for gyros
_gyro_vector = _imu->get_gyro();
_accel_vector = _imu->get_accel();
// Get current values for gyros
_gyro_vector = _imu->get_gyro();
_accel_vector = _imu->get_accel();
// Integrate the DCM matrix using gyro inputs
matrix_update(delta_t);
// Integrate the DCM matrix using gyro inputs
matrix_update(delta_t);
// Normalize the DCM matrix
normalize();
// Normalize the DCM matrix
normalize();
// Perform drift correction
drift_correction(delta_t);
// Perform drift correction
drift_correction(delta_t);
// paranoid check for bad values in the DCM matrix
check_matrix();
// paranoid check for bad values in the DCM matrix
check_matrix();
// Calculate pitch, roll, yaw for stabilization and navigation
euler_angles();
// Calculate pitch, roll, yaw for stabilization and navigation
euler_angles();
}
// update the DCM matrix using only the gyros
void
AP_AHRS_DCM::matrix_update(float _G_Dt)
{
// note that we do not include the P terms in _omega. This is
// because the spin_rate is calculated from _omega.length(),
// and including the P terms would give positive feedback into
// the _P_gain() calculation, which can lead to a very large P
// value
_omega = _gyro_vector + _omega_I;
// note that we do not include the P terms in _omega. This is
// because the spin_rate is calculated from _omega.length(),
// and including the P terms would give positive feedback into
// the _P_gain() calculation, which can lead to a very large P
// value
_omega = _gyro_vector + _omega_I;
_dcm_matrix.rotate((_omega + _omega_P + _omega_yaw_P) * _G_Dt);
_dcm_matrix.rotate((_omega + _omega_P + _omega_yaw_P) * _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)
{
// reset the integration terms
_omega_I.zero();
_omega_P.zero();
_omega_yaw_P.zero();
_omega.zero();
// reset the integration terms
_omega_I.zero();
_omega_P.zero();
_omega_yaw_P.zero();
_omega.zero();
// if the caller wants us to try to recover to the current
// attitude then calculate the dcm matrix from the current
// roll/pitch/yaw values
if (recover_eulers && !isnan(roll) && !isnan(pitch) && !isnan(yaw)) {
_dcm_matrix.from_euler(roll, pitch, yaw);
} else {
// otherwise make it flat
_dcm_matrix.from_euler(0, 0, 0);
}
// if the caller wants us to try to recover to the current
// attitude then calculate the dcm matrix from the current
// roll/pitch/yaw values
if (recover_eulers && !isnan(roll) && !isnan(pitch) && !isnan(yaw)) {
_dcm_matrix.from_euler(roll, pitch, yaw);
} else {
// otherwise make it flat
_dcm_matrix.from_euler(0, 0, 0);
}
}
/*
check the DCM matrix for pathological values
* check the DCM matrix for pathological values
*/
void
AP_AHRS_DCM::check_matrix(void)
{
if (_dcm_matrix.is_nan()) {
//Serial.printf("ERROR: DCM matrix NAN\n");
SITL_debug("ERROR: DCM matrix NAN\n");
renorm_blowup_count++;
reset(true);
return;
}
// some DCM matrix values can lead to an out of range error in
// the pitch calculation via asin(). These NaN values can
// feed back into the rest of the DCM matrix via the
// error_course value.
if (!(_dcm_matrix.c.x < 1.0 &&
_dcm_matrix.c.x > -1.0)) {
// We have an invalid matrix. Force a normalisation.
renorm_range_count++;
normalize();
if (_dcm_matrix.is_nan()) {
//Serial.printf("ERROR: DCM matrix NAN\n");
SITL_debug("ERROR: DCM matrix NAN\n");
renorm_blowup_count++;
reset(true);
return;
}
// some DCM matrix values can lead to an out of range error in
// the pitch calculation via asin(). These NaN values can
// feed back into the rest of the DCM matrix via the
// error_course value.
if (!(_dcm_matrix.c.x < 1.0 &&
_dcm_matrix.c.x > -1.0)) {
// We have an invalid matrix. Force a normalisation.
renorm_range_count++;
normalize();
if (_dcm_matrix.is_nan() ||
fabs(_dcm_matrix.c.x) > 10) {
// normalisation didn't fix the problem! We're
// in real trouble. All we can do is reset
//Serial.printf("ERROR: DCM matrix error. _dcm_matrix.c.x=%f\n",
// _dcm_matrix.c.x);
SITL_debug("ERROR: DCM matrix error. _dcm_matrix.c.x=%f\n",
_dcm_matrix.c.x);
renorm_blowup_count++;
reset(true);
}
}
if (_dcm_matrix.is_nan() ||
fabs(_dcm_matrix.c.x) > 10) {
// normalisation didn't fix the problem! We're
// in real trouble. All we can do is reset
//Serial.printf("ERROR: DCM matrix error. _dcm_matrix.c.x=%f\n",
// _dcm_matrix.c.x);
SITL_debug("ERROR: DCM matrix error. _dcm_matrix.c.x=%f\n",
_dcm_matrix.c.x);
renorm_blowup_count++;
reset(true);
}
}
}
// renormalise one vector component of the DCM matrix
@ -143,82 +143,82 @@ AP_AHRS_DCM::check_matrix(void)
bool
AP_AHRS_DCM::renorm(Vector3f const &a, Vector3f &result)
{
float renorm_val;
float renorm_val;
// numerical errors will slowly build up over time in DCM,
// causing inaccuracies. We can keep ahead of those errors
// using the renormalization technique from the DCM IMU paper
// (see equations 18 to 21).
// numerical errors will slowly build up over time in DCM,
// causing inaccuracies. We can keep ahead of those errors
// using the renormalization technique from the DCM IMU paper
// (see equations 18 to 21).
// For APM we don't bother with the taylor expansion
// optimisation from the paper as on our 2560 CPU the cost of
// the sqrt() is 44 microseconds, and the small time saving of
// the taylor expansion is not worth the potential of
// additional error buildup.
// For APM we don't bother with the taylor expansion
// optimisation from the paper as on our 2560 CPU the cost of
// the sqrt() is 44 microseconds, and the small time saving of
// the taylor expansion is not worth the potential of
// additional error buildup.
// Note that we can get significant renormalisation values
// when we have a larger delta_t due to a glitch eleswhere in
// APM, such as a I2c timeout or a set of EEPROM writes. While
// we would like to avoid these if possible, if it does happen
// we don't want to compound the error by making DCM less
// accurate.
// Note that we can get significant renormalisation values
// when we have a larger delta_t due to a glitch eleswhere in
// APM, such as a I2c timeout or a set of EEPROM writes. While
// we would like to avoid these if possible, if it does happen
// we don't want to compound the error by making DCM less
// accurate.
renorm_val = 1.0 / a.length();
renorm_val = 1.0 / a.length();
// keep the average for reporting
_renorm_val_sum += renorm_val;
_renorm_val_count++;
// keep the average for reporting
_renorm_val_sum += renorm_val;
_renorm_val_count++;
if (!(renorm_val < 2.0 && renorm_val > 0.5)) {
// this is larger than it should get - log it as a warning
renorm_range_count++;
if (!(renorm_val < 1.0e6 && renorm_val > 1.0e-6)) {
// we are getting values which are way out of
// range, we will reset the matrix and hope we
// can recover our attitude using drift
// correction before we hit the ground!
//Serial.printf("ERROR: DCM renormalisation error. renorm_val=%f\n",
// renorm_val);
SITL_debug("ERROR: DCM renormalisation error. renorm_val=%f\n",
renorm_val);
renorm_blowup_count++;
return false;
}
}
if (!(renorm_val < 2.0 && renorm_val > 0.5)) {
// this is larger than it should get - log it as a warning
renorm_range_count++;
if (!(renorm_val < 1.0e6 && renorm_val > 1.0e-6)) {
// we are getting values which are way out of
// range, we will reset the matrix and hope we
// can recover our attitude using drift
// correction before we hit the ground!
//Serial.printf("ERROR: DCM renormalisation error. renorm_val=%f\n",
// renorm_val);
SITL_debug("ERROR: DCM renormalisation error. renorm_val=%f\n",
renorm_val);
renorm_blowup_count++;
return false;
}
}
result = a * renorm_val;
return true;
result = a * renorm_val;
return true;
}
/*************************************************
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)
{
float error;
Vector3f t0, t1, t2;
float error;
Vector3f t0, t1, t2;
error = _dcm_matrix.a * _dcm_matrix.b; // eq.18
error = _dcm_matrix.a * _dcm_matrix.b; // eq.18
t0 = _dcm_matrix.a - (_dcm_matrix.b * (0.5f * error)); // eq.19
t1 = _dcm_matrix.b - (_dcm_matrix.a * (0.5f * error)); // eq.19
t2 = t0 % t1; // c= a x b // eq.20
t0 = _dcm_matrix.a - (_dcm_matrix.b * (0.5f * error)); // eq.19
t1 = _dcm_matrix.b - (_dcm_matrix.a * (0.5f * error)); // eq.19
t2 = t0 % t1; // c= a x b // eq.20
if (!renorm(t0, _dcm_matrix.a) ||
!renorm(t1, _dcm_matrix.b) ||
!renorm(t2, _dcm_matrix.c)) {
// Our solution is blowing up and we will force back
// to last euler angles
reset(true);
}
if (!renorm(t0, _dcm_matrix.a) ||
!renorm(t1, _dcm_matrix.b) ||
!renorm(t2, _dcm_matrix.c)) {
// Our solution is blowing up and we will force back
// to last euler angles
reset(true);
}
}
@ -227,24 +227,24 @@ AP_AHRS_DCM::normalize(void)
float
AP_AHRS_DCM::yaw_error_compass(void)
{
Vector3f mag = Vector3f(_compass->mag_x, _compass->mag_y, _compass->mag_z);
// get the mag vector in the earth frame
Vector3f rb = _dcm_matrix * mag;
Vector3f mag = Vector3f(_compass->mag_x, _compass->mag_y, _compass->mag_z);
// get the mag vector in the earth frame
Vector3f rb = _dcm_matrix * mag;
rb.normalize();
if (rb.is_inf()) {
// not a valid vector
return 0.0;
}
rb.normalize();
if (rb.is_inf()) {
// not a valid vector
return 0.0;
}
// get the earths magnetic field (only X and Y components needed)
Vector3f mag_earth = Vector3f(cos(_compass->get_declination()),
sin(_compass->get_declination()), 0);
// get the earths magnetic field (only X and Y components needed)
Vector3f mag_earth = Vector3f(cos(_compass->get_declination()),
sin(_compass->get_declination()), 0);
// calculate the error term in earth frame
Vector3f error = rb % mag_earth;
// calculate the error term in earth frame
Vector3f error = rb % mag_earth;
return error.z;
return error.z;
}
// produce a yaw error value using the GPS. The returned value is proportional
@ -252,32 +252,32 @@ AP_AHRS_DCM::yaw_error_compass(void)
float
AP_AHRS_DCM::yaw_error_gps(void)
{
return sin(ToRad(_gps->ground_course * 0.01) - yaw);
return sin(ToRad(_gps->ground_course * 0.01) - yaw);
}
// the _P_gain raises the gain of the PI controller
// when we are spinning fast. See the fastRotations
// paper from Bill.
// when we are spinning fast. See the fastRotations
// paper from Bill.
float
AP_AHRS_DCM::_P_gain(float spin_rate)
{
if (spin_rate < ToDeg(50)) {
return 1.0;
}
if (spin_rate > ToDeg(500)) {
return 10.0;
}
return spin_rate/ToDeg(50);
if (spin_rate < ToDeg(50)) {
return 1.0;
}
if (spin_rate > ToDeg(500)) {
return 10.0;
}
return spin_rate/ToDeg(50);
}
// return true if we have and should use GPS
bool AP_AHRS_DCM::have_gps(void)
{
if (!_gps || _gps->status() != GPS::GPS_OK || !_gps_use) {
return false;
}
return true;
if (!_gps || _gps->status() != GPS::GPS_OK || !_gps_use) {
return false;
}
return true;
}
// yaw drift correction using the compass or GPS
@ -286,78 +286,78 @@ bool AP_AHRS_DCM::have_gps(void)
void
AP_AHRS_DCM::drift_correction_yaw(void)
{
bool new_value = false;
float yaw_error;
float yaw_deltat;
bool new_value = false;
float yaw_error;
float yaw_deltat;
if (_compass && _compass->use_for_yaw()) {
if (_compass->last_update != _compass_last_update) {
yaw_deltat = (_compass->last_update - _compass_last_update) * 1.0e-6;
_compass_last_update = _compass->last_update;
// we force an additional compass read()
// here. This has the effect of throwing away
// the first compass value, which can be bad
if (!_have_initial_yaw && _compass->read()) {
float heading = _compass->calculate_heading(_dcm_matrix);
_dcm_matrix.from_euler(roll, pitch, heading);
_omega_yaw_P.zero();
_have_initial_yaw = true;
}
new_value = true;
yaw_error = yaw_error_compass();
}
} else if (_fly_forward && have_gps()) {
if (_gps->last_fix_time != _gps_last_update &&
_gps->ground_speed >= GPS_SPEED_MIN) {
yaw_deltat = (_gps->last_fix_time - _gps_last_update) * 1.0e-3;
_gps_last_update = _gps->last_fix_time;
if (!_have_initial_yaw) {
_dcm_matrix.from_euler(roll, pitch, ToRad(_gps->ground_course*0.01));
_omega_yaw_P.zero();
_have_initial_yaw = true;
}
new_value = true;
yaw_error = yaw_error_gps();
}
}
if (_compass && _compass->use_for_yaw()) {
if (_compass->last_update != _compass_last_update) {
yaw_deltat = (_compass->last_update - _compass_last_update) * 1.0e-6;
_compass_last_update = _compass->last_update;
// we force an additional compass read()
// here. This has the effect of throwing away
// the first compass value, which can be bad
if (!_have_initial_yaw && _compass->read()) {
float heading = _compass->calculate_heading(_dcm_matrix);
_dcm_matrix.from_euler(roll, pitch, heading);
_omega_yaw_P.zero();
_have_initial_yaw = true;
}
new_value = true;
yaw_error = yaw_error_compass();
}
} else if (_fly_forward && have_gps()) {
if (_gps->last_fix_time != _gps_last_update &&
_gps->ground_speed >= GPS_SPEED_MIN) {
yaw_deltat = (_gps->last_fix_time - _gps_last_update) * 1.0e-3;
_gps_last_update = _gps->last_fix_time;
if (!_have_initial_yaw) {
_dcm_matrix.from_euler(roll, pitch, ToRad(_gps->ground_course*0.01));
_omega_yaw_P.zero();
_have_initial_yaw = true;
}
new_value = true;
yaw_error = yaw_error_gps();
}
}
if (!new_value) {
// we don't have any new yaw information
// slowly decay _omega_yaw_P to cope with loss
// of our yaw source
_omega_yaw_P *= 0.97;
return;
}
if (!new_value) {
// we don't have any new yaw information
// slowly decay _omega_yaw_P to cope with loss
// of our yaw source
_omega_yaw_P *= 0.97;
return;
}
// the yaw error is a vector in earth frame
Vector3f error = Vector3f(0,0, yaw_error);
// the yaw error is a vector in earth frame
Vector3f error = Vector3f(0,0, yaw_error);
// convert the error vector to body frame
error = _dcm_matrix.mul_transpose(error);
// convert the error vector to body frame
error = _dcm_matrix.mul_transpose(error);
// the spin rate changes the P gain, and disables the
// integration at higher rates
float spin_rate = _omega.length();
// the spin rate changes the P gain, and disables the
// integration at higher rates
float spin_rate = _omega.length();
// update the proportional control to drag the
// yaw back to the right value. We use a gain
// that depends on the spin rate. See the fastRotations.pdf
// paper from Bill Premerlani
// update the proportional control to drag the
// yaw back to the right value. We use a gain
// that depends on the spin rate. See the fastRotations.pdf
// paper from Bill Premerlani
_omega_yaw_P.z = error.z * _P_gain(spin_rate) * _kp_yaw;
if (_fast_ground_gains) {
_omega_yaw_P.z *= 8;
}
_omega_yaw_P.z = error.z * _P_gain(spin_rate) * _kp_yaw;
if (_fast_ground_gains) {
_omega_yaw_P.z *= 8;
}
// don't update the drift term if we lost the yaw reference
// for more than 2 seconds
if (yaw_deltat < 2.0 && spin_rate < ToRad(SPIN_RATE_LIMIT)) {
// also add to the I term
_omega_I_sum.z += error.z * _ki_yaw * yaw_deltat;
}
// don't update the drift term if we lost the yaw reference
// for more than 2 seconds
if (yaw_deltat < 2.0 && spin_rate < ToRad(SPIN_RATE_LIMIT)) {
// also add to the I term
_omega_I_sum.z += error.z * _ki_yaw * yaw_deltat;
}
_error_yaw_sum += fabs(yaw_error);
_error_yaw_count++;
_error_yaw_sum += fabs(yaw_error);
_error_yaw_count++;
}
@ -397,25 +397,25 @@ AP_AHRS_DCM::drift_correction(float deltat)
// not enough time has accumulated
return;
}
float airspeed;
if (_airspeed && _airspeed->use()) {
airspeed = _airspeed->get_airspeed();
} else {
airspeed = _last_airspeed;
}
// use airspeed to estimate our ground velocity in
// earth frame by subtracting the wind
velocity = _dcm_matrix.colx() * airspeed;
float airspeed;
if (_airspeed && _airspeed->use()) {
airspeed = _airspeed->get_airspeed();
} else {
airspeed = _last_airspeed;
}
// use airspeed to estimate our ground velocity in
// earth frame by subtracting the wind
velocity = _dcm_matrix.colx() * airspeed;
// add in wind estimate
velocity += _wind;
// add in wind estimate
velocity += _wind;
last_correction_time = millis();
_have_gps_lock = false;
last_correction_time = millis();
_have_gps_lock = false;
// update position delta for get_position()
_position_offset_north += velocity.x * _ra_deltat;
_position_offset_east += velocity.y * _ra_deltat;
// update position delta for get_position()
_position_offset_north += velocity.x * _ra_deltat;
_position_offset_east += velocity.y * _ra_deltat;
} else {
if (_gps->last_fix_time == _ra_sum_start) {
// we don't have a new GPS fix - nothing more to do
@ -423,39 +423,39 @@ AP_AHRS_DCM::drift_correction(float deltat)
}
velocity = Vector3f(_gps->velocity_north(), _gps->velocity_east(), 0);
last_correction_time = _gps->last_fix_time;
if (_have_gps_lock == false) {
// if we didn't have GPS lock in the last drift
// correction interval then set the velocities equal
_last_velocity = velocity;
}
_have_gps_lock = true;
if (_have_gps_lock == false) {
// if we didn't have GPS lock in the last drift
// correction interval then set the velocities equal
_last_velocity = velocity;
}
_have_gps_lock = true;
// remember position for get_position()
_last_lat = _gps->latitude;
_last_lng = _gps->longitude;
_position_offset_north = 0;
_position_offset_east = 0;
// remember position for get_position()
_last_lat = _gps->latitude;
_last_lng = _gps->longitude;
_position_offset_north = 0;
_position_offset_east = 0;
// once we have a single GPS lock, we update using
// dead-reckoning from then on
_have_position = true;
// once we have a single GPS lock, we update using
// dead-reckoning from then on
_have_position = true;
// keep last airspeed estimate for dead-reckoning purposes
Vector3f airspeed = velocity - _wind;
airspeed.z = 0;
_last_airspeed = airspeed.length();
// keep last airspeed estimate for dead-reckoning purposes
Vector3f airspeed = velocity - _wind;
airspeed.z = 0;
_last_airspeed = airspeed.length();
}
#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();
// Z velocity is down
velocity.z = -_barometer->get_climb_rate();
}
#endif
@ -475,24 +475,24 @@ AP_AHRS_DCM::drift_correction(float deltat)
GA_e = Vector3f(0, 0, -1.0) + ((velocity - _last_velocity) * v_scale);
GA_e.normalize();
if (GA_e.is_inf()) {
// wait for some non-zero acceleration information
return;
// wait for some non-zero acceleration information
return;
}
// calculate the error term in earth frame.
Vector3f GA_b = _ra_sum / (_ra_deltat * _gravity);
float length = GA_b.length();
if (length > 1.0) {
GA_b /= length;
if (GA_b.is_inf()) {
// wait for some non-zero acceleration information
return;
}
GA_b /= length;
if (GA_b.is_inf()) {
// wait for some non-zero acceleration information
return;
}
}
Vector3f error = GA_b % GA_e;
#define YAW_INDEPENDENT_DRIFT_CORRECTION 0
#if YAW_INDEPENDENT_DRIFT_CORRECTION
#if YAW_INDEPENDENT_DRIFT_CORRECTION
// step 2 calculate earth_error_Z
float earth_error_Z = error.z;
@ -501,7 +501,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
// equation 11
float theta = atan2(GA_b.y, GA_b.x);
// equation 12
Vector3f GA_e2 = Vector3f(cos(theta)*tilt, sin(theta)*tilt, GA_e.z);
@ -515,20 +515,20 @@ AP_AHRS_DCM::drift_correction(float deltat)
// flat, but still allow for yaw correction using the
// accelerometers at high roll angles as long as we have a GPS
if (_compass && _compass->use_for_yaw()) {
if (have_gps() && gps_gain == 1.0) {
error.z *= sin(fabs(roll));
} else {
error.z = 0;
}
if (have_gps() && gps_gain == 1.0) {
error.z *= sin(fabs(roll));
} else {
error.z = 0;
}
}
// convert the error term to body frame
error = _dcm_matrix.mul_transpose(error);
if (error.is_nan() || error.is_inf()) {
// don't allow bad values
check_matrix();
return;
// don't allow bad values
check_matrix();
return;
}
_error_rp_sum += error.length();
@ -542,27 +542,27 @@ AP_AHRS_DCM::drift_correction(float deltat)
// accelerometer reading.
_omega_P = error * _P_gain(spin_rate) * _kp;
if (_fast_ground_gains) {
_omega_P *= 8;
_omega_P *= 8;
}
// accumulate some integrator error
if (spin_rate < ToRad(SPIN_RATE_LIMIT)) {
_omega_I_sum += error * _ki * _ra_deltat;
_omega_I_sum_time += _ra_deltat;
_omega_I_sum += error * _ki * _ra_deltat;
_omega_I_sum_time += _ra_deltat;
}
if (_omega_I_sum_time >= 5) {
// limit the rate of change of omega_I to the hardware
// reported maximum gyro drift rate. This ensures that
// short term errors don't cause a buildup of omega_I
// beyond the physical limits of the device
float change_limit = _gyro_drift_limit * _omega_I_sum_time;
_omega_I_sum.x = constrain(_omega_I_sum.x, -change_limit, change_limit);
_omega_I_sum.y = constrain(_omega_I_sum.y, -change_limit, change_limit);
_omega_I_sum.z = constrain(_omega_I_sum.z, -change_limit, change_limit);
_omega_I += _omega_I_sum;
_omega_I_sum.zero();
_omega_I_sum_time = 0;
// limit the rate of change of omega_I to the hardware
// reported maximum gyro drift rate. This ensures that
// short term errors don't cause a buildup of omega_I
// beyond the physical limits of the device
float change_limit = _gyro_drift_limit * _omega_I_sum_time;
_omega_I_sum.x = constrain(_omega_I_sum.x, -change_limit, change_limit);
_omega_I_sum.y = constrain(_omega_I_sum.y, -change_limit, change_limit);
_omega_I_sum.z = constrain(_omega_I_sum.z, -change_limit, change_limit);
_omega_I += _omega_I_sum;
_omega_I_sum.zero();
_omega_I_sum_time = 0;
}
// zero our accumulator ready for the next GPS step
@ -574,8 +574,8 @@ AP_AHRS_DCM::drift_correction(float deltat)
_last_velocity = velocity;
if (_have_gps_lock && _fly_forward) {
// update wind estimate
estimate_wind(velocity);
// update wind estimate
estimate_wind(velocity);
}
}
@ -583,56 +583,56 @@ AP_AHRS_DCM::drift_correction(float deltat)
// update our wind speed estimate
void AP_AHRS_DCM::estimate_wind(Vector3f &velocity)
{
// this is based on the wind speed estimation code from MatrixPilot by
// Bill Premerlani. Adaption for ArduPilot by Jon Challinger
// See http://gentlenav.googlecode.com/files/WindEstimation.pdf
Vector3f fuselageDirection = _dcm_matrix.colx();
Vector3f fuselageDirectionDiff = fuselageDirection - _last_fuse;
uint32_t now = millis();
// this is based on the wind speed estimation code from MatrixPilot by
// Bill Premerlani. Adaption for ArduPilot by Jon Challinger
// See http://gentlenav.googlecode.com/files/WindEstimation.pdf
Vector3f fuselageDirection = _dcm_matrix.colx();
Vector3f fuselageDirectionDiff = fuselageDirection - _last_fuse;
uint32_t now = millis();
// scrap our data and start over if we're taking too long to get a direction change
if (now - _last_wind_time > 10000) {
_last_wind_time = now;
_last_fuse = fuselageDirection;
_last_vel = velocity;
return;
}
// scrap our data and start over if we're taking too long to get a direction change
if (now - _last_wind_time > 10000) {
_last_wind_time = now;
_last_fuse = fuselageDirection;
_last_vel = velocity;
return;
}
float diff_length = fuselageDirectionDiff.length();
if (diff_length > 0.2) {
// when turning, use the attitude response to estimate
// wind speed
float V;
Vector3f velocityDiff = velocity - _last_vel;
float diff_length = fuselageDirectionDiff.length();
if (diff_length > 0.2) {
// when turning, use the attitude response to estimate
// wind speed
float V;
Vector3f velocityDiff = velocity - _last_vel;
// estimate airspeed it using equation 6
V = velocityDiff.length() / diff_length;
_last_fuse = fuselageDirection;
_last_vel = velocity;
Vector3f fuselageDirectionSum = fuselageDirection + _last_fuse;
Vector3f velocitySum = velocity + _last_vel;
float theta = atan2(velocityDiff.y, velocityDiff.x) - atan2(fuselageDirectionDiff.y, fuselageDirectionDiff.x);
float sintheta = sin(theta);
float costheta = cos(theta);
Vector3f wind = Vector3f();
wind.x = velocitySum.x - V * (costheta * fuselageDirectionSum.x - sintheta * fuselageDirectionSum.y);
wind.y = velocitySum.y - V * (sintheta * fuselageDirectionSum.x + costheta * fuselageDirectionSum.y);
wind.z = velocitySum.z - V * fuselageDirectionSum.z;
wind *= 0.5;
// estimate airspeed it using equation 6
V = velocityDiff.length() / diff_length;
_wind = _wind * 0.95 + wind * 0.05;
_last_fuse = fuselageDirection;
_last_vel = velocity;
_last_wind_time = now;
} else if (now - _last_wind_time > 2000 && _airspeed && _airspeed->use()) {
// when flying straight use airspeed to get wind estimate if available
Vector3f airspeed = _dcm_matrix.colx() * _airspeed->get_airspeed();
Vector3f wind = velocity - airspeed;
_wind = _wind * 0.92 + wind * 0.08;
}
Vector3f fuselageDirectionSum = fuselageDirection + _last_fuse;
Vector3f velocitySum = velocity + _last_vel;
float theta = atan2(velocityDiff.y, velocityDiff.x) - atan2(fuselageDirectionDiff.y, fuselageDirectionDiff.x);
float sintheta = sin(theta);
float costheta = cos(theta);
Vector3f wind = Vector3f();
wind.x = velocitySum.x - V * (costheta * fuselageDirectionSum.x - sintheta * fuselageDirectionSum.y);
wind.y = velocitySum.y - V * (sintheta * fuselageDirectionSum.x + costheta * fuselageDirectionSum.y);
wind.z = velocitySum.z - V * fuselageDirectionSum.z;
wind *= 0.5;
_wind = _wind * 0.95 + wind * 0.05;
_last_wind_time = now;
} else if (now - _last_wind_time > 2000 && _airspeed && _airspeed->use()) {
// when flying straight use airspeed to get wind estimate if available
Vector3f airspeed = _dcm_matrix.colx() * _airspeed->get_airspeed();
Vector3f wind = velocity - airspeed;
_wind = _wind * 0.92 + wind * 0.08;
}
}
@ -643,14 +643,14 @@ void AP_AHRS_DCM::estimate_wind(Vector3f &velocity)
void
AP_AHRS_DCM::euler_angles(void)
{
_dcm_matrix.to_euler(&roll, &pitch, &yaw);
_dcm_matrix.to_euler(&roll, &pitch, &yaw);
roll_sensor = degrees(roll) * 100;
pitch_sensor = degrees(pitch) * 100;
yaw_sensor = degrees(yaw) * 100;
roll_sensor = degrees(roll) * 100;
pitch_sensor = degrees(pitch) * 100;
yaw_sensor = degrees(yaw) * 100;
if (yaw_sensor < 0)
yaw_sensor += 36000;
if (yaw_sensor < 0)
yaw_sensor += 36000;
}
/* reporting of DCM state for MAVLink */
@ -658,41 +658,41 @@ AP_AHRS_DCM::euler_angles(void)
// average error_roll_pitch since last call
float AP_AHRS_DCM::get_error_rp(void)
{
if (_error_rp_count == 0) {
// this happens when telemetry is setup on two
// serial ports
return _error_rp_last;
}
_error_rp_last = _error_rp_sum / _error_rp_count;
_error_rp_sum = 0;
_error_rp_count = 0;
return _error_rp_last;
if (_error_rp_count == 0) {
// this happens when telemetry is setup on two
// serial ports
return _error_rp_last;
}
_error_rp_last = _error_rp_sum / _error_rp_count;
_error_rp_sum = 0;
_error_rp_count = 0;
return _error_rp_last;
}
// average error_yaw since last call
float AP_AHRS_DCM::get_error_yaw(void)
{
if (_error_yaw_count == 0) {
// this happens when telemetry is setup on two
// serial ports
return _error_yaw_last;
}
_error_yaw_last = _error_yaw_sum / _error_yaw_count;
_error_yaw_sum = 0;
_error_yaw_count = 0;
return _error_yaw_last;
if (_error_yaw_count == 0) {
// this happens when telemetry is setup on two
// serial ports
return _error_yaw_last;
}
_error_yaw_last = _error_yaw_sum / _error_yaw_count;
_error_yaw_sum = 0;
_error_yaw_count = 0;
return _error_yaw_last;
}
// return our current position estimate using
// return our current position estimate using
// dead-reckoning or GPS
bool AP_AHRS_DCM::get_position(struct Location *loc)
bool AP_AHRS_DCM::get_position(struct Location *loc)
{
if (!_have_position) {
return false;
}
loc->lat = _last_lat;
loc->lng = _last_lng;
location_offset(loc, _position_offset_north, _position_offset_east);
return true;
if (!_have_position) {
return false;
}
loc->lat = _last_lat;
loc->lng = _last_lng;
location_offset(loc, _position_offset_north, _position_offset_east);
return true;
}