zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AHRS: adapt the DCM library to the AHRS framework

master
Andrew Tridgell 13 years ago
parent
9d75ffeba1
commit
d4bb068d5b
2 changed files with 57 additions and 186 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 118
      libraries/AP_AHRS/AP_AHRS_DCM.cpp
  2. 125
      libraries/AP_AHRS/AP_AHRS_DCM.h

118
libraries/AP_AHRS/AP_AHRS_DCM.cpp
Unescape View File

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

125
libraries/AP_AHRS/AP_AHRS_DCM.h
Unescape View File

@ -1,85 +1,44 @@
#ifndef AP_DCM_h #ifndef AP_AHRS_DCM_H
#define AP_DCM_h #define AP_AHRS_DCM_H
/*
// temporarily include all other classes here DCM based AHRS (Attitude Heading Reference System) interface for
// since this naming is a bit off from the ArduPilot
// convention and the AP_DCM should be the top
// header file This library is free software; you can redistribute it and/or
#include "AP_DCM_HIL.h" modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
#include "../FastSerial/FastSerial.h" version 2.1 of the License, or (at your option) any later version.
#include "../AP_Math/AP_Math.h" */
#include <inttypes.h>
#include "../AP_Compass/AP_Compass.h" class AP_AHRS_DCM : public AP_AHRS
#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
{ {
public: public:
// Constructors // Constructors
AP_DCM(IMU *imu, GPS *&gps) : AP_AHRS_DCM(IMU *imu, GPS *&gps) : AP_AHRS(imu, 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)
{ {
// base the ki values by the sensors maximum drift _kp_roll_pitch = 0.13;
// rate. The APM2 has gyros which are much less drift _kp_yaw = 0.4;
// prone than the APM1, so we should have a lower ki, _dcm_matrix(Vector3f(1, 0, 0),
// which will make us less prone to increasing omegaI Vector3f(0, 1, 0),
// incorrectly due to sensor noise Vector3f(0, 0, 1));
_gyro_drift_limit = imu->get_gyro_drift_rate();
// base the ki values on the sensors drift rate
_ki_roll_pitch = _gyro_drift_limit * 5; _ki_roll_pitch = _gyro_drift_limit * 5;
_ki_yaw = _gyro_drift_limit * 8; _ki_yaw = _gyro_drift_limit * 8;
} }
// Accessors
// return the smoothed gyro vector corrected for drift // 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_matrix(void) {return _dcm_matrix; }
Matrix3f get_dcm_transposed(void) {Matrix3f temp = _dcm_matrix; return temp.transpose();}
// return the current drift correction integrator value // return the current drift correction integrator value
Vector3f get_gyro_drift(void) {return _omega_I; } 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 // Methods
void update_DCM(uint8_t drift_correction_frequency=1); void update(void);
void update(void) { update_DCM(); } void reset(bool recover_eulers = false);
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;
// status reporting // status reporting
float get_accel_weight(void);
float get_renorm_val(void);
float get_error_rp(void); float get_error_rp(void);
float get_error_yaw(void); float get_error_yaw(void);
@ -88,13 +47,10 @@ private:
float _ki_roll_pitch; float _ki_roll_pitch;
float _kp_yaw; float _kp_yaw;
float _ki_yaw; float _ki_yaw;
float _gyro_drift_limit; // radians/s/s
bool _have_initial_yaw; bool _have_initial_yaw;
// Methods // Methods
void read_adc_raw(void);
void accel_adjust(Vector3f &accel); void accel_adjust(Vector3f &accel);
float read_adc(int select);
void matrix_update(float _G_Dt); void matrix_update(float _G_Dt);
void normalize(void); void normalize(void);
void check_matrix(void); void check_matrix(void);
@ -102,34 +58,17 @@ private:
void drift_correction(float deltat); void drift_correction(float deltat);
void euler_angles(void); void euler_angles(void);
// members // primary representation of attitude
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;
Matrix3f _dcm_matrix; 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 _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_P; // accel Omega Proportional correction
Vector3f _omega_yaw_P; // yaw Omega Proportional correction Vector3f _omega_yaw_P; // yaw Omega Proportional correction
Vector3f _omega_I; // Omega Integrator correction Vector3f _omega_I; // Omega Integrator correction
Vector3f _omega_integ_corr; // Partially corrected Gyro_Vector data - used for centrepetal correction Vector3f _omega_integ_corr; // Partially corrected Gyro_Vector data - used for centrepetal correction
Vector3f _omega; // Corrected Gyro_Vector data Vector3f _omega; // Corrected Gyro_Vector data
Vector3f _omega_sum;
Vector3f _omega_smoothed;
float _health;
bool _centripetal;
uint8_t _toggle;
// state to support status reporting // state to support status reporting
float _renorm_val_sum; float _renorm_val_sum;
@ -139,16 +78,8 @@ private:
float _error_yaw_sum; float _error_yaw_sum;
uint16_t _error_yaw_count; 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 // time in millis when we last got a GPS heading
uint32_t _gps_last_update; 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

Write Preview
Loading…
Cancel
Save