2016-02-17 21:25:13 -04:00
|
|
|
#pragma once
|
2012-12-12 17:42:14 -04:00
|
|
|
|
2013-08-29 02:34:34 -03:00
|
|
|
/*
|
|
|
|
This program is free software: you can redistribute it and/or modify
|
|
|
|
it under the terms of the GNU General Public License as published by
|
|
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
|
|
(at your option) any later version.
|
|
|
|
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
|
|
GNU General Public License for more details.
|
|
|
|
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
|
|
*/
|
|
|
|
|
2012-03-19 03:34:12 -03:00
|
|
|
/*
|
2012-08-21 23:19:51 -03:00
|
|
|
* AHRS (Attitude Heading Reference System) interface for ArduPilot
|
|
|
|
*
|
|
|
|
*/
|
2012-03-19 03:34:12 -03:00
|
|
|
|
2015-08-11 03:28:42 -03:00
|
|
|
#include <AP_Math/AP_Math.h>
|
2012-03-19 03:34:12 -03:00
|
|
|
#include <inttypes.h>
|
2015-08-11 03:28:42 -03:00
|
|
|
#include <AP_Compass/AP_Compass.h>
|
|
|
|
#include <AP_Airspeed/AP_Airspeed.h>
|
2016-10-25 18:56:59 -03:00
|
|
|
#include <AP_Beacon/AP_Beacon.h>
|
2015-08-11 03:28:42 -03:00
|
|
|
#include <AP_GPS/AP_GPS.h>
|
|
|
|
#include <AP_InertialSensor/AP_InertialSensor.h>
|
|
|
|
#include <AP_Baro/AP_Baro.h>
|
|
|
|
#include <AP_Param/AP_Param.h>
|
2012-03-19 03:34:12 -03:00
|
|
|
|
2015-11-24 05:35:54 -04:00
|
|
|
class OpticalFlow;
|
2013-02-19 05:50:57 -04:00
|
|
|
#define AP_AHRS_TRIM_LIMIT 10.0f // maximum trim angle in degrees
|
2014-10-21 09:40:16 -03:00
|
|
|
#define AP_AHRS_RP_P_MIN 0.05f // minimum value for AHRS_RP_P parameter
|
|
|
|
#define AP_AHRS_YAW_P_MIN 0.05f // minimum value for AHRS_YAW_P parameter
|
2013-02-19 05:50:57 -04:00
|
|
|
|
2014-04-21 04:10:27 -03:00
|
|
|
enum AHRS_VehicleClass {
|
|
|
|
AHRS_VEHICLE_UNKNOWN,
|
|
|
|
AHRS_VEHICLE_GROUND,
|
|
|
|
AHRS_VEHICLE_COPTER,
|
|
|
|
AHRS_VEHICLE_FIXED_WING,
|
2017-02-18 12:07:19 -04:00
|
|
|
AHRS_VEHICLE_SUBMARINE,
|
2014-04-21 04:10:27 -03:00
|
|
|
};
|
|
|
|
|
|
|
|
|
2017-02-11 04:15:34 -04:00
|
|
|
// forward declare view class
|
|
|
|
class AP_AHRS_View;
|
|
|
|
|
2012-03-19 03:34:12 -03:00
|
|
|
class AP_AHRS
|
|
|
|
{
|
|
|
|
public:
|
2017-02-11 04:15:34 -04:00
|
|
|
friend class AP_AHRS_View;
|
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
// Constructor
|
2014-03-30 08:00:25 -03:00
|
|
|
AP_AHRS(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps) :
|
2014-07-14 11:17:43 -03:00
|
|
|
roll(0.0f),
|
|
|
|
pitch(0.0f),
|
|
|
|
yaw(0.0f),
|
|
|
|
roll_sensor(0),
|
|
|
|
pitch_sensor(0),
|
|
|
|
yaw_sensor(0),
|
2014-04-21 04:10:27 -03:00
|
|
|
_vehicle_class(AHRS_VEHICLE_UNKNOWN),
|
2016-10-30 02:24:21 -03:00
|
|
|
_compass(nullptr),
|
|
|
|
_optflow(nullptr),
|
|
|
|
_airspeed(nullptr),
|
2016-10-25 18:56:59 -03:00
|
|
|
_beacon(nullptr),
|
2014-07-14 11:17:43 -03:00
|
|
|
_compass_last_update(0),
|
2012-11-05 00:29:00 -04:00
|
|
|
_ins(ins),
|
2014-02-15 04:38:43 -04:00
|
|
|
_baro(baro),
|
|
|
|
_gps(gps),
|
2014-02-08 02:52:03 -04:00
|
|
|
_cos_roll(1.0f),
|
|
|
|
_cos_pitch(1.0f),
|
|
|
|
_cos_yaw(1.0f),
|
|
|
|
_sin_roll(0.0f),
|
|
|
|
_sin_pitch(0.0f),
|
2014-02-26 18:41:28 -04:00
|
|
|
_sin_yaw(0.0f),
|
|
|
|
_active_accel_instance(0)
|
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
|
2015-11-03 09:46:29 -04:00
|
|
|
// rate.
|
2013-11-03 23:34:32 -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;
|
2014-01-02 07:06:10 -04:00
|
|
|
|
|
|
|
// initialise _home
|
|
|
|
_home.options = 0;
|
|
|
|
_home.alt = 0;
|
|
|
|
_home.lng = 0;
|
|
|
|
_home.lat = 0;
|
2016-08-09 15:47:59 -03:00
|
|
|
|
|
|
|
_last_trim = _trim.get();
|
|
|
|
_rotation_autopilot_body_to_vehicle_body.from_euler(_last_trim.x, _last_trim.y, 0.0f);
|
|
|
|
_rotation_vehicle_body_to_autopilot_body = _rotation_autopilot_body_to_vehicle_body.transposed();
|
2012-08-21 23:19:51 -03:00
|
|
|
}
|
|
|
|
|
2014-07-15 01:29:34 -03:00
|
|
|
// empty virtual destructor
|
|
|
|
virtual ~AP_AHRS() {}
|
|
|
|
|
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
|
|
|
|
2014-02-27 02:39:49 -04:00
|
|
|
bool get_fly_forward(void) const {
|
|
|
|
return _flags.fly_forward;
|
|
|
|
}
|
|
|
|
|
2017-06-19 00:16:22 -03:00
|
|
|
/*
|
|
|
|
set the "likely flying" flag. This is not guaranteed to be
|
|
|
|
accurate, but is the vehicle codes best guess as to the whether
|
|
|
|
the vehicle is currently flying
|
|
|
|
*/
|
|
|
|
void set_likely_flying(bool b) {
|
|
|
|
if (b && !_flags.likely_flying) {
|
|
|
|
_last_flying_ms = AP_HAL::millis();
|
|
|
|
}
|
|
|
|
_flags.likely_flying = b;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
get the likely flying status. Returns true if the vehicle code
|
|
|
|
thinks we are flying at the moment. Not guaranteed to be
|
|
|
|
accurate
|
|
|
|
*/
|
|
|
|
bool get_likely_flying(void) const {
|
|
|
|
return _flags.likely_flying;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
return time in milliseconds since likely_flying was set
|
|
|
|
true. Returns zero if likely_flying is currently false
|
|
|
|
*/
|
|
|
|
uint32_t get_time_flying_ms(void) const {
|
|
|
|
if (!_flags.likely_flying) {
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
return AP_HAL::millis() - _last_flying_ms;
|
|
|
|
}
|
|
|
|
|
2014-04-21 04:10:27 -03:00
|
|
|
AHRS_VehicleClass get_vehicle_class(void) const {
|
|
|
|
return _vehicle_class;
|
|
|
|
}
|
|
|
|
|
|
|
|
void set_vehicle_class(AHRS_VehicleClass vclass) {
|
|
|
|
_vehicle_class = vclass;
|
|
|
|
}
|
|
|
|
|
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();
|
|
|
|
}
|
|
|
|
|
2013-11-27 22:16:51 -04:00
|
|
|
const Compass* get_compass() const {
|
|
|
|
return _compass;
|
|
|
|
}
|
2015-01-02 20:08:50 -04:00
|
|
|
|
|
|
|
void set_optflow(const OpticalFlow *optflow) {
|
|
|
|
_optflow = optflow;
|
|
|
|
}
|
|
|
|
|
|
|
|
const OpticalFlow* get_optflow() const {
|
|
|
|
return _optflow;
|
|
|
|
}
|
2015-09-23 04:29:43 -03:00
|
|
|
|
2013-06-03 03:52:28 -03:00
|
|
|
// allow for runtime change of orientation
|
|
|
|
// this makes initial config easier
|
|
|
|
void set_orientation() {
|
2013-11-03 23:34:32 -04:00
|
|
|
_ins.set_board_orientation((enum Rotation)_board_orientation.get());
|
2016-10-30 02:24:21 -03:00
|
|
|
if (_compass != nullptr) {
|
2013-01-16 21:27:59 -04:00
|
|
|
_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;
|
|
|
|
}
|
|
|
|
|
2016-10-25 18:56:59 -03:00
|
|
|
void set_beacon(AP_Beacon *beacon) {
|
|
|
|
_beacon = beacon;
|
|
|
|
}
|
|
|
|
|
2013-12-30 06:24:18 -04:00
|
|
|
const AP_Airspeed *get_airspeed(void) const {
|
|
|
|
return _airspeed;
|
|
|
|
}
|
|
|
|
|
2016-10-25 18:56:59 -03:00
|
|
|
const AP_Beacon *get_beacon(void) const {
|
|
|
|
return _beacon;
|
|
|
|
}
|
|
|
|
|
2014-03-30 08:00:25 -03:00
|
|
|
const AP_GPS &get_gps() const {
|
2013-11-27 22:16:51 -04:00
|
|
|
return _gps;
|
|
|
|
}
|
|
|
|
|
2013-11-03 23:34:32 -04:00
|
|
|
const AP_InertialSensor &get_ins() const {
|
2015-09-23 04:29:43 -03:00
|
|
|
return _ins;
|
2012-08-21 23:14:39 -03:00
|
|
|
}
|
|
|
|
|
2014-01-02 02:06:30 -04:00
|
|
|
const AP_Baro &get_baro() const {
|
2015-09-23 04:29:43 -03:00
|
|
|
return _baro;
|
2014-01-02 02:06:30 -04:00
|
|
|
}
|
|
|
|
|
2016-09-03 04:54:36 -03:00
|
|
|
// get the index of the current primary accelerometer sensor
|
|
|
|
virtual uint8_t get_primary_accel_index(void) const {
|
|
|
|
return _ins.get_primary_accel();
|
|
|
|
}
|
|
|
|
|
|
|
|
// get the index of the current primary gyro sensor
|
|
|
|
virtual uint8_t get_primary_gyro_index(void) const {
|
|
|
|
return _ins.get_primary_gyro();
|
|
|
|
}
|
|
|
|
|
2012-12-12 03:22:56 -04:00
|
|
|
// accelerometer values in the earth frame in m/s/s
|
2015-09-23 04:29:43 -03:00
|
|
|
virtual const Vector3f &get_accel_ef(uint8_t i) const {
|
|
|
|
return _accel_ef[i];
|
|
|
|
}
|
|
|
|
virtual const Vector3f &get_accel_ef(void) const {
|
|
|
|
return get_accel_ef(_ins.get_primary_accel());
|
|
|
|
}
|
2014-10-31 19:06:49 -03:00
|
|
|
|
|
|
|
// blended accelerometer values in the earth frame in m/s/s
|
2015-09-23 04:29:43 -03:00
|
|
|
virtual const Vector3f &get_accel_ef_blended(void) const {
|
|
|
|
return _accel_ef_blended;
|
|
|
|
}
|
2012-12-12 03:22:56 -04:00
|
|
|
|
2014-10-06 17:15:19 -03:00
|
|
|
// get yaw rate in earth frame in radians/sec
|
2015-09-23 04:29:43 -03:00
|
|
|
float get_yaw_rate_earth(void) const {
|
2015-12-10 18:05:13 -04:00
|
|
|
return get_gyro() * get_rotation_body_to_ned().c;
|
2015-09-23 04:29:43 -03:00
|
|
|
}
|
2014-10-06 17:15:19 -03:00
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
// Methods
|
2017-04-02 22:03:45 -03:00
|
|
|
virtual void update(bool skip_ins_update=false) = 0;
|
2012-08-21 23:19:51 -03:00
|
|
|
|
2015-09-08 02:50:22 -03:00
|
|
|
// report any reason for why the backend is refusing to initialise
|
2015-09-23 04:29:43 -03:00
|
|
|
virtual const char *prearm_failure_reason(void) const {
|
|
|
|
return nullptr;
|
|
|
|
}
|
|
|
|
|
2016-05-04 01:02:12 -03:00
|
|
|
// is the EKF backend doing its own sensor logging?
|
|
|
|
virtual bool have_ekf_logging(void) const {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
// 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
|
2014-07-13 08:56:39 -03:00
|
|
|
virtual const Vector3f &get_gyro(void) const = 0;
|
2012-08-21 23:19:51 -03:00
|
|
|
|
2017-03-02 07:33:13 -04:00
|
|
|
// return a smoothed and corrected gyro vector using the latest ins data (which may not have been consumed by the EKF yet)
|
|
|
|
Vector3f get_gyro_latest(void) const;
|
|
|
|
|
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
|
|
|
|
2014-10-28 08:22:48 -03:00
|
|
|
// reset the current gyro drift estimate
|
|
|
|
// should be called if gyro offsets are recalculated
|
|
|
|
virtual void reset_gyro_drift(void) = 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;
|
|
|
|
|
2013-11-22 21:37:23 -04:00
|
|
|
// reset the current attitude, used on new IMU calibration
|
|
|
|
virtual void reset_attitude(const float &roll, const float &pitch, const float &yaw) = 0;
|
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
// return the average size of the roll/pitch error estimate
|
|
|
|
// since last call
|
2015-04-21 10:41:46 -03:00
|
|
|
virtual float get_error_rp(void) const = 0;
|
2012-08-21 23:19:51 -03:00
|
|
|
|
|
|
|
// return the average size of the yaw error estimate
|
|
|
|
// since last call
|
2015-04-21 10:41:46 -03:00
|
|
|
virtual float get_error_yaw(void) const = 0;
|
2012-08-21 23:19:51 -03:00
|
|
|
|
|
|
|
// return a DCM rotation matrix representing our current
|
|
|
|
// attitude
|
2015-12-10 18:05:13 -04:00
|
|
|
virtual const Matrix3f &get_rotation_body_to_ned(void) const = 0;
|
2016-08-09 15:47:59 -03:00
|
|
|
const Matrix3f& get_rotation_autopilot_body_to_vehicle_body(void) const { return _rotation_autopilot_body_to_vehicle_body; }
|
|
|
|
const Matrix3f& get_rotation_vehicle_body_to_autopilot_body(void) const { return _rotation_vehicle_body_to_autopilot_body; }
|
2012-08-21 23:19:51 -03:00
|
|
|
|
2014-01-02 07:06:10 -04:00
|
|
|
// get our current position estimate. Return true if a position is available,
|
|
|
|
// otherwise false. This call fills in lat, lng and alt
|
2014-10-17 22:14:34 -03:00
|
|
|
virtual bool get_position(struct Location &loc) const = 0;
|
2013-08-12 00:28:23 -03:00
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
// return a wind estimation vector, in m/s
|
2014-01-02 07:06:10 -04:00
|
|
|
virtual Vector3f wind_estimate(void) = 0;
|
2012-08-21 23:19:51 -03:00
|
|
|
|
2012-08-24 08:22:58 -03:00
|
|
|
// return an airspeed estimate if available. return true
|
|
|
|
// if we have an estimate
|
2013-12-29 03:36:40 -04:00
|
|
|
virtual bool airspeed_estimate(float *airspeed_ret) const;
|
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
|
2013-12-29 03:36:40 -04:00
|
|
|
bool airspeed_estimate_true(float *airspeed_ret) const {
|
2013-07-19 22:11:57 -03:00
|
|
|
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 {
|
2016-10-30 02:24:21 -03:00
|
|
|
return _airspeed != nullptr && _airspeed->use() && _airspeed->healthy();
|
2013-06-26 07:46:26 -03:00
|
|
|
}
|
|
|
|
|
2013-03-29 03:23:22 -03:00
|
|
|
// return a ground vector estimate in meters/second, in North/East order
|
2014-01-01 23:25:41 -04:00
|
|
|
virtual Vector2f groundspeed_vector(void);
|
2013-03-29 03:23:22 -03:00
|
|
|
|
2014-01-03 20:15:34 -04:00
|
|
|
// return a ground velocity in meters/second, North/East/Down
|
|
|
|
// order. This will only be accurate if have_inertial_nav() is
|
2015-09-23 04:29:43 -03:00
|
|
|
// true
|
|
|
|
virtual bool get_velocity_NED(Vector3f &vec) const {
|
|
|
|
return false;
|
|
|
|
}
|
2014-01-03 20:15:34 -04:00
|
|
|
|
2016-01-04 19:57:17 -04:00
|
|
|
// returns the expected NED magnetic field
|
|
|
|
virtual bool get_expected_mag_field_NED(Vector3f &ret) const {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
// returns the estimated magnetic field offsets in body frame
|
|
|
|
virtual bool get_mag_field_correction(Vector3f &ret) const {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2014-01-03 20:15:34 -04:00
|
|
|
// return a position relative to home in meters, North/East/Down
|
|
|
|
// order. This will only be accurate if have_inertial_nav() is
|
2015-09-23 04:29:43 -03:00
|
|
|
// true
|
2017-01-30 15:06:13 -04:00
|
|
|
virtual bool get_relative_position_NED_home(Vector3f &vec) const {
|
2015-09-23 04:29:43 -03:00
|
|
|
return false;
|
|
|
|
}
|
2014-01-03 20:15:34 -04:00
|
|
|
|
2017-01-30 15:06:13 -04:00
|
|
|
// return a position relative to origin in meters, North/East/Down
|
|
|
|
// order. This will only be accurate if have_inertial_nav() is
|
|
|
|
// true
|
|
|
|
virtual bool get_relative_position_NED_origin(Vector3f &vec) const {
|
|
|
|
return false;
|
|
|
|
}
|
2016-07-09 08:36:09 -03:00
|
|
|
// return a position relative to home in meters, North/East
|
|
|
|
// order. Return true if estimate is valid
|
2017-01-30 15:06:13 -04:00
|
|
|
virtual bool get_relative_position_NE_home(Vector2f &vecNE) const {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
// return a position relative to origin in meters, North/East
|
|
|
|
// order. Return true if estimate is valid
|
|
|
|
virtual bool get_relative_position_NE_origin(Vector2f &vecNE) const {
|
2016-07-09 08:36:09 -03:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
// return a Down position relative to home in meters
|
2017-01-30 15:06:13 -04:00
|
|
|
// if EKF is unavailable will return the baro altitude
|
2017-02-21 21:38:14 -04:00
|
|
|
virtual void get_relative_position_D_home(float &posD) const = 0;
|
2017-01-30 15:06:13 -04:00
|
|
|
|
|
|
|
// return a Down position relative to origin in meters
|
2016-07-09 08:36:09 -03:00
|
|
|
// Return true if estimate is valid
|
2017-01-30 15:06:13 -04:00
|
|
|
virtual bool get_relative_position_D_origin(float &posD) const {
|
2016-07-09 08:36:09 -03:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2013-09-08 21:17:45 -03:00
|
|
|
// return ground speed estimate in meters/second. Used by ground vehicles.
|
2016-05-31 02:55:41 -03:00
|
|
|
float groundspeed(void) {
|
|
|
|
return groundspeed_vector().length();
|
2013-09-08 21:17:45 -03:00
|
|
|
}
|
|
|
|
|
2013-03-28 23:48:25 -03:00
|
|
|
// return true if we will use compass for yaw
|
2015-09-23 04:29:43 -03:00
|
|
|
virtual bool use_compass(void) {
|
|
|
|
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
|
|
|
}
|
|
|
|
|
2013-05-06 01:31:49 -03:00
|
|
|
// set the correct centrifugal flag
|
|
|
|
// allows arducopter to disable corrections when disarmed
|
2014-02-18 19:28:14 -04:00
|
|
|
void set_correct_centrifugal(bool setting) {
|
2013-05-06 01:31:49 -03:00
|
|
|
_flags.correct_centrifugal = setting;
|
2012-08-21 23:19:51 -03:00
|
|
|
}
|
|
|
|
|
2014-02-18 19:28:14 -04:00
|
|
|
// get the correct centrifugal flag
|
|
|
|
bool get_correct_centrifugal(void) const {
|
|
|
|
return _flags.correct_centrifugal;
|
|
|
|
}
|
|
|
|
|
2013-02-19 05:50:57 -04:00
|
|
|
// get trim
|
2015-09-23 04:29:43 -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
|
|
|
|
2014-02-08 02:52:03 -04:00
|
|
|
// helper trig value accessors
|
2015-09-23 04:29:43 -03:00
|
|
|
float cos_roll() const {
|
|
|
|
return _cos_roll;
|
|
|
|
}
|
|
|
|
float cos_pitch() const {
|
|
|
|
return _cos_pitch;
|
|
|
|
}
|
|
|
|
float cos_yaw() const {
|
|
|
|
return _cos_yaw;
|
|
|
|
}
|
|
|
|
float sin_roll() const {
|
|
|
|
return _sin_roll;
|
|
|
|
}
|
|
|
|
float sin_pitch() const {
|
|
|
|
return _sin_pitch;
|
|
|
|
}
|
|
|
|
float sin_yaw() const {
|
|
|
|
return _sin_yaw;
|
|
|
|
}
|
2014-02-08 02:52:03 -04:00
|
|
|
|
2013-07-19 22:11:57 -03:00
|
|
|
// for holding parameters
|
|
|
|
static const struct AP_Param::GroupInfo var_info[];
|
|
|
|
|
2014-01-01 22:47:40 -04:00
|
|
|
// return secondary attitude solution if available, as eulers in radians
|
2015-09-23 04:29:43 -03:00
|
|
|
virtual bool get_secondary_attitude(Vector3f &eulers) {
|
|
|
|
return false;
|
|
|
|
}
|
2014-01-01 22:47:40 -04:00
|
|
|
|
2017-04-15 08:21:09 -03:00
|
|
|
// return secondary attitude solution if available, as quaternion
|
|
|
|
virtual bool get_secondary_quaternion(Quaternion &quat) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2014-01-01 22:47:40 -04:00
|
|
|
// return secondary position solution if available
|
2015-09-23 04:29:43 -03:00
|
|
|
virtual bool get_secondary_position(struct Location &loc) {
|
|
|
|
return false;
|
|
|
|
}
|
2014-01-01 22:47:40 -04:00
|
|
|
|
2014-01-02 07:06:10 -04:00
|
|
|
// get the home location. This is const to prevent any changes to
|
2015-09-23 04:29:43 -03:00
|
|
|
// home without telling AHRS about the change
|
|
|
|
const struct Location &get_home(void) const {
|
|
|
|
return _home;
|
|
|
|
}
|
2014-01-02 07:06:10 -04:00
|
|
|
|
|
|
|
// set the home location in 10e7 degrees. This should be called
|
|
|
|
// when the vehicle is at this position. It is assumed that the
|
|
|
|
// current barometer and GPS altitudes correspond to this altitude
|
2014-03-30 08:00:25 -03:00
|
|
|
virtual void set_home(const Location &loc) = 0;
|
2014-01-02 07:06:10 -04:00
|
|
|
|
2017-04-19 03:28:14 -03:00
|
|
|
// set the EKF's origin location in 10e7 degrees. This should only
|
|
|
|
// be called when the EKF has no absolute position reference (i.e. GPS)
|
|
|
|
// from which to decide the origin on its own
|
|
|
|
virtual bool set_origin(const Location &loc) { return false; }
|
|
|
|
|
2014-01-03 20:15:34 -04:00
|
|
|
// return true if the AHRS object supports inertial navigation,
|
|
|
|
// with very accurate position and velocity
|
2015-09-23 04:29:43 -03:00
|
|
|
virtual bool have_inertial_nav(void) const {
|
|
|
|
return false;
|
|
|
|
}
|
2014-01-03 20:15:34 -04:00
|
|
|
|
2014-02-26 18:41:28 -04:00
|
|
|
// return the active accelerometer instance
|
2015-09-23 04:29:43 -03:00
|
|
|
uint8_t get_active_accel_instance(void) const {
|
|
|
|
return _active_accel_instance;
|
|
|
|
}
|
2014-02-26 18:41:28 -04:00
|
|
|
|
2014-05-15 04:09:18 -03:00
|
|
|
// is the AHRS subsystem healthy?
|
2015-01-31 22:31:24 -04:00
|
|
|
virtual bool healthy(void) const = 0;
|
2014-05-15 04:09:18 -03:00
|
|
|
|
2014-10-02 01:43:46 -03:00
|
|
|
// true if the AHRS has completed initialisation
|
2015-09-23 04:29:43 -03:00
|
|
|
virtual bool initialised(void) const {
|
|
|
|
return true;
|
|
|
|
};
|
2014-09-29 05:37:14 -03:00
|
|
|
|
2015-09-23 04:46:51 -03:00
|
|
|
// return the amount of yaw angle change due to the last yaw angle reset in radians
|
2015-09-24 03:51:21 -03:00
|
|
|
// returns the time of the last yaw angle reset or 0 if no reset has ever occurred
|
2015-10-29 05:31:02 -03:00
|
|
|
virtual uint32_t getLastYawResetAngle(float &yawAng) const {
|
|
|
|
return 0;
|
2015-09-23 04:46:51 -03:00
|
|
|
};
|
|
|
|
|
2015-10-29 02:18:04 -03:00
|
|
|
// return the amount of NE position change in metres due to the last reset
|
|
|
|
// returns the time of the last reset or 0 if no reset has ever occurred
|
2015-10-29 05:34:41 -03:00
|
|
|
virtual uint32_t getLastPosNorthEastReset(Vector2f &pos) const {
|
2015-10-29 02:18:04 -03:00
|
|
|
return 0;
|
|
|
|
};
|
|
|
|
|
|
|
|
// return the amount of NE velocity change in metres/sec due to the last reset
|
|
|
|
// returns the time of the last reset or 0 if no reset has ever occurred
|
2015-10-29 05:34:41 -03:00
|
|
|
virtual uint32_t getLastVelNorthEastReset(Vector2f &vel) const {
|
2015-10-29 02:18:04 -03:00
|
|
|
return 0;
|
|
|
|
};
|
|
|
|
|
2016-11-22 01:28:14 -04:00
|
|
|
// return the amount of vertical position change due to the last reset in meters
|
|
|
|
// returns the time of the last reset or 0 if no reset has ever occurred
|
|
|
|
virtual uint32_t getLastPosDownReset(float &posDelta) const {
|
|
|
|
return 0;
|
|
|
|
};
|
|
|
|
|
2015-09-23 04:46:51 -03:00
|
|
|
// Resets the baro so that it reads zero at the current height
|
|
|
|
// Resets the EKF height to zero
|
|
|
|
// Adjusts the EKf origin height so that the EKF height + origin height is the same as before
|
|
|
|
// Returns true if the height datum reset has been performed
|
|
|
|
// If using a range finder for height no reset is performed and it returns false
|
|
|
|
virtual bool resetHeightDatum(void) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2016-09-21 18:00:21 -03:00
|
|
|
// get_variances - provides the innovations normalised using the innovation variance where a value of 0
|
|
|
|
// indicates prefect consistency between the measurement and the EKF solution and a value of of 1 is the maximum
|
|
|
|
// inconsistency that will be accpeted by the filter
|
|
|
|
// boolean false is returned if variances are not available
|
|
|
|
virtual bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2015-05-20 02:21:33 -03:00
|
|
|
// time that the AHRS has been up
|
|
|
|
virtual uint32_t uptime_ms(void) const = 0;
|
|
|
|
|
2016-01-06 17:34:32 -04:00
|
|
|
// get the selected ekf type, for allocation decisions
|
|
|
|
int8_t get_ekf_type(void) const {
|
|
|
|
return _ekf_type;
|
|
|
|
}
|
2016-08-11 16:58:02 -03:00
|
|
|
|
|
|
|
// Retrieves the corrected NED delta velocity in use by the inertial navigation
|
|
|
|
virtual void getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const { ret.zero(); _ins.get_delta_velocity(ret); dt = _ins.get_delta_velocity_dt(); }
|
2017-02-11 04:15:34 -04:00
|
|
|
|
|
|
|
// create a view
|
|
|
|
AP_AHRS_View *create_view(enum Rotation rotation);
|
2016-01-06 17:34:32 -04:00
|
|
|
|
2017-04-09 08:17:05 -03:00
|
|
|
// return calculated AOA
|
|
|
|
float getAOA(void);
|
|
|
|
|
|
|
|
// return calculated SSA
|
|
|
|
float getSSA(void);
|
|
|
|
|
|
|
|
virtual void update_AOA_SSA(void);
|
|
|
|
|
2013-07-19 22:11:57 -03:00
|
|
|
protected:
|
2014-04-21 04:10:27 -03:00
|
|
|
AHRS_VehicleClass _vehicle_class;
|
|
|
|
|
2013-07-19 22:11:57 -03:00
|
|
|
// settable parameters
|
2015-04-27 21:26:36 -03:00
|
|
|
// these are public for ArduCopter
|
|
|
|
AP_Float _kp_yaw;
|
|
|
|
AP_Float _kp;
|
|
|
|
AP_Float gps_gain;
|
|
|
|
|
2013-07-19 22:11:57 -03:00
|
|
|
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;
|
2013-11-04 00:50:33 -04:00
|
|
|
AP_Int8 _gps_delay;
|
2015-09-22 22:40:25 -03:00
|
|
|
AP_Int8 _ekf_type;
|
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 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
|
2017-06-19 00:16:22 -03:00
|
|
|
uint8_t likely_flying : 1; // 1 if vehicle is probably flying
|
2013-05-06 01:31:49 -03:00
|
|
|
} _flags;
|
2012-08-12 22:08:10 -03:00
|
|
|
|
2017-06-19 00:16:22 -03:00
|
|
|
// time when likely_flying last went true
|
|
|
|
uint32_t _last_flying_ms;
|
|
|
|
|
2017-02-11 04:15:34 -04:00
|
|
|
// calculate sin/cos of roll/pitch/yaw from rotation
|
|
|
|
void calc_trig(const Matrix3f &rot,
|
|
|
|
float &cr, float &cp, float &cy,
|
|
|
|
float &sr, float &sp, float &sy) const;
|
|
|
|
|
2014-02-08 02:52:03 -04:00
|
|
|
// update_trig - recalculates _cos_roll, _cos_pitch, etc based on latest attitude
|
|
|
|
// should be called after _dcm_matrix is updated
|
|
|
|
void update_trig(void);
|
|
|
|
|
2014-10-14 23:18:08 -03:00
|
|
|
// update roll_sensor, pitch_sensor and yaw_sensor
|
|
|
|
void update_cd_values(void);
|
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
// pointer to compass object, if available
|
|
|
|
Compass * _compass;
|
2012-03-19 03:34:12 -03:00
|
|
|
|
2015-01-02 20:08:50 -04:00
|
|
|
// pointer to OpticalFlow object, if available
|
|
|
|
const OpticalFlow *_optflow;
|
|
|
|
|
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
|
|
|
|
2016-10-25 18:56:59 -03:00
|
|
|
// pointer to beacon object, if available
|
|
|
|
AP_Beacon * _beacon;
|
|
|
|
|
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.
|
2013-11-03 23:34:32 -04:00
|
|
|
AP_InertialSensor &_ins;
|
2014-01-02 02:06:30 -04:00
|
|
|
AP_Baro &_baro;
|
2014-03-30 08:00:25 -03:00
|
|
|
const AP_GPS &_gps;
|
2012-11-05 00:29:00 -04:00
|
|
|
|
|
|
|
// a vector to capture the difference between the controller and body frames
|
|
|
|
AP_Vector3f _trim;
|
2012-03-19 03:34:12 -03:00
|
|
|
|
2016-08-09 15:47:59 -03:00
|
|
|
// cached trim rotations
|
|
|
|
Vector3f _last_trim;
|
|
|
|
Matrix3f _rotation_autopilot_body_to_vehicle_body;
|
|
|
|
Matrix3f _rotation_vehicle_body_to_autopilot_body;
|
|
|
|
|
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
|
2014-02-26 18:41:28 -04:00
|
|
|
Vector3f _accel_ef[INS_MAX_INSTANCES];
|
2014-10-31 19:06:49 -03:00
|
|
|
Vector3f _accel_ef_blended;
|
2012-12-12 03:22:56 -04:00
|
|
|
|
2015-09-23 04:29:43 -03:00
|
|
|
// Declare filter states for HPF and LPF used by complementary
|
|
|
|
// filter in AP_AHRS::groundspeed_vector
|
|
|
|
Vector2f _lp; // ground vector low-pass filter
|
|
|
|
Vector2f _hp; // ground vector high-pass filter
|
|
|
|
Vector2f _lastGndVelADS; // previous HPF input
|
2014-02-08 02:52:03 -04:00
|
|
|
|
2014-01-02 07:06:10 -04:00
|
|
|
// reference position for NED positions
|
|
|
|
struct Location _home;
|
|
|
|
|
2014-02-08 02:52:03 -04:00
|
|
|
// helper trig variables
|
|
|
|
float _cos_roll, _cos_pitch, _cos_yaw;
|
|
|
|
float _sin_roll, _sin_pitch, _sin_yaw;
|
2014-02-26 18:41:28 -04:00
|
|
|
|
|
|
|
// which accelerometer instance is active
|
|
|
|
uint8_t _active_accel_instance;
|
2017-02-11 04:15:34 -04:00
|
|
|
|
|
|
|
// optional view class
|
|
|
|
AP_AHRS_View *_view;
|
2017-04-09 08:17:05 -03:00
|
|
|
|
|
|
|
// AOA and SSA
|
|
|
|
float _AOA, _SSA;
|
|
|
|
uint32_t _last_AOA_update_ms;
|
2013-05-12 22:24:48 -03:00
|
|
|
};
|
2012-03-19 03:34:12 -03:00
|
|
|
|
2015-08-11 03:28:42 -03:00
|
|
|
#include "AP_AHRS_DCM.h"
|
|
|
|
#include "AP_AHRS_NavEKF.h"
|
2012-03-19 03:34:12 -03:00
|
|
|
|
2015-02-25 00:07:26 -04:00
|
|
|
#if AP_AHRS_NAVEKF_AVAILABLE
|
|
|
|
#define AP_AHRS_TYPE AP_AHRS_NavEKF
|
|
|
|
#else
|
|
|
|
#define AP_AHRS_TYPE AP_AHRS
|
|
|
|
#endif
|