ChibiOS: Support formatting of microSD card

libraries/AP_Filesystem/AP_Filesystem.cpp

@@ -70,6 +70,8 @@ const AP_Filesystem::Backend AP_Filesystem::backends[] = {
 #endif
 };
 
+extern const AP_HAL::HAL& hal;
+
 #define MAX_FD_PER_BACKEND 256U
 #define NUM_BACKENDS ARRAY_SIZE(backends)
 #define LOCAL_BACKEND backends[0]
@@ -269,6 +271,19 @@ bool AP_Filesystem::fgets(char *buf, uint8_t buflen, int fd)
     return i != 0;
 }
 
+// format filesystem
+bool AP_Filesystem::format(void)
+{
+#if AP_FILESYSTEM_FORMAT_ENABLED
+    if (hal.util->get_soft_armed()) {
+        return false;
+    }
+    return LOCAL_BACKEND.fs.format();
+#else
+    return false;
+#endif
+}
+
 namespace AP
 {
 AP_Filesystem &FS()

libraries/AP_Filesystem/AP_Filesystem.h

@@ -42,6 +42,13 @@ struct dirent {
 #include <fcntl.h>
 #include <errno.h>
 #include <unistd.h>
+
+#ifndef AP_FILESYSTEM_FORMAT_ENABLED
+// only enable for SDMMC filesystems for now as other types can't query
+// block size
+#define AP_FILESYSTEM_FORMAT_ENABLED (STM32_SDC_USE_SDMMC1==TRUE || STM32_SDC_USE_SDMMC2==TRUE)
+#endif
+
 #endif // HAL_BOARD_CHIBIOS
 #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX || CONFIG_HAL_BOARD == HAL_BOARD_SITL
 #include "AP_Filesystem_posix.h"
@@ -53,6 +60,10 @@ struct dirent {
 
 #include "AP_Filesystem_backend.h"
 
+#ifndef AP_FILESYSTEM_FORMAT_ENABLED
+#define AP_FILESYSTEM_FORMAT_ENABLED 0
+#endif
+
 class AP_Filesystem {
 private:
     struct DirHandle {
@@ -96,6 +107,9 @@ class AP_Filesystem {
     // returns null-terminated string; cr or lf terminates line
     bool fgets(char *buf, uint8_t buflen, int fd);
 
+    // format filesystem
+    bool format(void);
+
     /*
       load a full file. Use delete to free the data
      */

libraries/AP_Filesystem/AP_Filesystem_FATFS.cpp

@@ -10,6 +10,7 @@
 #if HAVE_FILESYSTEM_SUPPORT && CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
 
 #include <AP_HAL_ChibiOS/sdcard.h>
+#include <GCS_MAVLink/GCS.h>
 
 #if 0
 #define debug(fmt, args ...)  do {printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
@@ -851,6 +852,52 @@ void AP_Filesystem_FATFS::unmount(void)
     return sdcard_stop();
 }
 
+/*
+  format sdcard
+*/
+bool AP_Filesystem_FATFS::format(void)
+{
+#if FF_USE_MKFS
+    WITH_SEMAPHORE(sem);
+    hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&AP_Filesystem_FATFS::format_handler, void));
+    // the format is handled asyncronously, we inform user of success
+    // via a text message
+    format_pending = true;
+    return true;
+#else
+    return false;
+#endif
+}
+
+/*
+  format sdcard
+*/
+void AP_Filesystem_FATFS::format_handler(void)
+{
+#if FF_USE_MKFS
+    if (!format_pending) {
+        return;
+    }
+    WITH_SEMAPHORE(sem);
+    format_pending = false;
+    GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Formatting SDCard");
+    uint8_t *buf = (uint8_t *)hal.util->malloc_type(FF_MAX_SS, AP_HAL::Util::MEM_DMA_SAFE);
+    if (buf == nullptr) {
+        return;
+    }
+    // format first disk
+    auto ret = f_mkfs("0:", 0, buf, FF_MAX_SS);
+    hal.util->free_type(buf, FF_MAX_SS, AP_HAL::Util::MEM_DMA_SAFE);
+    if (ret == FR_OK) {
+        GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Format: OK");
+    } else {
+        GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Format: Failed (%d)", int(ret));
+    }
+    sdcard_stop();
+    sdcard_retry();
+#endif
+}
+
 /*
   convert POSIX errno to text with user message.
 */

libraries/AP_Filesystem/AP_Filesystem_FATFS.h

@@ -54,4 +54,11 @@ class AP_Filesystem_FATFS : public AP_Filesystem_Backend
 
     // unmount filesystem for reboot
     void unmount(void) override;
+
+    // format sdcard
+    bool format(void) override;
+
+private:
+    void format_handler(void);
+    bool format_pending;
 };

libraries/AP_Filesystem/AP_Filesystem_backend.h

@@ -73,6 +73,9 @@ class AP_Filesystem_Backend {
     // unmount filesystem for reboot
     virtual void unmount(void) {}
 
+    // format sdcard
+    virtual bool format(void) { return false; }
+
     /*
       load a full file. Use delete to free the data
      */

libraries/AP_HAL_ChibiOS/hwdef/common/ffconf.h

@@ -44,7 +44,10 @@
 /  f_findnext(). (0:Disable, 1:Enable 2:Enable with matching altname[] too) */
 
 
-#define FF_USE_MKFS       0
+#ifndef FF_USE_MKFS
+// f_mkfs() only supported on SDC so far
+#define FF_USE_MKFS       HAL_USE_SDC
+#endif
 /* This option switches f_mkfs() function. (0:Disable or 1:Enable) */
 
 

libraries/AP_HAL_ChibiOS/hwdef/common/mcuconf.h

@@ -52,3 +52,11 @@
 #else
 #error "Unsupported MCU"
 #endif
+
+#ifndef STM32_SDC_USE_SDMMC1
+#define STM32_SDC_USE_SDMMC1                FALSE
+#endif
+
+#ifndef STM32_SDC_USE_SDMMC2
+#define STM32_SDC_USE_SDMMC2                FALSE
+#endif

libraries/AP_HAL_ChibiOS/hwdef/common/stm32f47_mcuconf.h

@@ -475,7 +475,3 @@
 
 // limit ISR count per byte
 #define STM32_I2C_ISR_LIMIT                 6
-
-#ifndef STM32_SDC_USE_SDMMC2
-#define STM32_SDC_USE_SDMMC2                FALSE
-#endif

libraries/AP_HAL_ChibiOS/hwdef/common/stm32l4_mcuconf.h

@@ -238,9 +238,6 @@
 /*
  * SDC driver system settings.
  */
-#ifndef STM32_SDC_USE_SDMMC1
-#define STM32_SDC_USE_SDMMC1                FALSE
-#endif
 #define STM32_SDC_SDMMC_UNALIGNED_SUPPORT   TRUE
 #define STM32_SDC_SDMMC_WRITE_TIMEOUT       1000
 #define STM32_SDC_SDMMC_READ_TIMEOUT        1000

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -4655,6 +4655,14 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &p
         return handle_command_do_set_roi_sysid(packet);
     case MAV_CMD_DO_SET_HOME:
         return handle_command_int_do_set_home(packet);
+    case MAV_CMD_STORAGE_FORMAT: {
+        if (!is_equal(packet.param1, 1.0f) ||
+            !is_equal(packet.param2, 1.0f)) {
+            return MAV_RESULT_UNSUPPORTED;
+        }
+        return AP::FS().format() ? MAV_RESULT_ACCEPTED : MAV_RESULT_FAILED;
+    }
+
 #if AP_SCRIPTING_ENABLED
     case MAV_CMD_SCRIPTING:
         {