diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 17b2fb9b31..52cb19beaf 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -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 diff --git a/msg/telemetry_heartbeat.msg b/msg/telemetry_heartbeat.msg new file mode 100644 index 0000000000..705f4a0d7c --- /dev/null +++ b/msg/telemetry_heartbeat.msg @@ -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 diff --git a/msg/telemetry_status.msg b/msg/telemetry_status.msg index 878a472332..f7b399c1d2 100644 --- a/msg/telemetry_status.msg +++ b/msg/telemetry_status.msg @@ -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 diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index 420cf9a136..7be92d9d2a 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -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 diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index d9f1c158f2..6125b5dabb 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -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; } diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 998f8e28e1..6878093e3f 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -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)}; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index c2673543e5..ea92f695b6 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -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); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 19635985cf..424ba878a5 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -71,7 +71,7 @@ #include #include #include -#include +#include #include #include #include @@ -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 _telem_status_pub{ORB_ID(telemetry_status)}; + uORB::PublicationMulti _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) _param_mav_sys_id, diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 46a59e74e5..77b10e068a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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"); + } + } } }