diff --git a/msg/telemetry_status.msg b/msg/telemetry_status.msg index 3f6f3248c84f..9e723bb143a9 100644 --- a/msg/telemetry_status.msg +++ b/msg/telemetry_status.msg @@ -10,6 +10,7 @@ uint8 COMPONENT_ID_ALL = 0 uint8 COMPONENT_ID_AUTOPILOT1 = 1 uint8 COMPONENT_ID_CAMERA = 100 uint8 COMPONENT_ID_OBSTACLE_AVOIDANCE = 196 +uint8 COMPONENT_ID_LOG = 155 # MAV_TYPE (fill in as needed) uint8 MAV_TYPE_GENERIC = 0 diff --git a/msg/vehicle_status_flags.msg b/msg/vehicle_status_flags.msg index a5dd3dec1ed7..0b12b83e281a 100644 --- a/msg/vehicle_status_flags.msg +++ b/msg/vehicle_status_flags.msg @@ -36,3 +36,6 @@ bool usb_connected # status of the USB power supp 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 onboard_logging_system_required # Set to true if logging system is required for arming via the COM_ARM_OBLOG parameter +bool onboard_logging_system_valid # Status of the onboard logging system \ No newline at end of file diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index ab7d45610117..b4471a999097 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -607,6 +607,8 @@ Commander::Commander() : status_flags.rc_calibration_valid = true; status_flags.avoidance_system_valid = false; + status_flags.onboard_logging_system_valid = false; + } Commander::~Commander() @@ -1242,6 +1244,7 @@ Commander::run() param_t _param_rc_map_arm_switch = param_find("RC_MAP_ARM_SW"); status_flags.avoidance_system_required = _param_com_obs_avoid.get(); + status_flags.onboard_logging_system_required = _param_com_arm_ob_logger.get(); /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -1518,6 +1521,7 @@ Commander::run() /* Update OA parameter */ status_flags.avoidance_system_required = _param_com_obs_avoid.get(); + status_flags.onboard_logging_system_required = _param_com_arm_ob_logger.get(); /* handle power button state */ if (power_button_state_sub.updated()) { @@ -3778,31 +3782,49 @@ void Commander::data_link_check(bool &status_changed) status_changed = true; } - } + } else { - _datalink_last_heartbeat_onboard_controller = telemetry.heartbeat_time; + // AVOIDANCE SYSTEM update heartbeat values if needed - if (telemetry.remote_component_id == telemetry_status_s::COMPONENT_ID_OBSTACLE_AVOIDANCE) { - if (telemetry.heartbeat_time != _datalink_last_heartbeat_avoidance_system) { - _avoidance_system_status_change = _datalink_last_status_avoidance_system != telemetry.remote_system_status; + if (status_flags.avoidance_system_required) { + process_onboard_system_heartbeat(_avoidance, status_flags.avoidance_system_valid, status_changed, telemetry); } - _datalink_last_heartbeat_avoidance_system = telemetry.heartbeat_time; - _datalink_last_status_avoidance_system = telemetry.remote_system_status; + // ONBOARD LOGGING SYSTEM update heartbeat values if needed - if (_avoidance_system_lost) { - mavlink_log_info(&mavlink_log_pub, "Avoidance system regained"); - status_changed = true; - _avoidance_system_lost = false; - status_flags.avoidance_system_valid = true; + if (status_flags.onboard_logging_system_required) { + process_onboard_system_heartbeat(_logger, status_flags.onboard_logging_system_valid, status_changed, telemetry); } } + _datalink_last_heartbeat_onboard_controller = telemetry.heartbeat_time; + break; } } } + // AVOIDANCE SYSTEM state check (only if it is enabled) + + if (status_flags.avoidance_system_required) { + + //if avoidance never started + if (_datalink_last_heartbeat_avoidance_system == 0 && + _avoidance.print_msg_once == false && + hrt_elapsed_time(&_datalink_last_heartbeat_avoidance_system) > _param_com_oa_boot_t.get() * 1_s) { + _avoidance.print_msg_once = true; + mavlink_log_critical(&mavlink_log_pub, "avoidance system not available"); + + } else { + update_onboard_system_state(_avoidance, status_flags.avoidance_system_valid, status_changed); + } + } + + // ON BOARD LOGGING SYSTEM state check (only if it is enabled) + + if (status_flags.onboard_logging_system_required) { + update_onboard_system_state(_logger, status_flags.onboard_logging_system_valid, status_changed); + } // GCS data link loss failsafe if (!status.data_link_lost) { @@ -3828,67 +3850,94 @@ void Commander::data_link_check(bool &status_changed) status_changed = true; } - // AVOIDANCE SYSTEM state check (only if it is enabled) - if (status_flags.avoidance_system_required && !_onboard_controller_lost) { - - //if avoidance never started - if (_datalink_last_heartbeat_avoidance_system == 0 - && hrt_elapsed_time(&_datalink_last_heartbeat_avoidance_system) > _param_com_oa_boot_t.get() * 1_s) { - if (!_print_avoidance_msg_once) { - mavlink_log_critical(&mavlink_log_pub, "Avoidance system not available"); - _print_avoidance_msg_once = true; - - } - } + // high latency data link loss failsafe + 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 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; - mavlink_log_critical(&mavlink_log_pub, "Avoidance system lost"); - status_flags.avoidance_system_valid = false; - _print_avoidance_msg_once = false; + 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; } + } +} - //if status changed - if (_avoidance_system_status_change) { - if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_BOOT) { - mavlink_log_info(&mavlink_log_pub, "Avoidance system starting"); - } - - if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_ACTIVE) { - mavlink_log_info(&mavlink_log_pub, "Avoidance system connected"); - status_flags.avoidance_system_valid = true; - } +void Commander::process_onboard_system_heartbeat(OnboardHeartBeatMonitor &monitor, bool &status_flag_system_valid, + bool &status_changed, telemetry_status_s &telemetry) +{ + if (telemetry.remote_component_id == monitor.hb_component_id) { - if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_CRITICAL) { - mavlink_log_info(&mavlink_log_pub, "Avoidance system timed out"); - } + if (telemetry.heartbeat_time != monitor.datalink_last_heartbeat) { + monitor.system_status_change = monitor.datalink_last_status != telemetry.remote_system_status; + } - if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_FLIGHT_TERMINATION) { - mavlink_log_critical(&mavlink_log_pub, "Avoidance system rejected"); - status_flags.avoidance_system_valid = false; - status_changed = true; - } + monitor.datalink_last_heartbeat = telemetry.heartbeat_time; + monitor.datalink_last_status = telemetry.remote_system_status; - _avoidance_system_status_change = false; + if (monitor.system_lost) { + mavlink_log_warning(&mavlink_log_pub, "%s system regained", monitor.hb_name); + status_changed = true; + monitor.system_lost = false; + status_flag_system_valid = true; } } +} +void Commander::update_onboard_system_state(OnboardHeartBeatMonitor &monitor, bool &status_flag_system_valid, + bool &status_changed) +{ - // high latency data link loss failsafe - 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 heartbeat stops + if (!monitor.system_lost && (monitor.datalink_last_heartbeat > 0) + && (hrt_elapsed_time(&monitor.datalink_last_heartbeat) > 5_s)) { + monitor.system_lost = true; + mavlink_log_critical(&mavlink_log_pub, "%s system lost", monitor.hb_name); + status_flag_system_valid = false; + monitor.print_msg_once = false; + } - 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; + //if status changed + if (monitor.system_status_change) { + switch (monitor.datalink_last_status) { + case telemetry_status_s::MAV_STATE_UNINIT: + break; + + case telemetry_status_s::MAV_STATE_BOOT: + case telemetry_status_s::MAV_STATE_CALIBRATING: + mavlink_log_info(&mavlink_log_pub, "%s system starting", monitor.hb_name); + status_flag_system_valid = false; + break; + + case telemetry_status_s::MAV_STATE_STANDBY: + case telemetry_status_s::MAV_STATE_ACTIVE: + mavlink_log_info(&mavlink_log_pub, "%s system connected", monitor.hb_name); + status_flag_system_valid = true; + break; + + case telemetry_status_s::MAV_STATE_CRITICAL: + mavlink_log_critical(&mavlink_log_pub, "%s system failure", monitor.hb_name); + status_flag_system_valid = false; + break; + + case telemetry_status_s::MAV_STATE_EMERGENCY: + mavlink_log_critical(&mavlink_log_pub, "%s system emergency", monitor.hb_name); + status_flag_system_valid = false; + break; + + case telemetry_status_s::MAV_STATE_POWEROFF: + case telemetry_status_s::MAV_STATE_FLIGHT_TERMINATION: + mavlink_log_info(&mavlink_log_pub, "%s system shutdown", monitor.hb_name); + status_flag_system_valid = false; + break; } + + status_changed = true; + monitor.system_status_change = false; } } + void Commander::battery_status_check() { /* update battery status */ diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index a5fd4dcfd4b5..e26e70f090b8 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -153,7 +153,10 @@ class Commander : public ModuleBase, public ModuleParams (ParamInt) _param_cbrk_enginefail, (ParamInt) _param_cbrk_gpsfail, (ParamInt) _param_cbrk_flightterm, - (ParamInt) _param_cbrk_velposerr + (ParamInt) _param_cbrk_velposerr, + + (ParamInt) _param_com_arm_ob_logger + ) enum class PrearmedMode { @@ -249,14 +252,33 @@ class Commander : public ModuleBase, public ModuleParams hrt_abstime _datalink_last_heartbeat_gcs{0}; + struct OnboardHeartBeatMonitor { + + OnboardHeartBeatMonitor(uint8_t component_id, const char *name) : + hb_name(name), + hb_component_id(component_id) + {} + + const char *hb_name; + hrt_abstime datalink_last_heartbeat{0}; + bool system_lost{false}; + bool system_status_change{false}; + uint8_t datalink_last_status{telemetry_status_s::MAV_STATE_UNINIT}; + bool print_msg_once{true}; + uint8_t hb_component_id; + + } _avoidance{telemetry_status_s::COMPONENT_ID_OBSTACLE_AVOIDANCE, "avoidance"}, + _logger{telemetry_status_s::COMPONENT_ID_LOG, "logger"}; + + void update_onboard_system_state(OnboardHeartBeatMonitor &monitor, bool &status_flag_system_valid, + bool &status_changed); + void process_onboard_system_heartbeat(OnboardHeartBeatMonitor &monitor, bool &status_flag_system_valid, + bool &status_changed, telemetry_status_s &telemetry); + hrt_abstime _datalink_last_heartbeat_onboard_controller{0}; bool _onboard_controller_lost{false}; hrt_abstime _datalink_last_heartbeat_avoidance_system{0}; - bool _avoidance_system_lost{false}; - - bool _avoidance_system_status_change{false}; - uint8_t _datalink_last_status_avoidance_system{telemetry_status_s::MAV_STATE_UNINIT}; uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)}; @@ -272,8 +294,6 @@ class Commander : public ModuleBase, public ModuleParams systemlib::Hysteresis _auto_disarm_landed{false}; systemlib::Hysteresis _auto_disarm_killed{false}; - bool _print_avoidance_msg_once{false}; - // Subscriptions uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 4f884f4edcf5..03fee1d3dc5b 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -967,3 +967,10 @@ PARAM_DEFINE_INT32(COM_ARM_CHK_ESCS, 1); * @group Commander */ PARAM_DEFINE_INT32(COM_PREARM_MODE, 1); + +/* + * Allow arming without onboard logger being enabled and in ready state + * + * The default allows the vehicle to arm without the onboard logger reporting as ready via it's heartbeat + */ +PARAM_DEFINE_INT32(COM_ARM_OBLOG, 0); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 06396cb82159..a1b5410dfb17 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -1105,6 +1105,16 @@ bool prearm_check(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &s } } + + if (status_flags.onboard_logging_system_required && !status_flags.onboard_logging_system_valid) { + if (prearm_ok && reportFailures) { + mavlink_log_critical(mavlink_log_pub, "ARMING DENIED: Onboard logging system not ready"); + } + + prearm_ok = false; + + } + // Arm Requirements: authorization // check last, and only if everything else has passed if ((arm_requirements & ARM_REQ_ARM_AUTH_BIT) && prearm_ok) { @@ -1114,7 +1124,6 @@ bool prearm_check(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &s } } - return prearm_ok; }