diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 5ed487052f..67b4e95356 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -2663,12 +2663,12 @@ bool AP_AHRS::resetHeightDatum(void) } // send a EKF_STATUS_REPORT for current EKF -void AP_AHRS::send_ekf_status_report(mavlink_channel_t chan) const +void AP_AHRS::send_ekf_status_report(GCS_MAVLINK &link) const { switch (ekf_type()) { case EKFType::NONE: // send zero status report - dcm.send_ekf_status_report(chan); + dcm.send_ekf_status_report(link); break; #if AP_AHRS_SIM_ENABLED @@ -2686,26 +2686,26 @@ void AP_AHRS::send_ekf_status_report(mavlink_channel_t chan) const //EKF_CONST_POS_MODE | /* EKF is in constant position mode and does not know it's absolute or relative position. | */ EKF_PRED_POS_HORIZ_REL | /* Set if EKF's predicted horizontal position (relative) estimate is good. | */ EKF_PRED_POS_HORIZ_ABS; /* Set if EKF's predicted horizontal position (absolute) estimate is good. | */ - mavlink_msg_ekf_status_report_send(chan, flags, 0, 0, 0, 0, 0, 0); + mavlink_msg_ekf_status_report_send(link.get_chan(), flags, 0, 0, 0, 0, 0, 0); } break; #endif #if HAL_EXTERNAL_AHRS_ENABLED case EKFType::EXTERNAL: { - AP::externalAHRS().send_status_report(chan); + AP::externalAHRS().send_status_report(link); break; } #endif #if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: - return EKF2.send_status_report(chan); + return EKF2.send_status_report(link); #endif #if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: - return EKF3.send_status_report(chan); + return EKF3.send_status_report(link); #endif } diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 91d37556f6..6b965f1c14 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -301,7 +301,7 @@ public: bool resetHeightDatum(); // send a EKF_STATUS_REPORT for current EKF - void send_ekf_status_report(mavlink_channel_t chan) const; + void send_ekf_status_report(class GCS_MAVLINK &link) const; // get_hgt_ctrl_limit - get maximum height to be observed by the control loops in meters and a validity flag // this is used to limit height during optical flow navigation diff --git a/libraries/AP_AHRS/AP_AHRS_Backend.h b/libraries/AP_AHRS/AP_AHRS_Backend.h index cc59c55377..66f261d7f5 100644 --- a/libraries/AP_AHRS/AP_AHRS_Backend.h +++ b/libraries/AP_AHRS/AP_AHRS_Backend.h @@ -310,7 +310,7 @@ public: return false; } - virtual void send_ekf_status_report(mavlink_channel_t chan) const = 0; + virtual void send_ekf_status_report(class GCS_MAVLINK &link) const = 0; // Retrieves the corrected NED delta velocity in use by the inertial navigation virtual void getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const { diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 1c006c0795..82fbf6985b 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -1278,6 +1278,6 @@ bool AP_AHRS_DCM::get_relative_position_D_origin(float &posD) const return true; } -void AP_AHRS_DCM::send_ekf_status_report(mavlink_channel_t chan) const +void AP_AHRS_DCM::send_ekf_status_report(GCS_MAVLINK &link) const { } diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index d7776298cb..db64ea152d 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -123,7 +123,7 @@ public: bool get_relative_position_NE_origin(Vector2f &posNE) const override; bool get_relative_position_D_origin(float &posD) const override; - void send_ekf_status_report(mavlink_channel_t chan) const override; + void send_ekf_status_report(class GCS_MAVLINK &link) const override; private: