From 9c03579820a00112149e679f5b59fc23bc38a0a1 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Thu, 19 May 2016 19:24:55 +0200 Subject: [PATCH] *** Various Updates *** AlienFlight updates PWM updates MSP updates (compatibility with CleanFlight Configurator) Failsafe update 4wire updates for various targets SDCard support Balckbox updates USART 4/5 updates some more cleanups --- Makefile | 31 + docs/Board - AlienFlight.md | 45 + docs/Board - AlienWii32.md | 38 - docs/Spektrum bind.md | 20 +- fake_travis_build.sh | 4 +- src/main/blackbox/blackbox.c | 71 +- src/main/blackbox/blackbox.h | 2 + src/main/blackbox/blackbox_io.c | 300 +- src/main/blackbox/blackbox_io.h | 11 +- src/main/config/config.c | 168 +- src/main/config/config.h | 6 +- src/main/config/config_master.h | 3 +- src/main/drivers/bus_spi.c | 67 +- src/main/drivers/bus_spi.h | 4 + src/main/drivers/compass_ak8963.c | 4 +- src/main/drivers/light_led.c | 53 +- src/main/drivers/light_led.h | 6 +- src/main/drivers/pwm_mapping.c | 121 +- src/main/drivers/pwm_mapping.h | 21 +- src/main/drivers/pwm_output.c | 80 +- src/main/drivers/pwm_rx.c | 32 +- src/main/drivers/pwm_rx.h | 2 +- src/main/drivers/sdcard.c | 1094 ++++++ src/main/drivers/sdcard.h | 73 + src/main/drivers/sdcard_standard.c | 52 + src/main/drivers/sdcard_standard.h | 240 ++ src/main/drivers/serial_uart.c | 8 + src/main/drivers/serial_uart.h | 2 +- src/main/drivers/serial_uart_impl.h | 2 + src/main/drivers/serial_usb_vcp.c | 4 +- src/main/flight/failsafe.c | 18 +- src/main/flight/failsafe.h | 6 + src/main/flight/mixer.c | 6 +- src/main/io/asyncfatfs/asyncfatfs.c | 3660 +++++++++++++++++++++ src/main/io/asyncfatfs/asyncfatfs.h | 92 + src/main/io/asyncfatfs/fat_standard.c | 89 + src/main/io/asyncfatfs/fat_standard.h | 140 + src/main/io/beeper.c | 1 + src/main/io/i2c_bst.c | 2 +- src/main/io/serial.c | 18 +- src/main/io/serial_cli.c | 117 +- src/main/io/serial_msp.c | 283 +- src/main/io/serial_msp.h | 39 +- src/main/main.c | 82 +- src/main/mw.c | 7 +- src/main/platform.h | 4 + src/main/rx/pwm.c | 1 + src/main/rx/rx.c | 1 + src/main/scheduler.c | 1 + src/main/scheduler.h | 1 + src/main/sensors/compass.c | 2 +- src/main/sensors/initialisation.c | 33 +- src/main/target/ALIENFLIGHTF4/target.h | 10 +- src/main/target/AQ32_V2/target.h | 9 +- src/main/target/BLUEJAYF4/target.h | 30 +- src/main/target/CC3D/target.h | 13 +- src/main/target/CHEBUZZF3/target.h | 28 + src/main/target/IRCFUSIONF3/target.h | 8 +- src/main/target/KKNGF4/target.h | 27 +- src/main/target/LUX_RACE/target.h | 9 +- src/main/target/MOTOLAB/target.h | 7 +- src/main/target/NAZE/target.h | 27 +- src/main/target/QUANTON/target.h | 46 +- src/main/target/REVO/target.h | 10 +- src/main/target/REVONANO/target.h | 12 +- src/main/target/RMDO/target.h | 7 +- src/main/target/SPARKY/target.h | 7 +- src/main/target/SPARKY2/target.h | 10 +- src/main/target/SPRACINGF3/target.h | 9 +- src/main/target/STM32F3DISCOVERY/target.h | 46 +- src/main/target/VRCORE/target.h | 4 +- src/main/telemetry/smartport.c | 1 + top_makefile | 8 +- 73 files changed, 6935 insertions(+), 560 deletions(-) create mode 100644 docs/Board - AlienFlight.md delete mode 100644 docs/Board - AlienWii32.md create mode 100644 src/main/drivers/sdcard.c create mode 100644 src/main/drivers/sdcard.h create mode 100644 src/main/drivers/sdcard_standard.c create mode 100644 src/main/drivers/sdcard_standard.h create mode 100644 src/main/io/asyncfatfs/asyncfatfs.c create mode 100644 src/main/io/asyncfatfs/asyncfatfs.h create mode 100644 src/main/io/asyncfatfs/fat_standard.c create mode 100644 src/main/io/asyncfatfs/fat_standard.h diff --git a/Makefile b/Makefile index 9b539cbd0..aad673752 100755 --- a/Makefile +++ b/Makefile @@ -125,6 +125,13 @@ DEVICE_STDPERIPH_SRC := $(DEVICE_STDPERIPH_SRC)\ endif +ifeq ($(TARGET),$(filter $(TARGET),$(SDCARD_TARGET))) +INCLUDE_DIRS := $(INCLUDE_DIRS) \ + $(FATFS_DIR) + +VPATH := $(VPATH):$(FATFS_DIR) +endif + LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f303_$(FLASH_SIZE)k.ld ARCH_FLAGS = -mthumb -mcpu=cortex-m4 -mfloat-abi=hard -mfpu=fpv4-sp-d16 -fsingle-precision-constant -Wdouble-promotion @@ -716,6 +723,10 @@ ALIENFLIGHTF4_SRC = \ drivers/compass_hmc5883l.c \ drivers/light_ws2811strip.c \ drivers/light_ws2811strip_stm32f4xx.c \ + drivers/sdcard.c \ + drivers/sdcard_standard.c \ + io/asyncfatfs/asyncfatfs.c \ + io/asyncfatfs/fat_standard.c \ $(HIGHEND_SRC) \ $(COMMON_SRC) \ $(VCPF4_SRC) @@ -724,6 +735,10 @@ BLUEJAYF4_SRC = \ $(STM32F4xx_COMMON_SRC) \ drivers/accgyro_spi_mpu9250.c \ drivers/barometer_ms5611.c \ + drivers/sdcard.c \ + drivers/sdcard_standard.c \ + io/asyncfatfs/asyncfatfs.c \ + io/asyncfatfs/fat_standard.c \ $(HIGHEND_SRC) \ $(COMMON_SRC) \ $(VCPF4_SRC) @@ -741,6 +756,10 @@ AQ32_V2_SRC = \ drivers/accgyro_spi_mpu6000.c \ drivers/barometer_ms5611.c \ drivers/compass_hmc5883l.c \ + drivers/sdcard.c \ + drivers/sdcard_standard.c \ + io/asyncfatfs/asyncfatfs.c \ + io/asyncfatfs/fat_standard.c \ $(HIGHEND_SRC) \ $(COMMON_SRC) \ $(VCPF4_SRC) @@ -752,6 +771,10 @@ VRCORE_SRC = \ drivers/compass_hmc5883l.c \ drivers/light_ws2811strip.c \ drivers/light_ws2811strip_stm32f4xx.c \ + drivers/sdcard.c \ + drivers/sdcard_standard.c \ + io/asyncfatfs/asyncfatfs.c \ + io/asyncfatfs/fat_standard.c \ $(HIGHEND_SRC) \ $(COMMON_SRC) \ $(VCPF4_SRC) @@ -784,6 +807,10 @@ STM32F3DISCOVERY_SRC = \ drivers/barometer_ms5611.c \ drivers/barometer_bmp280.c \ drivers/compass_ak8975.c \ + drivers/sdcard.c \ + drivers/sdcard_standard.c \ + io/asyncfatfs/asyncfatfs.c \ + io/asyncfatfs/fat_standard.c \ $(HIGHEND_SRC) \ $(COMMON_SRC) @@ -946,6 +973,10 @@ SPRACINGF3MINI_SRC = \ drivers/serial_softserial.c \ drivers/serial_usb_vcp.c \ drivers/sonar_hcsr04.c \ + drivers/sdcard.c \ + drivers/sdcard_standard.c \ + io/asyncfatfs/asyncfatfs.c \ + io/asyncfatfs/fat_standard.c \ $(HIGHEND_SRC) \ $(COMMON_SRC) \ $(VCP_SRC) diff --git a/docs/Board - AlienFlight.md b/docs/Board - AlienFlight.md new file mode 100644 index 000000000..f39b5639a --- /dev/null +++ b/docs/Board - AlienFlight.md @@ -0,0 +1,45 @@ +# Board - AlienFlight (ALIENFLIGHTF1 and ALIENFLIGHTF3 target) + +AlienWii is now AlienFlight. This target supports various variants of brushed and brusless flight controllers. The designs for them are released for public use at: + +http://www.alienflight.com + +All published designs are flight tested by various people. The intention here is to make this flight controllers available and enable skilled users or RC vendors to build this designs. + +Here are the general hardware specifications for this boards: + +- STM32F103CBT6 MCU (ALIENFLIGHTF1) +- STM32F303CCT6 MCU (ALIENFLIGHTF3) +- MPU6050/6500/9250 accelerometer/gyro(/mag) sensor unit +- The MPU sensor interrupt is connected to the MCU for all new F3 designs and enabled in the firmware +- 4-8 x 4.2A to 9.5A brushed ESCs, integrated, to run the strongest micro motors +- extra-wide traces on the PCB, for maximum power throughput +- USB port, integrated +- (*) serial connection for external DSM2/DSMX sat receiver (e.g. Spektrum SAT, OrangeRx R100, Lemon RX or Deltang Rx31) +- CPPM input +- ground and 3.3V for the receiver +- hardware bind plug for easy binding +- motor connections are at the corners for a clean look with reduced wiring +- small footprint +- direct operation from an single cell lipoly battery +- 3.3V LDO power regulator (older prototypes) +- 3.3V buck-boost power converter (all new versions) +- 5V buck-boost power converter for FPV (some versions) +- battery monitoring with an LED for buzzer functionality (actually for some ALIENFLIGHTF3 variants only) + +(*) Spektrum Compatible DSM2 satellites are supported out of the box. DSMX sat will work with DSM2 protocol with default settings (DSM2, 11bit, 11ms is preset). This is chosen for maximum compatibility. For optimal connection it is recommended to adjust settings to match the capabilities of your transmitter and satellite receiver. If possible it is recommended to use the DSMX protocol since it is known as more reliable. Also to make use of additional channels you should adjust the following two parameters with the Cleanflight Configurator. + + set serialrx_provider = 1 (0 for 1024bit, 1 for 2048bit) + set spektrum_sat_bind = 5 + +For more detail of the different bind modes please refer the [Spektrum Bind](Spektrum bind.md) document + +Deltang receivers in serial mode will work like any other Spektrum satellite receiver (10bit, 22ms) only the bind process will be different. + +The pin layout for the ALIENFLIGHTF1 is very similar to NAZE32 or the related clones (MW32, Flip32, etc.). The hardware bind pin is connected to pin 41 (PB5). The pin layout for the ALIENFLIGHTF3 is similar to Sparky. The hardware bind pin is connected to pin 25 (PB12). The new AlienFlightF3 V2 design have the sensor connected via SPI and some slightly different pin layout. All AlienFlight/AlienWii F3 layouts running the same firmware which takes care on the differences with an hardware detection. + +The AlienFlight firmware will be built as target ALIENFLIGHTF1 or ALIENFLIGHTF3. The firmware image will come with alternative default settings which will give the user a plug and play experience. There is no computer needed to get this into the air with an small Quadcopter. An preconfigured custom mixer for an Octocopter is part of the default settings to allow clean straight wiring with the AlienFlight. The mixer can be activated with "mixer custom" in the CLI. To use the AlienFlight in an Hexa- or Octocopter or to do some more tuning. Additional configuration changes can be done as usual in the CLI or the Cleanflight configurator. + +## Flashing the firmware + +The firmware can be updated with the Cleanflight configurator as for any other target. All AlienFlight boards have an boot jumper which need to be closed for the initial flashing or for recovery from an broken firmware. \ No newline at end of file diff --git a/docs/Board - AlienWii32.md b/docs/Board - AlienWii32.md deleted file mode 100644 index 8697fe47f..000000000 --- a/docs/Board - AlienWii32.md +++ /dev/null @@ -1,38 +0,0 @@ -# Board - AlienWii32 (ALIENWIIF1 and ALIENWIIF3 target) - -The AlienWii32 is actually in prototype stage and few samples exist. There are some different variants and field testing with some users is ongoing. The information below is preliminary and will be updated as needed. - -Here are the hardware specifications: - -- STM32F103CBT6 MCU (ALIENWIIF1) -- STM32F303CCT6 MCU (ALIENWIIF3) -- MPU6050 accelerometer/gyro sensor unit -- 4-8 x 4.2A brushed ESCs, integrated, to run the strongest micro motors -- extra-wide traces on the PCB, for maximum power throughput -- USB port, integrated -- (*) serial connection for external DSM2/DSMX sat receiver (e.g. Spektrum SAT, OrangeRx R100, Lemon RX or Deltang Rx31) -- ground and 3.3V for the receiver -- hardware bind plug for easy binding -- motor connections are at the corners for a clean look with reduced wiring -- dimensions: 29x33mm -- direct operation from an single cell lipoly battery -- 3.3V LDO power regulator (older prototypes) -- 3.3V buck-boost power converter (newer prototypes and production versions) -- battery monitoring with an LED for buzzer functionality (actualy for an ALIENWIIF3 variant) - -(*) Spektrum Compatible DSM2 satellites are supported out of the box. DSMX sat will work with DSM2 protocol with default settings (DSM2, 11bit, 11ms is preset). This is chosen for maximum compatibility. For optimal connection it is recommended to adjust settings to match the capabilities of your transmitter and satellite receiver. If possible it is recommended to use the DSMX protocol since it is known as more reliable. Also to make use of additional channels you should adjust the following two parameters with the Cleanflight Configurator. - - set serialrx_provider = 1 (0 for 1024bit, 1 for 2048bit) - set spektrum_sat_bind = 5 - -For more detail of the different bind modes please refer the [Spektrum Bind](Spektrum bind.md) document - -Deltang receivers in serial mode will work like any other Spektrum satellite receiver (10bit, 22ms) only the bind process will be different. - -The pin layout for the ALIENWIIF1 is very similar to NAZE32 or the related clones (MW32, Flip32, etc.). The hardware bind pin is connected to pin 41 (PB5). The pin layout for the ALIENWIIF3 is similar to Sparky. The hardware bind pin is connected to pin 25 (PB12). The AlienWii32 firmware will be built as target ALIENWIIF1 or ALIENWIIF3. The firmware image will come with alternative default settings which will give the user a plug and play experience. There is no computer needed to get this into the air with an small Quadcopter. An preconfigured custom mixer for an Octocopter is part of the default settings to allow clean straight wiring with the AlienWii32. The mixer can be activated with "mixer custom" in the CLI. To use the AlienWii32 in an Hexa- or Octocopter or to do some more tuning additional configuration changes can be done as usual in the CLI or the Cleanflight configurator. - -## Flashing the firmware - -The AlienWii32 F1 board can be flashed like the Naze board or the related clones. All the different methods will work in the same way. - -The AlienWii32 F3 board needs to be flashed via the USB port in DFU mode. Flashing via the Cleanflight GUI is not possible yet. The DFU mode can be activated via setting the BOOT0 jumper during power on of the board. The second method is to connect with an terminal program (i.e. Putty) to the board and enter the character "R" immediately after connecting. Details about the flashing process can be found in the related section of the [Sparky](Board - Sparky.md) documentation. The BOOT0 jumper should be removed and the board needs to be repowerd after firmware flashing. Please be aware, during reboot of the AlienWii F3 board, the GUI will disconnect and an manual reconnect is required. \ No newline at end of file diff --git a/docs/Spektrum bind.md b/docs/Spektrum bind.md index 6d63d04d3..b06822491 100644 --- a/docs/Spektrum bind.md +++ b/docs/Spektrum bind.md @@ -2,7 +2,7 @@ Spektrum bind with hardware bind plug support. -The Spektrum bind code is actually enabled for the NAZE, NAZE32PRO, CJMCU, EUSTM32F103RC, SPARKY, CC3D, ALIENWIIF1, ALIENWIIF3 targets. +The Spektrum bind code is actually enabled for the NAZE, NAZE32PRO, CJMCU, EUSTM32F103RC, SPARKY, CC3D, ALIENFLIGHTF1, ALIENFLIGHTF3 targets. ## Configure the bind code @@ -20,7 +20,7 @@ This is to activate the hardware bind plug feature ## Hardware -The hardware bind plug will be enabled via defining HARDWARE_BIND_PLUG during building of the firmware. BINDPLUG_PORT and BINDPLUG_PIN also need to be defined (please see above). This is done automatically if the AlienWii32 firmware is built. The hardware bind plug is expected between the defined bind pin and ground. +The hardware bind plug will be enabled via defining HARDWARE_BIND_PLUG during building of the firmware. BINDPLUG_PORT and BINDPLUG_PIN also need to be defined (please see above). This is done automatically if the AlienFlight firmware is built. The hardware bind plug is expected between the defined bind pin and ground. ## Function @@ -34,15 +34,14 @@ Please refer to the satellite receiver documentation for more details of the spe ## Table with spektrum_sat_bind parameter value -| Value | Receiver mode | Notes | -| ----- | ------------------| -------------------| -| 3 | DSM2 1024bit/22ms | | -| 5 | DSM2 2048bit/11ms | default AlienWii32 | -| 7 | DSMX 1024bit/22ms | | -| 9 | DSMX 2048bit/11ms | | +| Value | Receiver mode | Notes | +| ----- | ------------------| --------------------| +| 3 | DSM2 1024bit/22ms | | +| 5 | DSM2 2048bit/11ms | default AlienFlight | +| 7 | DSMX 1024bit/22ms | | +| 8 | DSMX 2048bit/22ms | Used by new DXe | +| 9 | DSMX 2048bit/11ms | | -More detailed information regarding the satellite binding process can be found here: -http://wiki.openpilot.org/display/Doc/Spektrum+Satellite ### Supported Hardware @@ -62,3 +61,4 @@ In order to connect the satellite to a Flip32+, you have to wire the serial data | Lemon RX DSM2/DSMX | Spektrum DX8 | Bind value 5 | | Lemon RX DSMX | Walkera Devo10 | Bind value 9, Deviation firmware 4.01 up to 12 channels | | Lemon RX DSM2 | Walkera Devo7 | Bind value 9, Deviation firmware | +| Lemon RX DSMX | Spektrum DXe | Bind value 8, Supports up to 9 channels | diff --git a/fake_travis_build.sh b/fake_travis_build.sh index e37327c9a..8f20e8603 100755 --- a/fake_travis_build.sh +++ b/fake_travis_build.sh @@ -18,8 +18,8 @@ targets=("PUBLISHMETA=True" \ "TARGET=REVONANO" \ "TARGET=SPARKY" \ "TARGET=STM32F3DISCOVERY" \ - "TARGET=ALIENWIIF1" \ - "TARGET=ALIENWIIF3") + "TARGET=ALIENFLIGHTF1" \ + "TARGET=ALIENFLIGHTF3") #fake a travis build environment export TRAVIS_BUILD_NUMBER=$(date +%s) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 956b3d65b..be2885a5a 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -35,6 +35,7 @@ #include "drivers/serial.h" #include "drivers/compass.h" #include "drivers/timer.h" +#include "drivers/pwm_mapping.h" #include "drivers/pwm_rx.h" #include "drivers/accgyro.h" #include "drivers/light_led.h" @@ -256,6 +257,7 @@ static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = { typedef enum BlackboxState { BLACKBOX_STATE_DISABLED = 0, BLACKBOX_STATE_STOPPED, + BLACKBOX_STATE_PREPARE_LOG_FILE, BLACKBOX_STATE_SEND_HEADER, BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER, BLACKBOX_STATE_SEND_GPS_H_HEADER, @@ -344,6 +346,7 @@ STATIC_ASSERT((sizeof(blackboxConditionCache) * 8) >= FLIGHT_LOG_FIELD_CONDITION static uint32_t blackboxIteration; static uint16_t blackboxPFrameIndex, blackboxIFrameIndex; static uint16_t blackboxSlowFrameIterationTimer; +static bool blackboxLoggedAnyFrames; /* * We store voltages in I-frames relative to this, which was the voltage when the blackbox was activated. @@ -363,6 +366,14 @@ static blackboxMainState_t* blackboxHistory[3]; static bool blackboxModeActivationConditionPresent = false; +/** + * Return true if it is safe to edit the Blackbox configuration in the emasterConfig. + */ +bool blackboxMayEditConfig() +{ + return blackboxState <= BLACKBOX_STATE_STOPPED; +} + static bool blackboxIsOnlyLoggingIntraframes() { return masterConfig.blackbox_rate_num == 1 && masterConfig.blackbox_rate_denom == 32; } @@ -457,6 +468,9 @@ static void blackboxSetState(BlackboxState newState) { //Perform initial setup required for the new state switch (newState) { + case BLACKBOX_STATE_PREPARE_LOG_FILE: + blackboxLoggedAnyFrames = false; + break; case BLACKBOX_STATE_SEND_HEADER: blackboxHeaderBudget = 0; xmitState.headerIndex = 0; @@ -576,6 +590,8 @@ static void writeIntraframe(void) blackboxHistory[2] = blackboxHistory[0]; //And advance the current state over to a blank space ready to be filled blackboxHistory[0] = ((blackboxHistory[0] - blackboxHistoryRing + 1) % 3) + blackboxHistoryRing; + + blackboxLoggedAnyFrames = true; } static void blackboxWriteMainStateArrayUsingAveragePredictor(int arrOffsetInHistory, int count) @@ -691,6 +707,8 @@ static void writeInterframe(void) blackboxHistory[2] = blackboxHistory[1]; blackboxHistory[1] = blackboxHistory[0]; blackboxHistory[0] = ((blackboxHistory[0] - blackboxHistoryRing + 1) % 3) + blackboxHistoryRing; + + blackboxLoggedAnyFrames = true; } /* Write the contents of the global "slowHistory" to the log as an "S" frame. Because this data is logged so @@ -785,8 +803,20 @@ static void validateBlackboxConfig() masterConfig.blackbox_rate_denom /= div; } - if (masterConfig.blackbox_device >= BLACKBOX_DEVICE_END) { - masterConfig.blackbox_device = BLACKBOX_DEVICE_SERIAL; + // If we've chosen an unsupported device, change the device to serial + switch (masterConfig.blackbox_device) { +#ifdef USE_FLASHFS + case BLACKBOX_DEVICE_FLASH: +#endif +#ifdef USE_SDCARD + case BLACKBOX_DEVICE_SDCARD: +#endif + case BLACKBOX_DEVICE_SERIAL: + // Device supported, leave the setting alone + break; + + default: + masterConfig.blackbox_device = BLACKBOX_DEVICE_SERIAL; } } @@ -832,7 +862,7 @@ void startBlackbox(void) */ blackboxLastArmingBeep = getArmingBeepTimeMicros(); - blackboxSetState(BLACKBOX_STATE_SEND_HEADER); + blackboxSetState(BLACKBOX_STATE_PREPARE_LOG_FILE); } } @@ -841,18 +871,20 @@ void startBlackbox(void) */ void finishBlackbox(void) { - if (blackboxState == BLACKBOX_STATE_RUNNING || blackboxState == BLACKBOX_STATE_PAUSED) { - blackboxLogEvent(FLIGHT_LOG_EVENT_LOG_END, NULL); + switch (blackboxState) { + case BLACKBOX_STATE_DISABLED: + case BLACKBOX_STATE_STOPPED: + case BLACKBOX_STATE_SHUTTING_DOWN: + // We're already stopped/shutting down + break; - blackboxSetState(BLACKBOX_STATE_SHUTTING_DOWN); - } else if (blackboxState != BLACKBOX_STATE_DISABLED && blackboxState != BLACKBOX_STATE_STOPPED - && blackboxState != BLACKBOX_STATE_SHUTTING_DOWN) { - /* - * We're shutting down in the middle of transmitting headers, so we can't log a "log completed" event. - * Just give the port back and stop immediately. - */ - blackboxDeviceClose(); - blackboxSetState(BLACKBOX_STATE_STOPPED); + case BLACKBOX_STATE_RUNNING: + case BLACKBOX_STATE_PAUSED: + blackboxLogEvent(FLIGHT_LOG_EVENT_LOG_END, NULL); + + // Fall through + default: + blackboxSetState(BLACKBOX_STATE_SHUTTING_DOWN); } } @@ -1298,6 +1330,11 @@ void handleBlackbox(void) } switch (blackboxState) { + case BLACKBOX_STATE_PREPARE_LOG_FILE: + if (blackboxDeviceBeginLog()) { + blackboxSetState(BLACKBOX_STATE_SEND_HEADER); + } + break; case BLACKBOX_STATE_SEND_HEADER: //On entry of this state, xmitState.headerIndex is 0 and startTime is intialised @@ -1364,7 +1401,7 @@ void handleBlackbox(void) * (overflowing circular buffers causes all data to be discarded, so the first few logged iterations * could wipe out the end of the header if we weren't careful) */ - if (blackboxDeviceFlush()) { + if (blackboxDeviceFlushForce()) { blackboxSetState(BLACKBOX_STATE_RUNNING); } } @@ -1398,7 +1435,7 @@ void handleBlackbox(void) blackboxAdvanceIterationTimers(); break; case BLACKBOX_STATE_SHUTTING_DOWN: - //On entry of this state, startTime is set and a flush is performed + //On entry of this state, startTime is set /* * Wait for the log we've transmitted to make its way to the logger before we release the serial port, @@ -1406,7 +1443,7 @@ void handleBlackbox(void) * * Don't wait longer than it could possibly take if something funky happens. */ - if (millis() > xmitState.u.startTime + BLACKBOX_SHUTDOWN_TIMEOUT_MILLIS || blackboxDeviceFlush()) { + if (blackboxDeviceEndLog(blackboxLoggedAnyFrames) && (millis() > xmitState.u.startTime + BLACKBOX_SHUTDOWN_TIMEOUT_MILLIS || blackboxDeviceFlushForce())) { blackboxDeviceClose(); blackboxSetState(BLACKBOX_STATE_STOPPED); } diff --git a/src/main/blackbox/blackbox.h b/src/main/blackbox/blackbox.h index 6a94ccad6..082b8f3cb 100644 --- a/src/main/blackbox/blackbox.h +++ b/src/main/blackbox/blackbox.h @@ -25,3 +25,5 @@ void initBlackbox(void); void handleBlackbox(void); void startBlackbox(void); void finishBlackbox(void); + +bool blackboxMayEditConfig(); diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index 7c68fdbab..9d07e4528 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/src/main/blackbox/blackbox_io.c @@ -1,3 +1,20 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + #include #include #include @@ -18,6 +35,7 @@ #include "drivers/serial.h" #include "drivers/compass.h" #include "drivers/timer.h" +#include "drivers/pwm_mapping.h" #include "drivers/pwm_rx.h" #include "drivers/accgyro.h" #include "drivers/light_led.h" @@ -60,6 +78,7 @@ #include "config/config_master.h" #include "io/flashfs.h" +#include "io/asyncfatfs/asyncfatfs.h" #ifdef BLACKBOX @@ -74,6 +93,26 @@ int32_t blackboxHeaderBudget; static serialPort_t *blackboxPort = NULL; static portSharing_e blackboxPortSharing; +#ifdef USE_SDCARD + +static struct { + afatfsFilePtr_t logFile; + afatfsFilePtr_t logDirectory; + afatfsFinder_t logDirectoryFinder; + uint32_t largestLogFileNumber; + + enum { + BLACKBOX_SDCARD_INITIAL, + BLACKBOX_SDCARD_WAITING, + BLACKBOX_SDCARD_ENUMERATE_FILES, + BLACKBOX_SDCARD_CHANGE_INTO_LOG_DIRECTORY, + BLACKBOX_SDCARD_READY_TO_CREATE_LOG, + BLACKBOX_SDCARD_READY_TO_LOG + } state; +} blackboxSDCard; + +#endif + void blackboxWrite(uint8_t value) { switch (masterConfig.blackbox_device) { @@ -81,6 +120,11 @@ void blackboxWrite(uint8_t value) case BLACKBOX_DEVICE_FLASH: flashfsWriteByte(value); // Write byte asynchronously break; +#endif +#ifdef USE_SDCARD + case BLACKBOX_DEVICE_SDCARD: + afatfs_fputc(blackboxSDCard.logFile, value); + break; #endif case BLACKBOX_DEVICE_SERIAL: default: @@ -152,6 +196,13 @@ int blackboxPrint(const char *s) break; #endif +#ifdef USE_SDCARD + case BLACKBOX_DEVICE_SDCARD: + length = strlen(s); + afatfs_fwrite(blackboxSDCard.logFile, (const uint8_t*) s, length); // Ignore failures due to buffers filling up + break; +#endif + case BLACKBOX_DEVICE_SERIAL: default: pos = (uint8_t*) s; @@ -474,13 +525,36 @@ void blackboxWriteFloat(float value) /** * If there is data waiting to be written to the blackbox device, attempt to write (a portion of) that now. * - * Returns true if all data has been flushed to the device. + * Intended to be called regularly for the blackbox device to perform housekeeping. + */ +void blackboxDeviceFlush(void) +{ + switch (masterConfig.blackbox_device) { +#ifdef USE_FLASHFS + /* + * This is our only output device which requires us to call flush() in order for it to write anything. The other + * devices will progressively write in the background without Blackbox calling anything. + */ + case BLACKBOX_DEVICE_FLASH: + flashfsFlushAsync(); + break; +#endif + + default: + ; + } +} + +/** + * If there is data waiting to be written to the blackbox device, attempt to write (a portion of) that now. + * + * Returns true if all data has been written to the device. */ -bool blackboxDeviceFlush(void) +bool blackboxDeviceFlushForce(void) { switch (masterConfig.blackbox_device) { case BLACKBOX_DEVICE_SERIAL: - //Nothing to speed up flushing on serial, as serial is continuously being drained out of its buffer + // Nothing to speed up flushing on serial, as serial is continuously being drained out of its buffer return isSerialTransmitBufferEmpty(blackboxPort); #ifdef USE_FLASHFS @@ -488,6 +562,14 @@ bool blackboxDeviceFlush(void) return flashfsFlushAsync(); #endif +#ifdef USE_SDCARD + case BLACKBOX_DEVICE_SDCARD: + /* SD card will flush itself without us calling it, but we need to call flush manually in order to check + * if it's done yet or not! + */ + return afatfs_flush(); +#endif + default: return false; } @@ -551,6 +633,17 @@ bool blackboxDeviceOpen(void) return true; break; #endif +#ifdef USE_SDCARD + case BLACKBOX_DEVICE_SDCARD: + if (afatfs_getFilesystemState() == AFATFS_FILESYSTEM_STATE_FATAL || afatfs_getFilesystemState() == AFATFS_FILESYSTEM_STATE_UNKNOWN || afatfs_isFull()) { + return false; + } + + blackboxMaxHeaderBytesPerIteration = BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION; + + return true; + break; +#endif default: return false; } @@ -563,6 +656,7 @@ void blackboxDeviceClose(void) { switch (masterConfig.blackbox_device) { case BLACKBOX_DEVICE_SERIAL: + // Since the serial port could be shared with other processes, we have to give it back here closeSerialPort(blackboxPort); blackboxPort = NULL; @@ -573,12 +667,190 @@ void blackboxDeviceClose(void) if (blackboxPortSharing == PORTSHARING_SHARED) { mspAllocateSerialPorts(&masterConfig.serialConfig); } + break; + default: + ; + } +} + +#ifdef USE_SDCARD + +static void blackboxLogDirCreated(afatfsFilePtr_t directory) +{ + if (directory) { + blackboxSDCard.logDirectory = directory; + + afatfs_findFirst(blackboxSDCard.logDirectory, &blackboxSDCard.logDirectoryFinder); + + blackboxSDCard.state = BLACKBOX_SDCARD_ENUMERATE_FILES; + } else { + // Retry + blackboxSDCard.state = BLACKBOX_SDCARD_INITIAL; + } +} + +static void blackboxLogFileCreated(afatfsFilePtr_t file) +{ + if (file) { + blackboxSDCard.logFile = file; + + blackboxSDCard.largestLogFileNumber++; + + blackboxSDCard.state = BLACKBOX_SDCARD_READY_TO_LOG; + } else { + // Retry + blackboxSDCard.state = BLACKBOX_SDCARD_READY_TO_CREATE_LOG; + } +} + +static void blackboxCreateLogFile() +{ + uint32_t remainder = blackboxSDCard.largestLogFileNumber + 1; + + char filename[13]; + + filename[0] = 'L'; + filename[1] = 'O'; + filename[2] = 'G'; + + for (int i = 7; i >= 3; i--) { + filename[i] = (remainder % 10) + '0'; + remainder /= 10; + } + + filename[8] = '.'; + filename[9] = 'T'; + filename[10] = 'X'; + filename[11] = 'T'; + filename[12] = 0; + + blackboxSDCard.state = BLACKBOX_SDCARD_WAITING; + + afatfs_fopen(filename, "as", blackboxLogFileCreated); +} + +/** + * Begin a new log on the SDCard. + * + * Keep calling until the function returns true (open is complete). + */ +static bool blackboxSDCardBeginLog() +{ + fatDirectoryEntry_t *directoryEntry; + + doMore: + switch (blackboxSDCard.state) { + case BLACKBOX_SDCARD_INITIAL: + if (afatfs_getFilesystemState() == AFATFS_FILESYSTEM_STATE_READY) { + blackboxSDCard.state = BLACKBOX_SDCARD_WAITING; + + afatfs_mkdir("logs", blackboxLogDirCreated); + } break; -#ifdef USE_FLASHFS - case BLACKBOX_DEVICE_FLASH: - // No-op since the flash doesn't have a "close" and there's nobody else to hand control of it to. + + case BLACKBOX_SDCARD_WAITING: + // Waiting for directory entry to be created break; + + case BLACKBOX_SDCARD_ENUMERATE_FILES: + while (afatfs_findNext(blackboxSDCard.logDirectory, &blackboxSDCard.logDirectoryFinder, &directoryEntry) == AFATFS_OPERATION_SUCCESS) { + if (directoryEntry && !fat_isDirectoryEntryTerminator(directoryEntry)) { + // If this is a log file, parse the log number from the filename + if ( + directoryEntry->filename[0] == 'L' && directoryEntry->filename[1] == 'O' && directoryEntry->filename[2] == 'G' + && directoryEntry->filename[8] == 'T' && directoryEntry->filename[9] == 'X' && directoryEntry->filename[10] == 'T' + ) { + char logSequenceNumberString[6]; + + memcpy(logSequenceNumberString, directoryEntry->filename + 3, 5); + logSequenceNumberString[5] = '\0'; + + blackboxSDCard.largestLogFileNumber = MAX((uint32_t) atoi(logSequenceNumberString), blackboxSDCard.largestLogFileNumber); + } + } else { + // We're done checking all the files on the card, now we can create a new log file + afatfs_findLast(blackboxSDCard.logDirectory); + + blackboxSDCard.state = BLACKBOX_SDCARD_CHANGE_INTO_LOG_DIRECTORY; + goto doMore; + } + } + break; + + case BLACKBOX_SDCARD_CHANGE_INTO_LOG_DIRECTORY: + // Change into the log directory: + if (afatfs_chdir(blackboxSDCard.logDirectory)) { + // We no longer need our open handle on the log directory + afatfs_fclose(blackboxSDCard.logDirectory, NULL); + blackboxSDCard.logDirectory = NULL; + + blackboxSDCard.state = BLACKBOX_SDCARD_READY_TO_CREATE_LOG; + goto doMore; + } + break; + + case BLACKBOX_SDCARD_READY_TO_CREATE_LOG: + blackboxCreateLogFile(); + break; + + case BLACKBOX_SDCARD_READY_TO_LOG: + return true; // Log has been created! + } + + // Not finished init yet + return false; +} + #endif + +/** + * Begin a new log (for devices which support separations between the logs of multiple flights). + * + * Keep calling until the function returns true (open is complete). + */ +bool blackboxDeviceBeginLog(void) +{ + switch (masterConfig.blackbox_device) { +#ifdef USE_SDCARD + case BLACKBOX_DEVICE_SDCARD: + return blackboxSDCardBeginLog(); +#endif + default: + return true; + } + +} + +/** + * Terminate the current log (for devices which support separations between the logs of multiple flights). + * + * retainLog - Pass true if the log should be kept, or false if the log should be discarded (if supported). + * + * Keep calling until this returns true + */ +bool blackboxDeviceEndLog(bool retainLog) +{ +#ifndef USE_SDCARD + (void) retainLog; +#endif + + switch (masterConfig.blackbox_device) { +#ifdef USE_SDCARD + case BLACKBOX_DEVICE_SDCARD: + // Keep retrying until the close operation queues + if ( + (retainLog && afatfs_fclose(blackboxSDCard.logFile, NULL)) + || (!retainLog && afatfs_funlink(blackboxSDCard.logFile, NULL)) + ) { + // Don't bother waiting the for the close to complete, it's queued now and will complete eventually + blackboxSDCard.logFile = NULL; + blackboxSDCard.state = BLACKBOX_SDCARD_READY_TO_CREATE_LOG; + return true; + } + return false; +#endif + default: + return true; } } @@ -593,6 +865,11 @@ bool isBlackboxDeviceFull(void) return flashfsIsEOF(); #endif +#ifdef USE_SDCARD + case BLACKBOX_DEVICE_SDCARD: + return afatfs_isFull(); +#endif + default: return false; } @@ -614,6 +891,11 @@ void blackboxReplenishHeaderBudget() case BLACKBOX_DEVICE_FLASH: freeSpace = flashfsGetWriteBufferFreeSpace(); break; +#endif +#ifdef USE_SDCARD + case BLACKBOX_DEVICE_SDCARD: + freeSpace = afatfs_getFreeBufferSpace(); + break; #endif default: freeSpace = 0; @@ -677,6 +959,12 @@ blackboxBufferReserveStatus_e blackboxDeviceReserveBufferSpace(int32_t bytes) return BLACKBOX_RESERVE_TEMPORARY_FAILURE; #endif +#ifdef USE_SDCARD + case BLACKBOX_DEVICE_SDCARD: + // Assume that all writes will fit in the SDCard's buffers + return BLACKBOX_RESERVE_TEMPORARY_FAILURE; +#endif + default: return BLACKBOX_RESERVE_PERMANENT_FAILURE; } diff --git a/src/main/blackbox/blackbox_io.h b/src/main/blackbox/blackbox_io.h index dc201274c..37abd2695 100644 --- a/src/main/blackbox/blackbox_io.h +++ b/src/main/blackbox/blackbox_io.h @@ -26,7 +26,10 @@ typedef enum BlackboxDevice { BLACKBOX_DEVICE_SERIAL = 0, #ifdef USE_FLASHFS - BLACKBOX_DEVICE_FLASH, + BLACKBOX_DEVICE_FLASH = 1, +#endif +#ifdef USE_SDCARD + BLACKBOX_DEVICE_SDCARD = 2, #endif BLACKBOX_DEVICE_END @@ -69,10 +72,14 @@ void blackboxWriteTag8_8SVB(int32_t *values, int valueCount); void blackboxWriteU32(int32_t value); void blackboxWriteFloat(float value); -bool blackboxDeviceFlush(void); +void blackboxDeviceFlush(void); +bool blackboxDeviceFlushForce(void); bool blackboxDeviceOpen(void); void blackboxDeviceClose(void); +bool blackboxDeviceBeginLog(void); +bool blackboxDeviceEndLog(bool retainLog); + bool isBlackboxDeviceFull(void); void blackboxReplenishHeaderBudget(); diff --git a/src/main/config/config.c b/src/main/config/config.c index 666e860e9..cdd222c2f 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -23,6 +23,8 @@ #include "build_config.h" +#include "blackbox/blackbox_io.h" + #include "common/color.h" #include "common/axis.h" #include "common/maths.h" @@ -34,6 +36,7 @@ #include "drivers/system.h" #include "drivers/gpio.h" #include "drivers/timer.h" +#include "drivers/pwm_mapping.h" #include "drivers/pwm_rx.h" #include "drivers/serial.h" #include "drivers/gyro_sync.h" @@ -73,7 +76,11 @@ #include "config/config_master.h" #define BRUSHED_MOTORS_PWM_RATE 16000 -#define BRUSHLESS_MOTORS_PWM_RATE 4000 +#ifdef STM32F4 +#define BRUSHLESS_MOTORS_PWM_RATE 2000 +#else +#define BRUSHLESS_MOTORS_PWM_RATE 400 +#endif void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse); @@ -151,7 +158,7 @@ static uint32_t activeFeaturesLatch = 0; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 123; +static const uint8_t EEPROM_CONF_VERSION = 124; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { @@ -160,7 +167,7 @@ static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) accelerometerTrims->values.yaw = 0; } -static void resetPidProfile(pidProfile_t *pidProfile) +void resetPidProfile(pidProfile_t *pidProfile) { #if defined(STM32F411xE) || defined(STM32F40_41xxx) @@ -199,6 +206,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->D8[PIDVEL] = 1; pidProfile->gyro_lpf_hz = 70; // filtering ON by default +#ifdef STM32F4 pidProfile->dterm_lpf_hz = 70; // filtering ON by default pidProfile->yaw_pterm_cut_hz = 30; pidProfile->P_f[ROLL] = 5.000f; // new PID for raceflight. test carefully @@ -213,7 +221,22 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->A_level = 3.000f; pidProfile->H_level = 3.000f; pidProfile->H_sensitivity = 100; - +#else + pidProfile->dterm_lpf_hz = 40; // filtering ON by default + pidProfile->yaw_pterm_cut_hz = 30; + pidProfile->P_f[ROLL] = 1.1f; // new PID with preliminary defaults test carefully + pidProfile->I_f[ROLL] = 0.4f; + pidProfile->D_f[ROLL] = 0.01f; + pidProfile->P_f[PITCH] = 1.5f; + pidProfile->I_f[PITCH] = 0.4f; + pidProfile->D_f[PITCH] = 0.01f; + pidProfile->P_f[YAW] = 4.0f; + pidProfile->I_f[YAW] = 0.4f; + pidProfile->D_f[YAW] = 0.00f; + pidProfile->A_level = 3.000f; + pidProfile->H_level = 3.000f; + pidProfile->H_sensitivity = 100; +#endif #ifdef GTUNE pidProfile->gtune_lolimP[ROLL] = 10; // [0..200] Lower limit of ROLL P during G tune. @@ -241,6 +264,7 @@ void resetGpsProfile(gpsProfile_t *gpsProfile) } #endif +#ifdef BARO void resetBarometerConfig(barometerConfig_t *barometerConfig) { barometerConfig->baro_sample_count = 21; @@ -248,6 +272,7 @@ void resetBarometerConfig(barometerConfig_t *barometerConfig) barometerConfig->baro_cf_vel = 0.985f; barometerConfig->baro_cf_alt = 0.965f; } +#endif void resetSensorAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig) { @@ -272,6 +297,7 @@ void resetFlight3DConfig(flight3DConfig_t *flight3DConfig) flight3DConfig->deadband3d_throttle = 50; } +#ifdef TELEMETRY void resetTelemetryConfig(telemetryConfig_t *telemetryConfig) { telemetryConfig->telemetry_inversion = 0; @@ -283,6 +309,7 @@ void resetTelemetryConfig(telemetryConfig_t *telemetryConfig) telemetryConfig->frsky_vfas_precision = 0; telemetryConfig->hottAlarmSoundInterval = 5; } +#endif void resetBatteryConfig(batteryConfig_t *batteryConfig) { @@ -321,7 +348,7 @@ void resetSerialConfig(serialConfig_t *serialConfig) serialConfig->portConfigs[0].functionMask = FUNCTION_MSP; -#ifdef CC3D +#if defined(USE_VCP) // This allows MSP connection via USART & VCP so the board can be reconfigured. serialConfig->portConfigs[1].functionMask = FUNCTION_MSP; #endif @@ -420,8 +447,6 @@ static void resetConf(void) #endif featureSet(FEATURE_FAILSAFE); - featureSet(FEATURE_ONESHOT125); - featureSet(FEATURE_USE_PWM_RATE); featureSet(FEATURE_SBUS_INVERTER); // global settings @@ -448,20 +473,22 @@ static void resetConf(void) resetBatteryConfig(&masterConfig.batteryConfig); +#ifdef TELEMETRY resetTelemetryConfig(&masterConfig.telemetryConfig); +#endif #ifdef CONFIG_SERIALRX_PROVIDER - masterConfig.rxConfig.serialrx_provider = CONFIG_SERIALRX_PROVIDER; + masterConfig.rxConfig.serialrx_provider = CONFIG_SERIALRX_PROVIDER; #else - masterConfig.rxConfig.serialrx_provider = 0; + masterConfig.rxConfig.serialrx_provider = SERIALRX_SPEKTRUM1024; #endif masterConfig.rf_loop_ctrl = 2; // High DLPF, H4 #if defined(CC3D) || defined(MOTOLAB) || defined(LUX_RACE) - masterConfig.acc_hardware = 1; // default/autodetect + masterConfig.acc_hardware = ACC_NONE; // default/autodetect #endif -#if defined(STM32F40_41xxx) || defined (STM32F411xE) +#ifdef STM32F4 masterConfig.rxConfig.max_aux_channel = 99; #elif defined(STM32F303xC) masterConfig.rxConfig.max_aux_channel = 6; @@ -506,12 +533,14 @@ static void resetConf(void) resetFlight3DConfig(&masterConfig.flight3DConfig); #ifdef BRUSHED_MOTORS + masterConfig.motor_pwm_protocol = MOTOR_PWM_PROTOCOL_BRUSHED; masterConfig.motor_pwm_rate = BRUSHED_MOTORS_PWM_RATE; #else + masterConfig.motor_pwm_protocol = MOTOR_PWM_PROTOCOL_STD; masterConfig.motor_pwm_rate = BRUSHLESS_MOTORS_PWM_RATE; #endif masterConfig.servo_pwm_rate = 50; - masterConfig.use_fast_pwm = 0; + #ifdef CC3D masterConfig.use_buzzer_p6 = 0; #endif @@ -544,7 +573,9 @@ static void resetConf(void) currentProfile->accDeadband.z = 40; currentProfile->acc_unarmedcal = 1; +#ifdef BARO resetBarometerConfig(¤tProfile->barometerConfig); +#endif // Radio parseRcChannels("AETR1234", &masterConfig.rxConfig); @@ -560,6 +591,7 @@ static void resetConf(void) masterConfig.failsafeConfig.failsafe_throttle = 1000; // default throttle off. masterConfig.failsafeConfig.failsafe_kill_switch = 0; // default failsafe switch action is identical to rc link loss masterConfig.failsafeConfig.failsafe_throttle_low_delay = 100; // default throttle low delay for "just disarm" on failsafe condition + masterConfig.failsafeConfig.failsafe_procedure = 0; // default full failsafe procedure is 0: auto-landing #ifdef USE_SERVOS // servos @@ -595,19 +627,21 @@ static void resetConf(void) #ifdef CONFIG_BLACKBOX_DEVICE masterConfig.blackbox_device = CONFIG_BLACKBOX_DEVICE; #else - masterConfig.blackbox_device = 0; + masterConfig.blackbox_device = BLACKBOX_DEVICE_SERIAL; #endif masterConfig.blackbox_rate_num = 1; masterConfig.blackbox_rate_denom = 1; #endif - #ifdef CONFIG_FEATURE_RX_SERIAL featureSet(FEATURE_RX_SERIAL); #endif #ifdef CONFIG_FEATURE_ONESHOT125 - featureSet(FEATURE_ONESHOT125); + featureSet(FEATURE_ONESHOT); + masterConfig.motor_pwm_protocol = MOTOR_PWM_PROTOCOL_125; + masterConfig.motor_pwm_rate = BRUSHLESS_MOTORS_PWM_RATE; #endif + #ifdef CONFIG_MSP_PORT masterConfig.serialConfig.portConfigs[CONFIG_MSP_PORT].functionMask = FUNCTION_MSP; masterConfig.serialConfig.portConfigs[CONFIG_MSP_PORT].msp_baudrateIndex = BAUD_9600; @@ -667,87 +701,54 @@ static void resetConf(void) // alternative defaults settings for ALIENFLIGHTF1 and ALIENFLIGHTF3 targets #ifdef ALIENFLIGHT - featureSet(FEATURE_RX_SERIAL); featureSet(FEATURE_MOTOR_STOP); - featureClear(FEATURE_ONESHOT125); -#if defined(ALIENFLIGHTF3) || defined(ALIENFLIGHTF4) - masterConfig.serialConfig.portConfigs[1].functionMask = FUNCTION_MSP; //default config USART1 for MSP at 9600 for use with 1wire. - masterConfig.serialConfig.portConfigs[1].msp_baudrateIndex = BAUD_9600; - masterConfig.serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL; - masterConfig.batteryConfig.vbatscale = 20; -#else - masterConfig.serialConfig.portConfigs[1].functionMask = FUNCTION_RX_SERIAL; -#endif - masterConfig.rxConfig.serialrx_provider = 1; masterConfig.rxConfig.spektrum_sat_bind = 5; masterConfig.escAndServoConfig.minthrottle = 1000; masterConfig.escAndServoConfig.maxthrottle = 2000; + masterConfig.motor_pwm_protocol = MOTOR_PWM_PROTOCOL_BRUSHED; masterConfig.motor_pwm_rate = 32000; currentProfile->pidProfile.pidController = 2; +#ifdef ALIENFLIGHTF3 + masterConfig.batteryConfig.vbatscale = 20; + masterConfig.mag_hardware = MAG_NONE; // disabled by default + currentProfile->pidProfile.P_f[ROLL] = 1.0f; + currentProfile->pidProfile.I_f[ROLL] = 0.4f; + currentProfile->pidProfile.D_f[ROLL] = 0.01f; + currentProfile->pidProfile.P_f[PITCH] = 1.0f; + currentProfile->pidProfile.I_f[PITCH] = 0.4f; + currentProfile->pidProfile.D_f[PITCH] = 0.01f; + currentProfile->pidProfile.P_f[YAW] = 4.0f; + currentProfile->pidProfile.I_f[YAW] = 0.4f; + currentProfile->pidProfile.D_f[YAW] = 0.00f; +#endif +#ifdef ALIENFLIGHTF4 currentProfile->pidProfile.P_f[ROLL] = 5.000f; currentProfile->pidProfile.I_f[ROLL] = 1.000f; - currentProfile->pidProfile.D_f[ROLL] = 0.020f; + currentProfile->pidProfile.D_f[ROLL] = 0.080f; currentProfile->pidProfile.P_f[PITCH] = 5.000f; currentProfile->pidProfile.I_f[PITCH] = 1.000f; - currentProfile->pidProfile.D_f[PITCH] = 0.020f; + currentProfile->pidProfile.D_f[PITCH] = 0.080f; currentProfile->pidProfile.P_f[YAW] = 8.400f; - currentProfile->pidProfile.I_f[YAW] = 1.500f; - currentProfile->pidProfile.D_f[YAW] = 0.020f; + currentProfile->pidProfile.I_f[YAW] = 1.200f; + currentProfile->pidProfile.D_f[YAW] = 0.000f; +#endif masterConfig.failsafeConfig.failsafe_delay = 2; masterConfig.failsafeConfig.failsafe_off_delay = 0; + masterConfig.mixerConfig.yaw_jump_prevention_limit = YAW_JUMP_PREVENTION_LIMIT_HIGH; currentControlRateProfile->rcRate8 = 100; currentControlRateProfile->rates[FD_PITCH] = 20; currentControlRateProfile->rates[FD_ROLL] = 20; - currentControlRateProfile->rates[FD_YAW] = 20; + currentControlRateProfile->rates[FD_YAW] = 30; parseRcChannels("TAER1234", &masterConfig.rxConfig); - // { 1.0f, -0.414178f, 1.0f, -1.0f }, // REAR_R - masterConfig.customMotorMixer[0].throttle = 1.0f; - masterConfig.customMotorMixer[0].roll = -0.414178f; - masterConfig.customMotorMixer[0].pitch = 1.0f; - masterConfig.customMotorMixer[0].yaw = -1.0f; - - // { 1.0f, -0.414178f, -1.0f, 1.0f }, // FRONT_R - masterConfig.customMotorMixer[1].throttle = 1.0f; - masterConfig.customMotorMixer[1].roll = -0.414178f; - masterConfig.customMotorMixer[1].pitch = -1.0f; - masterConfig.customMotorMixer[1].yaw = 1.0f; - - // { 1.0f, 0.414178f, 1.0f, 1.0f }, // REAR_L - masterConfig.customMotorMixer[2].throttle = 1.0f; - masterConfig.customMotorMixer[2].roll = 0.414178f; - masterConfig.customMotorMixer[2].pitch = 1.0f; - masterConfig.customMotorMixer[2].yaw = 1.0f; - - // { 1.0f, 0.414178f, -1.0f, -1.0f }, // FRONT_L - masterConfig.customMotorMixer[3].throttle = 1.0f; - masterConfig.customMotorMixer[3].roll = 0.414178f; - masterConfig.customMotorMixer[3].pitch = -1.0f; - masterConfig.customMotorMixer[3].yaw = -1.0f; - - // { 1.0f, -1.0f, -0.414178f, -1.0f }, // MIDFRONT_R - masterConfig.customMotorMixer[4].throttle = 1.0f; - masterConfig.customMotorMixer[4].roll = -1.0f; - masterConfig.customMotorMixer[4].pitch = -0.414178f; - masterConfig.customMotorMixer[4].yaw = -1.0f; - - // { 1.0f, 1.0f, -0.414178f, 1.0f }, // MIDFRONT_L - masterConfig.customMotorMixer[5].throttle = 1.0f; - masterConfig.customMotorMixer[5].roll = 1.0f; - masterConfig.customMotorMixer[5].pitch = -0.414178f; - masterConfig.customMotorMixer[5].yaw = 1.0f; - - // { 1.0f, -1.0f, 0.414178f, 1.0f }, // MIDREAR_R - masterConfig.customMotorMixer[6].throttle = 1.0f; - masterConfig.customMotorMixer[6].roll = -1.0f; - masterConfig.customMotorMixer[6].pitch = 0.414178f; - masterConfig.customMotorMixer[6].yaw = 1.0f; - - // { 1.0f, 1.0f, 0.414178f, -1.0f }, // MIDREAR_L - masterConfig.customMotorMixer[7].throttle = 1.0f; - masterConfig.customMotorMixer[7].roll = 1.0f; - masterConfig.customMotorMixer[7].pitch = 0.414178f; - masterConfig.customMotorMixer[7].yaw = -1.0f; + masterConfig.customMotorMixer[0] = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R + masterConfig.customMotorMixer[1] = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R + masterConfig.customMotorMixer[2] = (motorMixer_t){ 1.0f, 0.414178f, 1.0f, 1.0f }; // REAR_L + masterConfig.customMotorMixer[3] = (motorMixer_t){ 1.0f, 0.414178f, -1.0f, -1.0f }; // FRONT_L + masterConfig.customMotorMixer[4] = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R + masterConfig.customMotorMixer[5] = (motorMixer_t){ 1.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L + masterConfig.customMotorMixer[6] = (motorMixer_t){ 1.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R + masterConfig.customMotorMixer[7] = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L #endif // copy first profile into remaining profile @@ -779,6 +780,7 @@ static bool isEEPROMContentValid(void) { const master_t *temp = (const master_t *) CONFIG_START_FLASH_ADDRESS; uint8_t checksum = 0; + // check version number if (EEPROM_CONF_VERSION != temp->version) return false; @@ -859,12 +861,14 @@ void activateConfig(void) currentProfile->throttle_correction_angle ); +#if defined(BARO) || defined(SONAR) configureAltitudeHold( ¤tProfile->pidProfile, ¤tProfile->barometerConfig, ¤tProfile->rcControlsConfig, &masterConfig.escAndServoConfig ); +#endif #ifdef BARO useBarometerConfig(¤tProfile->barometerConfig); @@ -1087,7 +1091,6 @@ void writeEEPROM(void) void ensureEEPROMContainsValidData(void) { - if (isEEPROMContentValid()) { return; } @@ -1099,7 +1102,6 @@ void resetEEPROM(void) { resetConf(); writeEEPROM(); - } void saveConfigAndNotify(void) @@ -1131,11 +1133,7 @@ void handleOneshotFeatureChangeOnRestart(void) StopPwmAllMotors(); delay(50); // Apply additional delay when OneShot125 feature changed from on to off state - if (feature(FEATURE_ONESHOT125) && !featureConfigured(FEATURE_ONESHOT125)) { - delay(ONESHOT_FEATURE_CHANGED_DELAY_ON_BOOT_MS); - } else if (feature(FEATURE_MULTISHOT) && !featureConfigured(FEATURE_MULTISHOT)) { - delay(ONESHOT_FEATURE_CHANGED_DELAY_ON_BOOT_MS); - } + delay(ONESHOT_FEATURE_CHANGED_DELAY_ON_BOOT_MS); } void latchActiveFeatures() diff --git a/src/main/config/config.h b/src/main/config/config.h index de57ceccd..d69291164 100644 --- a/src/main/config/config.h +++ b/src/main/config/config.h @@ -40,11 +40,11 @@ typedef enum { FEATURE_RSSI_ADC = 1 << 15, FEATURE_LED_STRIP = 1 << 16, FEATURE_DISPLAY = 1 << 17, - FEATURE_ONESHOT125 = 1 << 18, + FEATURE_ONESHOT = 1 << 18, FEATURE_BLACKBOX = 1 << 19, FEATURE_CHANNEL_FORWARDING = 1 << 20, - FEATURE_MULTISHOT = 1 << 21, - FEATURE_USE_PWM_RATE = 1 << 22, + FEATURE_RESERVED_MULTISHOT = 1 << 21, + FEATURE_RESERVED_USE_PWM_RATE = 1 << 22, FEATURE_RESERVED = 1 << 23, FEATURE_TX_STYLE_EXPO = 1 << 24, FEATURE_SBUS_INVERTER = 1 << 25, diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 87f0c5a72..db1d83afc 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -36,8 +36,9 @@ typedef struct master_t { flight3DConfig_t flight3DConfig; uint16_t motor_pwm_rate; // The update rate of motor outputs (50-498Hz) + uint8_t motor_pwm_protocol; // the PWM protocol utilised either: STD, 125, 42 or MS uint16_t servo_pwm_rate; // The update rate of servo outputs (50-498Hz) - uint8_t use_fast_pwm; // Use fast PWM implementation when oneshot enabled + #ifdef CC3D uint8_t use_buzzer_p6; #endif diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index 9df3b6a3f..5b078efed 100644 --- a/src/main/drivers/bus_spi.c +++ b/src/main/drivers/bus_spi.c @@ -36,7 +36,7 @@ #define GPIO_AF_SPI2 GPIO_AF_5 #endif #ifndef GPIO_AF_SPI3 -#define GPIO_AF_SPI3 GPIO_AF_5 +#define GPIO_AF_SPI3 GPIO_AF_6 #endif #endif @@ -73,14 +73,14 @@ static spiDevice_t spiHardwareMap[] = { #if defined(STM32F10X) - { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = 0 }, - { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = 0 }, + { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = 0, false }, + { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = 0, false }, #else - { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF_SPI1 }, - { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = GPIO_AF_SPI2 }, + { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF_SPI1, false }, + { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = GPIO_AF_SPI2, false }, #endif -#if defined(STM32F40_41xxx) || defined(STM32F411xE) - { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .af = GPIO_AF_SPI3 } +#if defined(STM32F303xC) || defined(STM32F40_41xxx) || defined(STM32F411xE) + { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .af = GPIO_AF_SPI3, false } #endif }; @@ -88,13 +88,13 @@ SPIDevice spiDeviceByInstance(SPI_TypeDef *instance) { if (instance == SPI1) return SPIDEV_1; - + if (instance == SPI2) return SPIDEV_2; - + if (instance == SPI3) return SPIDEV_3; - + return SPIINVALID; } @@ -103,18 +103,28 @@ void spiInitDevice(SPIDevice device) SPI_InitTypeDef spiInit; spiDevice_t *spi = &(spiHardwareMap[device]); - - // Enable SPI1 clock + +#ifdef SDCARD_SPI_INSTANCE + if (spi->dev == SDCARD_SPI_INSTANCE) + spi->sdcard = true; +#endif + + // Enable SPI clock RCC_ClockCmd(spi->rcc, ENABLE); RCC_ResetCmd(spi->rcc, ENABLE); - + IOInit(IOGetByTag(spi->sck), OWNER_SYSTEM, RESOURCE_SPI); IOInit(IOGetByTag(spi->miso), OWNER_SYSTEM, RESOURCE_SPI); IOInit(IOGetByTag(spi->mosi), OWNER_SYSTEM, RESOURCE_SPI); #if defined(STM32F303xC) || defined(STM32F40_41xxx) || defined(STM32F411xE) - IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_CFG, spi->af); - IOConfigGPIOAF(IOGetByTag(spi->miso), SPI_IO_AF_CFG, spi->af); + if (spi->sdcard) { + IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG, spi->af); + IOConfigGPIOAF(IOGetByTag(spi->miso), SPI_IO_AF_MISO_CFG, spi->af); + } else { + IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_CFG, spi->af); + IOConfigGPIOAF(IOGetByTag(spi->miso), SPI_IO_AF_CFG, spi->af); + } IOConfigGPIOAF(IOGetByTag(spi->mosi), SPI_IO_AF_CFG, spi->af); if (spi->nss) @@ -129,7 +139,7 @@ void spiInitDevice(SPIDevice device) IOConfigGPIO(IOGetByTag(spi->nss), SPI_IO_CS_CFG); #endif - // Init SPI1 hardware + // Init SPI hardware SPI_I2S_DeInit(spi->dev); spiInit.SPI_Mode = SPI_Mode_Master; @@ -138,10 +148,16 @@ void spiInitDevice(SPIDevice device) spiInit.SPI_NSS = SPI_NSS_Soft; spiInit.SPI_FirstBit = SPI_FirstBit_MSB; spiInit.SPI_CRCPolynomial = 7; - spiInit.SPI_CPOL = SPI_CPOL_High; - spiInit.SPI_CPHA = SPI_CPHA_2Edge; spiInit.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8; + if (spi->sdcard) { + spiInit.SPI_CPOL = SPI_CPOL_Low; + spiInit.SPI_CPHA = SPI_CPHA_1Edge; + } else { + spiInit.SPI_CPOL = SPI_CPOL_High; + spiInit.SPI_CPHA = SPI_CPHA_2Edge; + } + #ifdef STM32F303xC // Configure for 8-bit reads. SPI_RxFIFOThresholdConfig(spi->dev, SPI_RxFIFOThreshold_QF); @@ -220,6 +236,19 @@ uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t data) #endif } +/** + * Return true if the bus is currently in the middle of a transmission. + */ +bool spiIsBusBusy(SPI_TypeDef *instance) +{ +#ifdef STM32F303xC + return SPI_GetTransmissionFIFOStatus(instance) != SPI_TransmissionFIFOStatus_Empty || SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_BSY) == SET; +#else + return SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_TXE) == RESET || SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_BSY) == SET; +#endif + +} + bool spiTransfer(SPI_TypeDef *instance, uint8_t *out, const uint8_t *in, int len) { uint16_t spiTimeout = 1000; @@ -238,6 +267,7 @@ bool spiTransfer(SPI_TypeDef *instance, uint8_t *out, const uint8_t *in, int len #else SPI_I2S_SendData(instance, b); #endif + spiTimeout = 1000; while (SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_RXNE) == RESET) { if ((spiTimeout--) == 0) return spiTimeoutUserCallback(instance); @@ -327,4 +357,3 @@ void spiResetErrorCounter(SPI_TypeDef *instance) if (device != SPIINVALID) spiHardwareMap[device].errorCount = 0; } - diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index 6c3a40ada..9d1f7aed1 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -23,6 +23,8 @@ #if defined(STM32F40_41xxx) || defined (STM32F411xE) || defined(STM32F303xC) #define SPI_IO_AF_CFG IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_NOPULL) +#define SPI_IO_AF_SCK_CFG IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_DOWN) +#define SPI_IO_AF_MISO_CFG IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_UP) #define SPI_IO_CS_CFG IO_CONFIG(GPIO_Mode_OUT, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_NOPULL) #elif defined(STM32F10X) #define SPI_IO_AF_CFG IO_CONFIG(GPIO_Mode_AF_OD, GPIO_Speed_50MHz) @@ -63,11 +65,13 @@ typedef struct SPIDevice_s { rccPeriphTag_t rcc; uint8_t af; volatile uint16_t errorCount; + bool sdcard; } spiDevice_t; bool spiInit(SPIDevice device); void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor); uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t in); +bool spiIsBusBusy(SPI_TypeDef *instance); bool spiTransfer(SPI_TypeDef *instance, uint8_t *out, const uint8_t *in, int len); diff --git a/src/main/drivers/compass_ak8963.c b/src/main/drivers/compass_ak8963.c index 600b52ad3..42ffeb642 100644 --- a/src/main/drivers/compass_ak8963.c +++ b/src/main/drivers/compass_ak8963.c @@ -95,7 +95,7 @@ typedef struct ak8963Configuration_s { ak8963Configuration_t ak8963config; static float magGain[3] = { 1.0f, 1.0f, 1.0f }; -#ifdef USE_SPI +#if defined(USE_SPI) && defined(USE_GYRO_SPI_MPU9250) bool ak8963SPIRead(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *buf) { verifympu9250WriteRegister(MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read @@ -147,7 +147,7 @@ bool ak8963Detect(mag_t *mag) } #endif -#ifdef USE_SPI +#if defined(USE_SPI) && defined(USE_GYRO_SPI_MPU9250) // check for AK8963 on I2C master via SPI bus (as part of MPU9250) ack = verifympu9250WriteRegister(MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR diff --git a/src/main/drivers/light_led.c b/src/main/drivers/light_led.c index 0154dc93e..9238acd99 100644 --- a/src/main/drivers/light_led.c +++ b/src/main/drivers/light_led.c @@ -43,7 +43,24 @@ static const IO_t leds[] = { DEFIO_IO(LED2), #else DEFIO_IO(NONE), -#endif +#endif +#if defined(LED0_A) || defined(LED1_A) || defined(LED2_A) +#ifdef LED0_A + DEFIO_IO(LED0_A), +#else + DEFIO_IO(NONE), +#endif +#ifdef LED1_A + DEFIO_IO(LED1_A), +#else + DEFIO_IO(NONE), +#endif +#ifdef LED2_A + DEFIO_IO(LED2_A), +#else + DEFIO_IO(NONE), +#endif +#endif }; uint8_t ledPolarity = 0 @@ -55,21 +72,39 @@ uint8_t ledPolarity = 0 #endif #ifdef LED2_INVERTED | BIT(2) +#endif +#ifdef LED0_A_INVERTED + | BIT(3) +#endif +#ifdef LED1_A_INVERTED + | BIT(4) +#endif +#ifdef LED2_A_INVERTED + | BIT(5) #endif ; -void ledInit(void) +uint8_t ledOffset = 0; + +void ledInit(bool alternative_led) { uint32_t i; +#if defined(LED0_A) || defined(LED1_A) || defined(LED2_A) + if (alternative_led) + ledOffset = LED_NUMBER; +#else + UNUSED(alternative_led); +#endif + LED0_OFF; LED1_OFF; LED2_OFF; - for (i = 0; i < ARRAYLEN(leds); i++) { - if (leds[i]) { - IOInit(leds[i], OWNER_SYSTEM, RESOURCE_OUTPUT); - IOConfigGPIO(leds[i], IOCFG_OUT_PP); + for (i = 0; i < LED_NUMBER; i++) { + if (leds[i + ledOffset]) { + IOInit(leds[i + ledOffset], OWNER_SYSTEM, RESOURCE_OUTPUT); + IOConfigGPIO(leds[i + ledOffset], IOCFG_OUT_PP); } } @@ -80,11 +115,11 @@ void ledInit(void) void ledToggle(int led) { - IOToggle(leds[led]); + IOToggle(leds[led + ledOffset]); } void ledSet(int led, bool on) { - bool inverted = (1 << led) & ledPolarity; - IOWrite(leds[led], on ? inverted : !inverted); + bool inverted = (1 << (led + ledOffset)) & ledPolarity; + IOWrite(leds[led + ledOffset], on ? inverted : !inverted); } diff --git a/src/main/drivers/light_led.h b/src/main/drivers/light_led.h index 5b2fb95ed..55814e1a1 100644 --- a/src/main/drivers/light_led.h +++ b/src/main/drivers/light_led.h @@ -17,6 +17,8 @@ #pragma once +#define LED_NUMBER 3 + // Helpful macros #ifdef LED0 # define LED0_TOGGLE ledToggle(0) @@ -48,6 +50,6 @@ # define LED2_ON do {} while(0) #endif -void ledInit(void); +void ledInit(bool alternative_led); void ledToggle(int led); -void ledSet(int led, bool state); \ No newline at end of file +void ledSet(int led, bool state); diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 581968143..3902cc2e7 100755 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -25,17 +25,13 @@ #include "gpio.h" #include "timer.h" +#include "pwm_mapping.h" #include "pwm_output.h" #include "pwm_rx.h" -#include "pwm_mapping.h" -void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse); +void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate); void pwmBrushlessMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse); -void fastPWMMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse); -void pwmOneshotPwmRateMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse); -void pwmOneshotMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex); -void pwmMultiShotPwmRateMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse); -void pwmMultiShotMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex); +void pwmOneShotMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, pwmMotorProtocol_e protocol); void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse); /* @@ -1243,6 +1239,7 @@ if (init->useBuzzerP6) { #endif +/* TODO: For F4 targets ADC is currently using IO mapping so this no longer holds true. Ok for the minute as no clashes (more pins on F4 targets). */ #ifdef VBAT_ADC_GPIO if (init->useVbat && timerHardwarePtr->gpio == VBAT_ADC_GPIO && timerHardwarePtr->pin == VBAT_ADC_GPIO_PIN) { continue; @@ -1357,56 +1354,53 @@ if (init->useBuzzerP6) { } #endif +#define AVOIDANCE_CONDITION (!(init->motorPwmProtocol == MOTOR_PWM_PROTOCOL_STD)) + if (type == MAP_TO_PPM_INPUT) { #ifdef REVO - if (init->useMultiShot || init->useOneshot || isMotorBrushed(init->motorPwmRate)) { - ppmAvoidPWMTimerClash(timerHardwarePtr, TIM12); - ppmAvoidPWMTimerClash(timerHardwarePtr, TIM8); + if (AVOIDANCE_CONDITION) { + ppmAvoidPWMTimerClash(timerHardwarePtr, TIM12, init); + ppmAvoidPWMTimerClash(timerHardwarePtr, TIM8, init); } #endif -#ifdef REVONANO - if (init->useMultiShot || init->useOneshot || isMotorBrushed(init->motorPwmRate)) { - ppmAvoidPWMTimerClash(timerHardwarePtr, TIM2); +#if defined(REVONANO) || defined(SPARKY) || defined(ALIENFLIGHTF3) + if (AVOIDANCE_CONDITION) { + ppmAvoidPWMTimerClash(timerHardwarePtr, TIM2, init); } #endif #ifdef SPARKY2 - if (init->useMultiShot || init->useOneshot || isMotorBrushed(init->motorPwmRate)) { - ppmAvoidPWMTimerClash(timerHardwarePtr, TIM8); + if (AVOIDANCE_CONDITION) { + ppmAvoidPWMTimerClash(timerHardwarePtr, TIM8, init); } #endif #ifdef ALIENFLIGHTF4 - if (init->useMultiShot || init->useOneshot || isMotorBrushed(init->motorPwmRate)) { - ppmAvoidPWMTimerClash(timerHardwarePtr, TIM1); + if (AVOIDANCE_CONDITION) { + ppmAvoidPWMTimerClash(timerHardwarePtr, TIM1, init); } #endif #ifdef AQ32_V2 - if (init->useMultiShot || init->useOneshot || isMotorBrushed(init->motorPwmRate)) { - ppmAvoidPWMTimerClash(timerHardwarePtr, TIM4); + if (AVOIDANCE_CONDITION) { + ppmAvoidPWMTimerClash(timerHardwarePtr, TIM4, init); } #endif #ifdef VRCORE - if (init->useMultiShot || init->useOneshot || isMotorBrushed(init->motorPwmRate)) { - ppmAvoidPWMTimerClash(timerHardwarePtr, TIM1); + if (AVOIDANCE_CONDITION) { + ppmAvoidPWMTimerClash(timerHardwarePtr, TIM1, init); } #endif #ifdef YOUPIF4 - if (init->useMultiShot || init->useOneshot || isMotorBrushed(init->motorPwmRate)) { - ppmAvoidPWMTimerClash(timerHardwarePtr, TIM8); + if (AVOIDANCE_CONDITION) { + ppmAvoidPWMTimerClash(timerHardwarePtr, TIM8, init); } #endif //#ifdef KKNGF4 -// if (init->useMultiShot || init->useOneshot || isMotorBrushed(init->motorPwmRate)) { -// ppmAvoidPWMTimerClash(timerHardwarePtr, TIM8); +// if (AVOIDANCE_CONDITION) { +// ppmAvoidPWMTimerClash(timerHardwarePtr, TIM8, init); // } //#endif #ifdef CC3D - if (init->useMultiShot || init->useOneshot || isMotorBrushed(init->motorPwmRate)) { - ppmAvoidPWMTimerClash(timerHardwarePtr, TIM4); - } -#endif -#ifdef SPARKY - if (init->useMultiShot || init->useOneshot || isMotorBrushed(init->motorPwmRate)) { - ppmAvoidPWMTimerClash(timerHardwarePtr, TIM2); + if (AVOIDANCE_CONDITION) { + ppmAvoidPWMTimerClash(timerHardwarePtr, TIM2, init); } #endif ppmInConfig(timerHardwarePtr); @@ -1414,42 +1408,35 @@ if (init->useBuzzerP6) { pwmInConfig(timerHardwarePtr, channelIndex); channelIndex++; } else if (type == MAP_TO_MOTOR_OUTPUT) { - if (init->useOneshot) - { - if (init->useFastPWM) - { - fastPWMMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate, init->idlePulse); - } - else - { - if (init->usePwmRate) - { - pwmOneshotPwmRateMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate, init->idlePulse); - } - else - { - pwmOneshotMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount); - } - } - pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_ONESHOT|PWM_PF_OUTPUT_PROTOCOL_PWM ; - } - else if (init->useMultiShot) { - if (init->usePwmRate) - { - pwmMultiShotPwmRateMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate, init->idlePulse); - } - else - { - pwmMultiShotMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount); - } - pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_MULTISHOT|PWM_PF_OUTPUT_PROTOCOL_PWM ; - } else if (isMotorBrushed(init->motorPwmRate)) { - pwmBrushedMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate, init->idlePulse); - pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_MOTOR_MODE_BRUSHED | PWM_PF_OUTPUT_PROTOCOL_PWM; - } else { - pwmBrushlessMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate, init->idlePulse); - pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_PWM ; + +#ifdef CC3D + if (AVOIDANCE_CONDITION) { + // Skip it if it would cause PPM capture timer to be reconfigured or manually overflowed + if (timerHardwarePtr->tim == TIM2) + continue; } +#endif + + switch (init->motorPwmProtocol) + { + case MOTOR_PWM_PROTOCOL_125: // oneshot125 + case MOTOR_PWM_PROTOCOL_42: // oneshot42 + pwmOneShotMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate, init->motorPwmProtocol); + pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_ONESHOT | PWM_PF_OUTPUT_PROTOCOL_PWM; + break; + case MOTOR_PWM_PROTOCOL_MULTI: // multishot + pwmOneShotMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate, init->motorPwmProtocol); + pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_MULTISHOT | PWM_PF_OUTPUT_PROTOCOL_PWM; + break; + case MOTOR_PWM_PROTOCOL_BRUSHED: + pwmBrushedMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate); + pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_MOTOR_MODE_BRUSHED | PWM_PF_OUTPUT_PROTOCOL_PWM; + break; + default: + pwmBrushlessMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate, init->idlePulse); + pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_PWM; + } + pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].index = pwmOutputConfiguration.motorCount; pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].timerHardware = timerHardwarePtr; pwmOutputConfiguration.motorCount++; diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index f48ff0426..980228d1c 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -36,16 +36,14 @@ #define MAX_INPUTS 8 #define PWM_TIMER_MHZ 1 - -//these three have to be the same because of the ppmAvoidPWMTimerClash functions +#define ONESHOT42_TIMER_MHZ 24 #define ONESHOT125_TIMER_MHZ 8 -#if defined(STM32F40_41xxx) || defined(STM32F411xE) +#ifdef STM32F4 #define MULTISHOT_TIMER_MHZ 48 #else #define MULTISHOT_TIMER_MHZ 24 #endif -#define PWM_BRUSHED_TIMER_MHZ 8 - +#define PWM_BRUSHED_TIMER_MHZ 24 typedef struct sonarGPIOConfig_s { GPIO_TypeDef *gpio; @@ -65,14 +63,12 @@ typedef struct drv_pwm_config_s { #ifdef STM32F303xC bool useUART3; #endif -#if defined(STM32F40_41xxx) || defined (STM32F411xE) +#ifdef STM32F4 bool useUART2; bool useUART6; #endif bool useVbat; bool useOneshot; - bool useMultiShot; - bool usePwmRate; bool useFastPWM; bool useSoftSerial; bool useLEDStrip; @@ -90,6 +86,8 @@ typedef struct drv_pwm_config_s { #endif bool airplane; // fixed wing hardware config, lots of servos etc uint16_t motorPwmRate; + uint8_t motorPwmProtocol; + uint16_t idlePulse; // PWM value to use when initializing the driver. set this to either PULSE_1MS (regular pwm), // some higher value (used by 3d mode), or 0, for brushed pwm drivers. sonarGPIOConfig_t *sonarGPIOConfig; @@ -106,6 +104,13 @@ typedef enum { PWM_PF_OUTPUT_PROTOCOL_MULTISHOT = (1 << 5) } pwmPortFlags_e; +typedef enum { + MOTOR_PWM_PROTOCOL_STD = 0, + MOTOR_PWM_PROTOCOL_125, + MOTOR_PWM_PROTOCOL_42, + MOTOR_PWM_PROTOCOL_MULTI, + MOTOR_PWM_PROTOCOL_BRUSHED, +} pwmMotorProtocol_e; typedef struct pwmPortConfiguration_s { uint8_t index; diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 510a01c8e..60741aedd 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -28,7 +28,6 @@ #include "flight/failsafe.h" // FIXME dependency into the main code from a driver #include "pwm_mapping.h" - #include "pwm_output.h" typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function pointer used to write motors @@ -100,7 +99,6 @@ static pwmOutputPort_t *pwmOutConfig(const timerHardware_t *timerHardware, uint8 configTimeBase(timerHardware->tim, period, mhz); pwmGPIOConfig(timerHardware->gpio, timerHardware->pin, Mode_AF_PP); - pwmOCConfig(timerHardware->tim, timerHardware->channel, value); if (timerHardware->outputEnable) TIM_CtrlPWMOutputs(timerHardware->tim, ENABLE); @@ -128,7 +126,7 @@ static pwmOutputPort_t *pwmOutConfig(const timerHardware_t *timerHardware, uint8 static void pwmWriteBrushed(uint8_t index, uint16_t value) { - *motors[index]->ccr = (uint16_t)((float)((value - 1000) * motors[index]->period / 1000)); + *motors[index]->ccr = (((value - 1000) * motors[index]->period / 1000)); } static void pwmWriteStandard(uint8_t index, uint16_t value) @@ -138,15 +136,15 @@ static void pwmWriteStandard(uint8_t index, uint16_t value) static void pwmWriteOneshot(uint8_t index, uint16_t value) { - *motors[index]->ccr = (uint16_t)((float)value); + *motors[index]->ccr = value; } static void pwmWriteMultiShot(uint8_t index, uint16_t value) { -#if defined(STM32F40_41xxx) || defined(STM32F411xE) - *motors[index]->ccr = (uint16_t)((float)(value-1000) / 1.04166f)+ 240; +#ifdef STM32F4 + *motors[index]->ccr = ((value-1000) / 1.04166f) + 240; #else - *motors[index]->ccr = (uint16_t)((float)(value-1000) / 2.08333f)+ 120; + *motors[index]->ccr = ((value-1000) / 2.08333f) + 120; #endif } @@ -160,7 +158,7 @@ void pwmShutdownPulsesForAllMotors(uint8_t motorCount) { uint8_t index; - for(index = 0; index < motorCount; index++){ + for(index = 0; index < motorCount; index++) { // Set the compare register to 0, which stops the output pulsing if the timer overflows *motors[index]->ccr = 0; } @@ -181,11 +179,9 @@ void pwmCompleteOneshotMotorUpdate(uint8_t motorCount) uint8_t index; TIM_TypeDef *lastTimerPtr = NULL; - for(index = 0; index < motorCount; index++) - { + for(index = 0; index < motorCount; index++) { // Force the timer to overflow if it's the first motor to output, or if we change timers - if(motors[index]->tim != lastTimerPtr) - { + if(motors[index]->tim != lastTimerPtr) { lastTimerPtr = motors[index]->tim; timerForceOverflow(motors[index]->tim); } @@ -196,15 +192,10 @@ void pwmCompleteOneshotMotorUpdate(uint8_t motorCount) } } -bool isMotorBrushed(uint16_t motorPwmRate) -{ - return (motorPwmRate > 500); -} - -void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse) +void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate) { uint32_t hz = PWM_BRUSHED_TIMER_MHZ * 1000000; - motors[motorIndex] = pwmOutConfig(timerHardware, PWM_BRUSHED_TIMER_MHZ, hz / motorPwmRate, idlePulse); + motors[motorIndex] = pwmOutConfig(timerHardware, PWM_BRUSHED_TIMER_MHZ, hz / motorPwmRate, 0); motors[motorIndex]->pwmWritePtr = pwmWriteBrushed; } @@ -215,37 +206,28 @@ void pwmBrushlessMotorConfig(const timerHardware_t *timerHardware, uint8_t motor motors[motorIndex]->pwmWritePtr = pwmWriteStandard; } -void fastPWMMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse) -{ - uint32_t hz = PWM_BRUSHED_TIMER_MHZ * 1000000; - motors[motorIndex] = pwmOutConfig(timerHardware, PWM_BRUSHED_TIMER_MHZ, hz / motorPwmRate, idlePulse); - motors[motorIndex]->pwmWritePtr = pwmWriteStandard; -} - -void pwmOneshotPwmRateMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse) +void pwmOneShotMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, pwmMotorProtocol_e protocol) { - uint32_t hz = ONESHOT125_TIMER_MHZ * 1000000; - motors[motorIndex] = pwmOutConfig(timerHardware, ONESHOT125_TIMER_MHZ, hz / motorPwmRate, idlePulse); - motors[motorIndex]->pwmWritePtr = pwmWriteOneshot; -} - -void pwmOneshotMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex) -{ - motors[motorIndex] = pwmOutConfig(timerHardware, ONESHOT125_TIMER_MHZ, 0xFFFF, 0); - motors[motorIndex]->pwmWritePtr = pwmWriteOneshot; -} - -void pwmMultiShotPwmRateMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse) -{ - uint32_t hz = MULTISHOT_TIMER_MHZ * 1000000; - motors[motorIndex] = pwmOutConfig(timerHardware, MULTISHOT_TIMER_MHZ, hz / motorPwmRate, idlePulse); - motors[motorIndex]->pwmWritePtr = pwmWriteMultiShot; -} - -void pwmMultiShotMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex) -{ - motors[motorIndex] = pwmOutConfig(timerHardware, MULTISHOT_TIMER_MHZ, 0xFFFF, 0); - motors[motorIndex]->pwmWritePtr = pwmWriteMultiShot; + uint8_t mhz; + switch (protocol) { + case MOTOR_PWM_PROTOCOL_MULTI: + mhz = MULTISHOT_TIMER_MHZ; + break; + case MOTOR_PWM_PROTOCOL_42: + mhz = ONESHOT42_TIMER_MHZ; + break; + default: + mhz = ONESHOT125_TIMER_MHZ; + } + + if (motorPwmRate > 0) { + uint32_t hz = mhz * 1000000; + motors[motorIndex] = pwmOutConfig(timerHardware, mhz, hz / motorPwmRate, 0); + } else { + motors[motorIndex] = pwmOutConfig(timerHardware, mhz, 0xFFFF, 0); + } + + motors[motorIndex]->pwmWritePtr = (protocol == MOTOR_PWM_PROTOCOL_MULTI) ? pwmWriteMultiShot : pwmWriteOneshot; } #ifdef USE_SERVOS diff --git a/src/main/drivers/pwm_rx.c b/src/main/drivers/pwm_rx.c index 61834026d..c1155a22b 100644 --- a/src/main/drivers/pwm_rx.c +++ b/src/main/drivers/pwm_rx.c @@ -32,7 +32,7 @@ #include "timer.h" #include "pwm_mapping.h" - +#include "pwm_output.h" #include "pwm_rx.h" #define PPM_CAPTURE_COUNT 12 @@ -81,7 +81,7 @@ static uint16_t captures[PWM_PORTS_OR_PPM_CAPTURE_COUNT]; static uint8_t ppmFrameCount = 0; static uint8_t lastPPMFrameCount = 0; -static uint8_t ppmCountShift = 1; +static uint8_t ppmCountDivisor = 1; typedef struct ppmDevice_s { uint8_t pulseIndex; @@ -158,9 +158,9 @@ static void ppmEdgeCallback(timerCCHandlerRec_t* cbRec, captureCompare_t capture /* Convert to 32-bit timer result */ ppmDev.currentTime += ppmDev.largeCounter; - // Divide by 12 if Oneshot125 is active and this is a CC3D board - if (ppmCountShift > 1) { - ppmDev.currentTime = ppmDev.currentTime / ppmCountShift; + // Divide value if Oneshot, Multishot or brushed motors are active and the timer is shared + if (ppmCountDivisor > 1) { + ppmDev.currentTime = ppmDev.currentTime / ppmCountDivisor; } /* Capture computation */ @@ -326,11 +326,25 @@ void pwmInConfig(const timerHardware_t *timerHardwarePtr, uint8_t channel) #define UNUSED_PPM_TIMER_REFERENCE 0 #define FIRST_PWM_PORT 0 -void ppmAvoidPWMTimerClash(const timerHardware_t *timerHardwarePtr, TIM_TypeDef *sharedPwmTimer) +void ppmAvoidPWMTimerClash(const timerHardware_t *timerHardwarePtr, TIM_TypeDef *sharedPwmTimer, drv_pwm_config_t *init) { - if (timerHardwarePtr->tim == sharedPwmTimer) { - ppmCountShift = 12; // Divide by 12 if the timer is running at 12 MHz - } + if (timerHardwarePtr->tim == sharedPwmTimer) { + switch (init->motorPwmProtocol) + { + case MOTOR_PWM_PROTOCOL_125: + ppmCountDivisor = ONESHOT125_TIMER_MHZ; + break; + case MOTOR_PWM_PROTOCOL_42: + ppmCountDivisor = ONESHOT42_TIMER_MHZ; + break; + case MOTOR_PWM_PROTOCOL_MULTI: + ppmCountDivisor = MULTISHOT_TIMER_MHZ; + break; + case MOTOR_PWM_PROTOCOL_BRUSHED: + ppmCountDivisor = PWM_BRUSHED_TIMER_MHZ; + break; + } + } } void ppmInConfig(const timerHardware_t *timerHardwarePtr) diff --git a/src/main/drivers/pwm_rx.h b/src/main/drivers/pwm_rx.h index 46afd388c..9f8624116 100644 --- a/src/main/drivers/pwm_rx.h +++ b/src/main/drivers/pwm_rx.h @@ -26,7 +26,7 @@ typedef enum { void ppmInConfig(const timerHardware_t *timerHardwarePtr); -void ppmAvoidPWMTimerClash(const timerHardware_t *timerHardwarePtr, TIM_TypeDef *sharedPwmTimer); +void ppmAvoidPWMTimerClash(const timerHardware_t *timerHardwarePtr, TIM_TypeDef *sharedPwmTimer, drv_pwm_config_t *init); void pwmInConfig(const timerHardware_t *timerHardwarePtr, uint8_t channel); uint16_t pwmRead(uint8_t channel); diff --git a/src/main/drivers/sdcard.c b/src/main/drivers/sdcard.c new file mode 100644 index 000000000..89444d978 --- /dev/null +++ b/src/main/drivers/sdcard.c @@ -0,0 +1,1094 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#include "nvic.h" +#include "io.h" + +#include "drivers/bus_spi.h" +#include "drivers/system.h" + +#include "sdcard.h" +#include "sdcard_standard.h" + +#ifdef USE_SDCARD + +#ifdef AFATFS_USE_INTROSPECTIVE_LOGGING + #define SDCARD_PROFILING +#endif + +#define SET_CS_HIGH IOHi(sdCardCsPin) +#define SET_CS_LOW IOLo(sdCardCsPin) + +#define SDCARD_INIT_NUM_DUMMY_BYTES 10 +#define SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY 8 +// Chosen so that CMD8 will have the same CRC as CMD0: +#define SDCARD_IF_COND_CHECK_PATTERN 0xAB + +#define SDCARD_TIMEOUT_INIT_MILLIS 2000 +#define SDCARD_MAX_CONSECUTIVE_FAILURES 8 + +/* Break up 512-byte SD card sectors into chunks of this size when writing without DMA to reduce the peak overhead + * per call to sdcard_poll(). + */ +#define SDCARD_NON_DMA_CHUNK_SIZE 256 + +#define STATIC_ASSERT(condition, name ) \ + typedef char assert_failed_ ## name [(condition) ? 1 : -1 ] + +typedef enum { + // In these states we run at the initialization 400kHz clockspeed: + SDCARD_STATE_NOT_PRESENT = 0, + SDCARD_STATE_RESET, + SDCARD_STATE_CARD_INIT_IN_PROGRESS, + SDCARD_STATE_INITIALIZATION_RECEIVE_CID, + + // In these states we run at full clock speed + SDCARD_STATE_READY, + SDCARD_STATE_READING, + SDCARD_STATE_SENDING_WRITE, + SDCARD_STATE_WAITING_FOR_WRITE, + SDCARD_STATE_WRITING_MULTIPLE_BLOCKS, + SDCARD_STATE_STOPPING_MULTIPLE_BLOCK_WRITE, +} sdcardState_e; + +typedef struct sdcard_t { + struct { + uint8_t *buffer; + uint32_t blockIndex; + uint8_t chunkIndex; + + sdcard_operationCompleteCallback_c callback; + uint32_t callbackData; + +#ifdef SDCARD_PROFILING + uint32_t profileStartTime; +#endif + } pendingOperation; + + uint32_t operationStartTime; + + uint8_t failureCount; + + uint8_t version; + bool highCapacity; + + uint32_t multiWriteNextBlock; + uint32_t multiWriteBlocksRemain; + + sdcardState_e state; + + sdcardMetadata_t metadata; + sdcardCSD_t csd; + +#ifdef SDCARD_PROFILING + sdcard_profilerCallback_c profiler; +#endif +} sdcard_t; + +static sdcard_t sdcard; + +#ifdef SDCARD_SPI_CS_PIN +static IO_t sdCardCsPin = IO_NONE; +#endif +#ifdef SDCARD_DETECT_PIN +static IO_t sdCardDetctPin = IO_NONE; +#endif + + +#ifdef SDCARD_DMA_CHANNEL_TX + static bool useDMAForTx; +#else + // DMA channel not available so we can hard-code this to allow the non-DMA paths to be stripped by optimization + static const bool useDMAForTx = false; +#endif + +STATIC_ASSERT(sizeof(sdcardCSD_t) == 16, sdcard_csd_bitfields_didnt_pack_properly); + +void sdcardInsertionDetectDeinit(void) +{ +#ifdef SDCARD_DETECT_PIN + sdCardDetctPin = IOGetByTag(IO_TAG(SDCARD_DETECT_PIN)); + IOInit(sdCardDetctPin, OWNER_SYSTEM, RESOURCE_INPUT); + IOConfigGPIO(sdCardDetctPin, IOCFG_IN_FLOATING); +#endif +} + +void sdcardInsertionDetectInit(void) +{ +#ifdef SDCARD_DETECT_PIN + sdCardDetctPin = IOGetByTag(IO_TAG(SDCARD_DETECT_PIN)); + IOInit(sdCardDetctPin, OWNER_SYSTEM, RESOURCE_INPUT); + IOConfigGPIO(sdCardDetctPin, IOCFG_IPU); +#endif +} + +/** + * Detect if a SD card is physically present in the memory slot. + */ +bool sdcard_isInserted(void) +{ + bool result = true; + +#ifdef SDCARD_DETECT_PIN + + result = IORead(sdCardDetctPin) != 0; + +#ifdef SDCARD_DETECT_INVERTED + result = !result; +#endif + +#endif + + return result; +} + +/** + * Returns true if the card has already been, or is currently, initializing and hasn't encountered enough errors to + * trip our error threshold and be disabled (i.e. our card is in and working!) + */ +bool sdcard_isFunctional(void) +{ + return sdcard.state != SDCARD_STATE_NOT_PRESENT; +} + +static void sdcard_select(void) +{ + SET_CS_LOW; +} + +static void sdcard_deselect(void) +{ + // As per the SD-card spec, give the card 8 dummy clocks so it can finish its operation + //spiTransferByte(SDCARD_SPI_INSTANCE, 0xFF); + + while (spiIsBusBusy(SDCARD_SPI_INSTANCE)) { + } + + SET_CS_HIGH; +} + +/** + * Handle a failure of an SD card operation by resetting the card back to its initialization phase. + * + * Increments the failure counter, and when the failure threshold is reached, disables the card until + * the next call to sdcard_init(). + */ +static void sdcard_reset(void) +{ + if (sdcard.state >= SDCARD_STATE_READY) { + spiSetDivisor(SDCARD_SPI_INSTANCE, SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER); + } + + sdcard.failureCount++; + if (sdcard.failureCount >= SDCARD_MAX_CONSECUTIVE_FAILURES) { + sdcard.state = SDCARD_STATE_NOT_PRESENT; + } else { + sdcard.operationStartTime = millis(); + sdcard.state = SDCARD_STATE_RESET; + } +} + +/** + * The SD card spec requires 8 clock cycles to be sent by us on the bus after most commands so it can finish its + * processing of that command. The easiest way for us to do this is to just wait for the bus to become idle before + * we transmit a command, sending at least 8-bits onto the bus when we do so. + */ +static bool sdcard_waitForIdle(int maxBytesToWait) +{ + while (maxBytesToWait > 0) { + uint8_t b = spiTransferByte(SDCARD_SPI_INSTANCE, 0xFF); + if (b == 0xFF) { + return true; + } + maxBytesToWait--; + } + + return false; +} + +/** + * Wait for up to maxDelay 0xFF idle bytes to arrive from the card, returning the first non-idle byte found. + * + * Returns 0xFF on failure. + */ +static uint8_t sdcard_waitForNonIdleByte(int maxDelay) +{ + for (int i = 0; i < maxDelay + 1; i++) { // + 1 so we can wait for maxDelay '0xFF' bytes before reading a response byte afterwards + uint8_t response = spiTransferByte(SDCARD_SPI_INSTANCE, 0xFF); + + if (response != 0xFF) { + return response; + } + } + + return 0xFF; +} + +/** + * Waits up to SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY bytes for the card to become ready, send a command to the card + * with the given argument, waits up to SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY bytes for a reply, and returns the + * first non-0xFF byte of the reply. + * + * You must select the card first with sdcard_select() and deselect it afterwards with sdcard_deselect(). + * + * Upon failure, 0xFF is returned. + */ +static uint8_t sdcard_sendCommand(uint8_t commandCode, uint32_t commandArgument) +{ + uint8_t command[6] = { + 0x40 | commandCode, + commandArgument >> 24, + commandArgument >> 16, + commandArgument >> 8, + commandArgument, + 0x95 /* Static CRC. This CRC is valid for CMD0 with a 0 argument, and CMD8 with 0x1AB argument, which are the only + commands that require a CRC */ + }; + + // Go ahead and send the command even if the card isn't idle if this is the reset command + if (!sdcard_waitForIdle(SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY) && commandCode != SDCARD_COMMAND_GO_IDLE_STATE) + return 0xFF; + + spiTransfer(SDCARD_SPI_INSTANCE, NULL, command, sizeof(command)); + + /* + * The card can take up to SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY bytes to send the response, in the meantime + * it'll transmit 0xFF filler bytes. + */ + return sdcard_waitForNonIdleByte(SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY); +} + +static uint8_t sdcard_sendAppCommand(uint8_t commandCode, uint32_t commandArgument) +{ + sdcard_sendCommand(SDCARD_COMMAND_APP_CMD, 0); + + return sdcard_sendCommand(commandCode, commandArgument); +} + +/** + * Sends an IF_COND message to the card to check its version and validate its voltage requirements. Sets the global + * sdCardVersion with the detected version (0, 1, or 2) and returns true if the card is compatible. + */ +static bool sdcard_validateInterfaceCondition(void) +{ + uint8_t ifCondReply[4]; + + sdcard.version = 0; + + sdcard_select(); + + uint8_t status = sdcard_sendCommand(SDCARD_COMMAND_SEND_IF_COND, (SDCARD_VOLTAGE_ACCEPTED_2_7_to_3_6 << 8) | SDCARD_IF_COND_CHECK_PATTERN); + + // Don't deselect the card right away, because we'll want to read the rest of its reply if it's a V2 card + + if (status == (SDCARD_R1_STATUS_BIT_ILLEGAL_COMMAND | SDCARD_R1_STATUS_BIT_IDLE)) { + // V1 cards don't support this command + sdcard.version = 1; + } else if (status == SDCARD_R1_STATUS_BIT_IDLE) { + spiTransfer(SDCARD_SPI_INSTANCE, ifCondReply, NULL, sizeof(ifCondReply)); + + /* + * We don't bother to validate the SDCard's operating voltage range since the spec requires it to accept our + * 3.3V, but do check that it echoed back our check pattern properly. + */ + if (ifCondReply[3] == SDCARD_IF_COND_CHECK_PATTERN) { + sdcard.version = 2; + } + } + + sdcard_deselect(); + + return sdcard.version > 0; +} + +static bool sdcard_readOCRRegister(uint32_t *result) +{ + sdcard_select(); + + uint8_t status = sdcard_sendCommand(SDCARD_COMMAND_READ_OCR, 0); + + uint8_t response[4]; + + spiTransfer(SDCARD_SPI_INSTANCE, response, NULL, sizeof(response)); + + if (status == 0) { + sdcard_deselect(); + + *result = (response[0] << 24) | (response[1] << 16) | (response[2] << 8) | response[3]; + + return true; + } else { + sdcard_deselect(); + + return false; + } +} + +typedef enum { + SDCARD_RECEIVE_SUCCESS, + SDCARD_RECEIVE_BLOCK_IN_PROGRESS, + SDCARD_RECEIVE_ERROR, +} sdcardReceiveBlockStatus_e; + +/** + * Attempt to receive a data block from the SD card. + * + * Return true on success, otherwise the card has not responded yet and you should retry later. + */ +static sdcardReceiveBlockStatus_e sdcard_receiveDataBlock(uint8_t *buffer, int count) +{ + uint8_t dataToken = sdcard_waitForNonIdleByte(8); + + if (dataToken == 0xFF) { + return SDCARD_RECEIVE_BLOCK_IN_PROGRESS; + } + + if (dataToken != SDCARD_SINGLE_BLOCK_READ_START_TOKEN) { + return SDCARD_RECEIVE_ERROR; + } + + spiTransfer(SDCARD_SPI_INSTANCE, buffer, NULL, count); + + // Discard trailing CRC, we don't care + spiTransferByte(SDCARD_SPI_INSTANCE, 0xFF); + spiTransferByte(SDCARD_SPI_INSTANCE, 0xFF); + + return SDCARD_RECEIVE_SUCCESS; +} + +static bool sdcard_sendDataBlockFinish(void) +{ + // Send a dummy CRC + spiTransferByte(SDCARD_SPI_INSTANCE, 0x00); + spiTransferByte(SDCARD_SPI_INSTANCE, 0x00); + + uint8_t dataResponseToken = spiTransferByte(SDCARD_SPI_INSTANCE, 0xFF); + + /* + * Check if the card accepted the write (no CRC error / no address error) + * + * The lower 5 bits are structured as follows: + * | 0 | Status | 1 | + * | 0 | x x x | 1 | + * + * Statuses: + * 010 - Data accepted + * 101 - CRC error + * 110 - Write error + */ + return (dataResponseToken & 0x1F) == 0x05; +} + +/** + * Begin sending a buffer of SDCARD_BLOCK_SIZE bytes to the SD card. + */ +static void sdcard_sendDataBlockBegin(uint8_t *buffer, bool multiBlockWrite) +{ + // Card wants 8 dummy clock cycles between the write command's response and a data block beginning: + spiTransferByte(SDCARD_SPI_INSTANCE, 0xFF); + + spiTransferByte(SDCARD_SPI_INSTANCE, multiBlockWrite ? SDCARD_MULTIPLE_BLOCK_WRITE_START_TOKEN : SDCARD_SINGLE_BLOCK_WRITE_START_TOKEN); + + if (useDMAForTx) { +#ifdef SDCARD_DMA_CHANNEL_TX + // Queue the transmission of the sector payload +#ifdef SDCARD_DMA_CLK + RCC_AHB1PeriphClockCmd (SDCARD_DMA_CLK, ENABLE); +#endif + DMA_InitTypeDef DMA_InitStructure; + + DMA_StructInit(&DMA_InitStructure); +#ifdef SDCARD_DMA_CHANNEL + DMA_InitStructure.DMA_Channel = SDCARD_DMA_CHANNEL; + DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t) buffer; + DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral; +#else + DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; + DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t) buffer; + DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST; +#endif + DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t) &SDCARD_SPI_INSTANCE->DR; + DMA_InitStructure.DMA_Priority = DMA_Priority_Low; + DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; + DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; + + DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; + DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; + + DMA_InitStructure.DMA_BufferSize = SDCARD_BLOCK_SIZE; + DMA_InitStructure.DMA_Mode = DMA_Mode_Normal; + + DMA_DeInit(SDCARD_DMA_CHANNEL_TX); + DMA_Init(SDCARD_DMA_CHANNEL_TX, &DMA_InitStructure); + + DMA_Cmd(SDCARD_DMA_CHANNEL_TX, ENABLE); + + SPI_I2S_DMACmd(SDCARD_SPI_INSTANCE, SPI_I2S_DMAReq_Tx, ENABLE); +#endif + } else { + // Send the first chunk now + spiTransfer(SDCARD_SPI_INSTANCE, NULL, buffer, SDCARD_NON_DMA_CHUNK_SIZE); + } +} + +static bool sdcard_receiveCID(void) +{ + uint8_t cid[16]; + + if (sdcard_receiveDataBlock(cid, sizeof(cid)) != SDCARD_RECEIVE_SUCCESS) { + return false; + } + + sdcard.metadata.manufacturerID = cid[0]; + sdcard.metadata.oemID = (cid[1] << 8) | cid[2]; + sdcard.metadata.productName[0] = cid[3]; + sdcard.metadata.productName[1] = cid[4]; + sdcard.metadata.productName[2] = cid[5]; + sdcard.metadata.productName[3] = cid[6]; + sdcard.metadata.productName[4] = cid[7]; + sdcard.metadata.productRevisionMajor = cid[8] >> 4; + sdcard.metadata.productRevisionMinor = cid[8] & 0x0F; + sdcard.metadata.productSerial = (cid[9] << 24) | (cid[10] << 16) | (cid[11] << 8) | cid[12]; + sdcard.metadata.productionYear = (((cid[13] & 0x0F) << 4) | (cid[14] >> 4)) + 2000; + sdcard.metadata.productionMonth = cid[14] & 0x0F; + + return true; +} + +static bool sdcard_fetchCSD(void) +{ + uint32_t readBlockLen, blockCount, blockCountMult; + uint64_t capacityBytes; + + sdcard_select(); + + /* The CSD command's data block should always arrive within 8 idle clock cycles (SD card spec). This is because + * the information about card latency is stored in the CSD register itself, so we can't use that yet! + */ + bool success = + sdcard_sendCommand(SDCARD_COMMAND_SEND_CSD, 0) == 0 + && sdcard_receiveDataBlock((uint8_t*) &sdcard.csd, sizeof(sdcard.csd)) == SDCARD_RECEIVE_SUCCESS + && SDCARD_GET_CSD_FIELD(sdcard.csd, 1, TRAILER) == 1; + + if (success) { + switch (SDCARD_GET_CSD_FIELD(sdcard.csd, 1, CSD_STRUCTURE_VER)) { + case SDCARD_CSD_STRUCTURE_VERSION_1: + // Block size in bytes (doesn't have to be 512) + readBlockLen = 1 << SDCARD_GET_CSD_FIELD(sdcard.csd, 1, READ_BLOCK_LEN); + blockCountMult = 1 << (SDCARD_GET_CSD_FIELD(sdcard.csd, 1, CSIZE_MULT) + 2); + blockCount = (SDCARD_GET_CSD_FIELD(sdcard.csd, 1, CSIZE) + 1) * blockCountMult; + + // We could do this in 32 bits but it makes the 2GB case awkward + capacityBytes = (uint64_t) blockCount * readBlockLen; + + // Re-express that capacity (max 2GB) in our standard 512-byte block size + sdcard.metadata.numBlocks = capacityBytes / SDCARD_BLOCK_SIZE; + break; + case SDCARD_CSD_STRUCTURE_VERSION_2: + sdcard.metadata.numBlocks = (SDCARD_GET_CSD_FIELD(sdcard.csd, 2, CSIZE) + 1) * 1024; + break; + default: + success = false; + } + } + + sdcard_deselect(); + + return success; +} + +/** + * Check if the SD Card has completed its startup sequence. Must be called with sdcard.state == SDCARD_STATE_INITIALIZATION. + * + * Returns true if the card has finished its init process. + */ +static bool sdcard_checkInitDone(void) +{ + sdcard_select(); + + uint8_t status = sdcard_sendAppCommand(SDCARD_ACOMMAND_SEND_OP_COND, sdcard.version == 2 ? 1 << 30 /* We support high capacity cards */ : 0); + + sdcard_deselect(); + + // When card init is complete, the idle bit in the response becomes zero. + return status == 0x00; +} + +/** + * Begin the initialization process for the SD card. This must be called first before any other sdcard_ routine. + */ +void sdcard_init(bool useDMA) +{ +#ifdef SDCARD_DMA_CHANNEL_TX + useDMAForTx = useDMA; +#else + // DMA is not available + (void) useDMA; +#endif +#ifdef SDCARD_SPI_CS_PIN + sdCardCsPin = IOGetByTag(IO_TAG(SDCARD_SPI_CS_PIN)); + IOInit(sdCardCsPin, OWNER_SYSTEM, RESOURCE_SPI); + IOConfigGPIO(sdCardCsPin, SPI_IO_CS_CFG); +#endif + + // Max frequency is initially 400kHz + spiSetDivisor(SDCARD_SPI_INSTANCE, SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER); + + // SDCard wants 1ms minimum delay after power is applied to it + delay(1000); + + // Transmit at least 74 dummy clock cycles with CS high so the SD card can start up + SET_CS_HIGH; + + spiTransfer(SDCARD_SPI_INSTANCE, NULL, NULL, SDCARD_INIT_NUM_DUMMY_BYTES); + + // Wait for that transmission to finish before we enable the SDCard, so it receives the required number of cycles: + while (spiIsBusBusy(SDCARD_SPI_INSTANCE)) { + } + + sdcard.operationStartTime = millis(); + sdcard.state = SDCARD_STATE_RESET; + sdcard.failureCount = 0; +} + +static bool sdcard_setBlockLength(uint32_t blockLen) +{ + sdcard_select(); + + uint8_t status = sdcard_sendCommand(SDCARD_COMMAND_SET_BLOCKLEN, blockLen); + + sdcard_deselect(); + + return status == 0; +} + +/* + * Returns true if the card is ready to accept read/write commands. + */ +static bool sdcard_isReady() +{ + return sdcard.state == SDCARD_STATE_READY || sdcard.state == SDCARD_STATE_WRITING_MULTIPLE_BLOCKS; +} + +/** + * Send the stop-transmission token to complete a multi-block write. + * + * Returns: + * SDCARD_OPERATION_IN_PROGRESS - We're now waiting for that stop to complete, the card will enter + * the SDCARD_STATE_STOPPING_MULTIPLE_BLOCK_WRITE state. + * SDCARD_OPERATION_SUCCESS - The multi-block write finished immediately, the card will enter + * the SDCARD_READY state. + * + */ +static sdcardOperationStatus_e sdcard_endWriteBlocks() +{ + sdcard.multiWriteBlocksRemain = 0; + + // 8 dummy clocks to guarantee N_WR clocks between the last card response and this token + spiTransferByte(SDCARD_SPI_INSTANCE, 0xFF); + + spiTransferByte(SDCARD_SPI_INSTANCE, SDCARD_MULTIPLE_BLOCK_WRITE_STOP_TOKEN); + + // Card may choose to raise a busy (non-0xFF) signal after at most N_BR (1 byte) delay + if (sdcard_waitForNonIdleByte(1) == 0xFF) { + sdcard.state = SDCARD_STATE_READY; + return SDCARD_OPERATION_SUCCESS; + } else { + sdcard.state = SDCARD_STATE_STOPPING_MULTIPLE_BLOCK_WRITE; + sdcard.operationStartTime = millis(); + + return SDCARD_OPERATION_IN_PROGRESS; + } +} + +/** + * Call periodically for the SD card to perform in-progress transfers. + * + * Returns true if the card is ready to accept commands. + */ +bool sdcard_poll(void) +{ + uint8_t initStatus; + bool sendComplete; + +#ifdef SDCARD_PROFILING + bool profilingComplete; +#endif + + doMore: + switch (sdcard.state) { + case SDCARD_STATE_RESET: + sdcard_select(); + + initStatus = sdcard_sendCommand(SDCARD_COMMAND_GO_IDLE_STATE, 0); + + sdcard_deselect(); + + if (initStatus == SDCARD_R1_STATUS_BIT_IDLE) { + // Check card voltage and version + if (sdcard_validateInterfaceCondition()) { + + sdcard.state = SDCARD_STATE_CARD_INIT_IN_PROGRESS; + goto doMore; + } else { + // Bad reply/voltage, we ought to refrain from accessing the card. + sdcard.state = SDCARD_STATE_NOT_PRESENT; + } + } + break; + + case SDCARD_STATE_CARD_INIT_IN_PROGRESS: + if (sdcard_checkInitDone()) { + if (sdcard.version == 2) { + // Check for high capacity card + uint32_t ocr; + + if (!sdcard_readOCRRegister(&ocr)) { + sdcard_reset(); + goto doMore; + } + + sdcard.highCapacity = (ocr & (1 << 30)) != 0; + } else { + // Version 1 cards are always low-capacity + sdcard.highCapacity = false; + } + + // Now fetch the CSD and CID registers + if (sdcard_fetchCSD()) { + sdcard_select(); + + uint8_t status = sdcard_sendCommand(SDCARD_COMMAND_SEND_CID, 0); + + if (status == 0) { + // Keep the card selected to receive the response block + sdcard.state = SDCARD_STATE_INITIALIZATION_RECEIVE_CID; + goto doMore; + } else { + sdcard_deselect(); + + sdcard_reset(); + goto doMore; + } + } + } + break; + case SDCARD_STATE_INITIALIZATION_RECEIVE_CID: + if (sdcard_receiveCID()) { + sdcard_deselect(); + + /* The spec is a little iffy on what the default block size is for Standard Size cards (it can be changed on + * standard size cards) so let's just set it to 512 explicitly so we don't have a problem. + */ + if (!sdcard.highCapacity && !sdcard_setBlockLength(SDCARD_BLOCK_SIZE)) { + sdcard_reset(); + goto doMore; + } + + // Now we're done with init and we can switch to the full speed clock (<25MHz) + spiSetDivisor(SDCARD_SPI_INSTANCE, SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER); + + sdcard.multiWriteBlocksRemain = 0; + + sdcard.state = SDCARD_STATE_READY; + goto doMore; + } // else keep waiting for the CID to arrive + break; + case SDCARD_STATE_SENDING_WRITE: + // Have we finished sending the write yet? + sendComplete = false; + +#ifdef SDCARD_DMA_CHANNEL_TX +#ifdef SDCARD_DMA_CHANNEL + if (useDMAForTx && DMA_GetFlagStatus(SDCARD_DMA_CHANNEL_TX, SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG) == SET) { + DMA_ClearFlag(SDCARD_DMA_CHANNEL_TX, SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG); +#else + if (useDMAForTx && DMA_GetFlagStatus(SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG) == SET) { + DMA_ClearFlag(SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG); +#endif + + DMA_Cmd(SDCARD_DMA_CHANNEL_TX, DISABLE); + + // Drain anything left in the Rx FIFO (we didn't read it during the write) + while (SPI_I2S_GetFlagStatus(SDCARD_SPI_INSTANCE, SPI_I2S_FLAG_RXNE) == SET) { + SDCARD_SPI_INSTANCE->DR; + } + + // Wait for the final bit to be transmitted + while (spiIsBusBusy(SDCARD_SPI_INSTANCE)) { + } + + SPI_I2S_DMACmd(SDCARD_SPI_INSTANCE, SPI_I2S_DMAReq_Tx, DISABLE); + + sendComplete = true; + } +#endif + + if (!useDMAForTx) { + // Send another chunk + spiTransfer(SDCARD_SPI_INSTANCE, NULL, sdcard.pendingOperation.buffer + SDCARD_NON_DMA_CHUNK_SIZE * sdcard.pendingOperation.chunkIndex, SDCARD_NON_DMA_CHUNK_SIZE); + + sdcard.pendingOperation.chunkIndex++; + + sendComplete = sdcard.pendingOperation.chunkIndex == SDCARD_BLOCK_SIZE / SDCARD_NON_DMA_CHUNK_SIZE; + } + + if (sendComplete) { + // Finish up by sending the CRC and checking the SD-card's acceptance/rejectance + if (sdcard_sendDataBlockFinish()) { + // The SD card is now busy committing that write to the card + sdcard.state = SDCARD_STATE_WAITING_FOR_WRITE; + sdcard.operationStartTime = millis(); + + // Since we've transmitted the buffer we can go ahead and tell the caller their operation is complete + if (sdcard.pendingOperation.callback) { + sdcard.pendingOperation.callback(SDCARD_BLOCK_OPERATION_WRITE, sdcard.pendingOperation.blockIndex, sdcard.pendingOperation.buffer, sdcard.pendingOperation.callbackData); + } + } else { + /* Our write was rejected! This could be due to a bad address but we hope not to attempt that, so assume + * the card is broken and needs reset. + */ + sdcard_reset(); + + // Announce write failure: + if (sdcard.pendingOperation.callback) { + sdcard.pendingOperation.callback(SDCARD_BLOCK_OPERATION_WRITE, sdcard.pendingOperation.blockIndex, NULL, sdcard.pendingOperation.callbackData); + } + + goto doMore; + } + } + break; + case SDCARD_STATE_WAITING_FOR_WRITE: + if (sdcard_waitForIdle(SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY)) { +#ifdef SDCARD_PROFILING + profilingComplete = true; +#endif + + sdcard.failureCount = 0; // Assume the card is good if it can complete a write + + // Still more blocks left to write in a multi-block chain? + if (sdcard.multiWriteBlocksRemain > 1) { + sdcard.multiWriteBlocksRemain--; + sdcard.multiWriteNextBlock++; + sdcard.state = SDCARD_STATE_WRITING_MULTIPLE_BLOCKS; + } else if (sdcard.multiWriteBlocksRemain == 1) { + // This function changes the sd card state for us whether immediately succesful or delayed: + if (sdcard_endWriteBlocks() == SDCARD_OPERATION_SUCCESS) { + sdcard_deselect(); + } else { +#ifdef SDCARD_PROFILING + // Wait for the multi-block write to be terminated before finishing timing + profilingComplete = false; +#endif + } + } else { + sdcard.state = SDCARD_STATE_READY; + sdcard_deselect(); + } + +#ifdef SDCARD_PROFILING + if (profilingComplete && sdcard.profiler) { + sdcard.profiler(SDCARD_BLOCK_OPERATION_WRITE, sdcard.pendingOperation.blockIndex, micros() - sdcard.pendingOperation.profileStartTime); + } +#endif + } else if (millis() > sdcard.operationStartTime + SDCARD_TIMEOUT_WRITE_MSEC) { + /* + * The caller has already been told that their write has completed, so they will have discarded + * their buffer and have no hope of retrying the operation. But this should be very rare and it allows + * them to reuse their buffer milliseconds faster than they otherwise would. + */ + sdcard_reset(); + goto doMore; + } + break; + case SDCARD_STATE_READING: + switch (sdcard_receiveDataBlock(sdcard.pendingOperation.buffer, SDCARD_BLOCK_SIZE)) { + case SDCARD_RECEIVE_SUCCESS: + sdcard_deselect(); + + sdcard.state = SDCARD_STATE_READY; + sdcard.failureCount = 0; // Assume the card is good if it can complete a read + +#ifdef SDCARD_PROFILING + if (sdcard.profiler) { + sdcard.profiler(SDCARD_BLOCK_OPERATION_READ, sdcard.pendingOperation.blockIndex, micros() - sdcard.pendingOperation.profileStartTime); + } +#endif + + if (sdcard.pendingOperation.callback) { + sdcard.pendingOperation.callback( + SDCARD_BLOCK_OPERATION_READ, + sdcard.pendingOperation.blockIndex, + sdcard.pendingOperation.buffer, + sdcard.pendingOperation.callbackData + ); + } + break; + case SDCARD_RECEIVE_BLOCK_IN_PROGRESS: + if (millis() <= sdcard.operationStartTime + SDCARD_TIMEOUT_READ_MSEC) { + break; // Timeout not reached yet so keep waiting + } + // Timeout has expired, so fall through to convert to a fatal error + + case SDCARD_RECEIVE_ERROR: + sdcard_deselect(); + + sdcard_reset(); + + if (sdcard.pendingOperation.callback) { + sdcard.pendingOperation.callback( + SDCARD_BLOCK_OPERATION_READ, + sdcard.pendingOperation.blockIndex, + NULL, + sdcard.pendingOperation.callbackData + ); + } + + goto doMore; + break; + } + break; + case SDCARD_STATE_STOPPING_MULTIPLE_BLOCK_WRITE: + if (sdcard_waitForIdle(SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY)) { + sdcard_deselect(); + + sdcard.state = SDCARD_STATE_READY; + +#ifdef SDCARD_PROFILING + if (sdcard.profiler) { + sdcard.profiler(SDCARD_BLOCK_OPERATION_WRITE, sdcard.pendingOperation.blockIndex, micros() - sdcard.pendingOperation.profileStartTime); + } +#endif + } else if (millis() > sdcard.operationStartTime + SDCARD_TIMEOUT_WRITE_MSEC) { + sdcard_reset(); + goto doMore; + } + break; + case SDCARD_STATE_NOT_PRESENT: + default: + ; + } + + // Is the card's initialization taking too long? + if (sdcard.state >= SDCARD_STATE_RESET && sdcard.state < SDCARD_STATE_READY + && millis() - sdcard.operationStartTime > SDCARD_TIMEOUT_INIT_MILLIS) { + sdcard_reset(); + } + + return sdcard_isReady(); +} + +/** + * Write the 512-byte block from the given buffer into the block with the given index. + * + * If the write does not complete immediately, your callback will be called later. If the write was successful, the + * buffer pointer will be the same buffer you originally passed in, otherwise the buffer will be set to NULL. + * + * Returns: + * SDCARD_OPERATION_IN_PROGRESS - Your buffer is currently being transmitted to the card and your callback will be + * called later to report the completion. The buffer pointer must remain valid until + * that time. + * SDCARD_OPERATION_SUCCESS - Your buffer has been transmitted to the card now. + * SDCARD_OPERATION_BUSY - The card is already busy and cannot accept your write + * SDCARD_OPERATION_FAILURE - Your write was rejected by the card, card will be reset + */ +sdcardOperationStatus_e sdcard_writeBlock(uint32_t blockIndex, uint8_t *buffer, sdcard_operationCompleteCallback_c callback, uint32_t callbackData) +{ + uint8_t status; + +#ifdef SDCARD_PROFILING + sdcard.pendingOperation.profileStartTime = micros(); +#endif + + doMore: + switch (sdcard.state) { + case SDCARD_STATE_WRITING_MULTIPLE_BLOCKS: + // Do we need to cancel the previous multi-block write? + if (blockIndex != sdcard.multiWriteNextBlock) { + if (sdcard_endWriteBlocks() == SDCARD_OPERATION_SUCCESS) { + // Now we've entered the ready state, we can try again + goto doMore; + } else { + return SDCARD_OPERATION_BUSY; + } + } + + // We're continuing a multi-block write + break; + case SDCARD_STATE_READY: + // We're not continuing a multi-block write so we need to send a single-block write command + sdcard_select(); + + // Standard size cards use byte addressing, high capacity cards use block addressing + status = sdcard_sendCommand(SDCARD_COMMAND_WRITE_BLOCK, sdcard.highCapacity ? blockIndex : blockIndex * SDCARD_BLOCK_SIZE); + + if (status != 0) { + sdcard_deselect(); + + sdcard_reset(); + + return SDCARD_OPERATION_FAILURE; + } + break; + default: + return SDCARD_OPERATION_BUSY; + } + + sdcard_sendDataBlockBegin(buffer, sdcard.state == SDCARD_STATE_WRITING_MULTIPLE_BLOCKS); + + sdcard.pendingOperation.buffer = buffer; + sdcard.pendingOperation.blockIndex = blockIndex; + sdcard.pendingOperation.callback = callback; + sdcard.pendingOperation.callbackData = callbackData; + sdcard.pendingOperation.chunkIndex = 1; // (for non-DMA transfers) we've sent chunk #0 already + sdcard.state = SDCARD_STATE_SENDING_WRITE; + + return SDCARD_OPERATION_IN_PROGRESS; +} + +/** + * Begin writing a series of consecutive blocks beginning at the given block index. This will allow (but not require) + * the SD card to pre-erase the number of blocks you specifiy, which can allow the writes to complete faster. + * + * Afterwards, just call sdcard_writeBlock() as normal to write those blocks consecutively. + * + * It's okay to abort the multi-block write at any time by writing to a non-consecutive address, or by performing a read. + * + * Returns: + * SDCARD_OPERATION_SUCCESS - Multi-block write has been queued + * SDCARD_OPERATION_BUSY - The card is already busy and cannot accept your write + * SDCARD_OPERATION_FAILURE - A fatal error occured, card will be reset + */ +sdcardOperationStatus_e sdcard_beginWriteBlocks(uint32_t blockIndex, uint32_t blockCount) +{ + if (sdcard.state != SDCARD_STATE_READY) { + if (sdcard.state == SDCARD_STATE_WRITING_MULTIPLE_BLOCKS) { + if (blockIndex == sdcard.multiWriteNextBlock) { + // Assume that the caller wants to continue the multi-block write they already have in progress! + return SDCARD_OPERATION_SUCCESS; + } else if (sdcard_endWriteBlocks() != SDCARD_OPERATION_SUCCESS) { + return SDCARD_OPERATION_BUSY; + } // Else we've completed the previous multi-block write and can fall through to start the new one + } else { + return SDCARD_OPERATION_BUSY; + } + } + + sdcard_select(); + + if ( + sdcard_sendAppCommand(SDCARD_ACOMMAND_SET_WR_BLOCK_ERASE_COUNT, blockCount) == 0 + && sdcard_sendCommand(SDCARD_COMMAND_WRITE_MULTIPLE_BLOCK, sdcard.highCapacity ? blockIndex : blockIndex * SDCARD_BLOCK_SIZE) == 0 + ) { + sdcard.state = SDCARD_STATE_WRITING_MULTIPLE_BLOCKS; + sdcard.multiWriteBlocksRemain = blockCount; + sdcard.multiWriteNextBlock = blockIndex; + + // Leave the card selected + return SDCARD_OPERATION_SUCCESS; + } else { + sdcard_deselect(); + + sdcard_reset(); + + return SDCARD_OPERATION_FAILURE; + } +} + +/** + * Read the 512-byte block with the given index into the given 512-byte buffer. + * + * When the read completes, your callback will be called. If the read was successful, the buffer pointer will be the + * same buffer you originally passed in, otherwise the buffer will be set to NULL. + * + * You must keep the pointer to the buffer valid until the operation completes! + * + * Returns: + * true - The operation was successfully queued for later completion, your callback will be called later + * false - The operation could not be started due to the card being busy (try again later). + */ +bool sdcard_readBlock(uint32_t blockIndex, uint8_t *buffer, sdcard_operationCompleteCallback_c callback, uint32_t callbackData) +{ + if (sdcard.state != SDCARD_STATE_READY) { + if (sdcard.state == SDCARD_STATE_WRITING_MULTIPLE_BLOCKS) { + if (sdcard_endWriteBlocks() != SDCARD_OPERATION_SUCCESS) { + return false; + } + } else { + return false; + } + } + +#ifdef SDCARD_PROFILING + sdcard.pendingOperation.profileStartTime = micros(); +#endif + + sdcard_select(); + + // Standard size cards use byte addressing, high capacity cards use block addressing + uint8_t status = sdcard_sendCommand(SDCARD_COMMAND_READ_SINGLE_BLOCK, sdcard.highCapacity ? blockIndex : blockIndex * SDCARD_BLOCK_SIZE); + + if (status == 0) { + sdcard.pendingOperation.buffer = buffer; + sdcard.pendingOperation.blockIndex = blockIndex; + sdcard.pendingOperation.callback = callback; + sdcard.pendingOperation.callbackData = callbackData; + + sdcard.state = SDCARD_STATE_READING; + + sdcard.operationStartTime = millis(); + + // Leave the card selected for the whole transaction + + return true; + } else { + sdcard_deselect(); + + return false; + } +} + +/** + * Returns true if the SD card has successfully completed its startup procedures. + */ +bool sdcard_isInitialized(void) +{ + return sdcard.state >= SDCARD_STATE_READY; +} + +const sdcardMetadata_t* sdcard_getMetadata(void) +{ + return &sdcard.metadata; +} + +#ifdef SDCARD_PROFILING + +void sdcard_setProfilerCallback(sdcard_profilerCallback_c callback) +{ + sdcard.profiler = callback; +} + +#endif + +#endif diff --git a/src/main/drivers/sdcard.h b/src/main/drivers/sdcard.h new file mode 100644 index 000000000..18b8e9d5a --- /dev/null +++ b/src/main/drivers/sdcard.h @@ -0,0 +1,73 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#include +#include + +typedef struct sdcardMetadata_t { + uint8_t manufacturerID; + uint16_t oemID; + + char productName[5]; + + uint8_t productRevisionMajor; + uint8_t productRevisionMinor; + uint32_t productSerial; + + uint16_t productionYear; + uint8_t productionMonth; + + uint32_t numBlocks; /* Card capacity in 512-byte blocks*/ +} sdcardMetadata_t; + +typedef enum { + SDCARD_BLOCK_OPERATION_READ, + SDCARD_BLOCK_OPERATION_WRITE, + SDCARD_BLOCK_OPERATION_ERASE, +} sdcardBlockOperation_e; + +typedef enum { + SDCARD_OPERATION_IN_PROGRESS, + SDCARD_OPERATION_BUSY, + SDCARD_OPERATION_SUCCESS, + SDCARD_OPERATION_FAILURE +} sdcardOperationStatus_e; + +typedef void(*sdcard_operationCompleteCallback_c)(sdcardBlockOperation_e operation, uint32_t blockIndex, uint8_t *buffer, uint32_t callbackData); + +typedef void(*sdcard_profilerCallback_c)(sdcardBlockOperation_e operation, uint32_t blockIndex, uint32_t duration); + +void sdcard_init(bool useDMA); + +bool sdcard_readBlock(uint32_t blockIndex, uint8_t *buffer, sdcard_operationCompleteCallback_c callback, uint32_t callbackData); + +sdcardOperationStatus_e sdcard_beginWriteBlocks(uint32_t blockIndex, uint32_t blockCount); +sdcardOperationStatus_e sdcard_writeBlock(uint32_t blockIndex, uint8_t *buffer, sdcard_operationCompleteCallback_c callback, uint32_t callbackData); + +void sdcardInsertionDetectDeinit(void); +void sdcardInsertionDetectInit(void); + +bool sdcard_isInserted(); +bool sdcard_isInitialized(); +bool sdcard_isFunctional(); + +bool sdcard_poll(); +const sdcardMetadata_t* sdcard_getMetadata(); + +void sdcard_setProfilerCallback(sdcard_profilerCallback_c callback); diff --git a/src/main/drivers/sdcard_standard.c b/src/main/drivers/sdcard_standard.c new file mode 100644 index 000000000..e0b69712b --- /dev/null +++ b/src/main/drivers/sdcard_standard.c @@ -0,0 +1,52 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include + +#include "sdcard_standard.h" + +#define MIN(a, b) ((a) < (b) ? (a) : (b)) + +/** + * Read a bitfield from an array of bits (the bit at index 0 being the most-significant bit of the first byte in + * the buffer). + */ +uint32_t readBitfield(uint8_t *buffer, unsigned bitIndex, unsigned bitLen) +{ + uint32_t result = 0; + unsigned bitInByteOffset = bitIndex % 8; + uint8_t bufferByte; + + buffer += bitIndex / 8; + + // Align the bitfield to be read to the top of the buffer + bufferByte = *buffer << bitInByteOffset; + + while (bitLen > 0) { + unsigned bitsThisLoop = MIN(8 - bitInByteOffset, bitLen); + + result = (result << bitsThisLoop) | (bufferByte >> (8 - bitsThisLoop)); + + buffer++; + bufferByte = *buffer; + + bitLen -= bitsThisLoop; + bitInByteOffset = 0; + } + + return result; +} diff --git a/src/main/drivers/sdcard_standard.h b/src/main/drivers/sdcard_standard.h new file mode 100644 index 000000000..ec94db301 --- /dev/null +++ b/src/main/drivers/sdcard_standard.h @@ -0,0 +1,240 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#include + +typedef struct sdcardCSD_t { + uint8_t data[16]; +} sdcardCSD_t; + +#define SDCARD_GET_CSD_FIELD(csd, version, fieldname) \ + readBitfield(csd.data, SDCARD_CSD_V ## version ## _ ## fieldname ## _OFFSET, SDCARD_CSD_V ## version ## _ ## fieldname ## _LEN) + +// For v1 and Standard Capacity cards +#define SDCARD_CSD_V1_CSD_STRUCTURE_VER_OFFSET 0 +#define SDCARD_CSD_V1_CSD_STRUCTURE_VER_LEN 2 + +#define SDCARD_CSD_V1_TAAC_OFFSET 8 +#define SDCARD_CSD_V1_TAAC_LEN 8 + +#define SDCARD_CSD_V1_NSAC_OFFSET 16 +#define SDCARD_CSD_V1_NSAC_LEN 8 + +#define SDCARD_CSD_V1_TRAN_SPEED_OFFSET 24 +#define SDCARD_CSD_V1_TRAN_SPEED_LEN 8 + +#define SDCARD_CSD_V1_CCC_OFFSET 32 +#define SDCARD_CSD_V1_CCC_LEN 12 + +#define SDCARD_CSD_V1_READ_BLOCK_LEN_OFFSET 44 +#define SDCARD_CSD_V1_READ_BLOCK_LEN_LEN 4 + +#define SDCARD_CSD_V1_READ_BLOCK_PARTIAL_ALLOWED_OFFSET 48 +#define SDCARD_CSD_V1_READ_BLOCK_PARTIAL_ALLOWED_LEN 1 + +#define SDCARD_CSD_V1_WRITE_BLOCK_MISALIGN_OFFSET 49 +#define SDCARD_CSD_V1_WRITE_BLOCK_MISALIGN_LEN 1 + +#define SDCARD_CSD_V1_READ_BLOCK_MISALIGN_OFFSET 50 +#define SDCARD_CSD_V1_READ_BLOCK_MISALIGN_LEN 1 + +#define SDCARD_CSD_V1_DSR_IMPLEMENTED_OFFSET 51 +#define SDCARD_CSD_V1_DSR_IMPLEMENTED_LEN 1 + +#define SDCARD_CSD_V1_CSIZE_OFFSET 54 +#define SDCARD_CSD_V1_CSIZE_LEN 12 + +#define SDCARD_CSD_V1_VDD_READ_CURR_MIN_OFFSET 66 +#define SDCARD_CSD_V1_VDD_READ_CURR_MIN_LEN 3 + +#define SDCARD_CSD_V1_VDD_READ_CURR_MAX_OFFSET 69 +#define SDCARD_CSD_V1_VDD_READ_CURR_MAX_LEN 3 + +#define SDCARD_CSD_V1_VDD_WRITE_CURR_MIN_OFFSET 72 +#define SDCARD_CSD_V1_VDD_WRITE_CURR_MIN_LEN 3 + +#define SDCARD_CSD_V1_VDD_WRITE_CURR_MAX_OFFSET 75 +#define SDCARD_CSD_V1_VDD_WRITE_CURR_MAX_LEN 3 + +#define SDCARD_CSD_V1_CSIZE_MULT_OFFSET 78 +#define SDCARD_CSD_V1_CSIZE_MULT_LEN 3 + +#define SDCARD_CSD_V1_ERASE_SINGLE_BLOCK_ALLOWED_OFFSET 81 +#define SDCARD_CSD_V1_ERASE_SINGLE_BLOCK_ALLOWED_LEN 1 + +#define SDCARD_CSD_V1_SECTOR_SIZE_OFFSET 82 +#define SDCARD_CSD_V1_SECTOR_SIZE_LEN 7 + +#define SDCARD_CSD_V1_WRITE_PROTECT_GROUP_SIZE_OFFSET 89 +#define SDCARD_CSD_V1_WRITE_PROTECT_GROUP_SIZE_LEN 7 + +#define SDCARD_CSD_V1_WRITE_PROTECT_GROUP_ENABLE_OFFSET 96 +#define SDCARD_CSD_V1_WRITE_PROTECT_GROUP_ENABLE_LEN 1 + +#define SDCARD_CSD_V1_R2W_FACTOR_OFFSET 99 +#define SDCARD_CSD_V1_R2W_FACTOR_LEN 3 + +#define SDCARD_CSD_V1_WRITE_BLOCK_LEN_OFFSET 102 +#define SDCARD_CSD_V1_WRITE_BLOCK_LEN_LEN 4 + +#define SDCARD_CSD_V1_WRITE_BLOCK_PARTIAL_ALLOWED_OFFSET 106 +#define SDCARD_CSD_V1_WRITE_BLOCK_PARTIAL_ALLOWED_LEN 1 + +#define SDCARD_CSD_V1_FILE_FORMAT_GROUP_OFFSET 112 +#define SDCARD_CSD_V1_FILE_FORMAT_GROUP_LEN 1 + +#define SDCARD_CSD_V1_COPY_OFFSET 113 +#define SDCARD_CSD_V1_COPY_LEN 1 + +#define SDCARD_CSD_V1_PERMANENT_WRITE_PROTECT_OFFSET 114 +#define SDCARD_CSD_V1_PERMANENT_WRITE_PROTECT_LEN 1 + +#define SDCARD_CSD_V1_TEMPORARY_WRITE_PROTECT_OFFSET 115 +#define SDCARD_CSD_V1_TEMPORARY_WRITE_PROTECT_LEN 1 + +#define SDCARD_CSD_V1_FILE_FORMAT_OFFSET 116 +#define SDCARD_CSD_V1_FILE_FORMAT_LEN 2 + +#define SDCARD_CSD_V1_CRC_OFFSET 120 +#define SDCARD_CSD_V1_CRC_LEN 7 + +#define SDCARD_CSD_V1_TRAILER_OFFSET 127 +#define SDCARD_CSD_V1_TRAILER_LEN 1 + +// For v2 High Capacity cards +#define SDCARD_CSD_V2_CSD_STRUCTURE_VER_OFFSET 0 +#define SDCARD_CSD_V2_CSD_STRUCTURE_VER_LEN 2 + +#define SDCARD_CSD_V2_TAAC_OFFSET 8 +#define SDCARD_CSD_V2_TAAC_LEN 8 + +#define SDCARD_CSD_V2_NSAC_OFFSET 16 +#define SDCARD_CSD_V2_NSAC_LEN 8 + +#define SDCARD_CSD_V2_TRAN_SPEED_OFFSET 24 +#define SDCARD_CSD_V2_TRAN_SPEED_LEN 8 + +#define SDCARD_CSD_V2_CCC_OFFSET 32 +#define SDCARD_CSD_V2_CCC_LEN 12 + +#define SDCARD_CSD_V2_READ_BLOCK_LEN_OFFSET 44 +#define SDCARD_CSD_V2_READ_BLOCK_LEN_LEN 4 + +#define SDCARD_CSD_V2_READ_BLOCK_PARTIAL_ALLOWED_OFFSET 48 +#define SDCARD_CSD_V2_READ_BLOCK_PARTIAL_ALLOWED_LEN 1 + +#define SDCARD_CSD_V2_WRITE_BLOCK_MISALIGN_OFFSET 49 +#define SDCARD_CSD_V2_WRITE_BLOCK_MISALIGN_LEN 1 + +#define SDCARD_CSD_V2_READ_BLOCK_MISALIGN_OFFSET 50 +#define SDCARD_CSD_V2_READ_BLOCK_MISALIGN_LEN 1 + +#define SDCARD_CSD_V2_DSR_IMPLEMENTED_OFFSET 51 +#define SDCARD_CSD_V2_DSR_IMPLEMENTED_LEN 1 + +#define SDCARD_CSD_V2_CSIZE_OFFSET 58 +#define SDCARD_CSD_V2_CSIZE_LEN 22 + +#define SDCARD_CSD_V2_ERASE_SINGLE_BLOCK_ALLOWED_OFFSET 81 +#define SDCARD_CSD_V2_ERASE_SINGLE_BLOCK_ALLOWED_LEN 1 + +#define SDCARD_CSD_V2_SECTOR_SIZE_OFFSET 82 +#define SDCARD_CSD_V2_SECTOR_SIZE_LEN 7 + +#define SDCARD_CSD_V2_WRITE_PROTECT_GROUP_SIZE_OFFSET 89 +#define SDCARD_CSD_V2_WRITE_PROTECT_GROUP_SIZE_LEN 7 + +#define SDCARD_CSD_V2_WRITE_PROTECT_GROUP_ENABLE_OFFSET 96 +#define SDCARD_CSD_V2_WRITE_PROTECT_GROUP_ENABLE_LEN 1 + +#define SDCARD_CSD_V2_R2W_FACTOR_OFFSET 99 +#define SDCARD_CSD_V2_R2W_FACTOR_LEN 3 + +#define SDCARD_CSD_V2_WRITE_BLOCK_LEN_OFFSET 102 +#define SDCARD_CSD_V2_WRITE_BLOCK_LEN_LEN 4 + +#define SDCARD_CSD_V2_WRITE_BLOCK_PARTIAL_ALLOWED_OFFSET 106 +#define SDCARD_CSD_V2_WRITE_BLOCK_PARTIAL_ALLOWED_LEN 1 + +#define SDCARD_CSD_V2_FILE_FORMAT_GROUP_OFFSET 112 +#define SDCARD_CSD_V2_FILE_FORMAT_GROUP_LEN 1 + +#define SDCARD_CSD_V2_COPY_OFFSET 113 +#define SDCARD_CSD_V2_COPY_LEN 1 + +#define SDCARD_CSD_V2_PERMANENT_WRITE_PROTECT_OFFSET 114 +#define SDCARD_CSD_V2_PERMANENT_WRITE_PROTECT_LEN 1 + +#define SDCARD_CSD_V2_TEMPORARY_WRITE_PROTECT_OFFSET 115 +#define SDCARD_CSD_V2_TEMPORARY_WRITE_PROTECT_LEN 1 + +#define SDCARD_CSD_V2_FILE_FORMAT_OFFSET 116 +#define SDCARD_CSD_V2_FILE_FORMAT_LEN 2 + +#define SDCARD_CSD_V2_CRC_OFFSET 120 +#define SDCARD_CSD_V2_CRC_LEN 7 + +#define SDCARD_CSD_V2_TRAILER_OFFSET 127 +#define SDCARD_CSD_V2_TRAILER_LEN 1 + +#define SDCARD_SINGLE_BLOCK_READ_START_TOKEN 0xFE +#define SDCARD_SINGLE_BLOCK_WRITE_START_TOKEN 0xFE +#define SDCARD_MULTIPLE_BLOCK_WRITE_START_TOKEN 0xFC +#define SDCARD_MULTIPLE_BLOCK_WRITE_STOP_TOKEN 0xFD + +#define SDCARD_BLOCK_SIZE 512 + +// Idle bit is set to 1 only when idle during intialization phase: +#define SDCARD_R1_STATUS_BIT_IDLE 1 +#define SDCARD_R1_STATUS_BIT_ERASE_RESET 2 +#define SDCARD_R1_STATUS_BIT_ILLEGAL_COMMAND 4 +#define SDCARD_R1_STATUS_BIT_COM_CRC_ERROR 8 +#define SDCARD_R1_STATUS_BIT_ERASE_SEQUENCE_ERROR 16 +#define SDCARD_R1_STATUS_BIT_ADDRESS_ERROR 32 +#define SDCARD_R1_STATUS_BIT_PARAMETER_ERROR 64 + +#define SDCARD_CSD_STRUCTURE_VERSION_1 0 +#define SDCARD_CSD_STRUCTURE_VERSION_2 1 + +#define SDCARD_VOLTAGE_ACCEPTED_2_7_to_3_6 0x01 +#define SDCARD_VOLTAGE_ACCEPTED_LVR 0x02 + +#define SDCARD_COMMAND_GO_IDLE_STATE 0 +#define SDCARD_COMMAND_SEND_OP_COND 1 +#define SDCARD_COMMAND_SEND_IF_COND 8 +#define SDCARD_COMMAND_SEND_CSD 9 +#define SDCARD_COMMAND_SEND_CID 10 +#define SDCARD_COMMAND_STOP_TRANSMISSION 12 +#define SDCARD_COMMAND_SEND_STATUS 13 +#define SDCARD_COMMAND_SET_BLOCKLEN 16 +#define SDCARD_COMMAND_READ_SINGLE_BLOCK 17 +#define SDCARD_COMMAND_READ_MULTIPLE_BLOCK 18 +#define SDCARD_COMMAND_WRITE_BLOCK 24 +#define SDCARD_COMMAND_WRITE_MULTIPLE_BLOCK 25 +#define SDCARD_COMMAND_APP_CMD 55 +#define SDCARD_COMMAND_READ_OCR 58 + +#define SDCARD_ACOMMAND_SEND_OP_COND 41 +#define SDCARD_ACOMMAND_SET_WR_BLOCK_ERASE_COUNT 23 + +// These are worst-case timeouts defined for High Speed cards +#define SDCARD_TIMEOUT_READ_MSEC 100 +#define SDCARD_TIMEOUT_WRITE_MSEC 250 + +uint32_t readBitfield(uint8_t *buffer, unsigned bitIndex, unsigned bitLen); diff --git a/src/main/drivers/serial_uart.c b/src/main/drivers/serial_uart.c index fad6bd16f..20e7ca038 100644 --- a/src/main/drivers/serial_uart.c +++ b/src/main/drivers/serial_uart.c @@ -108,6 +108,14 @@ serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, } else if (USARTx == USART3) { s = serialUSART3(baudRate, mode, options); #endif +#ifdef USE_USART4 + } else if (USARTx == UART4) { + s = serialUSART4(baudRate, mode, options); +#endif +#ifdef USE_USART5 + } else if (USARTx == UART5) { + s = serialUSART5(baudRate, mode, options); +#endif #ifdef USE_USART6 } else if (USARTx == USART6) { s = serialUSART6(baudRate, mode, options); diff --git a/src/main/drivers/serial_uart.h b/src/main/drivers/serial_uart.h index 22411de72..c5bc41052 100644 --- a/src/main/drivers/serial_uart.h +++ b/src/main/drivers/serial_uart.h @@ -39,7 +39,7 @@ typedef struct { serialPort_t port; -#if defined(STM32F40_41xxx) || defined (STM32F411xE) +#if defined(STM32F4) DMA_Stream_TypeDef *rxDMAStream; DMA_Stream_TypeDef *txDMAStream; uint32_t rxDMAChannel; diff --git a/src/main/drivers/serial_uart_impl.h b/src/main/drivers/serial_uart_impl.h index 3e123cd23..5b0c2b3e0 100644 --- a/src/main/drivers/serial_uart_impl.h +++ b/src/main/drivers/serial_uart_impl.h @@ -26,5 +26,7 @@ void uartStartTxDMA(uartPort_t *s); uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode, portOptions_t options); uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode, portOptions_t options); uartPort_t *serialUSART3(uint32_t baudRate, portMode_t mode, portOptions_t options); +uartPort_t *serialUSART4(uint32_t baudRate, portMode_t mode, portOptions_t options); +uartPort_t *serialUSART5(uint32_t baudRate, portMode_t mode, portOptions_t options); uartPort_t *serialUSART6(uint32_t baudRate, portMode_t mode, portOptions_t options); diff --git a/src/main/drivers/serial_usb_vcp.c b/src/main/drivers/serial_usb_vcp.c index d8032dab4..7163443f4 100644 --- a/src/main/drivers/serial_usb_vcp.c +++ b/src/main/drivers/serial_usb_vcp.c @@ -26,7 +26,7 @@ #include "common/utils.h" #include "usb_core.h" -#if defined(STM32F411xE) || defined(STM32F40_41xxx) +#ifdef STM32F4 #include "usbd_cdc_vcp.h" #else #include "usb_init.h" @@ -156,7 +156,7 @@ serialPort_t *usbVcpOpen(void) { vcpPort_t *s; -#if defined(STM32F411xE) || defined(STM32F40_41xxx) +#ifdef STM32F4 USBD_Init(&USB_OTG_dev, USB_OTG_FS_CORE_ID, &USR_desc, diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 11682c81f..9eb64486c 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -167,7 +167,7 @@ void failsafeUpdateState(void) return; } - bool receivingRxData = failsafeIsReceivingRxData(); // FIXME - Original check for received data + bool receivingRxData = failsafeIsReceivingRxData(); bool armed = ARMING_FLAG(ARMED); bool failsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE); beeperMode_e beeperMode = BEEPER_SILENCE; @@ -222,8 +222,20 @@ void failsafeUpdateState(void) if (receivingRxData) { failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; } else { - // Stabilize, and set Throttle to specified level - failsafeActivate(); + switch (failsafeConfig->failsafe_procedure) { + default: + case FAILSAFE_PROCEDURE_AUTO_LANDING: + // Stabilize, and set Throttle to specified level + failsafeActivate(); + break; + + case FAILSAFE_PROCEDURE_DROP_IT: + // Drop the craft + failsafeActivate(); + failsafeState.phase = FAILSAFE_LANDED; // skip auto-landing procedure + failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS; // require 3 seconds of valid rxData + break; + } } reprocessState = true; break; diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index 7d841dd11..9214e5824 100644 --- a/src/main/flight/failsafe.h +++ b/src/main/flight/failsafe.h @@ -33,6 +33,7 @@ typedef struct failsafeConfig_s { uint16_t failsafe_throttle; // Throttle level used for landing - specify value between 1000..2000 (pwm pulse width for slightly below hover). center throttle = 1500. uint8_t failsafe_kill_switch; // failsafe switch action is 0: identical to rc link loss, 1: disarms instantly uint16_t failsafe_throttle_low_delay; // Time throttle stick must have been below 'min_check' to "JustDisarm" instead of "full failsafe procedure". + uint8_t failsafe_procedure; // selected full failsafe procedure is 0: auto-landing, 1: Drop it } failsafeConfig_t; typedef enum { @@ -49,6 +50,11 @@ typedef enum { FAILSAFE_RXLINK_UP } failsafeRxLinkState_e; +typedef enum { + FAILSAFE_PROCEDURE_AUTO_LANDING = 0, + FAILSAFE_PROCEDURE_DROP_IT +} failsafeProcedure_e; + typedef struct failsafeState_s { int16_t events; bool monitoring; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 42e21a023..0ea73694b 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -641,10 +641,8 @@ void writeMotors(void) for (i = 0; i < motorCount; i++) pwmWriteMotor(i, motor[i]); - if (feature(FEATURE_MULTISHOT) || (feature(FEATURE_ONESHOT125))) { - if (!feature(FEATURE_USE_PWM_RATE)) { - pwmCompleteOneshotMotorUpdate(motorCount); - } + if (feature(FEATURE_ONESHOT)) { + pwmCompleteOneshotMotorUpdate(motorCount); } } diff --git a/src/main/io/asyncfatfs/asyncfatfs.c b/src/main/io/asyncfatfs/asyncfatfs.c new file mode 100644 index 000000000..0fc5c23b9 --- /dev/null +++ b/src/main/io/asyncfatfs/asyncfatfs.c @@ -0,0 +1,3660 @@ +/** + * This is a FAT16/FAT32 filesystem for SD cards which uses asynchronous operations: The caller need never wait + * for the SD card to be ready. + * + * On top of the regular FAT32 concepts, we add the idea of a "super cluster". Given one FAT sector, a super cluster is + * the series of clusters which corresponds to all of the cluster entries in that FAT sector. If files are allocated + * on super-cluster boundaries, they will have FAT sectors which are dedicated to them and independent of all other + * files. + * + * We can pre-allocate a "freefile" which is a file on disk made up of contiguous superclusters. Then when we want + * to allocate a file on disk, we can carve it out of the freefile, and know that the clusters will be contiguous + * without needing to read the FAT at all (the freefile's FAT is completely determined from its start cluster and file + * size, which we get from the directory entry). This allows for extremely fast append-only logging. + */ + +#include +#include +#include + +#include "platform.h" + +#ifdef AFATFS_DEBUG +#include +#include +#endif + +#include "asyncfatfs.h" + +#include "fat_standard.h" +#include "drivers/sdcard.h" + +#ifdef AFATFS_DEBUG + #define ONLY_EXPOSE_FOR_TESTING +#else + #define ONLY_EXPOSE_FOR_TESTING static +#endif + +#define AFATFS_NUM_CACHE_SECTORS 8 + +// FAT filesystems are allowed to differ from these parameters, but we choose not to support those weird filesystems: +#define AFATFS_SECTOR_SIZE 512 +#define AFATFS_NUM_FATS 2 + +#define AFATFS_MAX_OPEN_FILES 3 + +#define AFATFS_DEFAULT_FILE_DATE FAT_MAKE_DATE(2015, 12, 01) +#define AFATFS_DEFAULT_FILE_TIME FAT_MAKE_TIME(00, 00, 00) + +/* + * How many blocks will we write in a row before we bother using the SDcard's multiple block write method? + * If this define is omitted, this disables multi-block write. + */ +#define AFATFS_MIN_MULTIPLE_BLOCK_WRITE_COUNT 4 + +#define AFATFS_FILES_PER_DIRECTORY_SECTOR (AFATFS_SECTOR_SIZE / sizeof(fatDirectoryEntry_t)) + +#define AFATFS_FAT32_FAT_ENTRIES_PER_SECTOR (AFATFS_SECTOR_SIZE / sizeof(uint32_t)) +#define AFATFS_FAT16_FAT_ENTRIES_PER_SECTOR (AFATFS_SECTOR_SIZE / sizeof(uint16_t)) + +// We will read from the file +#define AFATFS_FILE_MODE_READ 1 +// We will write to the file +#define AFATFS_FILE_MODE_WRITE 2 +// We will append to the file, may not be combined with the write flag +#define AFATFS_FILE_MODE_APPEND 4 +// File will occupy a series of superclusters (only valid for creating new files): +#define AFATFS_FILE_MODE_CONTIGUOUS 8 +// File should be created if it doesn't exist: +#define AFATFS_FILE_MODE_CREATE 16 +// The file's directory entry should be locked in cache so we can read it with no latency: +#define AFATFS_FILE_MODE_RETAIN_DIRECTORY 32 + +// Open the cache sector for read access (it will be read from disk) +#define AFATFS_CACHE_READ 1 +// Open the cache sector for write access (it will be marked dirty) +#define AFATFS_CACHE_WRITE 2 +// Lock this sector to prevent its state from transitioning (prevent flushes to disk) +#define AFATFS_CACHE_LOCK 4 +// Discard this sector in preference to other sectors when it is in the in-sync state +#define AFATFS_CACHE_DISCARDABLE 8 +// Increase the retain counter of the cache sector to prevent it from being discarded when in the in-sync state +#define AFATFS_CACHE_RETAIN 16 + +// Turn the largest free block on the disk into one contiguous file for efficient fragment-free allocation +#define AFATFS_USE_FREEFILE + +// When allocating a freefile, leave this many clusters un-allocated for regular files to use +#define AFATFS_FREEFILE_LEAVE_CLUSTERS 100 + +// Filename in 8.3 format: +#define AFATFS_FREESPACE_FILENAME "FREESPAC.E" + +#define AFATFS_INTROSPEC_LOG_FILENAME "ASYNCFAT.LOG" + +#define MIN(a, b) ((a) < (b) ? (a) : (b)) +#define MAX(a, b) ((a) > (b) ? (a) : (b)) + +typedef enum { + AFATFS_SAVE_DIRECTORY_NORMAL, + AFATFS_SAVE_DIRECTORY_FOR_CLOSE, + AFATFS_SAVE_DIRECTORY_DELETED, +} afatfsSaveDirectoryEntryMode_e; + +typedef enum { + AFATFS_CACHE_STATE_EMPTY, + AFATFS_CACHE_STATE_IN_SYNC, + AFATFS_CACHE_STATE_READING, + AFATFS_CACHE_STATE_WRITING, + AFATFS_CACHE_STATE_DIRTY +} afatfsCacheBlockState_e; + +typedef enum { + AFATFS_FILE_TYPE_NONE, + AFATFS_FILE_TYPE_NORMAL, + AFATFS_FILE_TYPE_FAT16_ROOT_DIRECTORY, + AFATFS_FILE_TYPE_DIRECTORY +} afatfsFileType_e; + +typedef enum { + CLUSTER_SEARCH_FREE_AT_BEGINNING_OF_FAT_SECTOR, + CLUSTER_SEARCH_FREE, + CLUSTER_SEARCH_OCCUPIED, +} afatfsClusterSearchCondition_e; + +enum { + AFATFS_CREATEFILE_PHASE_INITIAL = 0, + AFATFS_CREATEFILE_PHASE_FIND_FILE, + AFATFS_CREATEFILE_PHASE_CREATE_NEW_FILE, + AFATFS_CREATEFILE_PHASE_SUCCESS, + AFATFS_CREATEFILE_PHASE_FAILURE, +}; + +typedef enum { + AFATFS_FIND_CLUSTER_IN_PROGRESS, + AFATFS_FIND_CLUSTER_FOUND, + AFATFS_FIND_CLUSTER_FATAL, + AFATFS_FIND_CLUSTER_NOT_FOUND, +} afatfsFindClusterStatus_e; + +struct afatfsFileOperation_t; + +typedef union afatfsFATSector_t { + uint8_t *bytes; + uint16_t *fat16; + uint32_t *fat32; +} afatfsFATSector_t; + +typedef struct afatfsCacheBlockDescriptor_t { + /* + * The physical sector index on disk that this cached block corresponds to + */ + uint32_t sectorIndex; + + // We use an increasing timestamp to identify cache access times. + + // This is the timestamp that this sector was first marked dirty at (so we can flush sectors in write-order). + uint32_t writeTimestamp; + + // This is the last time the sector was accessed + uint32_t accessTimestamp; + + /* This is set to non-zero when we expect to write a consecutive series of this many blocks (including this block), + * so we will tell the SD-card to pre-erase those blocks. + * + * This counter only needs to be set on the first block of a consecutive write (though setting it, appropriately + * decreased, on the subsequent blocks won't hurt). + */ + uint16_t consecutiveEraseBlockCount; + + afatfsCacheBlockState_e state; + + /* + * The state of this block must not transition (do not flush to disk, do not discard). This is useful for a sector + * which is currently being written to by the application (so flushing it would be a waste of time). + * + * This is a binary state rather than a counter because we assume that only one party will be responsible for and + * so consider locking a given sector. + */ + unsigned locked:1; + + /* + * A counter for how many parties want this sector to be retained in memory (not discarded). If this value is + * non-zero, the sector may be flushed to disk if dirty but must remain in the cache. This is useful if we require + * a directory sector to be cached in order to meet our response time requirements. + */ + unsigned retainCount:6; + + /* + * If this block is in the In Sync state, it should be discarded from the cache in preference to other blocks. + * This is useful for data that we don't expect to read again, e.g. data written to an append-only file. This hint + * is overridden by the locked and retainCount flags. + */ + unsigned discardable:1; +} afatfsCacheBlockDescriptor_t; + +typedef enum { + AFATFS_FAT_PATTERN_UNTERMINATED_CHAIN, + AFATFS_FAT_PATTERN_TERMINATED_CHAIN, + AFATFS_FAT_PATTERN_FREE +} afatfsFATPattern_e; + +typedef enum { + AFATFS_FREE_SPACE_SEARCH_PHASE_FIND_HOLE, + AFATFS_FREE_SPACE_SEARCH_PHASE_GROW_HOLE +} afatfsFreeSpaceSearchPhase_e; + +typedef struct afatfsFreeSpaceSearch_t { + uint32_t candidateStart; + uint32_t candidateEnd; + uint32_t bestGapStart; + uint32_t bestGapLength; + afatfsFreeSpaceSearchPhase_e phase; +} afatfsFreeSpaceSearch_t; + +typedef struct afatfsFreeSpaceFAT_t { + uint32_t startCluster; + uint32_t endCluster; +} afatfsFreeSpaceFAT_t; + +typedef struct afatfsCreateFile_t { + afatfsFileCallback_t callback; + + uint8_t phase; + uint8_t filename[FAT_FILENAME_LENGTH]; +} afatfsCreateFile_t; + +typedef struct afatfsSeek_t { + afatfsFileCallback_t callback; + + uint32_t seekOffset; +} afatfsSeek_t; + +typedef enum { + AFATFS_APPEND_SUPERCLUSTER_PHASE_INIT = 0, + AFATFS_APPEND_SUPERCLUSTER_PHASE_UPDATE_FREEFILE_DIRECTORY, + AFATFS_APPEND_SUPERCLUSTER_PHASE_UPDATE_FAT, + AFATFS_APPEND_SUPERCLUSTER_PHASE_UPDATE_FILE_DIRECTORY, +} afatfsAppendSuperclusterPhase_e; + +typedef struct afatfsAppendSupercluster_t { + uint32_t previousCluster; + uint32_t fatRewriteStartCluster; + uint32_t fatRewriteEndCluster; + afatfsAppendSuperclusterPhase_e phase; +} afatfsAppendSupercluster_t; + +typedef enum { + AFATFS_APPEND_FREE_CLUSTER_PHASE_INITIAL = 0, + AFATFS_APPEND_FREE_CLUSTER_PHASE_FIND_FREESPACE = 0, + AFATFS_APPEND_FREE_CLUSTER_PHASE_UPDATE_FAT1, + AFATFS_APPEND_FREE_CLUSTER_PHASE_UPDATE_FAT2, + AFATFS_APPEND_FREE_CLUSTER_PHASE_UPDATE_FILE_DIRECTORY, + AFATFS_APPEND_FREE_CLUSTER_PHASE_COMPLETE, + AFATFS_APPEND_FREE_CLUSTER_PHASE_FAILURE, +} afatfsAppendFreeClusterPhase_e; + +typedef struct afatfsAppendFreeCluster_t { + uint32_t previousCluster; + uint32_t searchCluster; + afatfsAppendFreeClusterPhase_e phase; +} afatfsAppendFreeCluster_t; + +typedef enum { + AFATFS_EXTEND_SUBDIRECTORY_PHASE_INITIAL = 0, + AFATFS_EXTEND_SUBDIRECTORY_PHASE_ADD_FREE_CLUSTER = 0, + AFATFS_EXTEND_SUBDIRECTORY_PHASE_WRITE_SECTORS, + AFATFS_EXTEND_SUBDIRECTORY_PHASE_SUCCESS, + AFATFS_EXTEND_SUBDIRECTORY_PHASE_FAILURE +} afatfsExtendSubdirectoryPhase_e; + +typedef struct afatfsExtendSubdirectory_t { + // We need to call this as a sub-operation so we have it as our first member to be compatible with its memory layout: + afatfsAppendFreeCluster_t appendFreeCluster; + + afatfsExtendSubdirectoryPhase_e phase; + + uint32_t parentDirectoryCluster; + afatfsFileCallback_t callback; +} afatfsExtendSubdirectory_t; + +typedef enum { + AFATFS_TRUNCATE_FILE_INITIAL = 0, + AFATFS_TRUNCATE_FILE_UPDATE_DIRECTORY = 0, + AFATFS_TRUNCATE_FILE_ERASE_FAT_CHAIN_NORMAL, +#ifdef AFATFS_USE_FREEFILE + AFATFS_TRUNCATE_FILE_ERASE_FAT_CHAIN_CONTIGUOUS, + AFATFS_TRUNCATE_FILE_PREPEND_TO_FREEFILE, +#endif + AFATFS_TRUNCATE_FILE_SUCCESS, +} afatfsTruncateFilePhase_e; + +typedef struct afatfsTruncateFile_t { + uint32_t startCluster; // First cluster to erase + uint32_t currentCluster; // Used to mark progress + uint32_t endCluster; // Optional, for contiguous files set to 1 past the end cluster of the file, otherwise set to 0 + afatfsFileCallback_t callback; + afatfsTruncateFilePhase_e phase; +} afatfsTruncateFile_t; + +typedef enum { + AFATFS_DELETE_FILE_DELETE_DIRECTORY_ENTRY, + AFATFS_DELETE_FILE_DEALLOCATE_CLUSTERS, +} afatfsDeleteFilePhase_e; + +typedef struct afatfsDeleteFile_t { + afatfsTruncateFile_t truncateFile; + afatfsCallback_t callback; +} afatfsUnlinkFile_t; + +typedef struct afatfsCloseFile_t { + afatfsCallback_t callback; +} afatfsCloseFile_t; + +typedef enum { + AFATFS_FILE_OPERATION_NONE, + AFATFS_FILE_OPERATION_CREATE_FILE, + AFATFS_FILE_OPERATION_SEEK, // Seek the file's cursorCluster forwards by seekOffset bytes + AFATFS_FILE_OPERATION_CLOSE, + AFATFS_FILE_OPERATION_TRUNCATE, + AFATFS_FILE_OPERATION_UNLINK, +#ifdef AFATFS_USE_FREEFILE + AFATFS_FILE_OPERATION_APPEND_SUPERCLUSTER, + AFATFS_FILE_OPERATION_LOCKED, +#endif + AFATFS_FILE_OPERATION_APPEND_FREE_CLUSTER, + AFATFS_FILE_OPERATION_EXTEND_SUBDIRECTORY, +} afatfsFileOperation_e; + +typedef struct afatfsFileOperation_t { + afatfsFileOperation_e operation; + union { + afatfsCreateFile_t createFile; + afatfsSeek_t seek; + afatfsAppendSupercluster_t appendSupercluster; + afatfsAppendFreeCluster_t appendFreeCluster; + afatfsExtendSubdirectory_t extendSubdirectory; + afatfsUnlinkFile_t unlinkFile; + afatfsTruncateFile_t truncateFile; + afatfsCloseFile_t closeFile; + } state; +} afatfsFileOperation_t; + +typedef struct afatfsFile_t { + afatfsFileType_e type; + + // The byte offset of the cursor within the file + uint32_t cursorOffset; + + /* The file size in bytes as seen by users of the filesystem (the exact length of the file they've written). + * + * This is only used by users of the filesystem, not us, so it only needs to be up to date for fseek() (to clip + * seeks to the EOF), fread(), feof(), and fclose() (which writes the logicalSize to the directory). + * + * It becomes out of date when we fwrite() to extend the length of the file. In this situation, feof() is properly + * true, so we don't have to update the logicalSize for fread() or feof() to get the correct result. We only need + * to update it when we seek backwards (so we don't forget the logical EOF position), or fclose(). + */ + uint32_t logicalSize; + + /* The allocated size in bytes based on how many clusters have been assigned to the file. Always a multiple of + * the cluster size. + * + * This is an underestimate for existing files, because we don't bother to check precisely how long the chain is + * at the time the file is opened (it might be longer than needed to contain the logical size), but assuming the + * filesystem metadata is correct, it should always be at least as many clusters as needed to contain logicalSize. + * + * Since this is an estimate, we only use it to exaggerate the filesize in the directory entry of a file that is + * currently being written (so that the final cluster of the file will be entirely readable if power is lost before + * we can could update the directory entry with a new logicalSize). + */ + uint32_t physicalSize; + + /* + * The cluster that the file pointer is currently within. When seeking to the end of the file, this will be + * set to zero. + */ + uint32_t cursorCluster; + + /* + * The cluster before the one the file pointer is inside. This is set to zero when at the start of the file. + */ + uint32_t cursorPreviousCluster; + + uint8_t mode; // A combination of AFATFS_FILE_MODE_* flags + uint8_t attrib; // Combination of FAT_FILE_ATTRIBUTE_* flags for the directory entry of this file + + /* We hold on to one sector entry in the cache and remember its index here. The cache is invalidated when we + * seek across a sector boundary. This allows fwrite() to complete faster because it doesn't need to check the + * cache on every call. + */ + int8_t writeLockedCacheIndex; + // Ditto for fread(): + int8_t readRetainCacheIndex; + + // The position of our directory entry on the disk (so we can update it without consulting a parent directory file) + afatfsDirEntryPointer_t directoryEntryPos; + + // The first cluster number of the file, or 0 if this file is empty + uint32_t firstCluster; + + // State for a queued operation on the file + struct afatfsFileOperation_t operation; +} afatfsFile_t; + +typedef enum { + AFATFS_INITIALIZATION_READ_MBR, + AFATFS_INITIALIZATION_READ_VOLUME_ID, + +#ifdef AFATFS_USE_FREEFILE + AFATFS_INITIALIZATION_FREEFILE_CREATE, + AFATFS_INITIALIZATION_FREEFILE_CREATING, + AFATFS_INITIALIZATION_FREEFILE_FAT_SEARCH, + AFATFS_INITIALIZATION_FREEFILE_UPDATE_FAT, + AFATFS_INITIALIZATION_FREEFILE_SAVE_DIR_ENTRY, + AFATFS_INITIALIZATION_FREEFILE_LAST = AFATFS_INITIALIZATION_FREEFILE_SAVE_DIR_ENTRY, +#endif + +#ifdef AFATFS_USE_INTROSPECTIVE_LOGGING + AFATFS_INITIALIZATION_INTROSPEC_LOG_CREATE, + AFATFS_INITIALIZATION_INTROSPEC_LOG_CREATING, +#endif + + AFATFS_INITIALIZATION_DONE +} afatfsInitializationPhase_e; + +typedef struct afatfs_t { + fatFilesystemType_e filesystemType; + + afatfsFilesystemState_e filesystemState; + afatfsInitializationPhase_e initPhase; + + // State used during FS initialisation where only one member of the union is used at a time +#ifdef AFATFS_USE_FREEFILE + union { + afatfsFreeSpaceSearch_t freeSpaceSearch; + afatfsFreeSpaceFAT_t freeSpaceFAT; + } initState; +#endif + + uint8_t cache[AFATFS_SECTOR_SIZE * AFATFS_NUM_CACHE_SECTORS]; + afatfsCacheBlockDescriptor_t cacheDescriptor[AFATFS_NUM_CACHE_SECTORS]; + uint32_t cacheTimer; + + int cacheDirtyEntries; // The number of cache entries in the AFATFS_CACHE_STATE_DIRTY state + bool cacheFlushInProgress; + + afatfsFile_t openFiles[AFATFS_MAX_OPEN_FILES]; + +#ifdef AFATFS_USE_FREEFILE + afatfsFile_t freeFile; +#endif + +#ifdef AFATFS_USE_INTROSPECTIVE_LOGGING + afatfsFile_t introSpecLog; +#endif + + afatfsError_e lastError; + + bool filesystemFull; + + // The current working directory: + afatfsFile_t currentDirectory; + + uint32_t partitionStartSector; // The physical sector that the first partition on the device begins at + + uint32_t fatStartSector; // The first sector of the first FAT + uint32_t fatSectors; // The size in sectors of a single FAT + + /* + * Number of clusters available for storing user data. Note that clusters are numbered starting from 2, so the + * index of the last cluster on the volume is numClusters + 1 !!! + */ + uint32_t numClusters; + uint32_t clusterStartSector; // The physical sector that the clusters area begins at + uint32_t sectorsPerCluster; + + /* + * Number of the cluster we last allocated (i.e. free->occupied). Searches for a free cluster will begin after this + * cluster. + */ + uint32_t lastClusterAllocated; + + /* Mask to be ANDed with a byte offset within a file to give the offset within the cluster */ + uint32_t byteInClusterMask; + + uint32_t rootDirectoryCluster; // Present on FAT32 and set to zero for FAT16 + uint32_t rootDirectorySectors; // Zero on FAT32, for FAT16 the number of sectors that the root directory occupies +} afatfs_t; + +static afatfs_t afatfs; + +static void afatfs_fileOperationContinue(afatfsFile_t *file); +static uint8_t* afatfs_fileLockCursorSectorForWrite(afatfsFilePtr_t file); +static uint8_t* afatfs_fileRetainCursorSectorForRead(afatfsFilePtr_t file); + +static uint32_t roundUpTo(uint32_t value, uint32_t rounding) +{ + uint32_t remainder = value % rounding; + + if (remainder > 0) { + value += rounding - remainder; + } + + return value; +} + +static bool isPowerOfTwo(unsigned int x) +{ + return ((x != 0) && ((x & (~x + 1)) == x)); +} + +/** + * Check for conditions that should always be true (and if otherwise mean a bug or a corrupt filesystem). + * + * If the condition is false, the filesystem is marked as being in a fatal state. + * + * Returns the value of the condition. + */ +static bool afatfs_assert(bool condition) +{ + if (!condition) { + if (afatfs.lastError == AFATFS_ERROR_NONE) { + afatfs.lastError = AFATFS_ERROR_GENERIC; + } + afatfs.filesystemState = AFATFS_FILESYSTEM_STATE_FATAL; +#ifdef AFATFS_DEBUG + raise(SIGTRAP); +#endif + } + + return condition; +} + +static bool afatfs_fileIsBusy(afatfsFilePtr_t file) +{ + return file->operation.operation != AFATFS_FILE_OPERATION_NONE; +} + +/** + * The number of FAT table entries that fit within one AFATFS sector size. + * + * Note that this is the same as the number of clusters in an AFATFS supercluster. + */ +static uint32_t afatfs_fatEntriesPerSector() +{ + return afatfs.filesystemType == FAT_FILESYSTEM_TYPE_FAT32 ? AFATFS_FAT32_FAT_ENTRIES_PER_SECTOR : AFATFS_FAT16_FAT_ENTRIES_PER_SECTOR; +} + +/** + * Size of a FAT cluster in bytes + */ +ONLY_EXPOSE_FOR_TESTING +uint32_t afatfs_clusterSize() +{ + return afatfs.sectorsPerCluster * AFATFS_SECTOR_SIZE; +} + +/** + * Given a byte offset within a file, return the byte offset of that position within the cluster it belongs to. + */ +static uint32_t afatfs_byteIndexInCluster(uint32_t byteOffset) +{ + return afatfs.byteInClusterMask & byteOffset; +} + +/** + * Given a byte offset within a file, return the index of the sector within the cluster it belongs to. + */ +static uint32_t afatfs_sectorIndexInCluster(uint32_t byteOffset) +{ + return afatfs_byteIndexInCluster(byteOffset) / AFATFS_SECTOR_SIZE; +} + +// Get the buffer memory for the cache entry of the given index. +static uint8_t *afatfs_cacheSectorGetMemory(int cacheEntryIndex) +{ + return afatfs.cache + cacheEntryIndex * AFATFS_SECTOR_SIZE; +} + +static int afatfs_getCacheDescriptorIndexForBuffer(uint8_t *memory) +{ + int index = (memory - afatfs.cache) / AFATFS_SECTOR_SIZE; + + if (afatfs_assert(index >= 0 && index < AFATFS_NUM_CACHE_SECTORS)) { + return index; + } else { + return -1; + } +} + +static afatfsCacheBlockDescriptor_t* afatfs_getCacheDescriptorForBuffer(uint8_t *memory) +{ + return afatfs.cacheDescriptor + afatfs_getCacheDescriptorIndexForBuffer(memory); +} + +static void afatfs_cacheSectorMarkDirty(afatfsCacheBlockDescriptor_t *descriptor) +{ + if (descriptor->state != AFATFS_CACHE_STATE_DIRTY) { + descriptor->writeTimestamp = ++afatfs.cacheTimer; + descriptor->state = AFATFS_CACHE_STATE_DIRTY; + afatfs.cacheDirtyEntries++; + } +} + +static void afatfs_cacheSectorInit(afatfsCacheBlockDescriptor_t *descriptor, uint32_t sectorIndex, bool locked) +{ + descriptor->sectorIndex = sectorIndex; + + descriptor->accessTimestamp = descriptor->writeTimestamp = ++afatfs.cacheTimer; + + descriptor->consecutiveEraseBlockCount = 0; + + descriptor->state = AFATFS_CACHE_STATE_EMPTY; + + descriptor->locked = locked; + descriptor->retainCount = 0; + descriptor->discardable = 0; +} + +/** + * Called by the SD card driver when one of our read operations completes. + */ +static void afatfs_sdcardReadComplete(sdcardBlockOperation_e operation, uint32_t sectorIndex, uint8_t *buffer, uint32_t callbackData) +{ + (void) operation; + (void) callbackData; + + for (int i = 0; i < AFATFS_NUM_CACHE_SECTORS; i++) { + if (afatfs.cacheDescriptor[i].state != AFATFS_CACHE_STATE_EMPTY + && afatfs.cacheDescriptor[i].sectorIndex == sectorIndex + ) { + if (buffer == NULL) { + // Read failed, mark the sector as empty and whoever asked for it will ask for it again later to retry + afatfs.cacheDescriptor[i].state = AFATFS_CACHE_STATE_EMPTY; + } else { + afatfs_assert(afatfs_cacheSectorGetMemory(i) == buffer && afatfs.cacheDescriptor[i].state == AFATFS_CACHE_STATE_READING); + + afatfs.cacheDescriptor[i].state = AFATFS_CACHE_STATE_IN_SYNC; + } + + break; + } + } +} + +/** + * Called by the SD card driver when one of our write operations completes. + */ +static void afatfs_sdcardWriteComplete(sdcardBlockOperation_e operation, uint32_t sectorIndex, uint8_t *buffer, uint32_t callbackData) +{ + (void) operation; + (void) callbackData; + + afatfs.cacheFlushInProgress = false; + + for (int i = 0; i < AFATFS_NUM_CACHE_SECTORS; i++) { + /* Keep in mind that someone may have marked the sector as dirty after writing had already begun. In this case we must leave + * it marked as dirty because those modifications may have been made too late to make it to the disk! + */ + if (afatfs.cacheDescriptor[i].sectorIndex == sectorIndex + && afatfs.cacheDescriptor[i].state == AFATFS_CACHE_STATE_WRITING + ) { + if (buffer == NULL) { + // Write failed, remark the sector as dirty + afatfs.cacheDescriptor[i].state = AFATFS_CACHE_STATE_DIRTY; + afatfs.cacheDirtyEntries++; + } else { + afatfs_assert(afatfs_cacheSectorGetMemory(i) == buffer); + + afatfs.cacheDescriptor[i].state = AFATFS_CACHE_STATE_IN_SYNC; + } + break; + } + } +} + +/** + * Attempt to flush the dirty cache entry with the given index to the SDcard. + */ +static void afatfs_cacheFlushSector(int cacheIndex) +{ + afatfsCacheBlockDescriptor_t *cacheDescriptor = &afatfs.cacheDescriptor[cacheIndex]; + +#ifdef AFATFS_MIN_MULTIPLE_BLOCK_WRITE_COUNT + if (cacheDescriptor->consecutiveEraseBlockCount) { + sdcard_beginWriteBlocks(cacheDescriptor->sectorIndex, cacheDescriptor->consecutiveEraseBlockCount); + } +#endif + + switch (sdcard_writeBlock(cacheDescriptor->sectorIndex, afatfs_cacheSectorGetMemory(cacheIndex), afatfs_sdcardWriteComplete, 0)) { + case SDCARD_OPERATION_IN_PROGRESS: + // The card will call us back later when the buffer transmission finishes + afatfs.cacheDirtyEntries--; + cacheDescriptor->state = AFATFS_CACHE_STATE_WRITING; + afatfs.cacheFlushInProgress = true; + break; + + case SDCARD_OPERATION_SUCCESS: + // Buffer is already transmitted + afatfs.cacheDirtyEntries--; + cacheDescriptor->state = AFATFS_CACHE_STATE_IN_SYNC; + break; + + case SDCARD_OPERATION_BUSY: + case SDCARD_OPERATION_FAILURE: + default: + ; + } +} + +/** + * Find a sector in the cache which corresponds to the given physical sector index, or NULL if the sector isn't + * cached. Note that the cached sector could be in any state including completely empty. + */ +static afatfsCacheBlockDescriptor_t* afatfs_findCacheSector(uint32_t sectorIndex) +{ + for (int i = 0; i < AFATFS_NUM_CACHE_SECTORS; i++) { + if (afatfs.cacheDescriptor[i].sectorIndex == sectorIndex) { + return &afatfs.cacheDescriptor[i]; + } + } + + return NULL; +} + +/** + * Find or allocate a cache sector for the given sector index on disk. Returns a block which matches one of these + * conditions (in descending order of preference): + * + * - The requested sector that already exists in the cache + * - The index of an empty sector + * - The index of a synced discardable sector + * - The index of the oldest synced sector + * + * Otherwise it returns -1 to signal failure (cache is full!) + */ +static int afatfs_allocateCacheSector(uint32_t sectorIndex) +{ + int allocateIndex; + int emptyIndex = -1, discardableIndex = -1; + + uint32_t oldestSyncedSectorLastUse = 0xFFFFFFFF; + int oldestSyncedSectorIndex = -1; + + if ( + !afatfs_assert( + afatfs.numClusters == 0 // We're unable to check sector bounds during startup since we haven't read volume label yet + || sectorIndex < afatfs.clusterStartSector + afatfs.numClusters * afatfs.sectorsPerCluster + ) + ) { + return -1; + } + + for (int i = 0; i < AFATFS_NUM_CACHE_SECTORS; i++) { + if (afatfs.cacheDescriptor[i].sectorIndex == sectorIndex) { + /* + * If the sector is actually empty then do a complete re-init of it just like the standard + * empty case. (Sectors marked as empty should be treated as if they don't have a block index assigned) + */ + if (afatfs.cacheDescriptor[i].state == AFATFS_CACHE_STATE_EMPTY) { + emptyIndex = i; + break; + } + + // Bump the last access time + afatfs.cacheDescriptor[i].accessTimestamp = ++afatfs.cacheTimer; + return i; + } + + switch (afatfs.cacheDescriptor[i].state) { + case AFATFS_CACHE_STATE_EMPTY: + emptyIndex = i; + break; + case AFATFS_CACHE_STATE_IN_SYNC: + // Is this a synced sector that we could evict from the cache? + if (!afatfs.cacheDescriptor[i].locked && afatfs.cacheDescriptor[i].retainCount == 0) { + if (afatfs.cacheDescriptor[i].discardable) { + discardableIndex = i; + } else if (afatfs.cacheDescriptor[i].accessTimestamp < oldestSyncedSectorLastUse) { + // This is older than last block we decided to evict, so evict this one in preference + oldestSyncedSectorLastUse = afatfs.cacheDescriptor[i].accessTimestamp; + oldestSyncedSectorIndex = i; + } + } + break; + default: + ; + } + } + + if (emptyIndex > -1) { + allocateIndex = emptyIndex; + } else if (discardableIndex > -1) { + allocateIndex = discardableIndex; + } else if (oldestSyncedSectorIndex > -1) { + allocateIndex = oldestSyncedSectorIndex; + } else { + allocateIndex = -1; + } + + if (allocateIndex > -1) { + afatfs_cacheSectorInit(&afatfs.cacheDescriptor[allocateIndex], sectorIndex, false); + } + + return allocateIndex; +} + +/** + * Attempt to flush dirty cache pages out to the sdcard, returning true if all flushable data has been flushed. + */ +bool afatfs_flush() +{ + if (afatfs.cacheDirtyEntries > 0) { + // Flush the oldest flushable sector + uint32_t earliestSectorTime = 0xFFFFFFFF; + int earliestSectorIndex = -1; + + for (int i = 0; i < AFATFS_NUM_CACHE_SECTORS; i++) { + if (afatfs.cacheDescriptor[i].state == AFATFS_CACHE_STATE_DIRTY && !afatfs.cacheDescriptor[i].locked + && (earliestSectorIndex == -1 || afatfs.cacheDescriptor[i].writeTimestamp < earliestSectorTime) + ) { + earliestSectorIndex = i; + earliestSectorTime = afatfs.cacheDescriptor[i].writeTimestamp; + } + } + + if (earliestSectorIndex > -1) { + afatfs_cacheFlushSector(earliestSectorIndex); + + // That flush will take time to complete so we may as well tell caller to come back later + return false; + } + } + + return true; +} + +/** + * Returns true if either the freefile or the regular cluster pool has been exhausted during a previous write operation. + */ +bool afatfs_isFull() +{ + return afatfs.filesystemFull; +} + +/** + * Get the physical sector number that corresponds to the FAT sector of the given fatSectorIndex within the given + * FAT (fatIndex may be 0 or 1). (0, 0) gives the first sector of the first FAT. + */ +static uint32_t afatfs_fatSectorToPhysical(int fatIndex, uint32_t fatSectorIndex) +{ + return afatfs.fatStartSector + (fatIndex ? afatfs.fatSectors : 0) + fatSectorIndex; +} + +static uint32_t afatfs_fileClusterToPhysical(uint32_t clusterNumber, uint32_t sectorIndex) +{ + return afatfs.clusterStartSector + (clusterNumber - 2) * afatfs.sectorsPerCluster + sectorIndex; +} + +static uint32_t afatfs_fileGetCursorPhysicalSector(afatfsFilePtr_t file) +{ + if (file->type == AFATFS_FILE_TYPE_FAT16_ROOT_DIRECTORY) { + return afatfs.fatStartSector + AFATFS_NUM_FATS * afatfs.fatSectors + file->cursorOffset / AFATFS_SECTOR_SIZE; + } else { + uint32_t cursorSectorInCluster = afatfs_sectorIndexInCluster(file->cursorOffset); + return afatfs_fileClusterToPhysical(file->cursorCluster, cursorSectorInCluster); + } +} + +/** + * Sector here is the sector index within the cluster. + */ +static void afatfs_fileGetCursorClusterAndSector(afatfsFilePtr_t file, uint32_t *cluster, uint16_t *sector) +{ + *cluster = file->cursorCluster; + + if (file->type == AFATFS_FILE_TYPE_FAT16_ROOT_DIRECTORY) { + *sector = file->cursorOffset / AFATFS_SECTOR_SIZE; + } else { + *sector = afatfs_sectorIndexInCluster(file->cursorOffset); + } +} + +/** + * Get a cache entry for the given sector and store a pointer to the cached memory in *buffer. + * + * physicalSectorIndex - The index of the sector in the SD card to cache + * sectorflags - A union of AFATFS_CACHE_* constants that says which operations the sector will be cached for. + * buffer - A pointer to the 512-byte memory buffer for the sector will be stored here upon success + * eraseCount - For write operations, set to a non-zero number to hint that we plan to write that many sectors + * consecutively (including this sector) + * + * Returns: + * AFATFS_OPERATION_SUCCESS - On success + * AFATFS_OPERATION_IN_PROGRESS - Card is busy, call again later + * AFATFS_OPERATION_FAILURE - When the filesystem encounters a fatal error + */ +static afatfsOperationStatus_e afatfs_cacheSector(uint32_t physicalSectorIndex, uint8_t **buffer, uint8_t sectorFlags, uint32_t eraseCount) +{ + // We never write to the MBR, so any attempt to write there is an asyncfatfs bug + if (!afatfs_assert((sectorFlags & AFATFS_CACHE_WRITE) == 0 || physicalSectorIndex != 0)) { + return AFATFS_OPERATION_FAILURE; + } + + int cacheSectorIndex = afatfs_allocateCacheSector(physicalSectorIndex); + + if (cacheSectorIndex == -1) { + // We don't have enough free cache to service this request right now, try again later + return AFATFS_OPERATION_IN_PROGRESS; + } + + switch (afatfs.cacheDescriptor[cacheSectorIndex].state) { + case AFATFS_CACHE_STATE_READING: + return AFATFS_OPERATION_IN_PROGRESS; + break; + + case AFATFS_CACHE_STATE_EMPTY: + if ((sectorFlags & AFATFS_CACHE_READ) != 0) { + if (sdcard_readBlock(physicalSectorIndex, afatfs_cacheSectorGetMemory(cacheSectorIndex), afatfs_sdcardReadComplete, 0)) { + afatfs.cacheDescriptor[cacheSectorIndex].state = AFATFS_CACHE_STATE_READING; + } + return AFATFS_OPERATION_IN_PROGRESS; + } + + // We only get to decide these fields if we're the first ones to cache the sector: + afatfs.cacheDescriptor[cacheSectorIndex].discardable = (sectorFlags & AFATFS_CACHE_DISCARDABLE) != 0 ? 1 : 0; + +#ifdef AFATFS_MIN_MULTIPLE_BLOCK_WRITE_COUNT + // Don't bother pre-erasing for small block sequences + if (eraseCount < AFATFS_MIN_MULTIPLE_BLOCK_WRITE_COUNT) { + eraseCount = 0; + } else { + eraseCount = MIN(eraseCount, UINT16_MAX); // If caller asked for a longer chain of sectors we silently truncate that here + } + + afatfs.cacheDescriptor[cacheSectorIndex].consecutiveEraseBlockCount = eraseCount; +#endif + + // Fall through + + case AFATFS_CACHE_STATE_WRITING: + case AFATFS_CACHE_STATE_IN_SYNC: + if ((sectorFlags & AFATFS_CACHE_WRITE) != 0) { + afatfs_cacheSectorMarkDirty(&afatfs.cacheDescriptor[cacheSectorIndex]); + } + // Fall through + + case AFATFS_CACHE_STATE_DIRTY: + if ((sectorFlags & AFATFS_CACHE_LOCK) != 0) { + afatfs.cacheDescriptor[cacheSectorIndex].locked = 1; + } + if ((sectorFlags & AFATFS_CACHE_RETAIN) != 0) { + afatfs.cacheDescriptor[cacheSectorIndex].retainCount++; + } + + *buffer = afatfs_cacheSectorGetMemory(cacheSectorIndex); + + return AFATFS_OPERATION_SUCCESS; + break; + + default: + // Cache block in unknown state, should never happen + afatfs_assert(false); + return AFATFS_OPERATION_FAILURE; + } +} + +/** + * Parse the details out of the given MBR sector (512 bytes long). Return true if a compatible filesystem was found. + */ +static bool afatfs_parseMBR(const uint8_t *sector) +{ + // Check MBR signature + if (sector[AFATFS_SECTOR_SIZE - 2] != 0x55 || sector[AFATFS_SECTOR_SIZE - 1] != 0xAA) + return false; + + mbrPartitionEntry_t *partition = (mbrPartitionEntry_t *) (sector + 446); + + for (int i = 0; i < 4; i++) { + if ( + partition[i].lbaBegin > 0 + && ( + partition[i].type == MBR_PARTITION_TYPE_FAT32 + || partition[i].type == MBR_PARTITION_TYPE_FAT32_LBA + || partition[i].type == MBR_PARTITION_TYPE_FAT16 + || partition[i].type == MBR_PARTITION_TYPE_FAT16_LBA + ) + ) { + afatfs.partitionStartSector = partition[i].lbaBegin; + + return true; + } + } + + return false; +} + +static bool afatfs_parseVolumeID(const uint8_t *sector) +{ + fatVolumeID_t *volume = (fatVolumeID_t *) sector; + + afatfs.filesystemType = FAT_FILESYSTEM_TYPE_INVALID; + + if (volume->bytesPerSector != AFATFS_SECTOR_SIZE || volume->numFATs != AFATFS_NUM_FATS + || sector[510] != FAT_VOLUME_ID_SIGNATURE_1 || sector[511] != FAT_VOLUME_ID_SIGNATURE_2) { + return false; + } + + afatfs.fatStartSector = afatfs.partitionStartSector + volume->reservedSectorCount; + + afatfs.sectorsPerCluster = volume->sectorsPerCluster; + if (afatfs.sectorsPerCluster < 1 || afatfs.sectorsPerCluster > 128 || !isPowerOfTwo(afatfs.sectorsPerCluster)) { + return false; + } + + afatfs.byteInClusterMask = AFATFS_SECTOR_SIZE * afatfs.sectorsPerCluster - 1; + + afatfs.fatSectors = volume->FATSize16 != 0 ? volume->FATSize16 : volume->fatDescriptor.fat32.FATSize32; + + // Always zero on FAT32 since rootEntryCount is always zero (this is non-zero on FAT16) + afatfs.rootDirectorySectors = ((volume->rootEntryCount * FAT_DIRECTORY_ENTRY_SIZE) + (volume->bytesPerSector - 1)) / volume->bytesPerSector; + uint32_t totalSectors = volume->totalSectors16 != 0 ? volume->totalSectors16 : volume->totalSectors32; + uint32_t dataSectors = totalSectors - (volume->reservedSectorCount + (AFATFS_NUM_FATS * afatfs.fatSectors) + afatfs.rootDirectorySectors); + + afatfs.numClusters = dataSectors / volume->sectorsPerCluster; + + if (afatfs.numClusters <= FAT12_MAX_CLUSTERS) { + afatfs.filesystemType = FAT_FILESYSTEM_TYPE_FAT12; + + return false; // FAT12 is not a supported filesystem + } else if (afatfs.numClusters <= FAT16_MAX_CLUSTERS) { + afatfs.filesystemType = FAT_FILESYSTEM_TYPE_FAT16; + } else { + afatfs.filesystemType = FAT_FILESYSTEM_TYPE_FAT32; + } + + if (afatfs.filesystemType == FAT_FILESYSTEM_TYPE_FAT32) { + afatfs.rootDirectoryCluster = volume->fatDescriptor.fat32.rootCluster; + } else { + // FAT16 doesn't store the root directory in clusters + afatfs.rootDirectoryCluster = 0; + } + + uint32_t endOfFATs = afatfs.fatStartSector + AFATFS_NUM_FATS * afatfs.fatSectors; + + afatfs.clusterStartSector = endOfFATs + afatfs.rootDirectorySectors; + + return true; +} + +/** + * Get the position of the FAT entry for the cluster with the given number. + */ +static void afatfs_getFATPositionForCluster(uint32_t cluster, uint32_t *fatSectorIndex, uint32_t *fatSectorEntryIndex) +{ + if (afatfs.filesystemType == FAT_FILESYSTEM_TYPE_FAT16) { + uint32_t entriesPerFATSector = AFATFS_SECTOR_SIZE / sizeof(uint16_t); + + *fatSectorIndex = cluster / entriesPerFATSector; + *fatSectorEntryIndex = cluster & (entriesPerFATSector - 1); + } else { + uint32_t entriesPerFATSector = AFATFS_SECTOR_SIZE / sizeof(uint32_t); + + *fatSectorIndex = fat32_decodeClusterNumber(cluster) / entriesPerFATSector; + *fatSectorEntryIndex = cluster & (entriesPerFATSector - 1); + } +} + +static bool afatfs_FATIsEndOfChainMarker(uint32_t clusterNumber) +{ + if (afatfs.filesystemType == FAT_FILESYSTEM_TYPE_FAT32) { + return fat32_isEndOfChainMarker(clusterNumber); + } else { + return fat16_isEndOfChainMarker(clusterNumber); + } +} + +/** + * Look up the FAT to find out which cluster follows the one with the given number and store it into *nextCluster. + * + * Use fat_isFreeSpace() and fat_isEndOfChainMarker() on nextCluster to distinguish those special values from regular + * cluster numbers. + * + * Note that if you're trying to find the next cluster of a file, you should be calling afatfs_fileGetNextCluster() + * instead, as that one supports contiguous freefile-based files (which needn't consult the FAT). + * + * Returns: + * AFATFS_OPERATION_IN_PROGRESS - FS is busy right now, call again later + * AFATFS_OPERATION_SUCCESS - *nextCluster is set to the next cluster number + */ +static afatfsOperationStatus_e afatfs_FATGetNextCluster(int fatIndex, uint32_t cluster, uint32_t *nextCluster) +{ + uint32_t fatSectorIndex, fatSectorEntryIndex; + afatfsFATSector_t sector; + + afatfs_getFATPositionForCluster(cluster, &fatSectorIndex, &fatSectorEntryIndex); + + afatfsOperationStatus_e result = afatfs_cacheSector(afatfs_fatSectorToPhysical(fatIndex, fatSectorIndex), §or.bytes, AFATFS_CACHE_READ, 0); + + if (result == AFATFS_OPERATION_SUCCESS) { + if (afatfs.filesystemType == FAT_FILESYSTEM_TYPE_FAT16) { + *nextCluster = sector.fat16[fatSectorEntryIndex]; + } else { + *nextCluster = fat32_decodeClusterNumber(sector.fat32[fatSectorEntryIndex]); + } + } + + return result; +} + +/** + * Set the cluster number that follows the given cluster. Pass 0xFFFFFFFF for nextCluster to terminate the FAT chain. + * + * Returns: + * AFATFS_OPERATION_SUCCESS - On success + * AFATFS_OPERATION_IN_PROGRESS - Card is busy, call again later + * AFATFS_OPERATION_FAILURE - When the filesystem encounters a fatal error + */ +static afatfsOperationStatus_e afatfs_FATSetNextCluster(uint32_t startCluster, uint32_t nextCluster) +{ + afatfsFATSector_t sector; + uint32_t fatSectorIndex, fatSectorEntryIndex, fatPhysicalSector; + afatfsOperationStatus_e result; + +#ifdef AFATFS_DEBUG + afatfs_assert(startCluster >= FAT_SMALLEST_LEGAL_CLUSTER_NUMBER); +#endif + + afatfs_getFATPositionForCluster(startCluster, &fatSectorIndex, &fatSectorEntryIndex); + + fatPhysicalSector = afatfs_fatSectorToPhysical(0, fatSectorIndex); + + result = afatfs_cacheSector(fatPhysicalSector, §or.bytes, AFATFS_CACHE_READ | AFATFS_CACHE_WRITE, 0); + + if (result == AFATFS_OPERATION_SUCCESS) { + if (afatfs.filesystemType == FAT_FILESYSTEM_TYPE_FAT16) { + sector.fat16[fatSectorEntryIndex] = nextCluster; + } else { + sector.fat32[fatSectorEntryIndex] = nextCluster; + } + } + + return result; +} + +/** + * Bring the logical filesize up to date with the current cursor position. + */ +static void afatfs_fileUpdateFilesize(afatfsFile_t *file) +{ + file->logicalSize = MAX(file->logicalSize, file->cursorOffset); +} + +static void afatfs_fileUnlockCacheSector(afatfsFilePtr_t file) +{ + if (file->writeLockedCacheIndex != -1) { + afatfs.cacheDescriptor[file->writeLockedCacheIndex].locked = 0; + file->writeLockedCacheIndex = -1; + } + if (file->readRetainCacheIndex != -1) { + afatfs.cacheDescriptor[file->readRetainCacheIndex].retainCount = MAX((int) afatfs.cacheDescriptor[file->readRetainCacheIndex].retainCount - 1, 0); + file->readRetainCacheIndex = -1; + } +} + +/** + * Starting from and including the given cluster number, find the number of the first cluster which matches the given + * condition. + * + * searchLimit - Last cluster to examine (exclusive). To search the entire volume, pass: + * afatfs.numClusters + FAT_SMALLEST_LEGAL_CLUSTER_NUMBER + * + * Condition: + * CLUSTER_SEARCH_FREE_AT_BEGINNING_OF_FAT_SECTOR - Find a cluster marked as free in the FAT which lies at the + * beginning of its FAT sector. The passed initial search 'cluster' must correspond to the first entry of a FAT sector. + * CLUSTER_SEARCH_FREE - Find a cluster marked as free in the FAT + * CLUSTER_SEARCH_OCCUPIED - Find a cluster marked as occupied in the FAT. + * + * Returns: + * AFATFS_FIND_CLUSTER_FOUND - A cluster matching the criteria was found and stored in *cluster + * AFATFS_FIND_CLUSTER_IN_PROGRESS - The search is not over, call this routine again later with the updated *cluster value to resume + * AFATFS_FIND_CLUSTER_FATAL - An unexpected read error occurred, the volume should be abandoned + * AFATFS_FIND_CLUSTER_NOT_FOUND - The entire device was searched without finding a suitable cluster (the + * *cluster points to just beyond the final cluster). + */ +static afatfsFindClusterStatus_e afatfs_findClusterWithCondition(afatfsClusterSearchCondition_e condition, uint32_t *cluster, uint32_t searchLimit) +{ + afatfsFATSector_t sector; + uint32_t fatSectorIndex, fatSectorEntryIndex; + + uint32_t fatEntriesPerSector = afatfs_fatEntriesPerSector(); + bool lookingForFree = condition == CLUSTER_SEARCH_FREE_AT_BEGINNING_OF_FAT_SECTOR || condition == CLUSTER_SEARCH_FREE; + + int jump; + + // Get the FAT entry which corresponds to this cluster so we can begin our search there + afatfs_getFATPositionForCluster(*cluster, &fatSectorIndex, &fatSectorEntryIndex); + + switch (condition) { + case CLUSTER_SEARCH_FREE_AT_BEGINNING_OF_FAT_SECTOR: + jump = fatEntriesPerSector; + + // We're supposed to call this routine with the cluster properly aligned + if (!afatfs_assert(fatSectorEntryIndex == 0)) { + return AFATFS_FIND_CLUSTER_FATAL; + } + break; + case CLUSTER_SEARCH_OCCUPIED: + case CLUSTER_SEARCH_FREE: + jump = 1; + break; + default: + afatfs_assert(false); + return AFATFS_FIND_CLUSTER_FATAL; + } + + while (*cluster < searchLimit) { + +#ifdef AFATFS_USE_FREEFILE + // If we're looking inside the freefile, we won't find any free clusters! Skip it! + if (afatfs.freeFile.logicalSize > 0 && *cluster == afatfs.freeFile.firstCluster) { + *cluster += (afatfs.freeFile.logicalSize + afatfs_clusterSize() - 1) / afatfs_clusterSize(); + + // Maintain alignment + *cluster = roundUpTo(*cluster, jump); + continue; // Go back to check that the new cluster number is within the volume + } +#endif + + afatfsOperationStatus_e status = afatfs_cacheSector(afatfs_fatSectorToPhysical(0, fatSectorIndex), §or.bytes, AFATFS_CACHE_READ | AFATFS_CACHE_DISCARDABLE, 0); + + switch (status) { + case AFATFS_OPERATION_SUCCESS: + do { + uint32_t clusterNumber; + + switch (afatfs.filesystemType) { + case FAT_FILESYSTEM_TYPE_FAT16: + clusterNumber = sector.fat16[fatSectorEntryIndex]; + break; + case FAT_FILESYSTEM_TYPE_FAT32: + clusterNumber = fat32_decodeClusterNumber(sector.fat32[fatSectorEntryIndex]); + break; + default: + return AFATFS_FIND_CLUSTER_FATAL; + } + + if (fat_isFreeSpace(clusterNumber) == lookingForFree) { + /* + * The final FAT sector may have fewer than fatEntriesPerSector entries in it, so we need to + * check the cluster number is valid here before we report a bogus success! + */ + if (*cluster < searchLimit) { + return AFATFS_FIND_CLUSTER_FOUND; + } else { + *cluster = searchLimit; + return AFATFS_FIND_CLUSTER_NOT_FOUND; + } + } + + (*cluster) += jump; + fatSectorEntryIndex += jump; + } while (fatSectorEntryIndex < fatEntriesPerSector); + + // Move on to the next FAT sector + fatSectorIndex++; + fatSectorEntryIndex = 0; + break; + case AFATFS_OPERATION_FAILURE: + return AFATFS_FIND_CLUSTER_FATAL; + break; + case AFATFS_OPERATION_IN_PROGRESS: + return AFATFS_FIND_CLUSTER_IN_PROGRESS; + break; + } + } + + // We looked at every available cluster and didn't find one matching the condition + *cluster = searchLimit; + return AFATFS_FIND_CLUSTER_NOT_FOUND; +} + +/** + * Get the cluster that follows the currentCluster in the FAT chain for the given file. + * + * Returns: + * AFATFS_OPERATION_IN_PROGRESS - FS is busy right now, call again later + * AFATFS_OPERATION_SUCCESS - *nextCluster is set to the next cluster number + */ +static afatfsOperationStatus_e afatfs_fileGetNextCluster(afatfsFilePtr_t file, uint32_t currentCluster, uint32_t *nextCluster) +{ +#ifndef AFATFS_USE_FREEFILE + (void) file; +#else + if ((file->mode & AFATFS_FILE_MODE_CONTIGUOUS) != 0) { + uint32_t freeFileStart = afatfs.freeFile.firstCluster; + + afatfs_assert(currentCluster + 1 <= freeFileStart); + + // Would the next cluster lie outside the allocated file? (i.e. beyond the end of the file into the start of the freefile) + if (currentCluster + 1 == freeFileStart) { + *nextCluster = 0; + } else { + *nextCluster = currentCluster + 1; + } + + return AFATFS_OPERATION_SUCCESS; + } else +#endif + { + return afatfs_FATGetNextCluster(0, currentCluster, nextCluster); + } +} + +#ifdef AFATFS_USE_FREEFILE + +/** + * Update the FAT to fill the contiguous series of clusters with indexes [*startCluster...endCluster) with the + * specified pattern. + * + * AFATFS_FAT_PATTERN_TERMINATED_CHAIN - Chain the clusters together in linear sequence and terminate the final cluster + * AFATFS_FAT_PATTERN_CHAIN - Chain the clusters together without terminating the final entry + * AFATFS_FAT_PATTERN_FREE - Mark the clusters as free space + * + * Returns - + * AFATFS_OPERATION_SUCCESS - When the entire chain has been written + * AFATFS_OPERATION_IN_PROGRESS - Call again later with the updated *startCluster value in order to resume writing. + */ +static afatfsOperationStatus_e afatfs_FATFillWithPattern(afatfsFATPattern_e pattern, uint32_t *startCluster, uint32_t endCluster) +{ + afatfsFATSector_t sector; + uint32_t fatSectorIndex, firstEntryIndex, fatPhysicalSector; + uint8_t fatEntrySize; + uint32_t nextCluster; + afatfsOperationStatus_e result; + uint32_t eraseSectorCount; + + // Find the position of the initial cluster to begin our fill + afatfs_getFATPositionForCluster(*startCluster, &fatSectorIndex, &firstEntryIndex); + + fatPhysicalSector = afatfs_fatSectorToPhysical(0, fatSectorIndex); + + // How many consecutive FAT sectors will we be overwriting? + eraseSectorCount = (endCluster - *startCluster + firstEntryIndex + afatfs_fatEntriesPerSector() - 1) / afatfs_fatEntriesPerSector(); + + while (*startCluster < endCluster) { + // The last entry we will fill inside this sector (exclusive): + uint32_t lastEntryIndex = MIN(firstEntryIndex + (endCluster - *startCluster), afatfs_fatEntriesPerSector()); + + uint8_t cacheFlags = AFATFS_CACHE_WRITE | AFATFS_CACHE_DISCARDABLE; + + if (firstEntryIndex > 0 || lastEntryIndex < afatfs_fatEntriesPerSector()) { + // We're not overwriting the entire FAT sector so we must read the existing contents + cacheFlags |= AFATFS_CACHE_READ; + } + + result = afatfs_cacheSector(fatPhysicalSector, §or.bytes, cacheFlags, eraseSectorCount); + + if (result != AFATFS_OPERATION_SUCCESS) { + return result; + } + +#ifdef AFATFS_DEBUG_VERBOSE + if (pattern == AFATFS_FAT_PATTERN_FREE) { + fprintf(stderr, "Marking cluster %u to %u as free in FAT sector %u...\n", *startCluster, endCluster, fatPhysicalSector); + } else { + fprintf(stderr, "Writing FAT chain from cluster %u to %u in FAT sector %u...\n", *startCluster, endCluster, fatPhysicalSector); + } +#endif + + switch (pattern) { + case AFATFS_FAT_PATTERN_TERMINATED_CHAIN: + case AFATFS_FAT_PATTERN_UNTERMINATED_CHAIN: + nextCluster = *startCluster + 1; + // Write all the "next cluster" pointers + if (afatfs.filesystemType == FAT_FILESYSTEM_TYPE_FAT16) { + for (uint32_t i = firstEntryIndex; i < lastEntryIndex; i++, nextCluster++) { + sector.fat16[i] = nextCluster; + } + } else { + for (uint32_t i = firstEntryIndex; i < lastEntryIndex; i++, nextCluster++) { + sector.fat32[i] = nextCluster; + } + } + + *startCluster += lastEntryIndex - firstEntryIndex; + + if (pattern == AFATFS_FAT_PATTERN_TERMINATED_CHAIN && *startCluster == endCluster) { + // We completed the chain! Overwrite the last entry we wrote with the terminator for the end of the chain + if (afatfs.filesystemType == FAT_FILESYSTEM_TYPE_FAT16) { + sector.fat16[lastEntryIndex - 1] = 0xFFFF; + } else { + sector.fat32[lastEntryIndex - 1] = 0xFFFFFFFF; + } + break; + } + break; + case AFATFS_FAT_PATTERN_FREE: + fatEntrySize = afatfs.filesystemType == FAT_FILESYSTEM_TYPE_FAT16 ? sizeof(uint16_t) : sizeof(uint32_t); + + memset(sector.bytes + firstEntryIndex * fatEntrySize, 0, (lastEntryIndex - firstEntryIndex) * fatEntrySize); + + *startCluster += lastEntryIndex - firstEntryIndex; + break; + } + + fatPhysicalSector++; + eraseSectorCount--; + firstEntryIndex = 0; + } + + return AFATFS_OPERATION_SUCCESS; +} + +#endif + +/** + * Write the directory entry for the file into its `directoryEntryPos` position in its containing directory. + * + * mode: + * AFATFS_SAVE_DIRECTORY_NORMAL - Store the file's physical size, not the logical size, in the directory entry + * AFATFS_SAVE_DIRECTORY_FOR_CLOSE - We're done extending the file so we can write the logical size now. + * AFATFS_SAVE_DIRECTORY_DELETED - Mark the directory entry as deleted + * + * Returns: + * AFATFS_OPERATION_SUCCESS - The directory entry has been stored into the directory sector in cache. + * AFATFS_OPERATION_IN_PROGRESS - Cache is too busy, retry later + * AFATFS_OPERATION_FAILURE - If the filesystem enters the fatal state + */ +static afatfsOperationStatus_e afatfs_saveDirectoryEntry(afatfsFilePtr_t file, afatfsSaveDirectoryEntryMode_e mode) +{ + uint8_t *sector; + afatfsOperationStatus_e result; + + if (file->directoryEntryPos.sectorNumberPhysical == 0) { + return AFATFS_OPERATION_SUCCESS; // Root directories don't have a directory entry + } + + result = afatfs_cacheSector(file->directoryEntryPos.sectorNumberPhysical, §or, AFATFS_CACHE_READ | AFATFS_CACHE_WRITE, 0); + +#ifdef AFATFS_DEBUG_VERBOSE + fprintf(stderr, "Saving directory entry to sector %u...\n", file->directoryEntryPos.sectorNumberPhysical); +#endif + + if (result == AFATFS_OPERATION_SUCCESS) { + if (afatfs_assert(file->directoryEntryPos.entryIndex >= 0)) { + fatDirectoryEntry_t *entry = (fatDirectoryEntry_t *) sector + file->directoryEntryPos.entryIndex; + + switch (mode) { + case AFATFS_SAVE_DIRECTORY_NORMAL: + /* We exaggerate the length of the written file so that if power is lost, the end of the file will + * still be readable (though the very tail of the file will be uninitialized data). + * + * This way we can avoid updating the directory entry too many times during fwrites() on the file. + */ + entry->fileSize = file->physicalSize; + break; + case AFATFS_SAVE_DIRECTORY_DELETED: + entry->filename[0] = FAT_DELETED_FILE_MARKER; + //Fall through + + case AFATFS_SAVE_DIRECTORY_FOR_CLOSE: + // We write the true length of the file on close. + entry->fileSize = file->logicalSize; + } + + // (sub)directories don't store a filesize in their directory entry: + if (file->type == AFATFS_FILE_TYPE_DIRECTORY) { + entry->fileSize = 0; + } + + entry->firstClusterHigh = file->firstCluster >> 16; + entry->firstClusterLow = file->firstCluster & 0xFFFF; + } else { + return AFATFS_OPERATION_FAILURE; + } + } + + return result; +} + +/** + * Attempt to add a free cluster to the end of the given file. If the file was previously empty, the directory entry + * is updated to point to the new cluster. + * + * Returns: + * AFATFS_OPERATION_SUCCESS - The cluster has been appended + * AFATFS_OPERATION_IN_PROGRESS - Cache was busy, so call again later to continue + * AFATFS_OPERATION_FAILURE - Cluster could not be appended because the filesystem ran out of space + * (afatfs.filesystemFull is set to true) + * + * If the file's operation was AFATFS_FILE_OPERATION_APPEND_FREE_CLUSTER, the file operation is cleared upon completion, + * otherwise it is left alone so that this operation can be called as a sub-operation of some other operation on the + * file. + */ +static afatfsOperationStatus_e afatfs_appendRegularFreeClusterContinue(afatfsFile_t *file) +{ + afatfsAppendFreeCluster_t *opState = &file->operation.state.appendFreeCluster; + afatfsOperationStatus_e status; + + doMore: + + switch (opState->phase) { + case AFATFS_APPEND_FREE_CLUSTER_PHASE_FIND_FREESPACE: + switch (afatfs_findClusterWithCondition(CLUSTER_SEARCH_FREE, &opState->searchCluster, afatfs.numClusters + FAT_SMALLEST_LEGAL_CLUSTER_NUMBER)) { + case AFATFS_FIND_CLUSTER_FOUND: + afatfs.lastClusterAllocated = opState->searchCluster; + + // Make the cluster available for us to write in + file->cursorCluster = opState->searchCluster; + file->physicalSize += afatfs_clusterSize(); + + if (opState->previousCluster == 0) { + // This is the new first cluster in the file + file->firstCluster = opState->searchCluster; + } + + opState->phase = AFATFS_APPEND_FREE_CLUSTER_PHASE_UPDATE_FAT1; + goto doMore; + break; + case AFATFS_FIND_CLUSTER_FATAL: + case AFATFS_FIND_CLUSTER_NOT_FOUND: + // We couldn't find an empty cluster to append to the file + opState->phase = AFATFS_APPEND_FREE_CLUSTER_PHASE_FAILURE; + goto doMore; + break; + case AFATFS_FIND_CLUSTER_IN_PROGRESS: + break; + } + break; + case AFATFS_APPEND_FREE_CLUSTER_PHASE_UPDATE_FAT1: + // Terminate the new cluster + status = afatfs_FATSetNextCluster(opState->searchCluster, 0xFFFFFFFF); + + if (status == AFATFS_OPERATION_SUCCESS) { + if (opState->previousCluster) { + opState->phase = AFATFS_APPEND_FREE_CLUSTER_PHASE_UPDATE_FAT2; + } else { + opState->phase = AFATFS_APPEND_FREE_CLUSTER_PHASE_UPDATE_FILE_DIRECTORY; + } + + goto doMore; + } + break; + case AFATFS_APPEND_FREE_CLUSTER_PHASE_UPDATE_FAT2: + // Add the new cluster to the pre-existing chain + status = afatfs_FATSetNextCluster(opState->previousCluster, opState->searchCluster); + + if (status == AFATFS_OPERATION_SUCCESS) { + opState->phase = AFATFS_APPEND_FREE_CLUSTER_PHASE_UPDATE_FILE_DIRECTORY; + goto doMore; + } + break; + case AFATFS_APPEND_FREE_CLUSTER_PHASE_UPDATE_FILE_DIRECTORY: + if (afatfs_saveDirectoryEntry(file, AFATFS_SAVE_DIRECTORY_NORMAL) == AFATFS_OPERATION_SUCCESS) { + opState->phase = AFATFS_APPEND_FREE_CLUSTER_PHASE_COMPLETE; + goto doMore; + } + break; + case AFATFS_APPEND_FREE_CLUSTER_PHASE_COMPLETE: + if (file->operation.operation == AFATFS_FILE_OPERATION_APPEND_FREE_CLUSTER) { + file->operation.operation = AFATFS_FILE_OPERATION_NONE; + } + + return AFATFS_OPERATION_SUCCESS; + break; + case AFATFS_APPEND_FREE_CLUSTER_PHASE_FAILURE: + if (file->operation.operation == AFATFS_FILE_OPERATION_APPEND_FREE_CLUSTER) { + file->operation.operation = AFATFS_FILE_OPERATION_NONE; + } + + afatfs.filesystemFull = true; + return AFATFS_OPERATION_FAILURE; + break; + } + + return AFATFS_OPERATION_IN_PROGRESS; +} + +static void afatfs_appendRegularFreeClusterInitOperationState(afatfsAppendFreeCluster_t *state, uint32_t previousCluster) +{ + state->phase = AFATFS_APPEND_FREE_CLUSTER_PHASE_INITIAL; + state->previousCluster = previousCluster; + state->searchCluster = afatfs.lastClusterAllocated; +} + +/** + * Queue up an operation to append a free cluster to the file and update the file's cursorCluster to point to it. + * + * You must seek to the end of the file first, so file.cursorCluster will be 0 for the first call, and + * `file.cursorPreviousCluster` will be the cluster to append after. + * + * Note that the cursorCluster will be updated before this operation is completely finished (i.e. before the FAT is + * updated) but you can go ahead and write to it before the operation succeeds. + * + * Returns: + * AFATFS_OPERATION_SUCCESS - The append completed successfully + * AFATFS_OPERATION_IN_PROGRESS - The operation was queued on the file and will complete later + * AFATFS_OPERATION_FAILURE - Operation could not be queued or append failed, check afatfs.fileSystemFull + */ +static afatfsOperationStatus_e afatfs_appendRegularFreeCluster(afatfsFilePtr_t file) +{ + if (file->operation.operation == AFATFS_FILE_OPERATION_APPEND_FREE_CLUSTER) + return AFATFS_OPERATION_IN_PROGRESS; + + if (afatfs.filesystemFull || afatfs_fileIsBusy(file)) { + return AFATFS_OPERATION_FAILURE; + } + + file->operation.operation = AFATFS_FILE_OPERATION_APPEND_FREE_CLUSTER; + + afatfs_appendRegularFreeClusterInitOperationState(&file->operation.state.appendFreeCluster, file->cursorPreviousCluster); + + return afatfs_appendRegularFreeClusterContinue(file); +} + +#ifdef AFATFS_USE_FREEFILE + +/** + * Size of a AFATFS supercluster in bytes + */ +ONLY_EXPOSE_FOR_TESTING +uint32_t afatfs_superClusterSize() +{ + return afatfs_fatEntriesPerSector() * afatfs_clusterSize(); +} + +/** + * Continue to attempt to add a supercluster to the end of the given file. + * + * If the file operation was set to AFATFS_FILE_OPERATION_APPEND_SUPERCLUSTER and the operation completes, the file's + * operation is cleared. + * + * Returns: + * AFATFS_OPERATION_SUCCESS - On completion + * AFATFS_OPERATION_IN_PROGRESS - Operation still in progress + */ +static afatfsOperationStatus_e afatfs_appendSuperclusterContinue(afatfsFile_t *file) +{ + afatfsAppendSupercluster_t *opState = &file->operation.state.appendSupercluster; + + afatfsOperationStatus_e status; + + doMore: + switch (opState->phase) { + case AFATFS_APPEND_SUPERCLUSTER_PHASE_INIT: + // Our file steals the first cluster of the freefile + + // We can go ahead and write to that space before the FAT and directory are updated + file->cursorCluster = afatfs.freeFile.firstCluster; + file->physicalSize += afatfs_superClusterSize(); + + /* Remove the first supercluster from the freefile + * + * Even if the freefile becomes empty, we still don't set its first cluster to zero. This is so that + * afatfs_fileGetNextCluster() can tell where a contiguous file ends (at the start of the freefile). + * + * Note that normally the freefile can't become empty because it is allocated as a non-integer number + * of superclusters to avoid precisely this situation. + */ + afatfs.freeFile.firstCluster += afatfs_fatEntriesPerSector(); + afatfs.freeFile.logicalSize -= afatfs_superClusterSize(); + afatfs.freeFile.physicalSize -= afatfs_superClusterSize(); + + // The new supercluster needs to have its clusters chained contiguously and marked with a terminator at the end + opState->fatRewriteStartCluster = file->cursorCluster; + opState->fatRewriteEndCluster = opState->fatRewriteStartCluster + afatfs_fatEntriesPerSector(); + + if (opState->previousCluster == 0) { + // This is the new first cluster in the file so we need to update the directory entry + file->firstCluster = file->cursorCluster; + } else { + /* + * We also need to update the FAT of the supercluster that used to end the file so that it no longer + * terminates there + */ + opState->fatRewriteStartCluster -= afatfs_fatEntriesPerSector(); + } + + opState->phase = AFATFS_APPEND_SUPERCLUSTER_PHASE_UPDATE_FREEFILE_DIRECTORY; + goto doMore; + break; + case AFATFS_APPEND_SUPERCLUSTER_PHASE_UPDATE_FREEFILE_DIRECTORY: + // First update the freefile's directory entry to remove the first supercluster so we don't risk cross-linking the file + status = afatfs_saveDirectoryEntry(&afatfs.freeFile, AFATFS_SAVE_DIRECTORY_NORMAL); + + if (status == AFATFS_OPERATION_SUCCESS) { + opState->phase = AFATFS_APPEND_SUPERCLUSTER_PHASE_UPDATE_FAT; + goto doMore; + } + break; + case AFATFS_APPEND_SUPERCLUSTER_PHASE_UPDATE_FAT: + status = afatfs_FATFillWithPattern(AFATFS_FAT_PATTERN_TERMINATED_CHAIN, &opState->fatRewriteStartCluster, opState->fatRewriteEndCluster); + + if (status == AFATFS_OPERATION_SUCCESS) { + opState->phase = AFATFS_APPEND_SUPERCLUSTER_PHASE_UPDATE_FILE_DIRECTORY; + goto doMore; + } + break; + case AFATFS_APPEND_SUPERCLUSTER_PHASE_UPDATE_FILE_DIRECTORY: + // Update the fileSize/firstCluster in the directory entry for the file + status = afatfs_saveDirectoryEntry(file, AFATFS_SAVE_DIRECTORY_NORMAL); + break; + } + + if ((status == AFATFS_OPERATION_FAILURE || status == AFATFS_OPERATION_SUCCESS) && file->operation.operation == AFATFS_FILE_OPERATION_APPEND_SUPERCLUSTER) { + file->operation.operation = AFATFS_FILE_OPERATION_NONE; + } + + return status; +} + +/** + * Attempt to queue up an operation to append the first supercluster of the freefile to the given `file` (file's cursor + * must be at end-of-file). + * + * The new cluster number will be set into the file's cursorCluster. + * + * Returns: + * AFATFS_OPERATION_SUCCESS - The append completed successfully and the file's cursorCluster has been updated + * AFATFS_OPERATION_IN_PROGRESS - The operation was queued on the file and will complete later, or there is already an + * append in progress. + * AFATFS_OPERATION_FAILURE - Operation could not be queued (file was busy) or append failed (filesystem is full). + * Check afatfs.fileSystemFull + */ +static afatfsOperationStatus_e afatfs_appendSupercluster(afatfsFilePtr_t file) +{ + uint32_t superClusterSize = afatfs_superClusterSize(); + + if (file->operation.operation == AFATFS_FILE_OPERATION_APPEND_SUPERCLUSTER) { + return AFATFS_OPERATION_IN_PROGRESS; + } + + if (afatfs.freeFile.logicalSize < superClusterSize) { + afatfs.filesystemFull = true; + } + + if (afatfs.filesystemFull || afatfs_fileIsBusy(file)) { + return AFATFS_OPERATION_FAILURE; + } + + afatfsAppendSupercluster_t *opState = &file->operation.state.appendSupercluster; + + file->operation.operation = AFATFS_FILE_OPERATION_APPEND_SUPERCLUSTER; + opState->phase = AFATFS_APPEND_SUPERCLUSTER_PHASE_INIT; + opState->previousCluster = file->cursorPreviousCluster; + + return afatfs_appendSuperclusterContinue(file); +} + +#endif + +/** + * Queue an operation to add a cluster of free space to the end of the file. Must be called when the file's cursor + * is beyond the last allocated cluster. + */ +static afatfsOperationStatus_e afatfs_appendFreeCluster(afatfsFilePtr_t file) +{ + afatfsOperationStatus_e status; + +#ifdef AFATFS_USE_FREEFILE + if ((file->mode & AFATFS_FILE_MODE_CONTIGUOUS) != 0) { + // Steal the first cluster from the beginning of the freefile if we can + status = afatfs_appendSupercluster(file); + } else +#endif + { + status = afatfs_appendRegularFreeCluster(file); + } + + return status; +} + +/** + * Returns true if the file's cursor is sitting beyond the end of the last allocated cluster (i.e. the logical fileSize + * is not checked). + */ +static bool afatfs_isEndOfAllocatedFile(afatfsFilePtr_t file) +{ + if (file->type == AFATFS_FILE_TYPE_FAT16_ROOT_DIRECTORY) { + return file->cursorOffset >= AFATFS_SECTOR_SIZE * afatfs.rootDirectorySectors; + } else { + return file->cursorCluster == 0 || afatfs_FATIsEndOfChainMarker(file->cursorCluster); + } +} + +/** + * Take a lock on the sector at the current file cursor position. + * + * Returns a pointer to the sector buffer if successful, or NULL if at the end of file (check afatfs_isEndOfAllocatedFile()) + * or the sector has not yet been read in from disk. + */ +static uint8_t* afatfs_fileRetainCursorSectorForRead(afatfsFilePtr_t file) +{ + uint8_t *result; + + uint32_t physicalSector = afatfs_fileGetCursorPhysicalSector(file); + + /* If we've already got a locked sector then we can assume that was the same one that's at the cursor (because this + * cache is invalidated when crossing a sector boundary) + */ + if (file->readRetainCacheIndex != -1) { + if (!afatfs_assert(physicalSector == afatfs.cacheDescriptor[file->readRetainCacheIndex].sectorIndex)) { + return NULL; + } + + result = afatfs_cacheSectorGetMemory(file->readRetainCacheIndex); + } else { + if (afatfs_isEndOfAllocatedFile(file)) { + return NULL; + } + + afatfs_assert(physicalSector > 0); // We never read the root sector using files + + afatfsOperationStatus_e status = afatfs_cacheSector( + physicalSector, + &result, + AFATFS_CACHE_READ | AFATFS_CACHE_RETAIN, + 0 + ); + + if (status != AFATFS_OPERATION_SUCCESS) { + // Sector not ready for read + return NULL; + } + + file->readRetainCacheIndex = afatfs_getCacheDescriptorIndexForBuffer(result); + } + + return result; +} + +/** + * Lock the sector at the file's cursor position for write, and return a reference to the memory for that sector. + * + * Returns NULL if the cache was too busy, try again later. + */ +static uint8_t* afatfs_fileLockCursorSectorForWrite(afatfsFilePtr_t file) +{ + afatfsOperationStatus_e status; + uint8_t *result; + uint32_t eraseBlockCount; + + // Do we already have a sector locked in our cache at the cursor position? + if (file->writeLockedCacheIndex != -1) { + uint32_t physicalSector = afatfs_fileGetCursorPhysicalSector(file); + + if (!afatfs_assert(physicalSector == afatfs.cacheDescriptor[file->writeLockedCacheIndex].sectorIndex)) { + return NULL; + } + + result = afatfs_cacheSectorGetMemory(file->writeLockedCacheIndex); + } else { + // Find / allocate a sector and lock it in the cache so we can rely on it sticking around + + // Are we at the start of an empty file or the end of a non-empty file? If so we need to add a cluster + if (afatfs_isEndOfAllocatedFile(file) && afatfs_appendFreeCluster(file) != AFATFS_OPERATION_SUCCESS) { + // The extension of the file is in progress so please call us again later to try again + return NULL; + } + + uint32_t physicalSector = afatfs_fileGetCursorPhysicalSector(file); + uint8_t cacheFlags = AFATFS_CACHE_WRITE | AFATFS_CACHE_LOCK; + uint32_t cursorOffsetInSector = file->cursorOffset % AFATFS_SECTOR_SIZE; + uint32_t offsetOfStartOfSector = file->cursorOffset & ~((uint32_t) AFATFS_SECTOR_SIZE - 1); + uint32_t offsetOfEndOfSector = offsetOfStartOfSector + AFATFS_SECTOR_SIZE; + + /* + * If there is data before the write point in this sector, or there could be data after the write-point + * then we need to have the original contents of the sector in the cache for us to merge into + */ + if ( + cursorOffsetInSector > 0 + || offsetOfEndOfSector < file->logicalSize + ) { + cacheFlags |= AFATFS_CACHE_READ; + } + + // In contiguous append mode, we'll pre-erase the whole supercluster + if ((file->mode & (AFATFS_FILE_MODE_APPEND | AFATFS_FILE_MODE_CONTIGUOUS)) == (AFATFS_FILE_MODE_APPEND | AFATFS_FILE_MODE_CONTIGUOUS)) { + uint32_t cursorOffsetInSupercluster = file->cursorOffset & (afatfs_superClusterSize() - 1); + + eraseBlockCount = afatfs_fatEntriesPerSector() * afatfs.sectorsPerCluster - cursorOffsetInSupercluster / AFATFS_SECTOR_SIZE; + } else { + eraseBlockCount = 0; + } + + status = afatfs_cacheSector( + physicalSector, + &result, + cacheFlags, + eraseBlockCount + ); + + if (status != AFATFS_OPERATION_SUCCESS) { + // Not enough cache available to accept this write / sector not ready for read + return NULL; + } + + file->writeLockedCacheIndex = afatfs_getCacheDescriptorIndexForBuffer(result); + } + + return result; +} + +/** + * Attempt to seek the file pointer by the offset, relative to the current position. + * + * Returns true if the seek was completed, or false if you should try again later by calling this routine again (the + * cursor is not moved and no seek operation is queued on the file for you). + * + * You can only seek forwards by the size of a cluster or less, or backwards to stay within the same cluster. Otherwise + * false will always be returned (calling this routine again will never make progress on the seek). + * + * This amount of seek is special because we will have to wait on at most one read operation, so it's easy to make + * the seek atomic. + */ +static bool afatfs_fseekAtomic(afatfsFilePtr_t file, int32_t offset) +{ + // Seeks within a sector + uint32_t newSectorOffset = offset + file->cursorOffset % AFATFS_SECTOR_SIZE; + + // i.e. newSectorOffset is non-negative and smaller than AFATFS_SECTOR_SIZE, we're staying within the same sector + if (newSectorOffset < AFATFS_SECTOR_SIZE) { + file->cursorOffset += offset; + return true; + } + + // We're seeking outside the sector so unlock it if we were holding it + afatfs_fileUnlockCacheSector(file); + + // FAT16 root directories are made up of contiguous sectors rather than clusters + if (file->type == AFATFS_FILE_TYPE_FAT16_ROOT_DIRECTORY) { + file->cursorOffset += offset; + + return true; + } + + uint32_t clusterSizeBytes = afatfs_clusterSize(); + uint32_t offsetInCluster = afatfs_byteIndexInCluster(file->cursorOffset); + uint32_t newOffsetInCluster = offsetInCluster + offset; + + afatfsOperationStatus_e status; + + if (offset > (int32_t) clusterSizeBytes || offset < -(int32_t) offsetInCluster) { + return false; + } + + // Are we seeking outside the cluster? If so we'll need to find out the next cluster number + if (newOffsetInCluster >= clusterSizeBytes) { + uint32_t nextCluster; + + status = afatfs_fileGetNextCluster(file, file->cursorCluster, &nextCluster); + + if (status == AFATFS_OPERATION_SUCCESS) { + // Seek to the beginning of the next cluster + uint32_t bytesToSeek = clusterSizeBytes - offsetInCluster; + + file->cursorPreviousCluster = file->cursorCluster; + file->cursorCluster = nextCluster; + file->cursorOffset += bytesToSeek; + + offset -= bytesToSeek; + } else { + // Try again later + return false; + } + } + + // If we didn't already hit the end of the file, add any remaining offset needed inside the cluster + if (!afatfs_isEndOfAllocatedFile(file)) { + file->cursorOffset += offset; + } + + return true; +} + +/** + * Returns true if the seek was completed, or false if it is still in progress. + */ +static bool afatfs_fseekInternalContinue(afatfsFile_t *file) +{ + afatfsSeek_t *opState = &file->operation.state.seek; + uint32_t clusterSizeBytes = afatfs_clusterSize(); + uint32_t offsetInCluster = afatfs_byteIndexInCluster(file->cursorOffset); + + afatfsOperationStatus_e status; + + // Keep advancing the cursor cluster forwards to consume seekOffset + while (offsetInCluster + opState->seekOffset >= clusterSizeBytes && !afatfs_isEndOfAllocatedFile(file)) { + uint32_t nextCluster; + + status = afatfs_fileGetNextCluster(file, file->cursorCluster, &nextCluster); + + if (status == AFATFS_OPERATION_SUCCESS) { + // Seek to the beginning of the next cluster + uint32_t bytesToSeek = clusterSizeBytes - offsetInCluster; + + file->cursorPreviousCluster = file->cursorCluster; + file->cursorCluster = nextCluster; + + file->cursorOffset += bytesToSeek; + opState->seekOffset -= bytesToSeek; + offsetInCluster = 0; + } else { + // Try again later + return false; + } + } + + // If we didn't already hit the end of the file, add any remaining offset needed inside the cluster + if (!afatfs_isEndOfAllocatedFile(file)) { + file->cursorOffset += opState->seekOffset; + } + + afatfs_fileUpdateFilesize(file); // TODO do we need this? + + file->operation.operation = AFATFS_FILE_OPERATION_NONE; + + if (opState->callback) { + opState->callback(file); + } + + return true; +} + +/** + * Seek the file pointer forwards by offset bytes. Calls the callback when the seek is complete. + * + * Will happily seek beyond the logical end of the file. + * + * Returns: + * AFATFS_OPERATION_SUCCESS - The seek was completed immediately + * AFATFS_OPERATION_IN_PROGRESS - The seek was queued and will complete later + * AFATFS_OPERATION_FAILURE - The seek could not be queued because the file was busy with another operation, + * try again later. + */ +static afatfsOperationStatus_e afatfs_fseekInternal(afatfsFilePtr_t file, uint32_t offset, afatfsFileCallback_t callback) +{ + // See if we can seek without queuing an operation + if (afatfs_fseekAtomic(file, offset)) { + if (callback) { + callback(file); + } + + return AFATFS_OPERATION_SUCCESS; + } else { + // Our operation must queue + if (afatfs_fileIsBusy(file)) { + return AFATFS_OPERATION_FAILURE; + } + + afatfsSeek_t *opState = &file->operation.state.seek; + + file->operation.operation = AFATFS_FILE_OPERATION_SEEK; + opState->callback = callback; + opState->seekOffset = offset; + + return AFATFS_OPERATION_IN_PROGRESS; + } +} + +/** + * Attempt to seek the file cursor from the given point (`whence`) by the given offset, just like C's fseek(). + * + * AFATFS_SEEK_SET with offset 0 will always return AFATFS_OPERATION_SUCCESS. + * + * Returns: + * AFATFS_OPERATION_SUCCESS - The seek was completed immediately + * AFATFS_OPERATION_IN_PROGRESS - The seek was queued and will complete later. Feel free to attempt read/write + * operations on the file, they'll fail until the seek is complete. + * AFATFS_OPERATION_FAILURE - The seek could not be queued because the file was busy with another operation, + * try again later. + */ +afatfsOperationStatus_e afatfs_fseek(afatfsFilePtr_t file, int32_t offset, afatfsSeek_e whence) +{ + // We need an up-to-date logical filesize so we can clamp seeks to the EOF + afatfs_fileUpdateFilesize(file); + + switch (whence) { + case AFATFS_SEEK_CUR: + if (offset >= 0) { + // Only forwards seeks are supported by this routine: + return afatfs_fseekInternal(file, MIN(file->cursorOffset + offset, file->logicalSize), NULL); + } + + // Convert a backwards relative seek into a SEEK_SET. TODO considerable room for improvement if within the same cluster + offset += file->cursorOffset; + break; + + case AFATFS_SEEK_END: + // Are we already at the right position? + if (file->logicalSize + offset == file->cursorOffset) { + return AFATFS_OPERATION_SUCCESS; + } + + // Convert into a SEEK_SET + offset += file->logicalSize; + break; + + case AFATFS_SEEK_SET: + ; + // Fall through + } + + // Now we have a SEEK_SET with a positive offset. Begin by seeking to the start of the file + afatfs_fileUnlockCacheSector(file); + + file->cursorPreviousCluster = 0; + file->cursorCluster = file->firstCluster; + file->cursorOffset = 0; + + // Then seek forwards by the offset + return afatfs_fseekInternal(file, MIN((uint32_t) offset, file->logicalSize), NULL); +} + +/** + * Get the byte-offset of the file's cursor from the start of the file. + * + * Returns true on success, or false if the file is busy (try again later). + */ +bool afatfs_ftell(afatfsFilePtr_t file, uint32_t *position) +{ + if (afatfs_fileIsBusy(file)) { + return false; + } else { + *position = file->cursorOffset; + return true; + } +} + +/** + * Attempt to advance the directory pointer `finder` to the next entry in the directory. + * + * Returns: + * AFATFS_OPERATION_SUCCESS - A pointer to the next directory entry has been loaded into *dirEntry. If the + * directory was exhausted then *dirEntry will be set to NULL. + * AFATFS_OPERATION_IN_PROGRESS - The disk is busy. The pointer is not advanced, call again later to retry. + */ +afatfsOperationStatus_e afatfs_findNext(afatfsFilePtr_t directory, afatfsFinder_t *finder, fatDirectoryEntry_t **dirEntry) +{ + uint8_t *sector; + + if (finder->entryIndex == AFATFS_FILES_PER_DIRECTORY_SECTOR - 1) { + if (afatfs_fseekAtomic(directory, AFATFS_SECTOR_SIZE)) { + finder->entryIndex = -1; + // Fall through to read the first entry of that new sector + } else { + return AFATFS_OPERATION_IN_PROGRESS; + } + } + + sector = afatfs_fileRetainCursorSectorForRead(directory); + + if (sector) { + finder->entryIndex++; + + *dirEntry = (fatDirectoryEntry_t*) sector + finder->entryIndex; + + finder->sectorNumberPhysical = afatfs_fileGetCursorPhysicalSector(directory); + + return AFATFS_OPERATION_SUCCESS; + } else { + if (afatfs_isEndOfAllocatedFile(directory)) { + *dirEntry = NULL; + + return AFATFS_OPERATION_SUCCESS; + } + + return AFATFS_OPERATION_IN_PROGRESS; + } +} + +/** + * Release resources associated with a find operation. Calling this more than once is harmless. + */ +void afatfs_findLast(afatfsFilePtr_t directory) +{ + afatfs_fileUnlockCacheSector(directory); +} + +/** + * Initialise the finder so that the first call with the directory to findNext() will return the first file in the + * directory. + */ +void afatfs_findFirst(afatfsFilePtr_t directory, afatfsFinder_t *finder) +{ + afatfs_fseek(directory, 0, AFATFS_SEEK_SET); + finder->entryIndex = -1; +} + +static afatfsOperationStatus_e afatfs_extendSubdirectoryContinue(afatfsFile_t *directory) +{ + afatfsExtendSubdirectory_t *opState = &directory->operation.state.extendSubdirectory; + afatfsOperationStatus_e status; + uint8_t *sectorBuffer; + uint32_t clusterNumber, physicalSector; + uint16_t sectorInCluster; + + doMore: + switch (opState->phase) { + case AFATFS_EXTEND_SUBDIRECTORY_PHASE_ADD_FREE_CLUSTER: + status = afatfs_appendRegularFreeClusterContinue(directory); + + if (status == AFATFS_OPERATION_SUCCESS) { + opState->phase = AFATFS_EXTEND_SUBDIRECTORY_PHASE_WRITE_SECTORS; + goto doMore; + } else if (status == AFATFS_OPERATION_FAILURE) { + opState->phase = AFATFS_EXTEND_SUBDIRECTORY_PHASE_FAILURE; + goto doMore; + } + break; + case AFATFS_EXTEND_SUBDIRECTORY_PHASE_WRITE_SECTORS: + // Now, zero out that cluster + afatfs_fileGetCursorClusterAndSector(directory, &clusterNumber, §orInCluster); + physicalSector = afatfs_fileGetCursorPhysicalSector(directory); + + while (1) { + status = afatfs_cacheSector(physicalSector, §orBuffer, AFATFS_CACHE_WRITE, 0); + + if (status != AFATFS_OPERATION_SUCCESS) { + return status; + } + + memset(sectorBuffer, 0, AFATFS_SECTOR_SIZE); + + // If this is the first sector of a non-root directory, create the "." and ".." entries + if (directory->directoryEntryPos.sectorNumberPhysical != 0 && directory->cursorOffset == 0) { + fatDirectoryEntry_t *dirEntries = (fatDirectoryEntry_t *) sectorBuffer; + + memset(dirEntries[0].filename, ' ', sizeof(dirEntries[0].filename)); + dirEntries[0].filename[0] = '.'; + dirEntries[0].firstClusterHigh = directory->firstCluster >> 16; + dirEntries[0].firstClusterLow = directory->firstCluster & 0xFFFF; + dirEntries[0].attrib = FAT_FILE_ATTRIBUTE_DIRECTORY; + + memset(dirEntries[1].filename, ' ', sizeof(dirEntries[1].filename)); + dirEntries[1].filename[0] = '.'; + dirEntries[1].filename[1] = '.'; + dirEntries[1].firstClusterHigh = opState->parentDirectoryCluster >> 16; + dirEntries[1].firstClusterLow = opState->parentDirectoryCluster & 0xFFFF; + dirEntries[1].attrib = FAT_FILE_ATTRIBUTE_DIRECTORY; + } + + if (sectorInCluster < afatfs.sectorsPerCluster - 1) { + // Move to next sector + afatfs_assert(afatfs_fseekAtomic(directory, AFATFS_SECTOR_SIZE)); + sectorInCluster++; + physicalSector++; + } else { + break; + } + } + + // Seek back to the beginning of the cluster + afatfs_assert(afatfs_fseekAtomic(directory, -AFATFS_SECTOR_SIZE * (afatfs.sectorsPerCluster - 1))); + opState->phase = AFATFS_EXTEND_SUBDIRECTORY_PHASE_SUCCESS; + goto doMore; + break; + case AFATFS_EXTEND_SUBDIRECTORY_PHASE_SUCCESS: + directory->operation.operation = AFATFS_FILE_OPERATION_NONE; + + if (opState->callback) { + opState->callback(directory); + } + + return AFATFS_OPERATION_SUCCESS; + break; + case AFATFS_EXTEND_SUBDIRECTORY_PHASE_FAILURE: + directory->operation.operation = AFATFS_FILE_OPERATION_NONE; + + if (opState->callback) { + opState->callback(NULL); + } + return AFATFS_OPERATION_FAILURE; + break; + } + + return AFATFS_OPERATION_IN_PROGRESS; +} + +/** + * Queue an operation to add a cluster to a sub-directory. + * + * Tthe new cluster is zero-filled. "." and ".." entries are added if it is the first cluster of a new subdirectory. + * + * The directory must not be busy, otherwise AFATFS_OPERATION_FAILURE is returned immediately. + * + * The directory's cursor must lie at the end of the directory file (i.e. isEndOfAllocatedFile() would return true). + * + * You must provide parentDirectory if this is the first extension to the subdirectory, otherwise pass NULL for that argument. + */ +static afatfsOperationStatus_e afatfs_extendSubdirectory(afatfsFile_t *directory, afatfsFilePtr_t parentDirectory, afatfsFileCallback_t callback) +{ + // FAT16 root directories cannot be extended + if (directory->type == AFATFS_FILE_TYPE_FAT16_ROOT_DIRECTORY || afatfs_fileIsBusy(directory)) { + return AFATFS_OPERATION_FAILURE; + } + + /* + * We'll assume that we're never asked to append the first cluster of a root directory, since any + * reasonably-formatted volume should have a root! + */ + afatfsExtendSubdirectory_t *opState = &directory->operation.state.extendSubdirectory; + + directory->operation.operation = AFATFS_FILE_OPERATION_EXTEND_SUBDIRECTORY; + + opState->phase = AFATFS_EXTEND_SUBDIRECTORY_PHASE_INITIAL; + opState->parentDirectoryCluster = parentDirectory ? parentDirectory->firstCluster : 0; + opState->callback = callback; + + afatfs_appendRegularFreeClusterInitOperationState(&opState->appendFreeCluster, directory->cursorPreviousCluster); + + return afatfs_extendSubdirectoryContinue(directory); +} + +/** + * Allocate space for a new directory entry to be written, store the position of that entry in the finder, and set + * the *dirEntry pointer to point to the entry within the cached FAT sector. This pointer's lifetime is only as good + * as the life of the cache, so don't dawdle. + * + * Before the first call to this function, call afatfs_findFirst() on the directory. + * + * The directory sector in the cache is marked as dirty, so any changes written through to the entry will be flushed out + * in a subsequent poll cycle. + * + * Returns: + * AFATFS_OPERATION_IN_PROGRESS - Call again later to continue + * AFATFS_OPERATION_SUCCESS - Entry has been inserted and *dirEntry and *finder have been updated + * AFATFS_OPERATION_FAILURE - When the directory is full. + */ +static afatfsOperationStatus_e afatfs_allocateDirectoryEntry(afatfsFilePtr_t directory, fatDirectoryEntry_t **dirEntry, afatfsFinder_t *finder) +{ + afatfsOperationStatus_e result; + + if (afatfs_fileIsBusy(directory)) { + return AFATFS_OPERATION_IN_PROGRESS; + } + + while ((result = afatfs_findNext(directory, finder, dirEntry)) == AFATFS_OPERATION_SUCCESS) { + if (*dirEntry) { + if (fat_isDirectoryEntryEmpty(*dirEntry) || fat_isDirectoryEntryTerminator(*dirEntry)) { + afatfs_cacheSectorMarkDirty(afatfs_getCacheDescriptorForBuffer((uint8_t*) *dirEntry)); + + afatfs_findLast(directory); + return AFATFS_OPERATION_SUCCESS; + } + } else { + // Need to extend directory size by adding a cluster + result = afatfs_extendSubdirectory(directory, NULL, NULL); + + if (result == AFATFS_OPERATION_SUCCESS) { + // Continue the search in the newly-extended directory + continue; + } else { + // The status (in progress or failure) of extending the directory becomes our status + break; + } + } + } + + return result; +} + +/** + * Return a pointer to a free entry in the open files table (a file whose type is "NONE"). You should initialize + * the file afterwards with afatfs_initFileHandle(). + */ +static afatfsFilePtr_t afatfs_allocateFileHandle() +{ + for (int i = 0; i < AFATFS_MAX_OPEN_FILES; i++) { + if (afatfs.openFiles[i].type == AFATFS_FILE_TYPE_NONE) { + return &afatfs.openFiles[i]; + } + } + + return NULL; +} + +/** + * Continue the file truncation. + * + * When truncation finally succeeds or fails, the current operation is cleared on the file (if the current operation + * was a truncate), then the truncate operation's callback is called. This allows the truncation to be called as a + * sub-operation without it clearing the parent file operation. + */ +static afatfsOperationStatus_e afatfs_ftruncateContinue(afatfsFilePtr_t file, bool markDeleted) +{ + afatfsTruncateFile_t *opState = &file->operation.state.truncateFile; + afatfsOperationStatus_e status; + +#ifdef AFATFS_USE_FREEFILE + uint32_t oldFreeFileStart, freeFileGrow; +#endif + + doMore: + + switch (opState->phase) { + case AFATFS_TRUNCATE_FILE_UPDATE_DIRECTORY: + status = afatfs_saveDirectoryEntry(file, markDeleted ? AFATFS_SAVE_DIRECTORY_DELETED : AFATFS_SAVE_DIRECTORY_NORMAL); + + if (status == AFATFS_OPERATION_SUCCESS) { +#ifdef AFATFS_USE_FREEFILE + if (opState->endCluster) { + opState->phase = AFATFS_TRUNCATE_FILE_ERASE_FAT_CHAIN_CONTIGUOUS; + } else +#endif + { + opState->phase = AFATFS_TRUNCATE_FILE_ERASE_FAT_CHAIN_NORMAL; + } + goto doMore; + } + break; +#ifdef AFATFS_USE_FREEFILE + case AFATFS_TRUNCATE_FILE_ERASE_FAT_CHAIN_CONTIGUOUS: + // Prepare the clusters to be added back on to the beginning of the freefile + status = afatfs_FATFillWithPattern(AFATFS_FAT_PATTERN_UNTERMINATED_CHAIN, &opState->currentCluster, opState->endCluster); + + if (status == AFATFS_OPERATION_SUCCESS) { + opState->phase = AFATFS_TRUNCATE_FILE_PREPEND_TO_FREEFILE; + goto doMore; + } + break; + case AFATFS_TRUNCATE_FILE_PREPEND_TO_FREEFILE: + // Note, it's okay to run this code several times: + oldFreeFileStart = afatfs.freeFile.firstCluster; + + afatfs.freeFile.firstCluster = opState->startCluster; + + freeFileGrow = (oldFreeFileStart - opState->startCluster) * afatfs_clusterSize(); + + afatfs.freeFile.logicalSize += freeFileGrow; + afatfs.freeFile.physicalSize += freeFileGrow; + + status = afatfs_saveDirectoryEntry(&afatfs.freeFile, AFATFS_SAVE_DIRECTORY_NORMAL); + if (status == AFATFS_OPERATION_SUCCESS) { + opState->phase = AFATFS_TRUNCATE_FILE_SUCCESS; + goto doMore; + } + break; +#endif + case AFATFS_TRUNCATE_FILE_ERASE_FAT_CHAIN_NORMAL: + while (!afatfs_FATIsEndOfChainMarker(opState->currentCluster)) { + uint32_t nextCluster; + + status = afatfs_FATGetNextCluster(0, opState->currentCluster, &nextCluster); + + if (status != AFATFS_OPERATION_SUCCESS) { + return status; + } + + status = afatfs_FATSetNextCluster(opState->currentCluster, 0); + + if (status != AFATFS_OPERATION_SUCCESS) { + return status; + } + + opState->currentCluster = nextCluster; + + // Searches for unallocated regular clusters should be told about this free cluster now + afatfs.lastClusterAllocated = MIN(afatfs.lastClusterAllocated, opState->currentCluster - 1); + } + + opState->phase = AFATFS_TRUNCATE_FILE_SUCCESS; + goto doMore; + break; + case AFATFS_TRUNCATE_FILE_SUCCESS: + if (file->operation.operation == AFATFS_FILE_OPERATION_TRUNCATE) { + file->operation.operation = AFATFS_FILE_OPERATION_NONE; + } + + if (opState->callback) { + opState->callback(file); + } + + return AFATFS_OPERATION_SUCCESS; + break; + } + + if (status == AFATFS_OPERATION_FAILURE && file->operation.operation == AFATFS_FILE_OPERATION_TRUNCATE) { + file->operation.operation = AFATFS_FILE_OPERATION_NONE; + } + + return status; +} + +/** + * Queue an operation to truncate the file to zero bytes in length. + * + * Returns true if the operation was successfully queued or false if the file is busy (try again later). + * + * The callback is called once the file has been truncated (some time after this routine returns). + */ +bool afatfs_ftruncate(afatfsFilePtr_t file, afatfsFileCallback_t callback) +{ + afatfsTruncateFile_t *opState; + + if (afatfs_fileIsBusy(file)) + return false; + + file->operation.operation = AFATFS_FILE_OPERATION_TRUNCATE; + + opState = &file->operation.state.truncateFile; + opState->callback = callback; + opState->phase = AFATFS_TRUNCATE_FILE_INITIAL; + opState->startCluster = file->firstCluster; + opState->currentCluster = opState->startCluster; + +#ifdef AFATFS_USE_FREEFILE + if ((file->mode & AFATFS_FILE_MODE_CONTIGUOUS) != 0) { + // The file is contiguous and ends where the freefile begins + opState->endCluster = afatfs.freeFile.firstCluster; + } else +#endif + { + // The range of clusters to delete is not contiguous, so follow it as a linked-list instead + opState->endCluster = 0; + } + + // We'll drop the cluster chain from the directory entry immediately + file->firstCluster = 0; + file->logicalSize = 0; + file->physicalSize = 0; + + afatfs_fseek(file, 0, AFATFS_SEEK_SET); + + return true; +} + +/** + * Load details from the given FAT directory entry into the file. + */ +static void afatfs_fileLoadDirectoryEntry(afatfsFile_t *file, fatDirectoryEntry_t *entry) +{ + file->firstCluster = (uint32_t) (entry->firstClusterHigh << 16) | entry->firstClusterLow; + file->logicalSize = entry->fileSize; + file->physicalSize = roundUpTo(entry->fileSize, afatfs_clusterSize()); + file->attrib = entry->attrib; +} + +static void afatfs_createFileContinue(afatfsFile_t *file) +{ + afatfsCreateFile_t *opState = &file->operation.state.createFile; + fatDirectoryEntry_t *entry; + afatfsOperationStatus_e status; + + doMore: + + switch (opState->phase) { + case AFATFS_CREATEFILE_PHASE_INITIAL: + afatfs_findFirst(&afatfs.currentDirectory, &file->directoryEntryPos); + opState->phase = AFATFS_CREATEFILE_PHASE_FIND_FILE; + goto doMore; + break; + case AFATFS_CREATEFILE_PHASE_FIND_FILE: + do { + status = afatfs_findNext(&afatfs.currentDirectory, &file->directoryEntryPos, &entry); + + switch (status) { + case AFATFS_OPERATION_SUCCESS: + // Is this the last entry in the directory? + if (entry == NULL || fat_isDirectoryEntryTerminator(entry)) { + afatfs_findLast(&afatfs.currentDirectory); + + if ((file->mode & AFATFS_FILE_MODE_CREATE) != 0) { + // The file didn't already exist, so we can create it. Allocate a new directory entry + afatfs_findFirst(&afatfs.currentDirectory, &file->directoryEntryPos); + + opState->phase = AFATFS_CREATEFILE_PHASE_CREATE_NEW_FILE; + goto doMore; + } else { + // File not found. + + opState->phase = AFATFS_CREATEFILE_PHASE_FAILURE; + goto doMore; + } + } else if (strncmp(entry->filename, (char*) opState->filename, FAT_FILENAME_LENGTH) == 0) { + // We found a file with this name! + afatfs_fileLoadDirectoryEntry(file, entry); + + afatfs_findLast(&afatfs.currentDirectory); + + opState->phase = AFATFS_CREATEFILE_PHASE_SUCCESS; + goto doMore; + } // Else this entry doesn't match, fall through and continue the search + break; + case AFATFS_OPERATION_FAILURE: + afatfs_findLast(&afatfs.currentDirectory); + opState->phase = AFATFS_CREATEFILE_PHASE_FAILURE; + goto doMore; + break; + case AFATFS_OPERATION_IN_PROGRESS: + ; + } + } while (status == AFATFS_OPERATION_SUCCESS); + break; + case AFATFS_CREATEFILE_PHASE_CREATE_NEW_FILE: + status = afatfs_allocateDirectoryEntry(&afatfs.currentDirectory, &entry, &file->directoryEntryPos); + + if (status == AFATFS_OPERATION_SUCCESS) { + memset(entry, 0, sizeof(*entry)); + + memcpy(entry->filename, opState->filename, FAT_FILENAME_LENGTH); + entry->attrib = file->attrib; + entry->creationDate = AFATFS_DEFAULT_FILE_DATE; + entry->creationTime = AFATFS_DEFAULT_FILE_TIME; + entry->lastWriteDate = AFATFS_DEFAULT_FILE_DATE; + entry->lastWriteTime = AFATFS_DEFAULT_FILE_TIME; + +#ifdef AFATFS_DEBUG_VERBOSE + fprintf(stderr, "Adding directory entry for %.*s to sector %u\n", FAT_FILENAME_LENGTH, opState->filename, file->directoryEntryPos.sectorNumberPhysical); +#endif + + opState->phase = AFATFS_CREATEFILE_PHASE_SUCCESS; + goto doMore; + } else if (status == AFATFS_OPERATION_FAILURE) { + opState->phase = AFATFS_CREATEFILE_PHASE_FAILURE; + goto doMore; + } + break; + case AFATFS_CREATEFILE_PHASE_SUCCESS: + if ((file->mode & AFATFS_FILE_MODE_RETAIN_DIRECTORY) != 0) { + /* + * For this high performance file type, we require the directory entry for the file to be retained + * in the cache at all times. + */ + uint8_t *directorySector; + + status = afatfs_cacheSector( + file->directoryEntryPos.sectorNumberPhysical, + &directorySector, + AFATFS_CACHE_READ | AFATFS_CACHE_RETAIN, + 0 + ); + + if (status != AFATFS_OPERATION_SUCCESS) { + // Retry next time + break; + } + } + + afatfs_fseek(file, 0, AFATFS_SEEK_SET); + + // Is file empty? + if (file->cursorCluster == 0) { +#ifdef AFATFS_USE_FREEFILE + if ((file->mode & AFATFS_FILE_MODE_CONTIGUOUS) != 0) { + if (afatfs_fileIsBusy(&afatfs.freeFile)) { + // Someone else's using the freefile, come back later. + break; + } else { + // Lock the freefile for our exclusive access + afatfs.freeFile.operation.operation = AFATFS_FILE_OPERATION_LOCKED; + } + } +#endif + } else { + // We can't guarantee that the existing file contents are contiguous + file->mode &= ~AFATFS_FILE_MODE_CONTIGUOUS; + + // Seek to the end of the file if it is in append mode + if ((file->mode & AFATFS_FILE_MODE_APPEND) != 0) { + // This replaces our open file operation + file->operation.operation = AFATFS_FILE_OPERATION_NONE; + afatfs_fseekInternal(file, file->logicalSize, opState->callback); + break; + } + + // If we're only writing (not reading) the file must be truncated + if (file->mode == (AFATFS_FILE_MODE_CREATE | AFATFS_FILE_MODE_WRITE)) { + // This replaces our open file operation + file->operation.operation = AFATFS_FILE_OPERATION_NONE; + afatfs_ftruncate(file, opState->callback); + break; + } + } + + file->operation.operation = AFATFS_FILE_OPERATION_NONE; + opState->callback(file); + break; + case AFATFS_CREATEFILE_PHASE_FAILURE: + file->type = AFATFS_FILE_TYPE_NONE; + opState->callback(NULL); + break; + } +} + +/** + * Reset the in-memory data for the given handle back to the zeroed initial state + */ +static void afatfs_initFileHandle(afatfsFilePtr_t file) +{ + memset(file, 0, sizeof(*file)); + file->writeLockedCacheIndex = -1; + file->readRetainCacheIndex = -1; +} + +static void afatfs_funlinkContinue(afatfsFilePtr_t file) +{ + afatfsUnlinkFile_t *opState = &file->operation.state.unlinkFile; + afatfsOperationStatus_e status; + + status = afatfs_ftruncateContinue(file, true); + + if (status == AFATFS_OPERATION_SUCCESS) { + // Once the truncation is completed, we can close the file handle + file->operation.operation = AFATFS_FILE_OPERATION_NONE; + afatfs_fclose(file, opState->callback); + } +} + +/** + * Delete and close the file. + * + * Returns true if the operation was successfully queued (callback will be called some time after this routine returns) + * or false if the file is busy and you should try again later. + */ +bool afatfs_funlink(afatfsFilePtr_t file, afatfsCallback_t callback) +{ + afatfsUnlinkFile_t *opState = &file->operation.state.unlinkFile; + + if (!file || file->type == AFATFS_FILE_TYPE_NONE) { + return true; + } + + /* + * Internally an unlink is implemented by first doing a ftruncate(), marking the directory entry as deleted, + * then doing a fclose() operation. + */ + + // Start the sub-operation of truncating the file + if (!afatfs_ftruncate(file, NULL)) + return false; + + /* + * The unlink operation has its own private callback field so that the truncate suboperation doesn't end up + * calling back early when it completes: + */ + opState->callback = callback; + + file->operation.operation = AFATFS_FILE_OPERATION_UNLINK; + + return true; +} + +/** + * Open (or create) a file in the CWD with the given filename. + * + * file - Memory location to store the newly opened file details + * name - Filename in "name.ext" format. No path. + * attrib - FAT file attributes to give the file (if created) + * fileMode - Bitset of AFATFS_FILE_MODE_* constants. Include AFATFS_FILE_MODE_CREATE to create the file if + * it does not exist. + * callback - Called when the operation is complete + */ +static afatfsFilePtr_t afatfs_createFile(afatfsFilePtr_t file, const char *name, uint8_t attrib, uint8_t fileMode, + afatfsFileCallback_t callback) +{ + afatfsCreateFile_t *opState = &file->operation.state.createFile; + + afatfs_initFileHandle(file); + + // Queue the operation to finish the file creation + file->operation.operation = AFATFS_FILE_OPERATION_CREATE_FILE; + + file->mode = fileMode; + + if (strcmp(name, ".") == 0) { + file->firstCluster = afatfs.currentDirectory.firstCluster; + file->physicalSize = afatfs.currentDirectory.physicalSize; + file->logicalSize = afatfs.currentDirectory.logicalSize; + file->attrib = afatfs.currentDirectory.attrib; + file->type = afatfs.currentDirectory.type; + } else { + fat_convertFilenameToFATStyle(name, opState->filename); + file->attrib = attrib; + + if ((attrib & FAT_FILE_ATTRIBUTE_DIRECTORY) != 0) { + file->type = AFATFS_FILE_TYPE_DIRECTORY; + } else { + file->type = AFATFS_FILE_TYPE_NORMAL; + } + } + + opState->callback = callback; + + if (strcmp(name, ".") == 0) { + // Since we already have the directory entry details, we can skip straight to the final operations requried + opState->phase = AFATFS_CREATEFILE_PHASE_SUCCESS; + } else { + opState->phase = AFATFS_CREATEFILE_PHASE_INITIAL; + } + + afatfs_createFileContinue(file); + + return file; +} + +static void afatfs_fcloseContinue(afatfsFilePtr_t file) +{ + afatfsCacheBlockDescriptor_t *descriptor; + afatfsCloseFile_t *opState = &file->operation.state.closeFile; + + /* + * Directories don't update their parent directory entries over time, because their fileSize field in the directory + * never changes (when we add the first cluster to the directory we save the directory entry at that point and it + * doesn't change afterwards). So don't bother trying to save their directory entries during fclose(). + * + * Also if we only opened the file for read then we didn't change the directory entry either. + */ + if (file->type != AFATFS_FILE_TYPE_DIRECTORY && file->type != AFATFS_FILE_TYPE_FAT16_ROOT_DIRECTORY + && (file->mode & (AFATFS_FILE_MODE_APPEND | AFATFS_FILE_MODE_WRITE)) != 0) { + if (afatfs_saveDirectoryEntry(file, AFATFS_SAVE_DIRECTORY_FOR_CLOSE) != AFATFS_OPERATION_SUCCESS) { + return; + } + } + + // Release our reservation on the directory cache if needed + if ((file->mode & AFATFS_FILE_MODE_RETAIN_DIRECTORY) != 0) { + descriptor = afatfs_findCacheSector(file->directoryEntryPos.sectorNumberPhysical); + + if (descriptor) { + descriptor->retainCount = MAX((int) descriptor->retainCount - 1, 0); + } + } + + // Release locks on the sector at the file cursor position + afatfs_fileUnlockCacheSector(file); + +#ifdef AFATFS_USE_FREEFILE + // Release our exclusive lock on the freefile if needed + if ((file->mode & AFATFS_FILE_MODE_CONTIGUOUS) != 0) { + afatfs_assert(afatfs.freeFile.operation.operation == AFATFS_FILE_OPERATION_LOCKED); + afatfs.freeFile.operation.operation = AFATFS_FILE_OPERATION_NONE; + } +#endif + + file->type = AFATFS_FILE_TYPE_NONE; + file->operation.operation = AFATFS_FILE_OPERATION_NONE; + + if (opState->callback) { + opState->callback(); + } +} + +/** + * Returns true if an operation was successfully queued to close the file and destroy the file handle. If the file is + * currently busy, false is returned and you should retry later. + * + * If provided, the callback will be called after the operation completes (pass NULL for no callback). + * + * If this function returns true, you should not make any further calls to the file (as the handle might be reused for a + * new file). + */ +bool afatfs_fclose(afatfsFilePtr_t file, afatfsCallback_t callback) +{ + if (!file || file->type == AFATFS_FILE_TYPE_NONE) { + return true; + } else if (afatfs_fileIsBusy(file)) { + return false; + } else { + afatfs_fileUpdateFilesize(file); + + file->operation.operation = AFATFS_FILE_OPERATION_CLOSE; + file->operation.state.closeFile.callback = callback; + afatfs_fcloseContinue(file); + return true; + } +} + +/** + * Create a new directory with the given name, or open the directory if it already exists. + * + * The directory will be passed to the callback, or NULL if the creation failed. + * + * Returns true if the directory creation was begun, or false if there are too many open files. + */ +bool afatfs_mkdir(const char *filename, afatfsFileCallback_t callback) +{ + afatfsFilePtr_t file = afatfs_allocateFileHandle(); + + if (file) { + afatfs_createFile(file, filename, FAT_FILE_ATTRIBUTE_DIRECTORY, AFATFS_FILE_MODE_CREATE | AFATFS_FILE_MODE_READ | AFATFS_FILE_MODE_WRITE, callback); + } else if (callback) { + callback(NULL); + } + + return file != NULL; +} + +/** + * Change the working directory to the directory with the given handle (use fopen). Pass NULL for `directory` in order to + * change to the root directory. + * + * Returns true on success, false if you should call again later to retry. After changing into a directory, your handle + * to that directory may be closed by fclose(). + */ +bool afatfs_chdir(afatfsFilePtr_t directory) +{ + if (afatfs_fileIsBusy(&afatfs.currentDirectory)) { + return false; + } + + if (directory) { + if (afatfs_fileIsBusy(directory)) { + return false; + } + + memcpy(&afatfs.currentDirectory, directory, sizeof(*directory)); + return true; + } else { + afatfs_initFileHandle(&afatfs.currentDirectory); + + afatfs.currentDirectory.mode = AFATFS_FILE_MODE_READ | AFATFS_FILE_MODE_WRITE; + + if (afatfs.filesystemType == FAT_FILESYSTEM_TYPE_FAT16) + afatfs.currentDirectory.type = AFATFS_FILE_TYPE_FAT16_ROOT_DIRECTORY; + else + afatfs.currentDirectory.type = AFATFS_FILE_TYPE_DIRECTORY; + + afatfs.currentDirectory.firstCluster = afatfs.rootDirectoryCluster; + afatfs.currentDirectory.attrib = FAT_FILE_ATTRIBUTE_DIRECTORY; + + // Root directories don't have a directory entry to represent themselves: + afatfs.currentDirectory.directoryEntryPos.sectorNumberPhysical = 0; + + afatfs_fseek(&afatfs.currentDirectory, 0, AFATFS_SEEK_SET); + + return true; + } +} + +/** + * Begin the process of opening a file with the given name in the current working directory (paths in the filename are + * not supported) using the given mode. + * + * To open the current working directory, pass "." for filename. + * + * The complete() callback is called when finished with either a file handle (file was opened) or NULL upon failure. + * + * Supported file mode strings: + * + * r - Read from an existing file + * w - Create a file for write access, if the file already exists then truncate it + * a - Create a file for write access to the end of the file only, if the file already exists then append to it + * + * r+ - Read and write from an existing file + * w+ - Read and write from an existing file, if the file doesn't already exist it is created + * a+ - Read from or append to an existing file, if the file doesn't already exist it is created TODO + * + * as - Create a new file which is stored contiguously on disk (high performance mode/freefile) for append or write + * ws If the file is already non-empty or freefile support is not compiled in then it will fall back to non-contiguous + * operation. + * + * All other mode strings are illegal. In particular, don't add "b" to the end of the mode string. + * + * Returns false if the the open failed really early (out of file handles). + */ +bool afatfs_fopen(const char *filename, const char *mode, afatfsFileCallback_t complete) +{ + uint8_t fileMode = 0; + afatfsFilePtr_t file; + + switch (mode[0]) { + case 'r': + fileMode = AFATFS_FILE_MODE_READ; + break; + case 'w': + fileMode = AFATFS_FILE_MODE_WRITE | AFATFS_FILE_MODE_CREATE; + break; + case 'a': + fileMode = AFATFS_FILE_MODE_APPEND | AFATFS_FILE_MODE_CREATE; + break; + } + + switch (mode[1]) { + case '+': + fileMode |= AFATFS_FILE_MODE_READ; + + if (fileMode == AFATFS_FILE_MODE_READ) { + fileMode |= AFATFS_FILE_MODE_WRITE; + } + break; + case 's': +#ifdef AFATFS_USE_FREEFILE + fileMode |= AFATFS_FILE_MODE_CONTIGUOUS | AFATFS_FILE_MODE_RETAIN_DIRECTORY; +#endif + break; + } + + file = afatfs_allocateFileHandle(); + + if (file) { + afatfs_createFile(file, filename, FAT_FILE_ATTRIBUTE_ARCHIVE, fileMode, complete); + } else if (complete) { + complete(NULL); + } + + return file != NULL; +} + +/** + * Write a single character to the file at the current cursor position. If the cache is too busy to accept the write, + * it is silently dropped. + */ +void afatfs_fputc(afatfsFilePtr_t file, uint8_t c) +{ + uint32_t cursorOffsetInSector = file->cursorOffset % AFATFS_SECTOR_SIZE; + + int cacheIndex = file->writeLockedCacheIndex; + + /* If we've already locked the current sector in the cache, and we won't be completing the sector, we won't + * be caching/uncaching/seeking, so we can just run this simpler fast case. + */ + if (cacheIndex != -1 && cursorOffsetInSector != AFATFS_SECTOR_SIZE - 1) { + afatfs_cacheSectorGetMemory(cacheIndex)[cursorOffsetInSector] = c; + file->cursorOffset++; + } else { + // Slow path + afatfs_fwrite(file, &c, sizeof(c)); + } +} + +/** + * Attempt to write `len` bytes from `buffer` into the `file`. + * + * Returns the number of bytes actually written. + * + * 0 will be returned when: + * The filesystem is busy (try again later) + * + * Fewer bytes will be written than requested when: + * The write spanned a sector boundary and the next sector's contents/location was not yet available in the cache. + * Or you tried to extend the length of the file but the filesystem is full (check afatfs_isFull()). + */ +uint32_t afatfs_fwrite(afatfsFilePtr_t file, const uint8_t *buffer, uint32_t len) +{ + if ((file->mode & (AFATFS_FILE_MODE_APPEND | AFATFS_FILE_MODE_WRITE)) == 0) { + return 0; + } + + if (afatfs_fileIsBusy(file)) { + // There might be a seek pending + return 0; + } + + uint32_t cursorOffsetInSector = file->cursorOffset % AFATFS_SECTOR_SIZE; + uint32_t writtenBytes = 0; + + while (len > 0) { + uint32_t bytesToWriteThisSector = MIN(AFATFS_SECTOR_SIZE - cursorOffsetInSector, len); + uint8_t *sectorBuffer; + + sectorBuffer = afatfs_fileLockCursorSectorForWrite(file); + if (!sectorBuffer) { + // Cache is currently busy + break; + } + + memcpy(sectorBuffer + cursorOffsetInSector, buffer, bytesToWriteThisSector); + + writtenBytes += bytesToWriteThisSector; + + /* + * If the seek doesn't complete immediately then we'll break and wait for that seek to complete by waiting for + * the file to be non-busy on entry again. + * + * A seek operation should always be able to queue on the file since we have checked that the file wasn't busy + * on entry (fseek will never return AFATFS_OPERATION_FAILURE). + * + * If the seek has to queue, when the seek completes, it'll update the fileSize for us to contain the cursor. + */ + if (afatfs_fseekInternal(file, bytesToWriteThisSector, NULL) == AFATFS_OPERATION_IN_PROGRESS) { + break; + } + +#ifdef AFATFS_USE_FREEFILE + if ((file->mode & AFATFS_FILE_MODE_CONTIGUOUS) != 0) { + afatfs_assert(file->cursorCluster < afatfs.freeFile.firstCluster); + } +#endif + + len -= bytesToWriteThisSector; + buffer += bytesToWriteThisSector; + cursorOffsetInSector = 0; + } + + return writtenBytes; +} + +/** + * Attempt to read `len` bytes from `file` into the `buffer`. + * + * Returns the number of bytes actually read. + * + * 0 will be returned when: + * The filesystem is busy (try again later) + * EOF was reached (check afatfs_isEof()) + * + * Fewer bytes than requested will be read when: + * The read spans a AFATFS_SECTOR_SIZE boundary and the following sector was not available in the cache yet. + */ +uint32_t afatfs_fread(afatfsFilePtr_t file, uint8_t *buffer, uint32_t len) +{ + if ((file->mode & AFATFS_FILE_MODE_READ) == 0) { + return 0; + } + + if (afatfs_fileIsBusy(file)) { + // There might be a seek pending + return 0; + } + + /* + * If we've just previously fwritten() to extend the file, the logical filesize will be out of date and the cursor + * will appear to be beyond the end of the file (but actually it's precisely at the end of the file, because if + * we had seeked backwards to where we could read something with fseek(), we would have updated the filesize). + */ + if (file->cursorOffset >= file->logicalSize) + return 0; + + len = MIN(file->logicalSize - file->cursorOffset, len); + + uint32_t readBytes = 0; + uint32_t cursorOffsetInSector = file->cursorOffset % AFATFS_SECTOR_SIZE; + + while (len > 0) { + uint32_t bytesToReadThisSector = MIN(AFATFS_SECTOR_SIZE - cursorOffsetInSector, len); + uint8_t *sectorBuffer; + + sectorBuffer = afatfs_fileRetainCursorSectorForRead(file); + if (!sectorBuffer) { + // Cache is currently busy + return readBytes; + } + + memcpy(buffer, sectorBuffer + cursorOffsetInSector, bytesToReadThisSector); + + readBytes += bytesToReadThisSector; + + /* + * If the seek doesn't complete immediately then we'll break and wait for that seek to complete by waiting for + * the file to be non-busy on entry again. + * + * A seek operation should always be able to queue on the file since we have checked that the file wasn't busy + * on entry (fseek will never return AFATFS_OPERATION_FAILURE). + */ + if (afatfs_fseekInternal(file, bytesToReadThisSector, NULL) == AFATFS_OPERATION_IN_PROGRESS) { + break; + } + + len -= bytesToReadThisSector; + buffer += bytesToReadThisSector; + cursorOffsetInSector = 0; + } + + return readBytes; +} + +/** + * Returns true if the file's pointer position currently lies at the end-of-file point (i.e. one byte beyond the last + * byte in the file). + */ +bool afatfs_feof(afatfsFilePtr_t file) +{ + return file->cursorOffset >= file->logicalSize; +} + +/** + * Continue any queued operations on the given file. + */ +static void afatfs_fileOperationContinue(afatfsFile_t *file) +{ + if (file->type == AFATFS_FILE_TYPE_NONE) + return; + + switch (file->operation.operation) { + case AFATFS_FILE_OPERATION_CREATE_FILE: + afatfs_createFileContinue(file); + break; + case AFATFS_FILE_OPERATION_SEEK: + afatfs_fseekInternalContinue(file); + break; + case AFATFS_FILE_OPERATION_CLOSE: + afatfs_fcloseContinue(file); + break; + case AFATFS_FILE_OPERATION_UNLINK: + afatfs_funlinkContinue(file); + break; + case AFATFS_FILE_OPERATION_TRUNCATE: + afatfs_ftruncateContinue(file, false); + break; +#ifdef AFATFS_USE_FREEFILE + case AFATFS_FILE_OPERATION_APPEND_SUPERCLUSTER: + afatfs_appendSuperclusterContinue(file); + break; + case AFATFS_FILE_OPERATION_LOCKED: + ; + break; +#endif + case AFATFS_FILE_OPERATION_APPEND_FREE_CLUSTER: + afatfs_appendRegularFreeClusterContinue(file); + break; + case AFATFS_FILE_OPERATION_EXTEND_SUBDIRECTORY: + afatfs_extendSubdirectoryContinue(file); + break; + case AFATFS_FILE_OPERATION_NONE: + ; + break; + } +} + +/** + * Check files for pending operations and execute them. + */ +static void afatfs_fileOperationsPoll() +{ + afatfs_fileOperationContinue(&afatfs.currentDirectory); + +#ifdef AFATFS_USE_INTROSPECTIVE_LOGGING + afatfs_fileOperationContinue(&afatfs.introSpecLog); +#endif + + for (int i = 0; i < AFATFS_MAX_OPEN_FILES; i++) { + afatfs_fileOperationContinue(&afatfs.openFiles[i]); + } +} + +#ifdef AFATFS_USE_FREEFILE + +/** + * Return the available size of the freefile (used for files in contiguous append mode) + */ +uint32_t afatfs_getContiguousFreeSpace() +{ + return afatfs.freeFile.logicalSize; +} + +/** + * Call to set up the initial state for finding the largest block of free space on the device whose corresponding FAT + * sectors are themselves entirely free space (so the free space has dedicated FAT sectors of its own). + */ +static void afatfs_findLargestContiguousFreeBlockBegin() +{ + // The first FAT sector has two reserved entries, so it isn't eligible for this search. Start at the next FAT sector. + afatfs.initState.freeSpaceSearch.candidateStart = afatfs_fatEntriesPerSector(); + afatfs.initState.freeSpaceSearch.candidateEnd = afatfs.initState.freeSpaceSearch.candidateStart; + afatfs.initState.freeSpaceSearch.bestGapStart = 0; + afatfs.initState.freeSpaceSearch.bestGapLength = 0; + afatfs.initState.freeSpaceSearch.phase = AFATFS_FREE_SPACE_SEARCH_PHASE_FIND_HOLE; +} + +/** + * Call to continue the search for the largest contiguous block of free space on the device. + * + * Returns: + * AFATFS_OPERATION_IN_PROGRESS - SD card is busy, call again later to resume + * AFATFS_OPERATION_SUCCESS - When the search has finished and afatfs.initState.freeSpaceSearch has been updated with the details of the best gap. + * AFATFS_OPERATION_FAILURE - When a read error occured + */ +static afatfsOperationStatus_e afatfs_findLargestContiguousFreeBlockContinue() +{ + afatfsFreeSpaceSearch_t *opState = &afatfs.initState.freeSpaceSearch; + uint32_t fatEntriesPerSector = afatfs_fatEntriesPerSector(); + uint32_t candidateGapLength, searchLimit; + afatfsFindClusterStatus_e searchStatus; + + while (1) { + switch (opState->phase) { + case AFATFS_FREE_SPACE_SEARCH_PHASE_FIND_HOLE: + // Find the first free cluster + switch (afatfs_findClusterWithCondition(CLUSTER_SEARCH_FREE_AT_BEGINNING_OF_FAT_SECTOR, &opState->candidateStart, afatfs.numClusters + FAT_SMALLEST_LEGAL_CLUSTER_NUMBER)) { + case AFATFS_FIND_CLUSTER_FOUND: + opState->candidateEnd = opState->candidateStart + 1; + opState->phase = AFATFS_FREE_SPACE_SEARCH_PHASE_GROW_HOLE; + break; + + case AFATFS_FIND_CLUSTER_FATAL: + // Some sort of read error occured + return AFATFS_OPERATION_FAILURE; + + case AFATFS_FIND_CLUSTER_NOT_FOUND: + // We finished searching the volume (didn't find any more holes to examine) + return AFATFS_OPERATION_SUCCESS; + + case AFATFS_FIND_CLUSTER_IN_PROGRESS: + return AFATFS_OPERATION_IN_PROGRESS; + } + break; + case AFATFS_FREE_SPACE_SEARCH_PHASE_GROW_HOLE: + // Find the first used cluster after the beginning of the hole (that signals the end of the hole) + + // Don't search beyond the end of the volume, or such that the freefile size would exceed the max filesize + searchLimit = MIN((uint64_t) opState->candidateStart + FAT_MAXIMUM_FILESIZE / afatfs_clusterSize(), afatfs.numClusters + FAT_SMALLEST_LEGAL_CLUSTER_NUMBER); + + searchStatus = afatfs_findClusterWithCondition(CLUSTER_SEARCH_OCCUPIED, &opState->candidateEnd, searchLimit); + + switch (searchStatus) { + case AFATFS_FIND_CLUSTER_NOT_FOUND: + case AFATFS_FIND_CLUSTER_FOUND: + // Either we found a used sector, or the search reached the end of the volume or exceeded the max filesize + candidateGapLength = opState->candidateEnd - opState->candidateStart; + + if (candidateGapLength > opState->bestGapLength) { + opState->bestGapStart = opState->candidateStart; + opState->bestGapLength = candidateGapLength; + } + + if (searchStatus == AFATFS_FIND_CLUSTER_NOT_FOUND) { + // This is the best hole there can be + return AFATFS_OPERATION_SUCCESS; + } else { + // Start a new search for a new hole + opState->candidateStart = roundUpTo(opState->candidateEnd + 1, fatEntriesPerSector); + opState->phase = AFATFS_FREE_SPACE_SEARCH_PHASE_FIND_HOLE; + } + break; + + case AFATFS_FIND_CLUSTER_FATAL: + // Some sort of read error occured + return AFATFS_OPERATION_FAILURE; + + case AFATFS_FIND_CLUSTER_IN_PROGRESS: + return AFATFS_OPERATION_IN_PROGRESS; + } + break; + } + } +} + +static void afatfs_freeFileCreated(afatfsFile_t *file) +{ + if (file) { + // Did the freefile already have allocated space? + if (file->logicalSize > 0) { + // We've completed freefile init, move on to the next init phase + afatfs.initPhase = AFATFS_INITIALIZATION_FREEFILE_LAST + 1; + } else { + // Allocate clusters for the freefile + afatfs_findLargestContiguousFreeBlockBegin(); + afatfs.initPhase = AFATFS_INITIALIZATION_FREEFILE_FAT_SEARCH; + } + } else { + // Failed to allocate an entry + afatfs.lastError = AFATFS_ERROR_GENERIC; + afatfs.filesystemState = AFATFS_FILESYSTEM_STATE_FATAL; + } +} + +#endif + +#ifdef AFATFS_USE_INTROSPECTIVE_LOGGING + +static void afatfs_introspecLogCreated(afatfsFile_t *file) +{ + if (file) { + afatfs.initPhase++; + } else { + afatfs.lastError = AFATFS_ERROR_GENERIC; + afatfs.filesystemState = AFATFS_FILESYSTEM_STATE_FATAL; + } +} + +#endif + +static void afatfs_initContinue() +{ +#ifdef AFATFS_USE_FREEFILE + afatfsOperationStatus_e status; +#endif + + uint8_t *sector; + + doMore: + + switch (afatfs.initPhase) { + case AFATFS_INITIALIZATION_READ_MBR: + if (afatfs_cacheSector(0, §or, AFATFS_CACHE_READ | AFATFS_CACHE_DISCARDABLE, 0) == AFATFS_OPERATION_SUCCESS) { + if (afatfs_parseMBR(sector)) { + afatfs.initPhase = AFATFS_INITIALIZATION_READ_VOLUME_ID; + goto doMore; + } else { + afatfs.lastError = AFATFS_ERROR_BAD_MBR; + afatfs.filesystemState = AFATFS_FILESYSTEM_STATE_FATAL; + } + } + break; + case AFATFS_INITIALIZATION_READ_VOLUME_ID: + if (afatfs_cacheSector(afatfs.partitionStartSector, §or, AFATFS_CACHE_READ | AFATFS_CACHE_DISCARDABLE, 0) == AFATFS_OPERATION_SUCCESS) { + if (afatfs_parseVolumeID(sector)) { + // Open the root directory + afatfs_chdir(NULL); + + afatfs.initPhase++; + } else { + afatfs.lastError = AFATFS_ERROR_BAD_FILESYSTEM_HEADER; + afatfs.filesystemState = AFATFS_FILESYSTEM_STATE_FATAL; + } + } + break; + +#ifdef AFATFS_USE_FREEFILE + case AFATFS_INITIALIZATION_FREEFILE_CREATE: + afatfs.initPhase = AFATFS_INITIALIZATION_FREEFILE_CREATING; + + afatfs_createFile(&afatfs.freeFile, AFATFS_FREESPACE_FILENAME, FAT_FILE_ATTRIBUTE_SYSTEM | FAT_FILE_ATTRIBUTE_READ_ONLY, + AFATFS_FILE_MODE_CREATE | AFATFS_FILE_MODE_RETAIN_DIRECTORY, afatfs_freeFileCreated); + break; + case AFATFS_INITIALIZATION_FREEFILE_CREATING: + afatfs_fileOperationContinue(&afatfs.freeFile); + break; + case AFATFS_INITIALIZATION_FREEFILE_FAT_SEARCH: + if (afatfs_findLargestContiguousFreeBlockContinue() == AFATFS_OPERATION_SUCCESS) { + // If the freefile ends up being empty then we only have to save its directory entry: + afatfs.initPhase = AFATFS_INITIALIZATION_FREEFILE_SAVE_DIR_ENTRY; + + if (afatfs.initState.freeSpaceSearch.bestGapLength > AFATFS_FREEFILE_LEAVE_CLUSTERS + 1) { + afatfs.initState.freeSpaceSearch.bestGapLength -= AFATFS_FREEFILE_LEAVE_CLUSTERS; + + /* So that the freefile never becomes empty, we want it to occupy a non-integer number of + * superclusters. So its size mod the number of clusters in a supercluster should be 1. + */ + afatfs.initState.freeSpaceSearch.bestGapLength = ((afatfs.initState.freeSpaceSearch.bestGapLength - 1) & ~(afatfs_fatEntriesPerSector() - 1)) + 1; + + // Anything useful left over? + if (afatfs.initState.freeSpaceSearch.bestGapLength > afatfs_fatEntriesPerSector()) { + uint32_t startCluster = afatfs.initState.freeSpaceSearch.bestGapStart; + // Points 1-beyond the final cluster of the freefile: + uint32_t endCluster = afatfs.initState.freeSpaceSearch.bestGapStart + afatfs.initState.freeSpaceSearch.bestGapLength; + + afatfs_assert(endCluster < afatfs.numClusters + FAT_SMALLEST_LEGAL_CLUSTER_NUMBER); + + afatfs.initState.freeSpaceFAT.startCluster = startCluster; + afatfs.initState.freeSpaceFAT.endCluster = endCluster; + + afatfs.freeFile.firstCluster = startCluster; + + afatfs.freeFile.logicalSize = afatfs.initState.freeSpaceSearch.bestGapLength * afatfs_clusterSize(); + afatfs.freeFile.physicalSize = afatfs.freeFile.logicalSize; + + // We can write the FAT table for the freefile now + afatfs.initPhase = AFATFS_INITIALIZATION_FREEFILE_UPDATE_FAT; + } // Else the freefile's FAT chain and filesize remains the default (empty) + } + + goto doMore; + } + break; + case AFATFS_INITIALIZATION_FREEFILE_UPDATE_FAT: + status = afatfs_FATFillWithPattern(AFATFS_FAT_PATTERN_TERMINATED_CHAIN, &afatfs.initState.freeSpaceFAT.startCluster, afatfs.initState.freeSpaceFAT.endCluster); + + if (status == AFATFS_OPERATION_SUCCESS) { + afatfs.initPhase = AFATFS_INITIALIZATION_FREEFILE_SAVE_DIR_ENTRY; + + goto doMore; + } else if (status == AFATFS_OPERATION_FAILURE) { + afatfs.lastError = AFATFS_ERROR_GENERIC; + afatfs.filesystemState = AFATFS_FILESYSTEM_STATE_FATAL; + } + break; + case AFATFS_INITIALIZATION_FREEFILE_SAVE_DIR_ENTRY: + status = afatfs_saveDirectoryEntry(&afatfs.freeFile, AFATFS_SAVE_DIRECTORY_NORMAL); + + if (status == AFATFS_OPERATION_SUCCESS) { + afatfs.initPhase++; + goto doMore; + } else if (status == AFATFS_OPERATION_FAILURE) { + afatfs.lastError = AFATFS_ERROR_GENERIC; + afatfs.filesystemState = AFATFS_FILESYSTEM_STATE_FATAL; + } + break; +#endif + +#ifdef AFATFS_USE_INTROSPECTIVE_LOGGING + case AFATFS_INITIALIZATION_INTROSPEC_LOG_CREATE: + afatfs.initPhase = AFATFS_INITIALIZATION_INTROSPEC_LOG_CREATING; + + afatfs_createFile(&afatfs.introSpecLog, AFATFS_INTROSPEC_LOG_FILENAME, FAT_FILE_ATTRIBUTE_ARCHIVE, + AFATFS_FILE_MODE_CREATE | AFATFS_FILE_MODE_APPEND, afatfs_introspecLogCreated); + break; + case AFATFS_INITIALIZATION_INTROSPEC_LOG_CREATING: + afatfs_fileOperationContinue(&afatfs.introSpecLog); + break; +#endif + + case AFATFS_INITIALIZATION_DONE: + afatfs.filesystemState = AFATFS_FILESYSTEM_STATE_READY; + break; + } +} + +/** + * Check to see if there are any pending operations on the filesystem and perform a little work (without waiting on the + * sdcard). You must call this periodically. + */ +void afatfs_poll() +{ + // Only attempt to continue FS operations if the card is present & ready, otherwise we would just be wasting time + if (sdcard_poll()) { + afatfs_flush(); + + switch (afatfs.filesystemState) { + case AFATFS_FILESYSTEM_STATE_INITIALIZATION: + afatfs_initContinue(); + break; + case AFATFS_FILESYSTEM_STATE_READY: + afatfs_fileOperationsPoll(); + break; + default: + ; + } + } +} + +#ifdef AFATFS_USE_INTROSPECTIVE_LOGGING + +void afatfs_sdcardProfilerCallback(sdcardBlockOperation_e operation, uint32_t blockIndex, uint32_t duration) +{ + // Make sure the log file has actually been opened before we try to log to it: + if (afatfs.introSpecLog.type == AFATFS_FILE_TYPE_NONE) { + return; + } + + enum { + LOG_ENTRY_SIZE = 16 // Log entry size should be a power of two to avoid partial fwrites() + }; + + uint8_t buffer[LOG_ENTRY_SIZE]; + + buffer[0] = operation; + + // Padding/reserved: + buffer[1] = 0; + buffer[2] = 0; + buffer[3] = 0; + + buffer[4] = blockIndex & 0xFF; + buffer[5] = (blockIndex >> 8) & 0xFF; + buffer[6] = (blockIndex >> 16) & 0xFF; + buffer[7] = (blockIndex >> 24) & 0xFF; + + buffer[8] = duration & 0xFF; + buffer[9] = (duration >> 8) & 0xFF; + buffer[10] = (duration >> 16) & 0xFF; + buffer[11] = (duration >> 24) & 0xFF; + + // Padding/reserved: + buffer[12] = 0; + buffer[13] = 0; + buffer[14] = 0; + buffer[15] = 0; + + // Ignore write failures + afatfs_fwrite(&afatfs.introSpecLog, buffer, LOG_ENTRY_SIZE); +} + +#endif + +afatfsFilesystemState_e afatfs_getFilesystemState() +{ + return afatfs.filesystemState; +} + +afatfsError_e afatfs_getLastError() +{ + return afatfs.lastError; +} + +void afatfs_init() +{ + afatfs.filesystemState = AFATFS_FILESYSTEM_STATE_INITIALIZATION; + afatfs.initPhase = AFATFS_INITIALIZATION_READ_MBR; + afatfs.lastClusterAllocated = FAT_SMALLEST_LEGAL_CLUSTER_NUMBER; + +#ifdef AFATFS_USE_INTROSPECTIVE_LOGGING + sdcard_setProfilerCallback(afatfs_sdcardProfilerCallback); +#endif +} + +/** + * Shut down the filesystem, flushing all data to the disk. Keep calling until it returns true. + * + * dirty - Set to true to skip the flush operation and terminate immediately (buffered data will be lost!) + */ +bool afatfs_destroy(bool dirty) +{ + // Only attempt detailed cleanup if the filesystem is in reasonable looking state + if (!dirty && afatfs.filesystemState == AFATFS_FILESYSTEM_STATE_READY) { + int openFileCount = 0; + + for (int i = 0; i < AFATFS_MAX_OPEN_FILES; i++) { + if (afatfs.openFiles[i].type != AFATFS_FILE_TYPE_NONE) { + afatfs_fclose(&afatfs.openFiles[i], NULL); + // The close operation might not finish right away, so count this file as still open for now + openFileCount++; + } + } + +#ifdef AFATFS_USE_INTROSPECTIVE_LOGGING + if (afatfs.introSpecLog.type != AFATFS_FILE_TYPE_NONE) { + afatfs_fclose(&afatfs.introSpecLog, NULL); + openFileCount++; + } +#endif + +#ifdef AFATFS_USE_FREEFILE + if (afatfs.freeFile.type != AFATFS_FILE_TYPE_NONE) { + afatfs_fclose(&afatfs.freeFile, NULL); + openFileCount++; + } +#endif + + if (afatfs.currentDirectory.type != AFATFS_FILE_TYPE_NONE) { + afatfs_fclose(&afatfs.currentDirectory, NULL); + openFileCount++; + } + + afatfs_poll(); + + if (!afatfs_flush()) { + return false; + } + + if (afatfs.cacheFlushInProgress) { + return false; + } + + if (openFileCount > 0) { + return false; + } + +#ifdef AFATFS_DEBUG + /* All sector locks should have been released by closing the files, so the subsequent flush should have written + * all dirty pages to disk. If not, something's wrong: + */ + for (int i = 0; i < AFATFS_NUM_CACHE_SECTORS; i++) { + afatfs_assert(afatfs.cacheDescriptor[i].state != AFATFS_CACHE_STATE_DIRTY); + } +#endif + } + + // Clear the afatfs so it's as if we never ran + memset(&afatfs, 0, sizeof(afatfs)); + + return true; +} + +/** + * Get a pessimistic estimate of the amount of buffer space that we have available to write to immediately. + */ +uint32_t afatfs_getFreeBufferSpace() +{ + uint32_t result = 0; + for (int i = 0; i < AFATFS_NUM_CACHE_SECTORS; i++) { + if (!afatfs.cacheDescriptor[i].locked && (afatfs.cacheDescriptor[i].state == AFATFS_CACHE_STATE_EMPTY || afatfs.cacheDescriptor[i].state == AFATFS_CACHE_STATE_IN_SYNC)) { + result += AFATFS_SECTOR_SIZE; + } + } + return result; +} diff --git a/src/main/io/asyncfatfs/asyncfatfs.h b/src/main/io/asyncfatfs/asyncfatfs.h new file mode 100644 index 000000000..fd515694b --- /dev/null +++ b/src/main/io/asyncfatfs/asyncfatfs.h @@ -0,0 +1,92 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#include +#include + +#include "fat_standard.h" + +typedef struct afatfsFile_t *afatfsFilePtr_t; + +typedef enum { + AFATFS_FILESYSTEM_STATE_UNKNOWN, + AFATFS_FILESYSTEM_STATE_FATAL, + AFATFS_FILESYSTEM_STATE_INITIALIZATION, + AFATFS_FILESYSTEM_STATE_READY, +} afatfsFilesystemState_e; + +typedef enum { + AFATFS_OPERATION_IN_PROGRESS, + AFATFS_OPERATION_SUCCESS, + AFATFS_OPERATION_FAILURE, +} afatfsOperationStatus_e; + +typedef enum { + AFATFS_ERROR_NONE = 0, + AFATFS_ERROR_GENERIC = 1, + AFATFS_ERROR_BAD_MBR = 2, + AFATFS_ERROR_BAD_FILESYSTEM_HEADER = 3 +} afatfsError_e; + +typedef struct afatfsDirEntryPointer_t { + uint32_t sectorNumberPhysical; + int16_t entryIndex; +} afatfsDirEntryPointer_t; + +typedef afatfsDirEntryPointer_t afatfsFinder_t; + +typedef enum { + AFATFS_SEEK_SET, + AFATFS_SEEK_CUR, + AFATFS_SEEK_END, +} afatfsSeek_e; + +typedef void (*afatfsFileCallback_t)(afatfsFilePtr_t file); +typedef void (*afatfsCallback_t)(); + +bool afatfs_fopen(const char *filename, const char *mode, afatfsFileCallback_t complete); +bool afatfs_ftruncate(afatfsFilePtr_t file, afatfsFileCallback_t callback); +bool afatfs_fclose(afatfsFilePtr_t file, afatfsCallback_t callback); +bool afatfs_funlink(afatfsFilePtr_t file, afatfsCallback_t callback); + +bool afatfs_feof(afatfsFilePtr_t file); +void afatfs_fputc(afatfsFilePtr_t file, uint8_t c); +uint32_t afatfs_fwrite(afatfsFilePtr_t file, const uint8_t *buffer, uint32_t len); +uint32_t afatfs_fread(afatfsFilePtr_t file, uint8_t *buffer, uint32_t len); +afatfsOperationStatus_e afatfs_fseek(afatfsFilePtr_t file, int32_t offset, afatfsSeek_e whence); +bool afatfs_ftell(afatfsFilePtr_t file, uint32_t *position); + +bool afatfs_mkdir(const char *filename, afatfsFileCallback_t complete); +bool afatfs_chdir(afatfsFilePtr_t dirHandle); + +void afatfs_findFirst(afatfsFilePtr_t directory, afatfsFinder_t *finder); +afatfsOperationStatus_e afatfs_findNext(afatfsFilePtr_t directory, afatfsFinder_t *finder, fatDirectoryEntry_t **dirEntry); +void afatfs_findLast(afatfsFilePtr_t directory); + +bool afatfs_flush(); +void afatfs_init(); +bool afatfs_destroy(bool dirty); +void afatfs_poll(); + +uint32_t afatfs_getFreeBufferSpace(); +uint32_t afatfs_getContiguousFreeSpace(); +bool afatfs_isFull(); + +afatfsFilesystemState_e afatfs_getFilesystemState(); +afatfsError_e afatfs_getLastError(); diff --git a/src/main/io/asyncfatfs/fat_standard.c b/src/main/io/asyncfatfs/fat_standard.c new file mode 100644 index 000000000..c91d136c3 --- /dev/null +++ b/src/main/io/asyncfatfs/fat_standard.c @@ -0,0 +1,89 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include + +#include "fat_standard.h" + +bool fat16_isEndOfChainMarker(uint16_t clusterNumber) +{ + return clusterNumber >= 0xFFF8; +} + +// Pass the cluster number after fat32_decodeClusterNumber(). +bool fat32_isEndOfChainMarker(uint32_t clusterNumber) +{ + return clusterNumber >= 0x0FFFFFF8; +} + +/** + * FAT32 cluster numbers are really only 28 bits, and the top 4 bits must be left alone and not treated as part of the + * cluster number (so various FAT drivers can use those bits for their own purposes, or they can be used in later + * extensions) + */ +uint32_t fat32_decodeClusterNumber(uint32_t clusterNumber) +{ + return clusterNumber & 0x0FFFFFFF; +} + +// fat32 needs fat32_decodeClusterNumber() applied first. +bool fat_isFreeSpace(uint32_t clusterNumber) +{ + return clusterNumber == 0; +} + +bool fat_isDirectoryEntryTerminator(fatDirectoryEntry_t *entry) +{ + return entry->filename[0] == 0x00; +} + +bool fat_isDirectoryEntryEmpty(fatDirectoryEntry_t *entry) +{ + return (unsigned char) entry->filename[0] == FAT_DELETED_FILE_MARKER; +} + +/** + * Convert the given "prefix.ext" style filename to the FAT format to be stored on disk. + * + * fatFilename must point to a buffer which is FAT_FILENAME_LENGTH bytes long. The buffer is not null-terminated. + */ +void fat_convertFilenameToFATStyle(const char *filename, uint8_t *fatFilename) +{ + for (int i = 0; i < 8; i++) { + if (*filename == '\0' || *filename == '.') { + *fatFilename = ' '; + } else { + *fatFilename = toupper((unsigned char)*filename); + filename++; + } + fatFilename++; + } + + if (*filename == '.') { + filename++; + } + + for (int i = 0; i < 3; i++) { + if (*filename == '\0') { + *fatFilename = ' '; + } else { + *fatFilename = toupper((unsigned char)*filename); + filename++; + } + fatFilename++; + } +} diff --git a/src/main/io/asyncfatfs/fat_standard.h b/src/main/io/asyncfatfs/fat_standard.h new file mode 100644 index 000000000..590bc43a5 --- /dev/null +++ b/src/main/io/asyncfatfs/fat_standard.h @@ -0,0 +1,140 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#include +#include + +#define MBR_PARTITION_TYPE_FAT16 0x06 +#define MBR_PARTITION_TYPE_FAT32 0x0B +#define MBR_PARTITION_TYPE_FAT32_LBA 0x0C +#define MBR_PARTITION_TYPE_FAT16_LBA 0x0E + +// Signature bytes found at index 510 and 511 in the volume ID sector +#define FAT_VOLUME_ID_SIGNATURE_1 0x55 +#define FAT_VOLUME_ID_SIGNATURE_2 0xAA + +#define FAT_DIRECTORY_ENTRY_SIZE 32 +#define FAT_SMALLEST_LEGAL_CLUSTER_NUMBER 2 + +#define FAT_MAXIMUM_FILESIZE 0xFFFFFFFF + +#define FAT12_MAX_CLUSTERS 4084 +#define FAT16_MAX_CLUSTERS 65524 + +#define FAT_FILE_ATTRIBUTE_READ_ONLY 0x01 +#define FAT_FILE_ATTRIBUTE_HIDDEN 0x02 +#define FAT_FILE_ATTRIBUTE_SYSTEM 0x04 +#define FAT_FILE_ATTRIBUTE_VOLUME_ID 0x08 +#define FAT_FILE_ATTRIBUTE_DIRECTORY 0x10 +#define FAT_FILE_ATTRIBUTE_ARCHIVE 0x20 + +#define FAT_FILENAME_LENGTH 11 +#define FAT_DELETED_FILE_MARKER 0xE5 + +#define FAT_MAKE_DATE(year, month, day) (day | (month << 5) | ((year - 1980) << 9)) +#define FAT_MAKE_TIME(hour, minute, second) ((second / 2) | (minute << 5) | (hour << 11)) + +typedef enum { + FAT_FILESYSTEM_TYPE_INVALID, + FAT_FILESYSTEM_TYPE_FAT12, + FAT_FILESYSTEM_TYPE_FAT16, + FAT_FILESYSTEM_TYPE_FAT32, +} fatFilesystemType_e; + +typedef struct mbrPartitionEntry_t { + uint8_t bootFlag; + uint8_t chsBegin[3]; + uint8_t type; + uint8_t chsEnd[3]; + uint32_t lbaBegin; + uint32_t numSectors; +} __attribute__((packed)) mbrPartitionEntry_t; + +typedef struct fat16Descriptor_t { + uint8_t driveNumber; + uint8_t reserved1; + uint8_t bootSignature; + uint32_t volumeID; + char volumeLabel[11]; + char fileSystemType[8]; +} __attribute__((packed)) fat16Descriptor_t; + +typedef struct fat32Descriptor_t { + uint32_t FATSize32; + uint16_t extFlags; + uint16_t fsVer; + uint32_t rootCluster; + uint16_t fsInfo; + uint16_t backupBootSector; + uint8_t reserved[12]; + uint8_t driveNumber; + uint8_t reserved1; + uint8_t bootSignature; + uint32_t volumeID; + char volumeLabel[11]; + char fileSystemType[8]; +} __attribute__((packed)) fat32Descriptor_t; + +typedef struct fatVolumeID_t { + uint8_t jmpBoot[3]; + char oemName[8]; + uint16_t bytesPerSector; + uint8_t sectorsPerCluster; + uint16_t reservedSectorCount; + uint8_t numFATs; + uint16_t rootEntryCount; + uint16_t totalSectors16; + uint8_t media; + uint16_t FATSize16; + uint16_t sectorsPerTrack; + uint16_t numHeads; + uint32_t hiddenSectors; + uint32_t totalSectors32; + union { + fat16Descriptor_t fat16; + fat32Descriptor_t fat32; + } fatDescriptor; +} __attribute__((packed)) fatVolumeID_t; + +typedef struct fatDirectoryEntry_t { + char filename[FAT_FILENAME_LENGTH]; + uint8_t attrib; + uint8_t ntReserved; + uint8_t creationTimeTenths; + uint16_t creationTime; + uint16_t creationDate; + uint16_t lastAccessDate; + uint16_t firstClusterHigh; + uint16_t lastWriteTime; + uint16_t lastWriteDate; + uint16_t firstClusterLow; + uint32_t fileSize; +} __attribute__((packed)) fatDirectoryEntry_t; + +uint32_t fat32_decodeClusterNumber(uint32_t clusterNumber); + +bool fat32_isEndOfChainMarker(uint32_t clusterNumber); +bool fat16_isEndOfChainMarker(uint16_t clusterNumber); + +bool fat_isFreeSpace(uint32_t clusterNumber); + +bool fat_isDirectoryEntryTerminator(fatDirectoryEntry_t *entry); +bool fat_isDirectoryEntryEmpty(fatDirectoryEntry_t *entry); + +void fat_convertFilenameToFATStyle(const char *filename, uint8_t *fatFilename); diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index 138ba415b..fabc9ed53 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -29,6 +29,7 @@ #include "drivers/serial.h" #include "drivers/compass.h" #include "drivers/timer.h" +#include "drivers/pwm_mapping.h" #include "drivers/pwm_rx.h" #include "drivers/accgyro.h" #include "drivers/light_led.h" diff --git a/src/main/io/i2c_bst.c b/src/main/io/i2c_bst.c index b1ce130ff..3d51fef45 100644 --- a/src/main/io/i2c_bst.c +++ b/src/main/io/i2c_bst.c @@ -65,7 +65,7 @@ #include "config/config_master.h" #include "version.h" -#ifdef NAZE +#ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" #endif diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 5f298ee73..86589c037 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -32,7 +32,7 @@ #include "drivers/serial_softserial.h" #endif -#if defined(USE_USART1) || defined(USE_USART2) || defined(USE_USART3) || defined(USE_USART6) +#if defined(USE_USART1) || defined(USE_USART2) || defined(USE_USART3) || defined(USE_USART4) || defined(USE_USART5) || defined(USE_USART6) #include "drivers/serial_uart.h" #endif @@ -66,6 +66,12 @@ const serialPortIdentifier_e serialPortIdentifiers[SERIAL_PORT_COUNT] = { #ifdef USE_USART3 SERIAL_PORT_USART3, #endif +#ifdef USE_USART4 + SERIAL_PORT_USART4, +#endif +#ifdef USE_USART5 + SERIAL_PORT_USART5, +#endif #ifdef USE_USART6 SERIAL_PORT_USART6, #endif @@ -293,6 +299,16 @@ serialPort_t *openSerialPort( serialPort = uartOpen(USART3, callback, baudRate, mode, options); break; #endif +#ifdef USE_USART4 + case SERIAL_PORT_USART4: + serialPort = uartOpen(UART4, callback, baudRate, mode, options); + break; +#endif +#ifdef USE_USART5 + case SERIAL_PORT_USART5: + serialPort = uartOpen(UART5, callback, baudRate, mode, options); + break; +#endif #ifdef USE_USART6 case SERIAL_PORT_USART6: serialPort = uartOpen(USART6, callback, baudRate, mode, options); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 862722e62..cd62fb6e3 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -45,7 +45,9 @@ #include "drivers/bus_i2c.h" #include "drivers/gpio.h" #include "drivers/timer.h" +#include "drivers/pwm_mapping.h" #include "drivers/pwm_rx.h" +#include "drivers/sdcard.h" #include "drivers/buf_writer.h" @@ -57,6 +59,7 @@ #include "io/ledstrip.h" #include "io/flashfs.h" #include "io/beeper.h" +#include "io/asyncfatfs/asyncfatfs.h" #include "rx/rx.h" #include "rx/spektrum.h" @@ -157,6 +160,10 @@ static void cliFlashRead(char *cmdline); #endif #endif +#ifdef USE_SDCARD +static void cliSdInfo(char *cmdline); +#endif + // buffer static char cliBuffer[48]; static uint32_t bufferIndex = 0; @@ -178,8 +185,8 @@ static const char * const featureNames[] = { "RX_PPM", "VBAT", "INFLIGHT_ACC_CAL", "RX_SERIAL", "MOTOR_STOP", "SERVO_TILT", "SOFTSERIAL", "GPS", "FAILSAFE", "SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM", - "RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "ONESHOT125", - "BLACKBOX", "CHANNEL_FORWARDING", "MULTISHOT", "USE_PWM_RATE", + "RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "ONESHOT", + "BLACKBOX", "CHANNEL_FORWARDING", "RESERVED_MULTISHOT", "RESERVED_USE_PWM_RATE", "RESERVED", "TX_STYLE_EXPO", "SBUS_INVERTER", NULL }; @@ -278,7 +285,7 @@ const clicmd_t cmdTable[] = { "[]\r\n", cliPlaySound), CLI_COMMAND_DEF("profile", "change profile", "[]", cliProfile), - CLI_COMMAND_DEF("rateprofile", "change rate profile", "[]", cliRateProfile), + CLI_COMMAND_DEF("rateprofile", "change rate profile", "[]", cliRateProfile), CLI_COMMAND_DEF("rxrange", "configure rx channel ranges", NULL, cliRxRange), CLI_COMMAND_DEF("rxfail", "show/set rx failsafe settings", NULL, cliRxFail), CLI_COMMAND_DEF("save", "save and reboot", NULL, cliSave), @@ -294,6 +301,9 @@ const clicmd_t cmdTable[] = { "\treset\r\n" "\tload \r\n" "\treverse r|n", cliServoMix), +#endif +#ifdef USE_SDCARD + CLI_COMMAND_DEF("sd_info", "sdcard info", NULL, cliSdInfo), #endif CLI_COMMAND_DEF("status", "show status", NULL, cliStatus), #ifndef SKIP_TASK_STATISTICS @@ -343,7 +353,7 @@ static const char * const lookupTableGimbalMode[] = { }; static const char * const lookupTableBlackboxDevice[] = { - "SERIAL", "SPIFLASH" + "SERIAL", "SPIFLASH", "SDCARD" }; @@ -388,7 +398,7 @@ static const char * const lookupTableAccHardware[] = { }; static const char * const lookupTableBaroHardware[] = { - "AUTO", + "AUTO", "NONE", "BMP085", "MS5611", @@ -429,6 +439,14 @@ static const char * const lookupTableRFLoopCtrl[] = { "M8" }; +static const char * const lookupTablePwmProtocol[] = { + "PWM", + "125", + "42", + "MULTI", + "BRUSHED" +}; + typedef struct lookupTableEntry_s { const char * const *values; const uint8_t valueCount; @@ -450,6 +468,7 @@ typedef enum { TABLE_PID_CONTROLLER, TABLE_SERIAL_RX, TABLE_RF_LOOP_CTRL, + TABLE_PWM_PROTOCOL, TABLE_GYRO_LPF, TABLE_ACC_HARDWARE, TABLE_BARO_HARDWARE, @@ -474,6 +493,7 @@ static const lookupTableEntry_t lookupTables[] = { { lookupTablePidController, sizeof(lookupTablePidController) / sizeof(char *) }, { lookupTableSerialRX, sizeof(lookupTableSerialRX) / sizeof(char *) }, { lookupTableRFLoopCtrl, sizeof(lookupTableRFLoopCtrl) / sizeof(char *) }, + { lookupTablePwmProtocol, sizeof(lookupTablePwmProtocol) / sizeof(char *) }, { lookupTableGyroLpf, sizeof(lookupTableGyroLpf) / sizeof(char *) }, { lookupTableAccHardware, sizeof(lookupTableAccHardware) / sizeof(char *) }, { lookupTableBaroHardware, sizeof(lookupTableBaroHardware) / sizeof(char *) }, @@ -532,7 +552,9 @@ typedef struct { } clivalue_t; const clivalue_t valueTable[] = { - { "mid_rc", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.midrc, .config.minmax = { 1200, 1700 } }, +// { "emf_avoidance", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.emf_avoidance, .config.lookup = { TABLE_OFF_ON } }, + + { "mid_rc", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.midrc, .config.minmax = { 1200, 1700 } }, { "min_check", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.mincheck, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "max_check", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.maxcheck, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "rssi_channel", VAR_INT8 | MASTER_VALUE, &masterConfig.rxConfig.rssi_channel, .config.minmax = { 0, MAX_SUPPORTED_RC_CHANNEL_COUNT } }, @@ -541,6 +563,8 @@ const clivalue_t valueTable[] = { { "input_filtering_mode", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.inputFilteringMode, .config.lookup = { TABLE_OFF_ON } }, { "rc_smoothing", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.rcSmoothing, .config.lookup = { TABLE_OFF_ON } }, { "roll_yaw_cam_mix_degrees", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.fpvCamAngleDegrees, .config.minmax = { 0, 50 } }, + { "max_aux_channels", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.max_aux_channel, .config.minmax = { 0, 13 } }, + { "debug_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.debug_mode, .config.lookup = { TABLE_DEBUG } }, { "min_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.minthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "max_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.maxthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, @@ -552,11 +576,11 @@ const clivalue_t valueTable[] = { { "3d_neutral", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.neutral3d, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "3d_deadband_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.deadband3d_throttle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, - { "enable_fast_pwm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.use_fast_pwm, .config.lookup = { TABLE_OFF_ON } }, #ifdef CC3D { "enable_buzzer_p6", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.use_buzzer_p6, .config.lookup = { TABLE_OFF_ON } }, #endif { "motor_pwm_rate", VAR_UINT16 | MASTER_VALUE, &masterConfig.motor_pwm_rate, .config.minmax = { 50, 32000 } }, + { "motor_pwm_protocol", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.motor_pwm_protocol, .config.lookup = { TABLE_PWM_PROTOCOL } }, { "servo_pwm_rate", VAR_UINT16 | MASTER_VALUE, &masterConfig.servo_pwm_rate, .config.minmax = { 50, 498 } }, { "disarm_kill_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.disarm_kill_switch, .config.lookup = { TABLE_OFF_ON } }, @@ -675,10 +699,10 @@ const clivalue_t valueTable[] = { { "failsafe_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_throttle, .config.minmax = { PWM_RANGE_MIN, PWM_RANGE_MAX } }, { "failsafe_kill_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.failsafeConfig.failsafe_kill_switch, .config.lookup = { TABLE_OFF_ON } }, { "failsafe_throttle_low_delay",VAR_UINT16 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_throttle_low_delay, .config.minmax = { 0, 300 } }, + { "failsafe_procedure", VAR_UINT8 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_procedure, .config.minmax = { 0, 1 } }, { "rx_min_usec", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.rx_min_usec, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX } }, { "rx_max_usec", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.rx_max_usec, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX } }, - { "max_aux_channel", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.max_aux_channel, .config.minmax = { 0, 99 } }, #ifdef USE_SERVOS { "gimbal_mode", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].gimbalConfig.mode, .config.lookup = { TABLE_GIMBAL_MODE } }, @@ -693,14 +717,18 @@ const clivalue_t valueTable[] = { { "acc_trim_pitch", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].accelerometerTrims.values.pitch, .config.minmax = { -300, 300 } }, { "acc_trim_roll", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].accelerometerTrims.values.roll, .config.minmax = { -300, 300 } }, +#ifdef BARO { "baro_tab_size", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].barometerConfig.baro_sample_count, .config.minmax = { 0, BARO_SAMPLE_COUNT_MAX } }, { "baro_noise_lpf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].barometerConfig.baro_noise_lpf, .config.minmax = { 0 , 1 } }, { "baro_cf_vel", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].barometerConfig.baro_cf_vel, .config.minmax = { 0 , 1 } }, { "baro_cf_alt", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].barometerConfig.baro_cf_alt, .config.minmax = { 0 , 1 } }, { "baro_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.baro_hardware, .config.lookup = { TABLE_BARO_HARDWARE } }, +#endif +#ifdef MAG { "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } }, { "mag_declination", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].mag_declination, .config.minmax = { -18000, 18000 } }, +#endif { "pid_controller", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidController, .config.lookup = { TABLE_PID_CONTROLLER } }, @@ -1527,6 +1555,77 @@ static void cliServoMix(char *cmdline) } #endif +#ifdef USE_SDCARD + +static void cliWriteBytes(const uint8_t *buffer, int count) +{ + while (count > 0) { + cliWrite(*buffer); + buffer++; + count--; + } +} + +static void cliSdInfo(char *cmdline) { + UNUSED(cmdline); + + cliPrint("SD card: "); + + if (!sdcard_isInserted()) { + cliPrint("None inserted\r\n"); + return; + } + + if (!sdcard_isInitialized()) { + cliPrint("Startup failed\r\n"); + return; + } + + const sdcardMetadata_t *metadata = sdcard_getMetadata(); + + cliPrintf("Manufacturer 0x%x, %ukB, %02d/%04d, v%d.%d, '", + metadata->manufacturerID, + metadata->numBlocks / 2, /* One block is half a kB */ + metadata->productionMonth, + metadata->productionYear, + metadata->productRevisionMajor, + metadata->productRevisionMinor + ); + + cliWriteBytes((uint8_t*)metadata->productName, sizeof(metadata->productName)); + + cliPrint("'\r\n" "Filesystem: "); + + switch (afatfs_getFilesystemState()) { + case AFATFS_FILESYSTEM_STATE_READY: + cliPrint("Ready"); + break; + case AFATFS_FILESYSTEM_STATE_INITIALIZATION: + cliPrint("Initializing"); + break; + case AFATFS_FILESYSTEM_STATE_UNKNOWN: + case AFATFS_FILESYSTEM_STATE_FATAL: + cliPrint("Fatal"); + + switch (afatfs_getLastError()) { + case AFATFS_ERROR_BAD_MBR: + cliPrint(" - no FAT MBR partitions"); + break; + case AFATFS_ERROR_BAD_FILESYSTEM_HEADER: + cliPrint(" - bad FAT header"); + break; + case AFATFS_ERROR_GENERIC: + case AFATFS_ERROR_NONE: + ; // Nothing more detailed to print + break; + } + + cliPrint("\r\n"); + break; + } +} + +#endif #ifdef USE_FLASHFS @@ -1722,6 +1821,7 @@ static void cliDump(char *cmdline) cliPrintf("%s\r\n", ftoa(yaw, buf)); } +#ifdef USE_SERVOS // print custom servo mixer if exists cliPrintf("smix reset\r\n"); @@ -1742,6 +1842,7 @@ static void cliDump(char *cmdline) ); } +#endif #endif cliPrint("\r\n\r\n# feature\r\n"); diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 329327448..fd3089a2d 100755 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -23,8 +23,8 @@ #include "build_config.h" #include "debug.h" - #include "platform.h" +#include "scheduler.h" #include "common/axis.h" #include "common/color.h" @@ -40,8 +40,10 @@ #include "drivers/bus_i2c.h" #include "drivers/gpio.h" #include "drivers/timer.h" +#include "drivers/pwm_mapping.h" #include "drivers/pwm_rx.h" #include "drivers/gyro_sync.h" +#include "drivers/sdcard.h" #include "drivers/buf_writer.h" #include "rx/rx.h" #include "rx/msp.h" @@ -54,6 +56,7 @@ #include "io/serial.h" #include "io/ledstrip.h" #include "io/flashfs.h" +#include "io/asyncfatfs/asyncfatfs.h" #include "telemetry/telemetry.h" @@ -73,6 +76,8 @@ #include "flight/navigation.h" #include "flight/altitudehold.h" +#include "blackbox/blackbox.h" + #include "mw.h" #include "config/runtime_config.h" @@ -95,9 +100,12 @@ static serialPort_t *mspSerialPort; extern uint16_t cycleTime; // FIXME dependency on mw.c extern uint16_t rssi; // FIXME dependency on mw.c +extern void resetPidProfile(pidProfile_t *pidProfile); void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse); +const char * const flightControllerIdentifier = RACEFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller. + typedef struct box_e { const uint8_t boxId; // see boxId_e const char *boxName; // GUI-readable box name @@ -165,10 +173,13 @@ static const char pidnames[] = "VEL;"; typedef enum { - UNUSED_PORT = 0, - FOR_GENERAL_MSP, - FOR_TELEMETRY -} mspPortUsage_e; + MSP_SDCARD_STATE_NOT_PRESENT = 0, + MSP_SDCARD_STATE_FATAL = 1, + MSP_SDCARD_STATE_CARD_INIT = 2, + MSP_SDCARD_STATE_FS_INIT = 3, + MSP_SDCARD_STATE_READY = 4 +} mspSDCardState_e; + STATIC_UNIT_TESTED mspPort_t mspPorts[MAX_MSP_PORT_COUNT]; @@ -312,17 +323,67 @@ static void serializeBoxNamesReply(void) } } +static void serializeSDCardSummaryReply(void) +{ + headSerialReply(3 + 4 + 4); + +#ifdef USE_SDCARD + uint8_t flags = 1 /* SD card supported */ ; + uint8_t state; + + serialize8(flags); + + // Merge the card and filesystem states together + if (!sdcard_isInserted()) { + state = MSP_SDCARD_STATE_NOT_PRESENT; + } else if (!sdcard_isFunctional()) { + state = MSP_SDCARD_STATE_FATAL; + } else { + switch (afatfs_getFilesystemState()) { + case AFATFS_FILESYSTEM_STATE_READY: + state = MSP_SDCARD_STATE_READY; + break; + case AFATFS_FILESYSTEM_STATE_INITIALIZATION: + if (sdcard_isInitialized()) { + state = MSP_SDCARD_STATE_FS_INIT; + } else { + state = MSP_SDCARD_STATE_CARD_INIT; + } + break; + case AFATFS_FILESYSTEM_STATE_FATAL: + case AFATFS_FILESYSTEM_STATE_UNKNOWN: + state = MSP_SDCARD_STATE_FATAL; + break; + } + } + + serialize8(state); + serialize8(afatfs_getLastError()); + // Write free space and total space in kilobytes + serialize32(afatfs_getContiguousFreeSpace() / 1024); + serialize32(sdcard_getMetadata()->numBlocks / 2); // Block size is half a kilobyte +#else + serialize8(0); + serialize8(0); + serialize8(0); + serialize32(0); + serialize32(0); +#endif +} + static void serializeDataflashSummaryReply(void) { headSerialReply(1 + 3 * 4); #ifdef USE_FLASHFS const flashGeometry_t *geometry = flashfsGetGeometry(); - serialize8(flashfsIsReady() ? 1 : 0); + uint8_t flags = (flashfsIsReady() ? 1 : 0) | 2 /* FlashFS is supported */; + + serialize8(flags); serialize32(geometry->sectors); serialize32(geometry->totalSize); serialize32(flashfsGetOffset()); // Effectively the current number of bytes stored on the volume #else - serialize8(0); + serialize8(0); // FlashFS is neither ready nor supported serialize32(0); serialize32(0); serialize32(0); @@ -411,9 +472,11 @@ void mspInit(serialConfig_t *serialConfig) activeBoxIds[activeBoxIdCount++] = BOXHORIZON; } +#ifdef BARO if (sensors(SENSOR_BARO)) { activeBoxIds[activeBoxIdCount++] = BOXBARO; } +#endif activeBoxIds[activeBoxIdCount++] = BOXAIRMODE; activeBoxIds[activeBoxIdCount++] = BOXACROPLUS; @@ -437,7 +500,7 @@ void mspInit(serialConfig_t *serialConfig) } #endif - if (masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_AIRPLANE) + if (masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE) activeBoxIds[activeBoxIdCount++] = BOXPASSTHRU; activeBoxIds[activeBoxIdCount++] = BOXBEEPERON; @@ -453,8 +516,10 @@ void mspInit(serialConfig_t *serialConfig) activeBoxIds[activeBoxIdCount++] = BOXOSD; +#ifdef TELEMETRY if (feature(FEATURE_TELEMETRY) && masterConfig.telemetryConfig.telemetry_switch) activeBoxIds[activeBoxIdCount++] = BOXTELEMETRY; +#endif if (feature(FEATURE_SONAR)){ activeBoxIds[activeBoxIdCount++] = BOXSONAR; @@ -522,7 +587,7 @@ static uint32_t packFlightModeFlags(void) IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << BOXAIRMODE| IS_ENABLED(IS_RC_MODE_ACTIVE(BOXACROPLUS)) << BOXACROPLUS| - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXALWAYSSTABILIZED)) << BOXALWAYSSTABILIZED| + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXALWAYSSTABILIZED)) << BOXALWAYSSTABILIZED| IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTEST1)) << BOXTEST1| IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTEST2)) << BOXTEST2; @@ -615,6 +680,20 @@ static bool processOutCommand(uint8_t cmdMSP) serialize32(CAP_DYNBALANCE); // "capability" break; + case MSP_STATUS_EX: + headSerialReply(14); + serialize16(cycleTime); +#ifdef USE_I2C + serialize16(i2cGetErrorCounter()); +#else + serialize16(0); +#endif + serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4); + serialize32(packFlightModeFlags()); + serialize8(masterConfig.current_profile_index); + serialize16(averageSystemLoadPercent); + break; + case MSP_STATUS: headSerialReply(11); serialize16(cycleTime); @@ -737,7 +816,7 @@ static bool processOutCommand(uint8_t cmdMSP) serialize8(masterConfig.profile[0].rcControlsConfig.yaw_deadband); serialize8(currentProfile->pidProfile.gyro_lpf_hz); serialize8(currentProfile->pidProfile.dterm_lpf_hz); - //serialize8(currentProfile->pidProfile.yaw_pterm_cut_hz); +// serialize8(currentProfile->pidProfile.yaw_pterm_cut_hz); break; case MSP_PID: headSerialReply(3 * PID_ITEM_COUNT); @@ -875,6 +954,12 @@ static bool processOutCommand(uint8_t cmdMSP) serialize8(masterConfig.mag_hardware); break; + case MSP_MOTOR_PWM: + headSerialReply(3); + serialize16(masterConfig.motor_pwm_rate); + serialize8(masterConfig.motor_pwm_protocol); + break; + case MSP_MOTOR_PINS: // FIXME This is hardcoded and should not be. headSerialReply(8); @@ -996,10 +1081,13 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_FAILSAFE_CONFIG: - headSerialReply(4); + headSerialReply(8); serialize8(masterConfig.failsafeConfig.failsafe_delay); serialize8(masterConfig.failsafeConfig.failsafe_off_delay); serialize16(masterConfig.failsafeConfig.failsafe_throttle); + serialize8(masterConfig.failsafeConfig.failsafe_kill_switch); + serialize16(masterConfig.failsafeConfig.failsafe_throttle_low_delay); + serialize8(masterConfig.failsafeConfig.failsafe_procedure); break; case MSP_RXFAIL_CONFIG: @@ -1094,6 +1182,41 @@ static bool processOutCommand(uint8_t cmdMSP) break; #endif + case MSP_BLACKBOX_CONFIG: + headSerialReply(4); + +#ifdef BLACKBOX + serialize8(1); //Blackbox supported + serialize8(masterConfig.blackbox_device); + serialize8(masterConfig.blackbox_rate_num); + serialize8(masterConfig.blackbox_rate_denom); +#else + serialize8(0); // Blackbox not supported + serialize8(0); + serialize8(0); + serialize8(0); +#endif + break; + + case MSP_SDCARD_SUMMARY: + serializeSDCardSummaryReply(); + break; + + case MSP_TRANSPONDER_CONFIG: +#ifdef TRANSPONDER + headSerialReply(1 + sizeof(masterConfig.transponderData)); + + serialize8(1); //Transponder supported + + for (i = 0; i < sizeof(masterConfig.transponderData); i++) { + serialize8(masterConfig.transponderData[i]); + } +#else + headSerialReply(1); + serialize8(0); // Transponder not supported +#endif + break; + case MSP_BF_BUILD_INFO: headSerialReply(11 + 4 + 4); for (i = 0; i < 11; i++) @@ -1102,6 +1225,28 @@ static bool processOutCommand(uint8_t cmdMSP) serialize32(0); // future exp break; + case MSP_3D: + headSerialReply(2 * 4); + serialize16(masterConfig.flight3DConfig.deadband3d_low); + serialize16(masterConfig.flight3DConfig.deadband3d_high); + serialize16(masterConfig.flight3DConfig.neutral3d); + serialize16(masterConfig.flight3DConfig.deadband3d_throttle); + break; + + case MSP_RC_DEADBAND: + headSerialReply(3); + serialize8(currentProfile->rcControlsConfig.deadband); + serialize8(currentProfile->rcControlsConfig.yaw_deadband); + serialize8(currentProfile->rcControlsConfig.alt_hold_deadband); + break; + + case MSP_SENSOR_ALIGNMENT: + headSerialReply(3); + serialize8(masterConfig.sensorAlignmentConfig.gyro_align); + serialize8(masterConfig.sensorAlignmentConfig.acc_align); + serialize8(masterConfig.sensorAlignmentConfig.mag_align); + break; + default: return false; } @@ -1157,7 +1302,6 @@ static bool processInCommand(void) masterConfig.disarm_kill_switch = read8(); break; case MSP_SET_LOOP_TIME: - targetLooptime = read16(); break; case MSP_SET_PID_CONTROLLER: currentProfile->pidProfile.pidController = constrain(read8(), 1, 2); // Temporary configurator compatibility @@ -1249,6 +1393,30 @@ static bool processInCommand(void) break; case MSP_SET_RC_TUNING: + if (currentPort->dataSize >= 10) { + currentControlRateProfile->rcRate8 = read8(); + currentControlRateProfile->rcExpo8 = read8(); + for (i = 0; i < 3; i++) { + rate = read8(); + currentControlRateProfile->rates[i] = MIN(rate, i == YAW ? CONTROL_RATE_CONFIG_YAW_RATE_MAX : CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); + } + rate = read8(); + currentControlRateProfile->dynThrPID = MIN(rate, CONTROL_RATE_CONFIG_TPA_MAX); + currentControlRateProfile->thrMid8 = read8(); + currentControlRateProfile->thrExpo8 = read8(); + currentControlRateProfile->tpa_breakpoint = read16(); + if (currentPort->dataSize >= 11) { + currentControlRateProfile->rcYawExpo8 = read8(); + } + } else { + headSerialError(0); + } + break; + + +/* + case MSP_SET_RC_TUNING: + break; if (currentPort->dataSize >= 10) { currentControlRateProfile->rcRate8 = read8(); currentControlRateProfile->rcExpo8 = read8(); @@ -1270,13 +1438,15 @@ static bool processInCommand(void) masterConfig.profile[0].rcControlsConfig.yaw_deadband = read8(); currentProfile->pidProfile.gyro_lpf_hz = read8(); currentProfile->pidProfile.dterm_lpf_hz = read8(); - //currentProfile->pidProfile.yaw_pterm_cut_hz = read8(); +// currentProfile->pidProfile.yaw_pterm_cut_hz = read8(); } } else { headSerialError(0); } break; +*/ case MSP_SET_MISC: + break; tmp = read16(); if (tmp < 1600 && tmp > 1400) masterConfig.rxConfig.midrc = tmp; @@ -1314,9 +1484,19 @@ static bool processInCommand(void) masterConfig.mag_hardware = read8(); } break; + + case MSP_SET_MOTOR_PWM: + masterConfig.motor_pwm_rate = read16(); + masterConfig.motor_pwm_protocol = read8(); + break; + case MSP_SET_MOTOR: - for (i = 0; i < 8; i++) // FIXME should this use MAX_MOTORS or MAX_SUPPORTED_MOTORS instead of 8 - motor_disarmed[i] = read16(); + for (i = 0; i < 8; i++) { + const int16_t disarmed = read16(); + if (i < MAX_SUPPORTED_MOTORS) { + motor_disarmed[i] = disarmed; + } + } break; case MSP_SET_SERVO_CONFIGURATION: #ifdef USE_SERVOS @@ -1358,20 +1538,46 @@ static bool processInCommand(void) #endif break; + case MSP_SET_3D: + masterConfig.flight3DConfig.deadband3d_low = read16(); + masterConfig.flight3DConfig.deadband3d_high = read16(); + masterConfig.flight3DConfig.neutral3d = read16(); + masterConfig.flight3DConfig.deadband3d_throttle = read16(); + break; + + case MSP_SET_RC_DEADBAND: + currentProfile->rcControlsConfig.deadband = read8(); + currentProfile->rcControlsConfig.yaw_deadband = read8(); + currentProfile->rcControlsConfig.alt_hold_deadband = read8(); + break; + + case MSP_SET_RESET_CURR_PID: + resetPidProfile(¤tProfile->pidProfile); + break; + + case MSP_SET_SENSOR_ALIGNMENT: + masterConfig.sensorAlignmentConfig.gyro_align = read8(); + masterConfig.sensorAlignmentConfig.acc_align = read8(); + masterConfig.sensorAlignmentConfig.mag_align = read8(); + break; + case MSP_RESET_CONF: if (!ARMING_FLAG(ARMED)) { resetEEPROM(); readEEPROM(); } break; + case MSP_ACC_CALIBRATION: if (!ARMING_FLAG(ARMED)) accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); break; + case MSP_MAG_CALIBRATION: if (!ARMING_FLAG(ARMED)) ENABLE_STATE(CALIBRATE_MAG); break; + case MSP_EEPROM_WRITE: if (ARMING_FLAG(ARMED)) { headSerialError(0); @@ -1381,6 +1587,32 @@ static bool processInCommand(void) readEEPROM(); break; +#ifdef BLACKBOX + case MSP_SET_BLACKBOX_CONFIG: + // Don't allow config to be updated while Blackbox is logging + if (blackboxMayEditConfig()) { + masterConfig.blackbox_device = read8(); + masterConfig.blackbox_rate_num = read8(); + masterConfig.blackbox_rate_denom = read8(); + } + break; +#endif + +#ifdef TRANSPONDER + case MSP_SET_TRANSPONDER_CONFIG: + if (currentPort->dataSize != sizeof(masterConfig.transponderData)) { + headSerialError(0); + break; + } + + for (i = 0; i < sizeof(masterConfig.transponderData); i++) { + masterConfig.transponderData[i] = read8(); + } + + transponderUpdateData(masterConfig.transponderData); + break; +#endif + #ifdef USE_FLASHFS case MSP_DATAFLASH_ERASE: flashfsEraseCompletely(); @@ -1473,19 +1705,18 @@ static bool processInCommand(void) masterConfig.failsafeConfig.failsafe_delay = read8(); masterConfig.failsafeConfig.failsafe_off_delay = read8(); masterConfig.failsafeConfig.failsafe_throttle = read16(); + masterConfig.failsafeConfig.failsafe_kill_switch = read8(); + masterConfig.failsafeConfig.failsafe_throttle_low_delay = read16(); + masterConfig.failsafeConfig.failsafe_procedure = read8(); break; case MSP_SET_RXFAIL_CONFIG: - { - uint8_t channelCount = currentPort->dataSize / 3; - if (channelCount > MAX_SUPPORTED_RC_CHANNEL_COUNT) { - headSerialError(0); - } else { - for (i = 0; i < channelCount; i++) { - masterConfig.rxConfig.failsafe_channel_configurations[i].mode = read8(); - masterConfig.rxConfig.failsafe_channel_configurations[i].step = CHANNEL_VALUE_TO_RXFAIL_STEP(read16()); - } - } + i = read8(); + if (i < MAX_SUPPORTED_RC_CHANNEL_COUNT) { + masterConfig.rxConfig.failsafe_channel_configurations[i].mode = read8(); + masterConfig.rxConfig.failsafe_channel_configurations[i].step = CHANNEL_VALUE_TO_RXFAIL_STEP(read16()); + } else { + headSerialError(0); } break; @@ -1644,7 +1875,7 @@ static bool mspProcessReceivedData(uint8_t c) } else if (currentPort->c_state == HEADER_M) { currentPort->c_state = (c == '<') ? HEADER_ARROW : IDLE; } else if (currentPort->c_state == HEADER_ARROW) { - if (c > MSP_PORT_INBUF_SIZE) { + if (c > MSP_PORT_INBUF_SIZE) { currentPort->c_state = IDLE; } else { diff --git a/src/main/io/serial_msp.h b/src/main/io/serial_msp.h index 8905ad4c4..5542c10ff 100644 --- a/src/main/io/serial_msp.h +++ b/src/main/io/serial_msp.h @@ -45,7 +45,7 @@ * that the newer API version may cause problems before using API commands that change FC state. * * It is for this reason that each MSP command should be specific as possible, such that changes - * to commands break as little functionality as possible. + * to commands break as little client functionality as possible. * * API client authors MAY use a compatibility matrix/table when determining if they can support * a given command from a given flight controller at a given api version level. @@ -59,7 +59,7 @@ #define MSP_PROTOCOL_VERSION 0 #define API_VERSION_MAJOR 1 // increment when major changes are made -#define API_VERSION_MINOR 14 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR +#define API_VERSION_MINOR 16 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR #define API_VERSION_LENGTH 2 @@ -70,7 +70,7 @@ #define BASEFLIGHT_IDENTIFIER "BAFL"; #define FLIGHT_CONTROLLER_IDENTIFIER_LENGTH 4 -static const char * const flightControllerIdentifier = RACEFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller. +extern const char * const flightControllerIdentifier; #define FLIGHT_CONTROLLER_VERSION_LENGTH 3 #define FLIGHT_CONTROLLER_VERSION_MASK 0xFFF @@ -161,6 +161,14 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; #define MSP_RXFAIL_CONFIG 77 //out message Returns RXFAIL settings #define MSP_SET_RXFAIL_CONFIG 78 //in message Sets RXFAIL settings +#define MSP_SDCARD_SUMMARY 79 //out message Get the state of the SD card + +#define MSP_BLACKBOX_CONFIG 80 //out message Get blackbox settings +#define MSP_SET_BLACKBOX_CONFIG 81 //in message Set blackbox settings + +#define MSP_TRANSPONDER_CONFIG 82 //in message Get transponder settings +#define MSP_SET_TRANSPONDER_CONFIG 83 //out message Set transponder settings + // // Baseflight MSP commands (if enabled they exist in Cleanflight) // @@ -208,7 +216,15 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; #define MSP_NAV_STATUS 121 //out message Returns navigation status #define MSP_NAV_CONFIG 122 //out message Returns navigation parameters #define MSP_PID_FLOAT 123 //out message P I D Used for Luxfloat +#define MSP_3D 124 //out message Settings needed for reversible ESCs +#define MSP_RC_DEADBAND 125 //out message deadbands for yaw alt pitch roll +#define MSP_SENSOR_ALIGNMENT 126 //out message orientation of acc,gyro,mag +/* raceflight (counting down from 200 for compatibility) */ +#define MSP_MOTOR_PWM 199 // out message configuration of the motor PWM details. +#define MSP_SET_MOTOR_PWM 198 // in message configuration of the motor PWM details. + +/* cleanflight beta flight */ #define MSP_SET_RAW_RC 200 //in message 8 rc chan #define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed #define MSP_SET_PID 202 //in message P I D coeff (9 are used currently) @@ -225,8 +241,13 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; #define MSP_SET_MOTOR 214 //in message PropBalance function #define MSP_SET_NAV_CONFIG 215 //in message Sets nav config parameters - write to the eeprom #define MSP_SET_PID_FLOAT 216 //in message P I D used for luxfloat +#define MSP_SET_3D 217 //in message Settings needed for reversible ESCs +#define MSP_SET_RC_DEADBAND 218 //in message deadbands for yaw alt pitch roll +#define MSP_SET_RESET_CURR_PID 219 //in message resetting the current pid profile to defaults +#define MSP_SET_SENSOR_ALIGNMENT 220 //in message set the orientation of the acc,gyro,mag // #define MSP_BIND 240 //in message no param +// #define MSP_ALARMS 242 #define MSP_EEPROM_WRITE 250 //in message no param @@ -234,16 +255,17 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; #define MSP_DEBUG 254 //out message debug1,debug2,debug3,debug4 // Additional commands that are not compatible with MultiWii +#define MSP_STATUS_EX 150 //out message cycletime, errors_count, CPU load, sensor present etc #define MSP_UID 160 //out message Unique device ID +#define MSP_GPSSVINFO 164 //out message get Signal Strength (only U-Blox) #define MSP_ACC_TRIM 240 //out message get acc angle trim values #define MSP_SET_ACC_TRIM 239 //in message set acc angle trim values -#define MSP_GPSSVINFO 164 //out message get Signal Strength (only U-Blox) #define MSP_SERVO_MIX_RULES 241 //out message Returns servo mixer configuration #define MSP_SET_SERVO_MIX_RULE 242 //in message Sets servo mixer configuration #define MSP_SET_1WIRE 243 //in message Sets 1Wire paththrough - #define MSP_SET_ESCSERIAL 244 //in message Sets escserial passthrough #define MSP_SET_4WAY_IF 245 //in message Sets 4way interface + // Each MSP port requires state and a receive buffer, revisit this default if someone needs more than 2 MSP ports. #define MAX_MSP_PORT_COUNT 2 @@ -257,6 +279,12 @@ typedef enum { COMMAND_RECEIVED } mspState_e; +typedef enum { + UNUSED_PORT = 0, + FOR_GENERAL_MSP, + FOR_TELEMETRY +} mspPortUsage_e; + #define MSP_PORT_INBUF_SIZE 64 typedef struct mspPort_s { @@ -268,6 +296,7 @@ typedef struct mspPort_s { uint8_t inBuf[MSP_PORT_INBUF_SIZE]; mspState_e c_state; uint8_t cmdMSP; + mspPortUsage_e mspPortUsage; } mspPort_t; void mspInit(serialConfig_t *serialConfig); diff --git a/src/main/main.c b/src/main/main.c index d9fa433fe..401177e3a 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -50,6 +50,7 @@ #include "drivers/inverter.h" #include "drivers/flash_m25p16.h" #include "drivers/sonar_hcsr04.h" +#include "drivers/sdcard.h" #include "drivers/gyro_sync.h" #include "drivers/exti.h" #include "drivers/io.h" @@ -65,6 +66,7 @@ #include "io/gimbal.h" #include "io/ledstrip.h" #include "io/display.h" +#include "io/asyncfatfs/asyncfatfs.h" #include "sensors/sensors.h" #include "sensors/sonar.h" @@ -186,19 +188,27 @@ void init(void) #endif //i2cSetOverclock(masterConfig.i2c_overclock); + systemInit(); + #ifdef USE_HARDWARE_REVISION_DETECTION detectHardwareRevision(); #endif - systemInit(); - // Latch active features to be used for feature() in the remainder of init(). latchActiveFeatures(); // initialize IO (needed for all IO operations) IOInitGlobal(); - ledInit(); +#ifdef ALIENFLIGHTF3 + if (hardwareRevision == AFF3_REV_1) { + ledInit(false); + } else { + ledInit(true); + } +#else + ledInit(false); +#endif #ifdef USE_EXTI EXTIInit(); @@ -283,38 +293,25 @@ void init(void) pwm_params.servoPwmRate = masterConfig.servo_pwm_rate; #endif - pwm_params.useOneshot = feature(FEATURE_ONESHOT125); - pwm_params.useMultiShot = feature(FEATURE_MULTISHOT); - pwm_params.usePwmRate = feature(FEATURE_USE_PWM_RATE); - pwm_params.useFastPWM = masterConfig.use_fast_pwm ? true : false; - pwm_params.motorPwmRate = masterConfig.motor_pwm_rate; - if (feature(FEATURE_3D)) - { + pwm_params.motorPwmProtocol = masterConfig.motor_pwm_protocol; + pwm_params.useOneshot = feature(FEATURE_ONESHOT); + + if (feature(FEATURE_ONESHOT)) { + pwm_params.motorPwmRate = 0; + } else { + pwm_params.motorPwmRate = masterConfig.motor_pwm_rate; + motorControlEnable = true; + } + + if (!feature(FEATURE_3D)) + pwm_params.idlePulse = masterConfig.escAndServoConfig.mincommand; + else pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d; - } - else - { - if ((pwm_params.motorPwmRate > 500 && !masterConfig.use_fast_pwm) && !feature(FEATURE_USE_PWM_RATE)) - { - pwm_params.idlePulse = 0; // brushed motors - } - else - { - if (feature(FEATURE_USE_PWM_RATE)) { - pwm_params.idlePulse = (uint16_t)((float)(masterConfig.escAndServoConfig.mincommand-1000) / 4.1666f)+60; - } else { - pwm_params.idlePulse = (uint16_t)((float)masterConfig.escAndServoConfig.mincommand*1.5f); - } - } - } pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params); mixerUsePWMOutputConfiguration(pwmOutputConfiguration); - if (!feature(FEATURE_ONESHOT125) && !feature(FEATURE_MULTISHOT)) - motorControlEnable = true; - systemState |= SYSTEM_STATE_MOTORS_READY; #ifdef BEEPER @@ -358,8 +355,14 @@ void init(void) #ifdef USE_SPI spiInit(SPIDEV_1); spiInit(SPIDEV_2); +#ifdef ALIENFLIGHTF3 + if (hardwareRevision == AFF3_REV_2) { + spiInit(SPIDEV_3); + } +#else spiInit(SPIDEV_3); #endif +#endif #ifdef USE_HARDWARE_REVISION_DETECTION updateHardwareRevision(); @@ -510,6 +513,27 @@ void init(void) flashfsInit(); #endif +#ifdef USE_SDCARD + bool sdcardUseDMA = false; + + sdcardInsertionDetectInit(); + +#ifdef SDCARD_DMA_CHANNEL_TX + +#if defined(LED_STRIP) && defined(WS2811_DMA_CHANNEL) + // Ensure the SPI Tx DMA doesn't overlap with the led strip + sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_CHANNEL; +#else + sdcardUseDMA = true; +#endif + +#endif + + sdcard_init(sdcardUseDMA); + + afatfs_init(); +#endif + #ifdef BLACKBOX initBlackbox(); #endif diff --git a/src/main/mw.c b/src/main/mw.c index ca5176214..c27a15517 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -40,6 +40,7 @@ #include "drivers/system.h" #include "drivers/serial.h" #include "drivers/timer.h" +#include "drivers/pwm_mapping.h" #include "drivers/pwm_rx.h" #include "drivers/gyro_sync.h" @@ -64,6 +65,7 @@ #include "io/serial_cli.h" #include "io/serial_msp.h" #include "io/statusindicator.h" +#include "io/asyncfatfs/asyncfatfs.h" #include "rx/rx.h" #include "rx/msp.h" @@ -394,7 +396,6 @@ void mwArm(void) } if (!ARMING_FLAG(PREVENT_ARMING)) { ENABLE_ARMING_FLAG(ARMED); - headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); #ifdef BLACKBOX @@ -786,6 +787,10 @@ void taskMainPidLoop(void) writeMotors(); } +#ifdef USE_SDCARD + afatfs_poll(); +#endif + #ifdef BLACKBOX if (!cliMode && feature(FEATURE_BLACKBOX)) { handleBlackbox(); diff --git a/src/main/platform.h b/src/main/platform.h index 53c79fdcf..e43e916ed 100644 --- a/src/main/platform.h +++ b/src/main/platform.h @@ -18,6 +18,7 @@ #pragma once #if defined(STM32F40_41xxx) || defined (STM32F411xE) +#define STM32F4 #include "stm32f4xx_conf.h" #include "stm32f4xx_rcc.h" #include "stm32f4xx_gpio.h" @@ -32,6 +33,8 @@ #ifdef STM32F303xC +#define STM32F3 + #include "stm32f30x_conf.h" #include "stm32f30x_rcc.h" #include "stm32f30x_gpio.h" @@ -45,6 +48,7 @@ #endif #ifdef STM32F10X +#define STM32F1 #include "stm32f10x_conf.h" #include "stm32f10x_gpio.h" diff --git a/src/main/rx/pwm.c b/src/main/rx/pwm.c index 894eb27a4..6fe3e567f 100644 --- a/src/main/rx/pwm.c +++ b/src/main/rx/pwm.c @@ -27,6 +27,7 @@ #include "drivers/gpio.h" #include "drivers/timer.h" +#include "drivers/pwm_mapping.h" #include "drivers/pwm_rx.h" #include "config/config.h" diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 89a80cbd8..f8ad9a986 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -39,6 +39,7 @@ #include "drivers/gpio.h" #include "drivers/timer.h" +#include "drivers/pwm_mapping.h" #include "drivers/pwm_rx.h" #include "drivers/system.h" #include "drivers/gyro_sync.h" diff --git a/src/main/scheduler.c b/src/main/scheduler.c index cfe69179c..3974f09c2 100644 --- a/src/main/scheduler.c +++ b/src/main/scheduler.c @@ -36,6 +36,7 @@ static uint32_t totalWaitingTasksSamples; static uint32_t realtimeGuardInterval; uint32_t currentTime = 0; +uint16_t averageSystemLoadPercent = 0; uint16_t averageWaitingTasks100 = 0; typedef struct { diff --git a/src/main/scheduler.h b/src/main/scheduler.h index 1796c5f1c..b34f4a957 100644 --- a/src/main/scheduler.h +++ b/src/main/scheduler.h @@ -84,6 +84,7 @@ typedef enum { } cfTaskId_e; extern uint16_t cpuLoad; +extern uint16_t averageSystemLoadPercent; extern uint16_t averageWaitingTasks100; void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t * taskInfo); diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 31d7688c6..6073ca351 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -35,7 +35,7 @@ #include "sensors/sensors.h" #include "sensors/compass.h" -#ifdef NAZE +#ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" #endif diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 9ef77e03d..924ef4c39 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -71,7 +71,7 @@ #include "sensors/sonar.h" #include "sensors/initialisation.h" -#ifdef NAZE +#ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" #endif @@ -112,6 +112,21 @@ const extiConfig_t *selectMPUIntExtiConfig(void) #endif #endif +#ifdef ALIENFLIGHTF3 + // MPU_INT output on V1 PA15 + static const extiConfig_t alienFlightF3V1MPUIntExtiConfig = { + .io = IO_TAG(PA15) + }; + // MPU_INT output on V2 PB13 + static const extiConfig_t alienFlightF3V2MPUIntExtiConfig = { + .io = IO_TAG(PB13) + }; + if (hardwareRevision == AFF3_REV_1) { + return &alienFlightF3V1MPUIntExtiConfig; + } else { + return &alienFlightF3V2MPUIntExtiConfig; + } +#endif return NULL; } @@ -227,7 +242,7 @@ bool detectGyro(void) ; // fallthrough case GYRO_MPU6500: -#ifdef USE_GYRO_MPU6500 +#if defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) #ifdef USE_GYRO_SPI_MPU6500 if (mpu6500GyroDetect(&gyro) || mpu6500SpiGyroDetect(&gyro)) #else @@ -373,7 +388,7 @@ static void detectAcc(accelerationSensor_e accHardwareToUse) #endif ; // fallthrough case ACC_MPU6500: -#ifdef USE_ACC_MPU6500 +#if defined(USE_ACC_MPU6500) || defined(USE_ACC_SPI_MPU6500) #ifdef USE_ACC_SPI_MPU6500 if (mpu6500AccDetect(&acc) || mpu6500SpiAccDetect(&acc)) #else @@ -390,7 +405,7 @@ static void detectAcc(accelerationSensor_e accHardwareToUse) ; // fallthrough case ACC_MPU9250: -#ifdef USE_ACC_MPU9250 +#ifdef USE_ACC_SPI_MPU9250 if (mpu9250SpiAccDetect(&acc)) { #ifdef ACC_MPU9250_ALIGN @@ -534,6 +549,14 @@ static void detectMag(magSensor_e magHardwareToUse) #endif +#ifdef AQ32_V2 + static const hmc5883Config_t aq32v2Hmc5883Config = { + .io = IO_TAG(PE2) + }; + + hmc5883Config = &aq32v2Hmc5883Config; +#endif + retry: magAlign = ALIGN_DEFAULT; @@ -661,7 +684,7 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t a } #if defined(USE_GYRO_SPI_MPU6500) - spiSetDivisor(MPU6500_SPI_INSTANCE, 6); + spiSetDivisor(MPU6500_SPI_INSTANCE, 5); #endif return true; diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index cf1b2c80b..5d482e758 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -132,9 +132,6 @@ #define SERIAL_PORT_COUNT 4 -//#define USE_ESCSERIAL -//#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 - #define USE_SPI #define USE_SPI_DEVICE_1 @@ -205,12 +202,7 @@ #define BINDPLUG_PORT GPIOB #define BINDPLUG_PIN Pin_2 -#define USE_SERIAL_1WIRE -#define ESC_COUNT 8 -#define S1W_TX_GPIO GPIOA -#define S1W_TX_PIN GPIO_Pin_9 -#define S1W_RX_GPIO GPIOA -#define S1W_RX_PIN GPIO_Pin_10 +#define USE_SERIAL_4WAY_BLHELI_INTERFACE #define USE_QUATERNION diff --git a/src/main/target/AQ32_V2/target.h b/src/main/target/AQ32_V2/target.h index 70efdf6dd..ef29d33b4 100644 --- a/src/main/target/AQ32_V2/target.h +++ b/src/main/target/AQ32_V2/target.h @@ -20,7 +20,7 @@ #define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) #define CONFIG_SERIALRX_PROVIDER SERIALRX_SBUS -//#define CONFIG_BLACKBOX_DEVICE BLACKBOX_DEVICE_SDCARD +#define CONFIG_BLACKBOX_DEVICE BLACKBOX_DEVICE_SDCARD #define CONFIG_FEATURE_RX_SERIAL #define CONFIG_RX_SERIAL_PORT 2 #define CONFIG_FEATURE_ONESHOT125 @@ -188,12 +188,7 @@ #define BIND_PORT GPIOD #define BIND_PIN Pin_6 -#define USE_SERIAL_1WIRE -#define ESC_COUNT 8 -#define S1W_TX_GPIO GPIOA -#define S1W_TX_PIN GPIO_Pin_9 -#define S1W_RX_GPIO GPIOA -#define S1W_RX_PIN GPIO_Pin_10 +#define USE_SERIAL_4WAY_BLHELI_INTERFACE #define USE_QUATERNION diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index 2387b09a2..ea46c777f 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -19,8 +19,8 @@ #define TARGET_BOARD_IDENTIFIER "BJF4" #define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) -#define CONFIG_SERIALRX_PROVIDER 2 -#define CONFIG_BLACKBOX_DEVICE 1 +#define CONFIG_SERIALRX_PROVIDER SERIALRX_SBUS +#define CONFIG_BLACKBOX_DEVICE BLACKBOX_DEVICE_FLASH #define CONFIG_FEATURE_RX_SERIAL #define CONFIG_FEATURE_ONESHOT125 #define CONFIG_RX_SERIAL_PORT 3 @@ -60,6 +60,32 @@ #define USE_BARO_MS5611 #define MS5611_I2C_INSTANCE I2CDEV_1 +//#define USE_SDCARD + +#define SDCARD_DETECT_INVERTED + +#define SDCARD_DETECT_PIN PD2 +#define SDCARD_DETECT_EXTI_LINE EXTI_Line2 +#define SDCARD_DETECT_EXTI_PIN_SOURCE EXTI_PinSource2 +#define SDCARD_DETECT_EXTI_PORT_SOURCE EXTI_PortSourceGPIOD +#define SDCARD_DETECT_EXTI_IRQn EXTI2_IRQn + +#define SDCARD_SPI_INSTANCE SPI3 +#define SDCARD_SPI_CS_PIN PA15 + +// SPI2 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init: +#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz +// Divide to under 25MHz for normal operation: +#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 // 21MHz + +#define SDCARD_DMA_CHANNEL_TX DMA1_Stream5 +#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF5 +#define SDCARD_DMA_CLK RCC_AHB1Periph_DMA1 +#define SDCARD_DMA_CHANNEL DMA_Channel_0 + +// Performance logging for SD card operations: +// #define AFATFS_USE_INTROSPECTIVE_LOGGING + #define M25P16_CS_PIN PB3 #define M25P16_SPI_INSTANCE SPI3 diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index 24efd0f98..3e7bb50c9 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -120,16 +120,9 @@ #define USE_SERVOS #define USE_CLI -#define USE_SERIAL_1WIRE - -// FlexPort (pin 21/22, TX/RX respectively): -// Note, FlexPort has 10k pullups on both TX and RX -// JST Pin3 TX - connect to external UART/USB RX -#define S1W_TX_GPIO GPIOB -#define S1W_TX_PIN GPIO_Pin_10 -// JST Pin4 RX - connect to external UART/USB TX -#define S1W_RX_GPIO GPIOB -#define S1W_RX_PIN GPIO_Pin_11 +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define USE_QUATERNION #undef DISPLAY #undef SONAR diff --git a/src/main/target/CHEBUZZF3/target.h b/src/main/target/CHEBUZZF3/target.h index 5c47196d8..c0d20d039 100644 --- a/src/main/target/CHEBUZZF3/target.h +++ b/src/main/target/CHEBUZZF3/target.h @@ -43,6 +43,34 @@ #define USE_SPI #define USE_SPI_DEVICE_1 +#define USE_SPI_DEVICE_2 + +#define USE_SDCARD + +#define SDCARD_DETECT_PIN PC14 +#define SDCARD_DETECT_EXTI_LINE EXTI_Line14 +#define SDCARD_DETECT_EXTI_PIN_SOURCE EXTI_PinSource14 +#define SDCARD_DETECT_EXTI_PORT_SOURCE EXTI_PortSourceGPIOC +#define SDCARD_DETECT_EXTI_IRQn EXTI15_10_IRQn + +#define SDCARD_SPI_INSTANCE SPI2 +#define SDCARD_SPI_CS_PIN PB12 + +// SPI2 is on the APB1 bus whose clock runs at 36MHz. Divide to under 400kHz for init: +#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128 +// Divide to under 25MHz for normal operation: +#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 2 + +// Note, this is the same DMA channel as USART1_RX. Luckily we don't use DMA for USART Rx. +#define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 +#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA1_FLAG_TC5 + +//#define USE_FLASHFS +//#define USE_FLASH_M25P16 + +//#define M25P16_CS_GPIO GPIOB +//#define M25P16_CS_PIN GPIO_Pin_12 +//#define M25P16_SPI_INSTANCE SPI2 #define GYRO #define USE_GYRO_L3GD20 diff --git a/src/main/target/IRCFUSIONF3/target.h b/src/main/target/IRCFUSIONF3/target.h index 3905e3cf7..2bcbb4d39 100644 --- a/src/main/target/IRCFUSIONF3/target.h +++ b/src/main/target/IRCFUSIONF3/target.h @@ -164,11 +164,9 @@ #define BIND_PORT GPIOB #define BIND_PIN Pin_11 -#define USE_SERIAL_1WIRE -#define S1W_TX_GPIO GPIOA -#define S1W_TX_PIN GPIO_Pin_9 -#define S1W_RX_GPIO GPIOA -#define S1W_RX_PIN GPIO_Pin_10 +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define USE_QUATERNION #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff diff --git a/src/main/target/KKNGF4/target.h b/src/main/target/KKNGF4/target.h index f27f3c000..274f8cfc5 100644 --- a/src/main/target/KKNGF4/target.h +++ b/src/main/target/KKNGF4/target.h @@ -18,8 +18,8 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "REVO" //Call it a revo for now so it connects to RFC for testing. #define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) -#define CONFIG_SERIALRX_PROVIDER 2 -#define CONFIG_BLACKBOX_DEVICE 1 +#define CONFIG_SERIALRX_PROVIDER SERIALRX_SBUS +#define CONFIG_BLACKBOX_DEVICE BLACKBOX_DEVICE_FLASH #define CONFIG_FEATURE_RX_SERIAL #define CONFIG_FEATURE_ONESHOT125 #define CONFIG_MSP_PORT 2 @@ -56,6 +56,29 @@ #define USE_BARO_MS5611 #define MS5611_I2C_INSTANCE I2CDEV_1 +//#define USE_SDCARD + +#define SDCARD_DETECT_INVERTED + +#define SDCARD_DETECT_PIN PD2 +#define SDCARD_DETECT_EXTI_LINE EXTI_Line2 +#define SDCARD_DETECT_EXTI_PIN_SOURCE EXTI_PinSource2 +#define SDCARD_DETECT_EXTI_PORT_SOURCE EXTI_PortSourceGPIOD +#define SDCARD_DETECT_EXTI_IRQn EXTI2_IRQn + +#define SDCARD_SPI_INSTANCE SPI3 +#define SDCARD_SPI_CS_PIN PB3 + +// SPI2 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init: +#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz +// Divide to under 25MHz for normal operation: +#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 // 21MHz + +#define SDCARD_DMA_CHANNEL_TX DMA1_Stream5 +#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF5 +#define SDCARD_DMA_CLK RCC_AHB1Periph_DMA1 +#define SDCARD_DMA_CHANNEL DMA_Channel_0 + #define USE_FLASHFS #define USE_FLASH_M25P16 #define M25P16_CS_PIN PB3 diff --git a/src/main/target/LUX_RACE/target.h b/src/main/target/LUX_RACE/target.h index 6e044bdb1..87e6d41b9 100644 --- a/src/main/target/LUX_RACE/target.h +++ b/src/main/target/LUX_RACE/target.h @@ -153,12 +153,9 @@ #define BIND_PORT GPIOC #define BIND_PIN Pin_5 -#define USE_SERIAL_1WIRE -// USART2, RX is on USART1 -#define S1W_TX_GPIO GPIOB -#define S1W_TX_PIN GPIO_Pin_10 -#define S1W_RX_GPIO GPIOB -#define S1W_RX_PIN GPIO_Pin_11 +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define USE_QUATERNION #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index 1f0a08ebe..abbe2a4d4 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/src/main/target/MOTOLAB/target.h @@ -174,12 +174,9 @@ #define BIND_PORT GPIOB #define BIND_PIN Pin_4 -#define USE_SERIAL_1WIRE +#define USE_SERIAL_4WAY_BLHELI_INTERFACE -#define S1W_TX_GPIO GPIOB -#define S1W_TX_PIN GPIO_Pin_6 -#define S1W_RX_GPIO GPIOB -#define S1W_RX_PIN GPIO_Pin_7 +#define USE_QUATERNION #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index d9cc8c437..2ffceae69 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -151,6 +151,8 @@ #define LED_STRIP #define LED_STRIP_TIMER TIM3 +#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC6 +#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH6_HANDLER #define BLACKBOX #define TELEMETRY @@ -164,20 +166,25 @@ #define BIND_PORT GPIOA #define BIND_PIN Pin_3 -#define USE_SERIAL_1WIRE +#define USE_SERIAL_4WAY_BLHELI_INTERFACE -// STM32F103CBT6-LQFP48 Pin30 (PA9) TX - PC3 connects to onboard CP2102 RX -#define S1W_TX_GPIO GPIOA -#define S1W_TX_PIN GPIO_Pin_9 -// STM32F103CBT6-LQFP48 Pin31 (PA10) RX - PC1 to onboard CP2102 TX -#define S1W_RX_GPIO GPIOA -#define S1W_RX_PIN GPIO_Pin_10 +#define USE_QUATERNION -// alternative defaults for ALIENFLIGHT F1 target +// alternative defaults for AlienFlight F1 target #ifdef ALIENFLIGHT #undef TARGET_BOARD_IDENTIFIER -#define TARGET_BOARD_IDENTIFIER "AFF1" // ALIENFLIGHTF1. +#define TARGET_BOARD_IDENTIFIER "AFF1" // ALIENFLIGHT F1. #undef BOARD_HAS_VOLTAGE_DIVIDER +#undef USE_SOFTSERIAL2 +#undef USE_SOFTSERIAL1 +#undef GPS +#undef BLACKBOX +#undef TELEMETRY + +#define CONFIG_SERIALRX_PROVIDER SERIALRX_SPEKTRUM2048 +#define CONFIG_FEATURE_RX_SERIAL +#define CONFIG_RX_SERIAL_PORT 1 + #define HARDWARE_BIND_PLUG // Hardware bind plug at PB5 (Pin 41) @@ -187,4 +194,4 @@ #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) \ No newline at end of file +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) diff --git a/src/main/target/QUANTON/target.h b/src/main/target/QUANTON/target.h index 3ae0e826a..e7b18cb9a 100644 --- a/src/main/target/QUANTON/target.h +++ b/src/main/target/QUANTON/target.h @@ -25,27 +25,24 @@ #define LED0 PC13 #define LED1 PC14 -#define MPU6000_CS_GPIO_CLK_PERIPHERAL RCC_AHB1Periph_GPIOC -#define MPU6000_CS_GPIO GPIOC -#define MPU6000_CS_PIN GPIO_Pin_4 +#define MPU6000_CS_PIN PC4 #define MPU6000_SPI_INSTANCE SPI1 #define ACC #define USE_ACC_MPU6000 #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW180_DEG +#define ACC_MPU6000_ALIGN CW270_DEG #define GYRO #define USE_GYRO_MPU6000 #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW180_DEG +#define GYRO_MPU6000_ALIGN CW270_DEG //#define BARO //#define USE_BARO_MS5611 //#define MS5611_I2C_INSTANCE I2CDEV_1 -//#define M25P16_CS_GPIO GPIOB -//#define M25P16_CS_PIN GPIO_Pin_12 +//#define M25P16_CS_PIN PB12 //#define M25P16_SPI_INSTANCE SPI3 //#define USE_FLASHFS @@ -84,39 +81,22 @@ #define USE_SPI #define USE_SPI_DEVICE_1 -#define SPI1_NSS_GPIO GPIOC -#define SPI1_NSS_PERIPHERAL RCC_AHBPeriph_GPIOC -#define SPI1_NSS_PIN GPIO_Pin_4 -#define SPI1_NSS_PIN_SOURCE GPIO_PinSource4 -#define SPI1_GPIO GPIOA -#define SPI1_GPIO_PERIPHERAL RCC_AHBPeriph_GPIOA -#define SPI1_SCK_PIN GPIO_Pin_5 -#define SPI1_SCK_PIN_SOURCE GPIO_PinSource5 -#define SPI1_MISO_PIN GPIO_Pin_6 -#define SPI1_MISO_PIN_SOURCE GPIO_PinSource6 -#define SPI1_MOSI_PIN GPIO_Pin_7 -#define SPI1_MOSI_PIN_SOURCE GPIO_PinSource7 +#define SPI1_NSS_PIN PC4 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 #define USE_SPI_DEVICE_3 -#define SPI3_NSS_GPIO GPIOB -#define SPI3_NSS_PERIPHERAL RCC_AHBPeriph_GPIOB -#define SPI3_NSS_PIN GPIO_Pin_3 -#define SPI3_NSS_PIN_SOURCE GPIO_PinSource3 -#define SPI3_GPIO GPIOC -#define SPI3_GPIO_PERIPHERAL RCC_AHBPeriph_GPIOC -#define SPI3_SCK_PIN GPIO_Pin_10 -#define SPI3_SCK_PIN_SOURCE GPIO_PinSource10 -#define SPI3_MISO_PIN GPIO_Pin_11 -#define SPI3_MISO_PIN_SOURCE GPIO_PinSource11 -#define SPI3_MOSI_PIN GPIO_Pin_12 -#define SPI3_MOSI_PIN_SOURCE GPIO_PinSource12 +#define SPI3_NSS_PIN PB3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 #define USE_I2C #define I2C_DEVICE (I2CDEV_1) #define USE_ADC -#define VBAT_ADC_GPIO GPIOC -#define VBAT_ADC_GPIO_PIN GPIO_Pin_12 +#define VBAT_ADC_PIN PC12 #define VBAT_ADC_CHANNEL ADC_Channel_13 //#define GPS diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index 491fd6657..5ac1b4791 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -18,8 +18,8 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "REVO" #define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) -#define CONFIG_SERIALRX_PROVIDER 2 -#define CONFIG_BLACKBOX_DEVICE 1 +#define CONFIG_SERIALRX_PROVIDER SERIALRX_SBUS +#define CONFIG_BLACKBOX_DEVICE BLACKBOX_DEVICE_FLASH #define CONFIG_FEATURE_RX_SERIAL #define CONFIG_FEATURE_ONESHOT125 #define CONFIG_MSP_PORT 2 @@ -93,12 +93,6 @@ #define SERIAL_PORT_COUNT 4 //VCP, USART1, USART3, USART6 -#define USE_SERIAL_1WIRE -#define S1W_TX_GPIO GPIOB -#define S1W_TX_PIN GPIO_Pin_10 -#define S1W_RX_GPIO GPIOB -#define S1W_RX_PIN GPIO_Pin_11 - #define USE_SPI #define USE_SPI_DEVICE_1 diff --git a/src/main/target/REVONANO/target.h b/src/main/target/REVONANO/target.h index 640f7c007..b3faea41e 100644 --- a/src/main/target/REVONANO/target.h +++ b/src/main/target/REVONANO/target.h @@ -19,8 +19,8 @@ #define TARGET_BOARD_IDENTIFIER "REVN" #define CONFIG_START_FLASH_ADDRESS (0x08060000) //0x08060000 to 0x08080000 (FLASH_Sector_7) -#define CONFIG_SERIALRX_PROVIDER 2 -#define CONFIG_BLACKBOX_DEVICE 1 +#define CONFIG_SERIALRX_PROVIDER SERIALRX_SBUS +#define CONFIG_BLACKBOX_DEVICE BLACKBOX_DEVICE_FLASH #define CONFIG_FEATURE_RX_SERIAL #define CONFIG_FEATURE_ONESHOT125 #define CONFIG_MSP_PORT 1 @@ -114,13 +114,7 @@ #define USE_SERVOS #define USE_CLI -#define USE_SERIAL_1WIRE - -#define S1W_TX_GPIO GPIOB -#define S1W_TX_PIN GPIO_Pin_6 - -#define S1W_RX_GPIO GPIOB -#define S1W_RX_PIN GPIO_Pin_7 +#define USE_SERIAL_4WAY_BLHELI_INTERFACE #define USE_QUATERNION diff --git a/src/main/target/RMDO/target.h b/src/main/target/RMDO/target.h index fe4c9a354..95d1e9b07 100644 --- a/src/main/target/RMDO/target.h +++ b/src/main/target/RMDO/target.h @@ -147,12 +147,9 @@ #define BIND_PORT GPIOB #define BIND_PIN Pin_11 -#define USE_SERIAL_1WIRE +#define USE_SERIAL_4WAY_BLHELI_INTERFACE -#define S1W_TX_GPIO GPIOA -#define S1W_TX_PIN GPIO_Pin_9 -#define S1W_RX_GPIO GPIOA -#define S1W_RX_PIN GPIO_Pin_10 +#define USE_QUATERNION #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index 1ac8df19c..ee7658c18 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -147,12 +147,9 @@ #define WS2811_IRQ DMA1_Channel7_IRQn #endif -#define USE_SERIAL_1WIRE +#define USE_SERIAL_4WAY_BLHELI_INTERFACE -#define S1W_TX_GPIO GPIOB -#define S1W_TX_PIN GPIO_Pin_6 -#define S1W_RX_GPIO GPIOB -#define S1W_RX_PIN GPIO_Pin_7 +#define USE_QUATERNION #define SPEKTRUM_BIND // USART2, PA3 diff --git a/src/main/target/SPARKY2/target.h b/src/main/target/SPARKY2/target.h index e56587774..a5a24fec8 100755 --- a/src/main/target/SPARKY2/target.h +++ b/src/main/target/SPARKY2/target.h @@ -19,8 +19,8 @@ #define TARGET_BOARD_IDENTIFIER "SPK2" #define CONFIG_START_FLASH_ADDRESS 0x08080000 //0x08080000 to 0x080A0000 (FLASH_Sector_8) -#define CONFIG_SERIALRX_PROVIDER 2 -#define CONFIG_BLACKBOX_DEVICE 1 +#define CONFIG_SERIALRX_PROVIDER SERIALRX_SBUS +#define CONFIG_BLACKBOX_DEVICE BLACKBOX_DEVICE_FLASH #define CONFIG_FEATURE_RX_SERIAL #define CONFIG_FEATURE_ONESHOT125 #define CONFIG_MSP_PORT 2 @@ -71,12 +71,6 @@ #define USABLE_TIMER_CHANNEL_COUNT 11 -#define USE_SERIAL_1WIRE -#define S1W_TX_GPIO GPIOB -#define S1W_TX_PIN GPIO_Pin_10 -#define S1W_RX_GPIO GPIOB -#define S1W_RX_PIN GPIO_Pin_11 - // MPU9250 interrupt //#define DEBUG_MPU_DATA_READY_INTERRUPT #define USE_MPU_DATA_READY_SIGNAL diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index c665950ef..b67c9c891 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -140,6 +140,8 @@ #define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1 #define WS2811_DMA_CHANNEL DMA1_Channel2 #define WS2811_IRQ DMA1_Channel2_IRQn +#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 +#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER #define BLACKBOX #define DISPLAY @@ -155,12 +157,7 @@ #define BIND_PORT GPIOB #define BIND_PIN Pin_11 -#define USE_SERIAL_1WIRE - -#define S1W_TX_GPIO GPIOA -#define S1W_TX_PIN GPIO_Pin_9 -#define S1W_RX_GPIO GPIOA -#define S1W_RX_PIN GPIO_Pin_10 +#define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - stm32f303cc in 48pin package #define TARGET_IO_PORTA 0xffff diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 09e9107d1..7ed0047c8 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -42,6 +42,26 @@ #define USE_SPI #define USE_SPI_DEVICE_1 +#define USE_SPI_DEVICE_2 + +// SPI1 +// PB5 SPI1_MOSI +// PB4 SPI1_MISO +// PB3 SPI1_SCK +// PA15 SPI1_NSS + +// SPI2 +// PB15 SPI2_MOSI +// PB14 SPI2_MISO +// PB13 SPI2_SCK +// PB12 SPI2_NSS + +//#define USE_FLASHFS +//#define USE_FLASH_M25P16 + +//#define M25P16_CS_GPIO GPIOB +//#define M25P16_CS_PIN GPIO_Pin_12 +//#define M25P16_SPI_INSTANCE SPI2 #define GYRO #define USE_GYRO_L3GD20 @@ -53,6 +73,23 @@ #define GYRO_L3GD20_ALIGN CW270_DEG +#define USE_SDCARD + +#define SDCARD_SPI_INSTANCE SPI2 +#define SDCARD_SPI_CS_PIN PB12 + +// SPI2 is on the APB1 bus whose clock runs at 36MHz. Divide to under 400kHz for init: +#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128 +// Divide to under 25MHz for normal operation: +#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 2 + +// Note, this is the same DMA channel as USART1_RX. Luckily we don't use DMA for USART Rx. +#define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 +#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA1_FLAG_TC5 + +// Performance logging for SD card operations: +// #define AFATFS_USE_INTROSPECTIVE_LOGGING + #define ACC #define USE_ACC_LSM303DLHC @@ -100,14 +137,9 @@ #define USE_SERVOS #define USE_CLI -#define USE_SERIAL_1WIRE +#define USE_SERIAL_4WAY_BLHELI_INTERFACE -// STM32F3DISCOVERY TX - PD5 connects to UART RX -#define S1W_TX_GPIO GPIOD -#define S1W_TX_PIN GPIO_Pin_5 -// STM32F3DISCOVERY RX - PD6 connects to UART TX -#define S1W_RX_GPIO GPIOD -#define S1W_RX_PIN GPIO_Pin_6 +#define USE_QUATERNION #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff diff --git a/src/main/target/VRCORE/target.h b/src/main/target/VRCORE/target.h index 6c9fbd64b..0ddf41973 100644 --- a/src/main/target/VRCORE/target.h +++ b/src/main/target/VRCORE/target.h @@ -19,8 +19,8 @@ #define TARGET_BOARD_IDENTIFIER "VRCORE" #define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) -#define CONFIG_SERIALRX_PROVIDER 2 -#define CONFIG_BLACKBOX_DEVICE 1 +#define CONFIG_SERIALRX_PROVIDER SERIALRX_SBUS +#define CONFIG_BLACKBOX_DEVICE BLACKBOX_DEVICE_FLASH #define CONFIG_FEATURE_RX_SERIAL #define CONFIG_FEATURE_ONESHOT125 #define CONFIG_MSP_PORT 1 diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index ef5389920..9bba95613 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -23,6 +23,7 @@ #include "drivers/bus_i2c.h" #include "drivers/gpio.h" #include "drivers/timer.h" +#include "drivers/pwm_mapping.h" #include "drivers/pwm_rx.h" #include "drivers/adc.h" #include "drivers/light_led.h" diff --git a/top_makefile b/top_makefile index 6c62647ec..89acf45c3 100644 --- a/top_makefile +++ b/top_makefile @@ -10,8 +10,8 @@ ALL_TARGETS += spracingf3 #ALL_TARGETS += port103r #ALL_TARGETS += sparky -#ALL_TARGETS += alienwiif1 -#ALL_TARGETS += alienwiif3 +#ALL_TARGETS += alienflightf1 +#ALL_TARGETS += alienflightf3 #ALL_TARGETS += colibri_race ALL_TARGETS += motolab ALL_TARGETS += rmdo @@ -43,8 +43,8 @@ clean_eustm32f103rc eustm32f103rc : opts := TARGET=EUSTM32F103RC clean_spracingf3 spracingf3 : opts := TARGET=SPRACINGF3 clean_port103r port103r : opts := TARGET=PORT103R clean_sparky sparky : opts := TARGET=SPARKY -clean_alienwiif1 alienwiif1 : opts := TARGET=ALIENWIIF1 -clean_alienwiif3 alienwiif3 : opts := TARGET=ALIENWIIF3 +clean_alienwiif1 alienflightf1 : opts := TARGET=ALIENWIIF1 +clean_alienwiif3 alienflightf3 : opts := TARGET=ALIENWIIF3 clean_colibri_race colibri_race : opts := TARGET=COLIBRI_RACE clean_motolab motolab : opts := TARGET=MOTOLAB clean_rmdo rmdo : opts := TARGET=RMDO