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_MPU6000.h

Browse Source
This commit is contained in:
uncrustify 2012-08-21 19:19:51 -07:00 committed by Pat Hickey
parent bc80148c75
commit 8c0e4f3987
1 changed files with 70 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

134
libraries/AP_AHRS/AP_AHRS_MPU6000.h
View File

@ -1,14 +1,14 @@
#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.
*/
* 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
@ -19,78 +19,84 @@ static const float _MPU6000_gyro_drift_rate = 0.5; // This correspond to 0.03 de
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();
// 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;
// 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;
}
// dmp related variable initialisation
_gyro_bias_from_gravity_gain = 0.008;
_compass_bias_time = 0;
}
// initialisation routine to start MPU6000's dmp
void init();
// 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 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; }
// 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);
// 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();
// 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);
// status reporting
float get_error_rp(void);
float get_error_yaw(void);
private:
float _ki;
float _ki_yaw;
AP_InertialSensor_MPU6000 *_mpu6000;
float _ki;
float _ki_yaw;
AP_InertialSensor_MPU6000 *_mpu6000;
// Methods
void drift_correction(float deltat);
// 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);
// 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;
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;
void drift_correction_yaw(void);
float yaw_error_compass();
void euler_angles(void);
Vector3f _accel_vector; // current accel vector
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;
Vector3f _omega_I; // Omega Integrator correction
Vector3f _omega_I_sum;
float _omega_I_sum_time;
// primary representation of attitude
Matrix3f _dcm_matrix;
// state to support status reporting
float _error_yaw_sum;
uint16_t _error_yaw_count;
float _error_yaw_last;
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