From fec187949b0fc7ba2abd4e22ca7f5b3f6ffcfe62 Mon Sep 17 00:00:00 2001 From: hali9 Date: Sun, 22 Dec 2019 14:19:21 +0100 Subject: [PATCH 1/2] fix scaleRangef --- src/main/navigation/navigation.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index b5468e0a69b..637f845b480 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1397,8 +1397,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na fpVector3_t tmpWaypoint; tmpWaypoint.x = posControl.activeWaypoint.pos.x; tmpWaypoint.y = posControl.activeWaypoint.pos.y; - tmpWaypoint.z = scaleRange(constrainf(posControl.wpDistance, posControl.wpInitialDistance / 10, posControl.wpInitialDistance), - posControl.wpInitialDistance, posControl.wpInitialDistance / 10, + tmpWaypoint.z = scaleRangef(constrainf(posControl.wpDistance, posControl.wpInitialDistance / 10.0f, posControl.wpInitialDistance), + posControl.wpInitialDistance, posControl.wpInitialDistance / 10.0f, posControl.wpInitialAltitude, posControl.activeWaypoint.pos.z); setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); return NAV_FSM_EVENT_NONE; // will re-process state in >10ms From 629c8a16f930954745393d7538fc2c203d1e8a11 Mon Sep 17 00:00:00 2001 From: hali9 Date: Wed, 25 Dec 2019 15:10:56 +0100 Subject: [PATCH 2/2] Development update --- docs/Cli.md | 9 +- docs/USB_Mass_Storage_(MSC)_mode.md | 93 +++ make/mcu/STM32F4.mk | 42 +- make/mcu/STM32F7.mk | 42 +- make/release.mk | 4 +- make/source.mk | 10 +- make/targets.mk | 2 +- src/main/blackbox/blackbox.c | 2 +- src/main/cms/cms_menu_imu.c | 2 +- src/main/common/filter.c | 33 +- src/main/common/filter.h | 7 +- src/main/drivers/osd.c | 4 +- src/main/drivers/sdmmc_sdio.h | 241 ++++++ src/main/drivers/usb_msc.h | 33 + src/main/drivers/usb_msc_f4xx.c | 159 ++++ src/main/drivers/usb_msc_f7xx.c | 158 ++++ src/main/fc/cli.c | 40 + src/main/fc/fc_init.c | 33 +- src/main/fc/fc_msp.c | 14 +- src/main/fc/settings.yaml | 22 +- src/main/flight/mixer.c | 8 +- src/main/flight/mixer.h | 1 - src/main/flight/pid.c | 50 +- src/main/flight/pid.h | 14 +- src/main/io/frsky_osd.c | 11 +- src/main/io/osd_canvas.c | 111 ++- src/main/msc/emfat.c | 735 ++++++++++++++++++ src/main/msc/emfat.h | 113 +++ src/main/msc/emfat_file.c | 526 +++++++++++++ src/main/msc/emfat_file.h | 23 + src/main/msc/usbd_msc_desc.c | 383 +++++++++ src/main/msc/usbd_msc_desc.h | 132 ++++ src/main/msc/usbd_storage.c | 38 + src/main/msc/usbd_storage.h | 46 ++ src/main/msc/usbd_storage_emfat.c | 142 ++++ src/main/msc/usbd_storage_emfat.h | 25 + src/main/msc/usbd_storage_sd_spi.c | 257 ++++++ src/main/msc/usbd_storage_sdio.c | 285 +++++++ src/main/navigation/navigation.c | 22 +- src/main/navigation/navigation.h | 4 +- src/main/rx/rx.c | 4 +- src/main/sensors/gyro.c | 75 +- src/main/sensors/gyro.h | 1 + src/main/target/AIKONF4/target.mk | 2 +- src/main/target/AIRBOTF4/target.mk | 2 +- src/main/target/ALIENFLIGHTNGF7/target.mk | 4 +- src/main/target/ANYFC/target.h | 4 + src/main/target/ANYFCF7/target.mk | 3 +- src/main/target/ANYFCM7/target.mk_ | 2 +- src/main/target/ASGARD32F4/target.mk | 2 +- src/main/target/ASGARD32F7/target.mk | 2 +- src/main/target/BEEROTORF4/target.mk | 4 +- src/main/target/BETAFLIGHTF4/target.mk | 2 +- src/main/target/BLUEJAYF4/target.mk | 4 +- src/main/target/CLRACINGF4AIR/target.h | 4 + src/main/target/COLIBRI/target.mk | 3 +- src/main/target/DALRCF405/target.mk | 3 +- src/main/target/DALRCF722DUAL/target.mk | 2 +- src/main/target/F4BY/target.mk | 4 +- src/main/target/FALCORE/config.c | 3 - src/main/target/FF_F35_LIGHTNING/target.h | 4 + src/main/target/FF_FORTINIF4/target.mk | 4 +- src/main/target/FF_PIKOF4/target.mk | 3 +- src/main/target/FIREWORKSV2/target.mk | 2 +- src/main/target/FISHDRONEF4/target.mk | 2 +- src/main/target/FOXEERF405/target.mk | 2 +- src/main/target/FOXEERF722DUAL/target.mk | 2 +- src/main/target/FRSKYF4/target.mk | 2 +- src/main/target/FURYF4OSD/target.mk | 2 +- src/main/target/IFLIGHTF4_TWING/target.mk | 4 +- src/main/target/IFLIGHTF7_TWING/target.mk | 2 +- src/main/target/KAKUTEF4/target.mk | 2 +- src/main/target/KAKUTEF7/KAKUTEF7HDV.mk | 1 + src/main/target/KAKUTEF7/target.h | 3 + src/main/target/KAKUTEF7/target.mk | 2 +- src/main/target/MAMBAF405US/target.mk | 2 +- src/main/target/MAMBAF722/target.mk | 2 +- src/main/target/MATEKF405/target.mk | 2 +- src/main/target/MATEKF405SE/target.mk | 2 +- src/main/target/MATEKF411/target.h | 4 + src/main/target/MATEKF411SE/target.h | 4 + src/main/target/MATEKF722/target.mk | 3 +- src/main/target/MATEKF722PX/target.mk | 3 +- src/main/target/MATEKF722SE/target.mk | 3 +- src/main/target/MATEKF765/target.mk | 3 +- src/main/target/NOX/target.mk | 3 +- src/main/target/OMNIBUSF4/OMNIBUSF4PRO.mk | 2 +- .../OMNIBUSF4/OMNIBUSF4PRO_LEDSTRIPM5.mk | 2 +- src/main/target/OMNIBUSF4/OMNIBUSF4V3.mk | 2 +- src/main/target/OMNIBUSF4/target.mk | 2 +- src/main/target/OMNIBUSF7/target.mk | 4 +- src/main/target/OMNIBUSF7NXT/target.mk | 2 +- src/main/target/PIXRACER/target.mk | 2 +- src/main/target/REVO/target.mk | 2 +- src/main/target/SPARKY2/target.mk | 2 +- src/main/target/SPEEDYBEEF4/target.mk | 2 +- src/main/target/SPRACINGF4EVO/target.mk | 4 +- src/main/target/SPRACINGF7DUAL/target.mk | 4 +- src/main/target/YUPIF7/target.mk | 2 +- src/main/target/common.h | 4 + src/main/vcp_hal/usbd_conf.h | 3 +- src/main/vcp_hal/usbd_desc.c | 36 + src/main/vcpf4/usbd_conf.h | 23 +- src/main/vcpf4/usbd_desc.h | 15 + 104 files changed, 3896 insertions(+), 291 deletions(-) create mode 100644 docs/USB_Mass_Storage_(MSC)_mode.md create mode 100644 src/main/drivers/sdmmc_sdio.h create mode 100644 src/main/drivers/usb_msc.h create mode 100644 src/main/drivers/usb_msc_f4xx.c create mode 100644 src/main/drivers/usb_msc_f7xx.c create mode 100644 src/main/msc/emfat.c create mode 100644 src/main/msc/emfat.h create mode 100644 src/main/msc/emfat_file.c create mode 100644 src/main/msc/emfat_file.h create mode 100644 src/main/msc/usbd_msc_desc.c create mode 100644 src/main/msc/usbd_msc_desc.h create mode 100644 src/main/msc/usbd_storage.c create mode 100644 src/main/msc/usbd_storage.h create mode 100644 src/main/msc/usbd_storage_emfat.c create mode 100644 src/main/msc/usbd_storage_emfat.h create mode 100644 src/main/msc/usbd_storage_sd_spi.c create mode 100644 src/main/msc/usbd_storage_sdio.c create mode 100644 src/main/target/KAKUTEF7/KAKUTEF7HDV.mk diff --git a/docs/Cli.md b/docs/Cli.md index 11c7fd08fca..57e51070219 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -80,6 +80,7 @@ After restoring it's always a good idea to `dump` or `diff` the settings once ag | `led` | configure leds | | `map` | mapping of rc channel order | | `motor` | get/set motor output value | +| `msc` | Enter USB Mass storage mode. See docs/USB_Mass_Storage_(MSC)_mode.md for usage information.| | `play_sound` | index, or none for next | | `profile` | index (0 to 2) | | `rxrange` | configure rx channel ranges (end-points) | @@ -200,6 +201,8 @@ A shorter form is also supported to enable and disable functions using `serial < | nav_rth_altitude | 1000 | Used in EXTRA, FIXED and AT_LEAST rth alt modes [cm] (Default 1000 means 10 meters) | | nav_rth_home_altitude | 0 | Aircraft will climb/descend to this altitude after reaching home if landing is not enabled. Set to 0 to stay at `nav_rth_altitude` (default) [cm] | | nav_rth_abort_threshold | 50000 | RTH sanity checking feature will notice if distance to home is increasing during RTH and once amount of increase exceeds the threshold defined by this parameter, instead of continuing RTH machine will enter emergency landing, self-level and go down safely. Default is 500m which is safe enough for both multirotor machines and airplanes. [cm] | +| nav_rth_home_offset_distance | 0 | Distance offset from GPS established home to "safe" position used for RTH (cm, 0 disables) | +| nav_rth_home_offset_direction | 0 | Direction offset from GPS established home to "safe" position used for RTH (degrees, 0=N, 90=E, 180=S, 270=W, requires non-zero offset distance) | | nav_mc_bank_angle | 30 | Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude | | nav_mc_hover_thr | 1500 | Multicopter hover throttle hint for altitude controller. Should be set to approximate throttle value when drone is hovering. | | nav_mc_auto_disarm_delay | 2000 | | @@ -421,10 +424,10 @@ A shorter form is also supported to enable and disable functions using `serial < | dyn_notch_width_percent | 8 | Distance in % of the attenuated frequency for double dynamic filter notched. When set to `0` single dynamic notch filter is used | | dyn_notch_range | MEDIUM | Dynamic gyro filter range. Possible values `LOW` `MEDIUM` `HIGH`. `MEDIUM` should work best for 5-6" multirotors. `LOW` should work best with 7" and bigger. `HIGH` should work with everything below 4" | | dyn_notch_q | 120 | Q factor for dynamic notches | -| dyn_notch_min_hz | 150 | Minimum frequency for dynamic notches. Default value of `150` works best with 5" multirors. Should be lowered with increased size of propellers. Values around `100` work fine on 7" drones. 10" can go down to `60` - `70` | +| dyn_notch_min_hz | 150 | Minimum frequency for dynamic notches. Default value of `150` works best with 5" multirors. Should be lowered with increased size of propellers. Values around `100` work fine on 7" drones. 10" can go down to `60` - `70` | | gyro_stage2_lowpass_hz | 0 | Software based second stage lowpass filter for gyro. Value is cutoff frequency (Hz). Currently experimental | -| pidsum_limit | 500 | A limitation to overall amount of correction Flight PID can request on each axis (Roll/Pitch/Yaw). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help | -| yaw_p_limit | 300 | | +| pidsum_limit | 500 | A limitation to overall amount of correction Flight PID can request on each axis (Roll/Pitch). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help | +| pidsum_limit_yaw | 400 | A limitation to overall amount of correction Flight PID can request on each axis (Yaw). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help | | iterm_windup | 50 | Used to prevent Iterm accumulation on during maneuvers. Iterm will be dampened when motors are reaching it's limit (when requested motor correction range is above percentage specified by this parameter) | | rate_accel_limit_roll_pitch | 0 | Limits acceleration of ROLL/PITCH rotation speed that can be requested by stick input. In degrees-per-second-squared. Small and powerful UAV flies great with high acceleration limit ( > 5000 dps^2 and even > 10000 dps^2). Big and heavy multirotors will benefit from low acceleration limit (~ 360 dps^2). When set correctly, it greatly improves stopping performance. Value of 0 disables limiting. | | rate_accel_limit_yaw | 10000 | Limits acceleration of YAW rotation speed that can be requested by stick input. In degrees-per-second-squared. Small and powerful UAV flies great with high acceleration limit ( > 10000 dps^2). Big and heavy multirotors will benefit from low acceleration limit (~ 180 dps^2). When set correctly, it greatly improves stopping performance and general stability during yaw turns. Value of 0 disables limiting. | diff --git a/docs/USB_Mass_Storage_(MSC)_mode.md b/docs/USB_Mass_Storage_(MSC)_mode.md new file mode 100644 index 00000000000..730230db12a --- /dev/null +++ b/docs/USB_Mass_Storage_(MSC)_mode.md @@ -0,0 +1,93 @@ +## Overview + +iNav (after 2.3.0) offers USB MSC (mass storage device class) SD card and internal flash access, meaning you can mount the FC (SD card / internal flash) as an OS file system via USB to read BB logs (and delete them from an SD card). + +When MSC mode is used with **internal flash** there are a few differences compared to **SD card** as it's a virtual file system: + +* The file system is read-only. In order to delete logs it is necessary to erase the flash as usual (configurator, CLI or other tool). +* The logs are presented as a single, consolidated file (`inav_all.bbl`) as well as individual logs (`inav_001.bbl` etc.). +* Other informative files (e.g. `readme.txt`) may also exist in the virtual file system. + + +## Usage + +To put the FC in MSC mode: + +* Enter the CLI +* Enter the CLI command `msc` ; the FC will reboot +* Close the CLI tool (`cliterm`, configurator etc.) +* Wait for the device to be recognised as USB storage device by the operating system (may take some time, 10-15 seconds perhaps). +* Copy files off the MSC mounted FC (sd card) (`cp`, file manager) +* Dismount / eject the FC (sd card) card using the standard OS method +* Power-cycle the FC to exit MSC mode. + +## Performance + +Internal flash is quite fast. + +For an SD card, reading is quite slow, typically c. 340kBs, for example: + +``` +#################### +## Using MSC mode ## +#################### +# FC is automounted to /run/media/jrh/BBOX-QUAD by the OS +$ rsync -P /run/media/jrh/BBOX-QUAD/LOGS/LOG00035.TXT /tmp/msclogs/ +LOG00035.TXT + 55,856,827 100% 339.15kB/s 0:02:40 (xfr#1, to-chk=0/1) +``` + +``` +######################### +## Using a card-reader ## +######################### +# SD Card is automounted to /run/media/jrh/BBOX-QUAD by the OS +$ rsync -P /run/media/jrh/BBOX-QUAD/LOGS/LOG00035.TXT /tmp/sdclogs/ +LOG00035.TXT + 55,856,827 100% 19.26MB/s 0:00:02 (xfr#1, to-chk=0/1) +``` +i.e c. 2.5 seconds for the card reader, 2 minutes 40 seconds for MSC (60 times slower). However, if the card is relatively inaccessible, this is a reasonable trade-off + +## Comparison and Integrity + +The same file (`LOG00035.TXT`, c 55MB) is copied by MSC to `/tmp/msclogs` and directly (SD Card Reader) to `/tmp/sdclogs`. + +``` +$ cmp /tmp/{msc,sdc}logs/LOG00035.TXT +# no differences reported ... +``` + +``` +$ md5sum /tmp/{msc,sdc}logs/LOG00035.TXT +7cd259777ba4f29ecbde2f76882b1840 /tmp/msclogs/LOG00035.TXT +7cd259777ba4f29ecbde2f76882b1840 /tmp/sdclogs/LOG00035.TXT +``` +You should also be able to run blackbox utilities (e.g. the iNav specific `blackbox_decode`) without errors on the files, e.g. + +``` +$ blackbox_decode --stdout --merge-gps > /dev/null /tmp/msclogs/LOG00035.TXT +Log 1 of 1, start 36:00.888, end 62:00.851, duration 25:59.963 + +Statistics +Looptime 1006 avg 9.2 std dev (0.9%) +I frames 48405 104.6 bytes avg 5062215 bytes total +P frames 726064 69.2 bytes avg 50246994 bytes total +H frames 380 10.0 bytes avg 3800 bytes total +G frames 15674 21.4 bytes avg 334701 bytes total +S frames 6198 33.0 bytes avg 204534 bytes total +Frames 774469 71.4 bytes avg 55309209 bytes total +Data rate 496Hz 35806 bytes/s 358100 baud + +3 frames failed to decode, rendering 4 loop iterations unreadable. 4 iterations are missing in total (4ms, 0.00%) +774472 loop iterations weren't logged because of your blackbox_rate settings (779980ms, 50.00%) +``` +## Developer Notes + +Providing MSC for a target requires that the `.mk` file includes in `FEATURES` the key `MSC` and at least one of `ONBOARDFLASH` and /or `SDCARD`. + +For F4 and F7 targets, `USE_USB_MSC` is set unconditionally in `common.h`; if your target does not support blackbox logging to either SD card or internal flash, you should over-ride this in `target.h` +``` +#ifdef USE_USB_MSC +# undef USE_USB_MSC +#endif +``` \ No newline at end of file diff --git a/make/mcu/STM32F4.mk b/make/mcu/STM32F4.mk index 9b529da91a5..02e25480748 100644 --- a/make/mcu/STM32F4.mk +++ b/make/mcu/STM32F4.mk @@ -84,12 +84,23 @@ USBCDC_DIR = $(ROOT)/lib/main/STM32_USB_Device_Library/Class/cdc USBCDC_SRC = $(notdir $(wildcard $(USBCDC_DIR)/src/*.c)) EXCLUDES = usbd_cdc_if_template.c USBCDC_SRC := $(filter-out ${EXCLUDES}, $(USBCDC_SRC)) -VPATH := $(VPATH):$(USBOTG_DIR)/src:$(USBCORE_DIR)/src:$(USBCDC_DIR)/src +USBMSC_DIR = $(ROOT)/lib/main/STM32_USB_Device_Library/Class/msc +USBMSC_SRC = $(notdir $(wildcard $(USBMSC_DIR)/src/*.c)) +EXCLUDES = usbd_storage_template.c +USBMSC_SRC := $(filter-out ${EXCLUDES}, $(USBMSC_SRC)) +USBHID_DIR = $(ROOT)/lib/main/STM32_USB_Device_Library/Class/hid +USBHID_SRC = $(notdir $(wildcard $(USBHID_DIR)/src/*.c)) +USBWRAPPER_DIR = $(ROOT)/lib/main/STM32_USB_Device_Library/Class/hid_cdc_wrapper +USBWRAPPER_SRC = $(notdir $(wildcard $(USBWRAPPER_DIR)/src/*.c)) +VPATH := $(VPATH):$(USBOTG_DIR)/src:$(USBCORE_DIR)/src:$(USBCDC_DIR)/src:$(USBMSC_DIR)/src:$(USBHID_DIR)/src:$(USBWRAPPER_DIR)/src DEVICE_STDPERIPH_SRC := $(STDPERIPH_SRC) \ $(USBOTG_SRC) \ $(USBCORE_SRC) \ - $(USBCDC_SRC) + $(USBCDC_SRC) \ + $(USBHID_SRC) \ + $(USBWRAPPER_SRC) \ + $(USBMSC_SRC) endif #CMSIS @@ -112,6 +123,9 @@ INCLUDE_DIRS := $(INCLUDE_DIRS) \ $(USBOTG_DIR)/inc \ $(USBCORE_DIR)/inc \ $(USBCDC_DIR)/inc \ + $(USBHID_DIR)/inc \ + $(USBWRAPPER_DIR)/inc \ + $(USBMSC_DIR)/inc \ $(USBFS_DIR)/inc \ $(CMSIS_DIR)/Core/Include \ $(ROOT)/lib/main/STM32F4/Drivers/CMSIS/Device/ST/STM32F4xx \ @@ -184,6 +198,30 @@ VCP_SRC = \ drivers/usb_io.c endif +MSC_SRC = \ + drivers/usb_msc_f4xx.c \ + msc/usbd_msc_desc.c \ + msc/usbd_storage.c + +ifneq ($(filter SDCARD,$(FEATURES)),) +MSC_SRC += \ + msc/usbd_storage_sd_spi.c +endif + +ifneq ($(filter SDIO,$(FEATURES)),) +MSC_SRC += \ + msc/usbd_storage_sdio.c +MCU_COMMON_SRC += \ + drivers/sdio_f4xx.c +endif + +ifneq ($(filter ONBOARDFLASH,$(FEATURES)),) +MSC_SRC += \ + msc/usbd_storage_emfat.c \ + msc/emfat.c \ + msc/emfat_file.c +endif + DSP_LIB := $(ROOT)/lib/main/CMSIS/DSP DEVICE_FLAGS += -DARM_MATH_MATRIX_CHECK -DARM_MATH_ROUNDING -D__FPU_PRESENT=1 -DUNALIGNED_SUPPORT_DISABLE -DARM_MATH_CM4 diff --git a/make/mcu/STM32F7.mk b/make/mcu/STM32F7.mk index c23e73bbd36..a7f23aa58d5 100644 --- a/make/mcu/STM32F7.mk +++ b/make/mcu/STM32F7.mk @@ -78,12 +78,25 @@ USBCDC_SRC = $(notdir $(wildcard $(USBCDC_DIR)/Src/*.c)) EXCLUDES = usbd_cdc_if_template.c USBCDC_SRC := $(filter-out ${EXCLUDES}, $(USBCDC_SRC)) -VPATH := $(VPATH):$(USBCDC_DIR)/Src:$(USBCORE_DIR)/Src:$(USBHID_DIR)/Src +USBHID_DIR = $(ROOT)/lib/main/STM32F7/Middlewares/ST/STM32_USB_Device_Library/Class/HID +USBHID_SRC = $(notdir $(wildcard $(USBHID_DIR)/Src/*.c)) + +USBHIDCDC_DIR = $(ROOT)/lib/main/STM32F7/Middlewares/ST/STM32_USB_Device_Library/Class/CDC_HID +USBHIDCDC_SRC = $(notdir $(wildcard $(USBHIDCDC_DIR)/Src/*.c)) + +USBMSC_DIR = $(ROOT)/lib/main/STM32F7/Middlewares/ST/STM32_USB_Device_Library/Class/MSC +USBMSC_SRC = $(notdir $(wildcard $(USBMSC_DIR)/Src/*.c)) +EXCLUDES = usbd_msc_storage_template.c +USBMSC_SRC := $(filter-out ${EXCLUDES}, $(USBMSC_SRC)) + +VPATH := $(VPATH):$(USBCDC_DIR)/Src:$(USBCORE_DIR)/Src:$(USBHID_DIR)/Src:$(USBHIDCDC_DIR)/Src:$(USBMSC_DIR)/Src DEVICE_STDPERIPH_SRC := $(STDPERIPH_SRC) \ $(USBCORE_SRC) \ $(USBCDC_SRC) \ - $(USBHID_SRC) + $(USBHID_SRC) \ + $(USBHIDCDC_SRC) \ + $(USBMSC_SRC) #CMSIS VPATH := $(VPATH):$(CMSIS_DIR)/Include:$(CMSIS_DIR)/Device/ST/STM32F7xx @@ -94,6 +107,8 @@ INCLUDE_DIRS := $(INCLUDE_DIRS) \ $(USBCORE_DIR)/Inc \ $(USBCDC_DIR)/Inc \ $(USBHID_DIR)/Inc \ + $(USBHIDCDC_DIR)/Inc \ + $(USBMSC_DIR)/Inc \ $(CMSIS_DIR)/Core/Include \ $(ROOT)/lib/main/STM32F7/Drivers/CMSIS/Device/ST/STM32F7xx/Include \ $(ROOT)/src/main/vcp_hal @@ -167,5 +182,28 @@ MCU_EXCLUDES = \ drivers/bus_i2c.c \ drivers/serial_uart.c +MSC_SRC = \ + drivers/usb_msc_f7xx.c \ + msc/usbd_storage.c + +ifneq ($(filter SDIO,$(FEATURES)),) +MCU_COMMON_SRC += \ + drivers/sdio_f7xx.c +MSC_SRC += \ + msc/usbd_storage_sdio.c +endif + +ifneq ($(filter SDCARD,$(FEATURES)),) +MSC_SRC += \ + msc/usbd_storage_sd_spi.c +endif + +ifneq ($(filter ONBOARDFLASH,$(FEATURES)),) +MSC_SRC += \ + msc/usbd_storage_emfat.c \ + msc/emfat.c \ + msc/emfat_file.c +endif + DSP_LIB := $(ROOT)/lib/main/CMSIS/DSP DEVICE_FLAGS += -DARM_MATH_MATRIX_CHECK -DARM_MATH_ROUNDING -D__FPU_PRESENT=1 -DUNALIGNED_SUPPORT_DISABLE -DARM_MATH_CM7 diff --git a/make/release.mk b/make/release.mk index 36f85ebc26c..cff7d397186 100644 --- a/make/release.mk +++ b/make/release.mk @@ -6,7 +6,7 @@ RELEASE_TARGETS += ALIENFLIGHTNGF7 RELEASE_TARGETS += BETAFLIGHTF3 BETAFLIGHTF4 RELEASE_TARGETS += FF_F35_LIGHTNING FF_FORTINIF4 -RELEASE_TARGETS += KAKUTEF4 KAKUTEF4V2 KAKUTEF7 KAKUTEF7MINI +RELEASE_TARGETS += KAKUTEF4 KAKUTEF4V2 KAKUTEF7 KAKUTEF7MINI KAKUTEF7HDV RELEASE_TARGETS += SPRACINGF3 SPRACINGF3EVO SPRACINGF3EVO_1SS SPRACINGF3MINI SPRACINGF4EVO SPRACINGF7DUAL @@ -15,7 +15,7 @@ RELEASE_TARGETS += OMNIBUSF4 OMNIBUSF4PRO OMNIBUSF4PRO_LEDSTRIPM5 DYSF4PRO DYSF4 RELEASE_TARGETS += OMNIBUSF4V3 OMNIBUSF4V3_S6_SS OMNIBUSF4V3_S5S6_SS OMNIBUSF4V3_S5_S6_2SS RELEASE_TARGETS += OMNIBUSF7 OMNIBUSF7V2 OMNIBUSF7NXT YUPIF7 -RELEASE_TARGETS += MATEKF405 MATEKF405_SERVOS6 MATEKF405OSD MATEKF722 MATEKF722_HEXSERVO MATEKF722SE MATEKF722MINI MATEKF405SE MATEKF411 MATEKF411_SFTSRL2 MATEKF411_FD_SFTSRL MATEKF411_RSSI MATEKF411SE MATEKF765 +RELEASE_TARGETS += MATEKF405 MATEKF405_SERVOS6 MATEKF405OSD MATEKF722 MATEKF722_HEXSERVO MATEKF722SE MATEKF722MINI MATEKF405SE MATEKF411 MATEKF411_SFTSRL2 MATEKF411_FD_SFTSRL MATEKF411_RSSI MATEKF411SE MATEKF765 MATEKF722PX RELEASE_TARGETS += FOXEERF405 FOXEERF722DUAL diff --git a/make/source.mk b/make/source.mk index bbfbc70ee0d..55d4a9470b8 100644 --- a/make/source.mk +++ b/make/source.mk @@ -235,7 +235,8 @@ TARGET_SRC := $(filter-out $(MCU_EXCLUDES), $(TARGET_SRC)) ifneq ($(filter ONBOARDFLASH,$(FEATURES)),) TARGET_SRC += \ drivers/flash_m25p16.c \ - io/flashfs.c + io/flashfs.c \ + $(MSC_SRC) endif ifneq ($(filter SDCARD,$(FEATURES)),) @@ -245,13 +246,18 @@ TARGET_SRC += \ drivers/sdcard/sdcard_sdio.c \ drivers/sdcard/sdcard_standard.c \ io/asyncfatfs/asyncfatfs.c \ - io/asyncfatfs/fat_standard.c + io/asyncfatfs/fat_standard.c \ + $(MSC_SRC) endif ifneq ($(filter VCP,$(FEATURES)),) TARGET_SRC += $(VCP_SRC) endif +ifneq ($(filter MSC,$(FEATURES)),) +TARGET_SRC += $(MSC_SRC) +endif + ifneq ($(DSP_LIB),) INCLUDE_DIRS += $(DSP_LIB)/Include diff --git a/make/targets.mk b/make/targets.mk index 8b55dc28e60..a284bdbed81 100644 --- a/make/targets.mk +++ b/make/targets.mk @@ -71,7 +71,7 @@ GROUP_4_TARGETS := ANYFC ANYFCF7 ANYFCF7_EXTERNAL_BARO ALIENFLIGHTNGF7 PIXRACER GROUP_5_TARGETS := ASGARD32F7 CLRACINGF4AIRV3 DALRCF405 DALRCF722DUAL DYSF4PROV2 F4BY FISHDRONEF4 FOXEERF405 FOXEERF722DUAL FRSKYF3 FRSKYF4 FURYF3 FURYF3_SPIFLASH FURYF4OSD GROUP_6_TARGETS := MAMBAF405 OMNIBUSF4V3 OMNIBUSF4V3_S6_SS OMNIBUSF4V3_S5S6_SS OMNIBUSF4V3_S5_S6_2SS AIKONF4 GROUP_7_TARGETS := KAKUTEF4 KAKUTEF4V2 KAKUTEF7 KAKUTEF7MINI KFC32F3_INAV MATEKF411_RSSI MATEKF411_SFTSRL2 MATEKF722MINI MATEKF722SE MATEKF722_HEXSERVO -GROUP_8_TARGETS := MATEKF765 +GROUP_8_TARGETS := MATEKF765 MATEKF722PX KAKUTEF7HDV GROUP_OTHER_TARGETS := $(filter-out $(GROUP_1_TARGETS) $(GROUP_2_TARGETS) $(GROUP_3_TARGETS) $(GROUP_4_TARGETS) $(GROUP_5_TARGETS) $(GROUP_6_TARGETS) $(GROUP_7_TARGETS) $(GROUP_8_TARGETS), $(VALID_TARGETS)) ## targets-group-1 : build some targets diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 7d5aaee7985..6e79f43fee5 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1680,7 +1680,6 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE("velPID", "%d,%d,%d", pidBank()->pid[PID_VEL_Z].P, pidBank()->pid[PID_VEL_Z].I, pidBank()->pid[PID_VEL_Z].D); - BLACKBOX_PRINT_HEADER_LINE("yaw_p_limit", "%d", pidProfile()->yaw_p_limit); BLACKBOX_PRINT_HEADER_LINE("yaw_lpf_hz", "%d", pidProfile()->yaw_lpf_hz); BLACKBOX_PRINT_HEADER_LINE("dterm_lpf_hz", "%d", pidProfile()->dterm_lpf_hz); BLACKBOX_PRINT_HEADER_LINE("dterm_notch_hz", "%d", pidProfile()->dterm_soft_notch_hz); @@ -1710,6 +1709,7 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE("gyro_stage2_lowpass_hz", "%d", gyroConfig()->gyro_stage2_lowpass_hz); BLACKBOX_PRINT_HEADER_LINE("dterm_setpoint_weight", "%f", (double)pidProfile()->dterm_setpoint_weight); BLACKBOX_PRINT_HEADER_LINE("pidSumLimit", "%d", pidProfile()->pidSumLimit); + BLACKBOX_PRINT_HEADER_LINE("pidSumLimitYaw", "%d", pidProfile()->pidSumLimitYaw); BLACKBOX_PRINT_HEADER_LINE("axisAccelerationLimitYaw", "%d", pidProfile()->axisAccelerationLimitYaw); BLACKBOX_PRINT_HEADER_LINE("axisAccelerationLimitRollPitch", "%d", pidProfile()->axisAccelerationLimitRollPitch); #ifdef USE_RPM_FILTER diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 690d5d160da..3b02bb19271 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -404,7 +404,7 @@ static const OSD_Entry cmsx_menuFilterPerProfileEntries[] = OSD_SETTING_ENTRY("DTERM LPF", SETTING_DTERM_LPF_HZ), OSD_SETTING_ENTRY("GYRO SLPF", SETTING_GYRO_LPF_HZ), - OSD_SETTING_ENTRY("YAW P LIM", SETTING_YAW_P_LIMIT), + OSD_SETTING_ENTRY("YAW SUM LIM", SETTING_PIDSUM_LIMIT_YAW), OSD_SETTING_ENTRY("YAW LPF", SETTING_YAW_LPF_HZ), OSD_BACK_AND_END_ENTRY, diff --git a/src/main/common/filter.c b/src/main/common/filter.c index a0c19d20f3b..3226e52b3a7 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -36,6 +36,14 @@ float nullFilterApply(void *filter, float input) return input; } +float nullFilterApply4(void *filter, float input, float f_cut, float dt) +{ + UNUSED(filter); + UNUSED(f_cut); + UNUSED(dt); + return input; +} + // PT1 Low Pass filter // f_cut = cutoff frequency @@ -126,31 +134,6 @@ void biquadFilterInitLPF(biquadFilter_t *filter, uint16_t filterFreq, uint32_t s biquadFilterInit(filter, filterFreq, samplingIntervalUs, BIQUAD_Q, FILTER_LPF); } -// ledvinap's proposed RC+FIR2 Biquad-- 1st order IIR, RC filter k -void biquadRCFIR2FilterInit(biquadFilter_t *filter, uint16_t f_cut, uint32_t samplingIntervalUs) -{ - if (f_cut < (1000000 / samplingIntervalUs / 2)) { - const float dT = (float) samplingIntervalUs * 0.000001f; - const float RC = 1.0f / ( 2.0f * M_PIf * f_cut ); - const float k = dT / (RC + dT); - filter->b0 = k / 2; - filter->b1 = k / 2; - filter->b2 = 0; - filter->a1 = -(1 - k); - filter->a2 = 0; - } else { - filter->b0 = 1.0f; - filter->b1 = 0.0f; - filter->b2 = 0.0f; - filter->a1 = 0.0f; - filter->a2 = 0.0f; - } - - // zero initial samples - filter->x1 = filter->x2 = 0; - filter->y1 = filter->y2 = 0; -} - void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samplingIntervalUs, float Q, biquadFilterType_e filterType) { // Check for Nyquist frequency and if it's not possible to initialize filter as requested - set to no filtering at all diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 3ee3d349ebb..5ce181dba67 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -40,8 +40,7 @@ typedef union { typedef enum { FILTER_PT1 = 0, - FILTER_BIQUAD, - FILTER_FIR, + FILTER_BIQUAD } filterType_e; typedef enum { @@ -57,8 +56,10 @@ typedef struct firFilter_s { } firFilter_t; typedef float (*filterApplyFnPtr)(void *filter, float input); +typedef float (*filterApply4FnPtr)(void *filter, float input, float f_cut, float dt); float nullFilterApply(void *filter, float input); +float nullFilterApply4(void *filter, float input, float f_cut, float dt); void pt1FilterInit(pt1Filter_t *filter, float f_cut, float dT); void pt1FilterInitRC(pt1Filter_t *filter, float tau, float dT); @@ -72,8 +73,6 @@ void pt1FilterReset(pt1Filter_t *filter, float input); void rateLimitFilterInit(rateLimitFilter_t *filter); float rateLimitFilterApply4(rateLimitFilter_t *filter, float input, float rate_limit, float dT); -void biquadRCFIR2FilterInit(biquadFilter_t *filter, uint16_t f_cut, uint32_t samplingIntervalUs); - void biquadFilterInitNotch(biquadFilter_t *filter, uint32_t samplingIntervalUs, uint16_t filterFreq, uint16_t cutoffHz); void biquadFilterInitLPF(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samplingIntervalUs); void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samplingIntervalUs, float Q, biquadFilterType_e filterType); diff --git a/src/main/drivers/osd.c b/src/main/drivers/osd.c index 12f9e518401..418758f61ef 100644 --- a/src/main/drivers/osd.c +++ b/src/main/drivers/osd.c @@ -72,8 +72,8 @@ void osdGridBufferClearGridRect(int x, int y, int w, int h) osdGridBufferConstrainRect(&x, &y, &w, &h, OSD_CHARACTER_GRID_MAX_WIDTH, OSD_CHARACTER_GRID_MAX_HEIGHT); int maxX = x + w; int maxY = y + h; - for (int ii = x; ii < maxX + w; ii++) { - for (int jj = y; jj < maxY; jj++) { + for (int ii = x; ii <= maxX + w; ii++) { + for (int jj = y; jj <= maxY; jj++) { *osdCharacterGridBufferGetEntryPtr(ii, jj) = 0; } } diff --git a/src/main/drivers/sdmmc_sdio.h b/src/main/drivers/sdmmc_sdio.h new file mode 100644 index 00000000000..075768189dd --- /dev/null +++ b/src/main/drivers/sdmmc_sdio.h @@ -0,0 +1,241 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software 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 and Betaflight are distributed in the hope that they + * 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 this software. + * + * If not, see . + */ + +/* + * Original author: Alain (https://github.com/aroyer-qc) + * Modified for F4 and BF source: Chris Hockuba (https://github.com/conkerkh) + * + */ + +#ifndef __fatfs_sd_sdio_H__ +#define __fatfs_sd_sdio_H__ + +#include + +#include "platform.h" +#ifdef STM32F4 +#include "stm32f4xx.h" +#endif + +#ifdef STM32F7 +#include "stm32f7xx.h" +#endif + + /* SDCARD pinouts + * + * SD CARD PINS + _________________ + / 1 2 3 4 5 6 7 8 | NR |SDIO INTERFACE + / | |NAME STM32F746 DESCRIPTION +/ 9 | | 4-BIT 1-BIT +| | | +| | 1 |CD/DAT3 PC11 - Connector data line 3 +| | 2 |CMD PD2 PD2 Command/Response line +| | 3 |VSS1 GND GND GND +| SD CARD Pinout | 4 |VDD 3.3V 3.3V 3.3V Power supply +| | 5 |CLK PC12 PC12 Clock +| | 6 |VSS2 GND GND GND +| | 7 |DAT0 PC8 PC8 Connector data line 0 +| | 8 |DAT1 PC9 - Connector data line 1 +|___________________| 9 |DAT2 PC10 - Connector data line 2 + + */ + +/* Define(s) --------------------------------------------------------------------------------------------------------*/ + +//#define MSD_OK ((uint8_t)0x00) +#define MSD_ERROR ((uint8_t)0x01) +#define MSD_ERROR_SD_NOT_PRESENT ((uint8_t)0x02) + +#define SD_PRESENT ((uint8_t)0x01) +#define SD_NOT_PRESENT ((uint8_t)0x00) + +#define SD_DATATIMEOUT ((uint32_t)100000000) + +#define SD_DETECT_GPIO_PORT GPIOC +#define SD_DETECT_PIN GPIO_PIN_13 + +/* Structure(s) -----------------------------------------------------------------------------------------------------*/ + +typedef enum +{ + // SD specific error defines + SD_CMD_CRC_FAIL = (1), // Command response received (but CRC check failed) + SD_DATA_CRC_FAIL = (2), // Data block sent/received (CRC check failed) + SD_CMD_RSP_TIMEOUT = (3), // Command response TimeOut + SD_DATA_TIMEOUT = (4), // Data TimeOut + SD_TX_UNDERRUN = (5), // Transmit FIFO underrun + SD_RX_OVERRUN = (6), // Receive FIFO overrun + SD_START_BIT_ERR = (7), // Start bit not detected on all data signals in wide bus mode + SD_CMD_OUT_OF_RANGE = (8), // Command's argument was out of range. + SD_ADDR_MISALIGNED = (9), // Misaligned address + SD_BLOCK_LEN_ERR = (10), // Transferred block length is not allowed for the card or the number of transferred bytes does not match the block length + SD_ERASE_SEQ_ERR = (11), // An error in the sequence of erase command occurs. + SD_BAD_ERASE_PARAM = (12), // An invalid selection for erase groups + SD_WRITE_PROT_VIOLATION = (13), // Attempt to program a write protect block + SD_LOCK_UNLOCK_FAILED = (14), // Sequence or password error has been detected in unlock command or if there was an attempt to access a locked card + SD_COM_CRC_FAILED = (15), // CRC check of the previous command failed + SD_ILLEGAL_CMD = (16), // Command is not legal for the card state + SD_CARD_ECC_FAILED = (17), // Card internal ECC was applied but failed to correct the data + SD_CC_ERROR = (18), // Internal card controller error + SD_GENERAL_UNKNOWN_ERROR = (19), // General or unknown error + SD_STREAM_READ_UNDERRUN = (20), // The card could not sustain data transfer in stream read operation. + SD_STREAM_WRITE_OVERRUN = (21), // The card could not sustain data programming in stream mode + SD_CID_CSD_OVERWRITE = (22), // CID/CSD overwrite error + SD_WP_ERASE_SKIP = (23), // Only partial address space was erased + SD_CARD_ECC_DISABLED = (24), // Command has been executed without using internal ECC + SD_ERASE_RESET = (25), // Erase sequence was cleared before executing because an out of erase sequence command was received + SD_AKE_SEQ_ERROR = (26), // Error in sequence of authentication. + SD_INVALID_VOLTRANGE = (27), + SD_ADDR_OUT_OF_RANGE = (28), + SD_SWITCH_ERROR = (29), + SD_SDMMC_DISABLED = (30), + SD_SDMMC_FUNCTION_BUSY = (31), + SD_SDMMC_FUNCTION_FAILED = (32), + SD_SDMMC_UNKNOWN_FUNCTION = (33), + SD_OUT_OF_BOUND = (34), + + + // Standard error defines + SD_INTERNAL_ERROR = (35), + SD_NOT_CONFIGURED = (36), + SD_REQUEST_PENDING = (37), + SD_REQUEST_NOT_APPLICABLE = (38), + SD_INVALID_PARAMETER = (39), + SD_UNSUPPORTED_FEATURE = (40), + SD_UNSUPPORTED_HW = (41), + SD_ERROR = (42), + SD_BUSY = (43), + SD_OK = (0) +} SD_Error_t; + + +typedef struct +{ + uint8_t DAT_BUS_WIDTH; // Shows the currently defined data bus width + uint8_t SECURED_MODE; // Card is in secured mode of operation + uint16_t SD_CARD_TYPE; // Carries information about card type + uint32_t SIZE_OF_PROTECTED_AREA; // Carries information about the capacity of protected area + uint8_t SPEED_CLASS; // Carries information about the speed class of the card + uint8_t PERFORMANCE_MOVE; // Carries information about the card's performance move + uint8_t AU_SIZE; // Carries information about the card's allocation unit size + uint16_t ERASE_SIZE; // Determines the number of AUs to be erased in one operation + uint8_t ERASE_TIMEOUT; // Determines the TimeOut for any number of AU erase + uint8_t ERASE_OFFSET; // Carries information about the erase offset +} SD_CardStatus_t; + +typedef struct +{ + uint8_t CSDStruct; // CSD structure + uint8_t SysSpecVersion; // System specification version + uint8_t Reserved1; // Reserved + uint8_t TAAC; // Data read access time 1 + uint8_t NSAC; // Data read access time 2 in CLK cycles + uint8_t MaxBusClkFrec; // Max. bus clock frequency + uint16_t CardComdClasses; // Card command classes + uint8_t RdBlockLen; // Max. read data block length + uint8_t PartBlockRead; // Partial blocks for read allowed + uint8_t WrBlockMisalign; // Write block misalignment + uint8_t RdBlockMisalign; // Read block misalignment + uint8_t DSRImpl; // DSR implemented + uint8_t Reserved2; // Reserved + uint32_t DeviceSize; // Device Size + uint8_t MaxRdCurrentVDDMin; // Max. read current @ VDD min + uint8_t MaxRdCurrentVDDMax; // Max. read current @ VDD max + uint8_t MaxWrCurrentVDDMin; // Max. write current @ VDD min + uint8_t MaxWrCurrentVDDMax; // Max. write current @ VDD max + uint8_t DeviceSizeMul; // Device size multiplier + uint8_t EraseGrSize; // Erase group size + uint8_t EraseGrMul; // Erase group size multiplier + uint8_t WrProtectGrSize; // Write protect group size + uint8_t WrProtectGrEnable; // Write protect group enable + uint8_t ManDeflECC; // Manufacturer default ECC + uint8_t WrSpeedFact; // Write speed factor + uint8_t MaxWrBlockLen; // Max. write data block length + uint8_t WriteBlockPaPartial; // Partial blocks for write allowed + uint8_t Reserved3; // Reserved + uint8_t ContentProtectAppli; // Content protection application + uint8_t FileFormatGrouop; // File format group + uint8_t CopyFlag; // Copy flag (OTP) + uint8_t PermWrProtect; // Permanent write protection + uint8_t TempWrProtect; // Temporary write protection + uint8_t FileFormat; // File format + uint8_t ECC; // ECC code + uint8_t CSD_CRC; // CSD CRC + uint8_t Reserved4; // Always 1 +} SD_CSD_t; + +typedef struct +{ + uint8_t ManufacturerID; // Manufacturer ID + uint16_t OEM_AppliID; // OEM/Application ID + uint32_t ProdName1; // Product Name part1 + uint8_t ProdName2; // Product Name part2 + uint8_t ProdRev; // Product Revision + uint32_t ProdSN; // Product Serial Number + uint8_t Reserved1; // Reserved1 + uint16_t ManufactDate; // Manufacturing Date + uint8_t CID_CRC; // CID CRC + uint8_t Reserved2; // Always 1 + +} SD_CID_t; + +typedef enum +{ + SD_STD_CAPACITY_V1_1 = 0, + SD_STD_CAPACITY_V2_0 = 1, + SD_HIGH_CAPACITY = 2, + SD_MULTIMEDIA = 3, + SD_SECURE_DIGITAL_IO = 4, + SD_HIGH_SPEED_MULTIMEDIA = 5, + SD_SECURE_DIGITAL_IO_COMBO = 6, + SD_HIGH_CAPACITY_MMC = 7, +} SD_CardType_t; + +typedef struct +{ + volatile SD_CSD_t SD_csd; // SD card specific data register + volatile SD_CID_t SD_cid; // SD card identification number register + uint64_t CardCapacity; // Card capacity + uint32_t CardBlockSize; // Card block size +} SD_CardInfo_t; + +/* Prototype(s) -----------------------------------------------------------------------------------------------------*/ + +extern SD_CardInfo_t SD_CardInfo; +extern SD_CardType_t SD_CardType; + +void SD_Initialize_LL (DMA_Stream_TypeDef *dma); +bool SD_Init (void); +bool SD_IsDetected (void); +bool SD_GetState (void); +SD_Error_t SD_GetCardInfo (void); + +SD_Error_t SD_ReadBlocks_DMA (uint64_t ReadAddress, uint32_t *buffer, uint32_t BlockSize, uint32_t NumberOfBlocks); +SD_Error_t SD_CheckRead (void); +SD_Error_t SD_WriteBlocks_DMA (uint64_t WriteAddress, uint32_t *buffer, uint32_t BlockSize, uint32_t NumberOfBlocks); +SD_Error_t SD_CheckWrite (void); + +SD_Error_t SD_Erase (uint64_t StartAddress, uint64_t EndAddress); +SD_Error_t SD_GetCardStatus (SD_CardStatus_t* pCardStatus); + +/* ------------------------------------------------------------------------------------------------------------------*/ + +#endif // __fatfs_sd_sdio_H__ diff --git a/src/main/drivers/usb_msc.h b/src/main/drivers/usb_msc.h new file mode 100644 index 00000000000..3995b3c170b --- /dev/null +++ b/src/main/drivers/usb_msc.h @@ -0,0 +1,33 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software 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 and Betaflight are distributed in the hope that they + * 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 this software. + * + * If not, see . + */ + +/* + * Author: Chris Hockuba (https://github.com/conkerkh) + */ + +#pragma once + +#define MSC_MAGIC 0xDDDD1010 + +void mscInit(void); +bool mscCheckBoot(void); +uint8_t mscStart(void); +bool mscCheckButton(void); +void mscWaitForButton(void); diff --git a/src/main/drivers/usb_msc_f4xx.c b/src/main/drivers/usb_msc_f4xx.c new file mode 100644 index 00000000000..2b9a7f847b8 --- /dev/null +++ b/src/main/drivers/usb_msc_f4xx.c @@ -0,0 +1,159 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software 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 and Betaflight are distributed in the hope that they + * 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 this software. + * + * If not, see . + */ + +/* + * Author: Chris Hockuba (https://github.com/conkerkh) + * + */ + +#include +#include +#include + +#include "platform.h" + +#if defined(USE_USB_MSC) + +#include "build/build_config.h" + +#include "common/utils.h" + +#include "blackbox/blackbox.h" +#include "blackbox/blackbox_io.h" + +#include "drivers/io.h" +#include "drivers/light_led.h" +#include "drivers/nvic.h" +#include "drivers/sdmmc_sdio.h" +#include "drivers/time.h" +#include "drivers/usb_msc.h" + +#if defined(STM32F4) +#include "usb_core.h" +#include "usbd_cdc_vcp.h" +#include "usb_io.h" +#elif defined(STM32F7) +#include "vcp_hal/usbd_cdc_interface.h" +#include "usb_io.h" +USBD_HandleTypeDef USBD_Device; +#else +#include "usb_core.h" +#include "usb_init.h" +#include "hw_config.h" +#endif + +#include "msc/usbd_storage.h" + +#define DEBOUNCE_TIME_MS 20 + +#if defined(MSC_USE_BUTTON) +static IO_t mscButton; +#endif + +void mscInit(void) +{ +#if defined(MSC_USE_BUTTON) + if (usbDevConfig()->mscButtonPin) { + mscButton = IOGetByTag(usbDevConfig()->mscButtonPin); + IOInit(mscButton, OWNER_USB_MSC_PIN, 0); + if (usbDevConfig()->mscButtonUsePullup) { + IOConfigGPIO(mscButton, IOCFG_IPU); + } else { + IOConfigGPIO(mscButton, IOCFG_IPD); + } + } +#endif +} + +uint8_t mscStart(void) +{ + ledInit(false); + + //Start USB + usbGenerateDisconnectPulse(); + + IOInit(IOGetByTag(IO_TAG(PA11)), OWNER_USB, 0, 0); + IOInit(IOGetByTag(IO_TAG(PA12)), OWNER_USB, 0, 0); + + switch (blackboxConfig()->device) { +#ifdef USE_SDCARD + case BLACKBOX_DEVICE_SDCARD: + USBD_STORAGE_fops = &USBD_MSC_MICRO_SDIO_fops; + break; +#endif + +#ifdef USE_FLASHFS + case BLACKBOX_DEVICE_FLASH: + USBD_STORAGE_fops = &USBD_MSC_EMFAT_fops; + break; +#endif + default: + return 1; + } + + USBD_Init(&USB_OTG_dev, USB_OTG_FS_CORE_ID, &MSC_desc, &USBD_MSC_cb, &USR_cb); + + // NVIC configuration for SYSTick + NVIC_DisableIRQ(SysTick_IRQn); + NVIC_SetPriority(SysTick_IRQn, NVIC_BUILD_PRIORITY(0, 0)); + NVIC_EnableIRQ(SysTick_IRQn); + + return 0; +} + +bool mscCheckBoot(void) +{ + if (*((uint32_t *)0x2001FFF0) == MSC_MAGIC) { + return true; + } + return false; +} + +bool mscCheckButton(void) +{ + bool result = false; +#if defined(MSC_USE_BUTTON) + if (mscButton) { + uint8_t state = IORead(mscButton); + if (usbDevConfig()->mscButtonUsePullup) { + result = state == 0; + } else { + result = state == 1; + } + } +#endif + return result; +} + +void mscWaitForButton(void) +{ + // In order to exit MSC mode simply disconnect the board, or push the button again. + while (mscCheckButton()); + delay(DEBOUNCE_TIME_MS); + while (true) { + asm("NOP"); + if (mscCheckButton()) { + *((uint32_t *)0x2001FFF0) = 0xFFFFFFFF; + delay(1); + NVIC_SystemReset(); + } + } +} +#endif diff --git a/src/main/drivers/usb_msc_f7xx.c b/src/main/drivers/usb_msc_f7xx.c new file mode 100644 index 00000000000..bf56fd7564d --- /dev/null +++ b/src/main/drivers/usb_msc_f7xx.c @@ -0,0 +1,158 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software 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 and Betaflight are distributed in the hope that they + * 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 this software. + * + * If not, see . + */ + +/* + * Author: Chris Hockuba (https://github.com/conkerkh) + * + */ + +#include +#include +#include + +#include "platform.h" + +#if defined(USE_USB_MSC) + +#include "build/build_config.h" + +#include "blackbox/blackbox.h" +#include "blackbox/blackbox_io.h" + +#include "common/utils.h" + +#include "drivers/io.h" +#include "drivers/light_led.h" +#include "drivers/nvic.h" +#include "drivers/time.h" +#include "drivers/usb_msc.h" + +#include "vcp_hal/usbd_cdc_interface.h" +#include "usb_io.h" +#include "usbd_msc.h" +#include "msc/usbd_storage.h" + +// declared in drivers/serial_usb_vcp.c for F7 +extern USBD_HandleTypeDef USBD_Device; + +#define DEBOUNCE_TIME_MS 20 + +#if defined(MSC_USE_BUTTON) +static IO_t mscButton; +#endif + +void mscInit(void) +{ +#if defined(MSC_USE_BUTTON) + if (usbDevConfig()->mscButtonPin) { + mscButton = IOGetByTag(usbDevConfig()->mscButtonPin); + IOInit(mscButton, OWNER_USB_MSC_PIN, 0); + if (usbDevConfig()->mscButtonUsePullup) { + IOConfigGPIO(mscButton, IOCFG_IPU); + } else { + IOConfigGPIO(mscButton, IOCFG_IPD); + } + } +#endif +} + +uint8_t mscStart(void) +{ + ledInit(false); + + //Start USB + usbGenerateDisconnectPulse(); + + IOInit(IOGetByTag(IO_TAG(PA11)), OWNER_USB, 0, 0); + IOInit(IOGetByTag(IO_TAG(PA12)), OWNER_USB, 0, 0); + + USBD_Init(&USBD_Device, &VCP_Desc, 0); + + /** Regsiter class */ + USBD_RegisterClass(&USBD_Device, USBD_MSC_CLASS); + + /** Register interface callbacks */ + switch (blackboxConfig()->device) { +#ifdef USE_SDCARD + case BLACKBOX_DEVICE_SDCARD: + USBD_MSC_RegisterStorage(&USBD_Device, &USBD_MSC_MICRO_SDIO_fops); + break; +#endif + +#ifdef USE_FLASHFS + case BLACKBOX_DEVICE_FLASH: + USBD_MSC_RegisterStorage(&USBD_Device, &USBD_MSC_EMFAT_fops); + break; +#endif + + default: + return 1; + } + + USBD_Start(&USBD_Device); + + // NVIC configuration for SYSTick + NVIC_DisableIRQ(SysTick_IRQn); + NVIC_SetPriority(SysTick_IRQn, NVIC_BUILD_PRIORITY(0, 0)); + NVIC_EnableIRQ(SysTick_IRQn); + + return 0; +} + +bool mscCheckBoot(void) +{ + if (*((__IO uint32_t *)BKPSRAM_BASE + 16) == MSC_MAGIC) { + return true; + } + return false; +} + +bool mscCheckButton(void) +{ + bool result = false; +#if defined(MSC_USE_BUTTON) + if (mscButton) { + uint8_t state = IORead(mscButton); + if (usbDevConfig()->mscButtonUsePullup) { + result = state == 0; + } else { + result = state == 1; + } + } +#endif + + return result; +} + +void mscWaitForButton(void) +{ + // In order to exit MSC mode simply disconnect the board, or push the button again. + while (mscCheckButton()); + delay(DEBOUNCE_TIME_MS); + while (true) { + asm("NOP"); + if (mscCheckButton()) { + *((uint32_t *)0x2001FFF0) = 0xFFFFFFFF; + delay(1); + NVIC_SystemReset(); + } + } +} +#endif diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index e5ccef4bfa8..f043800cb01 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -66,6 +66,7 @@ extern uint8_t __config_end; #include "drivers/system.h" #include "drivers/time.h" #include "drivers/timer.h" +#include "drivers/usb_msc.h" #include "fc/fc_core.h" #include "fc/cli.h" @@ -3277,6 +3278,42 @@ static void cliDiff(char *cmdline) printConfig(cmdline, true); } +#ifdef USE_USB_MSC +static void cliMsc(char *cmdline) +{ + UNUSED(cmdline); + + if (false +#ifdef USE_SDCARD + || sdcard_isFunctional() +#endif +#ifdef USE_FLASHFS + || flashfsGetSize() > 0 +#endif + ) { + cliPrintHashLine("restarting in mass storage mode"); + cliPrint("\r\nRebooting"); + bufWriterFlush(cliWriter); + delay(1000); + waitForSerialPortToFinishTransmitting(cliPort); + stopPwmAllMotors(); + +#ifdef STM32F7 + *((__IO uint32_t*) BKPSRAM_BASE + 16) = MSC_MAGIC; +#elif defined(STM32F4) + *((uint32_t *)0x2001FFF0) = MSC_MAGIC; +#endif + + __disable_irq(); + NVIC_SystemReset(); + } else { + cliPrint("\r\nStorage not present or failed to initialize!"); + bufWriterFlush(cliWriter); + } +} +#endif + + typedef struct { const char *name; #ifndef SKIP_CLI_COMMAND_HELP @@ -3358,6 +3395,9 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("memory", "view memory usage", NULL, cliMemory), CLI_COMMAND_DEF("mmix", "custom motor mixer", NULL, cliMotorMix), CLI_COMMAND_DEF("motor", "get/set motor", " []", cliMotor), +#ifdef USE_USB_MSC + CLI_COMMAND_DEF("msc", "switch into msc mode", NULL, cliMsc), +#endif #ifdef PLAY_SOUND CLI_COMMAND_DEF("play_sound", NULL, "[]\r\n", cliPlaySound), #endif diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index e1718b26d38..e2066da47e2 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -76,6 +76,10 @@ #include "drivers/io_pca9685.h" #include "drivers/vtx_rtc6705.h" #include "drivers/vtx_common.h" +#ifdef USE_USB_MSC +#include "drivers/usb_msc.h" +#include "msc/emfat_file.h" +#endif #include "drivers/sdcard/sdcard.h" #include "fc/cli.h" @@ -218,7 +222,7 @@ void init(void) // Latch active features to be used for feature() in the remainder of init(). latchActiveFeatures(); - + ledInit(false); #ifdef USE_EXTI @@ -362,6 +366,33 @@ void init(void) } #endif +#ifdef USE_USB_MSC + /* MSC mode will start after init, but will not allow scheduler to run, + * so there is no bottleneck in reading and writing data + */ + mscInit(); +#if defined(USE_FLASHFS) + // If the blackbox device is onboard flash, then initialize and scan + // it to identify the log files *before* starting the USB device to + // prevent timeouts of the mass storage device. + if (blackboxConfig()->device == BLACKBOX_DEVICE_FLASH) { +#ifdef USE_FLASH_M25P16 + // Must initialise the device to read _anything_ + m25p16_init(0); +#endif + emfat_init_files(); + } +#endif + + if (mscCheckBoot() || mscCheckButton()) { + if (mscStart() == 0) { + mscWaitForButton(); + } else { + NVIC_SystemReset(); + } + } +#endif + #ifdef USE_I2C #ifdef USE_I2C_DEVICE_1 #ifdef I2C_DEVICE_1_SHARES_UART3 diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index ccc95861368..6fed5b14112 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1119,7 +1119,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF case MSP_PID_ADVANCED: sbufWriteU16(dst, 0); // pidProfile()->rollPitchItermIgnoreRate sbufWriteU16(dst, 0); // pidProfile()->yawItermIgnoreRate - sbufWriteU16(dst, pidProfile()->yaw_p_limit); + sbufWriteU16(dst, 0); //pidProfile()->yaw_p_limit sbufWriteU8(dst, 0); //BF: pidProfile()->deltaMethod sbufWriteU8(dst, 0); //BF: pidProfile()->vbatPidCompensation sbufWriteU8(dst, 0); //BF: pidProfile()->setpointRelaxRatio @@ -1141,7 +1141,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU16(dst, 0); //Legacy, no longer in use async processing value sbufWriteU8(dst, pidProfile()->heading_hold_rate_limit); sbufWriteU8(dst, HEADING_HOLD_ERROR_LPF_FREQ); - sbufWriteU16(dst, mixerConfig()->yaw_jump_prevention_limit); + sbufWriteU16(dst, 0); sbufWriteU8(dst, gyroConfig()->gyro_lpf); sbufWriteU8(dst, accelerometerConfig()->acc_lpf_hz); sbufWriteU8(dst, 0); //reserved @@ -1374,7 +1374,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF case MSP2_INAV_MIXER: sbufWriteU8(dst, mixerConfig()->yaw_motor_direction); - sbufWriteU16(dst, mixerConfig()->yaw_jump_prevention_limit); + sbufWriteU16(dst, 0); sbufWriteU8(dst, mixerConfig()->platformType); sbufWriteU8(dst, mixerConfig()->hasFlaps); sbufWriteU16(dst, mixerConfig()->appliedMixerPreset); @@ -2033,7 +2033,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) if (dataSize >= 17) { sbufReadU16(src); // pidProfileMutable()->rollPitchItermIgnoreRate sbufReadU16(src); // pidProfileMutable()->yawItermIgnoreRate - pidProfileMutable()->yaw_p_limit = sbufReadU16(src); + sbufReadU16(src); //pidProfile()->yaw_p_limit sbufReadU8(src); //BF: pidProfileMutable()->deltaMethod sbufReadU8(src); //BF: pidProfileMutable()->vbatPidCompensation @@ -2059,10 +2059,10 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) sbufReadU16(src); //Legacy, no longer in use async processing value pidProfileMutable()->heading_hold_rate_limit = sbufReadU8(src); sbufReadU8(src); //HEADING_HOLD_ERROR_LPF_FREQ - mixerConfigMutable()->yaw_jump_prevention_limit = sbufReadU16(src); + sbufReadU16(src); //Legacy yaw_jump_prevention_limit gyroConfigMutable()->gyro_lpf = sbufReadU8(src); accelerometerConfigMutable()->acc_lpf_hz = sbufReadU8(src); - sbufReadU8(src); //reserved + sbufReadU8(src); //reserved sbufReadU8(src); //reserved sbufReadU8(src); //reserved sbufReadU8(src); //reserved @@ -2704,7 +2704,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) case MSP2_INAV_SET_MIXER: mixerConfigMutable()->yaw_motor_direction = sbufReadU8(src); - mixerConfigMutable()->yaw_jump_prevention_limit = sbufReadU16(src); + sbufReadU16(src); // Was yaw_jump_prevention_limit mixerConfigMutable()->platformType = sbufReadU8(src); mixerConfigMutable()->hasFlaps = sbufReadU8(src); mixerConfigMutable()->appliedMixerPreset = sbufReadU16(src); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 80fa4b320d7..189716b1859 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -80,7 +80,7 @@ tables: - name: debug_modes values: ["NONE", "GYRO", "AGL", "FLOW_RAW", "FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE", - "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "ITERM_RELAX", + "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "ITERM_RELAX", "ERPM", "RPM_FILTER", "RPM_FREQ"] - name: async_mode values: ["NONE", "GYRO", "ALL"] @@ -183,6 +183,9 @@ groups: field: gyro_stage2_lowpass_hz min: 0 max: 500 + - name: gyro_stage2_lowpass_type + field: gyro_stage2_lowpass_type + table: filter_type - name: dyn_notch_width_percent field: dyn_notch_width_percent condition: USE_DYNAMIC_FILTERS @@ -685,9 +688,6 @@ groups: - name: yaw_motor_direction min: -1 max: 1 - - name: yaw_jump_prevention_limit - min: YAW_JUMP_PREVENTION_LIMIT_LOW - max: YAW_JUMP_PREVENTION_LIMIT_HIGH - name: platform_type field: platformType type: uint8_t @@ -1122,9 +1122,10 @@ groups: field: pidSumLimit min: PID_SUM_LIMIT_MIN max: PID_SUM_LIMIT_MAX - - name: yaw_p_limit - min: YAW_P_LIMIT_MIN - max: YAW_P_LIMIT_MAX + - name: pidsum_limit_yaw + field: pidSumLimitYaw + min: PID_SUM_LIMIT_MIN + max: PID_SUM_LIMIT_MAX - name: iterm_windup field: itermWindupPointPercent min: 0 @@ -1479,7 +1480,12 @@ groups: - name: nav_rth_home_altitude field: general.rth_home_altitude max: 65000 - + - name: nav_rth_home_offset_distance + field: general.rth_home_offset_distance + max: 65000 + - name: nav_rth_home_offset_direction + field: general.rth_home_offset_direction + max: 359 - name: nav_mc_bank_angle field: mc.max_bank_angle min: 15 diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 60c8e03f069..a5b4da1c685 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -70,11 +70,10 @@ PG_RESET_TEMPLATE(flight3DConfig_t, flight3DConfig, .neutral3d = 1460 ); -PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 1); +PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 2); PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig, .yaw_motor_direction = 1, - .yaw_jump_prevention_limit = 350, .platformType = PLATFORM_MULTIROTOR, .hasFlaps = false, .appliedMixerPreset = -1, //This flag is not available in CLI and used by Configurator only @@ -306,11 +305,6 @@ void FAST_CODE NOINLINE mixTable(const float dT) input[ROLL] = axisPID[ROLL]; input[PITCH] = axisPID[PITCH]; input[YAW] = axisPID[YAW]; - - if (motorCount >= 4 && mixerConfig()->yaw_jump_prevention_limit < YAW_JUMP_PREVENTION_LIMIT_HIGH) { - // prevent "yaw jump" during yaw correction - input[YAW] = constrain(input[YAW], -mixerConfig()->yaw_jump_prevention_limit - ABS(rcCommand[YAW]), mixerConfig()->yaw_jump_prevention_limit + ABS(rcCommand[YAW])); - } } // Initial mixer concept by bdoiron74 reused and optimized for Air Mode diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 13f27dc91d0..20dae5af11d 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -64,7 +64,6 @@ PG_DECLARE_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer); typedef struct mixerConfig_s { int8_t yaw_motor_direction; - uint16_t yaw_jump_prevention_limit; // make limit configurable (original fixed value was 100) uint8_t platformType; bool hasFlaps; int16_t appliedMixerPreset; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 9d5d09fa7e8..cb208f729b1 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -92,6 +92,8 @@ typedef struct { pt1Filter_t dBoostLpf; biquadFilter_t dBoostGyroLpf; #endif + uint16_t pidSumLimit; + filterApply4FnPtr ptermFilterApplyFn; bool itermLimitActive; } pidState_t; @@ -132,9 +134,7 @@ static EXTENDED_FASTRAM float dBoostFactor; static EXTENDED_FASTRAM float dBoostMaxAtAlleceleration; #endif -static EXTENDED_FASTRAM uint16_t yawPLimit; static EXTENDED_FASTRAM uint8_t yawLpfHz; -static EXTENDED_FASTRAM uint16_t pidSumLimit; static EXTENDED_FASTRAM float motorItermWindupPoint; static EXTENDED_FASTRAM float antiWindupScaler; #ifdef USE_ANTIGRAVITY @@ -227,13 +227,12 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .axisAccelerationLimitYaw = 10000, // dps/s .axisAccelerationLimitRollPitch = 0, // dps/s - .yaw_p_limit = YAW_P_LIMIT_DEFAULT, - .heading_hold_rate_limit = HEADING_HOLD_RATE_LIMIT_DEFAULT, .max_angle_inclination[FD_ROLL] = 300, // 30 degrees .max_angle_inclination[FD_PITCH] = 300, // 30 degrees .pidSumLimit = PID_SUM_LIMIT_DEFAULT, + .pidSumLimitYaw = PID_SUM_LIMIT_YAW_DEFAULT, .fixedWingItermThrowLimit = FW_ITERM_THROW_LIMIT_DEFAULT, .fixedWingReferenceAirspeed = 1000, @@ -563,21 +562,10 @@ bool isFixedWingItermLimitActive(float stickPosition) return fabsf(stickPosition) > pidProfile()->fixedWingItermLimitOnStickPosition; } -static FAST_CODE NOINLINE float pTermProcess(pidState_t *pidState, flight_dynamics_index_t axis, float rateError, float dT) { +static FAST_CODE NOINLINE float pTermProcess(pidState_t *pidState, float rateError, float dT) { float newPTerm = rateError * pidState->kP; - if (axis == FD_YAW) { - // Constrain YAW by yaw_p_limit value if not servo driven (in that case servo limits apply) - if (yawPLimit) { - newPTerm = constrain(newPTerm, -yawPLimit, yawPLimit); - } - - if (yawLpfHz) { - newPTerm = pt1FilterApply4(&pidState->ptermLpfState, newPTerm, yawLpfHz, dT); - } - } - - return newPTerm; + return pidState->ptermFilterApplyFn(&pidState->ptermLpfState, newPTerm, yawLpfHz, dT); } static void applyItermLimiting(pidState_t *pidState) { @@ -597,7 +585,7 @@ static void nullRateController(pidState_t *pidState, flight_dynamics_index_t axi static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT) { const float rateError = pidState->rateTarget - pidState->gyroRate; - const float newPTerm = pTermProcess(pidState, axis, rateError, dT); + const float newPTerm = pTermProcess(pidState, rateError, dT); const float newFFTerm = pidState->rateTarget * pidState->kFF; // Calculate integral @@ -615,7 +603,7 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh } #endif - axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf, -pidSumLimit, +pidSumLimit); + axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf, -pidState->pidSumLimit, +pidState->pidSumLimit); #ifdef USE_BLACKBOX axisPID_P[axis] = newPTerm; @@ -683,7 +671,7 @@ static float applyDBoost(pidState_t *pidState, float dT) { static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT) { const float rateError = pidState->rateTarget - pidState->gyroRate; - const float newPTerm = pTermProcess(pidState, axis, rateError, dT); + const float newPTerm = pTermProcess(pidState, rateError, dT); // Calculate new D-term float newDTerm; @@ -716,7 +704,7 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid // TODO: Get feedback from mixer on available correction range for each axis const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf; - const float newOutputLimited = constrainf(newOutput, -pidSumLimit, +pidSumLimit); + const float newOutputLimited = constrainf(newOutput, -pidState->pidSumLimit, +pidState->pidSumLimit); float itermErrorRate = rateError; applyItermRelax(axis, pidState->gyroRate, pidState->rateTarget, &itermErrorRate); @@ -1033,13 +1021,7 @@ void pidInit(void) itermRelaxType = pidProfile()->iterm_relax_type; itermRelaxSetpointThreshold = MC_ITERM_RELAX_SETPOINT_THRESHOLD * MC_ITERM_RELAX_CUTOFF_DEFAULT / pidProfile()->iterm_relax_cutoff; - if (mixerConfig()->platformType == PLATFORM_MULTIROTOR) { - yawPLimit = pidProfile()->yaw_p_limit; - } else { - yawPLimit = 0; - } yawLpfHz = pidProfile()->yaw_lpf_hz; - pidSumLimit = pidProfile()->pidSumLimit; motorItermWindupPoint = 1.0f - (pidProfile()->itermWindupPointPercent / 100.0f); #ifdef USE_D_BOOST @@ -1052,6 +1034,20 @@ void pidInit(void) antigravityAccelerator = pidProfile()->antigravityAccelerator; #endif + for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) { + if (axis == FD_YAW) { + pidState[axis].pidSumLimit = pidProfile()->pidSumLimitYaw; + if (yawLpfHz) { + pidState[axis].ptermFilterApplyFn = (filterApply4FnPtr) pt1FilterApply4; + } else { + pidState[axis].ptermFilterApplyFn = (filterApply4FnPtr) nullFilterApply4; + } + } else { + pidState[axis].pidSumLimit = pidProfile()->pidSumLimit; + pidState[axis].ptermFilterApplyFn = (filterApply4FnPtr) nullFilterApply4; + } + } + if (pidProfile()->pidControllerType == PID_TYPE_AUTO) { if (mixerConfig()->platformType == PLATFORM_AIRPLANE) { usedPidControllerType = PID_TYPE_PIFF; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 4cf0f1110ce..2ff0e922f4f 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -20,13 +20,11 @@ #include "config/parameter_group.h" #include "fc/runtime_config.h" -#define GYRO_SATURATION_LIMIT 1800 // 1800dps -#define PID_SUM_LIMIT_MIN 100 -#define PID_SUM_LIMIT_MAX 1000 -#define PID_SUM_LIMIT_DEFAULT 500 -#define YAW_P_LIMIT_MIN 100 // Maximum value for yaw P limiter -#define YAW_P_LIMIT_MAX 500 // Maximum value for yaw P limiter -#define YAW_P_LIMIT_DEFAULT 500 // Default value for yaw P limiter +#define GYRO_SATURATION_LIMIT 1800 // 1800dps +#define PID_SUM_LIMIT_MIN 100 +#define PID_SUM_LIMIT_MAX 1000 +#define PID_SUM_LIMIT_DEFAULT 500 +#define PID_SUM_LIMIT_YAW_DEFAULT 350 #define HEADING_HOLD_RATE_LIMIT_MIN 10 #define HEADING_HOLD_RATE_LIMIT_MAX 250 @@ -111,7 +109,6 @@ typedef struct pidProfile_s { uint8_t use_dterm_fir_filter; // Use classical INAV FIR differentiator. Very noise robust, can be quite slowish uint8_t yaw_lpf_hz; - uint16_t yaw_p_limit; uint8_t heading_hold_rate_limit; // Maximum rotation rate HEADING_HOLD mode can feed to yaw rate PID controller @@ -124,6 +121,7 @@ typedef struct pidProfile_s { float dterm_setpoint_weight; uint16_t pidSumLimit; + uint16_t pidSumLimitYaw; // Airplane-specific parameters uint16_t fixedWingItermThrowLimit; diff --git a/src/main/io/frsky_osd.c b/src/main/io/frsky_osd.c index 5a1e435ba48..719476204e9 100644 --- a/src/main/io/frsky_osd.c +++ b/src/main/io/frsky_osd.c @@ -43,8 +43,8 @@ #define FRSKY_OSD_INFO_INTERVAL_MS 1000 #define FRSKY_OSD_TRACE(fmt, ...) -#define FRSKY_OSD_DEBUG(fmt, ...) LOG_D(OSD, fmt, ##__VA_ARGS__) -#define FRSKY_OSD_ERROR(fmt, ...) LOG_E(OSD, fmt, ##__VA_ARGS__) +#define FRSKY_OSD_DEBUG(fmt, ...) LOG_D(OSD, "FrSky OSD: " fmt, ##__VA_ARGS__) +#define FRSKY_OSD_ERROR(fmt, ...) LOG_E(OSD, "FrSky OSD: " fmt, ##__VA_ARGS__) #define FRSKY_OSD_ASSERT(x) typedef enum @@ -389,7 +389,7 @@ static bool frskyOSDHandleCommand(osdCommand_e cmd, const void *payload, size_t } const frskyOSDInfoResponse_t *resp = payload; if (resp->magic[0] != 'A' || resp->magic[1] != 'G' || resp->magic[2] != 'H') { - FRSKY_OSD_ERROR("Invalid magic number %x %x %x, expecting AGH", + FRSKY_OSD_ERROR("invalid magic number %x %x %x, expecting AGH", resp->magic[0], resp->magic[1], resp->magic[2]); return false; } @@ -400,7 +400,7 @@ static bool frskyOSDHandleCommand(osdCommand_e cmd, const void *payload, size_t state.info.viewport.width = resp->pixelWidth; state.info.viewport.height = resp->pixelHeight; if (!state.initialized) { - FRSKY_OSD_DEBUG("FrSky OSD initialized. Version %u.%u.%u, pixels=%ux%u, grid=%ux%u", + FRSKY_OSD_DEBUG("initialized. Version %u.%u.%u, pixels=%ux%u, grid=%ux%u", resp->versionMajor, resp->versionMinor, resp->versionPatch, resp->pixelWidth, resp->pixelHeight, resp->gridColumns, resp->gridRows); state.initialized = true; @@ -525,12 +525,11 @@ static uint8_t frskyOSDEncodeAttr(textAttributes_t attr) bool frskyOSDInit(videoSystem_e videoSystem) { UNUSED(videoSystem); - FRSKY_OSD_TRACE("frskyOSDInit()"); // TODO: Use videoSystem to set the signal standard when // no input is detected. const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_FRSKY_OSD); if (portConfig) { - FRSKY_OSD_TRACE("FrSky OSD configured, trying to connect..."); + FRSKY_OSD_TRACE("configured, trying to connect..."); portOptions_t portOptions = 0; serialPort_t *port = openSerialPort(portConfig->identifier, FUNCTION_FRSKY_OSD, NULL, NULL, FRSKY_OSD_BAUDRATE, diff --git a/src/main/io/osd_canvas.c b/src/main/io/osd_canvas.c index 56793628832..5a4c310a353 100644 --- a/src/main/io/osd_canvas.c +++ b/src/main/io/osd_canvas.c @@ -30,11 +30,12 @@ #if defined(USE_CANVAS) +#define AHI_MIN_DRAW_INTERVAL_MS 100 #define AHI_MAX_DRAW_INTERVAL_MS 1000 #include "common/log.h" #include "common/maths.h" -#include "common/printf.h" +#include "common/typeconversion.h" #include "common/utils.h" #include "drivers/display.h" @@ -125,18 +126,10 @@ void osdCanvasDrawDirArrow(displayPort_t *display, displayCanvas_t *canvas, cons displayCanvasStrokeLineToPoint(canvas, -6, -7); } -static void osdDrawArtificialHorizonLevelLine(displayCanvas_t *canvas, int width, int pos, int margin, bool erase) +static void osdDrawArtificialHorizonLevelLine(displayCanvas_t *canvas, int width, int pos, int margin) { displayCanvasSetLineOutlineType(canvas, DISPLAY_CANVAS_OUTLINE_TYPE_BOTTOM); - if (erase) { - displayCanvasSetStrokeColor(canvas, DISPLAY_CANVAS_COLOR_TRANSPARENT); - displayCanvasSetLineOutlineColor(canvas, DISPLAY_CANVAS_COLOR_TRANSPARENT); - } else { - displayCanvasSetStrokeColor(canvas, DISPLAY_CANVAS_COLOR_WHITE); - displayCanvasSetLineOutlineColor(canvas, DISPLAY_CANVAS_COLOR_BLACK); - } - int yoff = pos >= 0 ? 10 : -10; int yc = -pos - 1; int sz = width / 2; @@ -156,55 +149,59 @@ static void osdDrawArtificialHorizonLevelLine(displayCanvas_t *canvas, int width displayCanvasStrokeLineToPoint(canvas, sz, yc + yoff); } -static void osdDrawArtificialHorizonShapes(displayCanvas_t *canvas, float pitchAngle, float rollAngle, bool erase) +static void osdArtificialHorizonRect(displayCanvas_t *canvas, int *lx, int *ty, int *w, int *h) +{ + *w = (OSD_AHI_WIDTH + 1) * canvas->gridElementWidth; + *h = OSD_AHI_HEIGHT * canvas->gridElementHeight; + + *lx = (canvas->width - *w) / 2; + *ty = (canvas->height - *h) / 2; +} + +static void osdDrawArtificialHorizonShapes(displayCanvas_t *canvas, float pitchAngle, float rollAngle) { int barWidth = (OSD_AHI_WIDTH - 1) * canvas->gridElementWidth; int levelBarWidth = barWidth * (3.0/4); int crosshairMargin = 6; float pixelsPerDegreeLevel = 3.5f; - int maxWidth = (OSD_AHI_WIDTH + 1) * canvas->gridElementWidth; - int maxHeight = OSD_AHI_HEIGHT * canvas->gridElementHeight; int borderSize = 3; char buf[12]; - displayCanvasContextPush(canvas); + int lx; + int ty; + int maxWidth; + int maxHeight; + + osdArtificialHorizonRect(canvas, &lx, &ty, &maxWidth, &maxHeight); - int lx = (canvas->width - maxWidth) / 2; - int ty = (canvas->height - maxHeight) / 2; + displayCanvasContextPush(canvas); - if (!erase) { - int rx = lx + maxWidth; - int by = ty + maxHeight; + int rx = lx + maxWidth; + int by = ty + maxHeight; - displayCanvasSetStrokeColor(canvas, DISPLAY_CANVAS_COLOR_BLACK); + displayCanvasSetStrokeColor(canvas, DISPLAY_CANVAS_COLOR_BLACK); - displayCanvasMoveToPoint(canvas, lx, ty + borderSize); - displayCanvasStrokeLineToPoint(canvas, lx, ty); - displayCanvasStrokeLineToPoint(canvas, lx + borderSize, ty); + displayCanvasMoveToPoint(canvas, lx, ty + borderSize); + displayCanvasStrokeLineToPoint(canvas, lx, ty); + displayCanvasStrokeLineToPoint(canvas, lx + borderSize, ty); - displayCanvasMoveToPoint(canvas, rx, ty + borderSize); - displayCanvasStrokeLineToPoint(canvas, rx, ty); - displayCanvasStrokeLineToPoint(canvas, rx - borderSize, ty); + displayCanvasMoveToPoint(canvas, rx, ty + borderSize); + displayCanvasStrokeLineToPoint(canvas, rx, ty); + displayCanvasStrokeLineToPoint(canvas, rx - borderSize, ty); - displayCanvasMoveToPoint(canvas,lx, by - borderSize); - displayCanvasStrokeLineToPoint(canvas, lx, by); - displayCanvasStrokeLineToPoint(canvas, lx + borderSize, by); + displayCanvasMoveToPoint(canvas,lx, by - borderSize); + displayCanvasStrokeLineToPoint(canvas, lx, by); + displayCanvasStrokeLineToPoint(canvas, lx + borderSize, by); - displayCanvasMoveToPoint(canvas, rx, by - borderSize); - displayCanvasStrokeLineToPoint(canvas, rx, by); - displayCanvasStrokeLineToPoint(canvas, rx - borderSize, by); - } + displayCanvasMoveToPoint(canvas, rx, by - borderSize); + displayCanvasStrokeLineToPoint(canvas, rx, by); + displayCanvasStrokeLineToPoint(canvas, rx - borderSize, by); displayCanvasClipToRect(canvas, lx + 1, ty + 1, maxWidth - 2, maxHeight - 2); osdGridBufferClearPixelRect(canvas, lx, ty, maxWidth, maxHeight); - if (erase) { - displayCanvasSetStrokeAndFillColor(canvas, DISPLAY_CANVAS_COLOR_TRANSPARENT); - displayCanvasSetLineOutlineColor(canvas, DISPLAY_CANVAS_COLOR_TRANSPARENT); - } else { - displayCanvasSetStrokeColor(canvas, DISPLAY_CANVAS_COLOR_WHITE); - displayCanvasSetLineOutlineColor(canvas, DISPLAY_CANVAS_COLOR_BLACK); - } + displayCanvasSetStrokeColor(canvas, DISPLAY_CANVAS_COLOR_WHITE); + displayCanvasSetLineOutlineColor(canvas, DISPLAY_CANVAS_COLOR_BLACK); // The draw just the 5 bars closest to the current pitch level float pitchDegrees = RADIANS_TO_DEGREES(pitchAngle); @@ -215,7 +212,7 @@ static void osdDrawArtificialHorizonShapes(displayCanvas_t *canvas, float pitchA displayCanvasCtmTranslate(canvas, 0, pitchOffset); displayCanvasContextPush(canvas); - displayCanvasCtmRotate(canvas, -rollAngle); + displayCanvasCtmRotate(canvas, rollAngle); displayCanvasCtmTranslate(canvas, translateX, translateY); @@ -231,7 +228,7 @@ static void osdDrawArtificialHorizonShapes(displayCanvas_t *canvas, float pitchA int pos = ii * 10 * pixelsPerDegreeLevel; int margin = (ii > 9 || ii < -9) ? 9 : 6; - osdDrawArtificialHorizonLevelLine(canvas, levelBarWidth, -pos, margin, erase); + osdDrawArtificialHorizonLevelLine(canvas, levelBarWidth, -pos, margin); } displayCanvasContextPop(canvas); @@ -240,8 +237,8 @@ static void osdDrawArtificialHorizonShapes(displayCanvas_t *canvas, float pitchA displayCanvasCtmScale(canvas, 0.5f, 0.5f); // Draw line labels - float sx = sin_approx(-rollAngle); - float sy = cos_approx(rollAngle); + float sx = sin_approx(rollAngle); + float sy = cos_approx(-rollAngle); for (int ii = pitchCenter - 2; ii <= pitchCenter + 2; ii++) { if (ii == 0) { continue; @@ -249,17 +246,13 @@ static void osdDrawArtificialHorizonShapes(displayCanvas_t *canvas, float pitchA int level = ii * 10; int absLevel = ABS(level); - tfp_snprintf(buf, sizeof(buf), "%d", absLevel); + itoa(absLevel, buf, 10); int pos = level * pixelsPerDegreeLevel; int charY = 9 - pos * 2; int cx = (absLevel >= 100 ? -1.5f : -1.0) * canvas->gridElementWidth; int px = cx + (pitchOffset + pos) * sx * 2; int py = -charY - (pitchOffset + pos) * (1 - sy) * 2; - if (erase) { - displayCanvasDrawStringMask(canvas, px, py, buf, DISPLAY_CANVAS_COLOR_TRANSPARENT, 0); - } else { - displayCanvasDrawString(canvas, px, py, buf, 0); - } + displayCanvasDrawString(canvas, px, py, buf, 0); } displayCanvasContextPop(canvas); } @@ -271,19 +264,23 @@ void osdCanvasDrawArtificialHorizon(displayPort_t *display, displayCanvas_t *can static float prevPitchAngle = 9999; static float prevRollAngle = 9999; - static timeMs_t nextDrawMs = 0; + static timeMs_t nextDrawMaxMs = 0; + static timeMs_t nextDrawMinMs = 0; timeMs_t now = millis(); - if (fabsf(prevPitchAngle - pitchAngle) > 0.01f || - fabsf(prevRollAngle - rollAngle) > 0.01f || - now > nextDrawMs) { + float totalError = fabsf(prevPitchAngle - pitchAngle) + fabsf(prevRollAngle - rollAngle); + if ((now > nextDrawMinMs && totalError > 0.05f)|| now > nextDrawMaxMs) { + + int x, y, w, h; + osdArtificialHorizonRect(canvas, &x, &y, &w, &h); + displayCanvasClearRect(canvas, x, y, w, h); - osdDrawArtificialHorizonShapes(canvas, prevPitchAngle, prevRollAngle, true); - osdDrawArtificialHorizonShapes(canvas, pitchAngle, rollAngle, false); + osdDrawArtificialHorizonShapes(canvas, pitchAngle, rollAngle); prevPitchAngle = pitchAngle; prevRollAngle = rollAngle; - nextDrawMs = now + AHI_MAX_DRAW_INTERVAL_MS; + nextDrawMinMs = now + AHI_MIN_DRAW_INTERVAL_MS; + nextDrawMaxMs = now + AHI_MAX_DRAW_INTERVAL_MS; } } diff --git a/src/main/msc/emfat.c b/src/main/msc/emfat.c new file mode 100644 index 00000000000..bcf87ee5fec --- /dev/null +++ b/src/main/msc/emfat.c @@ -0,0 +1,735 @@ +/* + * Derived from + * https://github.com/fetisov/emfat/blob/master/project/emfat.c + * version: 1.1 (2.04.2017) + */ + +/* + * The MIT License (MIT) + * + * Copyright (c) 2015 by Sergey Fetisov + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include "common/utils.h" + +#include "emfat.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define SECT 512 +#define CLUST 4096 +#define SECT_PER_CLUST (CLUST / SECT) +#define SIZE_TO_NSECT(s) ((s) == 0 ? 1 : ((s) + SECT - 1) / SECT) +#define SIZE_TO_NCLUST(s) ((s) == 0 ? 1 : ((s) + CLUST - 1) / CLUST) + +#define CLUST_FREE 0x00000000 +#define CLUST_RESERVED 0x00000001 +#define CLUST_BAD 0x0FFFFFF7 +#define CLUST_ROOT_END 0X0FFFFFF8 +#define CLUST_EOF 0x0FFFFFFF + +#define MAX_DIR_ENTRY_CNT 16 +#define FILE_SYS_TYPE_OFF 82 +#define BYTES_PER_SEC_OFF 11 +#define SEC_PER_CLUS_OFF 13 +#define RES_SEC_CNT_OFF 14 +#define FAT_CNT_OFF 16 +#define TOT_SEC_CNT_OFF 32 +#define SEC_PER_FAT 36 +#define ROOT_DIR_STRT_CLUS_OFF 44 +#define FS_INFOSECTOR_OFF 48 +#define BACKUP_BOOT_SEC_OFF 50 +#define NXT_FREE_CLUS_OFF 492 +#define FILE_SYS_TYPE_LENGTH 8 +#define SHRT_FILE_NAME_LEN 11 +#define STRT_CLUS_LOW_OFF 26 +#define STRT_CLUS_HIGH_OFF 20 +#define FILE_SIZE_OFF 28 +#define ATTR_OFF 11 +#define FILE_STAT_LEN 21 +#define CHECK_SUM_OFF 13 +#define FILE_NAME_SHRT_LEN 8 +#define FILE_NAME_EXTN_LEN 3 +#define LONG_FILE_NAME_LEN 255 +#define LOW_CLUSWORD_MASK 0x0000FFFF +#define HIGH_CLUSWORD_MASK 0xFFFF0000 +#define LONG_FNAME_MASK 0x0F +#define LAST_ORD_FIELD_SEQ 0x40 +#define LFN_END_MARK 0xFFFF +#define LFN_TERM_MARK 0x0000 +#define LFN_FIRST_OFF 0x01 +#define LFN_SIXTH_OFF 0x0E +#define LFN_TWELVETH_OFF 0x1C +#define LFN_FIRST_SET_CNT 5 +#define LFN_SEC_SET_CNT 6 +#define LFN_THIRD_SET_CNT 2 +#define LFN_FIRST_SET_LEN 10 +#define LFN_SEC_SET_LEN 12 +#define LFN_THIRD_SET_LEN 4 +#define LFN_EMPTY_LEN 2 +#define LFN_LEN_PER_ENTRY 13 +#define FNAME_EXTN_SEP_OFF 6 +#define FNAME_SEQ_NUM_OFF 7 +#define BYTES_PER_CLUSTER_ENTRY 4 +#define DIR_ENTRY_LEN 32 +#define VOL_ID_LEN 4 +#define VOL_LABEL_LEN 11 +#define RESERV_LEN 12 +#define FS_VER_LEN 2 +#define OEM_NAME_LEN 8 +#define JUMP_INS_LEN 3 +#define MAX_FAT_CNT 2 +#define SPACE_VAL 32 +#define FILE_READ 0x01 +#define FILE_WRITE 0X02 +#define FILE_CREATE_NEW 0x04 +#define FILE_CREATE_ALWAYS 0x08 +#define FILE_APPEND 0x10 +#define FREE_DIR_ENTRY 0x00 +#define DEL_DIR_ENTRY 0xE5 +#define DOT_DIR_ENTRY 0x2E +#define ASCII_DIFF 32 +#define FILE_SEEK_SET 0 +#define FILE_SEEK_CUR 1 +#define FILE_SEEK_END 2 +#define DELIMITER '/' +#define EXTN_DELIMITER '.' +#define TILDE '~' +#define FULL_SHRT_NAME_LEN 13 + +#pragma pack(push, 1) + +typedef struct +{ + uint8_t status; // 0x80 for bootable, 0x00 for not bootable, anything else for invalid + uint8_t start_head; // The head of the start + uint8_t start_sector; // (S | ((C >> 2) & 0xC0)) where S is the sector of the start and C is the cylinder of the start. Note that S is counted from one. + uint8_t start_cylinder; // (C & 0xFF) where C is the cylinder of the start + uint8_t PartType; + uint8_t end_head; + uint8_t end_sector; + uint8_t end_cylinder; + uint32_t StartLBA; // linear address of first sector in partition. Multiply by sector size (usually 512) for real offset + uint32_t EndLBA; // linear address of last sector in partition. Multiply by sector size (usually 512) for real offset +} mbr_part_t; + +typedef struct +{ + uint8_t Code[440]; + uint32_t DiskSig; //This is optional + uint16_t Reserved; //Usually 0x0000 + mbr_part_t PartTable[4]; + uint8_t BootSignature[2]; //0x55 0xAA for bootable +} mbr_t; + +typedef struct +{ + uint8_t jump[JUMP_INS_LEN]; + uint8_t OEM_name[OEM_NAME_LEN]; + uint16_t bytes_per_sec; + uint8_t sec_per_clus; + uint16_t reserved_sec_cnt; + uint8_t fat_cnt; + uint16_t root_dir_max_cnt; + uint16_t tot_sectors; + uint8_t media_desc; + uint16_t sec_per_fat_fat16; + uint16_t sec_per_track; + uint16_t number_of_heads; + uint32_t hidden_sec_cnt; + uint32_t tol_sector_cnt; + uint32_t sectors_per_fat; + uint16_t ext_flags; + uint8_t fs_version[FS_VER_LEN]; + uint32_t root_dir_strt_cluster; + uint16_t fs_info_sector; + uint16_t backup_boot_sector; + uint8_t reserved[RESERV_LEN]; + uint8_t drive_number; + uint8_t reserved1; + uint8_t boot_sig; + uint8_t volume_id[VOL_ID_LEN]; + uint8_t volume_label[VOL_LABEL_LEN]; + uint8_t file_system_type[FILE_SYS_TYPE_LENGTH]; +} boot_sector; + +typedef struct +{ + uint32_t signature1; /* 0x41615252L */ + uint32_t reserved1[120]; /* Nothing as far as I can tell */ + uint32_t signature2; /* 0x61417272L */ + uint32_t free_clusters; /* Free cluster count. -1 if unknown */ + uint32_t next_cluster; /* Most recently allocated cluster */ + uint32_t reserved2[3]; + uint32_t signature3; +} fsinfo_t; + +typedef struct +{ + uint8_t name[FILE_NAME_SHRT_LEN]; + uint8_t extn[FILE_NAME_EXTN_LEN]; + uint8_t attr; + uint8_t reserved; + uint8_t crt_time_tenth; + uint16_t crt_time; + uint16_t crt_date; + uint16_t lst_access_date; + uint16_t strt_clus_hword; + uint16_t lst_mod_time; + uint16_t lst_mod_date; + uint16_t strt_clus_lword; + uint32_t size; +} dir_entry; + +typedef struct +{ + uint8_t ord_field; + uint8_t fname0_4[LFN_FIRST_SET_LEN]; + uint8_t flag; + uint8_t reserved; + uint8_t chksum; + uint8_t fname6_11[LFN_SEC_SET_LEN]; + uint8_t empty[LFN_EMPTY_LEN]; + uint8_t fname12_13[LFN_THIRD_SET_LEN]; +} lfn_entry; + +#pragma pack(pop) + +bool emfat_init_entries(emfat_entry_t *entries) +{ + emfat_entry_t *e; + int i, n; + + e = &entries[0]; + if (e->level != 0 || !e->dir || e->name == NULL) return false; + + e->priv.top = NULL; + e->priv.next = NULL; + e->priv.sub = NULL; + e->priv.num_subentry = 0; + + n = 0; + for (i = 1; entries[i].name != NULL; i++) { + entries[i].priv.top = NULL; + entries[i].priv.next = NULL; + entries[i].priv.sub = NULL; + entries[i].priv.num_subentry = 0; + if (entries[i].level == n - 1) { + if (n == 0) return false; + e = e->priv.top; + n--; + } + + if (entries[i].level == n + 1) { + if (!e->dir) return false; + e->priv.sub = &entries[i]; + entries[i].priv.top = e; + e = &entries[i]; + n++; + continue; + } + + if (entries[i].level == n) { + if (n == 0) return false; + e->priv.top->priv.num_subentry++; + entries[i].priv.top = e->priv.top; + e->priv.next = &entries[i]; + e = &entries[i]; + continue; + } + + return false; + } + + return true; +} + +static void lba_to_chs(int lba, uint8_t *cl, uint8_t *ch, uint8_t *dh) +{ + int cylinder, head, sector; + int sectors = 63; + int heads = 255; + int cylinders = 1024; + sector = lba % sectors + 1; + head = (lba / sectors) % heads; + cylinder = lba / (sectors * heads); + if (cylinder >= cylinders) { + *cl = *ch = *dh = 0xff; + return; + } + *cl = sector | ((cylinder & 0x300) >> 2); + *ch = cylinder & 0xFF; + *dh = head; +} + +bool emfat_init(emfat_t *emfat, const char *label, emfat_entry_t *entries) +{ + uint32_t sect_per_fat; + uint32_t clust; + uint32_t reserved_clust = 0; + emfat_entry_t *e; + int i; + + if (emfat == NULL || label == NULL || entries == NULL) { + return false; + } + + if (!emfat_init_entries(entries)) { + return false; + } + + clust = 2; + for (i = 0; entries[i].name != NULL; i++) { + e = &entries[i]; + if (e->dir) { + e->curr_size = 0; + e->max_size = 0; + e->priv.first_clust = clust; + e->priv.last_clust = clust + SIZE_TO_NCLUST(e->priv.num_subentry * sizeof(dir_entry)) - 1; + e->priv.last_reserved = e->priv.last_clust; + } else { + e->priv.first_clust = clust; + e->priv.last_clust = e->priv.first_clust + SIZE_TO_NCLUST(entries[i].curr_size) - 1; + e->priv.last_reserved = e->priv.first_clust + SIZE_TO_NCLUST(entries[i].max_size) - 1; + } + reserved_clust += e->priv.last_reserved - e->priv.last_clust; + clust = e->priv.last_reserved + 1; + } + clust -= 2; + + emfat->vol_label = label; + emfat->priv.num_entries = i; + emfat->priv.boot_lba = 62; + emfat->priv.fsinfo_lba = emfat->priv.boot_lba + 1; + emfat->priv.fat1_lba = emfat->priv.fsinfo_lba + 1; + emfat->priv.num_clust = clust; + emfat->priv.free_clust = reserved_clust; + sect_per_fat = SIZE_TO_NSECT((uint64_t)emfat->priv.num_clust * 4); + emfat->priv.fat2_lba = emfat->priv.fat1_lba + sect_per_fat; + emfat->priv.root_lba = emfat->priv.fat2_lba + sect_per_fat; + emfat->priv.entries = entries; + emfat->priv.last_entry = entries; + emfat->disk_sectors = clust * SECT_PER_CLUST + emfat->priv.root_lba; + emfat->vol_size = (uint64_t)emfat->disk_sectors * SECT; + /* calc cyl number */ +// i = ((emfat->disk_sectors + 63*255 - 1) / (63*255)); +// emfat->disk_sectors = i * 63*255; + return true; +} + +void read_mbr_sector(const emfat_t *emfat, uint8_t *sect) +{ + mbr_t *mbr; + memset(sect, 0, SECT); + mbr = (mbr_t *)sect; + mbr->DiskSig = 0; + mbr->Reserved = 0; + mbr->PartTable[0].status = 0x80; + mbr->PartTable[0].PartType = 0x0C; + mbr->PartTable[0].StartLBA = emfat->priv.boot_lba; + mbr->PartTable[0].EndLBA = emfat->disk_sectors; + lba_to_chs(mbr->PartTable[0].StartLBA, &mbr->PartTable[0].start_sector, &mbr->PartTable[0].start_cylinder, &mbr->PartTable[0].start_head); + lba_to_chs(emfat->disk_sectors - 1, &mbr->PartTable[0].end_sector, &mbr->PartTable[0].end_cylinder, &mbr->PartTable[0].end_head); + mbr->BootSignature[0] = 0x55; + mbr->BootSignature[1] = 0xAA; +} + +void read_boot_sector(const emfat_t *emfat, uint8_t *sect) +{ + boot_sector *bs; + memset(sect, 0, SECT); + bs = (boot_sector *)sect; + bs->jump[0] = 0xEB; + bs->jump[1] = 0x58; + bs->jump[2] = 0x90; + memcpy(bs->OEM_name, "MSDOS5.0", 8); + bs->bytes_per_sec = SECT; + bs->sec_per_clus = 8; /* 4 kb per cluster */ + bs->reserved_sec_cnt = 2; /* boot sector & fsinfo sector */ + bs->fat_cnt = 2; /* two tables */ + bs->root_dir_max_cnt = 0; + bs->tot_sectors = 0; + bs->media_desc = 0xF8; + bs->sec_per_fat_fat16 = 0; + bs->sec_per_track = 63; + bs->number_of_heads = 0xFF; + bs->hidden_sec_cnt = 62; + bs->tol_sector_cnt = emfat->disk_sectors - emfat->priv.boot_lba; + bs->sectors_per_fat = emfat->priv.fat2_lba - emfat->priv.fat1_lba; + bs->ext_flags = 0; + bs->fs_version[0] = 0; + bs->fs_version[1] = 0; + bs->root_dir_strt_cluster = 2; + bs->fs_info_sector = 1; + bs->backup_boot_sector = 0; /* not used */ + bs->drive_number = 128; + bs->boot_sig = 0x29; + bs->volume_id[0] = 148; + bs->volume_id[1] = 14; + bs->volume_id[2] = 13; + bs->volume_id[3] = 8; + memcpy(bs->volume_label, "NO NAME ", 12); + memcpy(bs->file_system_type, "FAT32 ", 8); + sect[SECT - 2] = 0x55; + sect[SECT - 1] = 0xAA; +} + +#define IS_CLUST_OF(clust, entry) ((clust) >= (entry)->priv.first_clust && (clust) <= (entry)->priv.last_reserved) + +emfat_entry_t *find_entry(const emfat_t *emfat, uint32_t clust, emfat_entry_t *nearest) +{ + if (nearest == NULL) { + nearest = emfat->priv.entries; + } + + if (nearest->priv.first_clust > clust) { + while (nearest >= emfat->priv.entries) { // backward finding + if (IS_CLUST_OF(clust, nearest)) + return nearest; + nearest--; + } + } else { + while (nearest->name != NULL) { // forward finding + if (IS_CLUST_OF(clust, nearest)) + return nearest; + nearest++; + } + } + return NULL; +} + +void read_fsinfo_sector(const emfat_t *emfat, uint8_t *sect) +{ + UNUSED(emfat); + + fsinfo_t *info = (fsinfo_t *)sect; + info->signature1 = 0x41615252L; + info->signature2 = 0x61417272L; + //info->free_clusters = 0; + info->free_clusters = emfat->priv.free_clust; + //info->next_cluster = emfat->priv.num_clust + 2; + info->next_cluster = 0xffffffff; + memset(info->reserved1, 0, sizeof(info->reserved1)); + memset(info->reserved2, 0, sizeof(info->reserved2)); + info->signature3 = 0xAA550000; +} + +void read_fat_sector(emfat_t *emfat, uint8_t *sect, uint32_t index) +{ + emfat_entry_t *le; + uint32_t *values; + uint32_t count; + uint32_t curr; + + values = (uint32_t *)sect; + curr = index * 128; + count = 128; + + if (curr == 0) { + *values++ = CLUST_ROOT_END; + *values++ = 0xFFFFFFFF; + count -= 2; + curr += 2; + } + + le = emfat->priv.last_entry; + while (count != 0) { + if (!IS_CLUST_OF(curr, le)) { + le = find_entry(emfat, curr, le); + if (le == NULL) { + le = emfat->priv.last_entry; + *values = CLUST_RESERVED; + values++; + count--; + curr++; + continue; + } + } + if (le->dir) { + if (curr == le->priv.last_clust) { + *values = CLUST_EOF; + } else { + *values = curr + 1; + } + } else { + if (curr == le->priv.last_clust) { + *values = CLUST_EOF; + } else if (curr > le->priv.last_clust) { + *values = CLUST_FREE; + } else { + *values = curr + 1; + } + } + values++; + count--; + curr++; + } + emfat->priv.last_entry = le; +} + +void fill_entry(dir_entry *entry, const char *name, uint8_t attr, uint32_t clust, const uint32_t cma[3], uint32_t size) +{ + int i, l, l1, l2; + int dot_pos; + + memset(entry, 0, sizeof(dir_entry)); + + if (cma) { + entry->crt_date = cma[0] >> 16; + entry->crt_time = cma[0] & 0xFFFF; + entry->lst_mod_date = cma[1] >> 16; + entry->lst_mod_time = cma[1] & 0xFFFF; + entry->lst_access_date = cma[2] >> 16; + } + + l = strlen(name); + dot_pos = -1; + + if ((attr & ATTR_DIR) == 0) { + for (i = l - 1; i >= 0; i--) { + if (name[i] == '.') + { + dot_pos = i; + break; + } + } + } + + if (dot_pos == -1) { + l1 = l > FILE_NAME_SHRT_LEN ? FILE_NAME_SHRT_LEN : l; + l2 = 0; + } else { + l1 = dot_pos; + l1 = l1 > FILE_NAME_SHRT_LEN ? FILE_NAME_SHRT_LEN : l1; + l2 = l - dot_pos - 1; + l2 = l2 > FILE_NAME_EXTN_LEN ? FILE_NAME_EXTN_LEN : l2; + } + + memset(entry->name, ' ', FILE_NAME_SHRT_LEN + FILE_NAME_EXTN_LEN); + memcpy(entry->name, name, l1); + memcpy(entry->extn, name + dot_pos + 1, l2); + + for (i = 0; i < FILE_NAME_SHRT_LEN; i++) { + if (entry->name[i] >= 'a' && entry->name[i] <= 'z') { + entry->name[i] -= 0x20; + } + } + + for (i = 0; i < FILE_NAME_EXTN_LEN; i++) { + if (entry->extn[i] >= 'a' && entry->extn[i] <= 'z') { + entry->extn[i] -= 0x20; + } + } + + entry->attr = attr; + entry->reserved = 24; + entry->strt_clus_hword = clust >> 16; + entry->strt_clus_lword = clust; + entry->size = size; + + return; +} + +void fill_dir_sector(emfat_t *emfat, uint8_t *data, emfat_entry_t *entry, uint32_t rel_sect) +{ + dir_entry *de; + uint32_t avail; + + memset(data, 0, SECT); + de = (dir_entry *)data; + avail = SECT; + + if (rel_sect == 0) { // 1. first sector of directory + if (entry->priv.top == NULL) { + fill_entry(de++, emfat->vol_label, ATTR_VOL_LABEL, 0, 0, 0); + avail -= sizeof(dir_entry); + } else { + fill_entry(de++, ".", ATTR_DIR | ATTR_READ, entry->priv.first_clust, 0, 0); + if (entry->priv.top->priv.top == NULL) { + fill_entry(de++, "..", ATTR_DIR | ATTR_READ, 0, 0, 0); + } else { + fill_entry(de++, "..", ATTR_DIR | ATTR_READ, entry->priv.top->priv.first_clust, 0, 0); + } + avail -= sizeof(dir_entry) * 2; + } + entry = entry->priv.sub; + } else { // 2. not a first sector + int n; + n = rel_sect * (SECT / sizeof(dir_entry)); + n -= entry->priv.top == NULL ? 1 : 2; + entry = entry->priv.sub; + + while (n > 0 && entry != NULL) { + entry = entry->priv.next; + n--; + } + } + + while (entry != NULL && avail >= sizeof(dir_entry)) { + if (entry->dir) { + fill_entry(de++, entry->name, ATTR_DIR | ATTR_READ, entry->priv.first_clust, entry->cma_time, 0); + } else { + //fill_entry(de++, entry->name, ATTR_ARCHIVE | ATTR_READ, entry->priv.first_clust, entry->cma_time, entry->curr_size); + fill_entry(de++, entry->name, ATTR_ARCHIVE | ATTR_READ | entry->attr, entry->priv.first_clust, entry->cma_time, entry->curr_size); + } + entry = entry->priv.next; + avail -= sizeof(dir_entry); + } +} + +void read_data_sector(emfat_t *emfat, uint8_t *data, uint32_t rel_sect) +{ + emfat_entry_t *le; + uint32_t cluster; + cluster = rel_sect / 8 + 2; + rel_sect = rel_sect % 8; + + le = emfat->priv.last_entry; + if (!IS_CLUST_OF(cluster, le)) { + le = find_entry(emfat, cluster, le); + if (le == NULL) { + int i; + for (i = 0; i < SECT / 4; i++) + ((uint32_t *)data)[i] = 0xEFBEADDE; + return; + } + emfat->priv.last_entry = le; + } + + if (le->dir) { + fill_dir_sector(emfat, data, le, rel_sect); + return; + } + + if (le->readcb == NULL) { + memset(data, 0, SECT); + } else { + uint32_t offset = cluster - le->priv.first_clust; + offset = offset * CLUST + rel_sect * SECT; + le->readcb(data, SECT, offset + le->offset, le); + } + + return; +} + +void emfat_read(emfat_t *emfat, uint8_t *data, uint32_t sector, int num_sectors) +{ + while (num_sectors > 0) { + if (sector >= emfat->priv.root_lba) { + read_data_sector(emfat, data, sector - emfat->priv.root_lba); + } else if (sector == 0) { + read_mbr_sector(emfat, data); + } else if (sector == emfat->priv.fsinfo_lba) { + read_fsinfo_sector(emfat, data); + } else if (sector == emfat->priv.boot_lba) { + read_boot_sector(emfat, data); + } else if (sector >= emfat->priv.fat1_lba && sector < emfat->priv.fat2_lba) { + read_fat_sector(emfat, data, sector - emfat->priv.fat1_lba); + } else if (sector >= emfat->priv.fat2_lba && sector < emfat->priv.root_lba) { + read_fat_sector(emfat, data, sector - emfat->priv.fat2_lba); + } else { + memset(data, 0, SECT); + } + data += SECT; + num_sectors--; + sector++; + } +} + +void write_data_sector(emfat_t *emfat, const uint8_t *data, uint32_t rel_sect) +{ + emfat_entry_t *le; + uint32_t cluster; + cluster = rel_sect / 8 + 2; + rel_sect = rel_sect % 8; + + le = emfat->priv.last_entry; + + if (!IS_CLUST_OF(cluster, le)) { + le = find_entry(emfat, cluster, le); + if (le == NULL) return; + emfat->priv.last_entry = le; + } + + if (le->dir) { + // TODO: handle changing a filesize + return; + } + + if (le->writecb != NULL) { + le->writecb(data, SECT, rel_sect * SECT + le->offset, le); + } +} + +#define FEBRUARY 2 +#define STARTOFTIME 1970 +#define SECDAY 86400L +#define SECYR (SECDAY * 365) +#define leapyear(year) ((year) % 4 == 0) +#define days_in_year(a) (leapyear(a) ? 366 : 365) +#define days_in_month(a) (month_days[(a) - 1]) + +static int month_days[12] = { + 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 +}; + +uint32_t emfat_cma_time_from_unix(uint32_t tim) +{ + register int i; + register long tmp, day; + int ymd[3]; + int hms[3]; + + day = tim / SECDAY; + tmp = tim % SECDAY; + + /* Hours, minutes, seconds are easy */ + + hms[0] = tmp / 3600; + hms[1] = (tmp % 3600) / 60; + hms[2] = (tmp % 3600) % 60; + + /* Number of years in days */ + for (i = STARTOFTIME; day >= days_in_year(i); i++) + day -= days_in_year(i); + ymd[0] = i; + + /* Number of months in days left */ + if (leapyear(ymd[0])) { + days_in_month(FEBRUARY) = 29; + } + for (i = 1; day >= days_in_month(i); i++) { + day -= days_in_month(i); + } + days_in_month(FEBRUARY) = 28; + ymd[1] = i; + + /* Days are what is left over (+1) from all that. */ + ymd[2] = day + 1; + + return EMFAT_ENCODE_CMA_TIME(ymd[2], ymd[1], ymd[0], hms[0], hms[1], hms[2]); +} + +#ifdef __cplusplus +} +#endif diff --git a/src/main/msc/emfat.h b/src/main/msc/emfat.h new file mode 100644 index 00000000000..ada82fb8cf2 --- /dev/null +++ b/src/main/msc/emfat.h @@ -0,0 +1,113 @@ +/* + * Derived from + * https://github.com/fetisov/emfat/blob/master/project/emfat.c + * version: 1.0 (4.01.2015) + */ + +/* + * The MIT License (MIT) + * + * Copyright (c) 2015 by Sergey Fetisov + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once + +#include +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +struct emfat_entry_s; +typedef void (*emfat_readcb_t)(uint8_t *dest, int size, uint32_t offset, struct emfat_entry_s *entry); +typedef void (*emfat_writecb_t)(const uint8_t *data, int size, uint32_t offset, struct emfat_entry_s *entry); + +typedef struct emfat_entry_s { + const char *name; + bool dir; + uint8_t attr; + int level; + uint32_t offset; + uint32_t curr_size; + uint32_t max_size; + long user_data; + uint32_t cma_time[3]; /**< create/mod/access time in unix format */ + emfat_readcb_t readcb; + emfat_writecb_t writecb; + struct + { + uint32_t first_clust; + uint32_t last_clust; + uint32_t last_reserved; + uint32_t num_subentry; + struct emfat_entry_s *top; + struct emfat_entry_s *sub; + struct emfat_entry_s *next; + } priv; +} emfat_entry_t; + +typedef struct emfat_s { + uint64_t vol_size; + uint32_t disk_sectors; + const char *vol_label; + struct { + uint32_t boot_lba; + uint32_t fsinfo_lba; + uint32_t fat1_lba; + uint32_t fat2_lba; + uint32_t root_lba; + uint32_t num_clust; + uint32_t free_clust; + emfat_entry_t *entries; + emfat_entry_t *last_entry; + int num_entries; + } priv; +} emfat_t; + +bool emfat_init(emfat_t *emfat, const char *label, emfat_entry_t *entries); +void emfat_read(emfat_t *emfat, uint8_t *data, uint32_t sector, int num_sectors); +void emfat_write(emfat_t *emfat, const uint8_t *data, uint32_t sector, int num_sectors); + +#define EMFAT_ENCODE_CMA_TIME(D,M,Y,h,m,s) \ + ((((((Y)-1980) << 9) | ((M) << 5) | (D)) << 16) | \ + (((h) << 11) | ((m) << 5) | (s >> 1))) + +static inline uint32_t emfat_encode_cma_time(int D, int M, int Y, int h, int m, int s) +{ + return EMFAT_ENCODE_CMA_TIME(D,M,Y,h,m,s); +} + +uint32_t emfat_cma_time_from_unix(uint32_t unix_time); + +#define ATTR_READ 0x01 +#define ATTR_HIDDEN 0x02 +#define ATTR_SYSTEM 0x04 +#define ATTR_VOL_LABEL 0x08 +#define ATTR_DIR 0x10 +#define ATTR_ARCHIVE 0x20 +#define ATTR_LONG_FNAME 0x0F + +#ifdef __cplusplus +} +#endif diff --git a/src/main/msc/emfat_file.c b/src/main/msc/emfat_file.c new file mode 100644 index 00000000000..5d3c0d81422 --- /dev/null +++ b/src/main/msc/emfat_file.c @@ -0,0 +1,526 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software 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 and Betaflight are distributed in the hope that they + * 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 this software. + * + * If not, see . + */ + +/* + * Author: jflyper@github.com + */ + +#include "platform.h" +#include "common/utils.h" +#include "common/printf.h" + +#include "emfat.h" +#include "emfat_file.h" +#include "io/flashfs.h" +#include "common/typeconversion.h" + +#define FILESYSTEM_SIZE_MB 256 +#define HDR_BUF_SIZE 32 + +#define USE_EMFAT_AUTORUN +#define USE_EMFAT_ICON +#define USE_EMFAT_README + +#ifdef USE_EMFAT_AUTORUN +static const char autorun_file[] = + "[autorun]\r\n" + "icon=icon.ico\r\n" + "label=iNav Onboard Flash\r\n" ; +#define AUTORUN_SIZE (sizeof(autorun_file) - 1) +#define EMFAT_INCR_AUTORUN 1 +#else +#define EMFAT_INCR_AUTORUN 0 +#endif + +#ifdef USE_EMFAT_README +static const char readme_file[] = + "inav_all.bbl: All log files concatenated\r\n" + "inav_NNN.bbl: Individual log file\r\n\r\n" + "Note that this file system is read-only\r\n"; + +#define README_SIZE (sizeof(readme_file) - 1) +#define EMFAT_INCR_README 1 +#else +#define EMFAT_INCR_README 0 +#endif + +#ifdef USE_EMFAT_ICON +static const unsigned char icon_file[] = +{ + 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x18, 0x18, 0x00, 0x00, 0x01, 0x00, + 0x20, 0x00, 0x88, 0x09, 0x00, 0x00, 0x16, 0x00, 0x00, 0x00, 0x28, 0x00, + 0x00, 0x00, 0x18, 0x00, 0x00, 0x00, 0x30, 0x00, 0x00, 0x00, 0x01, 0x00, + 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x09, 0x00, 0x00, 0x3d, 0x0b, + 0x00, 0x00, 0x3d, 0x0b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0xd2, 0x97, 0x2a, 0x0b, 0xce, 0x91, 0x26, 0x52, 0xce, 0x90, + 0x25, 0x6e, 0xce, 0x90, 0x25, 0x6d, 0xce, 0x90, 0x25, 0x6d, 0xce, 0x90, + 0x25, 0x6d, 0xce, 0x90, 0x25, 0x6d, 0xce, 0x90, 0x25, 0x6d, 0xce, 0x90, + 0x25, 0x6d, 0xce, 0x90, 0x25, 0x6d, 0xce, 0x90, 0x25, 0x6d, 0xce, 0x90, + 0x25, 0x6d, 0xce, 0x90, 0x25, 0x6d, 0xce, 0x90, 0x25, 0x6d, 0xce, 0x90, + 0x25, 0x6d, 0xce, 0x90, 0x25, 0x6d, 0xce, 0x90, 0x25, 0x6d, 0xce, 0x90, + 0x25, 0x6d, 0xce, 0x90, 0x25, 0x6d, 0xce, 0x90, 0x25, 0x6d, 0xce, 0x90, + 0x25, 0x6d, 0xce, 0x90, 0x25, 0x6d, 0xcf, 0x91, 0x26, 0x44, 0xd5, 0x9d, + 0x2e, 0x04, 0xcc, 0x8d, 0x23, 0x5d, 0xcb, 0x8a, 0x21, 0xf0, 0xcb, 0x8a, + 0x20, 0xfd, 0xcb, 0x8a, 0x20, 0xfc, 0xcb, 0x8a, 0x20, 0xfc, 0xcb, 0x8a, + 0x20, 0xfc, 0xcb, 0x8a, 0x20, 0xfc, 0xcb, 0x8a, 0x20, 0xfc, 0xcb, 0x8a, + 0x20, 0xfc, 0xcb, 0x8a, 0x20, 0xfc, 0xcb, 0x8a, 0x20, 0xfc, 0xcb, 0x8a, + 0x20, 0xfc, 0xcb, 0x8a, 0x20, 0xfc, 0xcb, 0x8a, 0x20, 0xfc, 0xcb, 0x8a, + 0x20, 0xfc, 0xcb, 0x8a, 0x20, 0xfc, 0xcb, 0x8a, 0x20, 0xfc, 0xcb, 0x8a, + 0x20, 0xfc, 0xcb, 0x8a, 0x20, 0xfc, 0xcb, 0x8a, 0x20, 0xfc, 0xcb, 0x8a, + 0x20, 0xfc, 0xcb, 0x8a, 0x20, 0xfd, 0xcb, 0x8b, 0x21, 0xe0, 0xcd, 0x8e, + 0x23, 0x37, 0xcb, 0x8b, 0x21, 0x81, 0xcb, 0x8b, 0x21, 0xff, 0xcb, 0x8b, + 0x21, 0xff, 0xcb, 0x8b, 0x21, 0xff, 0xcb, 0x8b, 0x21, 0xff, 0xcb, 0x8b, + 0x21, 0xff, 0xcb, 0x8b, 0x21, 0xff, 0xcb, 0x8b, 0x21, 0xff, 0xcb, 0x8b, + 0x21, 0xff, 0xcb, 0x8b, 0x21, 0xff, 0xcb, 0x8b, 0x21, 0xff, 0xcb, 0x8b, + 0x21, 0xff, 0xcb, 0x8b, 0x21, 0xff, 0xcb, 0x8b, 0x21, 0xff, 0xcb, 0x8b, + 0x21, 0xff, 0xcb, 0x8b, 0x21, 0xff, 0xcb, 0x8b, 0x21, 0xff, 0xcb, 0x8b, + 0x21, 0xff, 0xcb, 0x8b, 0x21, 0xff, 0xcb, 0x8b, 0x21, 0xff, 0xcb, 0x8b, + 0x21, 0xff, 0xcb, 0x8b, 0x21, 0xff, 0xcb, 0x8b, 0x21, 0xf9, 0xcb, 0x8b, + 0x21, 0x53, 0xcd, 0x8d, 0x23, 0x81, 0xcc, 0x8d, 0x23, 0xff, 0xcc, 0x8d, + 0x23, 0xff, 0xcc, 0x8d, 0x23, 0xff, 0xcc, 0x8d, 0x23, 0xff, 0xcc, 0x8d, + 0x23, 0xff, 0xcc, 0x8d, 0x22, 0xff, 0xcc, 0x8c, 0x20, 0xff, 0xcc, 0x8d, + 0x21, 0xff, 0xcc, 0x8d, 0x23, 0xff, 0xcc, 0x8d, 0x23, 0xff, 0xcc, 0x8d, + 0x23, 0xff, 0xcc, 0x8d, 0x23, 0xff, 0xcc, 0x8d, 0x23, 0xff, 0xcc, 0x8d, + 0x23, 0xff, 0xcc, 0x8c, 0x21, 0xff, 0xcc, 0x8c, 0x20, 0xff, 0xcc, 0x8d, + 0x22, 0xff, 0xcc, 0x8d, 0x23, 0xff, 0xcc, 0x8d, 0x23, 0xff, 0xcc, 0x8d, + 0x23, 0xff, 0xcc, 0x8d, 0x23, 0xff, 0xcc, 0x8d, 0x23, 0xf8, 0xcc, 0x8d, + 0x23, 0x53, 0xce, 0x8f, 0x24, 0x81, 0xce, 0x8f, 0x24, 0xff, 0xce, 0x8f, + 0x24, 0xff, 0xce, 0x8f, 0x24, 0xff, 0xce, 0x8f, 0x24, 0xff, 0xcd, 0x8e, + 0x23, 0xff, 0xcf, 0x94, 0x33, 0xff, 0xd3, 0xa0, 0x4c, 0xff, 0xd2, 0x9c, + 0x44, 0xff, 0xcd, 0x90, 0x28, 0xff, 0xcd, 0x8f, 0x23, 0xff, 0xce, 0x8f, + 0x24, 0xff, 0xce, 0x8f, 0x24, 0xff, 0xcd, 0x8f, 0x23, 0xff, 0xcd, 0x90, + 0x28, 0xff, 0xd2, 0x9c, 0x44, 0xff, 0xd3, 0xa0, 0x4c, 0xff, 0xcf, 0x94, + 0x33, 0xff, 0xcd, 0x8e, 0x23, 0xff, 0xce, 0x8f, 0x24, 0xff, 0xce, 0x8f, + 0x24, 0xff, 0xce, 0x8f, 0x24, 0xff, 0xce, 0x8f, 0x24, 0xf8, 0xcd, 0x8f, + 0x24, 0x53, 0xcf, 0x91, 0x26, 0x81, 0xcf, 0x91, 0x26, 0xff, 0xcf, 0x91, + 0x26, 0xff, 0xcf, 0x91, 0x26, 0xff, 0xce, 0x90, 0x24, 0xff, 0xd7, 0xa7, + 0x58, 0xff, 0xe7, 0xca, 0x9e, 0xff, 0xe4, 0xc4, 0x8d, 0xff, 0xe6, 0xc8, + 0x97, 0xff, 0xe2, 0xc1, 0x8b, 0xff, 0xd0, 0x97, 0x34, 0xff, 0xce, 0x91, + 0x25, 0xff, 0xce, 0x91, 0x25, 0xff, 0xd0, 0x97, 0x34, 0xff, 0xe2, 0xc1, + 0x8b, 0xff, 0xe6, 0xc8, 0x97, 0xff, 0xe4, 0xc4, 0x8d, 0xff, 0xe7, 0xca, + 0x9e, 0xff, 0xd7, 0xa7, 0x58, 0xff, 0xce, 0x90, 0x24, 0xff, 0xcf, 0x91, + 0x26, 0xff, 0xcf, 0x91, 0x26, 0xff, 0xcf, 0x91, 0x26, 0xf8, 0xcf, 0x91, + 0x26, 0x53, 0xd0, 0x93, 0x27, 0x81, 0xd0, 0x93, 0x27, 0xff, 0xd0, 0x93, + 0x27, 0xff, 0xcf, 0x93, 0x26, 0xff, 0xd4, 0x9e, 0x3f, 0xff, 0xe9, 0xcd, + 0xa0, 0xff, 0xd7, 0xa3, 0x47, 0xff, 0xcf, 0x93, 0x29, 0xff, 0xd0, 0x95, + 0x2b, 0xff, 0xe1, 0xbb, 0x78, 0xff, 0xe2, 0xbe, 0x82, 0xff, 0xcf, 0x93, + 0x27, 0xff, 0xcf, 0x93, 0x27, 0xff, 0xe2, 0xbe, 0x82, 0xff, 0xe1, 0xbb, + 0x78, 0xff, 0xd0, 0x95, 0x2b, 0xff, 0xcf, 0x93, 0x29, 0xff, 0xd7, 0xa3, + 0x47, 0xff, 0xe9, 0xcd, 0xa0, 0xff, 0xd4, 0x9e, 0x3f, 0xff, 0xcf, 0x92, + 0x26, 0xff, 0xd0, 0x93, 0x27, 0xff, 0xd0, 0x93, 0x27, 0xf8, 0xd0, 0x93, + 0x27, 0x53, 0xd1, 0x95, 0x29, 0x81, 0xd1, 0x95, 0x29, 0xff, 0xd1, 0x95, + 0x29, 0xff, 0xd0, 0x93, 0x25, 0xff, 0xde, 0xb3, 0x68, 0xff, 0xe2, 0xbd, + 0x7c, 0xff, 0xd2, 0x99, 0x32, 0xff, 0xea, 0xd1, 0xaa, 0xff, 0xde, 0xb6, + 0x70, 0xff, 0xd3, 0x9b, 0x35, 0xff, 0xe8, 0xcd, 0x9c, 0xff, 0xd3, 0x9a, + 0x33, 0xff, 0xd3, 0x9a, 0x33, 0xff, 0xe8, 0xcd, 0x9c, 0xff, 0xd3, 0x9b, + 0x35, 0xff, 0xde, 0xb6, 0x70, 0xff, 0xea, 0xd1, 0xaa, 0xff, 0xd2, 0x99, + 0x32, 0xff, 0xe2, 0xbd, 0x7c, 0xff, 0xde, 0xb3, 0x68, 0xff, 0xd0, 0x93, + 0x25, 0xff, 0xd1, 0x95, 0x29, 0xff, 0xd1, 0x95, 0x29, 0xf8, 0xd1, 0x95, + 0x29, 0x53, 0xd2, 0x97, 0x2a, 0x81, 0xd2, 0x97, 0x2a, 0xff, 0xd2, 0x97, + 0x2a, 0xff, 0xd1, 0x95, 0x27, 0xff, 0xdf, 0xb5, 0x69, 0xff, 0xe2, 0xbe, + 0x7d, 0xff, 0xd3, 0x9b, 0x33, 0xff, 0xee, 0xd7, 0xad, 0xff, 0xed, 0xd8, + 0xb4, 0xff, 0xd7, 0xa4, 0x4a, 0xff, 0xe7, 0xca, 0x94, 0xff, 0xd4, 0x9e, + 0x3c, 0xff, 0xd4, 0x9e, 0x3c, 0xff, 0xe7, 0xca, 0x94, 0xff, 0xd7, 0xa4, + 0x4a, 0xff, 0xed, 0xd8, 0xb4, 0xff, 0xee, 0xd7, 0xad, 0xff, 0xd3, 0x9b, + 0x33, 0xff, 0xe2, 0xbe, 0x7d, 0xff, 0xdf, 0xb5, 0x69, 0xff, 0xd1, 0x95, + 0x27, 0xff, 0xd2, 0x97, 0x2a, 0xff, 0xd2, 0x97, 0x2a, 0xf8, 0xd2, 0x97, + 0x2a, 0x53, 0xd3, 0x99, 0x2c, 0x81, 0xd3, 0x99, 0x2c, 0xff, 0xd3, 0x99, + 0x2c, 0xff, 0xd2, 0x98, 0x2a, 0xff, 0xd7, 0xa4, 0x43, 0xff, 0xea, 0xd0, + 0xa2, 0xff, 0xd7, 0xa4, 0x4a, 0xff, 0xd2, 0x98, 0x2d, 0xff, 0xdf, 0xb6, + 0x6a, 0xff, 0xe8, 0xcc, 0x9c, 0xff, 0xdb, 0xad, 0x5a, 0xff, 0xe7, 0xcb, + 0x9c, 0xff, 0xe7, 0xcb, 0x9c, 0xff, 0xdb, 0xad, 0x5a, 0xff, 0xe8, 0xcc, + 0x9c, 0xff, 0xdf, 0xb6, 0x6a, 0xff, 0xd2, 0x98, 0x2d, 0xff, 0xd7, 0xa5, + 0x4a, 0xff, 0xea, 0xd0, 0xa2, 0xff, 0xd7, 0xa4, 0x43, 0xff, 0xd2, 0x98, + 0x2a, 0xff, 0xd3, 0x99, 0x2c, 0xff, 0xd3, 0x99, 0x2c, 0xf8, 0xd3, 0x99, + 0x2c, 0x53, 0xd4, 0x9a, 0x2d, 0x81, 0xd4, 0x9b, 0x2d, 0xff, 0xd4, 0x9b, + 0x2d, 0xff, 0xd4, 0x9a, 0x2d, 0xff, 0xd3, 0x9a, 0x2c, 0xff, 0xde, 0xb2, + 0x5d, 0xff, 0xe9, 0xcf, 0xa1, 0xff, 0xe5, 0xc6, 0x93, 0xff, 0xe0, 0xba, + 0x77, 0xff, 0xdb, 0xac, 0x53, 0xff, 0xe2, 0xbe, 0x7d, 0xff, 0xfd, 0xfc, + 0xfa, 0xff, 0xfd, 0xfc, 0xfa, 0xff, 0xe2, 0xbe, 0x7d, 0xff, 0xdb, 0xac, + 0x53, 0xff, 0xe0, 0xba, 0x77, 0xff, 0xe5, 0xc6, 0x93, 0xff, 0xe9, 0xcf, + 0xa1, 0xff, 0xde, 0xb2, 0x5d, 0xff, 0xd3, 0x9a, 0x2c, 0xff, 0xd4, 0x9b, + 0x2d, 0xff, 0xd4, 0x9b, 0x2d, 0xff, 0xd4, 0x9b, 0x2d, 0xf8, 0xd4, 0x9b, + 0x2d, 0x53, 0xd5, 0x9c, 0x2e, 0x81, 0xd5, 0x9c, 0x2e, 0xff, 0xd5, 0x9c, + 0x2e, 0xff, 0xd5, 0x9c, 0x2e, 0xff, 0xd5, 0x9c, 0x2e, 0xff, 0xd4, 0x9b, + 0x2d, 0xff, 0xd8, 0xa3, 0x3c, 0xff, 0xdd, 0xaf, 0x55, 0xff, 0xda, 0xa9, + 0x49, 0xff, 0xd7, 0xa2, 0x3e, 0xff, 0xf3, 0xe5, 0xcd, 0xff, 0xf6, 0xea, + 0xd1, 0xff, 0xf6, 0xea, 0xd1, 0xff, 0xf3, 0xe5, 0xcd, 0xff, 0xd7, 0xa2, + 0x3e, 0xff, 0xda, 0xa9, 0x49, 0xff, 0xdd, 0xaf, 0x55, 0xff, 0xd8, 0xa3, + 0x3c, 0xff, 0xd4, 0x9b, 0x2d, 0xff, 0xd5, 0x9c, 0x2e, 0xff, 0xd5, 0x9c, + 0x2e, 0xff, 0xd5, 0x9c, 0x2e, 0xff, 0xd5, 0x9c, 0x2e, 0xf8, 0xd5, 0x9c, + 0x2e, 0x53, 0xd6, 0x9e, 0x2f, 0x81, 0xd6, 0x9e, 0x2f, 0xff, 0xd6, 0x9e, + 0x2f, 0xff, 0xd6, 0x9e, 0x2f, 0xff, 0xd6, 0x9e, 0x2f, 0xff, 0xd6, 0x9e, + 0x2f, 0xff, 0xd5, 0x9d, 0x2d, 0xff, 0xd4, 0x9b, 0x2a, 0xff, 0xd4, 0x9b, + 0x28, 0xff, 0xe0, 0xb6, 0x66, 0xff, 0xf2, 0xe1, 0xc1, 0xff, 0xd9, 0xa6, + 0x40, 0xff, 0xd9, 0xa6, 0x40, 0xff, 0xf2, 0xe1, 0xc1, 0xff, 0xe0, 0xb6, + 0x66, 0xff, 0xd4, 0x9b, 0x28, 0xff, 0xd4, 0x9b, 0x2a, 0xff, 0xd5, 0x9d, + 0x2d, 0xff, 0xd6, 0x9e, 0x2f, 0xff, 0xd6, 0x9e, 0x2f, 0xff, 0xd6, 0x9e, + 0x2f, 0xff, 0xd6, 0x9e, 0x2f, 0xff, 0xd6, 0x9e, 0x2f, 0xf8, 0xd6, 0x9e, + 0x2f, 0x53, 0xd7, 0xa0, 0x31, 0x81, 0xd7, 0xa0, 0x31, 0xff, 0xd7, 0xa0, + 0x31, 0xff, 0xd7, 0xa0, 0x31, 0xff, 0xd6, 0xa0, 0x30, 0xff, 0xd6, 0x9f, + 0x31, 0xff, 0xda, 0xac, 0x54, 0xff, 0xe0, 0xba, 0x75, 0xff, 0xdc, 0xb1, + 0x5f, 0xff, 0xe1, 0xb9, 0x69, 0xff, 0xf0, 0xdd, 0xba, 0xff, 0xd6, 0xa1, + 0x37, 0xff, 0xd6, 0xa1, 0x37, 0xff, 0xf0, 0xdd, 0xba, 0xff, 0xe1, 0xb9, + 0x69, 0xff, 0xdc, 0xb1, 0x5f, 0xff, 0xe0, 0xba, 0x75, 0xff, 0xda, 0xac, + 0x54, 0xff, 0xd6, 0x9f, 0x31, 0xff, 0xd6, 0xa0, 0x30, 0xff, 0xd6, 0xa0, + 0x31, 0xff, 0xd6, 0x9f, 0x31, 0xff, 0xd7, 0xa0, 0x31, 0xf8, 0xd7, 0xa0, + 0x31, 0x53, 0xd7, 0xa1, 0x32, 0x81, 0xd7, 0xa1, 0x32, 0xff, 0xd7, 0xa1, + 0x32, 0xff, 0xd7, 0xa1, 0x32, 0xff, 0xd7, 0xa1, 0x32, 0xff, 0xe2, 0xbe, + 0x79, 0xff, 0xeb, 0xd1, 0xa0, 0xff, 0xe5, 0xc2, 0x79, 0xff, 0xe1, 0xb9, + 0x6a, 0xff, 0xe2, 0xbf, 0x7c, 0xff, 0xf0, 0xdd, 0xba, 0xff, 0xed, 0xd8, + 0xb3, 0xff, 0xed, 0xd8, 0xb3, 0xff, 0xf0, 0xdd, 0xba, 0xff, 0xe2, 0xbf, + 0x7c, 0xff, 0xe1, 0xb9, 0x6a, 0xff, 0xe5, 0xc1, 0x79, 0xff, 0xeb, 0xd1, + 0xa0, 0xff, 0xe2, 0xbe, 0x79, 0xff, 0xd7, 0xa1, 0x32, 0xff, 0xd7, 0xa1, + 0x32, 0xff, 0xd7, 0xa1, 0x32, 0xff, 0xd7, 0xa1, 0x32, 0xf8, 0xd7, 0xa1, + 0x32, 0x53, 0xd8, 0xa2, 0x33, 0x81, 0xd8, 0xa2, 0x33, 0xff, 0xd8, 0xa2, + 0x33, 0xff, 0xd8, 0xa2, 0x31, 0xff, 0xdd, 0xb0, 0x53, 0xff, 0xec, 0xd3, + 0xa1, 0xff, 0xda, 0xa8, 0x3f, 0xff, 0xd8, 0xa5, 0x40, 0xff, 0xe5, 0xc4, + 0x87, 0xff, 0xe9, 0xcd, 0x94, 0xff, 0xe3, 0xbf, 0x78, 0xff, 0xe4, 0xc0, + 0x75, 0xff, 0xe4, 0xc0, 0x75, 0xff, 0xe3, 0xbe, 0x78, 0xff, 0xe9, 0xcd, + 0x94, 0xff, 0xe5, 0xc4, 0x87, 0xff, 0xd8, 0xa5, 0x40, 0xff, 0xda, 0xa8, + 0x3f, 0xff, 0xec, 0xd3, 0xa1, 0xff, 0xdd, 0xb0, 0x53, 0xff, 0xd8, 0xa2, + 0x31, 0xff, 0xd8, 0xa2, 0x33, 0xff, 0xd8, 0xa2, 0x33, 0xf8, 0xd8, 0xa2, + 0x33, 0x53, 0xd9, 0xa4, 0x34, 0x81, 0xd9, 0xa4, 0x34, 0xff, 0xd9, 0xa4, + 0x34, 0xff, 0xd8, 0xa2, 0x30, 0xff, 0xe4, 0xbf, 0x73, 0xff, 0xe6, 0xc4, + 0x7d, 0xff, 0xda, 0xa9, 0x40, 0xff, 0xf3, 0xe5, 0xcc, 0xff, 0xf0, 0xdc, + 0xb3, 0xff, 0xdc, 0xab, 0x45, 0xff, 0xeb, 0xd2, 0x9e, 0xff, 0xda, 0xa8, + 0x3d, 0xff, 0xda, 0xa8, 0x3d, 0xff, 0xeb, 0xd2, 0x9e, 0xff, 0xdc, 0xab, + 0x45, 0xff, 0xf0, 0xdc, 0xb3, 0xff, 0xf3, 0xe5, 0xcc, 0xff, 0xda, 0xa9, + 0x40, 0xff, 0xe6, 0xc4, 0x7d, 0xff, 0xe4, 0xc0, 0x73, 0xff, 0xd8, 0xa2, + 0x30, 0xff, 0xd9, 0xa4, 0x34, 0xff, 0xd9, 0xa4, 0x34, 0xf8, 0xd9, 0xa4, + 0x34, 0x53, 0xd9, 0xa5, 0x35, 0x81, 0xd9, 0xa5, 0x35, 0xff, 0xd9, 0xa5, + 0x35, 0xff, 0xd9, 0xa4, 0x32, 0xff, 0xe3, 0xbc, 0x6a, 0xff, 0xe8, 0xc9, + 0x8b, 0xff, 0xda, 0xa7, 0x39, 0xff, 0xe9, 0xcb, 0x8a, 0xff, 0xe2, 0xb9, + 0x60, 0xff, 0xdc, 0xac, 0x47, 0xff, 0xec, 0xd3, 0xa1, 0xff, 0xdb, 0xa9, + 0x3c, 0xff, 0xdb, 0xa8, 0x3d, 0xff, 0xec, 0xd3, 0xa1, 0xff, 0xdc, 0xac, + 0x47, 0xff, 0xe2, 0xb9, 0x60, 0xff, 0xe9, 0xcb, 0x8a, 0xff, 0xda, 0xa7, + 0x39, 0xff, 0xe8, 0xc9, 0x8c, 0xff, 0xe3, 0xbc, 0x6a, 0xff, 0xd9, 0xa4, + 0x32, 0xff, 0xd9, 0xa5, 0x35, 0xff, 0xd9, 0xa5, 0x35, 0xf8, 0xd9, 0xa5, + 0x35, 0x53, 0xda, 0xa6, 0x35, 0x81, 0xda, 0xa6, 0x36, 0xff, 0xda, 0xa6, + 0x35, 0xff, 0xda, 0xa6, 0x35, 0xff, 0xdc, 0xac, 0x42, 0xff, 0xec, 0xd4, + 0xa2, 0xff, 0xe1, 0xb9, 0x6b, 0xff, 0xd9, 0xa6, 0x3c, 0xff, 0xdb, 0xaa, + 0x47, 0xff, 0xe9, 0xcc, 0x95, 0xff, 0xe6, 0xc4, 0x7a, 0xff, 0xda, 0xa5, + 0x34, 0xff, 0xda, 0xa5, 0x34, 0xff, 0xe6, 0xc4, 0x79, 0xff, 0xe9, 0xcc, + 0x95, 0xff, 0xdb, 0xaa, 0x47, 0xff, 0xd9, 0xa6, 0x3c, 0xff, 0xe1, 0xb9, + 0x6b, 0xff, 0xec, 0xd4, 0xa2, 0xff, 0xdc, 0xac, 0x42, 0xff, 0xda, 0xa6, + 0x35, 0xff, 0xda, 0xa6, 0x35, 0xff, 0xda, 0xa6, 0x36, 0xf8, 0xda, 0xa6, + 0x35, 0x53, 0xdb, 0xa7, 0x36, 0x81, 0xdb, 0xa7, 0x36, 0xff, 0xdb, 0xa7, + 0x36, 0xff, 0xdb, 0xa7, 0x36, 0xff, 0xda, 0xa7, 0x35, 0xff, 0xdf, 0xb2, + 0x50, 0xff, 0xeb, 0xd0, 0x97, 0xff, 0xeb, 0xd2, 0xa1, 0xff, 0xec, 0xd3, + 0xa2, 0xff, 0xe7, 0xc5, 0x7b, 0xff, 0xdb, 0xa9, 0x3a, 0xff, 0xdb, 0xa7, + 0x36, 0xff, 0xdb, 0xa7, 0x36, 0xff, 0xdb, 0xa9, 0x3a, 0xff, 0xe7, 0xc5, + 0x7b, 0xff, 0xec, 0xd3, 0xa2, 0xff, 0xeb, 0xd2, 0xa1, 0xff, 0xeb, 0xd0, + 0x97, 0xff, 0xdf, 0xb2, 0x50, 0xff, 0xda, 0xa7, 0x35, 0xff, 0xdb, 0xa7, + 0x36, 0xff, 0xdb, 0xa7, 0x36, 0xff, 0xdb, 0xa7, 0x36, 0xf8, 0xdb, 0xa7, + 0x36, 0x53, 0xdb, 0xa8, 0x37, 0x81, 0xdb, 0xa8, 0x37, 0xff, 0xdb, 0xa8, + 0x37, 0xff, 0xdb, 0xa8, 0x37, 0xff, 0xdb, 0xa8, 0x37, 0xff, 0xdb, 0xa7, + 0x36, 0xff, 0xdb, 0xa9, 0x39, 0xff, 0xde, 0xae, 0x44, 0xff, 0xdd, 0xac, + 0x40, 0xff, 0xdb, 0xa7, 0x35, 0xff, 0xdb, 0xa8, 0x37, 0xff, 0xdb, 0xa8, + 0x37, 0xff, 0xdb, 0xa8, 0x37, 0xff, 0xdb, 0xa8, 0x37, 0xff, 0xdb, 0xa7, + 0x35, 0xff, 0xdd, 0xac, 0x40, 0xff, 0xde, 0xae, 0x44, 0xff, 0xdb, 0xa9, + 0x39, 0xff, 0xdb, 0xa7, 0x36, 0xff, 0xdb, 0xa8, 0x37, 0xff, 0xdb, 0xa8, + 0x37, 0xff, 0xdb, 0xa8, 0x37, 0xff, 0xdb, 0xa8, 0x37, 0xf8, 0xdb, 0xa8, + 0x37, 0x53, 0xdb, 0xa8, 0x37, 0x82, 0xdb, 0xa8, 0x37, 0xff, 0xdb, 0xa8, + 0x37, 0xff, 0xdb, 0xa8, 0x37, 0xff, 0xdb, 0xa8, 0x37, 0xff, 0xdb, 0xa8, + 0x37, 0xff, 0xdb, 0xa8, 0x37, 0xff, 0xdb, 0xa8, 0x36, 0xff, 0xdb, 0xa8, + 0x37, 0xff, 0xdb, 0xa8, 0x37, 0xff, 0xdb, 0xa8, 0x37, 0xff, 0xdb, 0xa8, + 0x37, 0xff, 0xdb, 0xa8, 0x37, 0xff, 0xdb, 0xa8, 0x37, 0xff, 0xdb, 0xa8, + 0x37, 0xff, 0xdb, 0xa8, 0x37, 0xff, 0xdb, 0xa8, 0x36, 0xff, 0xdb, 0xa8, + 0x37, 0xff, 0xdb, 0xa8, 0x37, 0xff, 0xdb, 0xa8, 0x37, 0xff, 0xdb, 0xa8, + 0x37, 0xff, 0xdb, 0xa8, 0x37, 0xff, 0xdb, 0xa8, 0x37, 0xf8, 0xdb, 0xa8, + 0x37, 0x54, 0xdb, 0xa8, 0x37, 0x63, 0xdb, 0xa8, 0x37, 0xf6, 0xdb, 0xa8, + 0x37, 0xff, 0xdb, 0xa8, 0x37, 0xff, 0xdb, 0xa8, 0x37, 0xff, 0xdb, 0xa8, + 0x37, 0xff, 0xdb, 0xa8, 0x37, 0xff, 0xdb, 0xa8, 0x37, 0xff, 0xdb, 0xa8, + 0x37, 0xff, 0xdb, 0xa8, 0x37, 0xff, 0xdb, 0xa8, 0x37, 0xff, 0xdb, 0xa8, + 0x37, 0xff, 0xdb, 0xa8, 0x37, 0xff, 0xdb, 0xa8, 0x37, 0xff, 0xdb, 0xa8, + 0x37, 0xff, 0xdb, 0xa8, 0x37, 0xff, 0xdb, 0xa8, 0x37, 0xff, 0xdb, 0xa8, + 0x37, 0xff, 0xdb, 0xa8, 0x37, 0xff, 0xdb, 0xa8, 0x37, 0xff, 0xdb, 0xa8, + 0x37, 0xff, 0xdb, 0xa8, 0x37, 0xff, 0xdb, 0xa8, 0x37, 0xe7, 0xdb, 0xa8, + 0x37, 0x3c, 0xdb, 0xa8, 0x37, 0x10, 0xdb, 0xa8, 0x37, 0x63, 0xdb, 0xa8, + 0x37, 0x82, 0xdb, 0xa8, 0x37, 0x81, 0xdb, 0xa8, 0x37, 0x81, 0xdb, 0xa8, + 0x37, 0x81, 0xdb, 0xa8, 0x37, 0x81, 0xdb, 0xa8, 0x37, 0x81, 0xdb, 0xa8, + 0x37, 0x81, 0xdb, 0xa8, 0x37, 0x81, 0xdb, 0xa8, 0x37, 0x81, 0xdb, 0xa8, + 0x37, 0x81, 0xdb, 0xa8, 0x37, 0x81, 0xdb, 0xa8, 0x37, 0x81, 0xdb, 0xa8, + 0x37, 0x81, 0xdb, 0xa8, 0x37, 0x81, 0xdb, 0xa8, 0x37, 0x81, 0xdb, 0xa8, + 0x37, 0x81, 0xdb, 0xa8, 0x37, 0x81, 0xdb, 0xa8, 0x37, 0x81, 0xdb, 0xa8, + 0x37, 0x81, 0xdb, 0xa8, 0x37, 0x81, 0xdb, 0xa8, 0x37, 0x53, 0xdb, 0xa8, + 0x37, 0x06, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00 +}; + +#define ICON_SIZE (sizeof(icon_file)) +#define EMFAT_INCR_ICON 1 +#else +#define EMFAT_INCR_ICON 0 +#endif + +#define CMA_TIME EMFAT_ENCODE_CMA_TIME(25,12,2019, 13,0,0) +#define CMA { CMA_TIME, CMA_TIME, CMA_TIME } + +static void memory_read_proc(uint8_t *dest, int size, uint32_t offset, emfat_entry_t *entry) +{ + int len; + + if (offset > entry->curr_size) { + return; + } + + if (offset + size > entry->curr_size) { + len = entry->curr_size - offset; + } else { + len = size; + } + + memcpy(dest, &((char *)entry->user_data)[offset], len); +} + +static void bblog_read_proc(uint8_t *dest, int size, uint32_t offset, emfat_entry_t *entry) +{ + UNUSED(entry); + + flashfsReadAbs(offset, dest, size); +} + +static const emfat_entry_t entriesPredefined[] = +{ + // name dir attr lvl offset size max_size user time read write + { "", true, 0, 0, 0, 0, 0, 0, CMA, NULL, NULL, { 0 } }, +#ifdef USE_EMFAT_AUTORUN + { "autorun.inf", false, ATTR_HIDDEN, 1, 0, AUTORUN_SIZE, AUTORUN_SIZE, (long)autorun_file, CMA, memory_read_proc, NULL, { 0 } }, +#endif +#ifdef USE_EMFAT_ICON + { "icon.ico", false, ATTR_HIDDEN, 1, 0, ICON_SIZE, ICON_SIZE, (long)icon_file, CMA, memory_read_proc, NULL, { 0 } }, +#endif +#ifdef USE_EMFAT_README + { "readme.txt", false, 0, 1, 0, README_SIZE, 1024*1024, (long)readme_file, CMA, memory_read_proc, NULL, { 0 } }, +#endif + { "INAV_ALL.BBL", 0, 0, 1, 0, 0, 0, 0, CMA, bblog_read_proc, NULL, { 0 } }, + { ".PADDING.TXT", 0, ATTR_HIDDEN, 1, 0, 0, 0, 0, CMA, NULL, NULL, { 0 } }, +}; + +#define PREDEFINED_ENTRY_COUNT (1 + EMFAT_INCR_AUTORUN + EMFAT_INCR_ICON + EMFAT_INCR_README) +#define APPENDED_ENTRY_COUNT 2 + +#define EMFAT_MAX_LOG_ENTRY 100 +#define EMFAT_MAX_ENTRY (PREDEFINED_ENTRY_COUNT + EMFAT_MAX_LOG_ENTRY + APPENDED_ENTRY_COUNT) + +static emfat_entry_t entries[EMFAT_MAX_ENTRY]; + +emfat_t emfat; +static uint32_t cmaTime = CMA_TIME; + +static void emfat_set_entry_cma(emfat_entry_t *entry) +{ + // Set file creation/modification/access times to be the same, either the default date or that from the RTC + // In practise this will be when the filesystem is mounted as the date is passed from the host over USB + entry->cma_time[0] = cmaTime; + entry->cma_time[1] = cmaTime; + entry->cma_time[2] = cmaTime; +} + +#ifdef USE_FLASHFS +static void emfat_add_log(emfat_entry_t *entry, int number, uint32_t offset, uint32_t size) +{ + static char logNames[EMFAT_MAX_LOG_ENTRY][8+1+3]; + + tfp_sprintf(logNames[number], "INAV_%03d.BBL", number + 1); + entry->name = logNames[number]; + entry->level = 1; + entry->offset = offset; + entry->curr_size = size; + entry->max_size = entry->curr_size; + entry->readcb = bblog_read_proc; + // Set file modification/access times to be the same as the creation time + entry->cma_time[1] = entry->cma_time[0]; + entry->cma_time[2] = entry->cma_time[0]; +} + +static int emfat_find_log(emfat_entry_t *entry, int maxCount, int flashfsUsedSpace) +{ + int lastOffset = 0; + int currOffset = 0; + int buffOffset; + int hdrOffset; + int fileNumber = 0; + uint8_t buffer[HDR_BUF_SIZE]; + int logCount = 0; + char *logHeader = "H Product:Blackbox"; + int lenLogHeader = strlen(logHeader); + char *timeHeader = "H Log start datetime:"; + int lenTimeHeader = strlen(timeHeader); + int timeHeaderMatched = 0; + + for ( ; currOffset < flashfsUsedSpace ; currOffset += 2048) { // XXX 2048 = FREE_BLOCK_SIZE in io/flashfs.c + + flashfsReadAbs(currOffset, buffer, HDR_BUF_SIZE); + + if (strncmp((char *)buffer, logHeader, lenLogHeader)) { + continue; + } + + // The length of the previous record is now known + if (lastOffset != currOffset) { + // Record the previous entry + emfat_add_log(entry++, fileNumber++, lastOffset, currOffset - lastOffset); + + logCount++; + } + + // Find the "Log start datetime" entry, example encoding "H Log start datetime:2019-08-15T13:18:22.199+00:00" + + buffOffset = lenLogHeader; + hdrOffset = currOffset; + + // Set the default timestamp for this log entry in case the timestamp is not found + entry->cma_time[0] = cmaTime; + + // Search for the timestamp record + while (true) { + if (buffer[buffOffset++] == timeHeader[timeHeaderMatched]) { + // This matches the header we're looking for so far + if (++timeHeaderMatched == lenTimeHeader) { + // Complete match so read date/time into buffer + flashfsReadAbs(hdrOffset + buffOffset, buffer, HDR_BUF_SIZE); + + // Extract the time values to create the CMA time + + char *last; + char* tok = strtok_r((char *)buffer, "-T:.", &last); + int index=0; + int year=0,month,day,hour,min,sec; + while (tok != NULL) { + switch(index) { + case 0: + year = fastA2I(tok); + break; + case 1: + month = fastA2I(tok); + break; + case 2: + day = fastA2I(tok); + break; + case 3: + hour = fastA2I(tok); + break; + case 4: + min = fastA2I(tok); + break; + case 5: + sec = fastA2I(tok); + break; + } + if(index == 5) + break; + index++; + tok = strtok_r(NULL, "-T:.", &last); + } + // Set the file creation time + if (year) { + entry->cma_time[0] = EMFAT_ENCODE_CMA_TIME(day, month, year, hour, min, sec); + } + break; + } + } else { + timeHeaderMatched = 0; + } + + if (buffOffset == HDR_BUF_SIZE) { + // Read the next portion of the header + hdrOffset += HDR_BUF_SIZE; + + // Check for flash overflow + if (hdrOffset > flashfsUsedSpace) { + break; + } + flashfsReadAbs(hdrOffset, buffer, HDR_BUF_SIZE); + buffOffset = 0; + } + } + + if (fileNumber == maxCount) { + break; + } + + lastOffset = currOffset; + } + + // Now add the final entry + if (fileNumber != maxCount && lastOffset != currOffset) { + emfat_add_log(entry, fileNumber, lastOffset, currOffset - lastOffset); + ++logCount; + } + + return logCount; +} +#endif // USE_FLASHFS + +void emfat_init_files(void) +{ +#ifdef USE_FLASHFS + int flashfsUsedSpace = 0; + int entryIndex = PREDEFINED_ENTRY_COUNT; + emfat_entry_t *entry; + + flashfsInit(); + flashfsUsedSpace = flashfsIdentifyStartOfFreeSpace(); + + // Detect and create entries for each individual log + const int logCount = emfat_find_log(&entries[PREDEFINED_ENTRY_COUNT], EMFAT_MAX_LOG_ENTRY, flashfsUsedSpace); + + entryIndex += logCount; + + if (logCount > 0) { + // Use the first log time for predefined entries + cmaTime = entries[PREDEFINED_ENTRY_COUNT].cma_time[0]; + // Create the all logs entry that represents all used flash space to + // allow downloading the entire log in one file + entries[entryIndex] = entriesPredefined[PREDEFINED_ENTRY_COUNT]; + entry = &entries[entryIndex]; + entry->curr_size = flashfsUsedSpace; + entry->max_size = entry->curr_size; + // This entry has timestamps corresponding to when the filesystem is mounted + emfat_set_entry_cma(entry); + ++entryIndex; + } + + // Padding file to fill out the filesystem size to FILESYSTEM_SIZE_MB + if (flashfsUsedSpace * 2 < FILESYSTEM_SIZE_MB * 1024 * 1024) { + entries[entryIndex] = entriesPredefined[PREDEFINED_ENTRY_COUNT + 1]; + entry = &entries[entryIndex]; + // used space is doubled because of the individual files plus the single complete file + entry->curr_size = (FILESYSTEM_SIZE_MB * 1024 * 1024) - (flashfsUsedSpace * 2); + entry->max_size = entry->curr_size; + emfat_set_entry_cma(entry); + } +#endif // USE_FLASHFS + // create the predefined entries + for (size_t i = 0 ; i < PREDEFINED_ENTRY_COUNT ; i++) { + entries[i] = entriesPredefined[i]; + emfat_set_entry_cma(&entries[i]); + } + emfat_init(&emfat, "INAV_FC", entries); +} diff --git a/src/main/msc/emfat_file.h b/src/main/msc/emfat_file.h new file mode 100644 index 00000000000..139e181fe38 --- /dev/null +++ b/src/main/msc/emfat_file.h @@ -0,0 +1,23 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software 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 and Betaflight are distributed in the hope that they + * 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 this software. + * + * If not, see . + */ + +#pragma once + +void emfat_init_files(void); diff --git a/src/main/msc/usbd_msc_desc.c b/src/main/msc/usbd_msc_desc.c new file mode 100644 index 00000000000..a72c8b83c43 --- /dev/null +++ b/src/main/msc/usbd_msc_desc.c @@ -0,0 +1,383 @@ +/** + ****************************************************************************** + * @file usbd_desc.c + * @author MCD Application Team + * @version V1.2.0 + * @date 09-November-2015 + * @brief This file provides the USBD descriptors and string formating method. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Includes ------------------------------------------------------------------*/ +#include "usbd_core.h" +#include "usbd_desc.h" +#include "usbd_req.h" +#include "usbd_conf.h" +#include "usb_regs.h" +#include "platform.h" + +#define DEVICE_ID1 (0x1FFFF7E8) +#define DEVICE_ID2 (0x1FFFF7EA) +#define DEVICE_ID3 (0x1FFFF7EC) +#define USB_SIZ_STRING_SERIAL 0x1A + +/** @addtogroup STM32_USB_OTG_DEVICE_LIBRARY + * @{ + */ + + +/** @defgroup USBD_DESC + * @brief USBD descriptors module + * @{ + */ + +/** @defgroup USBD_DESC_Private_TypesDefinitions + * @{ + */ +/** + * @} + */ + + +/** @defgroup USBD_DESC_Private_Defines + * @{ + */ + +#define USBD_VID 0x0483 +#define USBD_PID 0x5720 + +#define USBD_LANGID_STRING 0x409 +#define USBD_MANUFACTURER_STRING "STMicroelectronics" +#define USBD_PRODUCT_HS_STRING "Mass Storage in HS Mode" +#define USBD_PRODUCT_FS_STRING "Mass Storage in FS Mode" +#define USBD_CONFIGURATION_HS_STRING "MSC Config" +#define USBD_INTERFACE_HS_STRING "MSC Interface" +#define USBD_CONFIGURATION_FS_STRING "MSC Config" +#define USBD_INTERFACE_FS_STRING "MSC Interface" +/** + * @} + */ + + +/** @defgroup USBD_DESC_Private_Macros + * @{ + */ +/** + * @} + */ + + +/** @defgroup USBD_DESC_Private_Variables + * @{ + */ + +USBD_DEVICE MSC_desc = +{ + USBD_MSC_DeviceDescriptor, + USBD_MSC_LangIDStrDescriptor, + USBD_MSC_ManufacturerStrDescriptor, + USBD_MSC_ProductStrDescriptor, + USBD_MSC_SerialStrDescriptor, + USBD_MSC_ConfigStrDescriptor, + USBD_MSC_InterfaceStrDescriptor, +}; + +#ifdef USB_OTG_HS_INTERNAL_DMA_ENABLED + #if defined ( __ICCARM__ ) /*!< IAR Compiler */ + #pragma data_alignment=4 + #endif +#endif /* USB_OTG_HS_INTERNAL_DMA_ENABLED */ +/* USB Standard Device Descriptor */ +__ALIGN_BEGIN uint8_t USBD_DeviceDesc_MSC[USB_SIZ_DEVICE_DESC] __ALIGN_END = +{ + 0x12, /*bLength */ + USB_DEVICE_DESCRIPTOR_TYPE, /*bDescriptorType*/ + 0x00, /*bcdUSB */ + 0x02, + 0x00, /*bDeviceClass*/ + 0x00, /*bDeviceSubClass*/ + 0x00, /*bDeviceProtocol*/ + USB_OTG_MAX_EP0_SIZE, /*bMaxPacketSize*/ + LOBYTE(USBD_VID), /*idVendor*/ + HIBYTE(USBD_VID), /*idVendor*/ + LOBYTE(USBD_PID), /*idVendor*/ + HIBYTE(USBD_PID), /*idVendor*/ + 0x00, /*bcdDevice rel. 2.00*/ + 0x02, + USBD_IDX_MFC_STR, /*Index of manufacturer string*/ + USBD_IDX_PRODUCT_STR, /*Index of product string*/ + USBD_IDX_SERIAL_STR, /*Index of serial number string*/ + USBD_CFG_MAX_NUM /*bNumConfigurations*/ +} ; /* USB_DeviceDescriptor */ + +#ifdef USB_OTG_HS_INTERNAL_DMA_ENABLED + #if defined ( __ICCARM__ ) /*!< IAR Compiler */ + #pragma data_alignment=4 + #endif +#endif /* USB_OTG_HS_INTERNAL_DMA_ENABLED */ +/* USB Standard Device Descriptor */ +__ALIGN_BEGIN uint8_t USBD_DeviceQualifierDesc_MSC[USB_LEN_DEV_QUALIFIER_DESC] __ALIGN_END = +{ + USB_LEN_DEV_QUALIFIER_DESC, + USB_DESC_TYPE_DEVICE_QUALIFIER, + 0x00, + 0x02, + 0x00, + 0x00, + 0x00, + 0x40, + 0x01, + 0x00, +}; + +#ifdef USB_OTG_HS_INTERNAL_DMA_ENABLED + #if defined ( __ICCARM__ ) /*!< IAR Compiler */ + #pragma data_alignment=4 + #endif +#endif /* USB_OTG_HS_INTERNAL_DMA_ENABLED */ +/* USB Standard Device Descriptor */ +__ALIGN_BEGIN uint8_t USBD_LangIDDesc_MSC[USB_SIZ_STRING_LANGID] __ALIGN_END = +{ + USB_SIZ_STRING_LANGID, + USB_DESC_TYPE_STRING, + LOBYTE(USBD_LANGID_STRING), + HIBYTE(USBD_LANGID_STRING), +}; + +uint8_t USBD_StringSerial_MSC[USB_SIZ_STRING_SERIAL] = +{ + USB_SIZ_STRING_SERIAL, + USB_DESC_TYPE_STRING, +}; + +#ifdef USB_OTG_HS_INTERNAL_DMA_ENABLED + #if defined ( __ICCARM__ ) /*!< IAR Compiler */ + #pragma data_alignment=4 + #endif +#endif /* USB_OTG_HS_INTERNAL_DMA_ENABLED */ +__ALIGN_BEGIN uint8_t USBD_StrDesc_MSC[USB_MAX_STR_DESC_SIZ] __ALIGN_END ; + +/** + * @} + */ + + +/** @defgroup USBD_DESC_Private_FunctionPrototypes + * @{ + */ +static void IntToUnicode (uint32_t value , uint8_t *pbuf , uint8_t len); +static void Get_SerialNum(void); +/** + * @} + */ + + +/** @defgroup USBD_DESC_Private_Functions + * @{ + */ + +/** +* @brief USBD_USR_DeviceDescriptor +* return the device descriptor +* @param speed : current device speed +* @param length : pointer to data length variable +* @retval pointer to descriptor buffer +*/ +uint8_t * USBD_MSC_DeviceDescriptor( uint8_t speed , uint16_t *length) +{ + (void)(speed); + *length = sizeof(USBD_DeviceDesc_MSC); + return (uint8_t*)USBD_DeviceDesc_MSC; +} + +/** +* @brief USBD_USR_LangIDStrDescriptor +* return the LangID string descriptor +* @param speed : current device speed +* @param length : pointer to data length variable +* @retval pointer to descriptor buffer +*/ +uint8_t * USBD_MSC_LangIDStrDescriptor( uint8_t speed , uint16_t *length) +{ + (void)(speed); + *length = sizeof(USBD_LangIDDesc_MSC); + return (uint8_t*)USBD_LangIDDesc_MSC; +} + + +/** +* @brief USBD_USR_ProductStrDescriptor +* return the product string descriptor +* @param speed : current device speed +* @param length : pointer to data length variable +* @retval pointer to descriptor buffer +*/ +uint8_t * USBD_MSC_ProductStrDescriptor( uint8_t speed , uint16_t *length) +{ + if(speed == 0) + { + USBD_GetString((uint8_t *)(uint8_t *)USBD_PRODUCT_HS_STRING, USBD_StrDesc_MSC, length); + } + else + { + USBD_GetString((uint8_t *)(uint8_t *)USBD_PRODUCT_FS_STRING, USBD_StrDesc_MSC, length); + } + return USBD_StrDesc_MSC; +} + +/** +* @brief USBD_USR_ManufacturerStrDescriptor +* return the manufacturer string descriptor +* @param speed : current device speed +* @param length : pointer to data length variable +* @retval pointer to descriptor buffer +*/ +uint8_t * USBD_MSC_ManufacturerStrDescriptor( uint8_t speed , uint16_t *length) +{ + (void)(speed); + USBD_GetString((uint8_t *)(uint8_t *)USBD_MANUFACTURER_STRING, USBD_StrDesc_MSC, length); + return USBD_StrDesc_MSC; +} + +/** +* @brief USBD_USR_SerialStrDescriptor +* return the serial number string descriptor +* @param speed : current device speed +* @param length : pointer to data length variable +* @retval pointer to descriptor buffer +*/ +uint8_t * USBD_MSC_SerialStrDescriptor( uint8_t speed , uint16_t *length) +{ + (void)(speed); + *length = USB_SIZ_STRING_SERIAL; + + /* Update the serial number string descriptor with the data from the unique ID*/ + Get_SerialNum(); + + return (uint8_t*)USBD_StringSerial_MSC; +} + +/** +* @brief USBD_USR_ConfigStrDescriptor +* return the configuration string descriptor +* @param speed : current device speed +* @param length : pointer to data length variable +* @retval pointer to descriptor buffer +*/ +uint8_t * USBD_MSC_ConfigStrDescriptor( uint8_t speed , uint16_t *length) +{ + if(speed == USB_OTG_SPEED_HIGH) + { + USBD_GetString((uint8_t *)(uint8_t *)USBD_CONFIGURATION_HS_STRING, USBD_StrDesc_MSC, length); + } + else + { + USBD_GetString((uint8_t *)(uint8_t *)USBD_CONFIGURATION_FS_STRING, USBD_StrDesc_MSC, length); + } + return USBD_StrDesc_MSC; +} + + +/** +* @brief USBD_USR_InterfaceStrDescriptor +* return the interface string descriptor +* @param speed : current device speed +* @param length : pointer to data length variable +* @retval pointer to descriptor buffer +*/ +uint8_t * USBD_MSC_InterfaceStrDescriptor( uint8_t speed , uint16_t *length) +{ + if(speed == 0) + { + USBD_GetString((uint8_t *)(uint8_t *)USBD_INTERFACE_HS_STRING, USBD_StrDesc_MSC, length); + } + else + { + USBD_GetString((uint8_t *)(uint8_t *)USBD_INTERFACE_FS_STRING, USBD_StrDesc_MSC, length); + } + return USBD_StrDesc_MSC; +} + +/** + * @brief Create the serial number string descriptor + * @param None + * @retval None + */ +static void Get_SerialNum(void) +{ + uint32_t deviceserial0, deviceserial1, deviceserial2; + + deviceserial0 = *(uint32_t*)DEVICE_ID1; + deviceserial1 = *(uint32_t*)DEVICE_ID2; + deviceserial2 = *(uint32_t*)DEVICE_ID3; + + deviceserial0 += deviceserial2; + + if (deviceserial0 != 0) + { + IntToUnicode (deviceserial0, &USBD_StringSerial_MSC[2] ,8); + IntToUnicode (deviceserial1, &USBD_StringSerial_MSC[18] ,4); + } +} + +/** + * @brief Convert Hex 32Bits value into char + * @param value: value to convert + * @param pbuf: pointer to the buffer + * @param len: buffer length + * @retval None + */ +static void IntToUnicode (uint32_t value , uint8_t *pbuf , uint8_t len) +{ + uint8_t idx = 0; + + for( idx = 0 ; idx < len ; idx ++) + { + if( ((value >> 28)) < 0xA ) + { + pbuf[ 2* idx] = (value >> 28) + '0'; + } + else + { + pbuf[2* idx] = (value >> 28) + 'A' - 10; + } + + value = value << 4; + + pbuf[ 2* idx + 1] = 0; + } +} + + +/** + * @} + */ + + +/** + * @} + */ + + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/src/main/msc/usbd_msc_desc.h b/src/main/msc/usbd_msc_desc.h new file mode 100644 index 00000000000..03a3d9bbcca --- /dev/null +++ b/src/main/msc/usbd_msc_desc.h @@ -0,0 +1,132 @@ +/** + ****************************************************************************** + * @file usbd_desc.h + * @author MCD Application Team + * @version V1.2.0 + * @date 09-November-2015 + * @brief header file for the usbd_desc.c file + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ + +#ifndef __USB_DESC_H +#define __USB_DESC_H + +/* Includes ------------------------------------------------------------------*/ +#include "usbd_req.h" + +/** @addtogroup STM32_USB_OTG_DEVICE_LIBRARY + * @{ + */ + +/** @defgroup USB_DESC + * @brief general defines for the usb device library file + * @{ + */ + +/** @defgroup USB_DESC_Exported_Defines + * @{ + */ +#define USB_DEVICE_DESCRIPTOR_TYPE 0x01 +#define USB_CONFIGURATION_DESCRIPTOR_TYPE 0x02 +#define USB_STRING_DESCRIPTOR_TYPE 0x03 +#define USB_INTERFACE_DESCRIPTOR_TYPE 0x04 +#define USB_ENDPOINT_DESCRIPTOR_TYPE 0x05 +#define USB_SIZ_DEVICE_DESC 18 +#define USB_SIZ_STRING_LANGID 4 + +#if !defined (USE_STM3210C_EVAL) +#define DEVICE_ID1 (0x1FFF7A10) +#define DEVICE_ID2 (0x1FFF7A14) +#define DEVICE_ID3 (0x1FFF7A18) + +#else +#define DEVICE_ID1 (0x1FFFF7E8) +#define DEVICE_ID2 (0x1FFFF7EA) +#define DEVICE_ID3 (0x1FFFF7EC) +#endif + +#define USB_SIZ_STRING_SERIAL 0x1A +/** + * @} + */ + + +/** @defgroup USBD_DESC_Exported_TypesDefinitions + * @{ + */ +/** + * @} + */ + + + +/** @defgroup USBD_DESC_Exported_Macros + * @{ + */ +/** + * @} + */ + +/** @defgroup USBD_DESC_Exported_Variables + * @{ + */ +extern uint8_t USBD_DeviceDesc [USB_SIZ_DEVICE_DESC]; +extern uint8_t USBD_StrDesc[USB_MAX_STR_DESC_SIZ]; +extern uint8_t USBD_OtherSpeedCfgDesc[USB_LEN_CFG_DESC]; +extern uint8_t USBD_DeviceQualifierDesc[USB_LEN_DEV_QUALIFIER_DESC]; +extern uint8_t USBD_LangIDDesc[USB_SIZ_STRING_LANGID]; +extern USBD_DEVICE USR_desc; +/** + * @} + */ + +/** @defgroup USBD_DESC_Exported_FunctionsPrototype + * @{ + */ + + +uint8_t * USBD_USR_DeviceDescriptor( uint8_t speed , uint16_t *length); +uint8_t * USBD_USR_LangIDStrDescriptor( uint8_t speed , uint16_t *length); +uint8_t * USBD_USR_ManufacturerStrDescriptor ( uint8_t speed , uint16_t *length); +uint8_t * USBD_USR_ProductStrDescriptor ( uint8_t speed , uint16_t *length); +uint8_t * USBD_USR_SerialStrDescriptor( uint8_t speed , uint16_t *length); +uint8_t * USBD_USR_ConfigStrDescriptor( uint8_t speed , uint16_t *length); +uint8_t * USBD_USR_InterfaceStrDescriptor( uint8_t speed , uint16_t *length); + +#ifdef USB_SUPPORT_USER_STRING_DESC +uint8_t * USBD_USR_USRStringDesc (uint8_t speed, uint8_t idx , uint16_t *length); +#endif /* USB_SUPPORT_USER_STRING_DESC */ + +/** + * @} + */ + +#endif /* __USBD_DESC_H */ + +/** + * @} + */ + +/** +* @} +*/ +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/src/main/msc/usbd_storage.c b/src/main/msc/usbd_storage.c new file mode 100644 index 00000000000..43121206996 --- /dev/null +++ b/src/main/msc/usbd_storage.c @@ -0,0 +1,38 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software 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 and Betaflight are distributed in the hope that they + * 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 this software. + * + * If not, see . + */ + +/* + * Author: jflyper (https://github.com/jflyper) + */ + +#ifdef USE_HAL_DRIVER +#include "usbd_msc.h" +#else +#include "usbd_msc_mem.h" +#include "usbd_msc_core.h" +#endif + +#include "usbd_storage.h" + +#ifdef USE_HAL_DRIVER +USBD_StorageTypeDef *USBD_STORAGE_fops; +#else +USBD_STORAGE_cb_TypeDef *USBD_STORAGE_fops; +#endif diff --git a/src/main/msc/usbd_storage.h b/src/main/msc/usbd_storage.h new file mode 100644 index 00000000000..3fb51e3e221 --- /dev/null +++ b/src/main/msc/usbd_storage.h @@ -0,0 +1,46 @@ +/* + * 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 . + */ +/* + * Author: Chris Hockuba (https://github.com/conkerkh) + */ + +#pragma once + +#ifdef USE_HAL_DRIVER +#include "usbd_msc.h" +#else +#include "usbd_msc_mem.h" +#include "usbd_msc_core.h" +#endif + +#ifdef USE_HAL_DRIVER +extern USBD_StorageTypeDef *USBD_STORAGE_fops; +#ifdef USE_SDCARD +extern USBD_StorageTypeDef USBD_MSC_MICRO_SDIO_fops; +#endif +#ifdef USE_FLASHFS +extern USBD_StorageTypeDef USBD_MSC_EMFAT_fops; +#endif +#else +extern USBD_STORAGE_cb_TypeDef *USBD_STORAGE_fops; +#ifdef USE_SDCARD +extern USBD_STORAGE_cb_TypeDef USBD_MSC_MICRO_SDIO_fops; +#endif +#ifdef USE_FLASHFS +extern USBD_STORAGE_cb_TypeDef USBD_MSC_EMFAT_fops; +#endif +#endif diff --git a/src/main/msc/usbd_storage_emfat.c b/src/main/msc/usbd_storage_emfat.c new file mode 100644 index 00000000000..070df79f830 --- /dev/null +++ b/src/main/msc/usbd_storage_emfat.c @@ -0,0 +1,142 @@ +/* + * Derived from github.com/fetisov/emfat/project/StorageMode.c + */ + +/* + * The MIT License (MIT) + * + * Copyright (c) 2015 by Sergey Fetisov + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include + +#include "platform.h" + +#include "common/utils.h" + +#include "drivers/light_led.h" +#include "drivers/time.h" +#include "drivers/flash.h" + +#include "io/flashfs.h" + +#include "usbd_storage.h" +#include "usbd_storage_emfat.h" +#include "emfat_file.h" + + +#define STORAGE_LUN_NBR 1 + +static const uint8_t STORAGE_Inquirydata[] = +{ + 0x00, 0x80, 0x02, 0x02, +#ifdef USE_HAL_DRIVER + (STANDARD_INQUIRY_DATA_LEN - 5), +#else + (USBD_STD_INQUIRY_LENGTH - 5), +#endif + 0x00, 0x00, 0x00, + 'I', 'N', 'A', 'V', ' ', 'F', 'C', ' ', // Manufacturer : 8 bytes + 'O', 'n', 'b', 'o', 'a', 'r', 'd', ' ', // Product : 16 Bytes + 'F', 'l', 'a', 's', 'h', ' ', ' ', ' ', // + ' ', ' ', ' ' ,' ', // Version : 4 Bytes +}; + +static int8_t STORAGE_Init(uint8_t lun) +{ + UNUSED(lun); + + LED0_OFF; + + return 0; +} + +#ifdef USE_HAL_DRIVER +static int8_t STORAGE_GetCapacity(uint8_t lun, uint32_t *block_num, uint16_t *block_size) +#else +static int8_t STORAGE_GetCapacity(uint8_t lun, uint32_t *block_num, uint32_t *block_size) +#endif +{ + UNUSED(lun); + *block_size = 512; + *block_num = emfat.disk_sectors; + return 0; +} + +static int8_t STORAGE_IsReady(uint8_t lun) +{ + UNUSED(lun); + return 0; +} + +static int8_t STORAGE_IsWriteProtected(uint8_t lun) +{ + UNUSED(lun); + return 1; +} + +static int8_t STORAGE_Read( + uint8_t lun, // logical unit number + uint8_t *buf, // Pointer to the buffer to save data + uint32_t blk_addr, // address of 1st block to be read + uint16_t blk_len) // nmber of blocks to be read +{ + UNUSED(lun); + LED0_ON; + emfat_read(&emfat, buf, blk_addr, blk_len); + LED0_OFF; + return 0; +} + +static int8_t STORAGE_Write(uint8_t lun, + uint8_t *buf, + uint32_t blk_addr, + uint16_t blk_len) +{ + UNUSED(lun); + UNUSED(buf); + UNUSED(blk_addr); + UNUSED(blk_len); + + return 1; +} + +static int8_t STORAGE_GetMaxLun(void) +{ + return (STORAGE_LUN_NBR - 1); +} + +#ifdef USE_HAL_DRIVER +USBD_StorageTypeDef +#else +USBD_STORAGE_cb_TypeDef +#endif +USBD_MSC_EMFAT_fops = +{ + STORAGE_Init, + STORAGE_GetCapacity, + STORAGE_IsReady, + STORAGE_IsWriteProtected, + STORAGE_Read, + STORAGE_Write, + STORAGE_GetMaxLun, + (int8_t *)STORAGE_Inquirydata, +}; diff --git a/src/main/msc/usbd_storage_emfat.h b/src/main/msc/usbd_storage_emfat.h new file mode 100644 index 00000000000..e91ead310b9 --- /dev/null +++ b/src/main/msc/usbd_storage_emfat.h @@ -0,0 +1,25 @@ +/* + * 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 . + * + * Author: jflyper (https://github.com/jflyper) + * + */ + +#pragma once + +#include "emfat.h" + +extern emfat_t emfat; diff --git a/src/main/msc/usbd_storage_sd_spi.c b/src/main/msc/usbd_storage_sd_spi.c new file mode 100644 index 00000000000..79c9bee225a --- /dev/null +++ b/src/main/msc/usbd_storage_sd_spi.c @@ -0,0 +1,257 @@ +/** + ****************************************************************************** + * @file usbd_storage_template.c + * @author MCD Application Team + * @version V1.2.0 + * @date 09-November-2015 + * @brief Memory management layer + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + + +/* Includes ------------------------------------------------------------------*/ +#include +#include + +#include "platform.h" +#include "common/utils.h" + +#include "usbd_storage.h" + +#include "drivers/sdcard/sdcard.h" +#include "drivers/light_led.h" +#include "drivers/io.h" +#include "drivers/bus_spi.h" + +/* Private typedef -----------------------------------------------------------*/ +/* Private define ------------------------------------------------------------*/ +/* Private macro -------------------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +/* Private function prototypes -----------------------------------------------*/ +/* Extern function prototypes ------------------------------------------------*/ +/* Private functions ---------------------------------------------------------*/ + +/* USB NVIC Priority has to be lower than both DMA and SDIO priority, + * otherwise SDIO won't be able to preempt USB. + */ + +#define STORAGE_LUN_NBR 1 +#define STORAGE_BLK_NBR 0x10000 +#define STORAGE_BLK_SIZ 0x200 + +static int8_t STORAGE_Init (uint8_t lun); + +#ifdef USE_HAL_DRIVER +static int8_t STORAGE_GetCapacity (uint8_t lun, + uint32_t *block_num, + uint16_t *block_size); +#else +static int8_t STORAGE_GetCapacity (uint8_t lun, + uint32_t *block_num, + uint32_t *block_size); +#endif + +static int8_t STORAGE_IsReady (uint8_t lun); + +static int8_t STORAGE_IsWriteProtected (uint8_t lun); + +static int8_t STORAGE_Read (uint8_t lun, + uint8_t *buf, + uint32_t blk_addr, + uint16_t blk_len); + +static int8_t STORAGE_Write (uint8_t lun, + uint8_t *buf, + uint32_t blk_addr, + uint16_t blk_len); + +static int8_t STORAGE_GetMaxLun (void); + +/* USB Mass storage Standard Inquiry Data */ +static uint8_t STORAGE_Inquirydata[] = {//36 + + /* LUN 0 */ + 0x00, + 0x80, + 0x02, + 0x02, +#ifdef USE_HAL_DRIVER + (STANDARD_INQUIRY_DATA_LEN - 5), +#else + (USBD_STD_INQUIRY_LENGTH - 5), +#endif + 0x00, + 0x00, + 0x00, + 'S', 'T', 'M', ' ', ' ', ' ', ' ', ' ', /* Manufacturer : 8 bytes */ + 'P', 'r', 'o', 'd', 'u', 't', ' ', ' ', /* Product : 16 Bytes */ + ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', + '0', '.', '0' ,'1', /* Version : 4 Bytes */ +}; + +#ifdef USE_HAL_DRIVER +USBD_StorageTypeDef USBD_MSC_MICRO_SDIO_fops = +{ + STORAGE_Init, + STORAGE_GetCapacity, + STORAGE_IsReady, + STORAGE_IsWriteProtected, + STORAGE_Read, + STORAGE_Write, + STORAGE_GetMaxLun, + (int8_t*)STORAGE_Inquirydata, +}; +#else +USBD_STORAGE_cb_TypeDef USBD_MSC_MICRO_SDIO_fops = +{ + STORAGE_Init, + STORAGE_GetCapacity, + STORAGE_IsReady, + STORAGE_IsWriteProtected, + STORAGE_Read, + STORAGE_Write, + STORAGE_GetMaxLun, + (int8_t*)STORAGE_Inquirydata, +}; +#endif + +/******************************************************************************* +* Function Name : Read_Memory +* Description : Handle the Read operation from the microSD card. +* Input : None. +* Output : None. +* Return : None. +*******************************************************************************/ +static int8_t STORAGE_Init (uint8_t lun) +{ + UNUSED(lun); + LED0_OFF; + sdcard_init(); + while (sdcard_poll() == 0); + LED0_ON; + return 0; +} + +/******************************************************************************* +* Function Name : Read_Memory +* Description : Handle the Read operation from the STORAGE card. +* Input : None. +* Output : None. +* Return : None. +*******************************************************************************/ +#ifdef USE_HAL_DRIVER +static int8_t STORAGE_GetCapacity (uint8_t lun, uint32_t *block_num, uint16_t *block_size) +#else +static int8_t STORAGE_GetCapacity (uint8_t lun, uint32_t *block_num, uint32_t *block_size) +#endif +{ + UNUSED(lun); + *block_num = sdcard_getMetadata()->numBlocks; + *block_size = 512; + return (0); +} + +/******************************************************************************* +* Function Name : Read_Memory +* Description : Handle the Read operation from the STORAGE card. +* Input : None. +* Output : None. +* Return : None. +*******************************************************************************/ +static int8_t STORAGE_IsReady (uint8_t lun) +{ + UNUSED(lun); + int8_t ret = -1; + if (sdcard_poll()) { + ret = 0; + } + return ret; +} + +/******************************************************************************* +* Function Name : Read_Memory +* Description : Handle the Read operation from the STORAGE card. +* Input : None. +* Output : None. +* Return : None. +*******************************************************************************/ +static int8_t STORAGE_IsWriteProtected (uint8_t lun) +{ + UNUSED(lun); + return 0; +} + +/******************************************************************************* +* Function Name : Read_Memory +* Description : Handle the Read operation from the STORAGE card. +* Input : None. +* Output : None. +* Return : None. +*******************************************************************************/ +static int8_t STORAGE_Read (uint8_t lun, + uint8_t *buf, + uint32_t blk_addr, + uint16_t blk_len) +{ + UNUSED(lun); + LED1_ON; + for (int i = 0; i < blk_len; i++) { + while (sdcard_readBlock(blk_addr + i, buf + (512 * i), NULL, NULL) == 0); + while (sdcard_poll() == 0); + } + LED1_OFF; + return 0; +} +/******************************************************************************* +* Function Name : Write_Memory +* Description : Handle the Write operation to the STORAGE card. +* Input : None. +* Output : None. +* Return : None. +*******************************************************************************/ +static int8_t STORAGE_Write (uint8_t lun, + uint8_t *buf, + uint32_t blk_addr, + uint16_t blk_len) +{ + UNUSED(lun); + LED1_ON; + for (int i = 0; i < blk_len; i++) { + while (sdcard_writeBlock(blk_addr + i, buf + (i * 512), NULL, NULL) != SDCARD_OPERATION_IN_PROGRESS) { + sdcard_poll(); + } + while (sdcard_poll() == 0); + } + LED1_OFF; + return 0; +} +/******************************************************************************* +* Function Name : Write_Memory +* Description : Handle the Write operation to the STORAGE card. +* Input : None. +* Output : None. +* Return : None. +*******************************************************************************/ +static int8_t STORAGE_GetMaxLun (void) +{ + return (STORAGE_LUN_NBR - 1); +} + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/src/main/msc/usbd_storage_sdio.c b/src/main/msc/usbd_storage_sdio.c new file mode 100644 index 00000000000..00f9c565321 --- /dev/null +++ b/src/main/msc/usbd_storage_sdio.c @@ -0,0 +1,285 @@ +/** + ****************************************************************************** + * @file usbd_storage_template.c + * @author MCD Application Team + * @version V1.2.0 + * @date 09-November-2015 + * @brief Memory management layer + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + + +/* Includes ------------------------------------------------------------------*/ +#include +#include + +#include "platform.h" + +#include "drivers/sdmmc_sdio.h" +#include "drivers/light_led.h" +#include "drivers/io.h" +#include "common/utils.h" + +#ifdef USE_HAL_DRIVER +#include "usbd_msc.h" +#else +#include "usbd_msc_mem.h" +#include "usbd_msc_core.h" +#endif + +#include "usbd_storage.h" + +/* Private typedef -----------------------------------------------------------*/ +/* Private define ------------------------------------------------------------*/ +/* Private macro -------------------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +/* Private function prototypes -----------------------------------------------*/ +/* Extern function prototypes ------------------------------------------------*/ +/* Private functions ---------------------------------------------------------*/ + +/* USB NVIC Priority has to be lower than both DMA and SDIO priority, + * otherwise SDIO won't be able to preempt USB. + */ + +#define STORAGE_LUN_NBR 1 +#define STORAGE_BLK_NBR 0x10000 +#define STORAGE_BLK_SIZ 0x200 + +static int8_t STORAGE_Init (uint8_t lun); + +#ifdef USE_HAL_DRIVER +static int8_t STORAGE_GetCapacity (uint8_t lun, + uint32_t *block_num, + uint16_t *block_size); +#else +static int8_t STORAGE_GetCapacity (uint8_t lun, + uint32_t *block_num, + uint32_t *block_size); +#endif + +static int8_t STORAGE_IsReady (uint8_t lun); + +static int8_t STORAGE_IsWriteProtected (uint8_t lun); + +static int8_t STORAGE_Read (uint8_t lun, + uint8_t *buf, + uint32_t blk_addr, + uint16_t blk_len); + +static int8_t STORAGE_Write (uint8_t lun, + uint8_t *buf, + uint32_t blk_addr, + uint16_t blk_len); + +static int8_t STORAGE_GetMaxLun (void); + +/* USB Mass storage Standard Inquiry Data */ +static uint8_t STORAGE_Inquirydata[] = {//36 + + /* LUN 0 */ + 0x00, + 0x80, + 0x02, + 0x02, +#ifdef USE_HAL_DRIVER + (STANDARD_INQUIRY_DATA_LEN - 5), +#else + (USBD_STD_INQUIRY_LENGTH - 5), +#endif + 0x00, + 0x00, + 0x00, + 'S', 'T', 'M', ' ', ' ', ' ', ' ', ' ', /* Manufacturer : 8 bytes */ + 'P', 'r', 'o', 'd', 'u', 't', ' ', ' ', /* Product : 16 Bytes */ + ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', + '0', '.', '0' ,'1', /* Version : 4 Bytes */ +}; + +#ifdef USE_HAL_DRIVER +USBD_StorageTypeDef USBD_MSC_MICRO_SDIO_fops = +{ + STORAGE_Init, + STORAGE_GetCapacity, + STORAGE_IsReady, + STORAGE_IsWriteProtected, + STORAGE_Read, + STORAGE_Write, + STORAGE_GetMaxLun, + (int8_t*)STORAGE_Inquirydata, +}; +#else +USBD_STORAGE_cb_TypeDef USBD_MSC_MICRO_SDIO_fops = +{ + STORAGE_Init, + STORAGE_GetCapacity, + STORAGE_IsReady, + STORAGE_IsWriteProtected, + STORAGE_Read, + STORAGE_Write, + STORAGE_GetMaxLun, + (int8_t*)STORAGE_Inquirydata, +}; +#endif + +/******************************************************************************* +* Function Name : Read_Memory +* Description : Handle the Read operation from the microSD card. +* Input : None. +* Output : None. +* Return : None. +*******************************************************************************/ +static int8_t STORAGE_Init (uint8_t lun) +{ + //Initialize SD_DET +#ifdef SDCARD_DETECT_PIN + const IO_t sd_det = IOGetByTag(IO_TAG(SDCARD_DETECT_PIN)); + IOInit(sd_det, OWNER_SDCARD_DETECT, 0); + IOConfigGPIO(sd_det, IOCFG_IPU); +#endif + + UNUSED(lun); + LED0_OFF; + SD_Initialize_LL(SDIO_DMA); + if (SD_Init() != 0) return 1; + LED0_ON; + return 0; +} + +/******************************************************************************* +* Function Name : Read_Memory +* Description : Handle the Read operation from the STORAGE card. +* Input : None. +* Output : None. +* Return : None. +*******************************************************************************/ +#ifdef USE_HAL_DRIVER +static int8_t STORAGE_GetCapacity (uint8_t lun, uint32_t *block_num, uint16_t *block_size) +#else +static int8_t STORAGE_GetCapacity (uint8_t lun, uint32_t *block_num, uint32_t *block_size) +#endif +{ + UNUSED(lun); + if (SD_IsDetected() == 0) { + return -1; + } + SD_GetCardInfo(); + + *block_num = SD_CardInfo.CardCapacity; + *block_size = 512; + return (0); +} + +/******************************************************************************* +* Function Name : Read_Memory +* Description : Handle the Read operation from the STORAGE card. +* Input : None. +* Output : None. +* Return : None. +*******************************************************************************/ +static int8_t STORAGE_IsReady (uint8_t lun) +{ + UNUSED(lun); + int8_t ret = -1; + if (SD_GetState() == true && SD_IsDetected() == SD_PRESENT) { + ret = 0; + } + return ret; +} + +/******************************************************************************* +* Function Name : Read_Memory +* Description : Handle the Read operation from the STORAGE card. +* Input : None. +* Output : None. +* Return : None. +*******************************************************************************/ +static int8_t STORAGE_IsWriteProtected (uint8_t lun) +{ + UNUSED(lun); + return 0; +} + +/******************************************************************************* +* Function Name : Read_Memory +* Description : Handle the Read operation from the STORAGE card. +* Input : None. +* Output : None. +* Return : None. +*******************************************************************************/ +static int8_t STORAGE_Read (uint8_t lun, + uint8_t *buf, + uint32_t blk_addr, + uint16_t blk_len) +{ + UNUSED(lun); + if (SD_IsDetected() == 0) { + return -1; + } + LED1_ON; + //buf should be 32bit aligned, but usually is so we don't do byte alignment + if (SD_ReadBlocks_DMA(blk_addr, (uint32_t*) buf, 512, blk_len) == 0) { + while (SD_CheckRead()); + while(SD_GetState() == false); + LED1_OFF; + return 0; + } + LED1_OFF; + return -1; +} +/******************************************************************************* +* Function Name : Write_Memory +* Description : Handle the Write operation to the STORAGE card. +* Input : None. +* Output : None. +* Return : None. +*******************************************************************************/ +static int8_t STORAGE_Write (uint8_t lun, + uint8_t *buf, + uint32_t blk_addr, + uint16_t blk_len) +{ + UNUSED(lun); + if (SD_IsDetected() == 0) { + return -1; + } + LED1_ON; + //buf should be 32bit aligned, but usually is so we don't do byte alignment + if (SD_WriteBlocks_DMA(blk_addr, (uint32_t*) buf, 512, blk_len) == 0) { + while (SD_CheckWrite()); + while(SD_GetState() == false); + LED1_OFF; + return 0; + } + LED1_OFF; + return -1; +} +/******************************************************************************* +* Function Name : Write_Memory +* Description : Handle the Write operation to the STORAGE card. +* Input : None. +* Output : None. +* Return : None. +*******************************************************************************/ +static int8_t STORAGE_GetMaxLun (void) +{ + return (STORAGE_LUN_NBR - 1); +} + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 637f845b480..3cfe2459ef1 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -80,7 +80,7 @@ radar_pois_t radar_pois[RADAR_MAX_POIS]; PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0); #endif -PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 4); +PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 5); PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { @@ -115,7 +115,9 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .rth_home_altitude = 0, // altitude in centimeters .rth_abort_threshold = 50000, // centimeters - 500m should be safe for all aircraft .max_terrain_follow_altitude = 100, // max altitude in centimeters in terrain following mode - }, + .rth_home_offset_distance = 0, // Distance offset from GPS established home to "safe" position used for RTH (cm, 0 disables) + .rth_home_offset_direction = 0, // Direction offset from GPS established home to "safe" position used for RTH (degrees, 0=N, 90=E, 180=S, 270=W, requires non-zero offset distance) + }, // MC-specific .mc = { @@ -129,7 +131,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .braking_boost_timeout = 750, // Timout boost after 750ms .braking_boost_speed_threshold = 150, // Boost can happen only above 1.5m/s .braking_boost_disengage_speed = 100, // Disable boost at 1m/s - .braking_bank_angle = 40, // Max braking angle + .braking_bank_angle = 40, // Max braking angle .posDecelerationTime = 120, // posDecelerationTime * 100 .posResponseExpo = 10, // posResponseExpo * 100 }, @@ -2247,7 +2249,15 @@ void updateHomePosition(void) break; } if (setHome) { - setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity()); + if (navConfig()->general.rth_home_offset_distance != 0) { // apply user defined offset + fpVector3_t offsetHome; + offsetHome.x = posControl.actualState.abs.pos.x + navConfig()->general.rth_home_offset_distance * cos_approx(DEGREES_TO_RADIANS(navConfig()->general.rth_home_offset_direction)); + offsetHome.y = posControl.actualState.abs.pos.y + navConfig()->general.rth_home_offset_distance * sin_approx(DEGREES_TO_RADIANS(navConfig()->general.rth_home_offset_direction)); + offsetHome.z = posControl.actualState.abs.pos.z; + setHomePosition(&offsetHome, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity()); + } else { + setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity()); + } } } } @@ -3140,7 +3150,7 @@ void navigationUsePIDs(void) // Initialize position hold P-controller for (int axis = 0; axis < 2; axis++) { navPidInit( - &posControl.pids.pos[axis], + &posControl.pids.pos[axis], (float)pidProfile()->bank_mc.pid[PID_POS_XY].P / 100.0f, 0.0f, 0.0f, @@ -3158,7 +3168,7 @@ void navigationUsePIDs(void) // Initialize altitude hold PID-controllers (pos_z, vel_z, acc_z navPidInit( - &posControl.pids.pos[Z], + &posControl.pids.pos[Z], (float)pidProfile()->bank_mc.pid[PID_POS_Z].P / 100.0f, 0.0f, 0.0f, diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index d8e81ca16d1..758b47a94e4 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -32,7 +32,7 @@ /* GPS Home location data */ extern gpsLocation_t GPS_home; -extern uint32_t GPS_distanceToHome; // distance to home point in meters +extern uint32_t GPS_distanceToHome; // distance to home point in meters extern int16_t GPS_directionToHome; // direction to home point in degrees extern bool autoThrottleManuallyIncreased; @@ -171,6 +171,8 @@ typedef struct navConfig_s { uint16_t min_rth_distance; // 0 Disables. Minimal distance for RTH in cm, otherwise it will just autoland uint16_t rth_abort_threshold; // Initiate emergency landing if during RTH we get this much [cm] away from home uint16_t max_terrain_follow_altitude; // Max altitude to be used in SURFACE TRACKING mode + uint16_t rth_home_offset_distance; // Distance offset from GPS established home to "safe" position used for RTH (cm, 0 disables) + uint16_t rth_home_offset_direction; // Direction offset from GPS established home to "safe" position used for RTH (degrees, 0=N, 90=E, 180=S, 270=W, requires non-zero offset distance) } general; struct { diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 6cf228eb802..7ba4884c9c5 100755 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -364,7 +364,7 @@ void rxUpdateRSSISource(void) } } - if (rxConfig()->rssi_source == RSSI_SOURCE_RX_PROTOCOL) { + if (rxConfig()->rssi_source == RSSI_SOURCE_RX_PROTOCOL || rxConfig()->rssi_source == RSSI_SOURCE_AUTO) { activeRssiSource = RSSI_SOURCE_RX_PROTOCOL; return; } @@ -736,4 +736,4 @@ uint16_t lqTrackerGet(rxLinkQualityTracker_e * lqTracker) } return lqTracker->lqValue; -} \ No newline at end of file +} diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index d17e38781aa..f1e7cbd9336 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -82,17 +82,17 @@ STATIC_FASTRAM int16_t gyroTemperature0; STATIC_FASTRAM_UNIT_TESTED zeroCalibrationVector_t gyroCalibration; STATIC_FASTRAM int32_t gyroADC[XYZ_AXIS_COUNT]; -STATIC_FASTRAM filterApplyFnPtr softLpfFilterApplyFn; -STATIC_FASTRAM void *softLpfFilter[XYZ_AXIS_COUNT]; +STATIC_FASTRAM filterApplyFnPtr gyroLpfApplyFn; +STATIC_FASTRAM filter_t gyroLpfState[XYZ_AXIS_COUNT]; + +STATIC_FASTRAM filterApplyFnPtr gyroLpf2ApplyFn; +STATIC_FASTRAM filter_t gyroLpf2State[XYZ_AXIS_COUNT]; STATIC_FASTRAM filterApplyFnPtr notchFilter1ApplyFn; STATIC_FASTRAM void *notchFilter1[XYZ_AXIS_COUNT]; STATIC_FASTRAM filterApplyFnPtr notchFilter2ApplyFn; STATIC_FASTRAM void *notchFilter2[XYZ_AXIS_COUNT]; -STATIC_FASTRAM filterApplyFnPtr gyroFilterStage2ApplyFn; -STATIC_FASTRAM void *stage2Filter[XYZ_AXIS_COUNT]; - #ifdef USE_DYNAMIC_FILTERS #define DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 350 @@ -105,7 +105,7 @@ static EXTENDED_FASTRAM biquadFilter_t notchFilterDyn2[XYZ_AXIS_COUNT]; EXTENDED_FASTRAM gyroAnalyseState_t gyroAnalyseState; #endif -PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 6); +PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 7); PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_lpf = GYRO_LPF_42HZ, // 42HZ value is defined for Invensense/TDK gyros @@ -121,6 +121,7 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_soft_notch_hz_2 = 0, .gyro_soft_notch_cutoff_2 = 1, .gyro_stage2_lowpass_hz = 0, + .gyro_stage2_lowpass_type = FILTER_BIQUAD, .dyn_notch_width_percent = 8, .dyn_notch_range = DYN_NOTCH_RANGE_MEDIUM, .dyn_notch_q = 120, @@ -327,48 +328,38 @@ bool gyroInit(void) return true; } -void gyroInitFilters(void) +static void initGyroFilter(filterApplyFnPtr *applyFn, filter_t state[], uint8_t type, uint16_t cutoff) { - STATIC_FASTRAM filter_t gyroFilterLPF[XYZ_AXIS_COUNT]; - softLpfFilterApplyFn = nullFilterApply; + *applyFn = nullFilterApply; + if (cutoff > 0) { + switch (type) + { + case FILTER_PT1: + *applyFn = (filterApplyFnPtr)pt1FilterApply; + for (int axis = 0; axis < 3; axis++) { + pt1FilterInit(&state[axis].pt1, cutoff, getLooptime()* 1e-6f); + } + break; + case FILTER_BIQUAD: + *applyFn = (filterApplyFnPtr)biquadFilterApply; + for (int axis = 0; axis < 3; axis++) { + biquadFilterInitLPF(&state[axis].biquad, cutoff, getLooptime()); + } + break; + } + } +} +void gyroInitFilters(void) +{ STATIC_FASTRAM biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT]; notchFilter1ApplyFn = nullFilterApply; STATIC_FASTRAM biquadFilter_t gyroFilterNotch_2[XYZ_AXIS_COUNT]; notchFilter2ApplyFn = nullFilterApply; - STATIC_FASTRAM biquadFilter_t gyroFilterStage2[XYZ_AXIS_COUNT]; - gyroFilterStage2ApplyFn = nullFilterApply; - - if (gyroConfig()->gyro_stage2_lowpass_hz > 0) { - gyroFilterStage2ApplyFn = (filterApplyFnPtr)biquadFilterApply; - for (int axis = 0; axis < 3; axis++) { - stage2Filter[axis] = &gyroFilterStage2[axis]; - biquadRCFIR2FilterInit(stage2Filter[axis], gyroConfig()->gyro_stage2_lowpass_hz, getLooptime()); - } - } - - if (gyroConfig()->gyro_soft_lpf_hz) { - - switch (gyroConfig()->gyro_soft_lpf_type) - { - case FILTER_PT1: - softLpfFilterApplyFn = (filterApplyFnPtr)pt1FilterApply; - for (int axis = 0; axis < 3; axis++) { - softLpfFilter[axis] = &gyroFilterLPF[axis].pt1; - pt1FilterInit(softLpfFilter[axis], gyroConfig()->gyro_soft_lpf_hz, getLooptime()* 1e-6f); - } - break; - case FILTER_BIQUAD: - softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply; - for (int axis = 0; axis < 3; axis++) { - softLpfFilter[axis] = &gyroFilterLPF[axis].biquad; - biquadFilterInitLPF(softLpfFilter[axis], gyroConfig()->gyro_soft_lpf_hz, getLooptime()); - } - break; - } - } + initGyroFilter(&gyroLpf2ApplyFn, gyroLpf2State, gyroConfig()->gyro_stage2_lowpass_type, gyroConfig()->gyro_stage2_lowpass_hz); + initGyroFilter(&gyroLpfApplyFn, gyroLpfState, gyroConfig()->gyro_soft_lpf_type, gyroConfig()->gyro_soft_lpf_hz); if (gyroConfig()->gyro_soft_notch_hz_1) { notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply; @@ -471,8 +462,8 @@ void FAST_CODE NOINLINE gyroUpdate() DEBUG_SET(DEBUG_RPM_FILTER, axis + 3, gyroADCf); #endif - gyroADCf = gyroFilterStage2ApplyFn(stage2Filter[axis], gyroADCf); - gyroADCf = softLpfFilterApplyFn(softLpfFilter[axis], gyroADCf); + gyroADCf = gyroLpf2ApplyFn((filter_t *) &gyroLpf2State[axis], gyroADCf); + gyroADCf = gyroLpfApplyFn((filter_t *) &gyroLpfState[axis], gyroADCf); gyroADCf = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf); gyroADCf = notchFilter2ApplyFn(notchFilter2[axis], gyroADCf); diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 9c1d627030c..3d9a3acd3b4 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -70,6 +70,7 @@ typedef struct gyroConfig_s { uint16_t gyro_soft_notch_hz_2; uint16_t gyro_soft_notch_cutoff_2; uint16_t gyro_stage2_lowpass_hz; + uint8_t gyro_stage2_lowpass_type; uint8_t dyn_notch_width_percent; uint8_t dyn_notch_range; uint16_t dyn_notch_q; diff --git a/src/main/target/AIKONF4/target.mk b/src/main/target/AIKONF4/target.mk index 4bc8ef110a9..a8604c383d1 100644 --- a/src/main/target/AIKONF4/target.mk +++ b/src/main/target/AIKONF4/target.mk @@ -1,5 +1,5 @@ F405_TARGETS += $(TARGET) -FEATURES = VCP ONBOARDFLASH +FEATURES = VCP ONBOARDFLASH MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu6000.c \ drivers/accgyro/accgyro_mpu6500.c \ diff --git a/src/main/target/AIRBOTF4/target.mk b/src/main/target/AIRBOTF4/target.mk index 3c0b675157a..8c881ea99f8 100644 --- a/src/main/target/AIRBOTF4/target.mk +++ b/src/main/target/AIRBOTF4/target.mk @@ -1,5 +1,5 @@ F405_TARGETS += $(TARGET) -FEATURES += VCP ONBOARDFLASH +FEATURES += VCP ONBOARDFLASH MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu6500.c \ diff --git a/src/main/target/ALIENFLIGHTNGF7/target.mk b/src/main/target/ALIENFLIGHTNGF7/target.mk index 4beb04ddd82..7f806ad190c 100644 --- a/src/main/target/ALIENFLIGHTNGF7/target.mk +++ b/src/main/target/ALIENFLIGHTNGF7/target.mk @@ -1,5 +1,5 @@ F7X2RE_TARGETS += $(TARGET) -FEATURES += SDCARD VCP ONBOARDFLASH +FEATURES += SDCARD VCP ONBOARDFLASH MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu6500.c \ @@ -15,4 +15,4 @@ TARGET_SRC = \ drivers/compass/compass_ist8308.c \ drivers/compass/compass_lis3mdl.c \ drivers/max7456.c \ - drivers/light_ws2811strip.c + drivers/light_ws2811strip.c diff --git a/src/main/target/ANYFC/target.h b/src/main/target/ANYFC/target.h index 1cf7698c36a..6959a3856be 100644 --- a/src/main/target/ANYFC/target.h +++ b/src/main/target/ANYFC/target.h @@ -136,3 +136,7 @@ #define TARGET_IO_PORTD 0xffff #define PCA9685_I2C_BUS BUS_I2C2 + +#ifdef USE_USB_MSC +# undef USE_USB_MSC +#endif diff --git a/src/main/target/ANYFCF7/target.mk b/src/main/target/ANYFCF7/target.mk index 9d26cbbb2bd..26d64b7bcad 100644 --- a/src/main/target/ANYFCF7/target.mk +++ b/src/main/target/ANYFCF7/target.mk @@ -1,5 +1,5 @@ F7X5XG_TARGETS += $(TARGET) -FEATURES += SDCARD VCP +FEATURES += SDCARD VCP MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu6000.c \ @@ -15,4 +15,3 @@ TARGET_SRC = \ drivers/compass/compass_lis3mdl.c \ drivers/light_ws2811strip.c \ drivers/max7456.c - diff --git a/src/main/target/ANYFCM7/target.mk_ b/src/main/target/ANYFCM7/target.mk_ index 371bcd26da1..62211096bcc 100644 --- a/src/main/target/ANYFCM7/target.mk_ +++ b/src/main/target/ANYFCM7/target.mk_ @@ -1,5 +1,5 @@ F7X2RE_TARGETS += $(TARGET) -FEATURES += VCP ONBOARDFLASH +FEATURES += VCP ONBOARDFLASH MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu6000.c \ diff --git a/src/main/target/ASGARD32F4/target.mk b/src/main/target/ASGARD32F4/target.mk index 62ba0215c8d..481f0495d8e 100644 --- a/src/main/target/ASGARD32F4/target.mk +++ b/src/main/target/ASGARD32F4/target.mk @@ -1,5 +1,5 @@ F405_TARGETS += $(TARGET) -FEATURES += VCP ONBOARDFLASH +FEATURES += VCP ONBOARDFLASH MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu6000.c \ diff --git a/src/main/target/ASGARD32F7/target.mk b/src/main/target/ASGARD32F7/target.mk index a7615967053..2859009996d 100644 --- a/src/main/target/ASGARD32F7/target.mk +++ b/src/main/target/ASGARD32F7/target.mk @@ -1,5 +1,5 @@ F7X2RE_TARGETS += $(TARGET) -FEATURES += VCP ONBOARDFLASH +FEATURES += VCP ONBOARDFLASH MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu6000.c \ diff --git a/src/main/target/BEEROTORF4/target.mk b/src/main/target/BEEROTORF4/target.mk index aaa180a4f8c..71ba33ec56d 100644 --- a/src/main/target/BEEROTORF4/target.mk +++ b/src/main/target/BEEROTORF4/target.mk @@ -1,5 +1,5 @@ F405_TARGETS += $(TARGET) -FEATURES += VCP SDCARD HIGHEND +FEATURES += VCP SDCARD HIGHEND MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu.c \ @@ -15,4 +15,4 @@ TARGET_SRC = \ drivers/compass/compass_mag3110.c \ drivers/compass/compass_lis3mdl.c \ drivers/max7456.c \ - drivers/light_ws2811strip.c \ No newline at end of file + drivers/light_ws2811strip.c diff --git a/src/main/target/BETAFLIGHTF4/target.mk b/src/main/target/BETAFLIGHTF4/target.mk index a5826eef035..b1dfa5e503d 100755 --- a/src/main/target/BETAFLIGHTF4/target.mk +++ b/src/main/target/BETAFLIGHTF4/target.mk @@ -1,6 +1,6 @@ F405_TARGETS += $(TARGET) -FEATURES += VCP ONBOARDFLASH +FEATURES += VCP ONBOARDFLASH MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu.c \ diff --git a/src/main/target/BLUEJAYF4/target.mk b/src/main/target/BLUEJAYF4/target.mk index 45a97b49995..9244b2aeec6 100644 --- a/src/main/target/BLUEJAYF4/target.mk +++ b/src/main/target/BLUEJAYF4/target.mk @@ -1,5 +1,5 @@ F405_TARGETS += $(TARGET) -FEATURES += SDCARD VCP ONBOARDFLASH +FEATURES += SDCARD VCP ONBOARDFLASH MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu6500.c \ @@ -13,4 +13,4 @@ TARGET_SRC = \ drivers/compass/compass_ist8308.c \ drivers/compass/compass_mag3110.c \ drivers/compass/compass_lis3mdl.c \ - drivers/light_ws2811strip.c \ No newline at end of file + drivers/light_ws2811strip.c diff --git a/src/main/target/CLRACINGF4AIR/target.h b/src/main/target/CLRACINGF4AIR/target.h index afe073190d6..54955f2910d 100644 --- a/src/main/target/CLRACINGF4AIR/target.h +++ b/src/main/target/CLRACINGF4AIR/target.h @@ -150,3 +150,7 @@ #define TARGET_IO_PORTB (0xffff) #define TARGET_IO_PORTC (0xffff) #define TARGET_IO_PORTD BIT(2) + +#ifdef USE_USB_MSC +# undef USE_USB_MSC +#endif diff --git a/src/main/target/COLIBRI/target.mk b/src/main/target/COLIBRI/target.mk index 12b8887096f..133b6f3eb5d 100755 --- a/src/main/target/COLIBRI/target.mk +++ b/src/main/target/COLIBRI/target.mk @@ -1,5 +1,5 @@ F405_TARGETS += $(TARGET) -FEATURES += VCP ONBOARDFLASH +FEATURES += VCP ONBOARDFLASH MSC HSE_VALUE = 16000000 TARGET_SRC = \ @@ -11,4 +11,3 @@ TARGET_SRC = \ drivers/compass/compass_ist8308.c \ drivers/compass/compass_mag3110.c \ drivers/compass/compass_lis3mdl.c - diff --git a/src/main/target/DALRCF405/target.mk b/src/main/target/DALRCF405/target.mk index 69db30708ab..28cba8a0f1f 100644 --- a/src/main/target/DALRCF405/target.mk +++ b/src/main/target/DALRCF405/target.mk @@ -1,5 +1,5 @@ F405_TARGETS += $(TARGET) -FEATURES += VCP ONBOARDFLASH +FEATURES += VCP ONBOARDFLASH MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu6500.c \ @@ -17,4 +17,3 @@ TARGET_SRC = \ drivers/pitotmeter_adc.c \ drivers/light_ws2811strip.c \ drivers/max7456.c - diff --git a/src/main/target/DALRCF722DUAL/target.mk b/src/main/target/DALRCF722DUAL/target.mk index d1602929817..565e12230b3 100644 --- a/src/main/target/DALRCF722DUAL/target.mk +++ b/src/main/target/DALRCF722DUAL/target.mk @@ -1,5 +1,5 @@ F7X2RE_TARGETS += $(TARGET) -FEATURES += VCP ONBOARDFLASH +FEATURES += VCP ONBOARDFLASH MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu.c \ diff --git a/src/main/target/F4BY/target.mk b/src/main/target/F4BY/target.mk index e5221e515ea..d5a0a84c3c4 100644 --- a/src/main/target/F4BY/target.mk +++ b/src/main/target/F4BY/target.mk @@ -1,5 +1,5 @@ F405_TARGETS += $(TARGET) -FEATURES += SDCARD VCP +FEATURES += SDCARD VCP MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu6000.c \ @@ -11,4 +11,4 @@ TARGET_SRC = \ drivers/compass/compass_mag3110.c \ drivers/compass/compass_lis3mdl.c \ drivers/rangefinder/rangefinder_hcsr04.c \ - drivers/max7456.c \ No newline at end of file + drivers/max7456.c diff --git a/src/main/target/FALCORE/config.c b/src/main/target/FALCORE/config.c index 231f0dd6b1b..270f27cc499 100755 --- a/src/main/target/FALCORE/config.c +++ b/src/main/target/FALCORE/config.c @@ -98,8 +98,6 @@ void targetConfiguration(void) boardAlignmentMutable()->pitchDeciDegrees = 165; boardAlignmentMutable()->yawDeciDegrees = 0; - mixerConfigMutable()->yaw_jump_prevention_limit = 200; - imuConfigMutable()->small_angle = 30; gpsConfigMutable()->provider = GPS_UBLOX; @@ -174,7 +172,6 @@ void targetConfiguration(void) pidProfileMutable()->dterm_soft_notch_hz = 0; pidProfileMutable()->dterm_soft_notch_cutoff = 1; pidProfileMutable()->pidSumLimit = 500; - pidProfileMutable()->yaw_p_limit = 300; pidProfileMutable()->axisAccelerationLimitRollPitch = 0; pidProfileMutable()->axisAccelerationLimitYaw = 10000; diff --git a/src/main/target/FF_F35_LIGHTNING/target.h b/src/main/target/FF_F35_LIGHTNING/target.h index e50a43485d1..dec7d460e68 100644 --- a/src/main/target/FF_F35_LIGHTNING/target.h +++ b/src/main/target/FF_F35_LIGHTNING/target.h @@ -144,3 +144,7 @@ #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(2)) + +#ifdef USE_USB_MSC +# undef USE_USB_MSC +#endif diff --git a/src/main/target/FF_FORTINIF4/target.mk b/src/main/target/FF_FORTINIF4/target.mk index 581916350df..4faee84af70 100644 --- a/src/main/target/FF_FORTINIF4/target.mk +++ b/src/main/target/FF_FORTINIF4/target.mk @@ -1,7 +1,7 @@ F405_TARGETS += $(TARGET) -FEATURES += VCP ONBOARDFLASH +FEATURES += VCP ONBOARDFLASH MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu6500.c \ drivers/max7456.c \ - drivers/light_ws2811strip.c \ No newline at end of file + drivers/light_ws2811strip.c diff --git a/src/main/target/FF_PIKOF4/target.mk b/src/main/target/FF_PIKOF4/target.mk index 234c6ad4458..b268ca55885 100644 --- a/src/main/target/FF_PIKOF4/target.mk +++ b/src/main/target/FF_PIKOF4/target.mk @@ -1,9 +1,8 @@ F405_TARGETS += $(TARGET) -FEATURES += VCP ONBOARDFLASH +FEATURES += VCP ONBOARDFLASH MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu6000.c \ drivers/accgyro/accgyro_mpu6500.c \ drivers/max7456.c \ drivers/light_ws2811strip.c - \ No newline at end of file diff --git a/src/main/target/FIREWORKSV2/target.mk b/src/main/target/FIREWORKSV2/target.mk index b7423b68ca7..7d8a5945de4 100644 --- a/src/main/target/FIREWORKSV2/target.mk +++ b/src/main/target/FIREWORKSV2/target.mk @@ -1,5 +1,5 @@ F405_TARGETS += $(TARGET) -FEATURES += VCP ONBOARDFLASH +FEATURES += VCP ONBOARDFLASH MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu6000.c \ diff --git a/src/main/target/FISHDRONEF4/target.mk b/src/main/target/FISHDRONEF4/target.mk index ab900e2a149..5583e8d0b93 100644 --- a/src/main/target/FISHDRONEF4/target.mk +++ b/src/main/target/FISHDRONEF4/target.mk @@ -1,5 +1,5 @@ F405_TARGETS += $(TARGET) -FEATURES += SDCARD VCP ONBOARDFLASH +FEATURES += SDCARD VCP ONBOARDFLASH MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu6500.c \ diff --git a/src/main/target/FOXEERF405/target.mk b/src/main/target/FOXEERF405/target.mk index 23c3774fdfe..86025b89c11 100644 --- a/src/main/target/FOXEERF405/target.mk +++ b/src/main/target/FOXEERF405/target.mk @@ -1,5 +1,5 @@ F405_TARGETS += $(TARGET) -FEATURES += VCP ONBOARDFLASH +FEATURES += VCP ONBOARDFLASH MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu6000.c \ drivers/accgyro/accgyro_mpu6500.c \ diff --git a/src/main/target/FOXEERF722DUAL/target.mk b/src/main/target/FOXEERF722DUAL/target.mk index ea8a3a9209f..30faa4e33cf 100644 --- a/src/main/target/FOXEERF722DUAL/target.mk +++ b/src/main/target/FOXEERF722DUAL/target.mk @@ -1,5 +1,5 @@ F7X2RE_TARGETS += $(TARGET) -FEATURES += VCP ONBOARDFLASH +FEATURES += VCP ONBOARDFLASH MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu6000.c \ drivers/accgyro/accgyro_mpu6500.c \ diff --git a/src/main/target/FRSKYF4/target.mk b/src/main/target/FRSKYF4/target.mk index 526c6d61878..43d1b37fe9b 100755 --- a/src/main/target/FRSKYF4/target.mk +++ b/src/main/target/FRSKYF4/target.mk @@ -1,6 +1,6 @@ F405_TARGETS += $(TARGET) -FEATURES = VCP SDCARD +FEATURES = VCP SDCARD MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu6000.c \ diff --git a/src/main/target/FURYF4OSD/target.mk b/src/main/target/FURYF4OSD/target.mk index 654b3888b0a..0b76bb02356 100644 --- a/src/main/target/FURYF4OSD/target.mk +++ b/src/main/target/FURYF4OSD/target.mk @@ -1,5 +1,5 @@ F405_TARGETS += $(TARGET) -FEATURES += VCP ONBOARDFLASH +FEATURES += VCP ONBOARDFLASH MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu6500.c \ diff --git a/src/main/target/IFLIGHTF4_TWING/target.mk b/src/main/target/IFLIGHTF4_TWING/target.mk index dbbd64e7ad9..89793194927 100644 --- a/src/main/target/IFLIGHTF4_TWING/target.mk +++ b/src/main/target/IFLIGHTF4_TWING/target.mk @@ -1,6 +1,6 @@ F405_TARGETS += $(TARGET) -FEATURES += VCP ONBOARDFLASH +FEATURES += VCP ONBOARDFLASH MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu6500.c \ @@ -12,4 +12,4 @@ TARGET_SRC = \ drivers/compass/compass_mag3110.c \ drivers/compass/compass_lis3mdl.c \ drivers/light_ws2811strip.c \ - drivers/max7456.c \ No newline at end of file + drivers/max7456.c diff --git a/src/main/target/IFLIGHTF7_TWING/target.mk b/src/main/target/IFLIGHTF7_TWING/target.mk index 9aef8a877ce..57bc66e9e85 100644 --- a/src/main/target/IFLIGHTF7_TWING/target.mk +++ b/src/main/target/IFLIGHTF7_TWING/target.mk @@ -1,5 +1,5 @@ F7X2RE_TARGETS += $(TARGET) -FEATURES += ONBOARDFLASH VCP +FEATURES += ONBOARDFLASH VCP MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu6500.c \ diff --git a/src/main/target/KAKUTEF4/target.mk b/src/main/target/KAKUTEF4/target.mk index 74fab0fb57f..37de6e063eb 100755 --- a/src/main/target/KAKUTEF4/target.mk +++ b/src/main/target/KAKUTEF4/target.mk @@ -1,5 +1,5 @@ F405_TARGETS += $(TARGET) -FEATURES += VCP ONBOARDFLASH +FEATURES += VCP ONBOARDFLASH MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu6500.c \ diff --git a/src/main/target/KAKUTEF7/KAKUTEF7HDV.mk b/src/main/target/KAKUTEF7/KAKUTEF7HDV.mk new file mode 100644 index 00000000000..d99ea879d89 --- /dev/null +++ b/src/main/target/KAKUTEF7/KAKUTEF7HDV.mk @@ -0,0 +1 @@ +#KAKUTEF7HDV.mk file \ No newline at end of file diff --git a/src/main/target/KAKUTEF7/target.h b/src/main/target/KAKUTEF7/target.h index 0b65e77e189..01984ed350a 100644 --- a/src/main/target/KAKUTEF7/target.h +++ b/src/main/target/KAKUTEF7/target.h @@ -113,9 +113,12 @@ #define SPI4_MOSI_PIN PE6 #define USE_OSD + +#ifndef KAKUTEF7HDV #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN SPI2_NSS_PIN +#endif #if defined(KAKUTEF7MINI) #define M25P16_CS_PIN SPI1_NSS_PIN diff --git a/src/main/target/KAKUTEF7/target.mk b/src/main/target/KAKUTEF7/target.mk index 612199437ab..07e5ccfc947 100755 --- a/src/main/target/KAKUTEF7/target.mk +++ b/src/main/target/KAKUTEF7/target.mk @@ -2,7 +2,7 @@ F7X5XG_TARGETS += $(TARGET) ifeq ($(TARGET), KAKUTEF7MINI) FEATURES += VCP ONBOARDFLASH else -FEATURES += SDCARD VCP +FEATURES += SDCARD VCP MSC endif TARGET_SRC = \ diff --git a/src/main/target/MAMBAF405US/target.mk b/src/main/target/MAMBAF405US/target.mk index 43eadf6d0ca..9600e1937b8 100644 --- a/src/main/target/MAMBAF405US/target.mk +++ b/src/main/target/MAMBAF405US/target.mk @@ -1,5 +1,5 @@ F405_TARGETS += $(TARGET) -FEATURES += VCP ONBOARDFLASH +FEATURES += VCP ONBOARDFLASH MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu.c \ diff --git a/src/main/target/MAMBAF722/target.mk b/src/main/target/MAMBAF722/target.mk index 1902b593105..5a3023d0843 100644 --- a/src/main/target/MAMBAF722/target.mk +++ b/src/main/target/MAMBAF722/target.mk @@ -1,5 +1,5 @@ F7X2RE_TARGETS += $(TARGET) -FEATURES += VCP ONBOARDFLASH +FEATURES += VCP ONBOARDFLASH MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu.c \ diff --git a/src/main/target/MATEKF405/target.mk b/src/main/target/MATEKF405/target.mk index 1d0fa6dad92..dd5ce7864a0 100755 --- a/src/main/target/MATEKF405/target.mk +++ b/src/main/target/MATEKF405/target.mk @@ -1,5 +1,5 @@ F405_TARGETS += $(TARGET) -FEATURES += SDCARD VCP ONBOARDFLASH +FEATURES += SDCARD VCP ONBOARDFLASH MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu6500.c \ diff --git a/src/main/target/MATEKF405SE/target.mk b/src/main/target/MATEKF405SE/target.mk index 3a860b32837..8a0cc40ea76 100644 --- a/src/main/target/MATEKF405SE/target.mk +++ b/src/main/target/MATEKF405SE/target.mk @@ -1,5 +1,5 @@ F405_TARGETS += $(TARGET) -FEATURES += SDCARD VCP ONBOARDFLASH +FEATURES += SDCARD VCP ONBOARDFLASH MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu6000.c \ diff --git a/src/main/target/MATEKF411/target.h b/src/main/target/MATEKF411/target.h index bee9c0b9364..2a9bcbe83dc 100755 --- a/src/main/target/MATEKF411/target.h +++ b/src/main/target/MATEKF411/target.h @@ -175,3 +175,7 @@ #define TARGET_IO_PORTD (BIT(2)) #define MAX_PWM_OUTPUT_PORTS 7 + +#ifdef USE_USB_MSC +# undef USE_USB_MSC +#endif diff --git a/src/main/target/MATEKF411SE/target.h b/src/main/target/MATEKF411SE/target.h index 3a6ec05412c..9a0a3c5ef1c 100755 --- a/src/main/target/MATEKF411SE/target.h +++ b/src/main/target/MATEKF411SE/target.h @@ -162,3 +162,7 @@ #define TARGET_IO_PORTD (BIT(2)) #define MAX_PWM_OUTPUT_PORTS 6 + +#ifdef USE_USB_MSC +# undef USE_USB_MSC +#endif diff --git a/src/main/target/MATEKF722/target.mk b/src/main/target/MATEKF722/target.mk index 730cfa99000..f221bb51ca9 100755 --- a/src/main/target/MATEKF722/target.mk +++ b/src/main/target/MATEKF722/target.mk @@ -1,5 +1,5 @@ F7X2RE_TARGETS += $(TARGET) -FEATURES += SDCARD VCP +FEATURES += SDCARD VCP MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu6500.c \ @@ -15,4 +15,3 @@ TARGET_SRC = \ drivers/light_ws2811strip.c \ drivers/max7456.c \ drivers/pitotmeter_adc.c \ - diff --git a/src/main/target/MATEKF722PX/target.mk b/src/main/target/MATEKF722PX/target.mk index 6e4ae52c63a..5c061571fb6 100755 --- a/src/main/target/MATEKF722PX/target.mk +++ b/src/main/target/MATEKF722PX/target.mk @@ -1,5 +1,5 @@ F7X2RE_TARGETS += $(TARGET) -FEATURES += SDCARD VCP ONBOARDFLASH +FEATURES += SDCARD VCP ONBOARDFLASH MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu6000.c \ @@ -14,4 +14,3 @@ TARGET_SRC = \ drivers/compass/compass_lis3mdl.c \ drivers/light_ws2811strip.c \ drivers/pitotmeter_adc.c \ - diff --git a/src/main/target/MATEKF722SE/target.mk b/src/main/target/MATEKF722SE/target.mk index 71e499c0d93..63b91d30001 100644 --- a/src/main/target/MATEKF722SE/target.mk +++ b/src/main/target/MATEKF722SE/target.mk @@ -1,5 +1,5 @@ F7X2RE_TARGETS += $(TARGET) -FEATURES += SDCARD VCP ONBOARDFLASH +FEATURES += SDCARD VCP ONBOARDFLASH MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu6000.c \ @@ -17,4 +17,3 @@ TARGET_SRC = \ drivers/light_ws2811strip.c \ drivers/max7456.c \ drivers/pitotmeter_adc.c \ - diff --git a/src/main/target/MATEKF765/target.mk b/src/main/target/MATEKF765/target.mk index d90bdab6b1b..11d75e59b31 100644 --- a/src/main/target/MATEKF765/target.mk +++ b/src/main/target/MATEKF765/target.mk @@ -1,5 +1,5 @@ F7X5XI_TARGETS += $(TARGET) -FEATURES += SDCARD VCP +FEATURES += SDCARD VCP MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu6000.c \ @@ -16,4 +16,3 @@ TARGET_SRC = \ drivers/light_ws2811strip.c \ drivers/max7456.c \ drivers/pitotmeter_adc.c \ - diff --git a/src/main/target/NOX/target.mk b/src/main/target/NOX/target.mk index d0c27801a35..913dfd86a72 100755 --- a/src/main/target/NOX/target.mk +++ b/src/main/target/NOX/target.mk @@ -1,5 +1,5 @@ F411_TARGETS += $(TARGET) -FEATURES += VCP ONBOARDFLASH +FEATURES += VCP ONBOARDFLASH MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu6000.c \ @@ -7,4 +7,3 @@ TARGET_SRC = \ drivers/light_ws2811strip.c \ drivers/flash_m25p16.c \ drivers/max7456.c - diff --git a/src/main/target/OMNIBUSF4/OMNIBUSF4PRO.mk b/src/main/target/OMNIBUSF4/OMNIBUSF4PRO.mk index 7e8eafe084b..277ca008108 100755 --- a/src/main/target/OMNIBUSF4/OMNIBUSF4PRO.mk +++ b/src/main/target/OMNIBUSF4/OMNIBUSF4PRO.mk @@ -1,2 +1,2 @@ # the OMNIBUSF4SD has an SDCARD instead of flash, a BMP280 baro and therefore a slightly different ppm/pwm and SPI mapping -FEATURES = VCP SDCARD \ No newline at end of file +FEATURES = VCP SDCARD MSC diff --git a/src/main/target/OMNIBUSF4/OMNIBUSF4PRO_LEDSTRIPM5.mk b/src/main/target/OMNIBUSF4/OMNIBUSF4PRO_LEDSTRIPM5.mk index 4e7f4308fd5..3947ddaa4fb 100755 --- a/src/main/target/OMNIBUSF4/OMNIBUSF4PRO_LEDSTRIPM5.mk +++ b/src/main/target/OMNIBUSF4/OMNIBUSF4PRO_LEDSTRIPM5.mk @@ -1,2 +1,2 @@ # the OMNIBUSF4SD has an SDCARD instead of flash, a BMP280 baro and therefore a slightly different ppm/pwm and SPI mapping -FEATURES = VCP SDCARD \ No newline at end of file +FEATURES = VCP SDCARD MSC diff --git a/src/main/target/OMNIBUSF4/OMNIBUSF4V3.mk b/src/main/target/OMNIBUSF4/OMNIBUSF4V3.mk index d1b118b0db1..18df391c675 100755 --- a/src/main/target/OMNIBUSF4/OMNIBUSF4V3.mk +++ b/src/main/target/OMNIBUSF4/OMNIBUSF4V3.mk @@ -1,3 +1,3 @@ # OMNIBUSF4V3 is a (almost identical) variant of OMNIBUSF4PRO target, # except for an inverter on UART6. -FEATURES = VCP SDCARD +FEATURES = VCP SDCARD MSC diff --git a/src/main/target/OMNIBUSF4/target.mk b/src/main/target/OMNIBUSF4/target.mk index fa363a0f651..b6c11e7c820 100644 --- a/src/main/target/OMNIBUSF4/target.mk +++ b/src/main/target/OMNIBUSF4/target.mk @@ -1,5 +1,5 @@ F405_TARGETS += $(TARGET) -FEATURES += VCP ONBOARDFLASH SDCARD +FEATURES += VCP ONBOARDFLASH SDCARD MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu6000.c \ diff --git a/src/main/target/OMNIBUSF7/target.mk b/src/main/target/OMNIBUSF7/target.mk index 1fc43f1f964..30cffe44327 100644 --- a/src/main/target/OMNIBUSF7/target.mk +++ b/src/main/target/OMNIBUSF7/target.mk @@ -1,5 +1,5 @@ F7X5XG_TARGETS += $(TARGET) -FEATURES += SDCARD VCP +FEATURES += SDCARD VCP MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu.c \ @@ -15,4 +15,4 @@ TARGET_SRC = \ drivers/compass/compass_mag3110.c \ drivers/compass/compass_lis3mdl.c \ drivers/light_ws2811strip.c \ - drivers/max7456.c \ No newline at end of file + drivers/max7456.c diff --git a/src/main/target/OMNIBUSF7NXT/target.mk b/src/main/target/OMNIBUSF7NXT/target.mk index ff0b3579f3d..9be9afc5faf 100644 --- a/src/main/target/OMNIBUSF7NXT/target.mk +++ b/src/main/target/OMNIBUSF7NXT/target.mk @@ -1,5 +1,5 @@ F7X2RE_TARGETS += $(TARGET) -FEATURES += VCP ONBOARDFLASH +FEATURES += VCP ONBOARDFLASH MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu6000.c \ drivers/accgyro/accgyro_mpu6500.c \ diff --git a/src/main/target/PIXRACER/target.mk b/src/main/target/PIXRACER/target.mk index 258785f2f3e..4f4343a7bfa 100755 --- a/src/main/target/PIXRACER/target.mk +++ b/src/main/target/PIXRACER/target.mk @@ -1,5 +1,5 @@ F427_TARGETS += $(TARGET) -FEATURES += SDCARD VCP +FEATURES += SDCARD VCP MSC HSE_VALUE = 24000000 diff --git a/src/main/target/REVO/target.mk b/src/main/target/REVO/target.mk index b30794eff9a..ff9b702cdb5 100644 --- a/src/main/target/REVO/target.mk +++ b/src/main/target/REVO/target.mk @@ -1,5 +1,5 @@ F405_TARGETS += $(TARGET) -FEATURES += VCP ONBOARDFLASH +FEATURES += VCP ONBOARDFLASH MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu6000.c \ diff --git a/src/main/target/SPARKY2/target.mk b/src/main/target/SPARKY2/target.mk index 1487b00b103..64c49354947 100644 --- a/src/main/target/SPARKY2/target.mk +++ b/src/main/target/SPARKY2/target.mk @@ -1,5 +1,5 @@ F405_TARGETS += $(TARGET) -FEATURES += VCP ONBOARDFLASH +FEATURES += VCP ONBOARDFLASH MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu.c \ diff --git a/src/main/target/SPEEDYBEEF4/target.mk b/src/main/target/SPEEDYBEEF4/target.mk index 62ba0215c8d..481f0495d8e 100644 --- a/src/main/target/SPEEDYBEEF4/target.mk +++ b/src/main/target/SPEEDYBEEF4/target.mk @@ -1,5 +1,5 @@ F405_TARGETS += $(TARGET) -FEATURES += VCP ONBOARDFLASH +FEATURES += VCP ONBOARDFLASH MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu6000.c \ diff --git a/src/main/target/SPRACINGF4EVO/target.mk b/src/main/target/SPRACINGF4EVO/target.mk index 179fd78bc76..c586a9a0619 100755 --- a/src/main/target/SPRACINGF4EVO/target.mk +++ b/src/main/target/SPRACINGF4EVO/target.mk @@ -1,5 +1,5 @@ F405_TARGETS += $(TARGET) -FEATURES = VCP SDCARD +FEATURES = VCP SDCARD MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu6500.c \ @@ -14,5 +14,5 @@ TARGET_SRC = \ drivers/compass/compass_lis3mdl.c \ drivers/barometer/barometer_ms56xx.c \ drivers/light_ws2811strip.c \ - drivers/max7456.c + drivers/max7456.c # drivers/vtx_rtc6705.c diff --git a/src/main/target/SPRACINGF7DUAL/target.mk b/src/main/target/SPRACINGF7DUAL/target.mk index 47302073242..01a33f4d6f1 100644 --- a/src/main/target/SPRACINGF7DUAL/target.mk +++ b/src/main/target/SPRACINGF7DUAL/target.mk @@ -1,5 +1,5 @@ F7X2RE_TARGETS += $(TARGET) -FEATURES = VCP SDCARD +FEATURES = VCP SDCARD MSC TARGET_SRC = \ drivers/accgyro/accgyro_fake.c \ @@ -17,5 +17,3 @@ TARGET_SRC = \ drivers/light_ws2811strip.c \ drivers/max7456.c \ drivers/vtx_rtc6705_soft_spi.c - - diff --git a/src/main/target/YUPIF7/target.mk b/src/main/target/YUPIF7/target.mk index 1ad0bac85eb..7c4c979b9bb 100644 --- a/src/main/target/YUPIF7/target.mk +++ b/src/main/target/YUPIF7/target.mk @@ -1,5 +1,5 @@ F7X2RE_TARGETS += $(TARGET) -FEATURES += VCP ONBOARDFLASH +FEATURES += VCP ONBOARDFLASH MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu.c \ drivers/accgyro/accgyro_mpu6050.c \ diff --git a/src/main/target/common.h b/src/main/target/common.h index 8f1158ae849..c130c090917 100755 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -44,6 +44,10 @@ #define SKIP_CLI_RESOURCES #endif +#if defined(STM32F4) || defined(STM32F7) +#define USE_USB_MSC +#endif + #define USE_ADC_AVERAGING #define USE_64BIT_TIME #define USE_BLACKBOX diff --git a/src/main/vcp_hal/usbd_conf.h b/src/main/vcp_hal/usbd_conf.h index 086de15e944..aaef99aaf48 100644 --- a/src/main/vcp_hal/usbd_conf.h +++ b/src/main/vcp_hal/usbd_conf.h @@ -60,12 +60,13 @@ /* Exported types ------------------------------------------------------------*/ /* Exported constants --------------------------------------------------------*/ /* Common Config */ -#define USBD_MAX_NUM_INTERFACES 0 +#define USBD_MAX_NUM_INTERFACES 3 #define USBD_MAX_NUM_CONFIGURATION 1 #define USBD_MAX_STR_DESC_SIZ 0x100 #define USBD_SUPPORT_USER_STRING 0 #define USBD_SELF_POWERED 1 #define USBD_DEBUG_LEVEL 0 +#define MSC_MEDIA_PACKET 512 #define USE_USB_FS /* Exported macro ------------------------------------------------------------*/ diff --git a/src/main/vcp_hal/usbd_desc.c b/src/main/vcp_hal/usbd_desc.c index ef34a0932b9..48806761364 100644 --- a/src/main/vcp_hal/usbd_desc.c +++ b/src/main/vcp_hal/usbd_desc.c @@ -46,11 +46,13 @@ */ /* Includes ------------------------------------------------------------------*/ +#include #include "usbd_core.h" #include "usbd_desc.h" #include "usbd_conf.h" #include "platform.h" #include "build/version.h" +#include "drivers/usb_msc.h" /* Private typedef -----------------------------------------------------------*/ /* Private define ------------------------------------------------------------*/ @@ -114,6 +116,34 @@ __ALIGN_BEGIN uint8_t USBD_DeviceDesc[USB_LEN_DEV_DESC] __ALIGN_END = { USBD_MAX_NUM_CONFIGURATION /* bNumConfigurations */ }; /* USB_DeviceDescriptor */ +#ifdef USE_USB_MSC + +#define USBD_PID_MSC 22314 + +__ALIGN_BEGIN uint8_t USBD_MSC_DeviceDesc[USB_LEN_DEV_DESC] __ALIGN_END = +{ + 0x12, /*bLength */ + USB_DESC_TYPE_DEVICE, /*bDescriptorType*/ + 0x00, /*bcdUSB */ + 0x02, + 0x00, /*bDeviceClass*/ + 0x00, /*bDeviceSubClass*/ + 0x00, /*bDeviceProtocol*/ + USB_MAX_EP0_SIZE, /*bMaxPacketSize*/ + LOBYTE(USBD_VID), /*idVendor*/ + HIBYTE(USBD_VID), /*idVendor*/ + LOBYTE(USBD_PID_MSC), /*idProduct*/ + HIBYTE(USBD_PID_MSC), /*idProduct*/ + 0x00, /*bcdDevice rel. 2.00*/ + 0x02, + USBD_IDX_MFC_STR, /*Index of manufacturer string*/ + USBD_IDX_PRODUCT_STR, /*Index of product string*/ + USBD_IDX_SERIAL_STR, /*Index of serial number string*/ + USBD_MAX_NUM_CONFIGURATION /*bNumConfigurations*/ +}; +#endif + + /* USB Standard Device Descriptor */ #if defined ( __ICCARM__ ) /*!< IAR Compiler */ #pragma data_alignment=4 @@ -149,6 +179,12 @@ static void Get_SerialNum(void); uint8_t *USBD_VCP_DeviceDescriptor(USBD_SpeedTypeDef speed, uint16_t *length) { (void)speed; +#ifdef USE_USB_MSC + if (mscCheckBoot()) { + *length = sizeof(USBD_MSC_DeviceDesc); + return USBD_MSC_DeviceDesc; + } +#endif *length = sizeof(USBD_DeviceDesc); return (uint8_t*)USBD_DeviceDesc; } diff --git a/src/main/vcpf4/usbd_conf.h b/src/main/vcpf4/usbd_conf.h index 1978c8d9572..9ece6c3da54 100644 --- a/src/main/vcpf4/usbd_conf.h +++ b/src/main/vcpf4/usbd_conf.h @@ -31,7 +31,7 @@ */ #define USBD_CFG_MAX_NUM 1 #define USBD_ITF_MAX_NUM 1 -#define USB_MAX_STR_DESC_SIZ 50 +#define USB_MAX_STR_DESC_SIZ 255 /** @defgroup USB_VCP_Class_Layer_Parameter * @{ @@ -40,6 +40,9 @@ #define CDC_OUT_EP 0x01 /* EP1 for data OUT */ #define CDC_CMD_EP 0x82 /* EP2 for CDC commands */ +#define HID_IN_EP 0x83 +#define HID_IN_PACKET 8 + /* CDC Endpoints parameters: you can fine tune these values depending on the needed baudrates and performance. */ #ifdef USE_USB_OTG_HS #define CDC_DATA_MAX_PACKET_SIZE 512 /* Endpoint IN & OUT Packet size */ @@ -59,6 +62,23 @@ #define APP_TX_DATA_SIZE 2048 /* total size of the OUT (inbound to FC) buffer */ #endif /* USE_USB_OTG_HS */ +/* MSC */ +#define MSC_IN_EP 0x81 +#define MSC_OUT_EP 0x01 +#ifdef USE_USB_OTG_HS +#ifdef USE_ULPI_PHY +#define MSC_MAX_PACKET 512 +#else +#define MSC_MAX_PACKET 64 +#endif +#else /*USE_USB_OTG_FS*/ +#define MSC_MAX_PACKET 64 +#endif + + +#define MSC_MEDIA_PACKET 4096 +/* END MSC */ + #define APP_FOPS VCP_fops /** * @} @@ -97,4 +117,3 @@ #endif //__USBD_CONF__H__ /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ - diff --git a/src/main/vcpf4/usbd_desc.h b/src/main/vcpf4/usbd_desc.h index 30c6f9789a5..90aa0a87f0a 100644 --- a/src/main/vcpf4/usbd_desc.h +++ b/src/main/vcpf4/usbd_desc.h @@ -77,6 +77,13 @@ extern uint8_t USBD_OtherSpeedCfgDesc[USB_LEN_CFG_DESC]; extern uint8_t USBD_DeviceQualifierDesc[USB_LEN_DEV_QUALIFIER_DESC]; extern uint8_t USBD_LangIDDesc[USB_SIZ_STRING_LANGID]; extern USBD_DEVICE USR_desc; + +extern uint8_t USBD_DeviceDesc_MSC [USB_SIZ_DEVICE_DESC]; +extern uint8_t USBD_StrDesc_MSC[USB_MAX_STR_DESC_SIZ]; +extern uint8_t USBD_OtherSpeedCfgDesc_MSC[USB_LEN_CFG_DESC]; +extern uint8_t USBD_DeviceQualifierDesc_MSC[USB_LEN_DEV_QUALIFIER_DESC]; +extern uint8_t USBD_LangIDDesc_MSC[USB_SIZ_STRING_LANGID]; +extern USBD_DEVICE MSC_desc; /** * @} */ @@ -94,6 +101,14 @@ uint8_t * USBD_USR_SerialStrDescriptor( uint8_t speed , uint16_t *length); uint8_t * USBD_USR_ConfigStrDescriptor( uint8_t speed , uint16_t *length); uint8_t * USBD_USR_InterfaceStrDescriptor( uint8_t speed , uint16_t *length); +uint8_t * USBD_MSC_DeviceDescriptor( uint8_t speed , uint16_t *length); +uint8_t * USBD_MSC_LangIDStrDescriptor( uint8_t speed , uint16_t *length); +uint8_t * USBD_MSC_ManufacturerStrDescriptor ( uint8_t speed , uint16_t *length); +uint8_t * USBD_MSC_ProductStrDescriptor ( uint8_t speed , uint16_t *length); +uint8_t * USBD_MSC_SerialStrDescriptor( uint8_t speed , uint16_t *length); +uint8_t * USBD_MSC_ConfigStrDescriptor( uint8_t speed , uint16_t *length); +uint8_t * USBD_MSC_InterfaceStrDescriptor( uint8_t speed , uint16_t *length); + #ifdef USB_SUPPORT_USER_STRING_DESC uint8_t * USBD_USR_USRStringDesc (uint8_t speed, uint8_t idx , uint16_t *length); #endif /* USB_SUPPORT_USER_STRING_DESC */