2012-12-12 17:42:14 -04:00
|
|
|
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
|
2012-11-14 12:10:15 -04:00
|
|
|
#ifndef __AP_AHRS_H__
|
|
|
|
#define __AP_AHRS_H__
|
2012-03-19 03:34:12 -03:00
|
|
|
/*
|
2012-08-21 23:19:51 -03:00
|
|
|
* 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.
|
|
|
|
*/
|
2012-03-19 03:34:12 -03:00
|
|
|
|
|
|
|
#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>
|
2012-11-05 00:29:00 -04:00
|
|
|
#include <AP_InertialSensor.h>
|
2012-06-19 01:47:09 -03:00
|
|
|
#include <AP_Baro.h>
|
2012-08-20 20:22:44 -03:00
|
|
|
#include <AP_Param.h>
|
2012-03-19 03:34:12 -03:00
|
|
|
|
2013-02-19 05:50:57 -04:00
|
|
|
#define AP_AHRS_TRIM_LIMIT 10.0f // maximum trim angle in degrees
|
|
|
|
|
2012-03-19 03:34:12 -03:00
|
|
|
class AP_AHRS
|
|
|
|
{
|
|
|
|
public:
|
2012-08-21 23:19:51 -03:00
|
|
|
// Constructor
|
2012-11-05 00:29:00 -04:00
|
|
|
AP_AHRS(AP_InertialSensor *ins, GPS *&gps) :
|
|
|
|
_ins(ins),
|
2013-04-14 02:40:26 -03:00
|
|
|
_gps(gps)
|
2012-08-21 23:19:51 -03:00
|
|
|
{
|
2012-12-12 17:42:14 -04:00
|
|
|
// load default values from var_info table
|
|
|
|
AP_Param::setup_object_defaults(this, var_info);
|
|
|
|
|
2012-08-21 23:19:51 -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
|
2012-11-05 00:29:00 -04:00
|
|
|
_gyro_drift_limit = ins->get_gyro_drift_rate();
|
2013-05-06 01:31:49 -03:00
|
|
|
|
|
|
|
// enable centrifugal correction by default
|
|
|
|
_flags.correct_centrifugal = true;
|
2012-08-21 23:19:51 -03:00
|
|
|
}
|
|
|
|
|
2013-01-13 01:03:35 -04:00
|
|
|
// init sets up INS board orientation
|
2012-11-14 12:10:15 -04:00
|
|
|
virtual void init() {
|
2013-06-03 03:52:28 -03:00
|
|
|
set_orientation();
|
2012-08-21 23:19:51 -03:00
|
|
|
};
|
|
|
|
|
|
|
|
// Accessors
|
2012-11-14 12:10:15 -04:00
|
|
|
void set_fly_forward(bool b) {
|
2013-05-06 01:31:49 -03:00
|
|
|
_flags.fly_forward = b;
|
2012-08-21 23:19:51 -03:00
|
|
|
}
|
2013-05-23 22:21:23 -03:00
|
|
|
|
|
|
|
void set_wind_estimation(bool b) {
|
|
|
|
_flags.wind_estimation = b;
|
|
|
|
}
|
|
|
|
|
2012-11-14 12:10:15 -04:00
|
|
|
void set_compass(Compass *compass) {
|
2012-08-21 23:19:51 -03:00
|
|
|
_compass = compass;
|
2013-06-03 03:52:28 -03:00
|
|
|
set_orientation();
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
// allow for runtime change of orientation
|
|
|
|
// this makes initial config easier
|
|
|
|
void set_orientation() {
|
|
|
|
_ins->set_board_orientation((enum Rotation)_board_orientation.get());
|
2013-01-16 21:27:59 -04:00
|
|
|
if (_compass != NULL) {
|
|
|
|
_compass->set_board_orientation((enum Rotation)_board_orientation.get());
|
|
|
|
}
|
2012-08-21 23:19:51 -03:00
|
|
|
}
|
2013-06-03 03:52:28 -03:00
|
|
|
|
2012-11-14 12:10:15 -04:00
|
|
|
void set_airspeed(AP_Airspeed *airspeed) {
|
2012-08-21 23:19:51 -03:00
|
|
|
_airspeed = airspeed;
|
|
|
|
}
|
|
|
|
|
2013-04-21 09:27:04 -03:00
|
|
|
AP_InertialSensor* get_ins() const {
|
2012-11-05 00:29:00 -04:00
|
|
|
return _ins;
|
2012-08-21 23:14:39 -03:00
|
|
|
}
|
|
|
|
|
2012-12-12 03:22:56 -04:00
|
|
|
// accelerometer values in the earth frame in m/s/s
|
2013-04-21 09:27:04 -03:00
|
|
|
const Vector3f &get_accel_ef(void) const { return _accel_ef; }
|
2012-12-12 03:22:56 -04:00
|
|
|
|
2012-08-21 23:19:51 -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
|
2013-04-19 04:45:54 -03:00
|
|
|
virtual const Vector3f get_gyro(void) const = 0;
|
2012-08-21 23:19:51 -03:00
|
|
|
|
|
|
|
// return the current estimate of the gyro drift
|
2013-04-21 09:27:04 -03:00
|
|
|
virtual const Vector3f &get_gyro_drift(void) const = 0;
|
2012-08-21 23:19:51 -03:00
|
|
|
|
|
|
|
// 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
|
2013-04-21 09:27:04 -03:00
|
|
|
virtual const Matrix3f &get_dcm_matrix(void) const = 0;
|
2012-08-21 23:19:51 -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
|
2013-08-04 21:16:31 -03:00
|
|
|
virtual bool get_position(struct Location &loc) {
|
2013-03-25 04:24:59 -03:00
|
|
|
if (!_gps || _gps->status() <= GPS::NO_FIX) {
|
2012-08-21 23:19:51 -03:00
|
|
|
return false;
|
|
|
|
}
|
2013-08-04 21:16:31 -03:00
|
|
|
loc.lat = _gps->latitude;
|
|
|
|
loc.lng = _gps->longitude;
|
2012-08-21 23:19:51 -03:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2013-05-16 19:32:21 -03:00
|
|
|
// get our projected position, based on our GPS position plus
|
|
|
|
// heading and ground speed
|
2013-08-04 21:16:31 -03:00
|
|
|
bool get_projected_position(struct Location &loc);
|
2013-05-16 19:32:21 -03:00
|
|
|
|
2013-08-12 00:28:23 -03:00
|
|
|
// return the estimated lag in our position due to GPS lag
|
|
|
|
float get_position_lag(void) const;
|
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
// return a wind estimation vector, in m/s
|
2013-03-29 04:22:03 -03:00
|
|
|
virtual Vector3f wind_estimate(void) {
|
2012-08-21 23:19:51 -03:00
|
|
|
return Vector3f(0,0,0);
|
|
|
|
}
|
|
|
|
|
2012-08-24 08:22:58 -03:00
|
|
|
// return an airspeed estimate if available. return true
|
|
|
|
// if we have an estimate
|
2013-03-29 04:22:03 -03:00
|
|
|
virtual bool airspeed_estimate(float *airspeed_ret);
|
2012-08-24 08:22:58 -03:00
|
|
|
|
2013-07-19 22:11:57 -03:00
|
|
|
// return a true airspeed estimate (navigation airspeed) if
|
|
|
|
// available. return true if we have an estimate
|
|
|
|
bool airspeed_estimate_true(float *airspeed_ret) {
|
|
|
|
if (!airspeed_estimate(airspeed_ret)) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
*airspeed_ret *= get_EAS2TAS();
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
// get apparent to true airspeed ratio
|
|
|
|
float get_EAS2TAS(void) const {
|
|
|
|
if (_airspeed) {
|
|
|
|
return _airspeed->get_EAS2TAS();
|
|
|
|
}
|
|
|
|
return 1.0f;
|
|
|
|
}
|
|
|
|
|
2013-06-26 07:46:26 -03:00
|
|
|
// return true if airspeed comes from an airspeed sensor, as
|
|
|
|
// opposed to an IMU estimate
|
|
|
|
bool airspeed_sensor_enabled(void) const {
|
|
|
|
return _airspeed != NULL && _airspeed->use();
|
|
|
|
}
|
|
|
|
|
2013-03-29 03:23:22 -03:00
|
|
|
// return a ground vector estimate in meters/second, in North/East order
|
|
|
|
Vector2f groundspeed_vector(void);
|
|
|
|
|
2013-03-28 23:48:25 -03:00
|
|
|
// return true if we will use compass for yaw
|
2013-04-21 09:27:04 -03:00
|
|
|
virtual bool use_compass(void) const { return _compass && _compass->use_for_yaw(); }
|
2013-03-28 23:48:25 -03:00
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
// return true if yaw has been initialised
|
2013-04-21 09:27:04 -03:00
|
|
|
bool yaw_initialised(void) const {
|
2013-05-06 01:31:49 -03:00
|
|
|
return _flags.have_initial_yaw;
|
2012-08-21 23:19:51 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
// set the fast gains flag
|
|
|
|
void set_fast_gains(bool setting) {
|
2013-05-06 01:31:49 -03:00
|
|
|
_flags.fast_ground_gains = setting;
|
|
|
|
}
|
|
|
|
|
|
|
|
// set the correct centrifugal flag
|
|
|
|
// allows arducopter to disable corrections when disarmed
|
|
|
|
void set_correct_centrifugal(bool setting) {
|
|
|
|
_flags.correct_centrifugal = setting;
|
2012-08-21 23:19:51 -03:00
|
|
|
}
|
|
|
|
|
2013-02-19 05:50:57 -04:00
|
|
|
// get trim
|
2013-04-21 09:27:04 -03:00
|
|
|
const Vector3f &get_trim() const { return _trim.get(); }
|
2012-11-05 00:29:00 -04:00
|
|
|
|
2013-02-19 05:50:57 -04:00
|
|
|
// set trim
|
|
|
|
virtual void set_trim(Vector3f new_trim);
|
2012-11-05 00:29:00 -04:00
|
|
|
|
|
|
|
// add_trim - adjust the roll and pitch trim up to a total of 10 degrees
|
2012-12-19 11:06:20 -04:00
|
|
|
virtual void add_trim(float roll_in_radians, float pitch_in_radians, bool save_to_eeprom = true);
|
2012-11-05 00:29:00 -04:00
|
|
|
|
2013-07-19 22:11:57 -03:00
|
|
|
// for holding parameters
|
|
|
|
static const struct AP_Param::GroupInfo var_info[];
|
|
|
|
|
|
|
|
// these are public for ArduCopter
|
2013-03-29 06:42:26 -03:00
|
|
|
AP_Float _kp_yaw;
|
2012-08-21 23:19:51 -03:00
|
|
|
AP_Float _kp;
|
|
|
|
AP_Float gps_gain;
|
2013-07-19 22:11:57 -03:00
|
|
|
|
|
|
|
protected:
|
|
|
|
// settable parameters
|
|
|
|
AP_Float beta;
|
2012-08-21 23:19:51 -03:00
|
|
|
AP_Int8 _gps_use;
|
2012-09-07 22:27:12 -03:00
|
|
|
AP_Int8 _wind_max;
|
2013-01-13 01:03:35 -04:00
|
|
|
AP_Int8 _board_orientation;
|
2013-05-04 23:47:49 -03:00
|
|
|
AP_Int8 _gps_minsats;
|
2012-08-21 23:19:51 -03:00
|
|
|
|
2013-05-06 01:31:49 -03:00
|
|
|
// flags structure
|
|
|
|
struct ahrs_flags {
|
|
|
|
uint8_t have_initial_yaw : 1; // whether the yaw value has been intialised with a reference
|
|
|
|
uint8_t fast_ground_gains : 1; // should we raise the gain on the accelerometers for faster convergence, used when disarmed for ArduCopter
|
|
|
|
uint8_t fly_forward : 1; // 1 if we can assume the aircraft will be flying forward on its X axis
|
|
|
|
uint8_t correct_centrifugal : 1; // 1 if we should correct for centrifugal forces (allows arducopter to turn this off when motors are disarmed)
|
2013-05-23 22:21:23 -03:00
|
|
|
uint8_t wind_estimation : 1; // 1 if we should do wind estimation
|
2013-05-06 01:31:49 -03:00
|
|
|
} _flags;
|
2012-08-12 22:08:10 -03:00
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
// pointer to compass object, if available
|
|
|
|
Compass * _compass;
|
2012-03-19 03:34:12 -03:00
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
// pointer to airspeed object, if available
|
|
|
|
AP_Airspeed * _airspeed;
|
2012-08-10 19:57:32 -03:00
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
// time in microseconds of last compass update
|
|
|
|
uint32_t _compass_last_update;
|
2012-03-19 03:34:12 -03:00
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
// note: we use ref-to-pointer here so that our caller can change the GPS without our noticing
|
|
|
|
// IMU under us without our noticing.
|
2012-11-05 00:29:00 -04:00
|
|
|
AP_InertialSensor *_ins;
|
|
|
|
GPS *&_gps;
|
|
|
|
|
|
|
|
// a vector to capture the difference between the controller and body frames
|
|
|
|
AP_Vector3f _trim;
|
2012-03-19 03:34:12 -03:00
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
// the limit of the gyro drift claimed by the sensors, in
|
|
|
|
// radians/s/s
|
|
|
|
float _gyro_drift_limit;
|
2012-04-15 21:26:16 -03:00
|
|
|
|
2012-12-12 03:22:56 -04:00
|
|
|
// accelerometer values in the earth frame in m/s/s
|
|
|
|
Vector3f _accel_ef;
|
|
|
|
|
2013-03-29 06:42:26 -03:00
|
|
|
// Declare filter states for HPF and LPF used by complementary
|
|
|
|
// filter in AP_AHRS::groundspeed_vector
|
2013-05-12 22:24:48 -03:00
|
|
|
Vector2f _lp; // ground vector low-pass filter
|
|
|
|
Vector2f _hp; // ground vector high-pass filter
|
|
|
|
Vector2f _lastGndVelADS; // previous HPF input
|
|
|
|
};
|
2012-03-19 03:34:12 -03:00
|
|
|
|
|
|
|
#include <AP_AHRS_DCM.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>
|
|
|
|
|
2012-11-14 12:10:15 -04:00
|
|
|
#endif // __AP_AHRS_H__
|