mirror of https://github.com/ArduPilot/ardupilot
278 lines
9.6 KiB
C++
278 lines
9.6 KiB
C++
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
#pragma once
|
|
|
|
/*
|
|
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
|
|
*
|
|
*/
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
#include "AP_AHRS.h"
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
|
#include <SITL/SITL.h>
|
|
#endif
|
|
|
|
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
|
|
#include <AP_NavEKF/AP_NavEKF.h>
|
|
#include <AP_NavEKF2/AP_NavEKF2.h>
|
|
#include <AP_NavEKF/AP_Nav_Common.h> // definitions shared by inertial and ekf nav filters
|
|
|
|
#define AP_AHRS_NAVEKF_AVAILABLE 1
|
|
#define AP_AHRS_NAVEKF_SETTLE_TIME_MS 20000 // time in milliseconds the ekf needs to settle after being started
|
|
|
|
/*
|
|
we are too close to running out of flash on px4, so disable
|
|
it. Leave it enabled on V4 for now as that has sufficient flash
|
|
space
|
|
*/
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 && (defined(CONFIG_ARCH_BOARD_PX4FMU_V1) || defined(CONFIG_ARCH_BOARD_PX4FMU_V2))
|
|
#define AP_AHRS_WITH_EKF1 0
|
|
#else
|
|
#define AP_AHRS_WITH_EKF1 1
|
|
#endif
|
|
|
|
class AP_AHRS_NavEKF : public AP_AHRS_DCM
|
|
{
|
|
public:
|
|
enum Flags {
|
|
FLAG_NONE = 0,
|
|
FLAG_ALWAYS_USE_EKF = 0x1,
|
|
};
|
|
|
|
// Constructor
|
|
AP_AHRS_NavEKF(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps, RangeFinder &rng,
|
|
NavEKF &_EKF1, NavEKF2 &_EKF2, Flags flags = FLAG_NONE);
|
|
|
|
// return the smoothed gyro vector corrected for drift
|
|
const Vector3f &get_gyro(void) const;
|
|
const Matrix3f &get_rotation_body_to_ned(void) const;
|
|
|
|
// return the current drift correction integrator value
|
|
const Vector3f &get_gyro_drift(void) const;
|
|
|
|
// reset the current gyro drift estimate
|
|
// should be called if gyro offsets are recalculated
|
|
void reset_gyro_drift(void);
|
|
|
|
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
|
|
bool get_position(struct Location &loc) const;
|
|
|
|
// get latest altitude estimate above ground level in metres and validity flag
|
|
bool get_hagl(float &hagl) const;
|
|
|
|
// status reporting of estimated error
|
|
float get_error_rp(void) const;
|
|
float get_error_yaw(void) const;
|
|
|
|
// 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
|
|
bool airspeed_estimate(float *airspeed_ret) const;
|
|
|
|
// true if compass is being used
|
|
bool use_compass(void);
|
|
|
|
// we will need to remove these to fully hide which EKF we are using
|
|
NavEKF &get_NavEKF(void) {
|
|
return EKF1;
|
|
}
|
|
const NavEKF &get_NavEKF_const(void) const {
|
|
return EKF1;
|
|
}
|
|
|
|
NavEKF2 &get_NavEKF2(void) {
|
|
return EKF2;
|
|
}
|
|
const NavEKF2 &get_NavEKF2_const(void) const {
|
|
return EKF2;
|
|
}
|
|
|
|
// 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);
|
|
|
|
// EKF has a better ground speed vector estimate
|
|
Vector2f groundspeed_vector(void);
|
|
|
|
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;
|
|
|
|
// set home location
|
|
void set_home(const Location &loc);
|
|
|
|
// returns the inertial navigation origin in lat/lon/alt
|
|
bool get_origin(Location &ret) const;
|
|
|
|
bool have_inertial_nav(void) const;
|
|
|
|
bool get_velocity_NED(Vector3f &vec) const;
|
|
bool get_relative_position_NED(Vector3f &vec) const;
|
|
|
|
// return the relative position in North/East order
|
|
// return true if the estimate is valid
|
|
bool get_relative_position_NE(Vector2f &posNE) const;
|
|
|
|
// return the relative position in North/East order
|
|
// return true if the estimate is valid
|
|
bool get_relative_position_D(float &posD) const;
|
|
|
|
// Get a derivative of the vertical position in m/s which is kinematically consistent with the vertical position is required by some control loops.
|
|
// This is different to the vertical velocity from the EKF which is not always consistent with the verical position due to the various errors that are being corrected for.
|
|
bool get_vert_pos_rate(float &velocity);
|
|
|
|
// write optical flow measurements to EKF
|
|
void writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas);
|
|
|
|
// inibit GPS usage
|
|
uint8_t setInhibitGPS(void);
|
|
|
|
// get speed limit
|
|
void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler);
|
|
|
|
void set_ekf_use(bool setting);
|
|
|
|
// is the AHRS subsystem healthy?
|
|
bool healthy(void) const;
|
|
|
|
// true if the AHRS has completed initialisation
|
|
bool initialised(void) const;
|
|
|
|
// get_filter_status - returns filter status as a series of flags
|
|
bool get_filter_status(nav_filter_status &status) const;
|
|
|
|
// get compass offset estimates
|
|
// true if offsets are valid
|
|
bool getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets);
|
|
|
|
// report any reason for why the backend is refusing to initialise
|
|
const char *prearm_failure_reason(void) const override;
|
|
|
|
// return the amount of yaw angle change due to the last yaw angle reset in radians
|
|
// returns the time of the last yaw angle reset or 0 if no reset has ever occurred
|
|
uint32_t getLastYawResetAngle(float &yawAng) const;
|
|
|
|
// 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
|
|
uint32_t getLastPosNorthEastReset(Vector2f &pos) const;
|
|
|
|
// 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
|
|
uint32_t getLastVelNorthEastReset(Vector2f &vel) const;
|
|
|
|
// 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
|
|
bool resetHeightDatum(void);
|
|
|
|
// send a EKF_STATUS_REPORT for current EKF
|
|
void send_ekf_status_report(mavlink_channel_t chan);
|
|
|
|
// get_hgt_ctrl_limit - get maximum height to be observed by the control loops in metres and a validity flag
|
|
// this is used to limit height during optical flow navigation
|
|
// it will return invalid when no limiting is required
|
|
bool get_hgt_ctrl_limit(float &limit) const;
|
|
|
|
// get_llh - updates the provided location with the latest calculated location including absolute altitude
|
|
// returns true on success (i.e. the EKF knows it's latest position), false on failure
|
|
bool get_location(struct Location &loc) const;
|
|
|
|
// 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
|
|
bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const;
|
|
|
|
// returns the expected NED magnetic field
|
|
bool get_mag_field_NED(Vector3f& ret) const;
|
|
|
|
// returns the estimated magnetic field offsets in body frame
|
|
bool get_mag_field_correction(Vector3f &ret) const;
|
|
|
|
void setTakeoffExpected(bool val);
|
|
void setTouchdownExpected(bool val);
|
|
|
|
bool getGpsGlitchStatus();
|
|
|
|
// used by Replay to force start at right timestamp
|
|
void force_ekf_start(void) { force_ekf = true; }
|
|
|
|
// is the EKF backend doing its own sensor logging?
|
|
bool have_ekf_logging(void) const override;
|
|
|
|
private:
|
|
enum EKF_TYPE {EKF_TYPE_NONE=0,
|
|
#if AP_AHRS_WITH_EKF1
|
|
EKF_TYPE1=1,
|
|
#endif
|
|
EKF_TYPE2=2
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
|
,EKF_TYPE_SITL=10
|
|
#endif
|
|
};
|
|
EKF_TYPE active_EKF_type(void) const;
|
|
|
|
bool always_use_EKF() const {
|
|
return _ekf_flags & FLAG_ALWAYS_USE_EKF;
|
|
}
|
|
|
|
NavEKF &EKF1;
|
|
NavEKF2 &EKF2;
|
|
bool ekf1_started:1;
|
|
bool ekf2_started:1;
|
|
bool force_ekf:1;
|
|
Matrix3f _dcm_matrix;
|
|
Vector3f _dcm_attitude;
|
|
Vector3f _gyro_bias;
|
|
Vector3f _gyro_estimate;
|
|
Vector3f _accel_ef_ekf[INS_MAX_INSTANCES];
|
|
Vector3f _accel_ef_ekf_blended;
|
|
const uint16_t startup_delay_ms = 1000;
|
|
uint32_t start_time_ms = 0;
|
|
Flags _ekf_flags;
|
|
|
|
uint8_t ekf_type(void) const;
|
|
void update_DCM(void);
|
|
void update_EKF1(void);
|
|
void update_EKF2(void);
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
|
SITL::SITL *_sitl;
|
|
void update_SITL(void);
|
|
#endif
|
|
};
|
|
#endif
|