2012-03-19 03:34:12 -03:00
|
|
|
#ifndef AP_AHRS_H
|
|
|
|
#define AP_AHRS_H
|
|
|
|
/*
|
|
|
|
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.
|
|
|
|
*/
|
|
|
|
|
|
|
|
#include <AP_Math.h>
|
|
|
|
#include <inttypes.h>
|
|
|
|
#include <AP_Compass.h>
|
2012-08-10 19:57:32 -03:00
|
|
|
#include <AP_Airspeed.h>
|
2012-03-19 03:34:12 -03:00
|
|
|
#include <AP_GPS.h>
|
|
|
|
#include <AP_IMU.h>
|
2012-06-19 01:47:09 -03:00
|
|
|
#include <AP_Baro.h>
|
2012-03-19 03:34:12 -03:00
|
|
|
|
|
|
|
#if defined(ARDUINO) && ARDUINO >= 100
|
|
|
|
#include "Arduino.h"
|
|
|
|
#else
|
|
|
|
#include "WProgram.h"
|
|
|
|
#endif
|
|
|
|
|
|
|
|
class AP_AHRS
|
|
|
|
{
|
|
|
|
public:
|
|
|
|
// Constructor
|
|
|
|
AP_AHRS(IMU *imu, GPS *&gps):
|
|
|
|
_imu(imu),
|
2012-06-27 22:09:22 -03:00
|
|
|
_gps(gps),
|
|
|
|
_barometer(NULL)
|
2012-03-19 03:34:12 -03:00
|
|
|
{
|
|
|
|
// base the ki values by the sensors maximum drift
|
|
|
|
// rate. The APM2 has gyros which are much less drift
|
|
|
|
// prone than the APM1, so we should have a lower ki,
|
|
|
|
// which will make us less prone to increasing omegaI
|
|
|
|
// incorrectly due to sensor noise
|
|
|
|
_gyro_drift_limit = imu->get_gyro_drift_rate();
|
|
|
|
}
|
|
|
|
|
2012-07-28 02:16:56 -03:00
|
|
|
// empty init
|
|
|
|
virtual void init() {};
|
|
|
|
|
2012-03-19 03:34:12 -03:00
|
|
|
// Accessors
|
2012-03-23 02:22:56 -03:00
|
|
|
void set_fly_forward(bool b) { _fly_forward = b; }
|
2012-03-19 03:34:12 -03:00
|
|
|
void set_compass(Compass *compass) { _compass = compass; }
|
2012-06-19 01:47:09 -03:00
|
|
|
void set_barometer(AP_Baro *barometer) { _barometer = barometer; }
|
2012-08-10 19:57:32 -03:00
|
|
|
void set_airspeed(AP_Airspeed *airspeed) { _airspeed = airspeed; }
|
2012-03-19 03:34:12 -03:00
|
|
|
|
|
|
|
// Methods
|
|
|
|
virtual void update(void) = 0;
|
|
|
|
|
|
|
|
// 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;
|
|
|
|
|
|
|
|
// return a smoothed and corrected gyro vector
|
|
|
|
virtual Vector3f get_gyro(void) = 0;
|
|
|
|
|
|
|
|
// return the current estimate of the gyro drift
|
|
|
|
virtual Vector3f get_gyro_drift(void) = 0;
|
|
|
|
|
|
|
|
// reset the current attitude, used on new IMU calibration
|
|
|
|
virtual void reset(bool recover_eulers=false) = 0;
|
|
|
|
|
|
|
|
// how often our attitude representation has gone out of range
|
|
|
|
uint8_t renorm_range_count;
|
|
|
|
|
|
|
|
// how often our attitude representation has blown up completely
|
|
|
|
uint8_t renorm_blowup_count;
|
|
|
|
|
|
|
|
// return the average size of the roll/pitch error estimate
|
|
|
|
// since last call
|
|
|
|
virtual float get_error_rp(void) = 0;
|
|
|
|
|
|
|
|
// return the average size of the yaw error estimate
|
|
|
|
// since last call
|
|
|
|
virtual float get_error_yaw(void) = 0;
|
|
|
|
|
|
|
|
// return a DCM rotation matrix representing our current
|
|
|
|
// attitude
|
|
|
|
virtual Matrix3f get_dcm_matrix(void) = 0;
|
|
|
|
|
2012-08-10 23:00:31 -03:00
|
|
|
// get our current position, either from GPS or via
|
|
|
|
// dead-reckoning. Return true if a position is available,
|
|
|
|
// otherwise false. This only updates the lat and lng fields
|
|
|
|
// of the Location
|
|
|
|
bool get_position(struct Location *loc) {
|
|
|
|
if (!_gps || _gps->status() != GPS::GPS_OK) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
loc->lat = _gps->latitude;
|
|
|
|
loc->lng = _gps->longitude;
|
|
|
|
return true;
|
|
|
|
}
|
2012-04-16 07:51:46 -03:00
|
|
|
|
2012-08-11 02:55:53 -03:00
|
|
|
// return a wind estimation vector, in m/s
|
|
|
|
Vector3f wind_estimate(void) {
|
|
|
|
return _wind;
|
|
|
|
}
|
|
|
|
|
2012-03-19 03:34:12 -03:00
|
|
|
protected:
|
2012-08-10 19:57:32 -03:00
|
|
|
// pointer to compass object, if available
|
2012-03-19 03:34:12 -03:00
|
|
|
Compass * _compass;
|
|
|
|
|
2012-08-10 19:57:32 -03:00
|
|
|
// pointer to airspeed object, if available
|
|
|
|
AP_Airspeed * _airspeed;
|
|
|
|
|
2012-03-19 03:34:12 -03:00
|
|
|
// 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.
|
|
|
|
IMU *_imu;
|
2012-04-09 05:35:19 -03:00
|
|
|
GPS *&_gps;
|
2012-06-27 22:09:22 -03:00
|
|
|
AP_Baro *_barometer;
|
2012-03-19 03:34:12 -03:00
|
|
|
|
2012-06-27 22:09:22 -03:00
|
|
|
// true if we can assume the aircraft will be flying forward
|
|
|
|
// on its X axis
|
2012-03-23 02:22:56 -03:00
|
|
|
bool _fly_forward;
|
2012-03-19 03:34:12 -03:00
|
|
|
|
|
|
|
// the limit of the gyro drift claimed by the sensors, in
|
|
|
|
// radians/s/s
|
|
|
|
float _gyro_drift_limit;
|
2012-08-11 02:55:53 -03:00
|
|
|
|
|
|
|
// estimated wind in m/s
|
|
|
|
Vector3f _wind;
|
2012-04-15 21:26:16 -03:00
|
|
|
|
|
|
|
// acceleration due to gravity in m/s/s
|
|
|
|
static const float _gravity = 9.80665;
|
2012-03-19 03:34:12 -03:00
|
|
|
};
|
|
|
|
|
|
|
|
#include <AP_AHRS_DCM.h>
|
|
|
|
#include <AP_AHRS_Quaternion.h>
|
2012-07-28 02:16:56 -03:00
|
|
|
#include <AP_AHRS_MPU6000.h>
|
2012-03-19 03:34:12 -03:00
|
|
|
#include <AP_AHRS_HIL.h>
|
|
|
|
|
|
|
|
#endif // AP_AHRS_H
|