MAVLink parachute system support improvements

msg/telemetry_status.msg

@@ -44,6 +44,7 @@ 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 per component
 bool heartbeat_component_telemetry_radio    # MAV_COMP_ID_TELEMETRY_RADIO
@@ -57,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 @@ class PreFlightCheck
 	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

@@ -1549,6 +1549,7 @@ void Commander::executeActionRequest(const action_request_s &action_request)
 
 			_status_changed = true;
 			_armed.manual_lockdown = true;
+			send_parachute_command();
 		}
 
 		break;
@@ -3424,6 +3425,21 @@ void Commander::data_link_check()
 				_datalink_last_heartbeat_onboard_controller = telemetry.timestamp;
 			}
 
+			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");
+					}
+				}
+
+				_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) {
 				if (_avoidance_system_lost) {
 					_avoidance_system_lost = false;
@@ -3464,6 +3480,16 @@ void Commander::data_link_check()
 		_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;
+		_status_changed = true;
+	}
+
 	// AVOIDANCE SYSTEM state check (only if it is enabled)
 	if (_status_flags.avoidance_system_required && !_onboard_controller_lost) {
 		// if heartbeats stop

src/modules/commander/Commander.hpp

@@ -328,8 +328,10 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
 	hrt_abstime	_datalink_last_heartbeat_gcs{0};
 	hrt_abstime	_datalink_last_heartbeat_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};

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

@@ -2150,6 +2150,12 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
 				_camera_status_pub.publish(camera_status);
 				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);
+				break;
+
 			default:
 				PX4_DEBUG("unhandled HEARTBEAT MAV_TYPE: %" PRIu8 " from SYSID: %" PRIu8 ", COMPID: %" PRIu8, hb.type, msg->sysid,
 					  msg->compid);
@@ -2950,6 +2956,7 @@ void MavlinkReceiver::CheckHeartbeats(const hrt_abstime &t, bool force)
 		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);

src/modules/mavlink/mavlink_receiver.h

@@ -377,6 +377,7 @@ class MavlinkReceiver : public ModuleParams
 	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};