diff --git a/msg/telemetry_status.msg b/msg/telemetry_status.msg index 6903f59279..ef72f26b3f 100644 --- a/msg/telemetry_status.msg +++ b/msg/telemetry_status.msg @@ -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 diff --git a/msg/vehicle_status_flags.msg b/msg/vehicle_status_flags.msg index 79d2548d74..1682705fcc 100644 --- a/msg/vehicle_status_flags.msg +++ b/msg/vehicle_status_flags.msg @@ -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 diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/parachuteCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/parachuteCheck.cpp index 4fd3b1094f..7fbec92d3f 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/parachuteCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/parachuteCheck.cpp @@ -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) { diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp index d0d113ece8..b24bb9225b 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp @@ -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"); } } diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index a176d23c4b..09c31fb568 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -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); diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index c89a26f652..cdbeaf6ef4 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -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}; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index c16658c208..f87853e7bb 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -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"); } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index fc536fe40d..5c9ce68f6f 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -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); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index b8409bc517..0e9b9af1ff 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 55fb16df96..6627d3e875 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -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};