AP_HAL_ChibiOS: add SD card support to bootloader

This commit is contained in:
Peter Barker 2022-08-18 16:45:30 +10:00 committed by Peter Barker
parent fbc9da904c
commit 48639c6810
4 changed files with 42 additions and 3 deletions

View File

@ -14,6 +14,15 @@
* *
* Code by Andrew Tridgell and Siddharth Bharat Purohit * Code by Andrew Tridgell and Siddharth Bharat Purohit
*/ */
#include <AP_HAL/AP_HAL_Boards.h>
#ifndef HAL_SCHEDULER_ENABLED
#define HAL_SCHEDULER_ENABLED 1
#endif
#if HAL_SCHEDULER_ENABLED
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <hal.h> #include <hal.h>
@ -788,5 +797,6 @@ void Scheduler::check_stack_free(void)
} }
#endif // CH_DBG_ENABLE_STACK_CHECK == TRUE #endif // CH_DBG_ENABLE_STACK_CHECK == TRUE
#endif // CH_CFG_USE_DYNAMIC #endif // CH_CFG_USE_DYNAMIC
#endif // HAL_SCHEDULER_ENABLED

View File

@ -1109,7 +1109,9 @@ def write_mcu_config(f):
#define HAL_USE_I2C FALSE #define HAL_USE_I2C FALSE
#define HAL_USE_PWM FALSE #define HAL_USE_PWM FALSE
#define HAL_NO_UARTDRIVER #define HAL_NO_UARTDRIVER
#ifndef CH_CFG_USE_DYNAMIC
#define CH_CFG_USE_DYNAMIC FALSE #define CH_CFG_USE_DYNAMIC FALSE
#endif
#define HAL_USE_EMPTY_STORAGE 1 #define HAL_USE_EMPTY_STORAGE 1
#ifndef HAL_STORAGE_SIZE #ifndef HAL_STORAGE_SIZE
#define HAL_STORAGE_SIZE 16384 #define HAL_STORAGE_SIZE 16384
@ -1136,11 +1138,19 @@ def write_mcu_config(f):
#endif #endif
#define HAL_NO_ROMFS_SUPPORT TRUE #define HAL_NO_ROMFS_SUPPORT TRUE
#define CH_CFG_USE_TM FALSE #define CH_CFG_USE_TM FALSE
#ifndef CH_CFG_USE_REGISTRY
#define CH_CFG_USE_REGISTRY FALSE #define CH_CFG_USE_REGISTRY FALSE
#endif
#ifndef CH_CFG_USE_WAITEXIT
#define CH_CFG_USE_WAITEXIT FALSE #define CH_CFG_USE_WAITEXIT FALSE
#endif
#ifndef CH_CFG_USE_MEMPOOLS
#define CH_CFG_USE_MEMPOOLS FALSE #define CH_CFG_USE_MEMPOOLS FALSE
#endif
#define CH_DBG_FILL_THREADS FALSE #define CH_DBG_FILL_THREADS FALSE
#ifndef CH_CFG_USE_MUTEXES
#define CH_CFG_USE_MUTEXES FALSE #define CH_CFG_USE_MUTEXES FALSE
#endif
#define CH_CFG_USE_EVENTS FALSE #define CH_CFG_USE_EVENTS FALSE
#define CH_CFG_USE_EVENTS_TIMEOUT FALSE #define CH_CFG_USE_EVENTS_TIMEOUT FALSE
#define HAL_USE_EMPTY_STORAGE 1 #define HAL_USE_EMPTY_STORAGE 1
@ -1149,14 +1159,22 @@ def write_mcu_config(f):
#endif #endif
#define HAL_USE_RTC FALSE #define HAL_USE_RTC FALSE
#define DISABLE_SERIAL_ESC_COMM TRUE #define DISABLE_SERIAL_ESC_COMM TRUE
#ifndef CH_CFG_USE_DYNAMIC
#define CH_CFG_USE_DYNAMIC FALSE #define CH_CFG_USE_DYNAMIC FALSE
#endif
#define DISABLE_WATCHDOG 1 #define DISABLE_WATCHDOG 1
''') ''')
if not env_vars['EXT_FLASH_SIZE_MB'] and not args.signed_fw: if not env_vars['EXT_FLASH_SIZE_MB'] and not args.signed_fw:
f.write(''' f.write('''
#ifndef CH_CFG_USE_MEMCORE
#define CH_CFG_USE_MEMCORE FALSE #define CH_CFG_USE_MEMCORE FALSE
#endif
#ifndef CH_CFG_USE_SEMAPHORES
#define CH_CFG_USE_SEMAPHORES FALSE #define CH_CFG_USE_SEMAPHORES FALSE
#endif
#ifndef CH_CFG_USE_HEAP
#define CH_CFG_USE_HEAP FALSE #define CH_CFG_USE_HEAP FALSE
#endif
''') ''')
if env_vars.get('ROMFS_UNCOMPRESSED', False): if env_vars.get('ROMFS_UNCOMPRESSED', False):
f.write('#define HAL_ROMFS_UNCOMPRESSED\n') f.write('#define HAL_ROMFS_UNCOMPRESSED\n')
@ -3014,6 +3032,8 @@ def add_bootloader_defaults(f):
// AP_Bootloader defaults // AP_Bootloader defaults
#define HAL_DSHOT_ALARM_ENABLED 0 #define HAL_DSHOT_ALARM_ENABLED 0
#define HAL_LOGGING_ENABLED 0
#define HAL_SCHEDULER_ENABLED 0
// bootloaders *definitely* don't use the FFT library: // bootloaders *definitely* don't use the FFT library:
#ifndef HAL_GYROFFT_ENABLED #ifndef HAL_GYROFFT_ENABLED

