From 6dec451babf1b4c6394fbf8678585d66932adefb Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Tue, 27 Nov 2018 17:43:22 -0500
Subject: [PATCH] HEARTBEAT and commander failsafe handling cleanup
---
msg/telemetry_status.msg | 43 +-
msg/vehicle_status.msg | 6 +-
src/modules/commander/Commander.cpp | 395 ++++++------------
src/modules/commander/Commander.hpp | 42 +-
src/modules/commander/commander_params.c | 42 ++
.../commander/state_machine_helper.cpp | 79 ++--
src/modules/commander/state_machine_helper.h | 12 +-
src/modules/mavlink/mavlink_main.cpp | 20 +-
src/modules/mavlink/mavlink_receiver.cpp | 12 +-
src/modules/navigator/navigator_params.c | 42 --
10 files changed, 301 insertions(+), 392 deletions(-)
diff --git a/msg/telemetry_status.msg b/msg/telemetry_status.msg
index 2bc34ce4d8..bce7ffc520 100644
--- a/msg/telemetry_status.msg
+++ b/msg/telemetry_status.msg
@@ -1,18 +1,41 @@
-uint64 timestamp # time since system start (microseconds)
+uint8 LINK_TYPE_GENERIC = 0
+uint8 LINK_TYPE_3DR_RADIO = 1
+uint8 LINK_TYPE_UBIQUITY_BULLET = 2
+uint8 LINK_TYPE_WIRE = 3
+uint8 LINK_TYPE_USB = 4
+uint8 LINK_TYPE_IRIDIUM = 5
+
+# MAV_COMPONENT (fill in as needed)
+uint8 COMPONENT_ID_ALL = 0
+uint8 COMPONENT_ID_AUTOPILOT1 = 1
+uint8 COMPONENT_ID_CAMERA = 100
+
+# 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
-uint8 TELEMETRY_STATUS_RADIO_TYPE_GENERIC = 0
-uint8 TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO = 1
-uint8 TELEMETRY_STATUS_RADIO_TYPE_UBIQUITY_BULLET = 2
-uint8 TELEMETRY_STATUS_RADIO_TYPE_WIRE = 3
-uint8 TELEMETRY_STATUS_RADIO_TYPE_USB = 4
-uint8 TELEMETRY_STATUS_RADIO_TYPE_IRIDIUM = 5
+uint64 timestamp # time since system start (microseconds)
uint64 heartbeat_time # Time of last received heartbeat from remote system
-uint8 type # type of the radio hardware
+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)
-uint8 system_id # system id of the remote system
-uint8 component_id # component id of the remote system
+uint8 type # type of the radio hardware
uint8 mode
diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg
index 956233fd95..531cfa5860 100644
--- a/msg/vehicle_status.msg
+++ b/msg/vehicle_status.msg
@@ -62,6 +62,7 @@ uint8 system_id # system id, contains MAVLink's system ID field
uint8 component_id # subsystem / component id, contains MAVLink's component ID field
bool is_rotary_wing # True if system is in rotary wing configuration, so for a VTOL this is only true while flying as a multicopter
+
bool is_vtol # True if the system is VTOL capable
bool vtol_fw_permanent_stab # True if VTOL should stabilize attitude for fw in manual mode
bool in_transition_mode # True if VTOL is doing a transition
@@ -71,10 +72,13 @@ bool rc_signal_lost # true if RC reception lost
uint8 rc_input_mode # set to 1 to disable the RC input, 2 to enable manual control to RC in mapping.
bool data_link_lost # datalink to GCS lost
-bool high_latency_data_link_active # all low latency datalinks to GCS lost
uint8 data_link_lost_counter # counts unique data link lost events
+
+bool high_latency_data_link_lost # all low latency datalinks to GCS lost
+
bool engine_failure # Set to true if an engine failure is detected
bool mission_failure # Set to true if mission could not continue/finish
+
uint8 failure_detector_status # Bitmask containing FailureDetector status [0, 0, 0, 0, 0, FAILURE_ALT, FAILURE_PITCH, FAILURE_ROLL]
# see SYS_STATUS mavlink message for the following
diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp
index 36534f4d80..6959b8e98f 100644
--- a/src/modules/commander/Commander.cpp
+++ b/src/modules/commander/Commander.cpp
@@ -559,11 +559,42 @@ Commander::Commander() :
_auto_disarm_landed.set_hysteresis_time_from(false, 10_s);
_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;
+ }
+
+ // We want to accept RC inputs as default
+ status.rc_input_mode = vehicle_status_s::RC_IN_MODE_DEFAULT;
+ internal_state.main_state = commander_state_s::MAIN_STATE_MANUAL;
+ internal_state.timestamp = hrt_absolute_time();
+ status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
+ status.arming_state = vehicle_status_s::ARMING_STATE_INIT;
+
+ /* mark all signals lost as long as they haven't been found */
+ status.rc_signal_lost = true;
+ status_flags.offboard_control_signal_lost = true;
+ status.data_link_lost = true;
+
+ status.timestamp = hrt_absolute_time();
+
+ status_flags.condition_power_input_valid = true;
+ status_flags.rc_calibration_valid = true;
}
Commander::~Commander()
{
orb_unsubscribe(_battery_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);
+ }
}
bool
@@ -1004,16 +1035,8 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
break;
case vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY: {
- bool hl_exists = false;
-
- for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
- if (_telemetry[i].high_latency) {
- hl_exists = true;
- }
- }
-
// if no high latency telemetry exists send a failed acknowledge
- if (!hl_exists) {
+ if (_high_latency_datalink_heartbeat > commander_boot_timestamp) {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_FAILED;
mavlink_log_critical(&mavlink_log_pub, "Control high latency failed, no hl telemetry available");
}
@@ -1161,15 +1184,8 @@ Commander::run()
param_t _param_sys_type = param_find("MAV_TYPE");
param_t _param_system_id = param_find("MAV_SYS_ID");
param_t _param_component_id = param_find("MAV_COMP_ID");
- param_t _param_enable_datalink_loss = param_find("NAV_DLL_ACT");
param_t _param_offboard_loss_act = param_find("COM_OBL_ACT");
param_t _param_offboard_loss_rc_act = param_find("COM_OBL_RC_ACT");
- param_t _param_enable_rc_loss = param_find("NAV_RCL_ACT");
- param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T");
- param_t _param_highlatencydatalink_loss_timeout = param_find("COM_HLDL_LOSS_T");
- param_t _param_rc_loss_timeout = param_find("COM_RC_LOSS_T");
- param_t _param_datalink_regain_timeout = param_find("COM_DL_REG_T");
- param_t _param_highlatencydatalink_regain_timeout = param_find("COM_HLDL_REG_T");
param_t _param_ef_throttle_thres = param_find("COM_EF_THROT");
param_t _param_ef_current2throttle_thres = param_find("COM_EF_C2T");
param_t _param_ef_time_thres = param_find("COM_EF_TIME");
@@ -1225,42 +1241,16 @@ Commander::run()
PX4_ERR("Failed to register power notification callback");
}
- // We want to accept RC inputs as default
- status.rc_input_mode = vehicle_status_s::RC_IN_MODE_DEFAULT;
- internal_state.main_state = commander_state_s::MAIN_STATE_MANUAL;
- internal_state.timestamp = hrt_absolute_time();
- status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
- status.arming_state = vehicle_status_s::ARMING_STATE_INIT;
-
- /* mark all signals lost as long as they haven't been found */
- status.rc_signal_lost = true;
- status_flags.offboard_control_signal_lost = true;
- status.data_link_lost = true;
-
- status.timestamp = hrt_absolute_time();
-
- status_flags.condition_power_input_valid = true;
- status_flags.rc_calibration_valid = true;
-
get_circuit_breaker_params();
/* publish initial state */
_status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
- if (_status_pub == nullptr) {
- warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n");
- warnx("exiting.");
- px4_task_exit(PX4_ERROR);
- }
-
- /* Initialize armed with all false */
- memset(&armed, 0, sizeof(armed));
/* armed topic */
orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
hrt_abstime last_disarmed_timestamp = 0;
/* vehicle control mode topic */
- memset(&control_mode, 0, sizeof(control_mode));
orb_advert_t control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode);
/* command ack */
@@ -1281,54 +1271,24 @@ Commander::run()
bool updated = false;
- /* Subscribe to safety topic */
- int safety_sub = orb_subscribe(ORB_ID(safety));
- memset(&safety, 0, sizeof(safety));
- safety.safety_switch_available = false;
- safety.safety_off = false;
-
- /* Subscribe to geofence result topic */
+ int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
+ int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
+ int cpuload_sub = orb_subscribe(ORB_ID(cpuload));
int geofence_result_sub = orb_subscribe(ORB_ID(geofence_result));
- struct geofence_result_s geofence_result;
- memset(&geofence_result, 0, sizeof(geofence_result));
-
- /* Subscribe to manual control data */
- int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
- memset(&sp_man, 0, sizeof(sp_man));
-
- /* Subscribe to offboard control data */
- int offboard_control_mode_sub = orb_subscribe(ORB_ID(offboard_control_mode));
- memset(&offboard_control_mode, 0, sizeof(offboard_control_mode));
-
- /* Subscribe to land detector */
int land_detector_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
- land_detector.landed = true;
-
- /* Subscribe to command topic */
- int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
-
- /* Subscribe to parameters changed topic */
+ int offboard_control_mode_sub = orb_subscribe(ORB_ID(offboard_control_mode));
int param_changed_sub = orb_subscribe(ORB_ID(parameter_update));
-
- /* Subscribe to subsystem info topic */
+ int safety_sub = orb_subscribe(ORB_ID(safety));
+ int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int subsys_sub = orb_subscribe(ORB_ID(subsystem_info));
- struct subsystem_info_s info;
- memset(&info, 0, sizeof(info));
-
- /* Subscribe to system power */
int system_power_sub = orb_subscribe(ORB_ID(system_power));
+ int vtol_vehicle_status_sub = orb_subscribe(ORB_ID(vtol_vehicle_status));
- /* Subscribe to actuator controls (outputs) */
- int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
+ struct geofence_result_s geofence_result {};
- /* Subscribe to vtol vehicle status topic */
- int vtol_vehicle_status_sub = orb_subscribe(ORB_ID(vtol_vehicle_status));
- //struct vtol_vehicle_status_s vtol_status;
- memset(&vtol_status, 0, sizeof(vtol_status));
- vtol_status.vtol_in_rw_mode = true; //default for vtol is rotary wing
+ land_detector.landed = true;
- int cpuload_sub = orb_subscribe(ORB_ID(cpuload));
- memset(&cpuload, 0, sizeof(cpuload));
+ vtol_status.vtol_in_rw_mode = true; // default for vtol is rotary wing
control_status_leds(&status, &armed, true, _battery_warning, &cpuload);
@@ -1371,20 +1331,11 @@ Commander::run()
param_get(_param_rc_arm_hyst, &rc_arm_hyst);
rc_arm_hyst *= COMMANDER_MONITORING_LOOPSPERMSEC;
- int32_t datalink_loss_act = 0;
- int32_t rc_loss_act = 0;
- int32_t datalink_loss_timeout = 10;
- int32_t highlatencydatalink_loss_timeout = 120;
- float rc_loss_timeout = 0.5;
- int32_t datalink_regain_timeout = 0;
- int32_t highlatencydatalink_regain_timeout = 0;
float offboard_loss_timeout = 0.0f;
int32_t offboard_loss_act = 0;
int32_t offboard_loss_rc_act = 0;
int32_t posctl_nav_loss_act = 0;
-
int32_t geofence_action = 0;
-
int32_t flight_uuid = 0;
int32_t airmode = 0;
int32_t rc_map_arm_switch = 0;
@@ -1479,11 +1430,6 @@ Commander::run()
}
/* Safety parameters */
- param_get(_param_enable_datalink_loss, &datalink_loss_act);
- param_get(_param_enable_rc_loss, &rc_loss_act);
- param_get(_param_datalink_loss_timeout, &datalink_loss_timeout);
- param_get(_param_highlatencydatalink_loss_timeout, &highlatencydatalink_loss_timeout);
- param_get(_param_rc_loss_timeout, &rc_loss_timeout);
param_get(_param_rc_in_off, &rc_in_off);
status.rc_input_mode = rc_in_off;
param_get(_param_rc_arm_hyst, &rc_arm_hyst);
@@ -1492,8 +1438,6 @@ Commander::run()
// percentage (* 0.01) needs to be doubled because RC total interval is 2, not 1
min_stick_change *= 0.02f;
rc_arm_hyst *= COMMANDER_MONITORING_LOOPSPERMSEC;
- param_get(_param_datalink_regain_timeout, &datalink_regain_timeout);
- param_get(_param_highlatencydatalink_regain_timeout, &highlatencydatalink_regain_timeout);
param_get(_param_ef_throttle_thres, &ef_throttle_thres);
param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres);
param_get(_param_ef_time_thres, &ef_time_thres);
@@ -1594,9 +1538,6 @@ Commander::run()
}
}
- // poll the telemetry status
- poll_telemetry_status();
-
orb_check(system_power_sub, &updated);
if (updated) {
@@ -1800,6 +1741,7 @@ Commander::run()
orb_check(subsys_sub, &updated);
if (updated) {
+ subsystem_info_s info{};
orb_copy(ORB_ID(subsystem_info), subsys_sub, &info);
set_health_flags(info.subsystem_type, info.present, info.enabled, info.ok, status);
status_changed = true;
@@ -2003,7 +1945,7 @@ Commander::run()
/* RC input check */
if (!status_flags.rc_input_blocked && sp_man.timestamp != 0 &&
- (hrt_elapsed_time(&sp_man.timestamp) < (rc_loss_timeout * 1_s))) {
+ (hrt_elapsed_time(&sp_man.timestamp) < (_rc_loss_threshold.get() * 1_s))) {
/* handle the case where RC signal was regained */
if (!status_flags.rc_signal_found_once) {
@@ -2180,8 +2122,7 @@ Commander::run()
}
// data link checks which update the status
- data_link_checks(highlatencydatalink_loss_timeout, highlatencydatalink_regain_timeout, datalink_loss_timeout,
- datalink_regain_timeout, &status_changed);
+ data_link_check(status_changed);
// engine failure detection
// TODO: move out of commander
@@ -2421,12 +2362,12 @@ Commander::run()
&armed,
&internal_state,
&mavlink_log_pub,
- (link_loss_actions_t)datalink_loss_act,
+ (link_loss_actions_t)_datalink_loss_action.get(),
_mission_result_sub.get().finished,
_mission_result_sub.get().stay_in_failsafe,
status_flags,
land_detector.landed,
- (link_loss_actions_t)rc_loss_act,
+ (link_loss_actions_t)_rc_loss_action.get(),
offboard_loss_act,
offboard_loss_rc_act,
posctl_nav_loss_act);
@@ -3888,203 +3829,121 @@ bool Commander::preflight_check(bool report)
return success;
}
-void Commander::poll_telemetry_status()
+void Commander::data_link_check(bool &status_changed)
{
- bool updated = false;
-
for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
- if (_telemetry[i].subscriber < 0) {
- _telemetry[i].subscriber = orb_subscribe_multi(ORB_ID(telemetry_status), i);
+ 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[i].subscriber, &updated);
-
- if (updated) {
- telemetry_status_s telemetry = {};
- orb_copy(ORB_ID(telemetry_status), _telemetry[i].subscriber, &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[i].last_heartbeat == 0 || (hrt_elapsed_time(&_telemetry[i].last_heartbeat) > 3_s)
- || !_telemetry[i].preflight_checks_reported) &&
- /* and this link has a communication partner */
- (telemetry.heartbeat_time > 0) &&
- /* and it is still connected */
- (hrt_elapsed_time(&telemetry.heartbeat_time) < 2_s) &&
- /* and the system is not already armed (and potentially flying) */
- !armed.armed) {
+ bool updated = false;
- /* flag the checks as reported for this link when we actually report them */
- _telemetry[i].preflight_checks_reported = status_flags.condition_system_hotplug_timeout;
+ orb_check(_telemetry_status_sub[i], &updated);
- preflight_check(true);
-
- // Provide feedback on mission state
- const mission_result_s &mission_result = _mission_result_sub.get();
-
- if ((mission_result.timestamp > commander_boot_timestamp) && status_flags.condition_system_hotplug_timeout &&
- (mission_result.instance_count > 0) && !mission_result.valid) {
-
- mavlink_log_critical(&mavlink_log_pub, "Planned mission fails check. Please upload again.");
- //set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MISSION, true, true, false, status); // TODO
-
- } else {
- //set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MISSION, true, true, true, status); // TODO
- }
- }
-
- /* 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) {
- status_flags.usb_connected = true;
- }
+ if (updated) {
+ telemetry_status_s telemetry;
- /* set latency type of the telemetry connection */
- if (telemetry.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_IRIDIUM) {
- _telemetry[i].high_latency = true;
+ if (orb_copy(ORB_ID(telemetry_status), _telemetry_status_sub[i], &telemetry) == PX4_OK) {
- } else {
- _telemetry[i].high_latency = false;
- }
-
- if (telemetry.heartbeat_time > 0 && (_telemetry[i].last_heartbeat < telemetry.heartbeat_time)) {
- _telemetry[i].last_heartbeat = telemetry.heartbeat_time;
- }
- }
+ // 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;
- // for iridium telemetry use the iridiumsbd_status to update the heartbeat
- if (_telemetry[i].high_latency) {
- if (_iridiumsbd_status_sub.update()) {
- const hrt_abstime isbd_timestamp = _iridiumsbd_status_sub.get().last_heartbeat;
+ case (telemetry_status_s::LINK_TYPE_IRIDIUM):
- if (isbd_timestamp > _telemetry[i].last_heartbeat) {
- _telemetry[i].last_heartbeat = isbd_timestamp;
- }
- }
- }
- }
-}
+ // lazily subscribe
+ if (orb_exists(ORB_ID(iridiumsbd_status), 0) == PX4_OK) {
+ _iridiumsbd_status_sub = orb_subscribe(ORB_ID(iridiumsbd_status));
+ }
-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;
+ if (_iridiumsbd_status_sub >= 0) {
+ bool iridiumsbd_updated = false;
+ orb_check(_iridiumsbd_status_sub, &iridiumsbd_updated);
- for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
- if (_telemetry[i].high_latency) {
- high_latency_link_exists = true;
+ if (iridiumsbd_updated) {
+ iridiumsbd_status_s iridium_status;
- if (status.high_latency_data_link_active) {
- dl_loss_timeout_local = highlatencydatalink_loss_timeout;
- dl_regain_timeout_local = highlatencydatalink_regain_timeout;
+ if (orb_copy(ORB_ID(iridiumsbd_status), _iridiumsbd_status_sub, &iridium_status) == PX4_OK) {
+ _high_latency_datalink_heartbeat = iridium_status.last_heartbeat;
- } 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;
- }
+ 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;
+ }
+ }
- } else {
- dl_loss_timeout_local = datalink_loss_timeout;
- dl_regain_timeout_local = datalink_regain_timeout;
- }
-
- if (_telemetry[i].last_heartbeat != 0 &&
- hrt_elapsed_time(&_telemetry[i].last_heartbeat) < 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[i].lost &&
- hrt_elapsed_time(&_telemetry[i].last_dl_loss) > dl_regain_timeout_local * 1e6) {
-
- /* report a regain */
- if (_telemetry[i].last_dl_loss > 0) {
- mavlink_and_console_log_info(&mavlink_log_pub, "data link #%i regained", i);
+ }
+ }
+ }
- } else if (_telemetry[i].last_dl_loss == 0) {
- /* new link */
+ break;
}
- /* got link again or new */
- *status_changed = true;
- _telemetry[i].lost = false;
- have_link = true;
+ // handle different remote types
+ switch (telemetry.remote_type) {
+ case (telemetry_status_s::MAV_TYPE_GCS):
+ _datalink_last_heartbeat_gcs = telemetry.heartbeat_time;
- if (!_telemetry[i].high_latency) {
- have_low_latency_link = true;
- }
+ if (status.data_link_lost) {
+ if (hrt_elapsed_time(&_datalink_lost) > (_datalink_regain_threshold.get() * 1_s)) {
+ status.data_link_lost = false;
+ status_changed = true;
+ }
+ }
- } else if (!_telemetry[i].lost) {
- /* telemetry was healthy also in last iteration
- * we don't have to check a timeout */
- have_link = true;
+ break;
- if (!_telemetry[i].high_latency) {
- have_low_latency_link = true;
+ case (telemetry_status_s::MAV_TYPE_ONBOARD_CONTROLLER):
+ _datalink_last_heartbeat_onboard_controller = telemetry.heartbeat_time;
+ // TODO: FYI @baumanta
+ break;
}
}
+ }
+ }
- } else {
+ // GCS data link loss failsafe
+ if ((_datalink_last_heartbeat_gcs > 0) &&
+ (hrt_elapsed_time(&_datalink_last_heartbeat_gcs) > (_datalink_loss_threshold.get() * 1_s))) {
- if (!_telemetry[i].lost) {
- /* only reset the timestamp to a different time on state change */
- _telemetry[i].last_dl_loss = hrt_absolute_time();
+ _datalink_lost = hrt_absolute_time();
- mavlink_and_console_log_info(&mavlink_log_pub, "data link #%i lost", i);
- _telemetry[i].lost = true;
- }
- }
- }
+ if (!status.data_link_lost) {
+ status.data_link_lost = true;
+ status.data_link_lost_counter++;
- if (have_link) {
- /* handle the case where data link was regained */
- if (status.data_link_lost) {
- status.data_link_lost = false;
- *status_changed = true;
- }
+ mavlink_log_critical(&mavlink_log_pub, "DATA LINK LOST");
- 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 LAT LINKS REGAINED, DEACTIVATE HIGH LAT LINK");
+ status_changed = true;
}
+ }
- } 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;
-
- // set heartbeat to current time for high latency so that the first message can be transmitted
- for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
- if (_telemetry[i].high_latency) {
- _telemetry[i].last_heartbeat = hrt_absolute_time();
- }
- }
-
- if (!status.data_link_lost) {
- mavlink_log_critical(&mavlink_log_pub, "ALL LOW LAT LINKS LOST, ACTIVATE HIGH LAT LINK");
+ // ONBOARD CONTROLLER data link loss failsafe (hard coded 5 seconds)
+ // only issue a periodic warning for now
+ if ((_datalink_last_heartbeat_onboard_controller > 0)
+ && (hrt_elapsed_time(&_datalink_last_heartbeat_onboard_controller) > 5_s) &&
+ (hrt_elapsed_time(&_onboard_controller_lost) > 5_s)) {
- } else {
- mavlink_log_critical(&mavlink_log_pub, "ACTIVATING AVAILABLE HIGH LATENCY LINK");
- }
+ _onboard_controller_lost = hrt_absolute_time();
+ mavlink_log_critical(&mavlink_log_pub, "ONBOARD CONTROLLER LOST");
+ }
- } else if (!status.data_link_lost) {
- if (armed.armed) {
- mavlink_log_critical(&mavlink_log_pub, "ALL DATA LINKS LOST");
- }
+ // high latency data link loss failsafe
+ if (hrt_elapsed_time(&_high_latency_datalink_heartbeat) > (_high_latency_datalink_loss_threshold.get() * 1_s)) {
+ _high_latency_datalink_lost = hrt_absolute_time();
- status.data_link_lost = true;
- status.data_link_lost_counter++;
- *status_changed = true;
+ if (!status.high_latency_data_link_lost) {
+ status.high_latency_data_link_lost = true;
+ mavlink_log_critical(&mavlink_log_pub, "HIGH LATENCY DATA LINK LOST");
+ status_changed = true;
}
}
}
diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp
index f0d1daed82..f6a8f4013d 100644
--- a/src/modules/commander/Commander.hpp
+++ b/src/modules/commander/Commander.hpp
@@ -93,6 +93,17 @@ public:
private:
DEFINE_PARAMETERS(
+
+ (ParamInt<px4::params::NAV_DLL_ACT>) _datalink_loss_action,
+ (ParamInt<px4::params::COM_DL_LOSS_T>) _datalink_loss_threshold,
+ (ParamInt<px4::params::COM_DL_REG_T>) _datalink_regain_threshold,
+
+ (ParamInt<px4::params::COM_HLDL_LOSS_T>) _high_latency_datalink_loss_threshold,
+ (ParamInt<px4::params::COM_HLDL_REG_T>) _high_latency_datalink_regain_threshold,
+
+ (ParamInt<px4::params::NAV_RCL_ACT>) _rc_loss_action,
+ (ParamFloat<px4::params::COM_RC_LOSS_T>) _rc_loss_threshold,
+
(ParamFloat<px4::params::COM_HOME_H_T>) _home_eph_threshold,
(ParamFloat<px4::params::COM_HOME_V_T>) _home_epv_threshold,
@@ -155,27 +166,21 @@ private:
void mission_init();
- /**
- * Update the telemetry status and the corresponding status variables.
- * Perform system checks when new telemetry link connected.
- */
- void poll_telemetry_status();
-
/**
* Checks the status of all available data links and handles switching between different system telemetry states.
*/
- void 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);
-
- // telemetry variables
- struct telemetry_data {
- int subscriber = -1;
- uint64_t last_heartbeat = 0u;
- uint64_t last_dl_loss = 0u;
- bool preflight_checks_reported = false;
- bool lost = true;
- bool high_latency = false;
- } _telemetry[ORB_MULTI_MAX_INSTANCES];
+ void data_link_check(bool &status_changed);
+ int _telemetry_status_sub[ORB_MULTI_MAX_INSTANCES] {};
+
+ uint64_t _datalink_last_heartbeat_gcs{0};
+ uint64_t _datalink_lost{0};
+
+ uint64_t _datalink_last_heartbeat_onboard_controller{0};
+ uint64_t _onboard_controller_lost{0};
+
+ int _iridiumsbd_status_sub{-1};
+ uint64_t _high_latency_datalink_heartbeat{0};
+ uint64_t _high_latency_datalink_lost{0};
void estimator_check(bool *status_changed);
@@ -190,7 +195,6 @@ private:
// Subscriptions
Subscription<estimator_status_s> _estimator_status_sub{ORB_ID(estimator_status)};
- Subscription<iridiumsbd_status_s> _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
Subscription<mission_result_s> _mission_result_sub{ORB_ID(mission_result)};
Subscription<vehicle_global_position_s> _global_position_sub{ORB_ID(vehicle_global_position)};
Subscription<vehicle_local_position_s> _local_position_sub{ORB_ID(vehicle_local_position)};
diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
index 307be18fdb..cda46920ba 100644
--- a/src/modules/commander/commander_params.c
+++ b/src/modules/commander/commander_params.c
@@ -769,3 +769,45 @@ PARAM_DEFINE_INT32(COM_FLIGHT_UUID, 0);
* @group Mission
*/
PARAM_DEFINE_INT32(COM_TAKEOFF_ACT, 0);
+
+/**
+ * Set data link loss failsafe mode
+ *
+ * The data link loss failsafe will only be entered after a timeout,
+ * set by COM_DL_LOSS_T in seconds. Once the timeout occurs the selected
+ * action will be executed. Setting this parameter to 4 will enable CASA
+ * Outback Challenge rules, which are only recommended to participants
+ * of that competition.
+ *
+ * @value 0 Disabled
+ * @value 1 Hold mode
+ * @value 2 Return mode
+ * @value 3 Land mode
+ * @value 4 Data Link Auto Recovery (CASA Outback Challenge rules)
+ * @value 5 Terminate
+ * @value 6 Lockdown
+ *
+ * @group Mission
+ */
+PARAM_DEFINE_INT32(NAV_DLL_ACT, 0);
+
+/**
+ * Set RC loss failsafe mode
+ *
+ * The RC loss failsafe will only be entered after a timeout,
+ * set by COM_RC_LOSS_T in seconds. If RC input checks have been disabled
+ * by setting the COM_RC_IN_MODE param it will not be triggered.
+ * Setting this parameter to 4 will enable CASA Outback Challenge rules,
+ * which are only recommended to participants of that competition.
+ *
+ * @value 0 Disabled
+ * @value 1 Hold mode
+ * @value 2 Return mode
+ * @value 3 Land mode
+ * @value 4 RC Auto Recovery (CASA Outback Challenge rules)
+ * @value 5 Terminate
+ * @value 6 Lockdown
+ *
+ * @group Mission
+ */
+PARAM_DEFINE_INT32(NAV_RCL_ACT, 2);
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 72891476c7..ef408ac293 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -834,48 +834,63 @@ void set_link_loss_nav_state(vehicle_status_s *status, actuator_armed_s *armed,
uint8_t auto_recovery_nav_state)
{
// do the best you can according to the action set
- if (link_loss_act == link_loss_actions_t::AUTO_RECOVER
- && status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) {
- status->nav_state = auto_recovery_nav_state;
- } else if (link_loss_act == link_loss_actions_t::AUTO_LOITER && status_flags.condition_global_position_valid) {
- status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
-
- } else if (link_loss_act == link_loss_actions_t::AUTO_RTL
- && status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) {
-
- main_state_transition(*status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, internal_state);
- status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
-
- } else if (link_loss_act == link_loss_actions_t::AUTO_LAND && status_flags.condition_local_position_valid) {
- status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
-
- } else if (link_loss_act == link_loss_actions_t::TERMINATE) {
- status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
- armed->force_failsafe = true;
+ switch (link_loss_act) {
+ case (link_loss_actions_t::DISABLED):
+ case (link_loss_actions_t::AUTO_RECOVER):
+ if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) {
+ status->nav_state = auto_recovery_nav_state;
+ return;
+ }
- } else if (link_loss_act == link_loss_actions_t::LOCKDOWN) {
- armed->lockdown = true;
+ // FALLTHROUGH
+ case (link_loss_actions_t::AUTO_RTL):
+ if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) {
+ main_state_transition(*status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, internal_state);
+ status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
+ return;
+ }
- // do the best you can according to the current state
+ // FALLTHROUGH
+ case (link_loss_actions_t::AUTO_LOITER):
+ if (status_flags.condition_global_position_valid) {
+ status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
+ return;
+ }
- } else if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) {
- status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
+ // FALLTHROUGH
+ case (link_loss_actions_t::AUTO_LAND):
+ if (status_flags.condition_global_position_valid) {
+ status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
+ return;
- } else if (status_flags.condition_local_position_valid) {
- status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
+ } else {
+ if (status->is_rotary_wing) {
+ if (status_flags.condition_local_position_valid) {
+ status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
+ return;
- } else if (status_flags.condition_local_altitude_valid) {
- if (status->is_rotary_wing) {
- status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+ } else if (status_flags.condition_local_altitude_valid) {
+ status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+ return;
+ }
- } else {
- // TODO: FW position controller doesn't run without condition_global_position_valid
- status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
+ } else {
+ // TODO: FW position controller doesn't run without condition_global_position_valid
+ status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
+ return;
+ }
}
- } else {
+ // FALLTHROUGH
+ case (link_loss_actions_t::TERMINATE):
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
+ armed->force_failsafe = true;
+ break;
+
+ case (link_loss_actions_t::LOCKDOWN):
+ armed->lockdown = true;
+ break;
}
}
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index 6a7839ce77..277600103a 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -60,12 +60,12 @@ typedef enum {
enum class link_loss_actions_t {
DISABLED = 0,
- AUTO_LOITER = 1,
- AUTO_RTL = 2,
- AUTO_LAND = 3,
- AUTO_RECOVER = 4,
- TERMINATE = 5,
- LOCKDOWN = 6,
+ AUTO_LOITER = 1, // Hold mode
+ AUTO_RTL = 2, // Return mode
+ AUTO_LAND = 3, // Land mode
+ AUTO_RECOVER = 4, // Data Link Auto Recovery (CASA Outback Challenge rules)
+ TERMINATE = 5, // Terminate
+ LOCKDOWN = 6, // Lockdown
};
typedef enum {
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 86cab64017..4f205362cd 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -699,7 +699,7 @@ Mavlink::mavlink_open_uart(int baud, const char *uart_name, bool force_flow_cont
/* USB has no baudrate, but use a magic number for 'fast' */
_baudrate = 2000000;
- set_telemetry_status_type(telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB);
+ set_telemetry_status_type(telemetry_status_s::LINK_TYPE_USB);
}
#if defined(__PX4_LINUX) || defined(__PX4_DARWIN) || defined(__PX4_CYGWIN)
@@ -1580,7 +1580,7 @@ Mavlink::update_rate_mult()
// check for RADIO_STATUS timeout and reset
if (hrt_elapsed_time(&_rstatus.timestamp) > 5_s) {
PX4_ERR("instance %d: RADIO_STATUS timeout", _instance_id);
- set_telemetry_status_type(telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_GENERIC);
+ set_telemetry_status_type(telemetry_status_s::LINK_TYPE_GENERIC);
_radio_status_available = false;
_radio_status_critical = false;
@@ -1601,7 +1601,7 @@ void
Mavlink::update_radio_status(const radio_status_s &radio_status)
{
_rstatus = radio_status;
- set_telemetry_status_type(telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO);
+ set_telemetry_status_type(telemetry_status_s::LINK_TYPE_3DR_RADIO);
/* check hardware limits */
_radio_status_available = true;
@@ -2022,7 +2022,7 @@ Mavlink::task_main(int argc, char *argv[])
} else if (strcmp(myoptarg, "iridium") == 0) {
_mode = MAVLINK_MODE_IRIDIUM;
- set_telemetry_status_type(telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_IRIDIUM);
+ set_telemetry_status_type(telemetry_status_s::LINK_TYPE_IRIDIUM);
} else if (strcmp(myoptarg, "minimal") == 0) {
_mode = MAVLINK_MODE_MINIMAL;
@@ -2233,14 +2233,16 @@ Mavlink::task_main(int argc, char *argv[])
set_manual_input_mode_generation(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_GENERATED);
if (_mode == MAVLINK_MODE_IRIDIUM) {
+
if (_transmitting_enabled &&
- !status.high_latency_data_link_active &&
+ status.high_latency_data_link_lost &&
!_transmitting_enabled_commanded &&
(_first_heartbeat_sent)) {
+
_transmitting_enabled = false;
mavlink_and_console_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s", _device_name);
- } else if (!_transmitting_enabled && status.high_latency_data_link_active) {
+ } else if (!_transmitting_enabled && !status.high_latency_data_link_lost) {
_transmitting_enabled = true;
mavlink_and_console_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s", _device_name);
}
@@ -2558,7 +2560,7 @@ void Mavlink::check_radio_config()
{
/* radio config check */
if (_uart_fd >= 0 && _param_radio_id.get() != 0
- && _tstatus.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO) {
+ && _tstatus.type == telemetry_status_s::LINK_TYPE_3DR_RADIO) {
/* request to configure radio and radio is present */
FILE *fs = fdopen(_uart_fd, "w");
@@ -2702,7 +2704,7 @@ Mavlink::display_status()
printf("\ttype:\t\t");
switch (_tstatus.type) {
- case telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO:
+ case telemetry_status_s::LINK_TYPE_3DR_RADIO:
printf("3DR RADIO\n");
printf("\t rssi:\t\t%d\n", _rstatus.rssi);
printf("\t remote rssi:\t%u\n", _rstatus.remote_rssi);
@@ -2713,7 +2715,7 @@ Mavlink::display_status()
printf("\t fixed:\t%u\n", _rstatus.fix);
break;
- case telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB:
+ case telemetry_status_s::LINK_TYPE_USB:
printf("USB CDC\n");
break;
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index ef54625ecc..716c1791ff 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -1882,17 +1882,19 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
mavlink_heartbeat_t hb;
mavlink_msg_heartbeat_decode(msg, &hb);
- /* ignore own heartbeats, accept only heartbeats from GCS */
- if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) {
+ // ignore own heartbeats
+ if ((msg->sysid != mavlink_system.sysid) && (msg->compid != mavlink_system.compid)) {
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 = tstatus.timestamp;
- tstatus.system_id = msg->sysid;
- tstatus.component_id = msg->compid;
+ 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;
}
}
}
diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c
index c7012bc440..a1e042c46d 100644
--- a/src/modules/navigator/navigator_params.c
+++ b/src/modules/navigator/navigator_params.c
@@ -111,48 +111,6 @@ PARAM_DEFINE_FLOAT(NAV_FW_ALTL_RAD, 5.0f);
*/
PARAM_DEFINE_FLOAT(NAV_MC_ALT_RAD, 0.8f);
-/**
- * Set data link loss failsafe mode
- *
- * The data link loss failsafe will only be entered after a timeout,
- * set by COM_DL_LOSS_T in seconds. Once the timeout occurs the selected
- * action will be executed. Setting this parameter to 4 will enable CASA
- * Outback Challenge rules, which are only recommended to participants
- * of that competition.
- *
- * @value 0 Disabled
- * @value 1 Hold mode
- * @value 2 Return mode
- * @value 3 Land mode
- * @value 4 Data Link Auto Recovery (CASA Outback Challenge rules)
- * @value 5 Terminate
- * @value 6 Lockdown
- *
- * @group Mission
- */
-PARAM_DEFINE_INT32(NAV_DLL_ACT, 0);
-
-/**
- * Set RC loss failsafe mode
- *
- * The RC loss failsafe will only be entered after a timeout,
- * set by COM_RC_LOSS_T in seconds. If RC input checks have been disabled
- * by setting the COM_RC_IN_MODE param it will not be triggered.
- * Setting this parameter to 4 will enable CASA Outback Challenge rules,
- * which are only recommended to participants of that competition.
- *
- * @value 0 Disabled
- * @value 1 Hold mode
- * @value 2 Return mode
- * @value 3 Land mode
- * @value 4 RC Auto Recovery (CASA Outback Challenge rules)
- * @value 5 Terminate
- * @value 6 Lockdown
- *
- * @group Mission
- */
-PARAM_DEFINE_INT32(NAV_RCL_ACT, 2);
-
/**
* Set traffic avoidance mode
*
--
GitLab