// get the index of the current primary accelerometer sensor
uint8_t AP_AHRS_NavEKF::get_primary_accel_index(void) const
{
if (ekf_type() != 0) {
return get_primary_IMU_index();
}
return _ins.get_primary_accel();
}
// 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 AP_AHRS_NavEKF::resetHeightDatum(void)
{
// support locked access functions to AHRS data
WITH_SEMAPHORE(_rsem);
switch (ekf_type()) {
case 2:
default: {
EKF3.resetHeightDatum();
return EKF2.resetHeightDatum();
}
case 3: {
EKF2.resetHeightDatum();
return EKF3.resetHeightDatum();
}
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL:
return false;
#endif
}
return false;
}
// get the index of the current primary gyro sensor
uint8_t AP_AHRS_NavEKF::get_primary_gyro_index(void) const
{
if (ekf_type() != 0) {
return get_primary_IMU_index();
}
return AP::ins().get_primary_gyro();
}
// passes a reference to the location of the inertial navigation origin
// in WGS-84 coordinates
// returns a boolean true when the inertial navigation origin has been set
bool AP_AHRS_NavEKF::get_origin(Location &ret) const
{
switch (ekf_type()) {
case EKF_TYPE_NONE:
return false;
#if AP_AHRS_WITH_EKF1
case EKF_TYPE1:
if (!EKF1.getOriginLLH(ret)) {
return false;
}
return true;
#endif
case EKF_TYPE2:
default:
if (!EKF2.getOriginLLH(ret)) {
return false;
}
return true;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL:
ret = get_home();
return ret.lat != 0 || ret.lng != 0;
#endif
}
}
// get_filter_status : returns filter status as a series of flags
bool AP_AHRS_NavEKF::get_filter_status(nav_filter_status &status) const
{
switch (ekf_type()) {
case EKF_TYPE_NONE:
return false;
#if AP_AHRS_WITH_EKF1
case EKF_TYPE1:
EKF1.getFilterStatus(status);
return true;
#endif
case EKF_TYPE2:
default:
EKF2.getFilterStatus(-1,status);
return true;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL:
memset(&status, 0, sizeof(status));
status.flags.attitude = 1;
status.flags.horiz_vel = 1;
status.flags.vert_vel = 1;
status.flags.horiz_pos_rel = 1;
status.flags.horiz_pos_abs = 1;
status.flags.vert_pos = 1;
status.flags.pred_horiz_pos_rel = 1;
status.flags.pred_horiz_pos_abs = 1;
status.flags.using_gps = 1;
return true;
#endif
}
}
// get the index of the current primary gyro sensor
uint8_t AP_AHRS_NavEKF::get_primary_gyro_index(void) const
{
if (ekf_type() == 2) {
return get_primary_IMU_index();
}
return _ins.get_primary_gyro();
}
// passes a reference to the location of the inertial navigation origin
// in WGS-84 coordinates
// returns a boolean true when the inertial navigation origin has been set
bool AP_AHRS_NavEKF::get_origin(Location &ret) const
{
switch (ekf_type()) {
case EKF_TYPE_NONE:
return false;
case EKF_TYPE2:
default:
if (!EKF2.getOriginLLH(-1,ret)) {
return false;
}
return true;
case EKF_TYPE3:
if (!EKF3.getOriginLLH(-1,ret)) {
return false;
}
return true;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL:
if (!_sitl) {
return false;
}
const struct SITL::sitl_fdm &fdm = _sitl->state;
ret = fdm.home;
return true;
#endif
}
}
// 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 AP_AHRS_NavEKF::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const
{
switch (ekf_type()) {
case EKF_TYPE_NONE:
// We are not using an EKF so no data
return false;
#if AP_AHRS_WITH_EKF1
case EKF_TYPE1:
// use EKF to get variance
EKF1.getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
return true;
#endif
case EKF_TYPE2:
default:
// use EKF to get variance
EKF2.getVariances(-1,velVar, posVar, hgtVar, magVar, tasVar, offset);
return true;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL:
velVar = 0;
posVar = 0;
hgtVar = 0;
magVar.zero();
tasVar = 0;
offset.zero();
return true;
#endif
}
}
// 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 AP_AHRS_NavEKF::getLastYawResetAngle(float &yawAng)
{
switch (ekf_type()) {
case 1:
return EKF1.getLastYawResetAngle(yawAng);
case 2:
return EKF2.getLastYawResetAngle(yawAng);
}
return false;
}
// is the EKF backend doing its own sensor logging?
bool AP_AHRS_NavEKF::have_ekf_logging(void) const
{
switch (ekf_type()) {
case 2:
return EKF2.have_ekf_logging();
default:
break;
}
return false;
}
// 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 AP_AHRS_NavEKF::getLastVelNorthEastReset(Vector2f &vel) const
{
switch (ekf_type()) {
case 1:
return EKF1.getLastVelNorthEastReset(vel);
case 2:
return EKF2.getLastVelNorthEastReset(vel);
}
return 0;
}
// report any reason for why the backend is refusing to initialise
const char *AP_AHRS_NavEKF::prearm_failure_reason(void) const
{
switch (ekf_type()) {
case 0:
return nullptr;
case 1:
return EKF1.prearm_failure_reason();
case 2:
return EKF2.prearm_failure_reason();
}
return nullptr;
}
// 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 AP_AHRS_NavEKF::resetHeightDatum(void)
{
switch (ekf_type()) {
case 1:
EKF2.resetHeightDatum();
return EKF1.resetHeightDatum();
case 2:
EKF1.resetHeightDatum();
return EKF2.resetHeightDatum();
}
return false;
}
// get compass offset estimates
// true if offsets are valid
bool AP_AHRS_NavEKF::getMagOffsets(Vector3f &magOffsets)
{
switch (ekf_type()) {
case 0:
case 1:
default:
return EKF1.getMagOffsets(magOffsets);
case 2:
return EKF2.getMagOffsets(magOffsets);
}
}
// inhibit GPS useage
uint8_t AP_AHRS_NavEKF::setInhibitGPS(void)
{
switch (ekf_type()) {
case 0:
case 1:
default:
return EKF1.setInhibitGPS();
case 2:
return EKF2.setInhibitGPS();
}
}
// 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 AP_AHRS_NavEKF::getLastVelNorthEastReset(Vector2f &vel) const
{
switch (ekf_type()) {
case 1:
return EKF1.getLastVelNorthEastReset(vel);
case 2:
return EKF2.getLastVelNorthEastReset(vel);
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL:
return 0;
#endif
}
return 0;
}
// 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 AP_AHRS_NavEKF::getLastYawResetAngle(float &yawAng) const
{
switch (ekf_type()) {
case 1:
return EKF1.getLastYawResetAngle(yawAng);
case 2:
return EKF2.getLastYawResetAngle(yawAng);
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL:
return 0;
#endif
}
return 0;
}
// get speed limit
void AP_AHRS_NavEKF::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler)
{
switch (ekf_type()) {
case 0:
case 1:
default:
EKF1.getEkfControlLimits(ekfGndSpdLimit,ekfNavVelGainScaler);
break;
case 2:
EKF2.getEkfControlLimits(ekfGndSpdLimit,ekfNavVelGainScaler);
break;
}
}
/*
check if the AHRS subsystem is healthy
*/
bool AP_AHRS_NavEKF::healthy(void) const
{
// If EKF is started we switch away if it reports unhealthy. This could be due to bad
// sensor data. If EKF reversion is inhibited, we only switch across if the EKF encounters
// an internal processing error, but not for bad sensor data.
switch (ekf_type()) {
case 0:
return AP_AHRS_DCM::healthy();
#if AP_AHRS_WITH_EKF1
case 1: {
bool ret = ekf1_started && EKF1.healthy();
if (!ret) {
return false;
}
if ((_vehicle_class == AHRS_VEHICLE_FIXED_WING ||
_vehicle_class == AHRS_VEHICLE_GROUND) &&
active_EKF_type() != EKF_TYPE1) {
// on fixed wing we want to be using EKF to be considered
// healthy if EKF is enabled
return false;
}
return true;
}
#endif
case 2: {
bool ret = ekf2_started && EKF2.healthy();
if (!ret) {
return false;
}
if ((_vehicle_class == AHRS_VEHICLE_FIXED_WING ||
_vehicle_class == AHRS_VEHICLE_GROUND) &&
active_EKF_type() != EKF_TYPE2) {
// on fixed wing we want to be using EKF to be considered
// healthy if EKF is enabled
return false;
}
return true;
}
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL:
return true;
#endif
}
return AP_AHRS_DCM::healthy();
}
// get_location - updates the provided location with the latest calculated location
// returns true on success (i.e. the EKF knows it's latest position), false on failure
bool AP_AHRS_NavEKF::get_location(struct Location &loc) const
{
switch (ekf_type()) {
case EKF_TYPE_NONE:
// We are not using an EKF so no data
return false;
case EKF_TYPE1:
default:
return EKF1.getLLH(loc);
case EKF_TYPE2:
return EKF2.getLLH(loc);
}
}
void AP_AHRS_NavEKF::setTouchdownExpected(bool val)
{
switch (ekf_type()) {
case EKF_TYPE1:
EKF1.setTouchdownExpected(val);
break;
case EKF_TYPE2:
EKF2.setTouchdownExpected(val);
break;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL:
break;
#endif
}
}
// 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
uint32_t AP_AHRS_NavEKF::getLastPosDownReset(float &posDelta) const
{
switch (ekf_type()) {
case EKF_TYPE2:
return EKF2.getLastPosDownReset(posDelta);
case EKF_TYPE3:
return EKF3.getLastPosDownReset(posDelta);
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL:
return 0;
#endif
}
return 0;
}
// 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 AP_AHRS_NavEKF::resetHeightDatum(void)
{
switch (ekf_type()) {
case 1:
EKF2.resetHeightDatum();
return EKF1.resetHeightDatum();
case 2:
EKF1.resetHeightDatum();
return EKF2.resetHeightDatum();
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL:
return false;
#endif
}
return false;
}
// get earth-frame accel vector for primary IMU
uint8_t AP_AHRS_NavEKF::get_primary_IMU_index() const
{
int8_t imu = -1;
switch (ekf_type()) {
case 2:
// let EKF2 choose primary IMU
imu = EKF2.getPrimaryCoreIMUIndex();
break;
default:
break;
}
if (imu == -1) {
imu = _ins.get_primary_accel();
}
return imu;
}
// true if the AHRS has completed initialisation
bool AP_AHRS_NavEKF::initialised(void) const
{
switch (ekf_type()) {
case 0:
return true;
case 1:
default:
// initialisation complete 10sec after ekf has started
return (ekf1_started && (hal.scheduler->millis() - start_time_ms > AP_AHRS_NAVEKF_SETTLE_TIME_MS));
case 2:
// initialisation complete 10sec after ekf has started
return (ekf2_started && (hal.scheduler->millis() - start_time_ms > AP_AHRS_NAVEKF_SETTLE_TIME_MS));
}
};
// Retrieves the NED delta velocity corrected
void AP_AHRS_NavEKF::getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const
{
if (ekf_type() == 2) {
uint8_t imu_idx = EKF2.getPrimaryCoreIMUIndex();
float accel_z_bias;
EKF2.getAccelZBias(-1,accel_z_bias);
ret.zero();
_ins.get_delta_velocity(imu_idx, ret);
dt = _ins.get_delta_velocity_dt(imu_idx);
ret.z -= accel_z_bias*dt;
ret = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * ret;
ret.z += GRAVITY_MSS*dt;
} else {
AP_AHRS::getCorrectedDeltaVelocityNED(ret, dt);
}
}
// inhibit GPS useage
uint8_t AP_AHRS_NavEKF::setInhibitGPS(void)
{
switch (ekf_type()) {
case 0:
case 1:
return EKF1.setInhibitGPS();
case 2:
default:
return EKF2.setInhibitGPS();
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL:
return false;
#endif
}
}
// 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 AP_AHRS_NavEKF::get_hgt_ctrl_limit(float& limit) const
{
switch (ekf_type()) {
case EKF_TYPE_NONE:
// We are not using an EKF so no limiting applies
return false;
case EKF_TYPE1:
default:
return EKF1.getHeightControlLimit(limit);
return true;
case EKF_TYPE2:
return EKF2.getHeightControlLimit(limit);
return true;
}
}
// get_filter_status : returns filter status as a series of flags
bool AP_AHRS_NavEKF::get_filter_status(nav_filter_status &status) const
{
switch (ekf_type()) {
case EKF_TYPE_NONE:
return false;
case EKF_TYPE1:
default:
EKF1.getFilterStatus(status);
return true;
case EKF_TYPE2:
EKF2.getFilterStatus(status);
return true;
}
}
void AP_AHRS_NavEKF::setTakeoffExpected(bool val)
{
switch (ekf_type()) {
#if AP_AHRS_WITH_EKF1
case EKF_TYPE1:
EKF1.setTakeoffExpected(val);
break;
#endif
case EKF_TYPE2:
EKF2.setTakeoffExpected(val);
break;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL:
break;
#endif
}
}
