mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-06-28 03:50:34 +08:00
Commander: Move telemetry logic to separate functions
This commit is contained in:
acfloria
+225
-217
@@ -102,7 +102,6 @@
|
||||
#include <uORB/topics/subsystem_info.h>
|
||||
#include <uORB/topics/system_power.h>
|
||||
#include <uORB/topics/telemetry_status.h>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/vehicle_command_ack.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_land_detected.h>
|
||||
@@ -624,6 +623,20 @@ transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, co
|
||||
return arming_res;
|
||||
}
|
||||
|
||||
Commander::Commander() :
|
||||
ModuleParams(nullptr),
|
||||
_mission_result_sub(ORB_ID(mission_result))
|
||||
{
|
||||
for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
|
||||
_telemetry_subs[i] = -1;
|
||||
_telemetry_last_heartbeat[i] = 0;
|
||||
_telemetry_last_dl_loss[i] = 0;
|
||||
_telemetry_lost[i] = true;
|
||||
_telemetry_preflight_checks_reported[i] = false;
|
||||
_telemetry_high_latency[i] = false;
|
||||
}
|
||||
}
|
||||
|
||||
bool
|
||||
Commander::handle_command(vehicle_status_s *status_local,
|
||||
const vehicle_command_s& cmd, actuator_armed_s *armed_local,
|
||||
@@ -1307,7 +1320,6 @@ Commander::run()
|
||||
/* command ack */
|
||||
orb_advert_t command_ack_pub = nullptr;
|
||||
orb_advert_t commander_state_pub = nullptr;
|
||||
orb_advert_t vehicle_cmd_pub = nullptr;
|
||||
orb_advert_t vehicle_status_flags_pub = nullptr;
|
||||
|
||||
/* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */
|
||||
@@ -1346,23 +1358,6 @@ Commander::run()
|
||||
int offboard_control_mode_sub = orb_subscribe(ORB_ID(offboard_control_mode));
|
||||
memset(&offboard_control_mode, 0, sizeof(offboard_control_mode));
|
||||
|
||||
/* Subscribe to telemetry status topics */
|
||||
int telemetry_subs[ORB_MULTI_MAX_INSTANCES];
|
||||
uint64_t telemetry_last_heartbeat[ORB_MULTI_MAX_INSTANCES];
|
||||
uint64_t telemetry_last_dl_loss[ORB_MULTI_MAX_INSTANCES];
|
||||
bool telemetry_preflight_checks_reported[ORB_MULTI_MAX_INSTANCES];
|
||||
bool telemetry_lost[ORB_MULTI_MAX_INSTANCES];
|
||||
bool telemetry_high_latency[ORB_MULTI_MAX_INSTANCES];
|
||||
|
||||
for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
|
||||
telemetry_subs[i] = -1;
|
||||
telemetry_last_heartbeat[i] = 0;
|
||||
telemetry_last_dl_loss[i] = 0;
|
||||
telemetry_lost[i] = true;
|
||||
telemetry_preflight_checks_reported[i] = false;
|
||||
telemetry_high_latency[i] = false;
|
||||
}
|
||||
|
||||
/* Subscribe to global position */
|
||||
int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
|
||||
struct vehicle_global_position_s global_position;
|
||||
@@ -1712,73 +1707,8 @@ Commander::run()
|
||||
}
|
||||
}
|
||||
|
||||
for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
|
||||
|
||||
if (telemetry_subs[i] < 0) {
|
||||
telemetry_subs[i] = orb_subscribe_multi(ORB_ID(telemetry_status), i);
|
||||
}
|
||||
|
||||
orb_check(telemetry_subs[i], &updated);
|
||||
|
||||
if (updated) {
|
||||
telemetry_status_s telemetry = {};
|
||||
orb_copy(ORB_ID(telemetry_status), telemetry_subs[i], &telemetry);
|
||||
|
||||
/* perform system checks when new telemetry link connected */
|
||||
if (/* we first connect a link or re-connect a link after loosing it or haven't yet reported anything */
|
||||
(telemetry_last_heartbeat[i] == 0 || (hrt_elapsed_time(&telemetry_last_heartbeat[i]) > 3 * 1000 * 1000)
|
||||
|| !telemetry_preflight_checks_reported[i]) &&
|
||||
/* and this link has a communication partner */
|
||||
(telemetry.heartbeat_time > 0) &&
|
||||
/* and it is still connected */
|
||||
(hrt_elapsed_time(&telemetry.heartbeat_time) < 2 * 1000 * 1000) &&
|
||||
/* and the system is not already armed (and potentially flying) */
|
||||
!armed.armed) {
|
||||
|
||||
hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;
|
||||
/* flag the checks as reported for this link when we actually report them */
|
||||
telemetry_preflight_checks_reported[i] = hotplug_timeout;
|
||||
|
||||
/* provide RC and sensor status feedback to the user */
|
||||
if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
|
||||
/* HITL configuration: check only RC input */
|
||||
Preflight::preflightCheck(&mavlink_log_pub, false, false,
|
||||
(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), false,
|
||||
true, is_vtol(&status), false, false, hrt_elapsed_time(&commander_boot_timestamp));
|
||||
} else {
|
||||
/* check sensors also */
|
||||
Preflight::preflightCheck(&mavlink_log_pub, true, checkAirspeed,
|
||||
(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), arm_requirements & ARM_REQ_GPS_BIT,
|
||||
true, is_vtol(&status), hotplug_timeout, false, hrt_elapsed_time(&commander_boot_timestamp));
|
||||
}
|
||||
|
||||
// Provide feedback on mission state
|
||||
const mission_result_s& mission_result = _mission_result_sub.get();
|
||||
if ((mission_result.timestamp > commander_boot_timestamp) && hotplug_timeout &&
|
||||
(mission_result.instance_count > 0) && !mission_result.valid) {
|
||||
|
||||
mavlink_log_critical(&mavlink_log_pub, "Planned mission fails check. Please upload again.");
|
||||
}
|
||||
}
|
||||
|
||||
/* set (and don't reset) telemetry via USB as active once a MAVLink connection is up */
|
||||
if (telemetry.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB) {
|
||||
_usb_telemetry_active = true;
|
||||
}
|
||||
|
||||
/* set latency type of the telemetry connection */
|
||||
if (telemetry.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_IRIDIUM) {
|
||||
telemetry_high_latency[i] = true;
|
||||
} else {
|
||||
telemetry_high_latency[i] = false;
|
||||
}
|
||||
|
||||
|
||||
if (telemetry.heartbeat_time > 0) {
|
||||
telemetry_last_heartbeat[i] = telemetry.heartbeat_time;
|
||||
}
|
||||
}
|
||||
}
|
||||
// poll the telemetry status
|
||||
poll_telemetry_status(checkAirspeed, &hotplug_timeout);
|
||||
|
||||
orb_check(system_power_sub, &updated);
|
||||
|
||||
@@ -2521,137 +2451,8 @@ Commander::run()
|
||||
}
|
||||
}
|
||||
|
||||
/* data links check */
|
||||
bool have_link = false;
|
||||
bool high_latency_link_exists = false;
|
||||
bool have_low_latency_link = false;
|
||||
int32_t dl_loss_timeout_local = 0;
|
||||
int32_t dl_regain_timeout_local = 0;
|
||||
|
||||
for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
|
||||
if (telemetry_high_latency[i]) {
|
||||
high_latency_link_exists = true;
|
||||
if (status.high_latency_data_link_active) {
|
||||
dl_loss_timeout_local = highlatencydatalink_loss_timeout;
|
||||
dl_regain_timeout_local = highlatencydatalink_regain_timeout;
|
||||
} else {
|
||||
// if the high latency link is inactive we do not want to accidentally detect it as an active link
|
||||
dl_loss_timeout_local = INT32_MIN;
|
||||
dl_regain_timeout_local = INT32_MAX;
|
||||
}
|
||||
} else {
|
||||
dl_loss_timeout_local = datalink_loss_timeout;
|
||||
dl_regain_timeout_local = datalink_regain_timeout;
|
||||
}
|
||||
|
||||
if (telemetry_last_heartbeat[i] != 0 &&
|
||||
hrt_elapsed_time(&telemetry_last_heartbeat[i]) < dl_loss_timeout_local * 1e6) {
|
||||
/* handle the case where data link was gained first time or regained,
|
||||
* accept datalink as healthy only after datalink_regain_timeout seconds
|
||||
* */
|
||||
if (telemetry_lost[i] &&
|
||||
hrt_elapsed_time(&telemetry_last_dl_loss[i]) > dl_regain_timeout_local * 1e6) {
|
||||
|
||||
/* report a regain */
|
||||
if (telemetry_last_dl_loss[i] > 0) {
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "data link #%i regained", i);
|
||||
} else if (telemetry_last_dl_loss[i] == 0) {
|
||||
/* new link */
|
||||
}
|
||||
|
||||
/* got link again or new */
|
||||
status_flags.condition_system_prearm_error_reported = false;
|
||||
status_changed = true;
|
||||
|
||||
telemetry_lost[i] = false;
|
||||
have_link = true;
|
||||
if (!telemetry_high_latency[i]) {
|
||||
have_low_latency_link = true;
|
||||
}
|
||||
|
||||
} else if (!telemetry_lost[i]) {
|
||||
/* telemetry was healthy also in last iteration
|
||||
* we don't have to check a timeout */
|
||||
have_link = true;
|
||||
if (!telemetry_high_latency[i]) {
|
||||
have_low_latency_link = true;
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
|
||||
if (!telemetry_lost[i]) {
|
||||
/* only reset the timestamp to a different time on state change */
|
||||
telemetry_last_dl_loss[i] = hrt_absolute_time();
|
||||
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "data link #%i lost", i);
|
||||
telemetry_lost[i] = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (have_link) {
|
||||
/* handle the case where data link was regained */
|
||||
if (status.data_link_lost) {
|
||||
status.data_link_lost = false;
|
||||
status_changed = true;
|
||||
}
|
||||
|
||||
if (status.high_latency_data_link_active && have_low_latency_link) {
|
||||
// regained a low latency telemetry link, deactivate the high latency link
|
||||
// to avoid transmitting unnecessary data over that link
|
||||
status.high_latency_data_link_active = false;
|
||||
status_changed = true;
|
||||
mavlink_log_critical(&mavlink_log_pub, "LOW LATENCY DATA LINKS REGAINED, DEACTIVATING HIGH LATENCY LINK");
|
||||
|
||||
vehicle_command_s vehicle_cmd;
|
||||
vehicle_cmd.command = vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY;
|
||||
vehicle_cmd.param1 = 0.0f;
|
||||
vehicle_cmd.from_external = false;
|
||||
vehicle_cmd.target_system = status.system_id;
|
||||
vehicle_cmd.target_component = 0;
|
||||
|
||||
if (vehicle_cmd_pub != nullptr) {
|
||||
orb_publish(ORB_ID(vehicle_command), vehicle_cmd_pub, &vehicle_cmd);
|
||||
|
||||
} else {
|
||||
vehicle_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vehicle_cmd);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
if (high_latency_link_exists && !status.high_latency_data_link_active && armed.armed) {
|
||||
// low latency telemetry lost and high latency link existing
|
||||
status.high_latency_data_link_active = true;
|
||||
status_changed = true;
|
||||
|
||||
vehicle_command_s vehicle_cmd;
|
||||
vehicle_cmd.command = vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY;
|
||||
vehicle_cmd.param1 = 1.0f;
|
||||
vehicle_cmd.from_external = false;
|
||||
vehicle_cmd.target_system = status.system_id;
|
||||
vehicle_cmd.target_component = 0;
|
||||
|
||||
if (vehicle_cmd_pub != nullptr) {
|
||||
orb_publish(ORB_ID(vehicle_command), vehicle_cmd_pub, &vehicle_cmd);
|
||||
|
||||
} else {
|
||||
vehicle_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vehicle_cmd);
|
||||
}
|
||||
|
||||
if (!status.data_link_lost) {
|
||||
mavlink_log_critical(&mavlink_log_pub, "ALL LOW LATENCY DATA LINKS LOST, ACTIVATING HIGH LATENCY LINK");
|
||||
} else {
|
||||
mavlink_log_critical(&mavlink_log_pub, "ACTIVATING AVAILABLE HIGH LATENCY LINK");
|
||||
}
|
||||
} else if (!status.data_link_lost) {
|
||||
if (armed.armed) {
|
||||
mavlink_log_critical(&mavlink_log_pub, "ALL DATA LINKS LOST");
|
||||
}
|
||||
status.data_link_lost = true;
|
||||
status.data_link_lost_counter++;
|
||||
status_changed = true;
|
||||
}
|
||||
}
|
||||
// data link checks which update the status
|
||||
data_link_checks(highlatencydatalink_loss_timeout, highlatencydatalink_regain_timeout, datalink_loss_timeout, datalink_regain_timeout, &status_changed);
|
||||
|
||||
// engine failure detection
|
||||
// TODO: move out of commander
|
||||
@@ -4288,3 +4089,210 @@ void Commander::mission_init()
|
||||
orb_unadvertise(mission_pub);
|
||||
}
|
||||
}
|
||||
|
||||
void Commander::poll_telemetry_status(bool checkAirspeed, bool *hotplug_timeout)
|
||||
{
|
||||
bool updated = false;
|
||||
for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
|
||||
|
||||
if (_telemetry_subs[i] < 0) {
|
||||
_telemetry_subs[i] = orb_subscribe_multi(ORB_ID(telemetry_status), i);
|
||||
}
|
||||
|
||||
orb_check(_telemetry_subs[i], &updated);
|
||||
|
||||
if (updated) {
|
||||
telemetry_status_s telemetry = {};
|
||||
orb_copy(ORB_ID(telemetry_status), _telemetry_subs[i], &telemetry);
|
||||
|
||||
/* perform system checks when new telemetry link connected */
|
||||
if (/* we first connect a link or re-connect a link after loosing it or haven't yet reported anything */
|
||||
(_telemetry_last_heartbeat[i] == 0 || (hrt_elapsed_time(&_telemetry_last_heartbeat[i]) > 3 * 1000 * 1000)
|
||||
|| !_telemetry_preflight_checks_reported[i]) &&
|
||||
/* and this link has a communication partner */
|
||||
(telemetry.heartbeat_time > 0) &&
|
||||
/* and it is still connected */
|
||||
(hrt_elapsed_time(&telemetry.heartbeat_time) < 2 * 1000 * 1000) &&
|
||||
/* and the system is not already armed (and potentially flying) */
|
||||
!armed.armed) {
|
||||
|
||||
*hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;
|
||||
/* flag the checks as reported for this link when we actually report them */
|
||||
_telemetry_preflight_checks_reported[i] = *hotplug_timeout;
|
||||
|
||||
/* provide RC and sensor status feedback to the user */
|
||||
if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
|
||||
/* HITL configuration: check only RC input */
|
||||
Preflight::preflightCheck(&mavlink_log_pub, false, false,
|
||||
(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), false,
|
||||
true, is_vtol(&status), false, false, hrt_elapsed_time(&commander_boot_timestamp));
|
||||
} else {
|
||||
/* check sensors also */
|
||||
Preflight::preflightCheck(&mavlink_log_pub, true, checkAirspeed,
|
||||
(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), arm_requirements & ARM_REQ_GPS_BIT,
|
||||
true, is_vtol(&status), *hotplug_timeout, false, hrt_elapsed_time(&commander_boot_timestamp));
|
||||
}
|
||||
|
||||
// Provide feedback on mission state
|
||||
const mission_result_s& mission_result = _mission_result_sub.get();
|
||||
if ((mission_result.timestamp > commander_boot_timestamp) && *hotplug_timeout &&
|
||||
(mission_result.instance_count > 0) && !mission_result.valid) {
|
||||
|
||||
mavlink_log_critical(&mavlink_log_pub, "Planned mission fails check. Please upload again.");
|
||||
}
|
||||
}
|
||||
|
||||
/* set (and don't reset) telemetry via USB as active once a MAVLink connection is up */
|
||||
if (telemetry.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB) {
|
||||
_usb_telemetry_active = true;
|
||||
}
|
||||
|
||||
/* set latency type of the telemetry connection */
|
||||
if (telemetry.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_IRIDIUM) {
|
||||
_telemetry_high_latency[i] = true;
|
||||
} else {
|
||||
_telemetry_high_latency[i] = false;
|
||||
}
|
||||
|
||||
if (telemetry.heartbeat_time > 0) {
|
||||
_telemetry_last_heartbeat[i] = telemetry.heartbeat_time;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Commander::data_link_checks(int32_t highlatencydatalink_loss_timeout, int32_t highlatencydatalink_regain_timeout,
|
||||
int32_t datalink_loss_timeout, int32_t datalink_regain_timeout, bool *status_changed)
|
||||
{
|
||||
/* data links check */
|
||||
bool have_link = false;
|
||||
bool high_latency_link_exists = false;
|
||||
bool have_low_latency_link = false;
|
||||
int32_t dl_loss_timeout_local = 0;
|
||||
int32_t dl_regain_timeout_local = 0;
|
||||
|
||||
for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
|
||||
if (_telemetry_high_latency[i]) {
|
||||
high_latency_link_exists = true;
|
||||
if (status.high_latency_data_link_active) {
|
||||
dl_loss_timeout_local = highlatencydatalink_loss_timeout;
|
||||
dl_regain_timeout_local = highlatencydatalink_regain_timeout;
|
||||
} else {
|
||||
// if the high latency link is inactive we do not want to accidentally detect it as an active link
|
||||
dl_loss_timeout_local = INT32_MIN;
|
||||
dl_regain_timeout_local = INT32_MAX;
|
||||
}
|
||||
} else {
|
||||
dl_loss_timeout_local = datalink_loss_timeout;
|
||||
dl_regain_timeout_local = datalink_regain_timeout;
|
||||
}
|
||||
|
||||
if (_telemetry_last_heartbeat[i] != 0 &&
|
||||
hrt_elapsed_time(&_telemetry_last_heartbeat[i]) < dl_loss_timeout_local * 1e6) {
|
||||
/* handle the case where data link was gained first time or regained,
|
||||
* accept datalink as healthy only after datalink_regain_timeout seconds
|
||||
* */
|
||||
if (_telemetry_lost[i] &&
|
||||
hrt_elapsed_time(&_telemetry_last_dl_loss[i]) > dl_regain_timeout_local * 1e6) {
|
||||
|
||||
/* report a regain */
|
||||
if (_telemetry_last_dl_loss[i] > 0) {
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "data link #%i regained", i);
|
||||
} else if (_telemetry_last_dl_loss[i] == 0) {
|
||||
/* new link */
|
||||
}
|
||||
|
||||
/* got link again or new */
|
||||
status_flags.condition_system_prearm_error_reported = false;
|
||||
*status_changed = true;
|
||||
|
||||
_telemetry_lost[i] = false;
|
||||
have_link = true;
|
||||
if (!_telemetry_high_latency[i]) {
|
||||
have_low_latency_link = true;
|
||||
}
|
||||
|
||||
} else if (!_telemetry_lost[i]) {
|
||||
/* telemetry was healthy also in last iteration
|
||||
* we don't have to check a timeout */
|
||||
have_link = true;
|
||||
if (!_telemetry_high_latency[i]) {
|
||||
have_low_latency_link = true;
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
|
||||
if (!_telemetry_lost[i]) {
|
||||
/* only reset the timestamp to a different time on state change */
|
||||
_telemetry_last_dl_loss[i] = hrt_absolute_time();
|
||||
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "data link #%i lost", i);
|
||||
_telemetry_lost[i] = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (have_link) {
|
||||
/* handle the case where data link was regained */
|
||||
if (status.data_link_lost) {
|
||||
status.data_link_lost = false;
|
||||
*status_changed = true;
|
||||
}
|
||||
|
||||
if (status.high_latency_data_link_active && have_low_latency_link) {
|
||||
// regained a low latency telemetry link, deactivate the high latency link
|
||||
// to avoid transmitting unnecessary data over that link
|
||||
status.high_latency_data_link_active = false;
|
||||
*status_changed = true;
|
||||
mavlink_log_critical(&mavlink_log_pub, "LOW LATENCY DATA LINKS REGAINED, DEACTIVATING HIGH LATENCY LINK");
|
||||
|
||||
vehicle_command_s vehicle_cmd;
|
||||
vehicle_cmd.command = vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY;
|
||||
vehicle_cmd.param1 = 0.0f;
|
||||
vehicle_cmd.from_external = false;
|
||||
vehicle_cmd.target_system = status.system_id;
|
||||
vehicle_cmd.target_component = 0;
|
||||
|
||||
if (_vehicle_cmd_pub != nullptr) {
|
||||
orb_publish(ORB_ID(vehicle_command), _vehicle_cmd_pub, &vehicle_cmd);
|
||||
|
||||
} else {
|
||||
_vehicle_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vehicle_cmd);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
if (high_latency_link_exists && !status.high_latency_data_link_active && armed.armed) {
|
||||
// low latency telemetry lost and high latency link existing
|
||||
status.high_latency_data_link_active = true;
|
||||
*status_changed = true;
|
||||
|
||||
vehicle_command_s vehicle_cmd;
|
||||
vehicle_cmd.command = vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY;
|
||||
vehicle_cmd.param1 = 1.0f;
|
||||
vehicle_cmd.from_external = false;
|
||||
vehicle_cmd.target_system = status.system_id;
|
||||
vehicle_cmd.target_component = 0;
|
||||
|
||||
if (_vehicle_cmd_pub != nullptr) {
|
||||
orb_publish(ORB_ID(vehicle_command), _vehicle_cmd_pub, &vehicle_cmd);
|
||||
|
||||
} else {
|
||||
_vehicle_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vehicle_cmd);
|
||||
}
|
||||
|
||||
if (!status.data_link_lost) {
|
||||
mavlink_log_critical(&mavlink_log_pub, "ALL LOW LATENCY DATA LINKS LOST, ACTIVATING HIGH LATENCY LINK");
|
||||
} else {
|
||||
mavlink_log_critical(&mavlink_log_pub, "ACTIVATING AVAILABLE HIGH LATENCY LINK");
|
||||
}
|
||||
} else if (!status.data_link_lost) {
|
||||
if (armed.armed) {
|
||||
mavlink_log_critical(&mavlink_log_pub, "ALL DATA LINKS LOST");
|
||||
}
|
||||
status.data_link_lost = true;
|
||||
status.data_link_lost_counter++;
|
||||
*status_changed = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user