diff --git a/msg/telemetry_status.msg b/msg/telemetry_status.msg index a3c202caf7..6903f59279 100644 --- a/msg/telemetry_status.msg +++ b/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 diff --git a/msg/vehicle_status_flags.msg b/msg/vehicle_status_flags.msg index dae9b1c6d5..79d2548d74 100644 --- a/msg/vehicle_status_flags.msg +++ b/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 diff --git a/src/modules/commander/Arming/PreFlightCheck/CMakeLists.txt b/src/modules/commander/Arming/PreFlightCheck/CMakeLists.txt index 7923a5ed8d..36dd4dcce9 100644 --- a/src/modules/commander/Arming/PreFlightCheck/CMakeLists.txt +++ b/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) diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp index 5afd1c4e3a..63dec5b82b 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp +++ b/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; diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp index 392f3bb9c8..0110e1ef58 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp +++ b/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); }; diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/parachuteCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/parachuteCheck.cpp new file mode 100644 index 0000000000..4fd3b1094f --- /dev/null +++ b/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 +#include + +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"), ¶m_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; +} diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 267583405b..04ba86661a 100644 --- a/src/modules/commander/Commander.cpp +++ b/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) diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index c8c41d1e04..dc2504ae42 100644 --- a/src/modules/commander/commander_params.c +++ b/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 * diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index a6108a5b82..4e3558699c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/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: