diff --git a/msg/estimator_status.msg b/msg/estimator_status.msg index 9dea01893d..85c0d76e71 100644 --- a/msg/estimator_status.msg +++ b/msg/estimator_status.msg @@ -45,3 +45,5 @@ uint16 filter_fault_flags # Bitmask to indicate EKF internal faults # 12 - true if fusion of the North position has encountered a numerical error # 13 - true if fusion of the East position has encountered a numerical error # 14 - true if fusion of the Down position has encountered a numerical error +float32 pos_horiz_accuracy +float32 pos_vert_accuracy \ No newline at end of file diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 9eb2d3a5f5..cdbf7e99f8 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -726,6 +726,9 @@ void BlockLocalPositionEstimator::publishEstimatorStatus() + (_sonarInitialized << SENSOR_SONAR) + (_visionInitialized << SENSOR_VISION) + (_mocapInitialized << SENSOR_MOCAP); + _pub_est_status.get().pos_horiz_accuracy = _pub_gpos.get().eph; + _pub_est_status.get().pos_vert_accuracy = _pub_gpos.get().epv; + _pub_est_status.update(); } diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 917abf2594..56a60ba3e4 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1702,9 +1702,13 @@ protected: if (_est_sub->update(&_est_time, &est)) { - // To be added and filled - // mavlink_estimator_status_t msg = {}; - //mavlink_msg_statustext_send_struct(_mavlink->get_channel(), &msg); + mavlink_estimator_status_t est_msg = {}; + + est_msg.time_usec = est.timestamp; + est_msg.pos_horiz_accuracy = est.pos_horiz_accuracy; + est_msg.pos_vert_accuracy = est.pos_vert_accuracy; + + mavlink_msg_estimator_status_send_struct(_mavlink->get_channel(), &est_msg); mavlink_vibration_t msg = {};