ardupilot/libraries/AP_Quaternion/AP_Quaternion.h

138 lines
3.2 KiB
C++

#ifndef AP_Quaternion_h
#define AP_Quaternion_h
#include <AP_Math.h>
#include <inttypes.h>
#include <AP_Compass.h>
#include <AP_GPS.h>
#include <AP_IMU.h>
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
class AP_Quaternion
{
public:
// Constructor
AP_Quaternion(IMU *imu, GPS *&gps) :
_imu(imu),
_gps(gps)
{
// reference direction of flux in earth frame
b_x = 0;
b_z = -1;
// scaled gyro drift limits
beta = sqrt(3.0f / 4.0f) * gyroMeasError;
zeta = sqrt(3.0f / 4.0f) * gyroMeasDrift;
}
// Accessors
void set_centripetal(bool b) {_centripetal = b;}
bool get_centripetal(void) {return _centripetal;}
void set_compass(Compass *compass);
// Methods
void update(void);
// Euler angles (radians)
float roll;
float pitch;
float yaw;
// integer Euler angles (Degrees * 100)
int32_t roll_sensor;
int32_t pitch_sensor;
int32_t yaw_sensor;
// compatibility methods with DCM
void update_DCM(void) { update(); }
void update_DCM_fast(void) { update(); }
Vector3f get_gyro(void) {
// notice the sign reversals here
return Vector3f(-_gyro_corrected.x, -_gyro_corrected.y, _gyro_corrected.z);
}
Vector3f get_gyro_drift(void) {
// notice the sign reversals here
return Vector3f(-gyro_bias.x, -gyro_bias.y, gyro_bias.z);
}
float get_accel_weight(void) { return 0; }
float get_renorm_val(void) { return 0; }
float get_health(void) { return 0; }
void matrix_reset(void) { }
uint8_t gyro_sat_count;
uint8_t renorm_range_count;
uint8_t renorm_blowup_count;
float get_error_rp(void);
float get_error_yaw(void);
Matrix3f get_dcm_matrix(void) {
Matrix3f ret;
quaternion_to_rotation_matrix(q, ret);
return ret;
}
private:
void update_IMU(float deltat, Vector3f &gyro, Vector3f &accel);
void update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, Vector3f &mag);
void update_drift(float deltat, Vector3f &gyro, Vector3f &accel, Vector3f &mag);
bool _have_initial_yaw;
// Methods
void accel_adjust(void);
// members
Compass * _compass;
// time in microseconds of last compass update
uint32_t _compass_last_update;
// 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;
IMU *_imu;
// true if we are doing centripetal acceleration correction
bool _centripetal;
// maximum gyroscope measurement error in rad/s (set to 7 degrees/second)
static const float gyroMeasError = 7.0 * (M_PI/180.0);
// maximum gyroscope drift rate in radians/s/s (set to 0.005
// degrees/s/s, which is 0.3 degrees/s/minute)
static const float gyroMeasDrift = 0.04 * (PI/180.0);
float beta;
float zeta;
// quaternion elements
Quaternion q;
// magnetic flux estimates. These are used for the automatic
// magnetometer calibration
float b_x;
float b_z;
// estimate gyroscope biases error
Vector3f gyro_bias;
// the current corrected gyro vector
Vector3f _gyro_corrected;
// accel and gyro accumulators for drift correction
Vector3f _gyro_sum;
Vector3f _accel_sum;
uint32_t _sum_count;
// estimate of error
float _error_rp_sum;
uint16_t _error_rp_count;
float _error_yaw_sum;
uint16_t _error_yaw_count;
};
#endif