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

mavlink: publish telemetry_status per instance with all HEARTBEATS from the same system

Browse Source 
- one telemetry_status publication per mavlink instance
 - each telemetry_status has an array of HEARTBEATS
This commit is contained in:
Daniel Agar
2020-08-07 12:23:52 -04:00
committed by GitHub
parent f950fe8a38
commit 3002e74b4f
9 changed files with 192 additions and 106 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+48 -12
View File
@@ -2045,25 +2045,61 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
{
/* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */
if (_mavlink->get_channel() < (mavlink_channel_t)ORB_MULTI_MAX_INSTANCES) {
const hrt_abstime now = hrt_absolute_time();
mavlink_heartbeat_t hb;
mavlink_msg_heartbeat_decode(msg, &hb);
/* Accept only heartbeats from GCS or ONBOARD Controller, skip heartbeats from other vehicles */
if ((msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) || (msg->sysid == mavlink_system.sysid
&& hb.type == MAV_TYPE_ONBOARD_CONTROLLER)) {
const bool same_system = (msg->sysid == mavlink_system.sysid);
if (same_system || hb.type == MAV_TYPE_GCS) {
telemetry_status_s &tstatus = _mavlink->get_telemetry_status();
/* set heartbeat time and topic time and publish -
* the telem status also gets updated on telemetry events
*/
tstatus.heartbeat_time = hrt_absolute_time();
tstatus.remote_system_id = msg->sysid;
tstatus.remote_component_id = msg->compid;
tstatus.remote_type = hb.type;
tstatus.remote_system_status = hb.system_status;
}
bool heartbeat_slot_found = false;
int heartbeat_slot = 0;
// find existing HEARTBEAT slot
for (size_t i = 0; i < (sizeof(tstatus.heartbeats) / sizeof(tstatus.heartbeats[0])); i++) {
if ((tstatus.heartbeats[i].system_id == msg->sysid)
&& (tstatus.heartbeats[i].component_id == msg->compid)
&& (tstatus.heartbeats[i].type == hb.type)) {
// found matching heartbeat slot
heartbeat_slot = i;
heartbeat_slot_found = true;
break;
}
}
// otherwise use first available slot
if (!heartbeat_slot_found) {
for (size_t i = 0; i < (sizeof(tstatus.heartbeats) / sizeof(tstatus.heartbeats[0])); i++) {
if ((tstatus.heartbeats[i].system_id == 0) && (tstatus.heartbeats[i].timestamp == 0)) {
heartbeat_slot = i;
heartbeat_slot_found = true;
break;
}
}
}
if (heartbeat_slot_found) {
_mavlink->lock_telemetry_status();
tstatus.heartbeats[heartbeat_slot].timestamp = now;
tstatus.heartbeats[heartbeat_slot].system_id = msg->sysid;
tstatus.heartbeats[heartbeat_slot].component_id = msg->compid;
tstatus.heartbeats[heartbeat_slot].type = hb.type;
tstatus.heartbeats[heartbeat_slot].state = hb.system_status;
_mavlink->telemetry_status_updated();
_mavlink->unlock_telemetry_status();
} else {
PX4_ERR("no telemetry heartbeat slots available");
}
}
}
}