示例#1
0
// 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();
}
示例#2
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)
{
    // 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;
}
示例#3
0
// 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();
}
示例#4
0
// 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
    }
}
示例#5
0
// 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
    }

}
示例#6
0
// 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();
}
示例#7
0
// 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
    }
}
示例#8
0
// 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
    }
}
示例#9
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)
{
    switch (ekf_type()) {
    case 1:
        return EKF1.getLastYawResetAngle(yawAng);
    case 2:
        return EKF2.getLastYawResetAngle(yawAng);
    }
    return false;
}
示例#10
0
// 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;
}
示例#11
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
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;
}
示例#12
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;
}
示例#13
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();
    }
    return false;    
}
示例#14
0
// 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);
    }
}
示例#15
0
// 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();
    }
}
示例#16
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
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;
}
示例#17
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;
}
示例#18
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;
    }
}
示例#19
0
/*
  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();
}
示例#20
0
// 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);
    }
}
示例#21
0
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
    }
}
示例#22
0
// 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;
}
示例#23
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;
}
示例#24
0
// 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;
}
示例#25
0
// 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));
    }
};
示例#26
0
// 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);
    }
}
示例#27
0
// 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
    }
}
示例#28
0
// 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;
    }
}
示例#29
0
// 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;
    }

}
示例#30
0
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
    }
}