mirror of https://github.com/ArduPilot/ardupilot
uncrustify libraries/AP_AHRS/AP_AHRS_DCM.h
This commit is contained in:
uncrustify
Pat Hickey
parent
31a4442904
commit
bc80148c75
|
|
@ -1,129 +1,135 @@
|
|||
#ifndef AP_AHRS_DCM_H
|
||||
#define AP_AHRS_DCM_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.
|
||||
*/
|
||||
|
||||
class AP_AHRS_DCM : public AP_AHRS
|
||||
{
|
||||
public:
|
||||
// Constructors
|
||||
AP_AHRS_DCM(IMU *imu, GPS *&gps) : AP_AHRS(imu, gps)
|
||||
{
|
||||
_dcm_matrix.identity();
|
||||
// Constructors
|
||||
AP_AHRS_DCM(IMU *imu, GPS *&gps) : AP_AHRS(imu, gps)
|
||||
{
|
||||
_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;
|
||||
}
|
||||
|
||||
// return the smoothed gyro vector corrected for drift
|
||||
Vector3f get_gyro(void) {return _omega + _omega_P + _omega_yaw_P; }
|
||||
Matrix3f get_dcm_matrix(void) {return _dcm_matrix; }
|
||||
// return the smoothed gyro vector corrected for drift
|
||||
Vector3f get_gyro(void) {
|
||||
return _omega + _omega_P + _omega_yaw_P;
|
||||
}
|
||||
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);
|
||||
|
||||
// dead-reckoning support
|
||||
bool get_position(struct Location *loc);
|
||||
// dead-reckoning support
|
||||
bool get_position(struct Location *loc);
|
||||
|
||||
// status reporting
|
||||
float get_error_rp(void);
|
||||
float get_error_yaw(void);
|
||||
// status reporting
|
||||
float get_error_rp(void);
|
||||
float get_error_yaw(void);
|
||||
|
||||
// return a wind estimation vector, in m/s
|
||||
Vector3f wind_estimate(void) {
|
||||
return _wind;
|
||||
}
|
||||
// return a wind estimation vector, in m/s
|
||||
Vector3f wind_estimate(void) {
|
||||
return _wind;
|
||||
}
|
||||
|
||||
private:
|
||||
float _ki;
|
||||
float _ki_yaw;
|
||||
float _ki;
|
||||
float _ki_yaw;
|
||||
|
||||
// Methods
|
||||
void matrix_update(float _G_Dt);
|
||||
void normalize(void);
|
||||
void check_matrix(void);
|
||||
bool renorm(Vector3f const &a, Vector3f &result);
|
||||
void drift_correction(float deltat);
|
||||
void drift_correction_yaw(void);
|
||||
float yaw_error_compass();
|
||||
float yaw_error_gps();
|
||||
void euler_angles(void);
|
||||
void estimate_wind(Vector3f &velocity);
|
||||
bool have_gps(void);
|
||||
// Methods
|
||||
void matrix_update(float _G_Dt);
|
||||
void normalize(void);
|
||||
void check_matrix(void);
|
||||
bool renorm(Vector3f const &a, Vector3f &result);
|
||||
void drift_correction(float deltat);
|
||||
void drift_correction_yaw(void);
|
||||
float yaw_error_compass();
|
||||
float yaw_error_gps();
|
||||
void euler_angles(void);
|
||||
void estimate_wind(Vector3f &velocity);
|
||||
bool have_gps(void);
|
||||
|
||||
// primary representation of attitude
|
||||
Matrix3f _dcm_matrix;
|
||||
// primary representation of attitude
|
||||
Matrix3f _dcm_matrix;
|
||||
|
||||
Vector3f _gyro_vector; // Store the gyros turn rate in a vector
|
||||
Vector3f _accel_vector; // current accel 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_yaw_P; // proportional yaw correction
|
||||
Vector3f _omega_I; // Omega Integrator correction
|
||||
Vector3f _omega_I_sum;
|
||||
float _omega_I_sum_time;
|
||||
Vector3f _omega; // Corrected Gyro_Vector data
|
||||
Vector3f _omega_P; // accel Omega proportional correction
|
||||
Vector3f _omega_yaw_P; // proportional yaw correction
|
||||
Vector3f _omega_I; // Omega Integrator correction
|
||||
Vector3f _omega_I_sum;
|
||||
float _omega_I_sum_time;
|
||||
Vector3f _omega; // Corrected Gyro_Vector data
|
||||
|
||||
// P term gain based on spin rate
|
||||
float _P_gain(float spin_rate);
|
||||
// P term gain based on spin rate
|
||||
float _P_gain(float spin_rate);
|
||||
|
||||
// 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_rp_last;
|
||||
float _error_yaw_sum;
|
||||
uint16_t _error_yaw_count;
|
||||
float _error_yaw_last;
|
||||
// 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_rp_last;
|
||||
float _error_yaw_sum;
|
||||
uint16_t _error_yaw_count;
|
||||
float _error_yaw_last;
|
||||
|
||||
// time in millis when we last got a GPS heading
|
||||
uint32_t _gps_last_update;
|
||||
// time in millis when we last got a GPS heading
|
||||
uint32_t _gps_last_update;
|
||||
|
||||
// state of accel drift correction
|
||||
Vector3f _ra_sum;
|
||||
Vector3f _last_velocity;
|
||||
float _ra_deltat;
|
||||
uint32_t _ra_sum_start;
|
||||
// state of accel drift correction
|
||||
Vector3f _ra_sum;
|
||||
Vector3f _last_velocity;
|
||||
float _ra_deltat;
|
||||
uint32_t _ra_sum_start;
|
||||
|
||||
// current drift error in earth frame
|
||||
Vector3f _drift_error_earth;
|
||||
// current drift error in earth frame
|
||||
Vector3f _drift_error_earth;
|
||||
|
||||
// whether we have GPS lock
|
||||
bool _have_gps_lock;
|
||||
// whether we have GPS lock
|
||||
bool _have_gps_lock;
|
||||
|
||||
// the lat/lng where we last had GPS lock
|
||||
int32_t _last_lat;
|
||||
int32_t _last_lng;
|
||||
// the lat/lng where we last had GPS lock
|
||||
int32_t _last_lat;
|
||||
int32_t _last_lng;
|
||||
|
||||
// position offset from last GPS lock
|
||||
float _position_offset_north;
|
||||
float _position_offset_east;
|
||||
// position offset from last GPS lock
|
||||
float _position_offset_north;
|
||||
float _position_offset_east;
|
||||
|
||||
// whether we have a position estimate
|
||||
bool _have_position;
|
||||
// whether we have a position estimate
|
||||
bool _have_position;
|
||||
|
||||
// support for wind estimation
|
||||
Vector3f _last_fuse;
|
||||
Vector3f _last_vel;
|
||||
uint32_t _last_wind_time;
|
||||
float _last_airspeed;
|
||||
// support for wind estimation
|
||||
Vector3f _last_fuse;
|
||||
Vector3f _last_vel;
|
||||
uint32_t _last_wind_time;
|
||||
float _last_airspeed;
|
||||
|
||||
// estimated wind in m/s
|
||||
Vector3f _wind;
|
||||
// estimated wind in m/s
|
||||
Vector3f _wind;
|
||||
};
|
||||
|
||||
#endif // AP_AHRS_DCM_H
|
||||
|
|
|
|||
Loading…
Reference in New Issue