adding feature to prevent arming before onboard logging is ready

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

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

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 */

src/modules/commander/Commander.hpp

@@ -153,7 +153,10 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
 		(ParamInt<px4::params::CBRK_ENGINEFAIL>) _param_cbrk_enginefail,
 		(ParamInt<px4::params::CBRK_GPSFAIL>) _param_cbrk_gpsfail,
 		(ParamInt<px4::params::CBRK_FLIGHTTERM>) _param_cbrk_flightterm,
-		(ParamInt<px4::params::CBRK_VELPOSERR>) _param_cbrk_velposerr
+		(ParamInt<px4::params::CBRK_VELPOSERR>) _param_cbrk_velposerr,
+
+		(ParamInt<px4::params::COM_ARM_OBLOG>) _param_com_arm_ob_logger
+
 	)
 
 	enum class PrearmedMode {
@@ -249,14 +252,33 @@ class Commander : public ModuleBase<Commander>, 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<Commander>, 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)};

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);

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;
 }