View File

@ -17,6 +17,7 @@
#include <hal.h> #include <hal.h>
#include "SPIDevice.h" #include "SPIDevice.h"
#include "sdcard.h" #include "sdcard.h"
#include "bouncebuffer.h"
#include "hwdef/common/spi_hook.h" #include "hwdef/common/spi_hook.h"
#include <AP_BoardConfig/AP_BoardConfig.h> #include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_Filesystem/AP_Filesystem.h> #include <AP_Filesystem/AP_Filesystem.h>
@ -26,9 +27,11 @@ extern const AP_HAL::HAL& hal;
#ifdef USE_POSIX #ifdef USE_POSIX
static FATFS SDC_FS; // FATFS object static FATFS SDC_FS; // FATFS object
static bool sdcard_running; #ifndef HAL_BOOTLOADER_BUILD
static HAL_Semaphore sem; static HAL_Semaphore sem;
#endif #endif
static bool sdcard_running;
#endif
#if HAL_USE_SDC #if HAL_USE_SDC
static SDCConfig sdcconfig = { static SDCConfig sdcconfig = {
@ -52,9 +55,13 @@ static SPIConfig highspeed;
bool sdcard_init() bool sdcard_init()
{ {
#ifdef USE_POSIX #ifdef USE_POSIX
#ifndef HAL_BOOTLOADER_BUILD
WITH_SEMAPHORE(sem); WITH_SEMAPHORE(sem);
uint8_t sd_slowdown = AP_BoardConfig::get_sdcard_slowdown(); uint8_t sd_slowdown = AP_BoardConfig::get_sdcard_slowdown();
#else
uint8_t sd_slowdown = 0; // maybe take from a define?
#endif
#if HAL_USE_SDC #if HAL_USE_SDC
#if STM32_SDC_USE_SDMMC2 == TRUE #if STM32_SDC_USE_SDMMC2 == TRUE
@ -134,7 +141,7 @@ bool sdcard_init()
} }
#endif #endif
sdcard_running = false; sdcard_running = false;
#endif #endif // USE_POSIX
return false; return false;
} }

View File

@ -15,6 +15,8 @@
*/ */
#pragma once #pragma once
#include <stdbool.h>
bool sdcard_init(); bool sdcard_init();
void sdcard_stop(); void sdcard_stop();
bool sdcard_retry(); bool sdcard_retry();