mirror of https://github.com/ArduPilot/ardupilot
99 lines
2.0 KiB
C++
99 lines
2.0 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, Compass *compass = NULL) :
|
|
_imu(imu),
|
|
_gps(gps),
|
|
_compass(compass)
|
|
{
|
|
// initial quaternion
|
|
SEq_1 = 1;
|
|
SEq_2 = 0;
|
|
SEq_3 = 0;
|
|
SEq_4 = 0;
|
|
|
|
// 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(float deltat);
|
|
|
|
// 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;
|
|
|
|
|
|
private:
|
|
void update_IMU(float deltat, Vector3f &gyro, Vector3f &accel);
|
|
void update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, Vector3f &mag);
|
|
|
|
bool _have_initial_yaw;
|
|
|
|
// Methods
|
|
void accel_adjust(void);
|
|
|
|
// members
|
|
Compass * _compass;
|
|
|
|
// 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 5 degrees/second)
|
|
static const float gyroMeasError = 5.0 * (M_PI/180.0);
|
|
|
|
// maximum gyroscope drift rate in radians/s/s (set to 0.2 degrees/s/s)
|
|
static const float gyroMeasDrift = 0.2 * (PI/180.0);
|
|
|
|
float beta;
|
|
float zeta;
|
|
|
|
// quaternion elements
|
|
float SEq_1, SEq_2, SEq_3, SEq_4;
|
|
|
|
float b_x;
|
|
float b_z;
|
|
|
|
// estimate gyroscope biases error
|
|
Vector3f gyro_bias;
|
|
};
|
|
|
|
#endif
|