aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AHRS: adapt the DCM library to the AHRS framework

Browse Source
This commit is contained in:
Andrew Tridgell 2012-03-11 18:59:53 +11:00
parent e976c70e19
commit bf96d05605
2 changed files with 56 additions and 185 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

120
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -1,22 +1,19 @@
/*
APM_DCM.cpp - DCM AHRS Library, fixed wing version, for Ardupilot Mega
Code by Doug Weibel, Jordi Muñoz and Jose Julio. DIYDrones.com
APM_AHRS_DCM.cpp
This library works with the ArduPilot Mega and "Oilpan"
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.
Methods:
update_DCM() : Updates the AHRS by integrating the rotation matrix over time using the IMU object data
get_gyro() : Returns gyro vector corrected for bias
get_accel() : Returns accelerometer vector
get_dcm_matrix() : Returns dcm matrix
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 <AP_DCM.h>
#include <FastSerial.h>
#include <AP_AHRS.h>
// this is the speed in cm/s above which we first get a yaw lock with
// the GPS
@ -26,21 +23,11 @@
// from the GPS and wait for the ground speed to get above GPS_SPEED_MIN
#define GPS_SPEED_RESET 100
void
AP_DCM::set_compass(Compass *compass)
{
_compass = compass;
}
// run a full DCM update round
// the drift_correction_frequency is how many steps of the algorithm
// to run before doing an accelerometer and yaw drift correction step
void
AP_DCM::update_DCM(uint8_t drift_correction_frequency)
AP_AHRS_DCM::update(void)
{
float delta_t;
Vector3f accel;
// tell the IMU to grab some data
_imu->update();
@ -50,43 +37,16 @@ AP_DCM::update_DCM(uint8_t drift_correction_frequency)
// Get current values for gyros
_gyro_vector = _imu->get_gyro();
// accumulate some more accelerometer data
accel = _imu->get_accel();
_accel_sum += accel;
_drift_correction_time += delta_t;
_accel_vector = _imu->get_accel();
// Integrate the DCM matrix using gyro inputs
matrix_update(delta_t);
// add up the omega vector so we pass a value to the drift
// correction averaged over the same time period as the
// accelerometers
_omega_sum += _omega_integ_corr;
// Normalize the DCM matrix
normalize();
// see if we will perform drift correction on this call
_drift_correction_count++;
if (_drift_correction_count >= drift_correction_frequency) {
// calculate the average accelerometer vector over
// since the last drift correction call
float scale = 1.0 / _drift_correction_count;
_accel_vector = _accel_sum * scale;
_accel_sum.zero();
// calculate the average omega value over this time
_omega_smoothed = _omega_sum * scale;
_omega_sum.zero();
// Perform drift correction
drift_correction(_drift_correction_time);
_drift_correction_time = 0;
_drift_correction_count = 0;
}
// Perform drift correction
drift_correction(delta_t);
// paranoid check for bad values in the DCM matrix
check_matrix();
@ -97,7 +57,7 @@ AP_DCM::update_DCM(uint8_t drift_correction_frequency)
// update the DCM matrix using only the gyros
void
AP_DCM::matrix_update(float _G_Dt)
AP_AHRS_DCM::matrix_update(float _G_Dt)
{
// _omega_integ_corr is used for _centripetal correction
// (theoretically better than _omega)
@ -132,7 +92,7 @@ AP_DCM::matrix_update(float _G_Dt)
// adjust an accelerometer vector for known acceleration forces
void
AP_DCM::accel_adjust(Vector3f &accel)
AP_AHRS_DCM::accel_adjust(Vector3f &accel)
{
float veloc;
// compensate for linear acceleration. This makes a
@ -149,8 +109,8 @@ AP_DCM::accel_adjust(Vector3f &accel)
// direction as positive
// Equation 26 broken up into separate pieces
accel.y -= _omega_smoothed.z * veloc;
accel.z += _omega_smoothed.y * veloc;
accel.y -= _omega_integ_corr.z * veloc;
accel.z += _omega_integ_corr.y * veloc;
}
/*
@ -158,7 +118,7 @@ AP_DCM::accel_adjust(Vector3f &accel)
extreme errors in the matrix
*/
void
AP_DCM::matrix_reset(bool recover_eulers)
AP_AHRS_DCM::reset(bool recover_eulers)
{
if (_compass != NULL) {
_compass->null_offsets_disable();
@ -169,7 +129,6 @@ AP_DCM::matrix_reset(bool recover_eulers)
_omega_P.zero();
_omega_yaw_P.zero();
_omega_integ_corr.zero();
_omega_smoothed.zero();
_omega.zero();
// if the caller wants us to try to recover to the current
@ -193,13 +152,13 @@ AP_DCM::matrix_reset(bool recover_eulers)
check the DCM matrix for pathological values
*/
void
AP_DCM::check_matrix(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++;
matrix_reset(true);
reset(true);
return;
}
// some DCM matrix values can lead to an out of range error in
@ -221,7 +180,7 @@ AP_DCM::check_matrix(void)
SITL_debug("ERROR: DCM matrix error. _dcm_matrix.c.x=%f\n",
_dcm_matrix.c.x);
renorm_blowup_count++;
matrix_reset(true);
reset(true);
}
}
}
@ -229,7 +188,7 @@ AP_DCM::check_matrix(void)
// renormalise one vector component of the DCM matrix
// this will return false if renormalization fails
bool
AP_DCM::renorm(Vector3f const &a, Vector3f &result)
AP_AHRS_DCM::renorm(Vector3f const &a, Vector3f &result)
{
float renorm_val;
@ -289,7 +248,7 @@ simple matter to stay ahead of it.
We call the process of enforcing the orthogonality conditions ÒrenormalizationÓ.
*/
void
AP_DCM::normalize(void)
AP_AHRS_DCM::normalize(void)
{
float error;
Vector3f t0, t1, t2;
@ -305,7 +264,7 @@ AP_DCM::normalize(void)
!renorm(t2, _dcm_matrix.c)) {
// Our solution is blowing up and we will force back
// to last euler angles
matrix_reset(true);
reset(true);
}
}
@ -319,7 +278,7 @@ AP_DCM::normalize(void)
// This function also updates _omega_yaw_P with a yaw correction term
// from our yaw reference vector
void
AP_DCM::drift_correction(float deltat)
AP_AHRS_DCM::drift_correction(float deltat)
{
float error_course = 0;
Vector3f accel;
@ -530,7 +489,7 @@ AP_DCM::drift_correction(float deltat)
// calculate the euler angles which will be used for high level
// navigation control
void
AP_DCM::euler_angles(void)
AP_AHRS_DCM::euler_angles(void)
{
_dcm_matrix.to_euler(&roll, &pitch, &yaw);
@ -544,27 +503,8 @@ AP_DCM::euler_angles(void)
/* reporting of DCM state for MAVLink */
// average accel_weight since last call
float AP_DCM::get_accel_weight(void)
{
return 1.0;
}
// average renorm_val since last call
float AP_DCM::get_renorm_val(void)
{
float ret;
if (_renorm_val_count == 0) {
return 0;
}
ret = _renorm_val_sum / _renorm_val_count;
_renorm_val_sum = 0;
_renorm_val_count = 0;
return ret;
}
// average error_roll_pitch since last call
float AP_DCM::get_error_rp(void)
float AP_AHRS_DCM::get_error_rp(void)
{
float ret;
if (_error_rp_count == 0) {
@ -577,7 +517,7 @@ float AP_DCM::get_error_rp(void)
}
// average error_yaw since last call
float AP_DCM::get_error_yaw(void)
float AP_AHRS_DCM::get_error_yaw(void)
{
float ret;
if (_error_yaw_count == 0) {

121
libraries/AP_AHRS/AP_AHRS_DCM.h
View File

@ -1,85 +1,44 @@
#ifndef AP_DCM_h
#define AP_DCM_h
#ifndef AP_AHRS_DCM_H
#define AP_AHRS_DCM_H
/*
DCM based AHRS (Attitude Heading Reference System) interface for
ArduPilot
// temporarily include all other classes here
// since this naming is a bit off from the
// convention and the AP_DCM should be the top
// header file
#include "AP_DCM_HIL.h"
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/FastSerial.h"
#include "../AP_Math/AP_Math.h"
#include <inttypes.h>
#include "../AP_Compass/AP_Compass.h"
#include "../AP_ADC/AP_ADC.h"
#include "../AP_GPS/AP_GPS.h"
#include "../AP_IMU/AP_IMU.h"
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
class AP_DCM
class AP_AHRS_DCM : public AP_AHRS
{
public:
// Constructors
AP_DCM(IMU *imu, GPS *&gps) :
_kp_roll_pitch(0.13),
_kp_yaw(0.4),
_gps(gps),
_imu(imu),
_dcm_matrix(1, 0, 0,
0, 1, 0,
0, 0, 1),
_health(1.),
_toggle(0)
AP_AHRS_DCM(IMU *imu, GPS *&gps) : AP_AHRS(imu, gps)
{
// base the ki values by the sensors maximum drift
// rate. The APM2 has gyros which are much less drift
// prone than the APM1, so we should have a lower ki,
// which will make us less prone to increasing omegaI
// incorrectly due to sensor noise
_gyro_drift_limit = imu->get_gyro_drift_rate();
_kp_roll_pitch = 0.13;
_kp_yaw = 0.4;
_dcm_matrix(Vector3f(1, 0, 0),
Vector3f(0, 1, 0),
Vector3f(0, 0, 1));
// base the ki values on the sensors drift rate
_ki_roll_pitch = _gyro_drift_limit * 5;
_ki_yaw = _gyro_drift_limit * 8;
}
// Accessors
// return the smoothed gyro vector corrected for drift
Vector3f get_gyro(void) {return _omega_smoothed; }
Vector3f get_gyro(void) {return _omega; }
Matrix3f get_dcm_matrix(void) {return _dcm_matrix; }
Matrix3f get_dcm_transposed(void) {Matrix3f temp = _dcm_matrix; return temp.transpose();}
// return the current drift correction integrator value
Vector3f get_gyro_drift(void) {return _omega_I; }
float get_health(void) {return _health;}
void set_centripetal(bool b) {_centripetal = b;}
bool get_centripetal(void) {return _centripetal;}
void set_compass(Compass *compass);
// Methods
void update_DCM(uint8_t drift_correction_frequency=1);
void update(void) { update_DCM(); }
void matrix_reset(bool recover_eulers = false);
long roll_sensor; // Degrees * 100
long pitch_sensor; // Degrees * 100
long yaw_sensor; // Degrees * 100
float roll; // Radians
float pitch; // Radians
float yaw; // Radians
uint8_t gyro_sat_count;
uint8_t renorm_range_count;
uint8_t renorm_blowup_count;
void update(void);
void reset(bool recover_eulers = false);
// status reporting
float get_accel_weight(void);
float get_renorm_val(void);
float get_error_rp(void);
float get_error_yaw(void);
@ -88,13 +47,10 @@ private:
float _ki_roll_pitch;
float _kp_yaw;
float _ki_yaw;
float _gyro_drift_limit; // radians/s/s
bool _have_initial_yaw;
// Methods
void read_adc_raw(void);
void accel_adjust(Vector3f &accel);
float read_adc(int select);
void matrix_update(float _G_Dt);
void normalize(void);
void check_matrix(void);
@ -102,34 +58,17 @@ private:
void drift_correction(float deltat);
void euler_angles(void);
// members
Compass * _compass;
// note: we use ref-to-pointer here so that our caller can change the GPS without our noticing
// IMU under us without our noticing.
GPS *&_gps; // note: this is a reference to a pointer owned by the caller
IMU *_imu;
// primary representation of attitude
Matrix3f _dcm_matrix;
// sum of accel vectors between drift_correction() calls
// this allows the drift correction to run at a different rate
// to the main DCM update code
Vector3f _accel_vector;
Vector3f _accel_sum;
Vector3f _gyro_vector; // Store the gyros turn rate in a vector
Vector3f _accel_vector; // current accel vector
Vector3f _omega_P; // accel Omega Proportional correction
Vector3f _omega_yaw_P; // yaw Omega Proportional correction
Vector3f _omega_I; // Omega Integrator correction
Vector3f _omega_integ_corr; // Partially corrected Gyro_Vector data - used for centrepetal correction
Vector3f _omega; // Corrected Gyro_Vector data
Vector3f _omega_sum;
Vector3f _omega_smoothed;
float _health;
bool _centripetal;
uint8_t _toggle;
// state to support status reporting
float _renorm_val_sum;
@ -139,16 +78,8 @@ private:
float _error_yaw_sum;
uint16_t _error_yaw_count;
// time in micros when we last got a compass fix
uint32_t _compass_last_update;
// time in millis when we last got a GPS heading
uint32_t _gps_last_update;
// counter of calls to update_DCM() without drift correction
uint8_t _drift_correction_count;
float _drift_correction_time;
};
#endif
#endif // AP_AHRS_DCM_H