dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Publishes LPE GPS epv and eph as estimator status. (#5413) 

* Publishes LPE GPS epv and eph as estimator status.

* Adds timestamp
This commit is contained in:
Miguel Arroyo 2016-09-03 16:00:51 -04:00 committed by Lorenz Meier
parent a071ef94db
commit f999fbe440
3 changed files with 12 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/estimator_status.msg
View File

@ -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

3
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
View File

@ -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();
}

10
src/modules/mavlink/mavlink_messages.cpp
View File

@ -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 = {};