Add optional preflight check for healthy MAVLink parachute system

msg/telemetry_status.msg

@@ -58,3 +58,4 @@ bool heartbeat_component_uart_bridge        # MAV_COMP_ID_UART_BRIDGE
 
 # Misc component health
 bool avoidance_system_healthy
+bool parachute_system_healthy

msg/vehicle_status_flags.msg

@@ -38,3 +38,6 @@ bool sd_card_detected_once                        # set to true if the SD card w
 
 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 parachute_system_present
+bool parachute_system_healthy

src/modules/commander/Arming/PreFlightCheck/CMakeLists.txt

@@ -49,6 +49,7 @@ px4_add_library(PreFlightCheck
 	checks/manualControlCheck.cpp
 	checks/cpuResourceCheck.cpp
 	checks/sdcardCheck.cpp
+	checks/parachuteCheck.cpp
 )
 target_include_directories(PreFlightCheck PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 target_link_libraries(PreFlightCheck PUBLIC ArmAuthorization HealthFlags sensor_calibration)

src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp

@@ -266,6 +266,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 
 	failed = failed || !manualControlCheck(mavlink_log_pub, report_failures);
 	failed = failed || !cpuResourceCheck(mavlink_log_pub, report_failures);
+	failed = failed || !parachuteCheck(mavlink_log_pub, report_failures, status_flags);
 
 	/* Report status */
 	return !failed;

src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp

@@ -121,4 +121,6 @@ private:
 	static bool airframeCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status);
 	static bool cpuResourceCheck(orb_advert_t *mavlink_log_pub, const bool report_fail);
 	static bool sdcardCheck(orb_advert_t *mavlink_log_pub, bool &sd_card_detected_once, const bool report_fail);
+	static bool parachuteCheck(orb_advert_t *mavlink_log_pub, const bool report_fail,
+				   const vehicle_status_flags_s &status_flags);
 };

src/modules/commander/Arming/PreFlightCheck/checks/parachuteCheck.cpp

@@ -0,0 +1,68 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "../PreFlightCheck.hpp"
+
+#include <lib/parameters/param.h>
+#include <lib/systemlib/mavlink_log.h>
+
+using namespace time_literals;
+
+bool PreFlightCheck::parachuteCheck(orb_advert_t *mavlink_log_pub, const bool report_fail,
+				    const vehicle_status_flags_s &status_flags)
+{
+	bool success = true;
+
+	int32_t param_com_parachute = false;
+	param_get(param_find("COM_PARACHUTE"), &param_com_parachute);
+	const bool parachute_required = param_com_parachute != 0;
+
+	if (parachute_required) {
+		if (!status_flags.parachute_system_present) {
+			success = false;
+
+			if (report_fail) {
+				mavlink_log_critical(mavlink_log_pub, "Fail: Parachute system missing");
+			}
+
+		} else if (!status_flags.parachute_system_healthy) {
+			success = false;
+
+			if (report_fail) {
+				mavlink_log_critical(mavlink_log_pub, "Fail: Parachute system not ready");
+			}
+		}
+	}
+
+	return success;
+}

src/modules/commander/Commander.cpp

@@ -3435,6 +3435,8 @@ void Commander::data_link_check()
 				}
 
 				_datalink_last_heartbeat_parachute_system = telemetry.timestamp;
+				_status_flags.parachute_system_present = true;
+				_status_flags.parachute_system_healthy = telemetry.parachute_system_healthy;
 			}
 
 			if (telemetry.heartbeat_component_obstacle_avoidance) {
@@ -3481,7 +3483,10 @@ void Commander::data_link_check()
 	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;
+		_status_changed = true;
 	}
 
 	// AVOIDANCE SYSTEM state check (only if it is enabled)

src/modules/commander/commander_params.c

@@ -919,6 +919,14 @@ PARAM_DEFINE_INT32(COM_RCL_EXCEPT, 0);
  */
 PARAM_DEFINE_INT32(COM_OBS_AVOID, 0);
 
+/**
+ * Expect and require a healthy MAVLink parachute system
+ *
+ * @boolean
+ * @group Commander
+ */
+PARAM_DEFINE_INT32(COM_PARACHUTE, 0);
+
 /**
  * User Flight Profile
  *

src/modules/mavlink/mavlink_receiver.cpp

@@ -2152,6 +2152,8 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
 
 			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);
 				break;
 
 			default: