mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-02 05:04:08 +08:00
commander: cleanup mavlink system present and validity checks
- try to handle mavlink system validity and present checks more consistently - telemetry_status unify heartbeats and healthy flags across MAV_TYPE and MAV_COMPONENT (once it's outside of mavlink we don't care about the difference) - add gimbal
This commit is contained in:
Daniel Agar
Matthias Grob
parent
47fcdb1fdb
commit
e389d6ffbb
@ -37,25 +37,38 @@ float32 rx_message_lost_rate
|
||||
|
||||
uint64 HEARTBEAT_TIMEOUT_US = 1500000 # Heartbeat timeout 1.5 seconds
|
||||
|
||||
# Heartbeats per type
|
||||
bool heartbeat_type_antenna_tracker # MAV_TYPE_ANTENNA_TRACKER
|
||||
bool heartbeat_type_gcs # MAV_TYPE_GCS
|
||||
bool heartbeat_type_onboard_controller # MAV_TYPE_ONBOARD_CONTROLLER
|
||||
bool heartbeat_type_gimbal # MAV_TYPE_GIMBAL
|
||||
bool heartbeat_type_adsb # MAV_TYPE_ADSB
|
||||
bool heartbeat_type_camera # MAV_TYPE_CAMERA
|
||||
bool heartbeat_type_parachute # MAV_TYPE_PARACHUTE
|
||||
# Heartbeats
|
||||
bool heartbeat_adsb # MAV_TYPE_ADSB
|
||||
bool heartbeat_antenna_tracker # MAV_TYPE_ANTENNA_TRACKER
|
||||
bool heartbeat_battery # MAV_TYPE_BATTERY
|
||||
bool heartbeat_camera # MAV_TYPE_CAMERA
|
||||
bool heartbeat_gcs # MAV_TYPE_GCS
|
||||
bool heartbeat_gimbal # MAV_TYPE_GIMBAL
|
||||
bool heartbeat_log # MAV_COMP_ID_LOG
|
||||
bool heartbeat_obstacle_avoidance # MAV_COMP_ID_OBSTACLE_AVOIDANCE
|
||||
bool heartbeat_onboard_controller # MAV_TYPE_ONBOARD_CONTROLLER
|
||||
bool heartbeat_osd # MAV_COMP_ID_OSD
|
||||
bool heartbeat_pairing_manager # MAV_COMP_ID_PAIRING_MANAGER
|
||||
bool heartbeat_parachute # MAV_TYPE_PARACHUTE
|
||||
bool heartbeat_telemetry_radio # MAV_COMP_ID_TELEMETRY_RADIO
|
||||
bool heartbeat_uart_bridge # MAV_COMP_ID_UART_BRIDGE
|
||||
bool heartbeat_udp_bridge # MAV_COMP_ID_UDP_BRIDGE
|
||||
bool heartbeat_vio # MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY
|
||||
|
||||
# Heartbeats per component
|
||||
bool heartbeat_component_telemetry_radio # MAV_COMP_ID_TELEMETRY_RADIO
|
||||
bool heartbeat_component_log # MAV_COMP_ID_LOG
|
||||
bool heartbeat_component_osd # MAV_COMP_ID_OSD
|
||||
bool heartbeat_component_obstacle_avoidance # MAV_COMP_ID_OBSTACLE_AVOIDANCE
|
||||
bool heartbeat_component_vio # MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY
|
||||
bool heartbeat_component_pairing_manager # MAV_COMP_ID_PAIRING_MANAGER
|
||||
bool heartbeat_component_udp_bridge # MAV_COMP_ID_UDP_BRIDGE
|
||||
bool heartbeat_component_uart_bridge # MAV_COMP_ID_UART_BRIDGE
|
||||
|
||||
# Misc component health
|
||||
bool avoidance_system_healthy
|
||||
bool parachute_system_healthy
|
||||
# system status healthy
|
||||
bool system_healthy_adsb
|
||||
bool system_healthy_antenna_tracker
|
||||
bool system_healthy_battery
|
||||
bool system_healthy_camera
|
||||
bool system_healthy_gcs
|
||||
bool system_healthy_gimbal
|
||||
bool system_healthy_log
|
||||
bool system_healthy_obstacle_avoidance
|
||||
bool system_healthy_onboard_controller
|
||||
bool system_healthy_osd
|
||||
bool system_healthy_pairing_manager
|
||||
bool system_healthy_parachute
|
||||
bool system_healthy_telemetry_radio
|
||||
bool system_healthy_uart_bridge
|
||||
bool system_healthy_udp_bridge
|
||||
bool system_healthy_vio
|
||||
|
||||
@ -36,8 +36,16 @@ bool vtol_transition_failure # Set to true if vtol transi
|
||||
bool usb_connected # status of the USB power supply
|
||||
bool sd_card_detected_once # set to true if the SD card was detected
|
||||
|
||||
bool avoidance_system_required # Set to true if avoidance system is enabled via COM_OBS_AVOID parameter
|
||||
bool avoidance_system_valid # Status of the obstacle avoidance system
|
||||
bool system_required_obstacle_avoidance # Set to true if avoidance system is enabled via COM_OBS_AVOID parameter
|
||||
|
||||
bool parachute_system_present
|
||||
bool parachute_system_healthy
|
||||
bool system_present_gcs
|
||||
bool system_present_gimbal
|
||||
bool system_present_obstacle_avoidance
|
||||
bool system_present_onboard_controller
|
||||
bool system_present_parachute
|
||||
|
||||
bool system_valid_gcs
|
||||
bool system_valid_gimbal
|
||||
bool system_valid_obstacle_avoidance
|
||||
bool system_valid_onboard_controller
|
||||
bool system_valid_parachute
|
||||
|
||||
@ -48,14 +48,14 @@ bool PreFlightCheck::parachuteCheck(orb_advert_t *mavlink_log_pub, const bool re
|
||||
const bool parachute_required = param_com_parachute != 0;
|
||||
|
||||
if (parachute_required) {
|
||||
if (!status_flags.parachute_system_present) {
|
||||
if (!status_flags.system_present_parachute) {
|
||||
success = false;
|
||||
|
||||
if (report_fail) {
|
||||
mavlink_log_critical(mavlink_log_pub, "Fail: Parachute system missing");
|
||||
}
|
||||
|
||||
} else if (!status_flags.parachute_system_healthy) {
|
||||
} else if (!status_flags.system_valid_parachute) {
|
||||
success = false;
|
||||
|
||||
if (report_fail) {
|
||||
|
||||
@ -123,7 +123,7 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st
|
||||
prearm_ok = false;
|
||||
}
|
||||
|
||||
if (status_flags.avoidance_system_required && !status_flags.avoidance_system_valid) {
|
||||
if (status_flags.system_required_obstacle_avoidance && !status_flags.system_valid_obstacle_avoidance) {
|
||||
if (prearm_ok) {
|
||||
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Avoidance system not ready"); }
|
||||
}
|
||||
|
||||
@ -1901,7 +1901,7 @@ Commander::run()
|
||||
_status_changed = true;
|
||||
}
|
||||
|
||||
_status_flags.avoidance_system_required = _param_com_obs_avoid.get();
|
||||
_status_flags.system_required_obstacle_avoidance = _param_com_obs_avoid.get();
|
||||
|
||||
_arm_requirements.arm_authorization = _param_arm_auth_required.get();
|
||||
_arm_requirements.esc_check = _param_escs_checks_required.get();
|
||||
@ -1953,7 +1953,7 @@ Commander::run()
|
||||
}
|
||||
|
||||
/* Update OA parameter */
|
||||
_status_flags.avoidance_system_required = _param_com_obs_avoid.get();
|
||||
_status_flags.system_required_obstacle_avoidance = _param_com_obs_avoid.get();
|
||||
|
||||
#if defined(BOARD_HAS_POWER_CONTROL)
|
||||
|
||||
@ -3436,124 +3436,152 @@ void Commander::data_link_check()
|
||||
}
|
||||
}
|
||||
|
||||
if (telemetry.heartbeat_type_gcs) {
|
||||
// Initial connection or recovery from data link lost
|
||||
if (_status.data_link_lost) {
|
||||
_status.data_link_lost = false;
|
||||
if (telemetry.heartbeat_gcs) {
|
||||
if (!_status_flags.system_present_gcs && (_datalink_last_heartbeat_gcs != 0)) {
|
||||
_status_changed = true;
|
||||
|
||||
if (_datalink_last_heartbeat_gcs != 0) {
|
||||
mavlink_log_info(&_mavlink_log_pub, "Data link regained\t");
|
||||
events::send(events::ID("commander_dl_regained"), events::Log::Info, "Data link regained");
|
||||
}
|
||||
|
||||
if (!_armed.armed && !_status_flags.condition_calibration_enabled) {
|
||||
// make sure to report preflight check failures to a connecting GCS
|
||||
PreFlightCheck::preflightCheck(&_mavlink_log_pub, _status, _status_flags, true, false,
|
||||
hrt_elapsed_time(&_boot_timestamp));
|
||||
}
|
||||
mavlink_log_info(&_mavlink_log_pub, "Data link regained\t");
|
||||
events::send(events::ID("commander_dl_regained"), events::Log::Info, "Data link regained");
|
||||
}
|
||||
|
||||
_datalink_last_heartbeat_gcs = telemetry.timestamp;
|
||||
_status_flags.system_valid_gcs = telemetry.system_healthy_onboard_controller;
|
||||
_status_flags.system_present_gcs = true;
|
||||
|
||||
|
||||
// Initial connection or recovery from data link lost
|
||||
if (_status.data_link_lost) {
|
||||
_status.data_link_lost = false;
|
||||
}
|
||||
|
||||
if (!_armed.armed && !_status_flags.condition_calibration_enabled) {
|
||||
// make sure to report preflight check failures to a connecting GCS
|
||||
PreFlightCheck::preflightCheck(&_mavlink_log_pub, _status, _status_flags, true, false,
|
||||
hrt_elapsed_time(&_boot_timestamp));
|
||||
}
|
||||
}
|
||||
|
||||
if (telemetry.heartbeat_type_onboard_controller) {
|
||||
if (_onboard_controller_lost) {
|
||||
_onboard_controller_lost = false;
|
||||
if (telemetry.heartbeat_onboard_controller) {
|
||||
if (!_status_flags.system_present_onboard_controller && (_datalink_last_heartbeat_onboard_controller != 0)) {
|
||||
_status_changed = true;
|
||||
|
||||
if (_datalink_last_heartbeat_onboard_controller != 0) {
|
||||
mavlink_log_info(&_mavlink_log_pub, "Onboard controller regained\t");
|
||||
events::send(events::ID("commander_onboard_ctrl_regained"), events::Log::Info, "Onboard controller regained");
|
||||
}
|
||||
mavlink_log_info(&_mavlink_log_pub, "Onboard controller regained\t");
|
||||
events::send(events::ID("commander_onboard_ctrl_regained"), events::Log::Info, "Onboard controller regained");
|
||||
}
|
||||
|
||||
_datalink_last_heartbeat_onboard_controller = telemetry.timestamp;
|
||||
_status_flags.system_valid_onboard_controller = telemetry.system_healthy_onboard_controller;
|
||||
_status_flags.system_present_onboard_controller = true;
|
||||
}
|
||||
|
||||
if (telemetry.heartbeat_type_parachute) {
|
||||
if (_parachute_system_lost) {
|
||||
_parachute_system_lost = false;
|
||||
|
||||
if (_datalink_last_heartbeat_parachute_system != 0) {
|
||||
mavlink_log_info(&_mavlink_log_pub, "Parachute system regained\t");
|
||||
events::send(events::ID("commander_parachute_regained"), events::Log::Info, "Parachute system regained");
|
||||
}
|
||||
if (telemetry.heartbeat_gimbal) {
|
||||
if (!_status_flags.system_present_gimbal && (_datalink_last_heartbeat_gimbal != 0)) {
|
||||
_status_changed = true;
|
||||
mavlink_log_info(&_mavlink_log_pub, "Gimbal regained\t");
|
||||
events::send(events::ID("commander_gimbal_regained"), events::Log::Info, "Gimbal regained");
|
||||
}
|
||||
|
||||
bool healthy = telemetry.parachute_system_healthy;
|
||||
_datalink_last_heartbeat_gimbal = telemetry.timestamp;
|
||||
_status_flags.system_valid_gimbal = telemetry.system_healthy_gimbal;
|
||||
_status_flags.system_present_gimbal = true;
|
||||
}
|
||||
|
||||
if (telemetry.heartbeat_parachute) {
|
||||
if (!_status_flags.system_present_parachute && (_datalink_last_heartbeat_parachute_system != 0)) {
|
||||
_status_changed = true;
|
||||
mavlink_log_info(&_mavlink_log_pub, "Parachute system regained\t");
|
||||
events::send(events::ID("commander_parachute_regained"), events::Log::Info, "Parachute system regained");
|
||||
}
|
||||
|
||||
const bool healthy = telemetry.system_healthy_parachute;
|
||||
|
||||
_datalink_last_heartbeat_parachute_system = telemetry.timestamp;
|
||||
_status_flags.parachute_system_present = true;
|
||||
_status_flags.parachute_system_healthy = healthy;
|
||||
_status_flags.system_present_parachute = true;
|
||||
_status_flags.system_valid_parachute = healthy;
|
||||
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_PARACHUTE, true, true, healthy, _status);
|
||||
}
|
||||
|
||||
if (telemetry.heartbeat_component_obstacle_avoidance) {
|
||||
if (_avoidance_system_lost) {
|
||||
_avoidance_system_lost = false;
|
||||
if (telemetry.heartbeat_obstacle_avoidance) {
|
||||
if (!_status_flags.system_present_obstacle_avoidance && (_datalink_last_heartbeat_obstacle_avoidance_system != 0)) {
|
||||
_status_changed = true;
|
||||
mavlink_log_info(&_mavlink_log_pub, "Obstacle avoidance system regained\t");
|
||||
events::send(events::ID("commander_obstacle_avoidance_regained"), events::Log::Info,
|
||||
"Obstacle avoidance system regained");
|
||||
}
|
||||
|
||||
_datalink_last_heartbeat_avoidance_system = telemetry.timestamp;
|
||||
_status_flags.avoidance_system_valid = telemetry.avoidance_system_healthy;
|
||||
_datalink_last_heartbeat_obstacle_avoidance_system = telemetry.timestamp;
|
||||
_status_flags.system_valid_obstacle_avoidance = telemetry.system_healthy_obstacle_avoidance;
|
||||
_status_flags.system_present_obstacle_avoidance = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static constexpr uint64_t TIMEOUT_DEFAULT = 3_s;
|
||||
|
||||
// GCS data link loss failsafe
|
||||
if (!_status.data_link_lost) {
|
||||
if ((_datalink_last_heartbeat_gcs != 0)
|
||||
&& hrt_elapsed_time(&_datalink_last_heartbeat_gcs) > (_param_com_dl_loss_t.get() * 1_s)) {
|
||||
|
||||
_status.data_link_lost = true;
|
||||
_status.data_link_lost_counter++;
|
||||
// GCS (data link loss failsafe)
|
||||
if (_status_flags.system_present_gcs
|
||||
&& hrt_elapsed_time(&_datalink_last_heartbeat_gcs) > (_param_com_dl_loss_t.get() * 1_s)) {
|
||||
|
||||
mavlink_log_info(&_mavlink_log_pub, "Connection to ground station lost\t");
|
||||
events::send(events::ID("commander_gcs_lost"), {events::Log::Warning, events::LogInternal::Info},
|
||||
"Connection to ground station lost");
|
||||
_status_flags.system_present_gcs = false;
|
||||
_status_flags.system_valid_gcs = false;
|
||||
_status_changed = true;
|
||||
mavlink_log_info(&_mavlink_log_pub, "Connection to ground station lost\t");
|
||||
events::send(events::ID("commander_gcs_lost"), {events::Log::Warning, events::LogInternal::Info},
|
||||
"Connection to ground station lost");
|
||||
|
||||
_status_changed = true;
|
||||
}
|
||||
_status.data_link_lost = true;
|
||||
_status.data_link_lost_counter++;
|
||||
}
|
||||
|
||||
// ONBOARD CONTROLLER data link loss failsafe
|
||||
if ((_datalink_last_heartbeat_onboard_controller > 0)
|
||||
&& (hrt_elapsed_time(&_datalink_last_heartbeat_onboard_controller) > (_param_com_obc_loss_t.get() * 1_s))
|
||||
&& !_onboard_controller_lost) {
|
||||
// gimbal
|
||||
if (_status_flags.system_present_gimbal
|
||||
&& (hrt_elapsed_time(&_datalink_last_heartbeat_gimbal) > TIMEOUT_DEFAULT)) {
|
||||
|
||||
_status_flags.system_present_gimbal = false;
|
||||
_status_flags.system_valid_gimbal = false;
|
||||
_status_changed = true;
|
||||
mavlink_log_critical(&_mavlink_log_pub, "Connection to gimbal lost\t");
|
||||
events::send(events::ID("commander_gimbal_lost"), events::Log::Critical, "Connection to gimbal lost");
|
||||
}
|
||||
|
||||
// obstacle avoidance
|
||||
if (_status_flags.system_present_obstacle_avoidance
|
||||
&& (hrt_elapsed_time(&_datalink_last_heartbeat_obstacle_avoidance_system) > TIMEOUT_DEFAULT)) {
|
||||
|
||||
_status_flags.system_present_obstacle_avoidance = false;
|
||||
_status_flags.system_valid_obstacle_avoidance = false;
|
||||
_status_changed = true;
|
||||
mavlink_log_critical(&_mavlink_log_pub, "Connection to obstacle avoidance lost\t");
|
||||
events::send(events::ID("commander_obstacle_avoidance_lost"), events::Log::Critical,
|
||||
"Connection to obstacle avoidance lost");
|
||||
}
|
||||
|
||||
// onboard controller
|
||||
if (_status_flags.system_present_onboard_controller
|
||||
&& (hrt_elapsed_time(&_datalink_last_heartbeat_onboard_controller) > (_param_com_obc_loss_t.get() * 1_s))) {
|
||||
|
||||
_status_flags.system_present_onboard_controller = false;
|
||||
_status_flags.system_valid_onboard_controller = false;
|
||||
_status_changed = true;
|
||||
mavlink_log_critical(&_mavlink_log_pub, "Connection to mission computer lost\t");
|
||||
events::send(events::ID("commander_mission_comp_lost"), events::Log::Critical, "Connection to mission computer lost");
|
||||
_onboard_controller_lost = true;
|
||||
_status_changed = true;
|
||||
}
|
||||
|
||||
// Parachute system
|
||||
if ((hrt_elapsed_time(&_datalink_last_heartbeat_parachute_system) > 3_s)
|
||||
&& !_parachute_system_lost) {
|
||||
mavlink_log_critical(&_mavlink_log_pub, "Parachute system lost");
|
||||
_status_flags.parachute_system_present = false;
|
||||
_status_flags.parachute_system_healthy = false;
|
||||
_parachute_system_lost = true;
|
||||
// parachute
|
||||
if (_status_flags.system_present_parachute
|
||||
&& (hrt_elapsed_time(&_datalink_last_heartbeat_parachute_system) > TIMEOUT_DEFAULT)) {
|
||||
|
||||
_status_flags.system_present_parachute = false;
|
||||
_status_flags.system_valid_parachute = false;
|
||||
_status_changed = true;
|
||||
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_PARACHUTE, false, true, false, _status);
|
||||
}
|
||||
|
||||
// AVOIDANCE SYSTEM state check (only if it is enabled)
|
||||
if (_status_flags.avoidance_system_required && !_onboard_controller_lost) {
|
||||
// if heartbeats stop
|
||||
if (!_avoidance_system_lost && (_datalink_last_heartbeat_avoidance_system > 0)
|
||||
&& (hrt_elapsed_time(&_datalink_last_heartbeat_avoidance_system) > 5_s)) {
|
||||
|
||||
_avoidance_system_lost = true;
|
||||
_status_flags.avoidance_system_valid = false;
|
||||
}
|
||||
mavlink_log_critical(&_mavlink_log_pub, "Connection to parachute lost\t");
|
||||
events::send(events::ID("commander_parachute_lost"), events::Log::Critical, "Connection to parachute lost");
|
||||
}
|
||||
|
||||
// high latency data link loss failsafe
|
||||
if (_high_latency_datalink_heartbeat > 0
|
||||
if ((_high_latency_datalink_heartbeat != 0)
|
||||
&& hrt_elapsed_time(&_high_latency_datalink_heartbeat) > (_param_com_hldl_loss_t.get() * 1_s)) {
|
||||
|
||||
_high_latency_datalink_lost = hrt_absolute_time();
|
||||
|
||||
if (!_status.high_latency_data_link_lost) {
|
||||
@ -3582,16 +3610,18 @@ void Commander::avoidance_check()
|
||||
const bool cp_enabled = _param_cp_dist.get() > 0.f;
|
||||
|
||||
const bool distance_sensor_valid = hrt_elapsed_time(&_valid_distance_sensor_time_us) < 500_ms;
|
||||
const bool cp_healthy = _status_flags.avoidance_system_valid || distance_sensor_valid;
|
||||
const bool cp_healthy = _status_flags.system_valid_obstacle_avoidance || distance_sensor_valid;
|
||||
|
||||
const bool sensor_oa_present = cp_healthy || _status_flags.avoidance_system_required || cp_enabled;
|
||||
const bool sensor_oa_present = cp_healthy || _status_flags.system_required_obstacle_avoidance || cp_enabled;
|
||||
|
||||
const bool auto_mode = _vehicle_control_mode.flag_control_auto_enabled;
|
||||
const bool pos_ctl_mode = (_vehicle_control_mode.flag_control_manual_enabled
|
||||
&& _vehicle_control_mode.flag_control_position_enabled);
|
||||
|
||||
const bool sensor_oa_enabled = ((auto_mode && _status_flags.avoidance_system_required) || (pos_ctl_mode && cp_enabled));
|
||||
const bool sensor_oa_healthy = ((auto_mode && _status_flags.avoidance_system_valid) || (pos_ctl_mode && cp_healthy));
|
||||
const bool sensor_oa_enabled = ((auto_mode && _status_flags.system_required_obstacle_avoidance) || (pos_ctl_mode
|
||||
&& cp_enabled));
|
||||
const bool sensor_oa_healthy = ((auto_mode && _status_flags.system_valid_obstacle_avoidance) || (pos_ctl_mode
|
||||
&& cp_healthy));
|
||||
|
||||
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_OBSTACLE_AVOIDANCE, sensor_oa_present, sensor_oa_enabled,
|
||||
sensor_oa_healthy, _status);
|
||||
|
||||
@ -327,12 +327,10 @@ private:
|
||||
|
||||
|
||||
hrt_abstime _datalink_last_heartbeat_gcs{0};
|
||||
hrt_abstime _datalink_last_heartbeat_avoidance_system{0};
|
||||
hrt_abstime _datalink_last_heartbeat_gimbal{0};
|
||||
hrt_abstime _datalink_last_heartbeat_obstacle_avoidance_system{0};
|
||||
hrt_abstime _datalink_last_heartbeat_onboard_controller{0};
|
||||
hrt_abstime _datalink_last_heartbeat_parachute_system{0};
|
||||
bool _onboard_controller_lost{false};
|
||||
bool _avoidance_system_lost{false};
|
||||
bool _parachute_system_lost{true};
|
||||
|
||||
hrt_abstime _high_latency_datalink_heartbeat{0};
|
||||
hrt_abstime _high_latency_datalink_lost{0};
|
||||
|
||||
@ -2829,7 +2829,7 @@ Mavlink::display_status()
|
||||
_receiver.enable_message_statistics();
|
||||
#endif // !CONSTRAINED_FLASH
|
||||
|
||||
if (_tstatus.heartbeat_type_gcs) {
|
||||
if (_tstatus.heartbeat_gcs) {
|
||||
printf("\tGCS heartbeat valid\n");
|
||||
}
|
||||
|
||||
|
||||
@ -281,7 +281,7 @@ public:
|
||||
|
||||
bool get_forwarding_on() { return _forwarding_on; }
|
||||
|
||||
bool is_connected() { return _tstatus.heartbeat_type_gcs; }
|
||||
bool is_connected() { return _tstatus.heartbeat_gcs; }
|
||||
|
||||
#if defined(MAVLINK_UDP)
|
||||
static Mavlink *get_instance_for_network_port(unsigned long port);
|
||||
|
||||
@ -2118,41 +2118,63 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
|
||||
|
||||
if (same_system || hb.type == MAV_TYPE_GCS) {
|
||||
|
||||
camera_status_s camera_status{};
|
||||
const bool healthy = (hb.system_status == MAV_STATE_STANDBY) || (hb.system_status == MAV_STATE_ACTIVE);
|
||||
|
||||
switch (hb.type) {
|
||||
case MAV_TYPE_ANTENNA_TRACKER:
|
||||
_heartbeat_type_antenna_tracker = now;
|
||||
_heartbeat_antenna_tracker = now;
|
||||
_mavlink->telemetry_status().heartbeat_antenna_tracker = true;
|
||||
_mavlink->telemetry_status().system_healthy_antenna_tracker = healthy;
|
||||
break;
|
||||
|
||||
case MAV_TYPE_GCS:
|
||||
_heartbeat_type_gcs = now;
|
||||
_heartbeat_gcs = now;
|
||||
_mavlink->telemetry_status().heartbeat_gcs = true;
|
||||
_mavlink->telemetry_status().system_healthy_gcs = healthy;
|
||||
break;
|
||||
|
||||
case MAV_TYPE_ONBOARD_CONTROLLER:
|
||||
_heartbeat_type_onboard_controller = now;
|
||||
_heartbeat_onboard_controller = now;
|
||||
_mavlink->telemetry_status().heartbeat_onboard_controller = true;
|
||||
_mavlink->telemetry_status().system_healthy_onboard_controller = healthy;
|
||||
break;
|
||||
|
||||
case MAV_TYPE_GIMBAL:
|
||||
_heartbeat_type_gimbal = now;
|
||||
_heartbeat_gimbal = now;
|
||||
_mavlink->telemetry_status().heartbeat_gimbal = true;
|
||||
_mavlink->telemetry_status().system_healthy_gimbal = healthy;
|
||||
break;
|
||||
|
||||
case MAV_TYPE_ADSB:
|
||||
_heartbeat_type_adsb = now;
|
||||
_heartbeat_adsb = now;
|
||||
_mavlink->telemetry_status().heartbeat_adsb = true;
|
||||
_mavlink->telemetry_status().system_healthy_adsb = healthy;
|
||||
break;
|
||||
|
||||
case MAV_TYPE_CAMERA:
|
||||
_heartbeat_type_camera = now;
|
||||
camera_status.timestamp = now;
|
||||
camera_status.active_comp_id = msg->compid;
|
||||
camera_status.active_sys_id = msg->sysid;
|
||||
_camera_status_pub.publish(camera_status);
|
||||
_heartbeat_camera = now;
|
||||
_mavlink->telemetry_status().heartbeat_camera = true;
|
||||
_mavlink->telemetry_status().system_healthy_camera = healthy;
|
||||
{
|
||||
|
||||
camera_status_s camera_status{};
|
||||
camera_status.active_comp_id = msg->compid;
|
||||
camera_status.active_sys_id = msg->sysid;
|
||||
camera_status.timestamp = hrt_absolute_time();
|
||||
_camera_status_pub.publish(camera_status);
|
||||
}
|
||||
break;
|
||||
|
||||
case MAV_TYPE_BATTERY:
|
||||
_heartbeat_battery = now;
|
||||
_mavlink->telemetry_status().heartbeat_battery = true;
|
||||
_mavlink->telemetry_status().system_healthy_battery = healthy;
|
||||
break;
|
||||
|
||||
case MAV_TYPE_PARACHUTE:
|
||||
_heartbeat_type_parachute = now;
|
||||
_mavlink->telemetry_status().parachute_system_healthy =
|
||||
(hb.system_status == MAV_STATE_STANDBY) || (hb.system_status == MAV_STATE_ACTIVE);
|
||||
_heartbeat_parachute = now;
|
||||
_mavlink->telemetry_status().heartbeat_parachute = true;
|
||||
_mavlink->telemetry_status().system_healthy_parachute = healthy;
|
||||
break;
|
||||
|
||||
default:
|
||||
@ -2163,36 +2185,51 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
|
||||
|
||||
switch (msg->compid) {
|
||||
case MAV_COMP_ID_TELEMETRY_RADIO:
|
||||
_heartbeat_component_telemetry_radio = now;
|
||||
_heartbeat_telemetry_radio = now;
|
||||
_mavlink->telemetry_status().heartbeat_telemetry_radio = true;
|
||||
_mavlink->telemetry_status().system_healthy_telemetry_radio = healthy;
|
||||
break;
|
||||
|
||||
case MAV_COMP_ID_LOG:
|
||||
_heartbeat_component_log = now;
|
||||
_heartbeat_log = now;
|
||||
_mavlink->telemetry_status().heartbeat_osd = true;
|
||||
_mavlink->telemetry_status().system_healthy_osd = healthy;
|
||||
break;
|
||||
|
||||
case MAV_COMP_ID_OSD:
|
||||
_heartbeat_component_osd = now;
|
||||
_heartbeat_osd = now;
|
||||
_mavlink->telemetry_status().heartbeat_osd = true;
|
||||
_mavlink->telemetry_status().system_healthy_osd = healthy;
|
||||
break;
|
||||
|
||||
case MAV_COMP_ID_OBSTACLE_AVOIDANCE:
|
||||
_heartbeat_component_obstacle_avoidance = now;
|
||||
_mavlink->telemetry_status().avoidance_system_healthy = (hb.system_status == MAV_STATE_ACTIVE);
|
||||
_heartbeat_obstacle_avoidance = now;
|
||||
_mavlink->telemetry_status().heartbeat_obstacle_avoidance = true;
|
||||
_mavlink->telemetry_status().system_healthy_obstacle_avoidance = healthy;
|
||||
break;
|
||||
|
||||
case MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY:
|
||||
_heartbeat_component_visual_inertial_odometry = now;
|
||||
_heartbeat_visual_inertial_odometry = now;
|
||||
_mavlink->telemetry_status().heartbeat_vio = true;
|
||||
_mavlink->telemetry_status().system_healthy_vio = healthy;
|
||||
break;
|
||||
|
||||
case MAV_COMP_ID_PAIRING_MANAGER:
|
||||
_heartbeat_component_pairing_manager = now;
|
||||
_heartbeat_pairing_manager = now;
|
||||
_mavlink->telemetry_status().heartbeat_pairing_manager = true;
|
||||
_mavlink->telemetry_status().system_healthy_pairing_manager = healthy;
|
||||
break;
|
||||
|
||||
case MAV_COMP_ID_UDP_BRIDGE:
|
||||
_heartbeat_component_udp_bridge = now;
|
||||
_heartbeat_udp_bridge = now;
|
||||
_mavlink->telemetry_status().heartbeat_udp_bridge = true;
|
||||
_mavlink->telemetry_status().system_healthy_udp_bridge = healthy;
|
||||
break;
|
||||
|
||||
case MAV_COMP_ID_UART_BRIDGE:
|
||||
_heartbeat_component_uart_bridge = now;
|
||||
_heartbeat_uart_bridge = now;
|
||||
_mavlink->telemetry_status().heartbeat_uart_bridge = true;
|
||||
_mavlink->telemetry_status().system_healthy_uart_bridge = healthy;
|
||||
break;
|
||||
|
||||
default:
|
||||
@ -2200,7 +2237,7 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
|
||||
msg->compid);
|
||||
}
|
||||
|
||||
CheckHeartbeats(now, true);
|
||||
_mavlink->telemetry_status_updated();
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -2935,7 +2972,7 @@ void MavlinkReceiver::handle_message_statustext(mavlink_message_t *msg)
|
||||
}
|
||||
}
|
||||
|
||||
void MavlinkReceiver::CheckHeartbeats(const hrt_abstime &t, bool force)
|
||||
void MavlinkReceiver::CheckHeartbeats(const hrt_abstime &t)
|
||||
{
|
||||
// check HEARTBEATs for timeout
|
||||
static constexpr uint64_t TIMEOUT = telemetry_status_s::HEARTBEAT_TIMEOUT_US;
|
||||
@ -2944,25 +2981,25 @@ void MavlinkReceiver::CheckHeartbeats(const hrt_abstime &t, bool force)
|
||||
return;
|
||||
}
|
||||
|
||||
if ((t >= _last_heartbeat_check + (TIMEOUT / 2)) || force) {
|
||||
if ((t >= _last_heartbeat_check + (TIMEOUT / 2))) {
|
||||
telemetry_status_s &tstatus = _mavlink->telemetry_status();
|
||||
|
||||
tstatus.heartbeat_type_antenna_tracker = (t <= TIMEOUT + _heartbeat_type_antenna_tracker);
|
||||
tstatus.heartbeat_type_gcs = (t <= TIMEOUT + _heartbeat_type_gcs);
|
||||
tstatus.heartbeat_type_onboard_controller = (t <= TIMEOUT + _heartbeat_type_onboard_controller);
|
||||
tstatus.heartbeat_type_gimbal = (t <= TIMEOUT + _heartbeat_type_gimbal);
|
||||
tstatus.heartbeat_type_adsb = (t <= TIMEOUT + _heartbeat_type_adsb);
|
||||
tstatus.heartbeat_type_camera = (t <= TIMEOUT + _heartbeat_type_camera);
|
||||
tstatus.heartbeat_type_parachute = (t <= TIMEOUT + _heartbeat_type_parachute);
|
||||
|
||||
tstatus.heartbeat_component_telemetry_radio = (t <= TIMEOUT + _heartbeat_component_telemetry_radio);
|
||||
tstatus.heartbeat_component_log = (t <= TIMEOUT + _heartbeat_component_log);
|
||||
tstatus.heartbeat_component_osd = (t <= TIMEOUT + _heartbeat_component_osd);
|
||||
tstatus.heartbeat_component_obstacle_avoidance = (t <= TIMEOUT + _heartbeat_component_obstacle_avoidance);
|
||||
tstatus.heartbeat_component_vio = (t <= TIMEOUT + _heartbeat_component_visual_inertial_odometry);
|
||||
tstatus.heartbeat_component_pairing_manager = (t <= TIMEOUT + _heartbeat_component_pairing_manager);
|
||||
tstatus.heartbeat_component_udp_bridge = (t <= TIMEOUT + _heartbeat_component_udp_bridge);
|
||||
tstatus.heartbeat_component_uart_bridge = (t <= TIMEOUT + _heartbeat_component_uart_bridge);
|
||||
tstatus.heartbeat_adsb = (t <= TIMEOUT + _heartbeat_adsb);
|
||||
tstatus.heartbeat_antenna_tracker = (t <= TIMEOUT + _heartbeat_antenna_tracker);
|
||||
tstatus.heartbeat_battery = (t <= TIMEOUT + _heartbeat_battery);
|
||||
tstatus.heartbeat_camera = (t <= TIMEOUT + _heartbeat_camera);
|
||||
tstatus.heartbeat_gcs = (t <= TIMEOUT + _heartbeat_gcs);
|
||||
tstatus.heartbeat_gimbal = (t <= TIMEOUT + _heartbeat_gimbal);
|
||||
tstatus.heartbeat_log = (t <= TIMEOUT + _heartbeat_log);
|
||||
tstatus.heartbeat_obstacle_avoidance = (t <= TIMEOUT + _heartbeat_obstacle_avoidance);
|
||||
tstatus.heartbeat_onboard_controller = (t <= TIMEOUT + _heartbeat_onboard_controller);
|
||||
tstatus.heartbeat_osd = (t <= TIMEOUT + _heartbeat_osd);
|
||||
tstatus.heartbeat_pairing_manager = (t <= TIMEOUT + _heartbeat_pairing_manager);
|
||||
tstatus.heartbeat_parachute = (t <= TIMEOUT + _heartbeat_parachute);
|
||||
tstatus.heartbeat_telemetry_radio = (t <= TIMEOUT + _heartbeat_telemetry_radio);
|
||||
tstatus.heartbeat_uart_bridge = (t <= TIMEOUT + _heartbeat_uart_bridge);
|
||||
tstatus.heartbeat_udp_bridge = (t <= TIMEOUT + _heartbeat_udp_bridge);
|
||||
tstatus.heartbeat_vio = (t <= TIMEOUT + _heartbeat_visual_inertial_odometry);
|
||||
|
||||
_mavlink->telemetry_status_updated();
|
||||
_last_heartbeat_check = t;
|
||||
|
||||
@ -206,7 +206,7 @@ private:
|
||||
#endif // !CONSTRAINED_FLASH
|
||||
void handle_message_request_event(mavlink_message_t *msg);
|
||||
|
||||
void CheckHeartbeats(const hrt_abstime &t, bool force = false);
|
||||
void CheckHeartbeats(const hrt_abstime &t);
|
||||
|
||||
/**
|
||||
* Set the interval at which the given message stream is published.
|
||||
@ -373,22 +373,22 @@ private:
|
||||
|
||||
hrt_abstime _last_heartbeat_check{0};
|
||||
|
||||
hrt_abstime _heartbeat_type_antenna_tracker{0};
|
||||
hrt_abstime _heartbeat_type_gcs{0};
|
||||
hrt_abstime _heartbeat_type_onboard_controller{0};
|
||||
hrt_abstime _heartbeat_type_gimbal{0};
|
||||
hrt_abstime _heartbeat_type_adsb{0};
|
||||
hrt_abstime _heartbeat_type_camera{0};
|
||||
hrt_abstime _heartbeat_type_parachute{0};
|
||||
|
||||
hrt_abstime _heartbeat_component_telemetry_radio{0};
|
||||
hrt_abstime _heartbeat_component_log{0};
|
||||
hrt_abstime _heartbeat_component_osd{0};
|
||||
hrt_abstime _heartbeat_component_obstacle_avoidance{0};
|
||||
hrt_abstime _heartbeat_component_visual_inertial_odometry{0};
|
||||
hrt_abstime _heartbeat_component_pairing_manager{0};
|
||||
hrt_abstime _heartbeat_component_udp_bridge{0};
|
||||
hrt_abstime _heartbeat_component_uart_bridge{0};
|
||||
hrt_abstime _heartbeat_adsb{0};
|
||||
hrt_abstime _heartbeat_antenna_tracker{0};
|
||||
hrt_abstime _heartbeat_battery{0};
|
||||
hrt_abstime _heartbeat_camera{0};
|
||||
hrt_abstime _heartbeat_gcs{0};
|
||||
hrt_abstime _heartbeat_gimbal{0};
|
||||
hrt_abstime _heartbeat_log{0};
|
||||
hrt_abstime _heartbeat_obstacle_avoidance{0};
|
||||
hrt_abstime _heartbeat_onboard_controller{0};
|
||||
hrt_abstime _heartbeat_osd{0};
|
||||
hrt_abstime _heartbeat_pairing_manager{0};
|
||||
hrt_abstime _heartbeat_parachute{0};
|
||||
hrt_abstime _heartbeat_telemetry_radio{0};
|
||||
hrt_abstime _heartbeat_uart_bridge{0};
|
||||
hrt_abstime _heartbeat_udp_bridge{0};
|
||||
hrt_abstime _heartbeat_visual_inertial_odometry{0};
|
||||
|
||||
param_t _handle_sens_flow_maxhgt{PARAM_INVALID};
|
||||
param_t _handle_sens_flow_maxr{PARAM_INVALID};
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user