Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 03:49:12 +08:00
Code Issues Actions 6 Packages Projects Releases Wiki Activity

mavlink_messages: Subscribe to all distance_sensor instances

Browse Source 
This will make all distance_sensor messages be published to
mavlink. Previously, if you ran multiple distance_sensor publishers,
typically multiple distance sensors, then you'd only get one of them
on mavlink.

The modifications to MavlinkStreamDistanceSensor are based on
MavlinkStreamBatteryStatus, which already exists with multi-instance
subscription and streaming to mavlink.

Use SubscriptionMultiArray

Co-authored-by: Daniel Agar <daniel@agar.ca>
This commit is contained in:
Morten Fyhn Amundsen
2020-03-06 10:32:54 +01:00
committed by Daniel Agar
parent 0d4d21cd67
commit cfe4982023
1 changed files with 35 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_messages.cpp
+10 -6
View File
@@ -4515,11 +4515,11 @@ public:
unsigned get_size() override unsigned get_size() override
{ {
return _distance_sensor_sub.advertised() ? (MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; return _distance_sensor_subs.advertised_count() * (MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES);
} }
private: private:
uORB::Subscription _distance_sensor_sub{ORB_ID(distance_sensor)}; uORB::SubscriptionMultiArray<distance_sensor_s> _distance_sensor_subs{ORB_ID::distance_sensor};
/* do not allow top copying this class */ /* do not allow top copying this class */
MavlinkStreamDistanceSensor(MavlinkStreamDistanceSensor &) = delete; MavlinkStreamDistanceSensor(MavlinkStreamDistanceSensor &) = delete;
@@ -4531,9 +4531,12 @@ protected:
bool send(const hrt_abstime t) override bool send(const hrt_abstime t) override
{ {
bool updated = false;
for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
distance_sensor_s dist_sensor; distance_sensor_s dist_sensor;
if (_distance_sensor_sub.update(&dist_sensor)) { if (_distance_sensor_subs[i].update(&dist_sensor)) {
mavlink_distance_sensor_t msg{}; mavlink_distance_sensor_t msg{};
msg.time_boot_ms = dist_sensor.timestamp / 1000; /* us to ms */ msg.time_boot_ms = dist_sensor.timestamp / 1000; /* us to ms */
@@ -4557,7 +4560,7 @@ protected:
} }
msg.current_distance = dist_sensor.current_distance * 1e2f; // m to cm msg.current_distance = dist_sensor.current_distance * 1e2f; // m to cm
msg.id = dist_sensor.id; msg.id = i;
msg.max_distance = dist_sensor.max_distance * 1e2f; // m to cm msg.max_distance = dist_sensor.max_distance * 1e2f; // m to cm
msg.min_distance = dist_sensor.min_distance * 1e2f; // m to cm msg.min_distance = dist_sensor.min_distance * 1e2f; // m to cm
msg.orientation = dist_sensor.orientation; msg.orientation = dist_sensor.orientation;
@@ -4565,10 +4568,11 @@ protected:
mavlink_msg_distance_sensor_send_struct(_mavlink->get_channel(), &msg); mavlink_msg_distance_sensor_send_struct(_mavlink->get_channel(), &msg);
return true; updated = true;
}
} }
return false; return updated;
} }
}; };