diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp
index 0ea25af2431c87d3213f491f0d45f6e5b174bdf7..904b83efa60bf7d3ba37dd38092553afe40e4584 100644
--- a/src/modules/commander/Commander.cpp
+++ b/src/modules/commander/Commander.cpp
@@ -560,9 +560,9 @@ Commander::Commander() :
_auto_disarm_killed.set_hysteresis_time_from(false, 5_s);
_battery_sub = orb_subscribe(ORB_ID(battery_status));
- for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
- _telemetry_status_sub[i] = -1;
- }
+
+ _telemetry_status_sub = orb_subscribe(ORB_ID(telemetry_status));
+
// We want to accept RC inputs as default
status.rc_input_mode = vehicle_status_s::RC_IN_MODE_DEFAULT;
@@ -585,12 +585,8 @@ Commander::Commander() :
Commander::~Commander()
{
orb_unsubscribe(_battery_sub);
+ orb_unsubscribe(_telemetry_status_sub);
- for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
- if (_telemetry_status_sub[i] >= 0) {
- orb_unsubscribe(_telemetry_status_sub[i]);
- }
- }
if (_iridiumsbd_status_sub >= 0) {
orb_unsubscribe(_iridiumsbd_status_sub);
@@ -3831,112 +3827,105 @@ bool Commander::preflight_check(bool report)
void Commander::data_link_check(bool &status_changed)
{
- for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+ bool updated = false;
- if (_telemetry_status_sub[i] < 0) {
- if (orb_exists(ORB_ID(telemetry_status), i) == PX4_OK) {
- _telemetry_status_sub[i] = orb_subscribe_multi(ORB_ID(telemetry_status), i);
- }
- }
+ orb_check(_telemetry_status_sub, &updated);
- bool updated = false;
+ if (updated) {
- orb_check(_telemetry_status_sub[i], &updated);
+ telemetry_status_s telemetry;
- if (updated) {
- telemetry_status_s telemetry;
+ if (orb_copy(ORB_ID(telemetry_status), _telemetry_status_sub, &telemetry) == PX4_OK) {
- if (orb_copy(ORB_ID(telemetry_status), _telemetry_status_sub[i], &telemetry) == PX4_OK) {
-
- // handle different radio types
- switch (telemetry.type) {
- case telemetry_status_s::LINK_TYPE_USB:
- // set (but don't unset) telemetry via USB as active once a MAVLink connection is up
- status_flags.usb_connected = true;
- break;
+ // handle different radio types
+ switch (telemetry.type) {
+ case telemetry_status_s::LINK_TYPE_USB:
+ // set (but don't unset) telemetry via USB as active once a MAVLink connection is up
+ status_flags.usb_connected = true;
+ break;
- case telemetry_status_s::LINK_TYPE_IRIDIUM:
+ case telemetry_status_s::LINK_TYPE_IRIDIUM:
- // lazily subscribe
- if (_iridiumsbd_status_sub == -1 && orb_exists(ORB_ID(iridiumsbd_status), 0) == PX4_OK) {
- _iridiumsbd_status_sub = orb_subscribe(ORB_ID(iridiumsbd_status));
- }
+ // lazily subscribe
+ if (_iridiumsbd_status_sub == -1 && orb_exists(ORB_ID(iridiumsbd_status), 0) == PX4_OK) {
+ _iridiumsbd_status_sub = orb_subscribe(ORB_ID(iridiumsbd_status));
+ }
- if (_iridiumsbd_status_sub >= 0) {
- bool iridiumsbd_updated = false;
- orb_check(_iridiumsbd_status_sub, &iridiumsbd_updated);
+ if (_iridiumsbd_status_sub >= 0) {
+ bool iridiumsbd_updated = false;
+ orb_check(_iridiumsbd_status_sub, &iridiumsbd_updated);
- if (iridiumsbd_updated) {
- iridiumsbd_status_s iridium_status;
+ if (iridiumsbd_updated) {
+ iridiumsbd_status_s iridium_status;
- if (orb_copy(ORB_ID(iridiumsbd_status), _iridiumsbd_status_sub, &iridium_status) == PX4_OK) {
- _high_latency_datalink_heartbeat = iridium_status.last_heartbeat;
+ if (orb_copy(ORB_ID(iridiumsbd_status), _iridiumsbd_status_sub, &iridium_status) == PX4_OK) {
+ _high_latency_datalink_heartbeat = iridium_status.last_heartbeat;
- if (status.high_latency_data_link_lost) {
- if (hrt_elapsed_time(&_high_latency_datalink_lost) > (_high_latency_datalink_regain_threshold.get() * 1_s)) {
- status.high_latency_data_link_lost = false;
- status_changed = true;
- }
+ if (status.high_latency_data_link_lost) {
+ if (hrt_elapsed_time(&_high_latency_datalink_lost) > (_high_latency_datalink_regain_threshold.get() * 1_s)) {
+ status.high_latency_data_link_lost = false;
+ status_changed = true;
}
-
}
+
}
}
-
- break;
}
+ break;
+ }
- // handle different remote types
- switch (telemetry.remote_type) {
- case telemetry_status_s::MAV_TYPE_GCS:
- // Recover 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;
- mavlink_log_info(&mavlink_log_pub, "Data link regained");
- }
+ // handle different remote types
+ switch (telemetry.remote_type) {
+ case telemetry_status_s::MAV_TYPE_GCS:
+
+ // Recover 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;
+ mavlink_log_info(&mavlink_log_pub, "Data link regained");
}
+ }
- _datalink_last_heartbeat_gcs = telemetry.heartbeat_time;
+ _datalink_last_heartbeat_gcs = telemetry.heartbeat_time;
- break;
+ 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;
- }
+ 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;
}
- _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;
- }
+ _datalink_last_heartbeat_onboard_controller = telemetry.heartbeat_time;
- _datalink_last_heartbeat_avoidance_system = telemetry.heartbeat_time;
- _datalink_last_status_avoidance_system = telemetry.remote_system_status;
+ 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;
+ }
- if (_avoidance_system_lost) {
- mavlink_log_info(&mavlink_log_pub, "Avoidance system regained");
- }
+ _datalink_last_heartbeat_avoidance_system = telemetry.heartbeat_time;
+ _datalink_last_status_avoidance_system = telemetry.remote_system_status;
- _avoidance_system_lost = false;
+ if (_avoidance_system_lost) {
+ mavlink_log_info(&mavlink_log_pub, "Avoidance system regained");
}
- break;
+ _avoidance_system_lost = false;
}
+
+ break;
}
}
}
+
// GCS data link loss failsafe
if (!status.data_link_lost) {
if (_datalink_last_heartbeat_gcs != 0
diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp
index 8aa9d635c7eea31a0ef9223e3040719c608620ee..ce55e0dd5f7a5b09fd0a99caee66af2fe570a871 100644
--- a/src/modules/commander/Commander.hpp
+++ b/src/modules/commander/Commander.hpp
@@ -173,7 +173,7 @@ private:
* Checks the status of all available data links and handles switching between different system telemetry states.
*/
void data_link_check(bool &status_changed);
- int _telemetry_status_sub[ORB_MULTI_MAX_INSTANCES] {};
+ int _telemetry_status_sub{-1};
hrt_abstime _datalink_last_heartbeat_gcs{0};
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 4f205362cde8470c0e0fc65663ff6e7d1c0dd725..3cea61845ede2cda847aa679b839f24940c23b20 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -2552,8 +2552,13 @@ void Mavlink::publish_telemetry_status()
_tstatus.streams = _streams.size();
_tstatus.timestamp = hrt_absolute_time();
- int instance;
- orb_publish_auto(ORB_ID(telemetry_status), &_telem_status_pub, &_tstatus, &instance, ORB_PRIO_DEFAULT);
+
+ if (_telem_status_pub == nullptr) {
+ _telem_status_pub = orb_advertise_queue(ORB_ID(telemetry_status), &_tstatus, 3);
+
+ } else {
+ orb_publish(ORB_ID(telemetry_status), _telem_status_pub, &_tstatus);
+ }
}
void Mavlink::check_radio_config()
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index bea892ef02331ef7ba5890ebb39d6f5f6ed2463d..4dc67274f8af389a01cf973db255208b9dbd5730 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -1896,14 +1896,8 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
tstatus.remote_component_id = msg->compid;
tstatus.remote_type = hb.type;
tstatus.remote_system_status = hb.system_status;
-
- if (_telem_status_pub == nullptr) {
- _telem_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
-
- } else {
- orb_publish(ORB_ID(telemetry_status), _telem_status_pub, &tstatus);
- }
}
+
}
}
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 676b441d77cdd5efdb3c7de3910178c23b001206..caa294ea6e749f3391ae365d3207f1538ea2d4cf 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -253,7 +253,6 @@ private:
orb_advert_t _rc_pub{nullptr};
orb_advert_t _trajectory_waypoint_pub{nullptr};
orb_advert_t _transponder_report_pub{nullptr};
- orb_advert_t _telem_status_pub{nullptr};
orb_advert_t _visual_odometry_pub{nullptr};
static constexpr int _gps_inject_data_queue_size{6};