mirror of https://github.com/ArduPilot/ardupilot
106 lines
3.0 KiB
C
106 lines
3.0 KiB
C
|
#ifndef AP_AHRS_MPU6000_H
|
||
|
|
#define AP_AHRS_MPU6000_H
|
||
|
|
/*
|
||
|
|
DCM based AHRS (Attitude Heading Reference System) interface for
|
||
|
|
ArduPilot
|
||
|
|
|
||
|
|
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.
|
||
|
|
*/
|
||
|
|
|
||
|
|
#define GYRO_BIAS_FROM_GRAVITY_RATE 4 // Rate of the gyro bias from gravity correction (200Hz/4) => 50Hz
|
||
|
|
#define HEADING_UNKNOWN -9999 // Initial value to detect that compass correction is not initialized
|
||
|
|
|
||
|
|
// max rate of gyro drift in gyro_LSB_units/s (16.4LSB = 1deg/s)
|
||
|
|
static const float _MPU6000_gyro_drift_rate = 0.5; // This correspond to 0.03 degrees/s/s;
|
||
|
|
|
||
|
|
class AP_AHRS_MPU6000 : public AP_AHRS
|
||
|
|
{
|
||
|
|
public:
|
||
|
|
// Constructors
|
||
|
|
AP_AHRS_MPU6000(AP_IMU_INS *imu, GPS *&gps, AP_InertialSensor_MPU6000 *mpu6000) : AP_AHRS(imu, gps), _mpu6000(mpu6000)
|
||
|
|
{
|
||
|
|
_dcm_matrix.identity();
|
||
|
|
|
||
|
|
// these are experimentally derived from the simulator
|
||
|
|
// with large drift levels
|
||
|
|
_ki = 0.0087;
|
||
|
|
_ki_yaw = 0.01;
|
||
|
|
|
||
|
|
// dmp related variable initialisation
|
||
|
|
_gyro_bias_from_gravity_gain = 0.008;
|
||
|
|
_compass_bias_time = 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
// initialisation routine to start MPU6000's dmp
|
||
|
|
void init();
|
||
|
|
|
||
|
|
// return the smoothed gyro vector corrected for drift
|
||
|
|
Vector3f get_gyro(void) {return _imu->get_gyro(); }
|
||
|
|
Matrix3f get_dcm_matrix(void) {return _dcm_matrix; }
|
||
|
|
|
||
|
|
// return the current drift correction integrator value
|
||
|
|
Vector3f get_gyro_drift(void) {return _omega_I; }
|
||
|
|
|
||
|
|
// Methods
|
||
|
|
void update(void);
|
||
|
|
void reset(bool recover_eulers = false);
|
||
|
|
|
||
|
|
// push offsets down from IMU to INS (required so MPU6000 can perform it's own attitude estimation)
|
||
|
|
void push_offsets_to_ins();
|
||
|
|
void push_gains_to_dmp();
|
||
|
|
|
||
|
|
// status reporting
|
||
|
|
float get_error_rp(void);
|
||
|
|
float get_error_yaw(void);
|
||
|
|
|
||
|
|
// settable parameters
|
||
|
|
AP_Float _kp_yaw;
|
||
|
|
AP_Float _kp;
|
||
|
|
AP_Float gps_gain; // not currently supported
|
||
|
|
|
||
|
|
// for holding parameters
|
||
|
|
static const struct AP_Param::GroupInfo var_info[];
|
||
|
|
|
||
|
|
private:
|
||
|
|
float _ki;
|
||
|
|
float _ki_yaw;
|
||
|
|
bool _have_initial_yaw;
|
||
|
|
AP_InertialSensor_MPU6000 *_mpu6000;
|
||
|
|
|
||
|
|
// Methods
|
||
|
|
void drift_correction(float deltat);
|
||
|
|
|
||
|
|
// Compass correction variables. TO-DO: move or replace?
|
||
|
|
float wrap_PI(float angle_in_radians); // TO-DO: move this to standard AP_AHRS methods
|
||
|
|
long _compass_bias_time;
|
||
|
|
|
||
|
|
void drift_correction_yaw(void);
|
||
|
|
float yaw_error_compass();
|
||
|
|
void euler_angles(void);
|
||
|
|
|
||
|
|
Vector3f _accel_filtered;
|
||
|
|
int16_t _accel_filtered_samples;
|
||
|
|
float _gyro_bias[3]; // bias_tracking
|
||
|
|
float _gyro_bias_from_gravity_gain; // bias correction algorithm gain
|
||
|
|
uint8_t _gyro_bias_from_gravity_counter;
|
||
|
|
|
||
|
|
// primary representation of attitude
|
||
|
|
Matrix3f _dcm_matrix;
|
||
|
|
|
||
|
|
Vector3f _accel_vector; // current accel vector
|
||
|
|
|
||
|
|
Vector3f _omega_I; // Omega Integrator correction
|
||
|
|
Vector3f _omega_I_sum;
|
||
|
|
float _omega_I_sum_time;
|
||
|
|
|
||
|
|
// state to support status reporting
|
||
|
|
float _error_yaw_sum;
|
||
|
|
uint16_t _error_yaw_count;
|
||
|
|
float _error_yaw_last;
|
||
|
|
};
|
||
|
|
|
||
|
|
#endif // AP_AHRS_MPU6000_H
|