2014-01-01 21:15:58 -04:00
|
|
|
#ifndef __AP_AHRS_NAVEKF_H__
|
|
|
|
#define __AP_AHRS_NAVEKF_H__
|
|
|
|
/*
|
|
|
|
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/>.
|
|
|
|
*/
|
|
|
|
|
|
|
|
/*
|
|
|
|
* NavEKF based AHRS (Attitude Heading Reference System) interface for
|
|
|
|
* ArduPilot
|
|
|
|
*
|
|
|
|
*/
|
|
|
|
|
2014-02-16 02:57:09 -04:00
|
|
|
#include <AP_HAL.h>
|
2014-01-01 21:15:58 -04:00
|
|
|
#include <AP_AHRS.h>
|
|
|
|
|
|
|
|
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
|
|
|
|
#include <AP_NavEKF.h>
|
|
|
|
|
|
|
|
#define AP_AHRS_NAVEKF_AVAILABLE 1
|
2014-10-02 01:43:46 -03:00
|
|
|
#define AP_AHRS_NAVEKF_SETTLE_TIME_MS 20000 // time in milliseconds the ekf needs to settle after being started
|
2014-01-01 21:15:58 -04:00
|
|
|
|
|
|
|
class AP_AHRS_NavEKF : public AP_AHRS_DCM
|
|
|
|
{
|
|
|
|
public:
|
|
|
|
// Constructor
|
2014-03-30 08:00:25 -03:00
|
|
|
AP_AHRS_NavEKF(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps) :
|
2014-04-21 04:10:27 -03:00
|
|
|
AP_AHRS_DCM(ins, baro, gps),
|
2014-01-01 21:15:58 -04:00
|
|
|
EKF(this, baro),
|
2014-01-01 22:05:28 -04:00
|
|
|
ekf_started(false),
|
2015-03-28 14:52:22 -03:00
|
|
|
startup_delay_ms(10000),
|
|
|
|
start_time_ms(0)
|
2014-01-01 21:15:58 -04:00
|
|
|
{
|
|
|
|
}
|
|
|
|
|
|
|
|
// return the smoothed gyro vector corrected for drift
|
2014-07-13 08:56:39 -03:00
|
|
|
const Vector3f &get_gyro(void) const;
|
2014-01-01 21:15:58 -04:00
|
|
|
const Matrix3f &get_dcm_matrix(void) const;
|
|
|
|
|
|
|
|
// return the current drift correction integrator value
|
|
|
|
const Vector3f &get_gyro_drift(void) const;
|
|
|
|
|
2014-10-28 08:22:48 -03:00
|
|
|
// reset the current gyro drift estimate
|
|
|
|
// should be called if gyro offsets are recalculated
|
|
|
|
void reset_gyro_drift(void);
|
|
|
|
|
2014-01-01 21:15:58 -04:00
|
|
|
void update(void);
|
|
|
|
void reset(bool recover_eulers = false);
|
|
|
|
|
|
|
|
// reset the current attitude, used on new IMU calibration
|
|
|
|
void reset_attitude(const float &roll, const float &pitch, const float &yaw);
|
|
|
|
|
|
|
|
// dead-reckoning support
|
2014-10-17 22:14:34 -03:00
|
|
|
bool get_position(struct Location &loc) const;
|
2014-01-01 21:15:58 -04:00
|
|
|
|
|
|
|
// status reporting of estimated error
|
|
|
|
float get_error_rp(void);
|
|
|
|
float get_error_yaw(void);
|
|
|
|
|
|
|
|
// return a wind estimation vector, in m/s
|
|
|
|
Vector3f wind_estimate(void);
|
|
|
|
|
|
|
|
// return an airspeed estimate if available. return true
|
|
|
|
// if we have an estimate
|
2014-02-24 21:43:59 -04:00
|
|
|
bool airspeed_estimate(float *airspeed_ret) const;
|
2014-01-01 21:15:58 -04:00
|
|
|
|
|
|
|
// true if compass is being used
|
|
|
|
bool use_compass(void);
|
|
|
|
|
2014-01-20 01:47:35 -04:00
|
|
|
NavEKF &get_NavEKF(void) { return EKF; }
|
2015-01-30 02:37:52 -04:00
|
|
|
const NavEKF &get_NavEKF_const(void) const { return EKF; }
|
2014-01-01 21:15:58 -04:00
|
|
|
|
2014-01-01 22:47:40 -04:00
|
|
|
// return secondary attitude solution if available, as eulers in radians
|
|
|
|
bool get_secondary_attitude(Vector3f &eulers);
|
|
|
|
|
|
|
|
// return secondary position solution if available
|
|
|
|
bool get_secondary_position(struct Location &loc);
|
|
|
|
|
2014-01-01 23:25:41 -04:00
|
|
|
// EKF has a better ground speed vector estimate
|
|
|
|
Vector2f groundspeed_vector(void);
|
|
|
|
|
2014-10-31 19:07:18 -03:00
|
|
|
const Vector3f &get_accel_ef(uint8_t i) const;
|
|
|
|
const Vector3f &get_accel_ef() const { return get_accel_ef(_ins.get_primary_accel()); };
|
|
|
|
|
|
|
|
// blended accelerometer values in the earth frame in m/s/s
|
|
|
|
const Vector3f &get_accel_ef_blended(void) const;
|
|
|
|
|
2014-01-02 07:06:10 -04:00
|
|
|
// set home location
|
2014-03-30 08:00:25 -03:00
|
|
|
void set_home(const Location &loc);
|
2014-01-02 07:06:10 -04:00
|
|
|
|
2014-01-03 20:15:34 -04:00
|
|
|
bool have_inertial_nav(void) const;
|
|
|
|
|
2014-02-08 04:11:12 -04:00
|
|
|
bool get_velocity_NED(Vector3f &vec) const;
|
|
|
|
bool get_relative_position_NED(Vector3f &vec) const;
|
2014-01-03 20:15:34 -04:00
|
|
|
|
2014-08-06 21:31:24 -03:00
|
|
|
// write optical flow measurements to EKF
|
2014-09-10 06:28:02 -03:00
|
|
|
void writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, uint8_t &rangeHealth, float &rawSonarRange);
|
2014-08-06 21:31:24 -03:00
|
|
|
|
2014-11-12 14:49:15 -04:00
|
|
|
// inibit GPS useage
|
|
|
|
uint8_t setInhibitGPS(void);
|
|
|
|
|
2014-11-15 19:36:33 -04:00
|
|
|
// get speed limit
|
2014-11-15 21:42:30 -04:00
|
|
|
void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler);
|
2014-11-15 19:36:33 -04:00
|
|
|
|
2015-03-19 02:58:13 -03:00
|
|
|
void set_ekf_use(bool setting);
|
2014-01-03 21:39:20 -04:00
|
|
|
|
2014-05-15 04:09:18 -03:00
|
|
|
// is the AHRS subsystem healthy?
|
2015-01-31 22:31:24 -04:00
|
|
|
bool healthy(void) const;
|
2014-05-15 04:09:18 -03:00
|
|
|
|
2014-10-02 01:43:46 -03:00
|
|
|
// true if the AHRS has completed initialisation
|
|
|
|
bool initialised(void) const;
|
2014-09-29 05:37:14 -03:00
|
|
|
|
2015-03-16 18:29:11 -03:00
|
|
|
// get compass offset estimates
|
|
|
|
// true if offsets are valid
|
|
|
|
bool getMagOffsets(Vector3f &magOffsets);
|
|
|
|
|
2014-01-01 21:15:58 -04:00
|
|
|
private:
|
2014-02-14 18:24:12 -04:00
|
|
|
bool using_EKF(void) const;
|
2014-01-02 01:16:29 -04:00
|
|
|
|
2014-01-01 21:15:58 -04:00
|
|
|
NavEKF EKF;
|
2014-01-01 22:05:28 -04:00
|
|
|
bool ekf_started;
|
|
|
|
Matrix3f _dcm_matrix;
|
2014-01-01 22:47:40 -04:00
|
|
|
Vector3f _dcm_attitude;
|
2014-07-13 08:56:39 -03:00
|
|
|
Vector3f _gyro_bias;
|
|
|
|
Vector3f _gyro_estimate;
|
2014-10-31 19:07:18 -03:00
|
|
|
Vector3f _accel_ef_ekf[INS_MAX_INSTANCES];
|
|
|
|
Vector3f _accel_ef_ekf_blended;
|
2014-01-01 22:05:28 -04:00
|
|
|
const uint16_t startup_delay_ms;
|
|
|
|
uint32_t start_time_ms;
|
2014-01-01 21:15:58 -04:00
|
|
|
};
|
|
|
|
#endif
|
|
|
|
|
|
|
|
#endif // __AP_AHRS_NAVEKF_H__
|