From d4bea8323fd1053a503a8494e0f820d7121a9e0b Mon Sep 17 00:00:00 2001 From: Claudio Micheli Date: Thu, 29 Oct 2020 13:33:48 +0100 Subject: [PATCH] PreFlightCheck: Detect missing FMU SD card Signed-off-by: Claudio Micheli --- .../Arming/PreFlightCheck/CMakeLists.txt | 1 + .../Arming/PreFlightCheck/PreFlightCheck.cpp | 1 + .../Arming/PreFlightCheck/PreFlightCheck.hpp | 1 + .../PreFlightCheck/checks/sdcardCheck.cpp | 74 +++++++++++++++++++ src/modules/commander/commander_params.c | 14 ++++ 5 files changed, 91 insertions(+) create mode 100644 src/modules/commander/Arming/PreFlightCheck/checks/sdcardCheck.cpp diff --git a/src/modules/commander/Arming/PreFlightCheck/CMakeLists.txt b/src/modules/commander/Arming/PreFlightCheck/CMakeLists.txt index 0bf2c119f6..7923a5ed8d 100644 --- a/src/modules/commander/Arming/PreFlightCheck/CMakeLists.txt +++ b/src/modules/commander/Arming/PreFlightCheck/CMakeLists.txt @@ -48,6 +48,7 @@ px4_add_library(PreFlightCheck checks/failureDetectorCheck.cpp checks/manualControlCheck.cpp checks/cpuResourceCheck.cpp + checks/sdcardCheck.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 818e5812c6..b0555af020 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp @@ -64,6 +64,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu bool failed = false; failed = failed || !airframeCheck(mavlink_log_pub, status); + failed = failed || !sdcardCheck(mavlink_log_pub, report_failures); /* ---- MAG ---- */ { diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp index e4ce474d24..f7df78d659 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp @@ -119,4 +119,5 @@ private: static bool manualControlCheck(orb_advert_t *mavlink_log_pub, const bool report_fail); 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, const bool report_fail); }; diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/sdcardCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/sdcardCheck.cpp new file mode 100644 index 0000000000..7ff3097099 --- /dev/null +++ b/src/modules/commander/Arming/PreFlightCheck/checks/sdcardCheck.cpp @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * 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 +#ifdef __PX4_DARWIN +#include +#include +#else +#include +#endif + +bool PreFlightCheck::sdcardCheck(orb_advert_t *mavlink_log_pub, const bool report_fail) +{ + const char *microsd_dir = PX4_STORAGEDIR; + bool success = true; + struct statfs statfs_buf; + uint64_t total_bytes = 0; + + int32_t reporting_enabled = 0; + param_get(param_find("COM_ARM_SDCARD"), &reporting_enabled); + + if (reporting_enabled == 0) { + return success; + } + + if (statfs(microsd_dir, &statfs_buf) == 0) { + total_bytes = (uint64_t)statfs_buf.f_blocks * statfs_buf.f_bsize; + } + + if (total_bytes == 0) { // on NuttX we get 0 total bytes if no SD card is inserted + + if (reporting_enabled == 2) { + success = false; + } + + if (report_fail && reporting_enabled > 0) { + mavlink_log_critical(mavlink_log_pub, "Warning! Missing FMU SD Card."); + } + } + + return success; +} diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index ca8f0de3f8..bf4babe9ee 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -985,3 +985,17 @@ PARAM_DEFINE_FLOAT(COM_LKDOWN_TKO, 3.0f); * @value 1 Enabled */ PARAM_DEFINE_INT32(COM_ARM_ARSP_EN, 1); + +/** + * Enable FMU SD card detection check + * + * This check detects if the FMU SD card is missing. + * Depending on the value of the parameter, the check can be + * disabled, warn only or deny arming. + * + * @group Commander + * @value 0 Disabled + * @value 1 Warning only + * @value 2 Enforce SD card presence + */ +PARAM_DEFINE_INT32(COM_ARM_SDCARD, 1);