yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00: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

1
msg/CMakeLists.txt
View File

@@ -125,6 +125,7 @@ set(msg_files
system_power.msg
task_stack_info.msg
tecs_status.msg
telemetry_heartbeat.msg
telemetry_status.msg
test_motor.msg
timesync.msg

51
msg/telemetry_heartbeat.msg Normal file
View File

@@ -0,0 +1,51 @@
uint64 timestamp # time since system start (microseconds)
# COMPONENT (fill in as needed)
uint8 COMP_ID_ALL = 0
uint8 COMP_ID_AUTOPILOT1 = 1
uint8 COMP_ID_TELEMETRY_RADIO = 68
uint8 COMP_ID_CAMERA = 100
uint8 COMP_ID_GIMBAL = 154
uint8 COMP_ID_LOG = 155
uint8 COMP_ID_ADSB = 156
uint8 COMP_ID_OSD = 157
uint8 COMP_ID_PERIPHERAL = 158
uint8 COMP_ID_FLARM = 160
uint8 COMP_ID_MISSIONPLANNER = 190
uint8 COMP_ID_OBSTACLE_AVOIDANCE = 196
uint8 COMP_ID_VISUAL_INERTIAL_ODOMETRY = 197
uint8 COMP_ID_PAIRING_MANAGER = 198
uint8 COMP_ID_UDP_BRIDGE = 240
uint8 COMP_ID_UART_BRIDGE = 241
uint8 COMP_ID_TUNNEL_NODE = 242
uint8 system_id # system id of the remote system (Mavlink header sys_id)
uint8 component_id # component id of the remote system (Mavlink header comp_id)
# TYPE (fill in as needed)
uint8 TYPE_GENERIC = 0
uint8 TYPE_ANTENNA_TRACKER = 5
uint8 TYPE_GCS = 6
uint8 TYPE_ONBOARD_CONTROLLER = 18
uint8 TYPE_GIMBAL = 26
uint8 TYPE_ADSB = 27
uint8 TYPE_CAMERA = 30
uint8 TYPE_CHARGING_STATION = 31
uint8 type
# STATE
uint8 STATE_UNINIT = 0
uint8 STATE_BOOT = 1
uint8 STATE_CALIBRATING = 2
uint8 STATE_STANDBY = 3
uint8 STATE_ACTIVE = 4
uint8 STATE_CRITICAL = 5
uint8 STATE_EMERGENCY = 6
uint8 STATE_POWEROFF = 7
uint8 STATE_FLIGHT_TERMINATION = 8
uint8 state

31
msg/telemetry_status.msg
View File

@@ -4,36 +4,9 @@ uint8 LINK_TYPE_WIRE = 2
uint8 LINK_TYPE_USB = 3
uint8 LINK_TYPE_IRIDIUM = 4
# MAV_COMPONENT (fill in as needed)
uint8 COMPONENT_ID_ALL = 0
uint8 COMPONENT_ID_AUTOPILOT1 = 1
uint8 COMPONENT_ID_CAMERA = 100
uint8 COMPONENT_ID_OBSTACLE_AVOIDANCE = 196
# MAV_TYPE (fill in as needed)
uint8 MAV_TYPE_GENERIC = 0
uint8 MAV_TYPE_GCS = 6
uint8 MAV_TYPE_ONBOARD_CONTROLLER = 18
# MAV_STATE
uint8 MAV_STATE_UNINIT = 0
uint8 MAV_STATE_BOOT = 1
uint8 MAV_STATE_CALIBRATING = 2
uint8 MAV_STATE_STANDBY = 3
uint8 MAV_STATE_ACTIVE = 4
uint8 MAV_STATE_CRITICAL = 5
uint8 MAV_STATE_EMERGENCY = 6
uint8 MAV_STATE_POWEROFF = 7
uint8 MAV_STATE_FLIGHT_TERMINATION = 8
uint64 timestamp # time since system start (microseconds)
uint64 heartbeat_time # Time of last received heartbeat from remote system
uint8 remote_system_id # system id of the remote system (Mavlink header sys_id)
uint8 remote_component_id # component id of the remote system (Mavlink header comp_id)
uint8 remote_type # remote system status flag (Mavlink MAV_TYPE)
uint8 remote_system_status # remote system status flag (Mavlink MAV_STATE)
telemetry_heartbeat[4] heartbeats
uint8 type # type of the radio hardware (LINK_TYPE_*)
@@ -54,5 +27,3 @@ float32 rate_rx
float32 rate_tx
float32 rate_txerr
uint8 ORB_QUEUE_LENGTH = 3

2
msg/tools/uorb_rtps_message_ids.yaml
View File

@@ -295,6 +295,8 @@ rtps:
id: 131
- msg: yaw_estimator_status
id: 132
- msg: telemetry_heartbeat
id: 133
########## multi topics: begin ##########
- msg: actuator_controls_0
id: 150

107
src/modules/commander/Commander.cpp
View File

@@ -3716,11 +3716,10 @@ void Commander::mission_init()
void Commander::data_link_check()
{
if (_telemetry_status_sub.updated()) {
for (auto &telemetry_status : _telemetry_status_sub) {
telemetry_status_s telemetry;
if (_telemetry_status_sub.copy(&telemetry)) {
if (telemetry_status.update(&telemetry)) {
// handle different radio types
switch (telemetry.type) {
@@ -3730,7 +3729,6 @@ void Commander::data_link_check()
break;
case telemetry_status_s::LINK_TYPE_IRIDIUM: {
iridiumsbd_status_s iridium_status;
if (_iridiumsbd_status_sub.update(&iridium_status)) {
@@ -3748,65 +3746,65 @@ void Commander::data_link_check()
}
}
// handle different remote types
switch (telemetry.remote_type) {
case telemetry_status_s::MAV_TYPE_GCS:
for (const auto &hb : telemetry.heartbeats) {
// handle different remote types
switch (hb.type) {
case telemetry_heartbeat_s::TYPE_GCS:
// Initial connection or recovery from data link lost
if (status.data_link_lost) {
if (telemetry.heartbeat_time > _datalink_last_heartbeat_gcs) {
status.data_link_lost = false;
_status_changed = true;
// Initial connection or recovery from data link lost
if (status.data_link_lost) {
if (hb.timestamp > _datalink_last_heartbeat_gcs) {
status.data_link_lost = false;
_status_changed = true;
if (!armed.armed && !status_flags.condition_calibration_enabled) {
// make sure to report preflight check failures to a connecting GCS
PreFlightCheck::preflightCheck(&mavlink_log_pub, status, status_flags,
_arm_requirements.global_position, true, true, hrt_elapsed_time(&_boot_timestamp));
}
if (!armed.armed && !status_flags.condition_calibration_enabled) {
// make sure to report preflight check failures to a connecting GCS
PreFlightCheck::preflightCheck(&mavlink_log_pub, status, status_flags,
_arm_requirements.global_position, true, true, hrt_elapsed_time(&_boot_timestamp));
}
if (_datalink_last_heartbeat_gcs != 0) {
mavlink_log_info(&mavlink_log_pub, "Data link regained");
if (_datalink_last_heartbeat_gcs != 0) {
mavlink_log_info(&mavlink_log_pub, "Data link regained");
}
}
}
}
// Only keep the very last heartbeat timestamp, so we don't get confused
// by multiple mavlink instances publishing different timestamps.
if (telemetry.heartbeat_time > _datalink_last_heartbeat_gcs) {
_datalink_last_heartbeat_gcs = telemetry.heartbeat_time;
}
break;
case telemetry_status_s::MAV_TYPE_ONBOARD_CONTROLLER:
if (_onboard_controller_lost) {
if (telemetry.heartbeat_time > _datalink_last_heartbeat_onboard_controller) {
mavlink_log_info(&mavlink_log_pub, "Onboard controller regained");
_onboard_controller_lost = false;
_status_changed = true;
// Only keep the very last heartbeat timestamp, so we don't get confused
// by multiple mavlink instances publishing different timestamps.
if (hb.timestamp > _datalink_last_heartbeat_gcs) {
_datalink_last_heartbeat_gcs = hb.timestamp;
}
}
break;
_datalink_last_heartbeat_onboard_controller = telemetry.heartbeat_time;
if (telemetry.remote_component_id == telemetry_status_s::COMPONENT_ID_OBSTACLE_AVOIDANCE) {
if (telemetry.heartbeat_time != _datalink_last_heartbeat_avoidance_system) {
_avoidance_system_status_change = _datalink_last_status_avoidance_system != telemetry.remote_system_status;
case telemetry_heartbeat_s::TYPE_ONBOARD_CONTROLLER:
if (_onboard_controller_lost) {
if (hb.timestamp > _datalink_last_heartbeat_onboard_controller) {
mavlink_log_info(&mavlink_log_pub, "Onboard controller regained");
_onboard_controller_lost = false;
_status_changed = true;
}
}
_datalink_last_heartbeat_avoidance_system = telemetry.heartbeat_time;
_datalink_last_status_avoidance_system = telemetry.remote_system_status;
_datalink_last_heartbeat_onboard_controller = hb.timestamp;
if (_avoidance_system_lost) {
_status_changed = true;
_avoidance_system_lost = false;
status_flags.avoidance_system_valid = true;
if (hb.component_id == telemetry_heartbeat_s::COMP_ID_OBSTACLE_AVOIDANCE) {
if (hb.timestamp > _datalink_last_heartbeat_avoidance_system) {
_avoidance_system_status_change = (_datalink_last_status_avoidance_system != hb.state);
}
_datalink_last_heartbeat_avoidance_system = hb.timestamp;
_datalink_last_status_avoidance_system = hb.state;
if (_avoidance_system_lost) {
_status_changed = true;
_avoidance_system_lost = false;
status_flags.avoidance_system_valid = true;
}
}
}
break;
break;
}
}
}
}
@@ -3814,7 +3812,7 @@ void Commander::data_link_check()
// GCS data link loss failsafe
if (!status.data_link_lost) {
if (_datalink_last_heartbeat_gcs != 0
if ((_datalink_last_heartbeat_gcs != 0)
&& hrt_elapsed_time(&_datalink_last_heartbeat_gcs) > (_param_com_dl_loss_t.get() * 1_s)) {
status.data_link_lost = true;
@@ -3842,26 +3840,27 @@ void Commander::data_link_check()
//if heartbeats stop
if (!_avoidance_system_lost && (_datalink_last_heartbeat_avoidance_system > 0)
&& (hrt_elapsed_time(&_datalink_last_heartbeat_avoidance_system) > 5_s)) {
_avoidance_system_lost = true;
status_flags.avoidance_system_valid = false;
}
//if status changed
if (_avoidance_system_status_change) {
if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_BOOT) {
if (_datalink_last_status_avoidance_system == telemetry_heartbeat_s::STATE_BOOT) {
status_flags.avoidance_system_valid = false;
}
if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_ACTIVE) {
if (_datalink_last_status_avoidance_system == telemetry_heartbeat_s::STATE_ACTIVE) {
status_flags.avoidance_system_valid = true;
}
if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_CRITICAL) {
if (_datalink_last_status_avoidance_system == telemetry_heartbeat_s::STATE_CRITICAL) {
status_flags.avoidance_system_valid = false;
_status_changed = true;
}
if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_FLIGHT_TERMINATION) {
if (_datalink_last_status_avoidance_system == telemetry_heartbeat_s::STATE_FLIGHT_TERMINATION) {
status_flags.avoidance_system_valid = false;
_status_changed = true;
}

4
src/modules/commander/Commander.hpp
View File

@@ -320,7 +320,7 @@ private:
bool _onboard_controller_lost{false};
bool _avoidance_system_lost{false};
bool _avoidance_system_status_change{false};
uint8_t _datalink_last_status_avoidance_system{telemetry_status_s::MAV_STATE_UNINIT};
uint8_t _datalink_last_status_avoidance_system{telemetry_heartbeat_s::STATE_UNINIT};
hrt_abstime _high_latency_datalink_heartbeat{0};
hrt_abstime _high_latency_datalink_lost{0};
@@ -406,7 +406,7 @@ private:
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _subsys_sub{ORB_ID(subsystem_info)};
uORB::Subscription _system_power_sub{ORB_ID(system_power)};
uORB::Subscription _telemetry_status_sub{ORB_ID(telemetry_status)};
uORB::Subscription _telemetry_status_sub[ORB_MULTI_MAX_INSTANCES] {{ORB_ID(telemetry_status), 0}, {ORB_ID(telemetry_status), 1}, {ORB_ID(telemetry_status), 2}, {ORB_ID(telemetry_status), 3}};
uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};

31
src/modules/mavlink/mavlink_main.cpp
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);

11
src/modules/mavlink/mavlink_main.h
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,

60
src/modules/mavlink/mavlink_receiver.cpp
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");
}
}
}
}