mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-21 15:57:36 +08:00
HEARTBEAT and commander failsafe handling cleanup
This commit is contained in:
Daniel Agar
+33
-10
@@ -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
|
||||
|
||||
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
|
||||
# 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
|
||||
|
||||
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
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
+137
-278
@@ -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));
|
||||
int offboard_control_mode_sub = orb_subscribe(ORB_ID(offboard_control_mode));
|
||||
int param_changed_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||
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));
|
||||
int system_power_sub = orb_subscribe(ORB_ID(system_power));
|
||||
int vtol_vehicle_status_sub = orb_subscribe(ORB_ID(vtol_vehicle_status));
|
||||
|
||||
struct geofence_result_s geofence_result {};
|
||||
|
||||
land_detector.landed = true;
|
||||
|
||||
/* Subscribe to command topic */
|
||||
int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
|
||||
|
||||
/* Subscribe to parameters changed topic */
|
||||
int param_changed_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||
|
||||
/* Subscribe to subsystem info topic */
|
||||
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));
|
||||
|
||||
/* Subscribe to actuator controls (outputs) */
|
||||
int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
|
||||
|
||||
/* 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
|
||||
|
||||
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);
|
||||
bool updated = false;
|
||||
|
||||
orb_check(_telemetry_status_sub[i], &updated);
|
||||
|
||||
if (updated) {
|
||||
telemetry_status_s telemetry = {};
|
||||
orb_copy(ORB_ID(telemetry_status), _telemetry[i].subscriber, &telemetry);
|
||||
telemetry_status_s 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) {
|
||||
if (orb_copy(ORB_ID(telemetry_status), _telemetry_status_sub[i], &telemetry) == PX4_OK) {
|
||||
|
||||
/* flag the checks as reported for this link when we actually report them */
|
||||
_telemetry[i].preflight_checks_reported = status_flags.condition_system_hotplug_timeout;
|
||||
// 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;
|
||||
|
||||
preflight_check(true);
|
||||
case (telemetry_status_s::LINK_TYPE_IRIDIUM):
|
||||
|
||||
// Provide feedback on mission state
|
||||
const mission_result_s &mission_result = _mission_result_sub.get();
|
||||
// lazily subscribe
|
||||
if (orb_exists(ORB_ID(iridiumsbd_status), 0) == PX4_OK) {
|
||||
_iridiumsbd_status_sub = orb_subscribe(ORB_ID(iridiumsbd_status));
|
||||
}
|
||||
|
||||
if ((mission_result.timestamp > commander_boot_timestamp) && status_flags.condition_system_hotplug_timeout &&
|
||||
(mission_result.instance_count > 0) && !mission_result.valid) {
|
||||
if (_iridiumsbd_status_sub >= 0) {
|
||||
bool iridiumsbd_updated = false;
|
||||
orb_check(_iridiumsbd_status_sub, &iridiumsbd_updated);
|
||||
|
||||
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
|
||||
if (iridiumsbd_updated) {
|
||||
iridiumsbd_status_s iridium_status;
|
||||
|
||||
} else {
|
||||
//set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MISSION, true, true, true, status); // TODO
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/* 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;
|
||||
}
|
||||
|
||||
/* set latency type of the telemetry connection */
|
||||
if (telemetry.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_IRIDIUM) {
|
||||
_telemetry[i].high_latency = true;
|
||||
// handle different remote types
|
||||
switch (telemetry.remote_type) {
|
||||
case (telemetry_status_s::MAV_TYPE_GCS):
|
||||
_datalink_last_heartbeat_gcs = telemetry.heartbeat_time;
|
||||
|
||||
} else {
|
||||
_telemetry[i].high_latency = false;
|
||||
}
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
if (telemetry.heartbeat_time > 0 && (_telemetry[i].last_heartbeat < telemetry.heartbeat_time)) {
|
||||
_telemetry[i].last_heartbeat = telemetry.heartbeat_time;
|
||||
}
|
||||
}
|
||||
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;
|
||||
|
||||
if (isbd_timestamp > _telemetry[i].last_heartbeat) {
|
||||
_telemetry[i].last_heartbeat = isbd_timestamp;
|
||||
case (telemetry_status_s::MAV_TYPE_ONBOARD_CONTROLLER):
|
||||
_datalink_last_heartbeat_onboard_controller = telemetry.heartbeat_time;
|
||||
// TODO: FYI @baumanta
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
// GCS data link loss failsafe
|
||||
if ((_datalink_last_heartbeat_gcs > 0) &&
|
||||
(hrt_elapsed_time(&_datalink_last_heartbeat_gcs) > (_datalink_loss_threshold.get() * 1_s))) {
|
||||
|
||||
for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
|
||||
if (_telemetry[i].high_latency) {
|
||||
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[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 */
|
||||
}
|
||||
|
||||
/* got link again or new */
|
||||
*status_changed = true;
|
||||
|
||||
_telemetry[i].lost = false;
|
||||
have_link = true;
|
||||
|
||||
if (!_telemetry[i].high_latency) {
|
||||
have_low_latency_link = true;
|
||||
}
|
||||
|
||||
} else if (!_telemetry[i].lost) {
|
||||
/* telemetry was healthy also in last iteration
|
||||
* we don't have to check a timeout */
|
||||
have_link = true;
|
||||
|
||||
if (!_telemetry[i].high_latency) {
|
||||
have_low_latency_link = true;
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
|
||||
if (!_telemetry[i].lost) {
|
||||
/* only reset the timestamp to a different time on state change */
|
||||
_telemetry[i].last_dl_loss = hrt_absolute_time();
|
||||
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "data link #%i lost", i);
|
||||
_telemetry[i].lost = 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 LAT LINKS REGAINED, DEACTIVATE HIGH LAT LINK");
|
||||
}
|
||||
|
||||
} 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");
|
||||
|
||||
} 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");
|
||||
}
|
||||
_datalink_lost = hrt_absolute_time();
|
||||
|
||||
if (!status.data_link_lost) {
|
||||
status.data_link_lost = true;
|
||||
status.data_link_lost_counter++;
|
||||
*status_changed = true;
|
||||
|
||||
mavlink_log_critical(&mavlink_log_pub, "DATA LINK LOST");
|
||||
|
||||
status_changed = true;
|
||||
}
|
||||
}
|
||||
|
||||
// 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)) {
|
||||
|
||||
_onboard_controller_lost = hrt_absolute_time();
|
||||
mavlink_log_critical(&mavlink_log_pub, "ONBOARD CONTROLLER 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();
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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);
|
||||
void data_link_check(bool &status_changed);
|
||||
int _telemetry_status_sub[ORB_MULTI_MAX_INSTANCES] {};
|
||||
|
||||
// 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];
|
||||
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)};
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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;
|
||||
|
||||
} else if (link_loss_act == link_loss_actions_t::LOCKDOWN) {
|
||||
armed->lockdown = true;
|
||||
|
||||
// do the best you can according to the current state
|
||||
|
||||
} else if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) {
|
||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
|
||||
|
||||
} else if (status_flags.condition_local_position_valid) {
|
||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
|
||||
|
||||
} else if (status_flags.condition_local_altitude_valid) {
|
||||
if (status->is_rotary_wing) {
|
||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
|
||||
|
||||
} else {
|
||||
// TODO: FW position controller doesn't run without condition_global_position_valid
|
||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
|
||||
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 {
|
||||
// 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;
|
||||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
// 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->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) {
|
||||
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;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -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 {
|
||||
|
||||
@@ -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;
|
||||
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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
|
||||
*
|
||||
|
||||
Reference in New Issue
Block a user