From 960d4c0a80996bc8e249eec54ac52c04fce22d8b Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 29 Dec 2018 10:50:17 +1100
Subject: [PATCH] HAL_ChibiOS: allow mount of microSD after boot

when disarmed, try to mount sd card every 3s
---
 libraries/AP_HAL_ChibiOS/Scheduler.cpp | 10 ++++++++++
 libraries/AP_HAL_ChibiOS/sdcard.cpp    | 15 +++++++++++++--
 libraries/AP_HAL_ChibiOS/sdcard.h      |  1 +
 3 files changed, 24 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.cpp b/libraries/AP_HAL_ChibiOS/Scheduler.cpp
index 611dd6bb52..623023d1e7 100644
--- a/libraries/AP_HAL_ChibiOS/Scheduler.cpp
+++ b/libraries/AP_HAL_ChibiOS/Scheduler.cpp
@@ -347,11 +347,21 @@ void Scheduler::_io_thread(void* arg)
     while (!sched->_hal_initialized) {
         sched->delay_microseconds(1000);
     }
+    uint32_t last_sd_start_ms = AP_HAL::millis();
     while (true) {
         sched->delay_microseconds(1000);
 
         // run registered IO processes
         sched->_run_io();
+
+        if (!hal.util->get_soft_armed()) {
+            // if sdcard hasn't mounted then retry it every 3s in the IO
+            // thread when disarmed
+            uint32_t now = AP_HAL::millis();
+            if (now - last_sd_start_ms > 3000) {
+                sdcard_retry();
+            }
+        }
     }
 }
 
diff --git a/libraries/AP_HAL_ChibiOS/sdcard.cpp b/libraries/AP_HAL_ChibiOS/sdcard.cpp
index 4f43b65159..106879ab13 100644
--- a/libraries/AP_HAL_ChibiOS/sdcard.cpp
+++ b/libraries/AP_HAL_ChibiOS/sdcard.cpp
@@ -18,12 +18,14 @@
 #include "sdcard.h"
 #include "hwdef/common/spi_hook.h"
 #include <AP_BoardConfig/AP_BoardConfig.h>
+#include <AP_Common/Semaphore.h>
 
 extern const AP_HAL::HAL& hal;
 
 #ifdef USE_POSIX
 static FATFS SDC_FS; // FATFS object
 static bool sdcard_running;
+static HAL_Semaphore sem;
 #endif
 
 #if HAL_USE_SDC
@@ -48,6 +50,8 @@ static SPIConfig highspeed;
 bool sdcard_init()
 {
 #ifdef USE_POSIX
+    WITH_SEMAPHORE(sem);
+
     uint8_t sd_slowdown = AP_BoardConfig::get_sdcard_slowdown();
 #if HAL_USE_SDC
 
@@ -61,7 +65,6 @@ bool sdcard_init()
 
     const uint8_t tries = 3;
     for (uint8_t i=0; i<tries; i++) {
-        hal.scheduler->delay(10);
         sdcconfig.slowdown = sd_slowdown;
         sdcStart(&SDCD1, &sdcconfig);
         if(sdcConnect(&SDCD1) == HAL_FAILED) {
@@ -106,7 +109,6 @@ bool sdcard_init()
      */
     const uint8_t tries = 3;
     for (uint8_t i=0; i<tries; i++) {
-        hal.scheduler->delay(10);
         mmcStart(&MMCD1, &mmcconfig);
 
         if (mmcConnect(&MMCD1) == HAL_FAILED) {
@@ -154,6 +156,15 @@ void sdcard_stop(void)
 #endif
 }
 
+void sdcard_retry(void)
+{
+#ifdef USE_POSIX
+    if (!sdcard_running) {
+        sdcard_init();
+    }
+#endif
+}
+
 #if HAL_USE_MMC_SPI
 
 /*
diff --git a/libraries/AP_HAL_ChibiOS/sdcard.h b/libraries/AP_HAL_ChibiOS/sdcard.h
index 6a6cb6c196..766debcbb5 100644
--- a/libraries/AP_HAL_ChibiOS/sdcard.h
+++ b/libraries/AP_HAL_ChibiOS/sdcard.h
@@ -17,3 +17,4 @@
 
 bool sdcard_init();
 void sdcard_stop();
+void sdcard_retry();