fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 09:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink_receiver: only publish the distance measurement of the mavlink flow message on uORB if they are valid

Browse Source 
On the snapdragon, optical flow is computed in a separate process and sent to the mainapp via mavlink. Distance measurements come from a range sensor and are already published on uORB.
Here we make sure that meaningless distance values in the mavlink optical flow message do not interfere with the ones from other sources.
This commit is contained in:
Nicolas de Palezieux
2016-04-21 18:18:08 +02:00
parent 88626cf5f1
commit 8c246b3219
1 changed files with 15 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+15 -13
View File
@@ -478,20 +478,22 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
struct distance_sensor_s d;
memset(&d, 0, sizeof(d));
d.timestamp = flow.integration_time_us * 1000; /* ms to us */
d.min_distance = 0.3f;
d.max_distance = 5.0f;
d.current_distance = flow.distance; /* both are in m */
d.type = 1;
d.id = MAV_DISTANCE_SENSOR_ULTRASOUND;
d.orientation = 8;
d.covariance = 0.0;
if(flow.distance > 0.0f) { // negative values signal invalid data
d.timestamp = flow.integration_time_us * 1000; /* ms to us */
d.min_distance = 0.3f;
d.max_distance = 5.0f;
d.current_distance = flow.distance; /* both are in m */
d.type = 1;
d.id = MAV_DISTANCE_SENSOR_ULTRASOUND;
d.orientation = 8;
d.covariance = 0.0;
if (_flow_distance_sensor_pub == nullptr) {
_flow_distance_sensor_pub = orb_advertise_multi(ORB_ID(distance_sensor), &d,
&_orb_class_instance, ORB_PRIO_HIGH);
} else {
orb_publish(ORB_ID(distance_sensor), _flow_distance_sensor_pub, &d);
if (_flow_distance_sensor_pub == nullptr) {
_flow_distance_sensor_pub = orb_advertise_multi(ORB_ID(distance_sensor), &d,
&_orb_class_instance, ORB_PRIO_HIGH);
} else {
orb_publish(ORB_ID(distance_sensor), _flow_distance_sensor_pub, &d);
}
}
}