diff --git a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp index 5883c103f5..921bde7712 100644 --- a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp +++ b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp @@ -170,9 +170,6 @@ static void main_loop() ChibiOS::SPIDevice::test_clock_freq(); #endif - //Setup SD Card and Initialise FATFS bindings - sdcard_init(); - hal.uartB->begin(38400); hal.uartC->begin(57600); hal.analogin->init(); diff --git a/libraries/AP_HAL_ChibiOS/SPIDevice.cpp b/libraries/AP_HAL_ChibiOS/SPIDevice.cpp index 5e0bbcc7fb..e40faa623d 100644 --- a/libraries/AP_HAL_ChibiOS/SPIDevice.cpp +++ b/libraries/AP_HAL_ChibiOS/SPIDevice.cpp @@ -15,6 +15,7 @@ #include "SPIDevice.h" #include +#include #include #include "Util.h" #include "Scheduler.h" @@ -138,6 +139,15 @@ bool SPIDevice::set_speed(AP_HAL::Device::Speed speed) return true; } +/* + setup a bus slowdown factor for high speed mode + */ +void SPIDevice::set_slowdown(uint8_t slowdown) +{ + slowdown = constrain_int16(slowdown+1, 1, 32); + freq_flag_high = derive_freq_flag(device_desc.highspeed / slowdown); +} + /* low level transfer function */ diff --git a/libraries/AP_HAL_ChibiOS/SPIDevice.h b/libraries/AP_HAL_ChibiOS/SPIDevice.h index b4f2df7518..ea478d8eb1 100644 --- a/libraries/AP_HAL_ChibiOS/SPIDevice.h +++ b/libraries/AP_HAL_ChibiOS/SPIDevice.h @@ -36,6 +36,7 @@ public: void dma_allocate(Shared_DMA *ctx); void dma_deallocate(Shared_DMA *ctx); bool spi_started; + uint8_t slowdown; // we need an additional lock in the dma_allocate and // dma_deallocate functions to cope with 3-way contention as we @@ -110,7 +111,10 @@ public: // used to measure clock frequencies static void test_clock_freq(void); #endif - + + // setup a bus clock slowdown factor + void set_slowdown(uint8_t slowdown) override; + private: SPIBus &bus; SPIDesc &device_desc; diff --git a/libraries/AP_HAL_ChibiOS/Util.cpp b/libraries/AP_HAL_ChibiOS/Util.cpp index af58bf0a7d..251ce44bcd 100644 --- a/libraries/AP_HAL_ChibiOS/Util.cpp +++ b/libraries/AP_HAL_ChibiOS/Util.cpp @@ -23,6 +23,7 @@ #include "hwdef/common/stm32_util.h" #include "hwdef/common/flash.h" #include +#include "sdcard.h" #if HAL_WITH_IO_MCU #include @@ -260,3 +261,13 @@ bool Util::get_system_id_unformatted(uint8_t buf[], uint8_t &len) memcpy(buf, (const void *)UDID_START, len); return true; } + +#ifdef USE_POSIX +/* + initialise filesystem + */ +bool Util::fs_init(void) +{ + return sdcard_init(); +} +#endif diff --git a/libraries/AP_HAL_ChibiOS/Util.h b/libraries/AP_HAL_ChibiOS/Util.h index 068d3a14dd..49219d0641 100644 --- a/libraries/AP_HAL_ChibiOS/Util.h +++ b/libraries/AP_HAL_ChibiOS/Util.h @@ -51,6 +51,13 @@ public: void toneAlarm_set_buzzer_tone(float frequency, float volume, uint32_t duration_ms) override; #endif +#ifdef USE_POSIX + /* + initialise (or re-initialise) filesystem storage + */ + bool fs_init(void) override; +#endif + private: #ifdef HAL_PWM_ALARM struct ToneAlarmPwmGroup { diff --git a/libraries/AP_HAL_ChibiOS/sdcard.cpp b/libraries/AP_HAL_ChibiOS/sdcard.cpp index 37f670898e..4f43b65159 100644 --- a/libraries/AP_HAL_ChibiOS/sdcard.cpp +++ b/libraries/AP_HAL_ChibiOS/sdcard.cpp @@ -17,60 +17,83 @@ #include "SPIDevice.h" #include "sdcard.h" #include "hwdef/common/spi_hook.h" +#include extern const AP_HAL::HAL& hal; #ifdef USE_POSIX static FATFS SDC_FS; // FATFS object -static bool sdcard_init_done; +static bool sdcard_running; #endif -#if HAL_USE_MMC_SPI +#if HAL_USE_SDC +static SDCConfig sdcconfig = { + NULL, + SDC_MODE_4BIT, + 0 +}; +#elif HAL_USE_MMC_SPI MMCDriver MMCD1; static AP_HAL::OwnPtr device; static MMCConfig mmcconfig; static SPIConfig lowspeed; static SPIConfig highspeed; -static bool sdcard_running; #endif /* - initialise microSD card if avaialble + initialise microSD card if avaialble. This is called during + AP_BoardConfig initialisation. The parameter BRD_SD_SLOWDOWN + controls a scaling factor on the microSD clock */ -void sdcard_init() +bool sdcard_init() { #ifdef USE_POSIX - if (sdcard_init_done) { - return; - } - sdcard_init_done = true; + uint8_t sd_slowdown = AP_BoardConfig::get_sdcard_slowdown(); #if HAL_USE_SDC - bouncebuffer_init(&SDCD1.bouncebuffer, 512); - - sdcStart(&SDCD1, NULL); + if (SDCD1.bouncebuffer == nullptr) { + bouncebuffer_init(&SDCD1.bouncebuffer, 512); + } - if(sdcConnect(&SDCD1) == HAL_FAILED) { - printf("Err: Failed to initialize SDIO!\n"); - } else { - if (f_mount(&SDC_FS, "/", 1) != FR_OK) { - printf("Err: Failed to mount SD Card!\n"); - sdcDisconnect(&SDCD1); - } else { - printf("Successfully mounted SDCard..\n"); + if (sdcard_running) { + sdcard_stop(); + } + + const uint8_t tries = 3; + for (uint8_t i=0; idelay(10); + sdcconfig.slowdown = sd_slowdown; + sdcStart(&SDCD1, &sdcconfig); + if(sdcConnect(&SDCD1) == HAL_FAILED) { + sdcStop(&SDCD1); + continue; } - //Create APM Directory + if (f_mount(&SDC_FS, "/", 1) != FR_OK) { + sdcDisconnect(&SDCD1); + sdcStop(&SDCD1); + continue; + } + printf("Successfully mounted SDCard (slowdown=%u)\n", (unsigned)sd_slowdown); + + // Create APM Directory if needed mkdir("/APM", 0777); + sdcard_running = true; + return true; } #elif HAL_USE_MMC_SPI + if (sdcard_running) { + sdcard_stop(); + } + + sdcard_running = true; + device = AP_HAL::get_HAL().spi->get_device("sdcard"); if (!device) { printf("No sdcard SPI device found\n"); - return; + return false; } + device->set_slowdown(sd_slowdown); - sdcard_running = true; - mmcObjectInit(&MMCD1); mmcconfig.spip = @@ -82,33 +105,29 @@ void sdcard_init() try up to 3 times to init microSD interface */ const uint8_t tries = 3; - bool start_ok = false; for (uint8_t i=0; idelay(10); mmcStart(&MMCD1, &mmcconfig); - if (mmcConnect(&MMCD1) != HAL_FAILED) { - start_ok = true; - break; + if (mmcConnect(&MMCD1) == HAL_FAILED) { + mmcStop(&MMCD1); + continue; } - mmcStop(&MMCD1); - hal.scheduler->delay(100); - } - - if (!start_ok) { - printf("Err: Failed to initialize SDCARD_SPI!\n"); - sdcard_running = false; - } else { if (f_mount(&SDC_FS, "/", 1) != FR_OK) { - printf("Err: Failed to mount SD Card!\n"); mmcDisconnect(&MMCD1); - } else { - printf("Successfully mounted SDCard..\n"); + mmcStop(&MMCD1); + continue; } - //Create APM Directory + printf("Successfully mounted SDCard (slowdown=%u)\n", (unsigned)sd_slowdown); + + // Create APM Directory if needed mkdir("/APM", 0777); + return true; } + sdcard_running = false; #endif #endif + return false; } /* @@ -120,7 +139,13 @@ void sdcard_stop(void) // unmount f_mount(nullptr, "/", 1); #endif -#if HAL_USE_MMC_SPI +#if HAL_USE_SDC + if (sdcard_running) { + sdcDisconnect(&SDCD1); + sdcStop(&SDCD1); + sdcard_running = false; + } +#elif HAL_USE_MMC_SPI if (sdcard_running) { mmcDisconnect(&MMCD1); mmcStop(&MMCD1); diff --git a/libraries/AP_HAL_ChibiOS/sdcard.h b/libraries/AP_HAL_ChibiOS/sdcard.h index acea83113d..6a6cb6c196 100644 --- a/libraries/AP_HAL_ChibiOS/sdcard.h +++ b/libraries/AP_HAL_ChibiOS/sdcard.h @@ -15,5 +15,5 @@ */ #pragma once -void sdcard_init(); +bool sdcard_init(); void sdcard_stop();