fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 14:13: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_main.cpp
+26 -5
View File
@@ -355,6 +355,18 @@ Mavlink::get_instance_for_network_port(unsigned long port)
}
#endif // MAVLINK_UDP
bool
Mavlink::is_connected()
{
for (auto &hb : _tstatus.heartbeats) {
if ((hb.type == telemetry_heartbeat_s::TYPE_GCS) && (hrt_elapsed_time(&hb.timestamp) < 3_s)) {
return true;
}
}
return false;
}
int
Mavlink::destroy_all_instances()
{
@@ -826,7 +838,7 @@ void Mavlink::send_finish()
# endif // CONFIG_NET
if ((_mode != MAVLINK_MODE_ONBOARD) && broadcast_enabled() &&
(!get_client_source_initialized() || (hrt_elapsed_time(&_tstatus.heartbeat_time) > 3_s))) {
(!get_client_source_initialized() || !is_connected())) {
if (!_broadcast_address_found) {
find_broadcast_address();
@@ -2103,6 +2115,7 @@ Mavlink::task_main(int argc, char *argv[])
/* initialize send mutex */
pthread_mutex_init(&_send_mutex, nullptr);
pthread_mutex_init(&_telemetry_status_mutex, nullptr);
/* if we are passing on mavlink messages, we need to prepare a buffer for this instance */
if (_forwarding_on) {
@@ -2469,7 +2482,7 @@ Mavlink::task_main(int argc, char *argv[])
}
// publish status at 1 Hz, or sooner if HEARTBEAT has updated
if ((hrt_elapsed_time(&_tstatus.timestamp) >= 1_s) || (_tstatus.timestamp < _tstatus.heartbeat_time)) {
if ((hrt_elapsed_time(&_tstatus.timestamp) >= 1_s) || _tstatus_updated) {
publish_telemetry_status();
}
@@ -2505,6 +2518,9 @@ Mavlink::task_main(int argc, char *argv[])
_mavlink_ulog = nullptr;
}
pthread_mutex_destroy(&_send_mutex);
pthread_mutex_destroy(&_telemetry_status_mutex);
PX4_INFO("exiting channel %i", (int)_channel);
return OK;
@@ -2596,9 +2612,12 @@ void Mavlink::publish_telemetry_status()
_tstatus.streams = _streams.size();
// telemetry_status is also updated from the receiver thread, but never the same fields
lock_telemetry_status();
_tstatus.timestamp = hrt_absolute_time();
_telem_status_pub.publish(_tstatus);
_tstatus_updated = false;
unlock_telemetry_status();
}
void Mavlink::configure_sik_radio()
@@ -2737,8 +2756,10 @@ Mavlink::start(int argc, char *argv[])
void
Mavlink::display_status()
{
if (_tstatus.heartbeat_time > 0) {
printf("\tGCS heartbeat:\t%llu us ago\n", (unsigned long long)hrt_elapsed_time(&_tstatus.heartbeat_time));
for (const auto &hb : _tstatus.heartbeats) {
if ((hb.timestamp > 0) && (hb.type == telemetry_heartbeat_s::TYPE_GCS)) {
printf("\tGCS heartbeat:\t%llu us ago\n", (unsigned long long)hrt_elapsed_time(&hb.timestamp));
}
}
printf("\tmavlink chan: #%u\n", _channel);
src/modules/mavlink/mavlink_main.h
+8 -3
View File
@@ -71,7 +71,7 @@
#include <px4_platform_common/posix.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/uthash/utlist.h>
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/mavlink_log.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/radio_status.h>
@@ -258,7 +258,7 @@ public:
bool get_forwarding_on() { return _forwarding_on; }
bool is_connected() { return (hrt_elapsed_time(&_tstatus.heartbeat_time) < 3_s); }
bool is_connected();
#if defined(MAVLINK_UDP)
static Mavlink *get_instance_for_network_port(unsigned long port);
@@ -431,6 +431,9 @@ public:
* Get the receive status of this MAVLink link
*/
telemetry_status_s &get_telemetry_status() { return _tstatus; }
void lock_telemetry_status() { pthread_mutex_lock(&_telemetry_status_mutex); }
void unlock_telemetry_status() { pthread_mutex_unlock(&_telemetry_status_mutex); }
void telemetry_status_updated() { _tstatus_updated = true; }
void set_telemetry_status_type(uint8_t type) { _tstatus.type = type; }
@@ -530,7 +533,7 @@ private:
orb_advert_t _mavlink_log_pub{nullptr};
uORB::PublicationQueued<telemetry_status_s> _telem_status_pub{ORB_ID(telemetry_status)};
uORB::PublicationMulti<telemetry_status_s> _telem_status_pub{ORB_ID(telemetry_status)};
bool _task_running{true};
static bool _boot_complete;
@@ -632,6 +635,7 @@ private:
radio_status_s _rstatus {};
telemetry_status_s _tstatus {};
bool _tstatus_updated{false};
ping_statistics_s _ping_stats {};
@@ -646,6 +650,7 @@ private:
pthread_mutex_t _message_buffer_mutex {};
pthread_mutex_t _send_mutex {};
pthread_mutex_t _telemetry_status_mutex {};
DEFINE_PARAMETERS(
(ParamInt<px4::params::MAV_SYS_ID>) _param_mav_sys_id,
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");
}
}
}
}