C++ (Cpp) mavlink_msg_ekf_status_report_send Examples

Example #1

File: AP_NavEKF3_Outputs.cpp Project: gmorph/ardupilot

// send an EKF_STATUS message to GCS
void NavEKF3_core::send_status_report(mavlink_channel_t chan)
{
    // prepare flags
    uint16_t flags = 0;
    if (filterStatus.flags.attitude) {
        flags |= EKF_ATTITUDE;
    }
    if (filterStatus.flags.horiz_vel) {
        flags |= EKF_VELOCITY_HORIZ;
    }
    if (filterStatus.flags.vert_vel) {
        flags |= EKF_VELOCITY_VERT;
    }
    if (filterStatus.flags.horiz_pos_rel) {
        flags |= EKF_POS_HORIZ_REL;
    }
    if (filterStatus.flags.horiz_pos_abs) {
        flags |= EKF_POS_HORIZ_ABS;
    }
    if (filterStatus.flags.vert_pos) {
        flags |= EKF_POS_VERT_ABS;
    }
    if (filterStatus.flags.terrain_alt) {
        flags |= EKF_POS_VERT_AGL;
    }
    if (filterStatus.flags.const_pos_mode) {
        flags |= EKF_CONST_POS_MODE;
    }
    if (filterStatus.flags.pred_horiz_pos_rel) {
        flags |= EKF_PRED_POS_HORIZ_REL;
    }
    if (filterStatus.flags.pred_horiz_pos_abs) {
        flags |= EKF_PRED_POS_HORIZ_ABS;
    }
    if (filterStatus.flags.gps_glitching) {
        flags |= (1<<15);
    }

    // get variances
    float velVar, posVar, hgtVar, tasVar;
    Vector3f magVar;
    Vector2f offset;
    getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);

    // Only report range finder normalised innovation levels if the EKF needs the data for primary
    // height estimation or optical flow operation. This prevents false alarms at the GCS if a
    // range finder is fitted for other applications
    float temp;
    if ((frontend->_useRngSwHgt > 0) || PV_AidingMode == AID_RELATIVE || flowDataValid) {
        temp = sqrtf(auxRngTestRatio);
    } else {
        temp = 0.0f;
    }

    // send message
    mavlink_msg_ekf_status_report_send(chan, flags, velVar, posVar, hgtVar, magVar.length(), temp);

}

Example #2

File: AP_AHRS_NavEKF.cpp Project: langfan1990/ardupilot

// send a EKF_STATUS_REPORT for current EKF
void AP_AHRS_NavEKF::send_ekf_status_report(mavlink_channel_t chan) const
{
    switch (ekf_type()) {
    case EKF_TYPE_NONE:
        // send zero status report
        mavlink_msg_ekf_status_report_send(chan, 0, 0, 0, 0, 0, 0, 0);
        break;

#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    case EKF_TYPE_SITL:
        // send zero status report
        mavlink_msg_ekf_status_report_send(chan, 0, 0, 0, 0, 0, 0, 0);
        break;
#endif
        
    case EKF_TYPE2:
        return EKF2.send_status_report(chan);

    case EKF_TYPE3:
        return EKF3.send_status_report(chan);

    }
}

Example #3

File: AP_AHRS_NavEKF.cpp Project: RQRobert/ardupilot

// send a EKF_STATUS_REPORT for current EKF
void AP_AHRS_NavEKF::send_ekf_status_report(mavlink_channel_t chan)
{
    switch (active_EKF_type()) {
#if AP_AHRS_WITH_EKF1
    case EKF_TYPE1:
        return EKF1.send_status_report(chan);
#endif

#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    case EKF_TYPE_SITL:
        // send zero status report
        mavlink_msg_ekf_status_report_send(chan, 0, 0, 0, 0, 0, 0);
        break;
#endif
        
    case EKF_TYPE2:
    default:
        return EKF2.send_status_report(chan);
    }    
}