fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-23 09:07:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Removed subscribtion to multiple _telemetry_status instances.

Browse Source 
Since commander handles all telemetry_status the same there is no need to subscribe to multiple instances.


Signed-off-by: Claudio Micheli <claudio@auterion.com>
This commit is contained in:
Claudio Micheli
2019-03-07 17:30:18 +01:00
committed by Beat Küng
parent a53594135b
commit fb990d7de3
5 changed files with 87 additions and 100 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+78 -89
View File
@@ -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;
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;
}
}
_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_avoidance_system = telemetry.heartbeat_time;
_datalink_last_status_avoidance_system = telemetry.remote_system_status;
if (_avoidance_system_lost) {
mavlink_log_info(&mavlink_log_pub, "Avoidance system regained");
}
_avoidance_system_lost = false;
}
break;
}
_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;
}
}
_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_avoidance_system = telemetry.heartbeat_time;
_datalink_last_status_avoidance_system = telemetry.remote_system_status;
if (_avoidance_system_lost) {
mavlink_log_info(&mavlink_log_pub, "Avoidance system regained");
}
_avoidance_system_lost = false;
}
break;
}
}
}
// GCS data link loss failsafe
if (!status.data_link_lost) {
if (_datalink_last_heartbeat_gcs != 0
src/modules/commander/Commander.hpp
+1 -1
View File
@@ -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};
src/modules/mavlink/mavlink_main.cpp
+7 -2
View File
@@ -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()
src/modules/mavlink/mavlink_receiver.cpp
+1 -7
View File
@@ -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);
}
}
}
}
src/modules/mavlink/mavlink_receiver.h
-1
View File
@@ -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};