mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-27 11:08:29 -04:00
f70dfe440d
the centripetal code needs to take account of the current gyro bias. It turned out that the accel and gyro smoothing was causing significant control lag, and we're better off just letting the quaternion code handle it via its own smoothing parameters
130 lines
2.9 KiB
C++
130 lines
2.9 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(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_integrator(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);
|
|
|
|
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 10 degrees/second)
|
|
static const float gyroMeasError = 10.0 * (M_PI/180.0);
|
|
|
|
// maximum gyroscope drift rate in radians/s/s (set to 0.02
|
|
// degrees/s/s, which is 1.2 degrees/s/minute)
|
|
static const float gyroMeasDrift = 0.02 * (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;
|
|
|
|
// the current corrected gyro vector
|
|
Vector3f _gyro_corrected;
|
|
|
|
// estimate of error
|
|
float _error_rp_sum;
|
|
uint16_t _error_rp_count;
|
|
float _error_yaw_sum;
|
|
uint16_t _error_yaw_count;
|
|
};
|
|
|
|
#endif
|