2010-12-01 03:58:04 -04:00
|
|
|
#ifndef AP_DCM_h
|
|
|
|
#define AP_DCM_h
|
|
|
|
|
2011-09-11 19:02:47 -03:00
|
|
|
// temporarily include all other classes here
|
2010-12-10 06:17:27 -04:00
|
|
|
// 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"
|
|
|
|
|
2011-04-30 23:05:17 -03:00
|
|
|
#include "../FastSerial/FastSerial.h"
|
|
|
|
#include "../AP_Math/AP_Math.h"
|
2010-12-01 03:58:04 -04:00
|
|
|
#include <inttypes.h>
|
2011-04-30 23:05:17 -03:00
|
|
|
#include "../AP_Compass/AP_Compass.h"
|
|
|
|
#include "../AP_ADC/AP_ADC.h"
|
|
|
|
#include "../AP_GPS/AP_GPS.h"
|
|
|
|
#include "../AP_IMU/AP_IMU.h"
|
2012-01-27 23:25:47 -04:00
|
|
|
#if defined(ARDUINO) && ARDUINO >= 100
|
|
|
|
#include "Arduino.h"
|
|
|
|
#else
|
|
|
|
#include "WProgram.h"
|
|
|
|
#endif
|
2010-12-01 03:58:04 -04:00
|
|
|
|
|
|
|
class AP_DCM
|
|
|
|
{
|
|
|
|
public:
|
|
|
|
// Constructors
|
2012-03-07 00:09:17 -04:00
|
|
|
AP_DCM(IMU *imu, GPS *&gps) :
|
2012-03-08 03:12:46 -04:00
|
|
|
_kp_roll_pitch(0.13),
|
2012-03-12 03:49:15 -03:00
|
|
|
_kp_yaw(0.4),
|
2011-05-02 02:55:59 -03:00
|
|
|
_gps(gps),
|
|
|
|
_imu(imu),
|
2010-12-01 03:58:04 -04:00
|
|
|
_dcm_matrix(1, 0, 0,
|
2012-03-07 00:09:17 -04:00
|
|
|
0, 1, 0,
|
|
|
|
0, 0, 1),
|
2011-06-12 20:49:01 -03:00
|
|
|
_health(1.),
|
2012-02-29 09:45:26 -04:00
|
|
|
_toggle(0)
|
2012-03-08 03:12:46 -04:00
|
|
|
{
|
|
|
|
// 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();
|
2012-03-08 18:03:47 -04:00
|
|
|
_ki_roll_pitch = _gyro_drift_limit * 5;
|
|
|
|
_ki_yaw = _gyro_drift_limit * 8;
|
2012-03-08 03:12:46 -04:00
|
|
|
}
|
2010-12-01 03:58:04 -04:00
|
|
|
|
|
|
|
// Accessors
|
2012-03-07 00:09:17 -04:00
|
|
|
|
|
|
|
// return the smoothed gyro vector corrected for drift
|
|
|
|
Vector3f get_gyro(void) {return _omega_smoothed; }
|
2010-12-14 14:39:02 -04:00
|
|
|
Matrix3f get_dcm_matrix(void) {return _dcm_matrix; }
|
|
|
|
Matrix3f get_dcm_transposed(void) {Matrix3f temp = _dcm_matrix; return temp.transpose();}
|
2012-03-07 00:09:17 -04:00
|
|
|
|
|
|
|
// return the current drift correction integrator value
|
2012-03-08 03:12:46 -04:00
|
|
|
Vector3f get_gyro_drift(void) {return _omega_I; }
|
2011-12-03 21:57:12 -04:00
|
|
|
|
2011-11-05 12:02:23 -03:00
|
|
|
float get_health(void) {return _health;}
|
2011-03-07 21:47:43 -04:00
|
|
|
void set_centripetal(bool b) {_centripetal = b;}
|
|
|
|
bool get_centripetal(void) {return _centripetal;}
|
2010-12-04 02:24:21 -04:00
|
|
|
void set_compass(Compass *compass);
|
2010-12-01 15:53:40 -04:00
|
|
|
|
2010-12-01 03:58:04 -04:00
|
|
|
// Methods
|
2012-03-07 00:09:17 -04:00
|
|
|
void update_DCM(uint8_t drift_correction_frequency=1);
|
2012-03-08 03:12:46 -04:00
|
|
|
void update(void) { update_DCM(); }
|
2012-02-23 07:58:41 -04:00
|
|
|
void matrix_reset(bool recover_eulers = false);
|
2010-12-01 03:58:04 -04:00
|
|
|
|
|
|
|
long roll_sensor; // Degrees * 100
|
|
|
|
long pitch_sensor; // Degrees * 100
|
2012-03-07 00:09:17 -04:00
|
|
|
long yaw_sensor; // Degrees * 100
|
2010-12-01 03:58:04 -04:00
|
|
|
|
|
|
|
float roll; // Radians
|
|
|
|
float pitch; // Radians
|
|
|
|
float yaw; // Radians
|
|
|
|
|
|
|
|
uint8_t gyro_sat_count;
|
2012-02-17 01:15:27 -04:00
|
|
|
uint8_t renorm_range_count;
|
2010-12-01 03:58:04 -04:00
|
|
|
uint8_t renorm_blowup_count;
|
|
|
|
|
2012-03-01 00:22:39 -04:00
|
|
|
// status reporting
|
|
|
|
float get_accel_weight(void);
|
|
|
|
float get_renorm_val(void);
|
|
|
|
float get_error_rp(void);
|
|
|
|
float get_error_yaw(void);
|
2011-06-12 20:49:01 -03:00
|
|
|
|
2010-12-01 03:58:04 -04:00
|
|
|
private:
|
2011-06-12 20:49:01 -03:00
|
|
|
float _kp_roll_pitch;
|
|
|
|
float _ki_roll_pitch;
|
|
|
|
float _kp_yaw;
|
2011-07-08 00:57:12 -03:00
|
|
|
float _ki_yaw;
|
2012-03-08 03:12:46 -04:00
|
|
|
float _gyro_drift_limit; // radians/s/s
|
2012-02-23 20:44:21 -04:00
|
|
|
bool _have_initial_yaw;
|
2011-07-08 00:57:12 -03:00
|
|
|
|
2010-12-01 03:58:04 -04:00
|
|
|
// Methods
|
|
|
|
void read_adc_raw(void);
|
2012-03-02 20:53:31 -04:00
|
|
|
void accel_adjust(Vector3f &accel);
|
2010-12-01 03:58:04 -04:00
|
|
|
float read_adc(int select);
|
|
|
|
void matrix_update(float _G_Dt);
|
|
|
|
void normalize(void);
|
2012-02-22 17:00:25 -04:00
|
|
|
void check_matrix(void);
|
2012-03-07 00:09:17 -04:00
|
|
|
bool renorm(Vector3f const &a, Vector3f &result);
|
|
|
|
void drift_correction(float deltat);
|
2010-12-01 03:58:04 -04:00
|
|
|
void euler_angles(void);
|
|
|
|
|
|
|
|
// members
|
2010-12-02 01:13:35 -04:00
|
|
|
Compass * _compass;
|
2011-02-16 03:54:48 -04:00
|
|
|
|
|
|
|
// 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;
|
2010-12-01 03:58:04 -04:00
|
|
|
|
|
|
|
Matrix3f _dcm_matrix;
|
|
|
|
|
2012-03-02 20:53:31 -04:00
|
|
|
// 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;
|
|
|
|
|
2012-03-08 03:12:46 -04:00
|
|
|
Vector3f _gyro_vector; // Store the gyros turn rate in a 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
|
2012-03-07 00:09:17 -04:00
|
|
|
Vector3f _omega_sum;
|
2012-03-01 07:52:47 -04:00
|
|
|
Vector3f _omega_smoothed;
|
2010-12-01 18:52:11 -04:00
|
|
|
float _health;
|
|
|
|
bool _centripetal;
|
2011-09-11 15:03:55 -03:00
|
|
|
uint8_t _toggle;
|
2012-03-01 00:22:39 -04:00
|
|
|
|
|
|
|
// state to support status reporting
|
|
|
|
float _renorm_val_sum;
|
|
|
|
uint16_t _renorm_val_count;
|
|
|
|
float _error_rp_sum;
|
|
|
|
uint16_t _error_rp_count;
|
|
|
|
float _error_yaw_sum;
|
|
|
|
uint16_t _error_yaw_count;
|
|
|
|
|
2012-03-07 00:09:17 -04:00
|
|
|
// 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;
|
|
|
|
|
2010-12-01 03:58:04 -04:00
|
|
|
};
|
|
|
|
|
|
|
|
#endif
|