HAL_ChibiOS: stop sdcard interface on reboot

This commit is contained in:
Andrew Tridgell 2018-05-26 19:00:49 +10:00
parent 86ded2c40c
commit ea37cede28
2 changed files with 8 additions and 0 deletions

View File

@ -30,6 +30,7 @@
#include <AP_Scheduler/AP_Scheduler.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#include "shared_dma.h"
#include "sdcard.h"
#if CH_CFG_USE_DYNAMIC == TRUE
@ -241,6 +242,9 @@ void Scheduler::reboot(bool hold_in_bootloader)
hal.rcout->force_safety_on();
hal.rcout->force_safety_no_wait();
// stop sdcard driver, if active
sdcard_stop();
// lock all shared DMA channels. This has the effect of waiting
// till the sensor buses are idle
Shared_DMA::lock_all();

View File

@ -94,6 +94,10 @@ void sdcard_init()
*/
void sdcard_stop(void)
{
#ifdef USE_POSIX
// unmount
f_mount(nullptr, "/", 1);
#endif
#if HAL_USE_MMC_SPI
if (sdcard_running) {
mmcDisconnect(&MMCD1);