From f2019e8a26efabab390bd8b8643bff5fd5b67c8b Mon Sep 17 00:00:00 2001 From: Oleksiy Protas Date: Wed, 24 Sep 2025 13:07:51 +0300 Subject: [PATCH 1/2] AP_NavEKF: report active EKF source sets and lanes 1-based numbering to be consistent with MAV_CMD_SET_EKF_SOURCE_SET and parameters. Non-EKF AHRS report dummy values since they also emit EKF_STATUS_REPORT. --- libraries/AP_AHRS/AP_AHRS_SIM.cpp | 2 +- libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp | 2 +- libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp | 2 +- libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp | 4 +++- modules/mavlink | 2 +- 5 files changed, 7 insertions(+), 5 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_SIM.cpp b/libraries/AP_AHRS/AP_AHRS_SIM.cpp index 734ff7364bef7..e8be782388dde 100644 --- a/libraries/AP_AHRS/AP_AHRS_SIM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_SIM.cpp @@ -187,7 +187,7 @@ void AP_AHRS_SIM::send_ekf_status_report(GCS_MAVLINK &link) 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(link.get_chan(), flags, 0, 0, 0, 0, 0, 0); + mavlink_msg_ekf_status_report_send(link.get_chan(), flags, 0, 0, 0, 0, 0, 0, 1, 1); #endif // HAL_GCS_ENABLED } diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp index d68727f59bbc7..62312d8c95ffb 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp @@ -389,7 +389,7 @@ void AP_ExternalAHRS::send_status_report(GCS_MAVLINK &link) const velVar, posVar, hgtVar, - mag_var, 0, 0); + mag_var, 0, 0, 1, 1); } void AP_ExternalAHRS::update(void) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index df4e87cb4af2e..7bc7c72850111 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -556,7 +556,7 @@ void NavEKF2_core::send_status_report(GCS_MAVLINK &link) const } // send message - mavlink_msg_ekf_status_report_send(link.get_chan(), flags, velVar, posVar, hgtVar, mag_max, temp, tasVar); + mavlink_msg_ekf_status_report_send(link.get_chan(), flags, velVar, posVar, hgtVar, mag_max, temp, tasVar, 1, core_index + 1); } #endif // HAL_GCS_ENABLED diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index 3aff6f41a6a16..d6d5b6abad3fd 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -651,7 +651,9 @@ void NavEKF3_core::send_status_report(GCS_MAVLINK &link) const fmaxf(fmaxf(magVar.x,magVar.y),magVar.z), temp, flags, - tasVar + tasVar, + uint8_t(frontend->sources.getActiveSourceSet(core_index) + 1), + uint8_t(core_index + 1) }; // send message diff --git a/modules/mavlink b/modules/mavlink index 430715591a7a1..e025ecb265663 160000 --- a/modules/mavlink +++ b/modules/mavlink @@ -1 +1 @@ -Subproject commit 430715591a7a14a996f56d4c724f2e0335d74546 +Subproject commit e025ecb2656631d6ae511a112264a495b95c0c21 From cc238b199712f7611859209e852022bd76cd9111 Mon Sep 17 00:00:00 2001 From: Oleksiy Protas Date: Wed, 1 Oct 2025 01:43:05 +0300 Subject: [PATCH 2/2] AP_NavEKF: Include estimator type in reporting Reordered fields in the mavlink message allows not to pass inapplicable data, e.g. an external AHRS by definition has neither lanes nor source sets. --- libraries/AP_AHRS/AP_AHRS_SIM.cpp | 2 +- libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp | 2 +- libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp | 2 +- libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp | 3 ++- modules/mavlink | 2 +- 5 files changed, 6 insertions(+), 5 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_SIM.cpp b/libraries/AP_AHRS/AP_AHRS_SIM.cpp index e8be782388dde..13b7ecc582e2a 100644 --- a/libraries/AP_AHRS/AP_AHRS_SIM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_SIM.cpp @@ -187,7 +187,7 @@ void AP_AHRS_SIM::send_ekf_status_report(GCS_MAVLINK &link) 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(link.get_chan(), flags, 0, 0, 0, 0, 0, 0, 1, 1); + mavlink_msg_ekf_status_report_send(link.get_chan(), flags, 0, 0, 0, 0, 0, 0, ESTIMATOR_TYPE_SIM, 1, 0); #endif // HAL_GCS_ENABLED } diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp index 62312d8c95ffb..b44c36ec98443 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp @@ -389,7 +389,7 @@ void AP_ExternalAHRS::send_status_report(GCS_MAVLINK &link) const velVar, posVar, hgtVar, - mag_var, 0, 0, 1, 1); + mag_var, 0, 0, ESTIMATOR_TYPE_EXTERNAL, 0, 0); } void AP_ExternalAHRS::update(void) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index 7bc7c72850111..cd5b0c0845cd9 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -556,7 +556,7 @@ void NavEKF2_core::send_status_report(GCS_MAVLINK &link) const } // send message - mavlink_msg_ekf_status_report_send(link.get_chan(), flags, velVar, posVar, hgtVar, mag_max, temp, tasVar, 1, core_index + 1); + mavlink_msg_ekf_status_report_send(link.get_chan(), flags, velVar, posVar, hgtVar, mag_max, temp, tasVar, ESTIMATOR_TYPE_EKF2, core_index + 1, 0); } #endif // HAL_GCS_ENABLED diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index d6d5b6abad3fd..9730407ffbf13 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -652,8 +652,9 @@ void NavEKF3_core::send_status_report(GCS_MAVLINK &link) const temp, flags, tasVar, + ESTIMATOR_TYPE_EKF3, + uint8_t(core_index + 1), uint8_t(frontend->sources.getActiveSourceSet(core_index) + 1), - uint8_t(core_index + 1) }; // send message diff --git a/modules/mavlink b/modules/mavlink index e025ecb265663..8d32670a277fa 160000 --- a/modules/mavlink +++ b/modules/mavlink @@ -1 +1 @@ -Subproject commit e025ecb2656631d6ae511a112264a495b95c0c21 +Subproject commit 8d32670a277fad8645e31de2998c89695ec72848