diff --git a/.github/ISSUE_TEMPLATE/configuration-issue.md b/.github/ISSUE_TEMPLATE/configuration-issue.md new file mode 100644 index 0000000000..d2b60d68ed --- /dev/null +++ b/.github/ISSUE_TEMPLATE/configuration-issue.md @@ -0,0 +1,9 @@ +--- +name: Configuration Issue +about: Find help if you are struggling with configuring Betaflight + +--- + +**Important: This is not the right place to get help with configuration issues. Your issue will be closed without further comment.** + +Please get help from other user support options such as asking the manufacturer of the hardware you are using, RCGroups: https://rcgroups.com/forums/showthread.php?t=2464844, or Slack (registration at https://slack.betaflight.com/) or other user support forums & groups (e.g. Facebook). diff --git a/.github/ISSUE_TEMPLATE/feature-request.md b/.github/ISSUE_TEMPLATE/feature-request.md new file mode 100644 index 0000000000..ec295e0d9a --- /dev/null +++ b/.github/ISSUE_TEMPLATE/feature-request.md @@ -0,0 +1,19 @@ +--- +name: Feature Request +about: Suggest an idea for for a new feature for Betaflight + +--- + +**Is your feature request related to a problem? Please describe.** +A clear and concise description of what the problem is. Ex. I'm always frustrated when [...] + +**Describe the solution you'd like** +A clear and concise description of what you want to happen. + +**Describe alternatives you've considered** +A clear and concise description of any alternative solutions or features you've considered. + +**Additional context** +Add any other context or screenshots about the feature request here. + +Please note that feature requests are not 'fire and forget'. It is a lot more likely that the feature you would like to have will be implemented if you keep watching your feature request, and provide more details to developers looking into implementing your feature, and help them with testing. diff --git a/.github/ISSUE_TEMPLATE/firmware-bug-report.md b/.github/ISSUE_TEMPLATE/firmware-bug-report.md new file mode 100644 index 0000000000..86e7d66151 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/firmware-bug-report.md @@ -0,0 +1,25 @@ +--- +name: Firmware Bug Report +about: Create a report to help us fix bugs in the Betaflight firmware + +--- + +**Describe the bug** +A clear and concise description of what the bug is. + +**To Reproduce** +Steps to reproduce the behavior: + +**Expected behavior** +A clear and concise description of what you expected to happen. + +**Flight controller configuration** +Create a `diff` and post it here in a code block. Put ``` (three backticks) at the start and end of the `diff` block (instructions on how to do a diff: https://oscarliang.com/use-diff-not-dump-betaflight/) + +**Setup / Versions** + - Flight controller [what type is it, where was it bought from]; + - Other components [RX, VTX, brand / model for all of them, firmware version where applicable]; + - how are the different components wired up. + +**Additional context** +Add any other context about the problem here. diff --git a/.github/ISSUE_TEMPLATE/hardware-problem-report.md b/.github/ISSUE_TEMPLATE/hardware-problem-report.md new file mode 100644 index 0000000000..1e86f072e2 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/hardware-problem-report.md @@ -0,0 +1,9 @@ +--- +name: Hardware Problem Report +about: Report problems with faulty / broken hardware + +--- + +**Important: This is not the right place to report faulty / broken hardware. Your issue will be closed without further comment.** + +Contact the manufacturer or supplier of your hardware, or check RCGroups https://rcgroups.com/forums/showthread.php?t=2464844 to see if others with the same problem have found a solution. diff --git a/.github/issue_template.md b/.github/issue_template.md index 18d2a33ba3..b2c6264b56 100644 --- a/.github/issue_template.md +++ b/.github/issue_template.md @@ -1,6 +1,6 @@ # If your issue looks like a hardware fault or a configuration problem please don't raise an issue here. -## Please consider using other user support options such as asking the manufacturer of the hardware you are using, RCGroups: https://rcgroups.com/forums/showthread.php?t=2464844, or Slack: https://slack.betaflight.tech or other user support forums & groups (e.g. facebook). +## Please consider using other user support options such as asking the manufacturer of the hardware you are using, RCGroups: https://rcgroups.com/forums/showthread.php?t=2464844, or Slack (registration at https://slack.betaflight.com/) or other user support forums & groups (e.g. facebook). ## If you believe there is an issue with the firmware itself please follow these steps: 1. Describe your problem; diff --git a/.github/pull_request_template.md b/.github/pull_request_template.md index a8f3fe0868..23f8345637 100644 --- a/.github/pull_request_template.md +++ b/.github/pull_request_template.md @@ -1,14 +1,3 @@ -## Important: Feature freeze / release candidate phase for Betaflight 3.4 - -From 01/06/2018 until the release of Betaflight 3.4.0 (scheduled for 01/07/2018), the project is in a 'feature freeze / release candidate' phase. This means: - -1. Pull requests can still be submitted as normal. Comments / discussions will probably be slower than normal due to shifted priorities; - -2. If your pull request is a fix for an existing bug, or an update for a single target that has a low risk of side effect for other targets, it will be reviewed, and if accepted merged into `master` for the 3.4 release; - -3. All other pull requests will be scheduled for 3.5, and discussed / reviewed / merged into `master` after 3.4.0 has been released. Please keep in mind that this potentially means that you will have to rebase your changes if they are broken by bugfixes made to 3.4. - - ## Important considerations when opening a pull request: 1. Pull requests will only be accepted if they are opened against the `master` branch. Pull requests opened against other branches without prior consent from the maintainers will be closed; diff --git a/.travis.yml b/.travis.yml index 2c6eb9e53a..26c21c3290 100644 --- a/.travis.yml +++ b/.travis.yml @@ -53,10 +53,12 @@ before_script: - clang --version - clang++ --version - gcc --version + - export V=0 script: ./.travis.sh cache: + timeout: 1000 directories: - downloads - tools diff --git a/Makefile b/Makefile index c954a883ba..2a9d301945 100644 --- a/Makefile +++ b/Makefile @@ -206,10 +206,11 @@ CFLAGS += $(ARCH_FLAGS) \ $(addprefix -D,$(OPTIONS)) \ $(addprefix -I,$(INCLUDE_DIRS)) \ $(DEBUG_FLAGS) \ - -std=gnu99 \ + -std=gnu11 \ -Wall -Wextra -Wunsafe-loop-optimizations -Wdouble-promotion \ -ffunction-sections \ -fdata-sections \ + -fno-common \ -pedantic \ $(DEVICE_FLAGS) \ -D_GNU_SOURCE \ @@ -469,6 +470,44 @@ targets: @echo "targets-group-4: $(GROUP_4_TARGETS)" @echo "targets-group-rest: $(GROUP_OTHER_TARGETS)" + @echo "targets-group-1: $(words $(GROUP_1_TARGETS)) targets" + @echo "targets-group-2: $(words $(GROUP_2_TARGETS)) targets" + @echo "targets-group-3: $(words $(GROUP_3_TARGETS)) targets" + @echo "targets-group-4: $(words $(GROUP_4_TARGETS)) targets" + @echo "targets-group-rest: $(words $(GROUP_OTHER_TARGETS)) targets" + @echo "total in all groups $(words $(SUPPORTED_TARGETS)) targets" + +## target-mcu : print the MCU type of the target +target-mcu: + @echo $(TARGET_MCU) + +## targets-by-mcu : make all targets that have a MCU_TYPE mcu +targets-by-mcu: + @echo "Building all $(MCU_TYPE) targets..." + $(V1) for target in $(VALID_TARGETS); do \ + TARGET_MCU_TYPE=$$($(MAKE) -s TARGET=$${target} target-mcu); \ + if [ "$${TARGET_MCU_TYPE}" = "$${MCU_TYPE}" ]; then \ + echo "Building target $${target}..."; \ + $(MAKE) TARGET=$${target}; \ + if [ $$? -ne 0 ]; then \ + echo "Building target $${target} failed, aborting."; \ + exit 1; \ + fi; \ + fi; \ + done + +## targets-f3 : make all F3 targets +targets-f3: + $(V1) $(MAKE) -s targets-by-mcu MCU_TYPE=STM32F3 + +## targets-f4 : make all F4 targets +targets-f4: + $(V1) $(MAKE) -s targets-by-mcu MCU_TYPE=STM32F4 + +## targets-f7 : make all F7 targets +targets-f7: + $(V1) $(MAKE) -s targets-by-mcu MCU_TYPE=STM32F7 + ## test : run the cleanflight test suite ## junittest : run the cleanflight test suite, producing Junit XML result files. test junittest: diff --git a/README.md b/README.md index b0fb60863f..16c13f6dd7 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,3 @@ -![Important Notice: Support for STM32F1 based flight controllers has been dropped in Betaflight release 3.3. This includes NAZE, CC3D (original) and CJMCU like flight controllers](https://raw.githubusercontent.com/wiki/betaflight/betaflight/images/betaflight/stm32f1_support_notice.png) - - ![Betaflight](https://raw.githubusercontent.com/wiki/betaflight/betaflight/images/betaflight/bf_logo.png) Betaflight is flight controller software (firmware) used to fly multi-rotor craft and fixed wing craft. @@ -11,7 +8,8 @@ This fork differs from Baseflight and Cleanflight in that it focuses on flight p | Date | Event | | - | - | -| 01 July 2018 | Planned [release](https://github.com/betaflight/betaflight/milestone/7) date for Betaflight 3.4 | +| 01 December 2018 | Start of feature freeze / Release Candidate window for Betaflight 4.0 | +| 01 January 2019 | Planned [release](https://github.com/betaflight/betaflight/milestone/20) date for Betaflight 4.0 | ## Features @@ -20,7 +18,7 @@ Betaflight has the following features: * Multi-color RGB LED strip support (each LED can be a different color using variable length WS2811 Addressable RGB strips - use for Orientation Indicators, Low Battery Warning, Flight Mode Status, Initialization Troubleshooting, etc) * DShot (150, 300, 600 and 1200), Multishot, and Oneshot (125 and 42) motor protocol support * Blackbox flight recorder logging (to onboard flash or external microSD card where equipped) -* Support for targets that use the STM32 F7, F4, F3 and F1 processors +* Support for targets that use the STM32 F7, F4 and F3 processors * PWM, PPM, and Serial (SBus, SumH, SumD, Spektrum 1024/2048, XBus, etc) RX connection with failsafe detection * Multiple telemetry protocols (CSRF, FrSky, HoTT smart-port, MSP, etc) * RSSI via ADC - Uses ADC to read PWM RSSI signals, tested with FrSky D4R-II, X8R, X4R-SB, & XSR @@ -95,6 +93,7 @@ Origins for this fork (Thanks!): * **timecop** (for Baseflight), * **Dominic Clifton** (for Cleanflight), and * **Sambas** (for the original STM32F4 port). +* **borisbstyle** (Fork from Cleanflight). The Betaflight Configurator is forked from Cleanflight Configurator and its origins. diff --git a/_config.yml b/_config.yml new file mode 100644 index 0000000000..18854876c6 --- /dev/null +++ b/_config.yml @@ -0,0 +1 @@ +theme: jekyll-theme-midnight \ No newline at end of file diff --git a/docs/Cli.md b/docs/Cli.md index 0bf239f4f5..a1a1d2f2a7 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -279,3 +279,10 @@ Click on a variable to jump to the relevant documentation page. | `magzero_x` | Magnetometer calibration X offset | -32768 | 32767 | 0 | Master | INT16 | | `magzero_y` | Magnetometer calibration Y offset | -32768 | 32767 | 0 | Master | INT16 | | `magzero_z` | Magnetometer calibration Z offset | -32768 | 32767 | 0 | Master | INT16 | +| `vtx_band` | Configure the VTX band. Set to zero to use `vtx_freq`. Bands: 1: A, 2: B, 3: E, 4: F, 5: Race. | 0 | 5 | 4 | Master | UINT8 | +| `vtx_channel` | Channel to use within the configured `vtx_band`. Valid values are [1, 8]. | 1 | 8 | 1 | Master | UINT8 | +| `vtx_freq` | Set the VTX frequency using raw MHz. This parameter is ignored unless `vtx_band` is 0. | 0 | 5900 | 5740 | Master | UINT16 | +| `vtx_halfduplex` | Use half duplex UART to communicate with the VTX, using only a TX pin in the FC. | OFF | ON | ON | Master | UINT8 | +| `vtx_low_power_disarm` | When the craft is disarmed, set the VTX to its lowest power. `ON` will set the power to its minimum value on startup, increase it to `vtx_power` when arming and change it back to its lowest setting after disarming. `UNTIL_FIRST_ARM` will start with minimum power, but once the craft is armed it will increase to `vtx_power` and it will never decrease until the craft is power cycled. | | | OFF | Master | UINT8 | +| `vtx_pit_mode_freq` | Frequency to use (in MHz) when the VTX is in pit mode. | 0 | 5900 | 0 | Master | UINT16 | +| `vtx_power` | VTX RF power level to use. The exact number of mw depends on the VTX hardware. | 0 | 3 | 1 | Master | UINT8 | diff --git a/docs/Customized Version.md b/docs/Customized Version.md index 21a230d80c..ff82aefdeb 100644 --- a/docs/Customized Version.md +++ b/docs/Customized Version.md @@ -24,7 +24,7 @@ The 'text + data' gives you the flash size, and the 'data + bss' is the (static) ## Commons features for all Flight Controllers -The first file where the developers specify the features that are activated or not for a flight controller, is the `target/common_fc_pre.h`. +The first file where the developers specify the features that are activated or not for a flight controller, is the `target/common_pre.h`. This file specifies the features enabled/disabled depending on the memory flash size of the flight controller, or other conditions. @@ -98,9 +98,9 @@ After looking carefully to this file, you must know what features you want to di Each flight controller has it's own file to specify what features are enabled or disable only for it. Sometimes they have been disabled by space limitations, but other times it's for limited computing capacity or a bug, so enable it at your own risk. -This file is located in `target/[FLIGHT_CONTROLLER_NAME]/target.h` and it's loaded **after** the `target/common_fc_pre.h`. So any changes in this file will overwrite the default settings, so this file is the place where you must touch to create your custom firmware. +This file is located in `target/[FLIGHT_CONTROLLER_NAME]/target.h` and it's loaded **after** the `target/common_pre.h`. So any changes in this file will overwrite the default settings, so this file is the place where you must touch to create your custom firmware. -The first thing to do is to *#undef* all the features that we want to disable from the *common_fc_pre.h*. +The first thing to do is to *#undef* all the features that we want to disable from the *common_pre.h*. For example, in a NAZE32, if we're using Serial RX, with a FlySky receiver (that uses de iBus protocol) and we don't have a led strip we will add all this *#undef* to the file. diff --git a/docs/Gps.md b/docs/Gps.md index 01f170e3ae..6626ee8738 100644 --- a/docs/Gps.md +++ b/docs/Gps.md @@ -153,6 +153,7 @@ Module | Comments -------|-------- U-blox Neo-M8N w/Compass | Pinout can be found in Pixfalcon manual. SDA and SCL can be attached to I2C bus for compass, TX and RX can be attached to UART for GPS. Power must be applied for either to function. Reyax RY825AI | NEO-M8N, 18Hz UART USB interface GPS Glonass BeiDou QZSS antenna module flash. [eBay](http://www.ebay.com/itm/RY825AI-18Hz-UART-USB-interface-GPS-Glonass-BeiDou-QZSS-antenna-module-flash/181566850426) +mRo uGPS w/ LIS3MDL | Ultra compact and weights just 7.7 grams. Multiple constellation capabilities (GPS and GLONASS). Includes JST-GH pigtail. Available from [mRobotics](https://store.mrobotics.io/product-p/mro-ugps-samm8q-01.htm). #### NEO-7 Module | Comments diff --git a/docs/Modes.md b/docs/Modes.md index 1c5dde7ed9..3c576b145f 100644 --- a/docs/Modes.md +++ b/docs/Modes.md @@ -1,39 +1,47 @@ # Modes -Cleanflight has various modes that can be toggled on or off. Modes can be enabled/disabled by stick positions, -auxillary receiver channels and other events such as failsafe detection. +There are various modes that can be toggled on or off. Modes can be enabled/disabled by stick positions, auxillary receiver channels and other events such as failsafe detection. -| MSP ID | CLI ID | Short Name | Function | -| ------- | ------ | ---------- | -------------------------------------------------------------------- | -| 0 | 0 | ARM | Enables motors and flight stabilisation | -| 1 | 1 | ANGLE | Legacy auto-level flight mode | -| 2 | 2 | HORIZON | Auto-level flight mode | -| 3 | 3 | BARO | Altitude hold mode (Requires barometer sensor) | -| 4 | N/A | VARIO | Unused | -| 5 | 4 | MAG | Heading lock | -| 6 | 5 | HEADFREE | Head Free - When enabled yaw has no effect on pitch/roll inputs | -| 7 | 6 | HEADADJ | Heading Adjust - Sets a new yaw origin for HEADFREE mode | -| 8 | 7 | CAMSTAB | Camera Stabilisation | -| 9 | 8 | CAMTRIG | Unused | -| 10 | 9 | GPSHOME | Autonomous flight to HOME position | -| 11 | 10 | GPSHOLD | Maintain the same longitude/lattitude | -| 12 | 11 | PASSTHRU | Pass roll, yaw, and pitch directly from rx to servos in airplane mix | -| 13 | 12 | BEEPERON | Enable beeping - useful for locating a crashed aircraft | -| 14 | 13 | LEDMAX | | -| 15 | 14 | LEDLOW | | -| 16 | 15 | LLIGHTS | | -| 17 | 16 | CALIB | | -| 18 | 17 | GOV | Unused | -| 19 | 18 | OSD | Enable/Disable On-Screen-Display (OSD) | -| 20 | 19 | TELEMETRY | Enable telemetry via switch | -| 21 | 20 | GTUNE | G-Tune - auto tuning of Pitch/Roll/Yaw P values | -| 22 | 21 | SONAR | Altitude hold mode (sonar sensor only) | -| 23 | 22 | SERVO1 | Servo 1 | -| 24 | 23 | SERVO2 | Servo 2 | -| 25 | 24 | SERVO3 | Servo 3 | -| 26 | 25 | BLACKBOX | Enable BlackBox logging | -| 27 | 26 | FAILSAFE | Enter failsafe stage 2 manually | -| 28 | 27 | AIRMODE | Alternative mixer and additional PID logic for more stable copter | +| ID | Short Name | Function | +| -- | ------------------------ | ------------------------------------------------------------------------------------ | +| 0 | ARM | Enables motors and flight stabilisation | +| 1 | ANGLE | Legacy auto-level flight mode | +| 2 | HORIZON | Auto-level flight mode | +| 4 | ANTI GRAVITY | Prevents dips and rolls on fast throttle changes | +| 5 | MAG | Heading lock | +| 6 | HEADFREE | Head Free - When enabled yaw has no effect on pitch/roll inputs | +| 7 | HEADADJ | Heading Adjust - Sets a new yaw origin for HEADFREE mode | +| 8 | CAMSTAB | Camera Stabilisation | +| 12 | PASSTHRU | Pass roll, yaw, and pitch directly from rx to servos in airplane mix | +| 13 | BEEPERON | Enable beeping - useful for locating a crashed aircraft | +| 15 | LEDLOW | Switch off LED\_STRIP output | +| 17 | CALIB | Start in-flight calibration | +| 19 | OSD | Enable/Disable On-Screen-Display (OSD) | +| 20 | TELEMETRY | Enable telemetry via switch | +| 23 | SERVO1 | Servo 1 | +| 24 | SERVO2 | Servo 2 | +| 25 | SERVO3 | Servo 3 | +| 26 | BLACKBOX | Enable BlackBox logging | +| 27 | FAILSAFE | Enter failsafe stage 2 manually | +| 28 | AIRMODE | Alternative mixer and additional PID logic for more stable copter | +| 29 | 3D | Enable 3D mode | +| 30 | FPV ANGLE MIX | Apply yaw rotation relative to a FPV camera mounted at a preset angle | +| 31 | BLACKBOX ERASE | Erase the contents of the onboard flash log chip (takes > 30 s) | +| 32 | CAMERA CONTROL 1 | Control function 1 of the onboard camera (if supported) | +| 33 | CAMERA CONTROL 2 | Control function 2 of the onboard camera (if supported) | +| 34 | CAMERA CONTROL 3 | Control function 3 of the onboard camera (if supported) | +| 35 | FLIP OVER AFTER CRASH | Reverse the motors to flip over an upside down craft after a crash (DShot required) | +| 36 | BOXPREARM | When arming, wait for this switch to be activated before actually arming | +| 37 | BEEP GPS SATELLITE COUNT | Use a number of beeps to indicate the number of GPS satellites found | +| 39 | VTX PIT MODE | Switch the VTX into pit mode (low output power, if supported) | +| 40 | USER1 | User defined switch 1. Intended to be used to control an arbitrary output with PINIO | +| 41 | USER2 | User defined switch 2. Intended to be used to control an arbitrary output with PINIO | +| 42 | USER3 | User defined switch 3. Intended to be used to control an arbitrary output with PINIO | +| 43 | USER4 | User defined switch 4. Intended to be used to control an arbitrary output with PINIO | +| 44 | PID AUDIO | Enable output of PID controller state as audio | +| 45 | PARALYZE | Permanently disable a crashed craft until it is power cycled | +| 46 | GPS RESCUE | Enable 'GPS Rescue' to return the craft to the location where it was last armed | +| 47 | ACRO TRAINER | Enable 'acro trainer' angle limiting in acro mode | ## Auto-leveled flight @@ -57,29 +65,7 @@ In this mode, the "head" of the multicopter is always pointing to the same direc With this mode it is easier to control the multicopter, even fly it with the physical head towards you since the controls always respond the same. This is a friendly mode to new users of multicopters and can prevent losing the control when you don't know the head direction. -### GPS Return To Home - -WORK-IN-PROGRESS. This mode is not reliable yet, please share your experiences with the developers. - -In this mode the aircraft attempts to return to the GPS position recorded when the aircraft was armed. - -This mode should be enabled in conjunction with Angle or Horizon modes and an Altitude hold mode. - -Requires a 3D GPS fix and minimum of 5 satellites in view. - -### GPS Position Hold - -WORK-IN-PROGRESS. This mode is not reliable yet, please share your experiences with the developers. - -In this mode the aircraft attempts to stay at the same GPS position, as recorded when the mode is enabled. - -Disabling and re-enabling the mode will reset the GPS hold position. - -This mode should be enabled in conjunction with Angle or Horizon modes and an Altitude hold mode. - -Requires a 3D GPS fix and minimum of 5 satellites in view. - -## Airmode +### Airmode In the standard mixer / mode, when the roll, pitch and yaw gets calculated and saturates a motor, all motors will be reduced equally. When motor goes below minimum it gets clipped off. @@ -92,14 +78,14 @@ gliding and actobatics. But also the cornering / turns will be much tighter now possible correction performed. Airmode can also be enabled to work at all times by always putting it on the same switch like your arm switch or you can enable/disable it in air. Additional things and benefits: Airmode will additionally fully enable Iterm at zero throttle. Note that there is still some protection on the ground -when throttle zeroed (below min_check) and roll/pitch sticks centered. This is a basic protection to limit +when throttle zeroed (below min\check) and roll/pitch sticks centered. This is a basic protection to limit motors spooling up on the ground. Also the Iterm will be reset above 70% of stick input in acro mode to prevent quick Iterm windups during finishes of rolls and flips, which will provide much cleaner and more natural stops of flips and rolls what again opens the ability to have higher I gains for some. Note that AIRMODE will also overrule motor stop function! It will basically also act as an idle up switch. -## Auxillary Configuration +## Auxiliary Configuration Spare auxillary receiver channels can be used to enable/disable modes. Some modes can only be enabled this way. @@ -134,4 +120,3 @@ aux 0 0 0 1700 2100 ``` You can display the AUX configuration by using the `aux` command with no arguments. - diff --git a/docs/Release Notes/Betaflight 3.4 Release Notes.md b/docs/Release Notes/Betaflight 3.4 Release Notes.md new file mode 100644 index 0000000000..c34617ca36 --- /dev/null +++ b/docs/Release Notes/Betaflight 3.4 Release Notes.md @@ -0,0 +1,65 @@ +## Betaflight 3.4 Release Notes + +**Version 3.4.0: The best Betaflight ever!** + +We recognise that most of our users just want to two things with new firmware: Install it, and then go fly it. That's why we have spent a lot of time on working out default settings that fly great on most quads. Just install it and try for yourself! To make your craft fly even better, we've added a brand-new, experimental mode to improve the response to stick input by filtering it instead of interpolating. For more info on setting up 3.4 to optimise flight performance, read [these notes](https://github.com/betaflight/betaflight/wiki/Tuning-Tips-for-Betaflight-3.4). + +We've also put a lot of effort into optimising the performance of the firmware, especially for boards with an F7 MCU. Now you can go and buy that shiny new F7 board, while your friends still struggle with their boards with F3 and F4. :wink: + +For pilots who are into long range flying, we have added the brand new ['GPS Rescue' mode](https://github.com/betaflight/betaflight/wiki/GPS-rescue-mode). It is a simplified version of the 'return to home' mode that exists in other, more navigation oriented firmware, and the great news is that all it needs to work is a GPS, no fiddling to make a compass work required! + +Last but not least, we've also added a bunch of new features to improve the convenience of using Betaflight: You can now copy / paste your logs from an SD card or the on board flash chip by [mounting the flight controller as a storage device](https://github.com/betaflight/betaflight/wiki/Mass-Storage-Device-(MSC)-Support), and you can use your flight controller / TX to [emulate a joystick](https://github.com/betaflight/betaflight/wiki/HID-Joystick-Support) with no extra hardware needed, in order to fly on a simulator. + +If you are upgrading from an earlier version of Betaflight, please read the following section containing a list of things that you might have to change in your configuration. + +We have tried to make this release as bug free as possible. If you still find a **bug**, please report it back to us by opening an **issue [here](https://github.com/betaflight/betaflight/issues)**. + +Happy Props! + + +## Important information when upgrading + +- A number of changes and improvements in this release require changes to the Betaflight configurator. These changes have been added to Betaflight configurator 10.3.1 (installation instructions [here](https://github.com/betaflight/betaflight-configurator#installation)). Please update your Betaflight configurator to version 10.3.1. If you are using the Blackbox Log Viewer, there is an updated version 3.1.0 to go with Betaflight 3.4 (installation instructions [here](https://github.com/betaflight/blackbox-log-viewer#installation)); +- as part of the overhaul of the various filter stages, and improvements to the PID loop, the default settings have been re-evaluated and updated. The new default values are designed to make optimal use of the new filtering and improved PID loop, and be flyable with (almost) any hardware. Even if your current set up is working ok, it is probably worthwhile to try and only restore the non-filtering / PID loop settings, and have a go with the new default values (store your old `diff` in a safe place, just in case you don't like the new values :wink:). ([#6036](https://github.com/betaflight/betaflight/pull/6036)); +- also as part of the filtering overhaul, the names of the debug modes available to log filtering / tuning data have been improved `NOTCH` (gyro data after scaling, before filtering) is now `GYRO_SCALED`, `GYRO` (gyro data after all filtering has been applied) is now `GYRO_FILTERED` ([#6059](https://github.com/betaflight/betaflight/pull/6059)); +- the upper limit of `dterm_setpoint_weight` has been increased to 2000 (corresponding to a value of 20 for 'D Setpoint Weight' in Betaflight configurator). This means that pilots wanting a more 'locked in' stick feeling can increase this value beyond the previous maximum of 254. At the same time, an undocumented scaling change was reverted, and the scale is now again how it is described in the Betaflight configurator. If you are using a custom setting for `dterm_setpoint_weight`, divide your value by 1.27 to get the new value that will give you the same feeling as between 3.1.6 and 3.4.0 ([#5945](https://github.com/betaflight/betaflight/pull/5945), [#6052](https://github.com/betaflight/betaflight/pull/6052)); +- Dshot beacon configuraton has been changed. Now the `beacon` CLI command can be used analogous to how the `beeper` command is used. This allows for the Dshot beacon to be disabled individually for the conditions that are supported by it (`RX_SET` and `RX_LOST` at the moment). The old way of disabling the Dshot beacon by setting `beeper_dshot_beacon_tone` to `0` is no longer supported. The Dshot beacon is disabled for all conditions by default, if you want to enable it, use `beacon ` in CLI ([#5891](https://github.com/betaflight/betaflight/pull/5891), [#6070](https://github.com/betaflight/betaflight/pull/6070)); +- in previous versions of the firmware, there was a race condition that could cause Dshot commands (e.g. activation of crash flip) to be ignored by the ESC when the Dshot beacon was active. To prevent this, a timeout has been added to the Dshot beacon that prevents arming for 1.2 seconds after the Dshot beacon was active ([#6079](https://github.com/betaflight/betaflight/pull/6079)); +- validation has been added for the RSSI configuration. Unlike in previous versions, it is no longer possible to have multiple sources for RSSI configured simultaneously, since only one can be active at any one time. If you have got more than one of the supported RSSI sources (frame error count / ADC / RX channel) configured, all but the first on this list will be disabled ([#5644](https://github.com/betaflight/betaflight/pull/5644)); +- scaling has been added to all RSSI sources. If the RSSI mechanism that you are using does not give you the output range that you want for RSSI, you can now use the `rssi_scale` / `rssi_offset` CLI variables to set the scale and offset for RSSI ([#6001](https://github.com/betaflight/betaflight/pull/6001), [#6032](https://github.com/betaflight/betaflight/pull/6032)); +- the functionality of the crash flip mode has been improved: In addition to the existing front / back / left / right spinning of 2 propellers, it now supports spinning only 1 propeller (by moving the roll / pitch stick diagonally), and spinning 2 props that are diagonally opposite (by moving yaw), in order to yaw the overturned craft. The largest stick deflection in any of these directions determines which properllers are spun ([#5163](https://github.com/betaflight/betaflight/pull/5163)); +- the setting `moron_threshold` for the acceptable noise limit during gyro calibration was renamed to `gyro_calib_noise_limit`. Additionally, a new setting `gyro_calib_duration` was added. This allows users to configure a longer minimum gyro calibration duration (in 1/10ths of seconds, default: 125). Using a larger setting here will result in reduced gyro drift, which is helpful when flying line of sight ([#5932](https://github.com/betaflight/betaflight/pull/5932)); +- unfortunately bugfixes and improvements in the flight controller core functionality have led to an increase of the firmware size, causing it to overflow the available space on a number of F3 based flight controllers. As a result, some features have had to be removed from a number of F3 based flight controllers in order to make the firmware fit into flash. The following targets are affected: BETAFLIGHTF3, COLIBRI\_RACE, FRSKYF3, FURYF3OSD, LUX\_RACE, MIDELICF3, OMNIBUS, RCEXPLORERF3, RG\_SSD\_F3, SPRACINGF3EVO, SPRACINGF3NEO; +- the OSD elements `osd_crosshairs` (crosshairs) and `osd_ah_sbar` (artificial horizon sidebar) have been renamed in CLI to `osd_crosshairs_pos` and `osd_ah_sbar_pos` to make them consistent with the naming of OSD elements. If you are using these elements, please manually change the names in your backup before restoring ([#5534](https://github.com/betaflight/betaflight/pull/5534)); +- the range of the `vtx_band` parameter in CLI was extended to start at 0 instead of 1. Setting `vtx_band = 0` allows users of VTX using the SmartPort or Tramp protocols to set the desired frequency directly via the `vtx_freq` parameter. Since direct frequency setting is not supported by the RTC6705 (onboard) VTX driver `vtx_band = 0` does not work for these VTX, and should not be used ([#5465](https://github.com/betaflight/betaflight/pull/5465)). + + +## Major features: + +- Overhauled and improved filtering ([#5391](https://github.com/betaflight/betaflight/pull/5391), [#5458](https://github.com/betaflight/betaflight/pull/5458)); +- Optimised and massively improved the performance on F7 ([#5674](https://github.com/betaflight/betaflight/pull/5674)); +- Added GPS rescue mode ([#5753](https://github.com/betaflight/betaflight/pull/5753), [#5764](https://github.com/betaflight/betaflight/pull/5764)); +- Added support for accessing SD card / onboard flash as USB mass storage device (MSC) ([#5443](https://github.com/betaflight/betaflight/pull/5443), [#5629](https://github.com/betaflight/betaflight/pull/5629), [#5650](https://github.com/betaflight/betaflight/pull/5650)); +- Added support for reading RC input as USB joystick (HID) ([#5478](https://github.com/betaflight/betaflight/pull/5478), [#5596](https://github.com/betaflight/betaflight/pull/5596)); +- Added support for CMS configuration over CRSF ([#5743](https://github.com/betaflight/betaflight/pull/5743)); +- Added support for experimental filter based RC channel smoothing ([#6017](https://github.com/betaflight/betaflight/pull/6017)). + + +## Minor features: + +- Added acro trainer mode ([#5970](https://github.com/betaflight/betaflight/pull/5970)); +- Added throttle boost mode ([#5508](https://github.com/betaflight/betaflight/pull/5508)); +- Added support for throttle limiting ([#5608](https://github.com/betaflight/betaflight/pull/5608)); +- Added PID loop improvements ([#5968](https://github.com/betaflight/betaflight/pull/5968), [#5963](https://github.com/betaflight/betaflight/pull/5963), [#5962](https://github.com/betaflight/betaflight/pull/5962)); +- Added support for accelerated [yaw spin recovery](https://github.com/betaflight/betaflight/wiki/Yaw-Spin-Recovery-and-Gyro-Overflow-Detect) ([#5706](https://github.com/betaflight/betaflight/pull/5706)); +- Added support for direct adjustment of PID values through an RC channel ([#5584](https://github.com/betaflight/betaflight/pull/5584)); +- Added support for multiple overclocking speeds ([#5193](https://github.com/betaflight/betaflight/pull/5193)); +- Added MCU temperature monitoring ([#5322](https://github.com/betaflight/betaflight/pull/5322)); +- Added paralyse mode ([#5851](https://github.com/betaflight/betaflight/pull/5851)); +- Added support for QMC5883L compass ([#5309](https://github.com/betaflight/betaflight/pull/5309)); +- Added support for W25M flash chips ([#5722](https://github.com/betaflight/betaflight/pull/5722)). + + +## New targets: + +- Added SPRACINGF7DUAL with dual gyro support ([#5264](https://github.com/betaflight/betaflight/pull/5264)). diff --git a/docs/Release Notes/Betaflight 3.5 Release Notes.md b/docs/Release Notes/Betaflight 3.5 Release Notes.md new file mode 100644 index 0000000000..b8f20685d5 --- /dev/null +++ b/docs/Release Notes/Betaflight 3.5 Release Notes.md @@ -0,0 +1,35 @@ +## Betaflight 3.5 Release Notes + +**Wait, there is one more thing we can do to make it better!** + +This is what we realised about two months ago, as we were preparing for the release of Betaflight 3.4.0. And this is what led to the 'Feed Forward PID controller' being born. But when we came up with it it was already too late to add it to 3.4.0, and it needed some more refinement before it was ready to go out anyway. So we decided to do Betaflight 3.5.0, a release that focuses on more flight improvements. We put the Feed Forward PID controller into it, we have made the dynamic notch filter a whole lot more awesome, and we made improvements to how anti gravity works. Man, all of these improvements show when you fly it! + +To get the best out of the flight performance improvements, please read these [tuning tips](https://github.com/betaflight/betaflight/wiki/3.5-tuning-notes). + +If you are upgrading from an earlier version of Betaflight, please read the following section containing a list of things that you might have to change in your configuration. + +We have tried to make this release as bug free as possible. If you still find a **bug**, please report it back to us by opening an **issue [here](https://github.com/betaflight/betaflight/issues)**. + +We also have a Facebook Group: If you want to talk about Betaflight, ask configuration questions, or just hang out with fellow pilots, you can do this [here](https://www.facebook.com/groups/betaflightgroup/). + +Happy Props! + + +## Important information when upgrading + +- A number of changes and improvements in this release require changes to the Betaflight configurator. These changes have been added to [Betaflight configurator 10.4.0](https://github.com/betaflight/betaflight-configurator/releases/tag/10.4.0)(installation instructions [here](https://github.com/betaflight/betaflight-configurator#installation)), please update your Betaflight configurator to at least this version; +- if you are using the Blackbox Log Viewer, there is an updated [version 3.2.0](https://github.com/betaflight/blackbox-log-viewer/releases/tag/3.2.0) to go with Betaflight 3.5 (installation instructions [here](https://github.com/betaflight/blackbox-log-viewer#installation)). Please update to at least version 3.2.0; +- a new 'Feed Forward PID' algorithm has been implemented, replacing setpoint weight ([#6355](https://github.com/betaflight/betaflight/pull/6355)). In addition to this, the dynamic notch filter ([#6411](https://github.com/betaflight/betaflight/pull/6411)) and anti-gravity ([#6220](https://github.com/betaflight/betaflight/pull/6220)) have been optimised for improved flight performance. For all of these changes, default values have been chosen that should result in good flight characteristics for most setups. It is recommended to start testing with default settings, incorporating tuned settings from previous versions if needed, where needed. For more in-depth instructions for tuning Betaflight 3.5, please consult [these notes](https://github.com/betaflight/betaflight/wiki/3.5-tuning-notes). +- unfortunately, bugfixes in the flight controller core functionality have led to an increase of the firmware size, causing it to overflow the available space on a number of F3 based flight controllers. As a result, some features have had to be removed from a number of F3 based flight controllers in order to make the firmware fit into flash. The following targets are affected: CRAZYBEEF3FR, CRAZYBEEF3FS, FRSKYF3, FURYF3, FURYF3OSD, OMNIBUS, SPRACINGF3, SPRACINGF3EVO, SPRACINGF3MINI, SPRACINGF3NEO ([#6497](https://github.com/betaflight/betaflight/pull/6497), [#6501](https://github.com/betaflight/betaflight/pull/6501)) + + +## Major features: + +- Added support for feed forward to the PID controller ([#6355](https://github.com/betaflight/betaflight/pull/6355)). +- Improved the performance of the dynamic notch filter ([#6411](https://github.com/betaflight/betaflight/pull/6411)). + +## Minor features: + +- Improved the performance of anti-gravity ([#6220](https://github.com/betaflight/betaflight/pull/6220)); +- Added support for linking of modes ([#6335](https://github.com/betaflight/betaflight/pull/6335)); +- Added support for dynamic filter in dual gyro mode ([#6428](https://github.com/betaflight/betaflight/pull/6428)). diff --git a/docs/Rx.md b/docs/Rx.md index 63667858cb..b57f8f7b1e 100644 --- a/docs/Rx.md +++ b/docs/Rx.md @@ -167,6 +167,8 @@ Use a diode with cathode to receiver serial rx output (for example 1N4148), the anode is connected to the FC serial _TX_ pin, and also via a resistor (10KOhm) to the receiver ibus sensor port. +Note (2018-07-27): In some cases, the value of the series resistor may be too large, and going down to 1K[ohm] may provide a good result. + Enable with cli: ``` serial 1 1088 115200 57600 115200 115200 diff --git a/docs/Telemetry.md b/docs/Telemetry.md index 05ab199082..b9871c58d1 100644 --- a/docs/Telemetry.md +++ b/docs/Telemetry.md @@ -107,7 +107,10 @@ Use the latest Graupner firmware for your transmitter and receiver. Older HoTT transmitters required the EAM and GPS modules to be enabled in the telemetry menu of the transmitter. (e.g. on MX-20) -Serial ports use two wires but HoTT uses a single wire so some electronics are required so that the signals don't get mixed up. The TX and RX pins of +You can connect HoTT-Telemetry in two ways: + +#### Old way: +Serial ports use two wires but HoTT uses a single wire so some electronics are required so that the signals don't get mixed up. The TX and RX pins of a serial port should be connected using a diode and a single wire to the `T` port on a HoTT receiver. Connect as follows: @@ -122,6 +125,11 @@ The diode should be arranged to allow the data signals to flow the right way ``` 1N4148 diodes have been tested and work with the GR-24. + +When using the diode disable `tlm_halfduplex`, go to CLI and type `set tlm_halfduplex = OFF`, don't forget a `save` afterwards. + +#### New way: +You can use a single connection, connect HoTT RX/TX only to serial TX, leave serial RX open and make sure `tlm_halfduplex` is ON. As noticed by Skrebber the GR-12 (and probably GR-16/24, too) are based on a PIC 24FJ64GA-002, which has 5V tolerant digital pins. diff --git a/docs/Upgrading/Upgrading from 3.2 to 3.3.md b/docs/Upgrading/Upgrading from 3.2 to 3.3.md index 491b40b55b..636372fb9a 100644 --- a/docs/Upgrading/Upgrading from 3.2 to 3.3.md +++ b/docs/Upgrading/Upgrading from 3.2 to 3.3.md @@ -2,47 +2,47 @@ - To get optimal support for configuration and tuning of the firmware, please use the latest version of the Betaflight configurator (10.2.0 at the time of this release), available [here](https://github.com/betaflight/betaflight-configurator/releases); - If you are using OpenTX and the Betaflight lua scripts, please also make sure to use the latest version of these, available [here](https://github.com/betaflight/betaflight-tx-lua-scripts/releases); -- This release is introducing Runaway Takeoff Prevention (#4935). This feature will prevent uncontrollable acceleration ('tasmanian devil') when the craft is armed with misconfigured motor outputs or props on the wrong way, by disarming the craft if such a configuration is detected. It will also cause disarms when throttling up quickly on the bench with props off. If you get unwanted disarms right after arming, use the parameters (`runaway_takeoff_deactivate_delay`, `runaway_takeoff_deactivate_throttle_percent`) to tune the function to work for you, and please report back your working configuration. See the [wiki article](https://github.com/betaflight/betaflight/wiki/Runaway-Takeoff-Prevention) for details. +- This release is introducing Runaway Takeoff Prevention [#4935](https://github.com/betaflight/betaflight/pull/4935). This feature will prevent uncontrollable acceleration ('tasmanian devil') when the craft is armed with misconfigured motor outputs or props on the wrong way, by disarming the craft if such a configuration is detected. It will also cause disarms when throttling up quickly on the bench with props off. If you get unwanted disarms right after arming, use the parameters (`runaway_takeoff_deactivate_delay`, `runaway_takeoff_deactivate_throttle_percent`) to tune the function to work for you, and please report back your working configuration. See the [wiki article](https://github.com/betaflight/betaflight/wiki/Runaway-Takeoff-Prevention) for details. - The orientation of the AK8963 magnetometer has been changed, to make it match the orientation it has when it comes integrated in the MPU9250 gyro / accelerometer / magnetometer chip. If you are using an external AK8963 magnetometer, check your orientation to make sure it is still correct. If not, use the `align_mag` to configure the correct orientation; - More possible reasons for arming to be disabled have been added. Because of this, and because counting the 'arming disabled' beep indications was becoming difficult, the method of how the arming disabled reason is indicated has been changed. See the [wiki article](https://github.com/betaflight/betaflight/wiki/Arming-Sequence-&-Safety#arming-prevention) for details. - Arming is now disabled when the flight controller is connected to the Betaflight configurator. This was added in order to keep users from accidentally arming their craft when testing their RX / mode switches with a battery connected. In order to enable arming for bench testing, go to the 'Motors' tab and enable the 'Motor test mode' switch (**REMOVE ALL PROPELLERS FIRST**); - The parameter `sbus_inversion` has been changed into `serialrx_inverted`, and the way it is applied has been changed as follows: It now applies to all RX protocols, not just SBus, and instead of switching the UART to normal when off and inverted when on, 'off' now means that the port is set to whatever the default is for the selected protocol (i.e. inverted for SBus, not inverted for SUMD), and 'on' means that the port is inverted from default (i.e. not inverted for SBus, inverted for SUMD); -- The way that rates are configured has been changed (#4973). Betaflight now supports independent rates settings for roll / pitch / yaw. When updating from an older version of the firmware, make sure to convert your settings to the new parameters as follows: `rc_rate` becomes `roll_rc_rate` and `pitch_rc_rate`, `rc_rate_yaw` becomes `yaw_rc_rate`, `rc_expo` becomes `roll_expo` and `pitch_expo`, and `rc_expo_yaw` becomes `yaw_expo`. The same change also makes 'RaceFlight' type rate settings available. Set `rates_type = raceflight`. After this, 'rc_rate_' is RaceFlight 'rate' (scaled down by a factor of 10), `_expo` is RaceFlight 'expo', and `_srate` is RaceFlight 'acro+'; -- The `DISABLE 3D` mode is now called `DISABLE / SWITCH 3D` (#5179). In default configuration it works in the same way it used to work before. With `3d_switched_mode = on`, 'switched 3d mode' is enabled: With the 3D switch on, the throttle goes from forward thrust idle (min) to forward thrust full (max), which is the same as with `3d_switched_mode = off`. With the 3D switch off, the throttle goes from reversed thrust idle (min) to reversed thrust full (max). This allows the pilot to fly 3d by using the full throttle range in normal / reversed position, by switching motor directions with the switch when flipping over; +- The way that rates are configured has been changed [#4973](https://github.com/betaflight/betaflight/pull/4973). Betaflight now supports independent rates settings for roll / pitch / yaw. When updating from an older version of the firmware, make sure to convert your settings to the new parameters as follows: `rc_rate` becomes `roll_rc_rate` and `pitch_rc_rate`, `rc_rate_yaw` becomes `yaw_rc_rate`, `rc_expo` becomes `roll_expo` and `pitch_expo`, and `rc_expo_yaw` becomes `yaw_expo`. The same change also makes 'RaceFlight' type rate settings available. Set `rates_type = raceflight`. After this, 'rc_rate_' is RaceFlight 'rate' (scaled down by a factor of 10), `_expo` is RaceFlight 'expo', and `_srate` is RaceFlight 'acro+'; +- The `DISABLE 3D` mode is now called `DISABLE / SWITCH 3D` [#5179](https://github.com/betaflight/betaflight/pull/5179). In default configuration it works in the same way it used to work before. With `3d_switched_mode = on`, 'switched 3d mode' is enabled: With the 3D switch on, the throttle goes from forward thrust idle (min) to forward thrust full (max), which is the same as with `3d_switched_mode = off`. With the 3D switch off, the throttle goes from reversed thrust idle (min) to reversed thrust full (max). This allows the pilot to fly 3d by using the full throttle range in normal / reversed position, by switching motor directions with the switch when flipping over; - The `disarm_kill_switch` function that allowed (switch) disarming to be set up to be only possible on low throttle has been removed. There is no use case that requires it, and having it enabled introduces the safety risk of not being able to reliably disarm in an emergency; - The SmartAudio protocol implementation has been updated to be fully compliant with the SmartAudio specification. First generation AKK / RDQ VTX devices have a bug in their SmartAudio protocol implementation, causing them to fail to work with the SmartAudio protocol as implemented by Betaflight 3.3 (and KISS / RaceFlight). In order to not leave the owners of these VTX devices stranded, a branch off the Betaflight 3.3 maintenance branch has been created with a fix for these devices in it. This branch will be updated for all 3.3 patch releases. If you own one of these first generation AKK / RDQ VTX devices and are affected by this bug, please go to [this website](https://ci.betaflight.tech/job/Betaflight%20Maintenance%203.3%20%28AKK%20-%20RDQ%20VTX%20Patch%29/lastSuccessfulBuild/artifact/obj/) to download a Betaflight 3.3 version that works with your VTX device. (Go back to this page to download the latest version whenever a new version of Betaflight 3.3 has been released.) ## Major features: -- Added support for the FrSky FPort protocol (#4158); -- Added Spektrum VTX control (#4434); -- Added CMS configuration over Spektrum telemetry (#4545); -- Added FrSky X SPI RX protocol (#4683); -- Added fast Biquad RC+FIR2 filter (optimised version of Kalman gyro filter in #4890) (#4965); -- Added Runaway Takeoff Prevention (anti-taz) (#4935). +- Added support for the FrSky FPort protocol [#4158](https://github.com/betaflight/betaflight/pull/4158); +- Added Spektrum VTX control [#4434](https://github.com/betaflight/betaflight/pull/4434); +- Added CMS configuration over Spektrum telemetry [#4545](https://github.com/betaflight/betaflight/pull/4545); +- Added FrSky X SPI RX protocol [#4683](https://github.com/betaflight/betaflight/pull/4683); +- Added fast Biquad RC+FIR2 filter (optimised version of Kalman gyro filter in #4890) [#4965](https://github.com/betaflight/betaflight/pull/4965); +- Added Runaway Takeoff Prevention (anti-taz) [#4935](https://github.com/betaflight/betaflight/pull/4935). ## Minor features: -- Added CMS power menu (#3724); -- Added support for FlySky SPI receiver (#4060); -- Added use TIM_UP and DMAR for all timer channels with Dshot (#4073, #4843, #4852); -- Added '3D on a switch' mode (#4227); -- Added Dshot beacon activation to BEEPER_RX_LOST_LANDING (#4231); -- Added generic RunCam device protocol support (#4251), see the [wiki article](https://github.com/betaflight/betaflight/wiki/RunCam-Device-Protocol) for details; -- Added a reasonable default OSD layout (#4260); -- Added handling and display of date and time (#4289); -- Added MSP command to disable arming (#4320); -- Added support for Spektrum real RSSI from SRXL Rx and fake RSSI from both int and ext bound satellites (#4347); -- Changed after flight OSD statistics screen to only show when enabled (#4428); -- Added remaining time estimate based on flight used mAh rate to OSD (#4487, #4543, #4618); -- Added setting of RSSI value with MSP (#4507); -- Improved SmartAudio update frequency (make it QuietAudio) (#4532); -- Updated PID calculations to use actual deltaT (#4556); -- Added AND logic to modes (#4722); -- Add TCM support to F7 (#4757); -- Added Benewake TFmini/TF02 rangefinder support (#4793); -- Added selectable RaceFlight rates (#4973); -- Added KN (NRF24) SPI RX protocol (#4994); -- Added Spektrum VTX status via telemtry (#5081); -- Added PINIOBOX BOX to PINIO general purpose pin output mapper (#5110). +- Added CMS power menu [#3724](https://github.com/betaflight/betaflight/pull/3724); +- Added support for FlySky SPI receiver [#4060](https://github.com/betaflight/betaflight/pull/4060); +- Added use TIM_UP and DMAR for all timer channels with Dshot ([#4073](https://github.com/betaflight/betaflight/pull/4073), [#4843](https://github.com/betaflight/betaflight/pull/4843), [#4852](https://github.com/betaflight/betaflight/pull/4852)); +- Added '3D on a switch' mode [#4227](https://github.com/betaflight/betaflight/pull/4227); +- Added Dshot beacon activation to BEEPER_RX_LOST_LANDING [#4231](https://github.com/betaflight/betaflight/pull/4231); +- Added generic RunCam device protocol support [#4251](https://github.com/betaflight/betaflight/pull/4251), see the [wiki article](https://github.com/betaflight/betaflight/wiki/RunCam-Device-Protocol) for details; +- Added a reasonable default OSD layout [#4260](https://github.com/betaflight/betaflight/pull/4260); +- Added handling and display of date and time [#4289](https://github.com/betaflight/betaflight/pull/4289); +- Added MSP command to disable arming [#4320](https://github.com/betaflight/betaflight/pull/4320); +- Added support for Spektrum real RSSI from SRXL Rx and fake RSSI from both int and ext bound satellites [#4347](https://github.com/betaflight/betaflight/pull/4347); +- Changed after flight OSD statistics screen to only show when enabled [#4428](https://github.com/betaflight/betaflight/pull/4428); +- Added remaining time estimate based on flight used mAh rate to OSD ([#4487](https://github.com/betaflight/betaflight/pull/4487), [#4543](https://github.com/betaflight/betaflight/pull/4543), [#4618](https://github.com/betaflight/betaflight/pull/4618)); +- Added setting of RSSI value with MSP [#4507](https://github.com/betaflight/betaflight/pull/4507); +- Improved SmartAudio update frequency (make it QuietAudio) [#4532](https://github.com/betaflight/betaflight/pull/4532); +- Updated PID calculations to use actual deltaT [#4556](https://github.com/betaflight/betaflight/pull/4556); +- Added AND logic to modes [#4722](https://github.com/betaflight/betaflight/pull/4722); +- Add TCM support to F7 [#4757](https://github.com/betaflight/betaflight/pull/4757); +- Added Benewake TFmini/TF02 rangefinder support [#4793](https://github.com/betaflight/betaflight/pull/4793); +- Added selectable RaceFlight rates [#4973](https://github.com/betaflight/betaflight/pull/4973); +- Added KN (NRF24) SPI RX protocol [#4994](https://github.com/betaflight/betaflight/pull/4994); +- Added Spektrum VTX status via telemtry [#5081](https://github.com/betaflight/betaflight/pull/5081); +- Added PINIOBOX BOX to PINIO general purpose pin output mapper [#5110](https://github.com/betaflight/betaflight/pull/5110). diff --git a/docs/boards/Board - AikonF4.md b/docs/boards/Board - AikonF4.md index f6ba9874f4..b58bca3813 100644 --- a/docs/boards/Board - AikonF4.md +++ b/docs/boards/Board - AikonF4.md @@ -44,5 +44,5 @@ Andrey Mironov (@DieHertz) ## FAQ & Known Issues * First revision has no diode protecting the 5V input on the 11-pin ESC connector, make sure to pull the 5V wire coming from our ESC in order to avoid two regulators fighting against each other * First revision has RX4 (ESC Telemetry) and GND pad silkscreen swapped near motor outputs -![Aikon F4 top](aikon-f4-rev1-top.jpg) -![Aikon F4 bottom](aikon-f4-rev1-bottom.jpg) +![Aikon F4 top](images/aikon-f4-rev1-top.jpg) +![Aikon F4 bottom](images/aikon-f4-rev1-bottom.jpg) diff --git a/docs/boards/Board - CrazyBeeF3FR.md b/docs/boards/Board - CrazyBeeF3FR.md index ed05243b59..2238e42dd7 100644 --- a/docs/boards/Board - CrazyBeeF3FR.md +++ b/docs/boards/Board - CrazyBeeF3FR.md @@ -1,9 +1,11 @@ # CrazyBee F3 FR -![CrazyBee F3 FR front](CrazyBeeF3FRtop.jpg) -![CrazyBee F3 FR back](CrazyBeeF3FRbottom.jpg) +![CrazyBee F3 FR front](images/CrazyBeeF3FRtop.jpg) +![CrazyBee F3 FR back](images/CrazyBeeF3FRbottom.jpg) + ## Description CrazyBee F3 flight controller is a Highly integrated board for 1S Whoop brushless racing drone. It might be the world first Tiny whoop size brushless flight controller which integrated Receiver/4in1 ESC/OSD/Current Meter. + ## MCU, Sensors and Features ### Hardware and Features @@ -23,11 +25,8 @@ It might be the world first Tiny whoop size brushless flight controller which in - Beeper output: 2-pin, soldering pad - 4 Rx Indicating LEDs: 2 x red and 2 x white - - ## Resource mapping - | Label | Pin | Timer | DMA | Default | Note | |----------------------------|------|-------|-----|-------------|----------------------------------| | MPU6000_INT_EXTI | PC13 | | | | | @@ -63,12 +62,26 @@ https://www.banggood.com/Racerstar-Crazybee-F3-Flight-Controller-4-IN-1-5A-1S-Bl ## Designers - - ## Maintainers - ## FAQ & Known Issues + - The board specifications claim DSHOT600-ready, but due to the use of a type L (BB1 24MHz) ESC, only DSHOT300 is reliably supported, although DSHOT600 seems to be working for quite a few people. But just how clean that ESC control signal is when using DSHOT600, is untested. For a discussion on this, see https://www.rcgroups.com/forums/showthread.php?3036325-Racerstar-Crazybee-F3-Ultimate-Micro-AIO-FC%21-1S-5A-BlheliS-Frsky-Flysky-OSD/page3 . +- The factory default GYRO / PID config is 8KHz / 2KHz . There are reports that this may lead to possible instability and 4KHz / 4KHz is recommended. + +Specific information for the factory supplied Betaflight 3.3.0 version: + +- The DSHOT beeper function does not work. +- When turtle mode ("Flip over after Crash") is activated, the FC will only arm if the failsafe timeout is 1s (10 * 0.1s) or more. If the timeout is set lower than that, you may see the motors shortly try to spin up and then stop, and then the quad will not be armed. Rumour has it that this is fixed in BF 3.4 . + +FRSKY Version: +- To bind to your Taranis, you need to be running the non-eu OpenTX version, which allows you to use the required D8 setting to bind to the RX. The factory default BF receiver mode is FRSKY_X, so remember to configure this if needed. +- FrSky X (8 / 16 channels) and FrSky D (8 channels) work both reliably, including in combination with crash flip / Dshot beacon, as long as the TELEMETRY feature is disabled; +Basic telemetry information like RSSI and battery voltage will be sent even when the TELEMETRY feature is disabled; +- On FrSky D, the TELEMETRY feature causes occasional dropouts, depending on how many sensors (BARO, GPS, ...) are enabled, probably due to a timing overrun; +- On FrSky X, the TELEMETRY feature causes hard lockups due to a bug in the telemetry generation code. + + ## Other Resources - User Manual: http://img.banggood.com/file/products/20180209021414Crazybeef3.pdf \ No newline at end of file + User Manual: http://img.banggood.com/file/products/20180209021414Crazybeef3.pdf + diff --git a/docs/boards/Board - CrazyBeeF3FS.md b/docs/boards/Board - CrazyBeeF3FS.md index 471dbb7a9c..1c9b1bd607 100644 --- a/docs/boards/Board - CrazyBeeF3FS.md +++ b/docs/boards/Board - CrazyBeeF3FS.md @@ -1,6 +1,6 @@ # CrazyBee F3 FS -![CrazyBee F3 FS front](CrazyBeeF3FStop.jpg) -![CrazyBee F3 FS back](CrazyBeeF3FSbottom.jpg) +![CrazyBee F3 FS front](images/CrazyBeeF3FStop.jpg) +![CrazyBee F3 FS back](images/CrazyBeeF3FSbottom.jpg) ## Description CrazyBee F3 FS flight controller is a Highly integrated board for 1S Whoop brushless racing drone. It might be the world first Tiny whoop size brushless flight controller which integrated Receiver/4in1 ESC/OSD/Current Meter. @@ -63,12 +63,11 @@ https://www.banggood.com/Racerstar-Crazybee-F3-Flight-Controller-4-IN-1-5A-1S-Bl ## Designers - - ## Maintainers - ## FAQ & Known Issues +See the same item for [CrazyBeeF3FR](Board%20-%20CrazyBeeF3FR.md#FAQ%20&%20Known%20ssues) + ## Other Resources - User Manual: http://img.banggood.com/file/products/20180225215703Crazybeef3flysky.pdf \ No newline at end of file + User Manual: http://img.banggood.com/file/products/20180225215703Crazybeef3flysky.pdf diff --git a/docs/boards/Board - DALRCF722DUAL.md b/docs/boards/Board - DALRCF722DUAL.md new file mode 100644 index 0000000000..2b1ea289f0 --- /dev/null +++ b/docs/boards/Board - DALRCF722DUAL.md @@ -0,0 +1,113 @@ +# Board - DALRCF722DUAL + +The DALRCF722DUAL described here: + +This board use the STM32F722RET6 microcontroller and have the following features: +* High-performance and DSP with FPU, ARM Cortex-M7 MCU with 512 Kbytes Flash +* 216 MHz CPU,462 DMIPS/2.14 DMIPS/MHz (Dhrystone 2.1) and DSP instructions, Art Accelerator, L1 cache, SDRAM +* DUAL gyro MPU6000 and ICM20602,could choose mpu6000,more stable and smooth.Or choose ICM20602,higher rate(32K/32K).Choose on CLI mode,experience different feature gyro on same board +* OSD on board +* The 16M byte SPI flash on board for data logging +* USB VCP and boot select button on board(for DFU) +* Stable voltage regulation,DUAL BEC 5v/2.5A and 10v/2A for VTX/camera etc.And could select 5v/10v with pad +* Serial LED interface(LED_STRIP) +* VBAT/CURR/RSSI sensors input +* Suppose IRC Tramp/smart audio/FPV Camera Control/FPORT/telemetry +* Supports SBus(built-in inverters), Spektrum1024/2048, PPM +* Supports I2C device extend(baro/compass/OLED etc)(socket) +* Supports GPS (socket) + +### All uarts have pad on board +| Value | Identifier | RX | TX | Notes | +| ----- | ------------ | -----| -----| ------------------------------------------------------------------------------------------- | +| 1 | USART1 | PB7 | PB6 | PB7 FOR SBUS IN(inverter build in)/PPM | +| 2 | USART2 | PA3 | PA2 | PAD USE FOR TRAMP/smart audio | +| 3 | USART3 | PC11 | PC10| USE FOR GPS | +| 4 | USART4 | PA1 | PA0 | PA0 FOR RSSI/FPORT/TEL etc | +| 5 | USART5 | PD2 | PC12| PAD | + + +### I2C with GPS port together.Use for BARO or compass etc +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ---------| -------| ------------------------------------------------------------------------------------- | +| 1 | I2C1 | SDA | PB9 | +| 2 | I2C1 | SCL | PB8 | + + +### Buzzer/LED output +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ---------| -------| ------------------------------------------------------------------------------------- | +| 1 | LED0 | LED | PC14 |On board +| 2 | BEEPER | BEE | PC13 |Pad + + +### VBAT input with 1/10 divider ratio,Current signal input,Analog/digit RSSI input +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ----------| ------| ------------------------------------------------------------------------------------- | +| 1 | ADC1 | VBAT | PC1 | +| 2 | ADC1 | CURR | PC0 | +| 3 | ADC1 | RSSI | PA0 | + + +### 8 Outputs +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ----------| ------| ------------------------------------------------------------------------------------- | +| 1 | TIM4_CH2 | PPM | PB7 | PPM +| 2 | TIM8_CH1 | OUPUT1 | PC6 | DMA +| 3 | TIM8_CH2 | OUPUT2 | PC7 | DMA +| 4 | TIM8_CH3 | OUPUT3 | PC8 | DMA +| 5 | TIM8_CH4 | OUPUT4 | PC9 | DMA +| 6 | TIM1_CH1 | OUPUT5 | PA8 | DMA +| 7 | TIM1_CH2 | OUPUT6 | PA9 | DMA +| 8 | TIM1_CH3 | OUPUT7 | PA10 | DMA NO PAD +| 9 | TIM2_CH4 | OUPUT8 | PB11 | NO PAD +| 10 | TIM2_CH1 | PWM | PA15 | DMA LED_STRIP +| 11 | TIM3_CH4 | PWM | PB1 | FPV Camera Control(FCAM) + + +### Gyro & ACC ,support ICM20602 and MPU6000 +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ---------| -------| ------------------------------------------------------------------------------------- | +| 1 | SPI1 | SCK | PA5 | MPU6000 & ICM20602 +| 2 | SPI1 | MISO | PA6 | MPU6000 & ICM20602 +| 3 | SPI1 | MOSI | PA7 | MPU6000 & ICM20602 +| 4 | SPI1 | CS1 | PB0 | MPU6000 +| 5 | SPI1 | CS2 | PA4 | ICM20602 +| 6 | SPI1 | INT1 | PB10 | MPU6000 +| 7 | SPI1 | INT2 | PC4 | ICM20602 + +### OSD MAX7456 +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ---------| -------| ------------------------------------------------------------------------------------- | +| 1 | SPI2 | SCK | PB13 | +| 2 | SPI2 | MISO | PB14 | +| 3 | SPI2 | MOSI | PB15 | +| 4 | SPI2 | CS | PB12 | + +### 16Mbyte flash +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ---------| -------| ------------------------------------------------------------------------------------- | +| 1 | SPI3 | SCK | PB3 | +| 2 | SPI3 | MISO | PB4 | +| 3 | SPI3 | MOSI | PB4 | +| 4 | SPI3 | CS | PB2 | + +### SWD +| Pin | Function | Notes | +| --- | -------------- | -------------------------------------------- | +| 1 | SWCLK | PAD | +| 2 | Ground | PAD | +| 3 | SWDIO | PAD | +| 4 | 3V3 | PAD | + +![DALRCF722DUAL top](images/DALRCF722DUAL-Top.png) +![DALRCF722DUAL bottom](images/DALRCF722DUAL-Botton.jpg) + + +###Designers +* ZhengNyway(nyway@vip.qq.com) + + + + + diff --git a/docs/boards/Board - FLYWOOF405.md b/docs/boards/Board - FLYWOOF405.md new file mode 100755 index 0000000000..b7f5787e43 --- /dev/null +++ b/docs/boards/Board - FLYWOOF405.md @@ -0,0 +1,101 @@ +# Board - FLYWOOF405 + +This board use the STM32F405RGT6 microcontroller and have the following features: +* 1024K bytes of flash memory,192K bytes RAM,168 MHz CPU/210 DMIPS + +* The 16M byte SPI flash for data logging +* USB VCP and boot select button on board(for DFU) +* Stable voltage regulation,9V/1.5A DCDC BEC for VTX/camera etc.And could select 5v/9v with pad +* Serial LED interface(LED_STRIP) +* VBAT/CURR/RSSI sensors input +* Suppose IRC Tramp/smart audio/FPV Camera Control/FPORT/telemetry +* Supports SBus, Spektrum1024/2048, PPM. No external inverters required (built-in). +* Supports I2C device extend(baro/compass/OLED etc) +* Supports GPS + +### All uarts have pad on board +| Value | Identifier | RX | TX | Notes | +| ----- | ------------ | -----| -----| ------------------------------------------------------------------------------------------- | +| 1 | USART1 | PA10 | PB6 | USE smartport/FPORT/TEL etc | +| 2 | USART3 | PB11 | PB10| FOR SBUS IN(inverter build in) | +| 3 | USART4 | PA1 | PA0 | PAD USE FOR TRAMP/smart audio | +| 4 | USART5 | PD2 | / | PAD ESC sensor | +| 5 | USART6 | PC7 | PC6 | PAD USE FOR GPS/BLE etc | + + +### I2C with GPS port together.Use for BARO or compass etc +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ---------| -------| ------------------------------------------------------------------------------------- | +| 1 | I2C1 | SDA | PB9 | with GPS outlet +| 2 | I2C1 | SCL | PB8 | with GPS outlet + + +### Buzzer/LED output +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ---------| -------| ------------------------------------------------------------------------------------- | +| 1 | LED0 | LED | PC14 | +| 2 | BEEPER | BEE | PC13 | + + +### VBAT input with 1/10 divider ratio,Current signal input,Analog/digit RSSI input +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ----------| ------| ------------------------------------------------------------------------------------- | +| 1 | ADC1 | VBAT | PC3 | DMA2_Stream0 +| 2 | ADC1 | CURR | PC2 | DMA2_Stream0 +| 3 | ADC1 | RSSI | PC1 | DMA2_Stream0 + + +### 8 Outputs, 1 PPM input +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ----------| ------| ------------------------------------------------------------------------------------- | +| 1 | TIM10_CH1 | PPM | PB7 | PPM +| 2 | TIM3_CH3 | OUPUT1 | PB0 | DMA1_Stream7 +| 3 | TIM3_CH4 | OUPUT2 | PB1 | DMA1_Stream2 +| 4 | TIM2_CH4 | OUPUT3 | PA3 | DMA1_Stream6 +| 5 | TIM2_CH3 | OUPUT4 | PA2 | DMA1_Stream1 +| 6 | TIM3_CH2 | OUPUT5 | PB5 | DMA1_Stream5 +| 7 | TIM4_CH2 | OUPUT6 | PB7 | DMA1_Stream3 +| 8 | TIM8_CH4 | OUPUT7 | PC9 | DMA2_Stream7 +| 9 | TIM3_CH1 | OUPUT8 | PB4 | DMA1_Stream4 +| 10 | TIM8_CH3 | LED | PC8 | DMA2_Stream2 LED_STRIP +| 11 | TIM1_CH2 | PWM | PA9 | FPV Camera Control(FCAM) + + +### Gyro & ACC ICM20689 +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ---------| -------| ------------------------------------------------------------------------------------- | +| 1 | SPI1 | SCK | PA5 | +| 2 | SPI1 | MISO | PA6 | +| 3 | SPI1 | MOSI | PA7 | +| 4 | SPI1 | CS | PC4 | + +### OSD MAX7456 +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ---------| -------| ------------------------------------------------------------------------------------- | +| 1 | SPI3 | SCK | PC10 | +| 2 | SPI3 | MISO | PC11 | +| 3 | SPI3 | MOSI | PC12 | +| 4 | SPI3 | CS | PB14 | + +### 16Mbyte flash +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ---------| -------| ------------------------------------------------------------------------------------- | +| 1 | SPI3 | SCK | PC10 | +| 2 | SPI3 | MISO | PC11 | +| 3 | SPI3 | MOSI | PC12 | +| 4 | SPI3 | CS | PB3 | + +### SWD +| Pin | Function | Notes | +| --- | -------------- | -------------------------------------------- | +| 1 | SWCLK | PAD | +| 2 | Ground | PAD | +| 3 | SWDIO | PAD | +| 4 | 3V3 | PAD | + +* FLYWOO TECH + + + + + diff --git a/docs/boards/Board - FOXEERF722DUAL.md b/docs/boards/Board - FOXEERF722DUAL.md new file mode 100644 index 0000000000..6e168aaac4 --- /dev/null +++ b/docs/boards/Board - FOXEERF722DUAL.md @@ -0,0 +1,103 @@ +# Board - FOXEERF722DUAL + +The FOXEERF722DUAL described here: + +This board use the STM32F722RET6 microcontroller and have the following features: +* High-performance and DSP with FPU, ARM Cortex-M7 MCU with 512 Kbytes Flash +* 216 MHz CPU,462 DMIPS/2.14 DMIPS/MHz (Dhrystone 2.1) and DSP instructions, Art Accelerator, L1 cache, SDRAM +* DUAL gyro MPU6000 and ICM20602,could choose mpu6000,more stable and smooth.Or choose ICM20602,higher rate(32K/32K).Choose on CLI mode,experience different feature gyro on same board +* OSD on board +* The 16M byte SPI flash on board for data logging +* USB VCP and boot select button on board(for DFU) +* Stable voltage regulation,DUAL BEC 5v/2.5A and 9v/2A for VTX/camera etc.And could select 5v/9v with pad +* Serial LED interface(LED_STRIP) +* VBAT/CURR/RSSI sensors input +* Suppose IRC Tramp/smart audio/FPV Camera Control/FPORT/telemetry +* Supports SBus, Spektrum1024/2048, PPM +* Supports I2C device extend(baro/compass/OLED etc) +* Supports GPS + +### All uarts have pad on board +| Value | Identifier | RX | TX | Notes | +| ----- | ------------ | -----| -----| ------------------------------------------------------------------------------------------- | +| 1 | USART1 | PB7 | PB6 | PB7 FOR SBUS IN(inverter build in)/PPM | +| 2 | USART2 | PA3 | PA2 | PAD USE FOR TRAMP/smart audio | +| 3 | USART3 | PB11 | PB10| USE FOR GPS | +| 4 | USART4 | PA1 | PA0 | PA0 FOR RSSI/FPORT/TEL etc | +| 5 | USART5 | PD2 | PC12| PAD | + + +### I2C with GPS port together.Use for BARO or compass etc +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ---------| -------| ------------------------------------------------------------------------------------- | +| 1 | I2C1 | SDA | PB9 | outlet +| 2 | I2C1 | SCL | PB8 | outlet + + +### Buzzer/LED output +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ---------| -------| ------------------------------------------------------------------------------------- | +| 1 | LED0 | LED | PC15 |On board +| 2 | BEEPER | BEE | PA4 |Pad + + +### VBAT input with 1/10 divider ratio,Current signal input,Analog/digit RSSI input +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ----------| ------| ------------------------------------------------------------------------------------- | +| 1 | ADC1 | VBAT | PC0 | On board +| 2 | ADC1 | CURR | PC2 | +| 3 | ADC1 | RSSI | PA0 | + + +### 8 Outputs +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ----------| ------| ------------------------------------------------------------------------------------- | +| 1 | TIM4_CH2 | PPM | PB7 | PPM +| 2 | TIM1_CH2 | OUPUT1 | PA9 | DMA +| 3 | TIM1_CH1 | OUPUT2 | PA8 | DMA +| 4 | TIM8_CH4 | OUPUT3 | PC9 | DMA +| 5 | TIM8_CH3 | OUPUT4 | PC8 | DMA +| 6 | TIM8_CH1 | OUPUT5 | PC6 | DMA +| 7 | TIM8_CH2 | OUPUT6 | PC7 | DMA +| 8 | TIM2_CH1 | PWM | PA15 | DMA LED_STRIP +| 9 | TIM2_CH2 | PWM | PB3 | FPV Camera Control(FCAM) + + +### Gyro & ACC ,support ICM20602 and MPU6000 +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ---------| -------| ------------------------------------------------------------------------------------- | +| 1 | SPI1 | SCK | PA5 | MPU6000 & ICM20602 +| 2 | SPI1 | MISO | PA6 | MPU6000 & ICM20602 +| 3 | SPI1 | MOSI | PA7 | MPU6000 & ICM20602 +| 4 | SPI1 | CS1 | PB2 | MPU6000 +| 5 | SPI1 | CS2 | PB1 | ICM20602 +| 6 | SPI1 | INT1 | PC4 | MPU6000 +| 7 | SPI1 | INT2 | PB0 | ICM20602 + +### OSD MAX7456 +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ---------| -------| ------------------------------------------------------------------------------------- | +| 1 | SPI3 | SCK | PC10 | +| 2 | SPI3 | MISO | PC11 | +| 3 | SPI3 | MOSI | PB5 | +| 4 | SPI3 | CS | PC3 | + +### 16Mbyte flash +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ---------| -------| ------------------------------------------------------------------------------------- | +| 1 | SPI2 | SCK | PB13 | +| 2 | SPI2 | MISO | PB14 | +| 3 | SPI2 | MOSI | PB15 | +| 4 | SPI2 | CS | PB12 | + +### SWD +| Pin | Function | Notes | +| --- | -------------- | -------------------------------------------- | +| 1 | SWCLK | PAD | +| 2 | Ground | PAD | +| 3 | SWDIO | PAD | +| 4 | 3V3 | PAD | + + +###Designers +* NywayZheng(nyway@vip.qq.com) \ No newline at end of file diff --git a/docs/boards/Board - HAKRCF722.md b/docs/boards/Board - HAKRCF722.md new file mode 100755 index 0000000000..85b4c04ece --- /dev/null +++ b/docs/boards/Board - HAKRCF722.md @@ -0,0 +1,122 @@ +# Board - HAKRCF722 + + +### Hardware and Features + + - STM32CubeMX + - MCU STM32F722RET6 + - IMU: MPU6000 (SPI) + - VCP: yes + - OSD: Betaflight OSD + - Battery Voltage Sensor: yes + - Stable voltage regulation,9V/2A DCDC BEC for VTX/camera etc.And could select 5v/9v with pad + - Serial LED interface(LED_STRIP) + - 5 UART + - BARO: QMP6988/BMP280(I2C_1) + - GPS + - + + +PERIPHERALS MODES FUNCTIONS PINS +ADC1 IN10 ADC1_IN10 PC0 +ADC1 IN11 ADC1_IN11 PC1 +ADC1 IN12 ADC1_IN12 PC2 +I2C1 SMBus-two-wire-Interface I2C1_SCL PB8 +I2C1 SMBus-two-wire-Interface I2C1_SDA PB9 +RCC Crystal/Ceramic Resonator RCC_OSC_IN PH0-OSC_IN +RCC Crystal/Ceramic Resonator RCC_OSC_OUT PH1-OSC_OUT +SPI1 Full-Duplex Master SPI1_MISO PA6 +SPI1 Full-Duplex Master SPI1_MOSI PA7 +SPI1 Full-Duplex Master SPI1_SCK PB3 +SPI1 Hardware NSS Input Signal SPI1_NSS PA4 +SPI2 Full-Duplex Master SPI2_MISO PB14 +SPI2 Full-Duplex Master SPI2_MOSI PC3 +SPI2 Full-Duplex Master SPI2_SCK PB13 +SPI2 Hardware NSS Output Signal SPI2_NSS PB12 +SPI3 Full-Duplex Master SPI3_MISO PC11 +SPI3 Full-Duplex Master SPI3_MOSI PB5 +SPI3 Full-Duplex Master SPI3_SCK PC10 +SYS SysTick SYS_VS_Systick VP_SYS_VS_Systick +TIM1 Output Compare CH1 TIM1_CH1 PA8 +TIM1 Output Compare CH3 TIM1_CH3 PA10 +TIM2 Output Compare CH1 TIM2_CH1 PA5 +TIM3 Output Compare CH2 TIM3_CH2 PC7 +TIM3 Output Compare CH3 TIM3_CH3 PB0 +TIM3 Output Compare CH4 TIM3_CH4 PB1 +TIM4 Output Compare CH1 TIM4_CH1 PB6 +TIM8 Output Compare CH1 TIM8_CH1 PC6 +TIM8 Output Compare CH3 TIM8_CH3 PC8 +TIM8 Output Compare CH4 TIM8_CH4 PC9 +TIM12 Input Capture direct mode TIM12_CH2 PB15 +UART4 Multiprocessor Communication UART4_RX PA1 +UART4 Multiprocessor Communication UART4_TX PA0-WKUP +UART5 Multiprocessor Communication UART5_RX PD2 +UART5 Multiprocessor Communication UART5_TX PC12 +USART1 Multiprocessor Communication USART1_RX PB7 +USART1 Multiprocessor Communication USART1_TX PA9 +USART2 Multiprocessor Communication USART2_RX PA3 +USART2 Multiprocessor Communication USART2_TX PA2 +USART3 Multiprocessor Communication USART3_RX PB11 +USART3 Multiprocessor Communication USART3_TX PB10 + + + +Pin Nb PINs FUNCTIONs LABELs +2 PC13-ANTI_TAMP GPIO_Output LED0 +3 PC14-OSC32_IN GPIO_Output Beep +5 PH0-OSC_IN RCC_OSC_IN +6 PH1-OSC_OUT RCC_OSC_OUT +8 PC0 ADC1_IN10 RSSI +9 PC1 ADC1_IN11 VBAT +10 PC2 ADC1_IN12 CURR +11 PC3 SPI2_MOSI +14 PA0-WKUP UART4_TX extend/UART4_TX +15 PA1 UART4_RX +16 PA2 USART2_TX +17 PA3 USART2_RX +20 PA4 SPI1_NSS +21 PA5 TIM2_CH1 +22 PA6 SPI1_MISO +23 PA7 SPI1_MOSI +24 PC4 GPIO_Output SPI1_CS +26 PB0 TIM3_CH3 OUPUT1 +27 PB1 TIM3_CH4 OUPUT6 +29 PB10 USART3_TX +30 PB11 USART3_RX +33 PB12 SPI2_NSS +34 PB13 SPI2_SCK +35 PB14 SPI2_MISO +36 PB15 TIM12_CH2 PPM +37 PC6 TIM8_CH1 OUPUT2 +38 PC7 TIM3_CH2 OUPUT7 +39 PC8 TIM8_CH3 OUPUT5 +40 PC9 TIM8_CH4 OUPUT8 +41 PA8 TIM1_CH1 OUPUT4 +42 PA9 USART1_TX +43 PA10 TIM1_CH3 OUPUT3 +46 PA13* SYS_JTMS-SWDIO +49 PA14* SYS_JTCK-SWCLK +50 PA15* SPI3_NSS +51 PC10 SPI3_SCK +52 PC11 SPI3_MISO +53 PC12 UART5_TX +54 PD2 UART5_RX +55 PB3 SPI1_SCK +56 PB4 GPIO_Output Inverter +57 PB5 SPI3_MOSI +58 PB6 TIM4_CH1 LED1 +59 PB7 USART1_RX USART1_RX +61 PB8 I2C1_SCL +62 PB9 I2C1_SDA + + +## Manufacturers and Distributors + +HAKRC Loopur + + +## Designers + +HAKRC Loopur + + diff --git a/docs/boards/Board - SKYZONEF405.md b/docs/boards/Board - SKYZONEF405.md new file mode 100644 index 0000000000..907820c7f8 --- /dev/null +++ b/docs/boards/Board - SKYZONEF405.md @@ -0,0 +1,67 @@ +# SKYZONEF405 + +## Description + +The SKYZONEF405 has been designed as a versatile flight controller, to allow efficient control of craft with 4, 6 or 8 motors. + +It is available in 2 variants: + +### SKYZONEF405\_STD + +Standard 30 * 30 mm size + +![SKYZONEF405\_STD top](images/skyzonef405_std_front.jpg) +![SKYZONEF405\_STD bottom](images/skyzonef405_std_back.jpg) + +### SKYZONEF405\_PDB + +30 x 30 mm 'extended corners' design with integrated PDB / current sensing shunt. + +![SKYZONEF405\_PDB top](images/skyzonef405_pdb_front.jpg) +![SKYZONEF405\_PDB bottom](images/skyzonef405_pdb_back.jpg) + + +## Hardware + +Type|Description +---|--- +MCU|STM32F405 +IMU|ICM-20689 +IMU Interrupt|yes +Motor outputs|up to 8 +Barometer|optional +Magnetometer|optional +VCP (supporting joystick emulation)|yes +Hardware serial|5 (1 with inverter for SBus) +Software serial|2 +OSD|yes +Blackbox|16 MB onboard flash +PPM in|yes +Camera control output|yes +LED strip (WS2811) output|yes +Battery voltage sensor|yes +Current sensor|yes (SKYZONEF405\_PDB only) +Integrated voltage regulator|yes +Buttons|boot +LEDs|power, activity + +## Features + +Plenty of connectivity options, including 5 UARTs, most of them with through-hole solder points, minimising the risk of ripped off solder pads. + +ESC signal / telemetry solder points next to the corners for motors 1 - 4 (SKYZONEF405\_PDB only). + + +## Manufacturers and Distributors + +skyzone Hobbies + + +## Designers + +skyzone Hobbies + + +## Maintainers + +[Daniel Zhou](mailto:daniel@skyzonehobbies.com) diff --git a/docs/boards/Board - SPEDIXF4.md b/docs/boards/Board - SPEDIXF4.md new file mode 100644 index 0000000000..f84ecdcabe --- /dev/null +++ b/docs/boards/Board - SPEDIXF4.md @@ -0,0 +1,31 @@ +# SPEDIX F4 + +## Description +One of the main considerations while designing the SPEDIX F4 was to provide users the maximum amount of UARTs, as well as motor outputs, while retaining maximum efficiency, a careful choice of pinmap allowed to run 6-8 DSHOT motors off just 2 timers, while breaking out all the 6 UARTs that the STM32F405 provides. +Timer design favors running multirotor and fixed wing alike. +Plug & play connectivity to SPEDIX and AIKON 4in1 ESCs, + +## MCU, Sensors and Features + +### Hardware + - MCU: STM32F405 + - IMU: ICM-20602 or MPU-6000 + - 6 DSHOT motors outputs, 8 by remapping UART6 as M7 & 8, using just 2 timers + - BMP280 SPI + - 6 hardware UARTs, UART5 with a controllable inverter for SBUS, USART1 with bidirectional inverter for FPORT/SPORT + - Onboard regulator supports up to 6S + - Dataflash blackbox + - External I2C port + - JST-SH 10 pin 4in1 ESC plug + + +## Designers +Kyle Lee (SPEDIX) +Andrey Mironov (@DieHertz) + + +## Maintainers +Andrey Mironov (@DieHertz) + + +![SPEDIX F4 top](images/spedixf4-top.png) diff --git a/docs/boards/CrazyBeeF3FRbottom.jpg b/docs/boards/images/CrazyBeeF3FRbottom.jpg similarity index 100% rename from docs/boards/CrazyBeeF3FRbottom.jpg rename to docs/boards/images/CrazyBeeF3FRbottom.jpg diff --git a/docs/boards/CrazyBeeF3FRtop.jpg b/docs/boards/images/CrazyBeeF3FRtop.jpg similarity index 100% rename from docs/boards/CrazyBeeF3FRtop.jpg rename to docs/boards/images/CrazyBeeF3FRtop.jpg diff --git a/docs/boards/CrazyBeeF3FSbottom.jpg b/docs/boards/images/CrazyBeeF3FSbottom.jpg similarity index 100% rename from docs/boards/CrazyBeeF3FSbottom.jpg rename to docs/boards/images/CrazyBeeF3FSbottom.jpg diff --git a/docs/boards/CrazyBeeF3FStop.jpg b/docs/boards/images/CrazyBeeF3FStop.jpg similarity index 100% rename from docs/boards/CrazyBeeF3FStop.jpg rename to docs/boards/images/CrazyBeeF3FStop.jpg diff --git a/docs/boards/images/DALRCF722DUAL-Botton.jpg b/docs/boards/images/DALRCF722DUAL-Botton.jpg new file mode 100644 index 0000000000..6d6377d5da Binary files /dev/null and b/docs/boards/images/DALRCF722DUAL-Botton.jpg differ diff --git a/docs/boards/images/DALRCF722DUAL-Top.png b/docs/boards/images/DALRCF722DUAL-Top.png new file mode 100644 index 0000000000..c77179a4e2 Binary files /dev/null and b/docs/boards/images/DALRCF722DUAL-Top.png differ diff --git a/docs/boards/aikon-f4-rev1-bottom.jpg b/docs/boards/images/aikon-f4-rev1-bottom.jpg similarity index 100% rename from docs/boards/aikon-f4-rev1-bottom.jpg rename to docs/boards/images/aikon-f4-rev1-bottom.jpg diff --git a/docs/boards/aikon-f4-rev1-top.jpg b/docs/boards/images/aikon-f4-rev1-top.jpg similarity index 100% rename from docs/boards/aikon-f4-rev1-top.jpg rename to docs/boards/images/aikon-f4-rev1-top.jpg diff --git a/docs/boards/images/skyzonef405_pdb_back.jpg b/docs/boards/images/skyzonef405_pdb_back.jpg new file mode 100644 index 0000000000..46f78cb554 Binary files /dev/null and b/docs/boards/images/skyzonef405_pdb_back.jpg differ diff --git a/docs/boards/images/skyzonef405_pdb_front.jpg b/docs/boards/images/skyzonef405_pdb_front.jpg new file mode 100644 index 0000000000..8539f1e6cb Binary files /dev/null and b/docs/boards/images/skyzonef405_pdb_front.jpg differ diff --git a/docs/boards/images/skyzonef405_std_back.jpg b/docs/boards/images/skyzonef405_std_back.jpg new file mode 100644 index 0000000000..769002b198 Binary files /dev/null and b/docs/boards/images/skyzonef405_std_back.jpg differ diff --git a/docs/boards/images/skyzonef405_std_front.jpg b/docs/boards/images/skyzonef405_std_front.jpg new file mode 100644 index 0000000000..e749266bc7 Binary files /dev/null and b/docs/boards/images/skyzonef405_std_front.jpg differ diff --git a/docs/boards/images/spedixf4-top.png b/docs/boards/images/spedixf4-top.png new file mode 100644 index 0000000000..b410f5100b Binary files /dev/null and b/docs/boards/images/spedixf4-top.png differ diff --git a/docs/development/Building in Eclipse.md b/docs/development/Building in Eclipse.md index 06c55fb91d..bccc53f8aa 100644 --- a/docs/development/Building in Eclipse.md +++ b/docs/development/Building in Eclipse.md @@ -1,6 +1,6 @@ # Building in Eclipse -How to build, test & debug Cleanflight in Eclipse on Linux, Windows & MacOS. +How to build, test & debug Betaflight in Eclipse on Linux, Windows & MacOS. ## Checklist @@ -17,7 +17,7 @@ Use this checklist to make sure you didn't miss a step. Versions mandated below - [ ] Optionally [Download and Install](https://github.com/gnuarmeclipse/qemu/releases) the latest GNU ARM Eclipse QEMU [read more](#install-qemu) - [ ] Add a new update site to Eclipse named "GNU ARM Eclipse Plugins" with the URL "http://gnuarmeclipse.sourceforge.net/updates" and install all the features offered - [ ] Configure [the recommended workspace settings](http://gnuarmeclipse.github.io/eclipse/workspace/preferences/) -- [ ] Checkout the cleanflight source code [read more](#checkout-cleanflight) +- [ ] Checkout the betaflight source code [read more](#checkout-betaflight) - [ ] *Windows platform only:* Add the msys or cygwin bin directory to the project path - [ ] Build the code by going to *Project menu -> Build All* [read more](#build) - [ ] Run the tests by creating and running a make target named "test" @@ -47,85 +47,85 @@ Retain the default installation directories so that the GNU ARM Plugins can loca ### Install OpenOCD -You should install OpenOCD If you will be debugging on real hardware, such as the STM32F3DISCOVERY dev board. It is not required to simply build Cleanflight or run the tests. +You should install OpenOCD If you will be debugging on real hardware, such as the STM32F3DISCOVERY dev board. It is not required to simply build Betaflight or run the tests. ### Install QEMU No tests currently run on the QEMU emulator therefore this install is entirely optional. It is useful to test your installation, since you can compile and run a blinky demo. -### Checkout Cleanflight +### Checkout Betaflight -If you'll be submitting changes to cleanflight, [fork the repository](https://help.github.com/articles/fork-a-repo/) on GitHub and checkout your copy. +If you'll be submitting changes to betaflight, [fork the repository](https://help.github.com/articles/fork-a-repo/) on GitHub and checkout your copy. In Eclipse go to *File -> Import* choose *Git -> Projects from Git* -![projects from git](assets/building-in-eclipse/checkout-cleanflight-001.PNG) +![projects from git](assets/building-in-eclipse/checkout-betaflight-001.PNG) Choose *Clone URI* -![clone uri](assets/building-in-eclipse/checkout-cleanflight-002.PNG) +![clone uri](assets/building-in-eclipse/checkout-betaflight-002.PNG) -Enter the URI https://github.com/cleanflight/cleanflight or if you've forked the repo, enter your URI instead. With a fork, you will need to specify your authentication details +Enter the URI https://github.com/betaflight/betaflight or if you've forked the repo, enter your URI instead. With a fork, you will need to specify your authentication details -![enter uri](assets/building-in-eclipse/checkout-cleanflight-003.PNG) +![enter uri](assets/building-in-eclipse/checkout-betaflight-003.PNG) On the branch selection dialog, de-select all branches and select only *master* -![choose branches to clone](assets/building-in-eclipse/checkout-cleanflight-004.PNG) +![choose branches to clone](assets/building-in-eclipse/checkout-betaflight-004.PNG) Select the default destination directory -![destination](assets/building-in-eclipse/checkout-cleanflight-005.PNG) +![destination](assets/building-in-eclipse/checkout-betaflight-005.PNG) When the download completes, choose *Use the New Project wizard* and click Finish -![finish](assets/building-in-eclipse/checkout-cleanflight-006.PNG) +![finish](assets/building-in-eclipse/checkout-betaflight-006.PNG) Choose *C/C++ -> Makefile Project with Existing Code* -![makefile project](assets/building-in-eclipse/checkout-cleanflight-007.PNG) +![makefile project](assets/building-in-eclipse/checkout-betaflight-007.PNG) -Enter cleanflight as the project name and browse to your home directory -> git -> cleanflight for the existing code location. Ensure the C (cleanflight) and C++ (tests) languages are checked. Choose the Cross ARM GCC toolchain for the Indexer Settings. Click finish. +Enter betaflight as the project name and browse to your home directory -> git -> betaflight for the existing code location. Ensure the C (betaflight) and C++ (tests) languages are checked. Choose the Cross ARM GCC toolchain for the Indexer Settings. Click finish. -![finish checkout](assets/building-in-eclipse/checkout-cleanflight-008.PNG) +![finish checkout](assets/building-in-eclipse/checkout-betaflight-008.PNG) Set your build and debug targets by going to project properties -> C/C++ Build and choose the Behaviour tab. Replace "all" in the build box with "TARGET=xxx DEBUG=GDB" where xxx is your platform such as NAZE -![build](assets/building-in-eclipse/checkout-cleanflight-012.PNG) +![build](assets/building-in-eclipse/checkout-betaflight-012.PNG) On Windows only, add msys or cygwin bin directory to the project's path by right clicking the project and choosing properties -![properties](assets/building-in-eclipse/checkout-cleanflight-009.PNG) +![properties](assets/building-in-eclipse/checkout-betaflight-009.PNG) Edit the path variable in *C/C++ Build -> Environment* -![edit path](assets/building-in-eclipse/checkout-cleanflight-010.PNG) +![edit path](assets/building-in-eclipse/checkout-betaflight-010.PNG) Append the full path to the relevant bin dir -![append bin dir](assets/building-in-eclipse/checkout-cleanflight-011.PNG) +![append bin dir](assets/building-in-eclipse/checkout-betaflight-011.PNG) ### Build Choose project -> build all -![build all](assets/building-in-eclipse/checkout-cleanflight-013.PNG) +![build all](assets/building-in-eclipse/checkout-betaflight-013.PNG) ### Configure Debugging Choose debug -> debug configurations -![debug configurations](assets/building-in-eclipse/checkout-cleanflight-014.PNG) +![debug configurations](assets/building-in-eclipse/checkout-betaflight-014.PNG) -Create a new OpenOCD configuration for the obj\main\cleanflight_XXX.elf application (this file is generated by the build step above) +Create a new OpenOCD configuration for the obj\main\betaflight_XXX.elf application (this file is generated by the build step above) -![debug configurations](assets/building-in-eclipse/checkout-cleanflight-015.PNG) +![debug configurations](assets/building-in-eclipse/checkout-betaflight-015.PNG) Add the appropriate openocd board configuration parameter for your dev platform -![openocd target](assets/building-in-eclipse/checkout-cleanflight-016.PNG) +![openocd target](assets/building-in-eclipse/checkout-betaflight-016.PNG) Add the target to your debug menu favourites -![debug favs](assets/building-in-eclipse/checkout-cleanflight-017.PNG) +![debug favs](assets/building-in-eclipse/checkout-betaflight-017.PNG) diff --git a/docs/development/Building in Fedora.md b/docs/development/Building in Fedora.md index c7b8e712ff..55b7828c88 100644 --- a/docs/development/Building in Fedora.md +++ b/docs/development/Building in Fedora.md @@ -1,6 +1,6 @@ # Building in Fedora -Assuming you already have wget and git available, you should be able to build Cleanflight on a fresh install of Fedora with the following commands (tested on F18, F20 and Ubuntu 14.04): +Assuming you already have wget and git available, you should be able to build Betaflight on a fresh install of Fedora with the following commands (tested on F18, F20 and Ubuntu 14.04): ``` wget http://distribute.atmel.no/tools/opensource/Atmel-ARM-GNU-Toolchain/4.8.4/arm-gnu-toolchain-4.8.4.371-linux.any.x86_64.tar.gz @@ -8,9 +8,9 @@ wget http://distribute.atmel.no/tools/opensource/Atmel-ARM-GNU-Toolchain/4.8.4/a tar xf arm-gnu-toolchain-4.8.4.371-linux.any.x86_64.tar.gz export PATH=$PATH:$PWD/arm-none-eabi/bin -git clone https://github.com/cleanflight/cleanflight.git -cd cleanflight +git clone https://github.com/betaflight/betaflight.git +cd betaflight TARGET=NAZE make ``` -This will create cleanflight_NAZE.hex in the obj folder. +This will create betaflight_NAZE.hex in the obj folder. diff --git a/docs/development/Building in Mac OS X.md b/docs/development/Building in Mac OS X.md index ddad2e706e..f005f7dd1a 100644 --- a/docs/development/Building in Mac OS X.md +++ b/docs/development/Building in Mac OS X.md @@ -4,7 +4,7 @@ Building in Mac OS X can be accomplished in just a few steps: * Install general development tools (clang, make, git) * Install ARM GCC 4.9 series compiler -* Checkout Cleanflight sourcecode through git +* Checkout Betaflight sourcecode through git * Build the code ## Install general development tools (clang, make, git) @@ -37,7 +37,7 @@ installation, open up XCode and enter its preferences menu. Go to the "downloads ## Install ARM GCC 4.9 series compiler -Cleanflight is built using the 4.9 series GCC compiler provided by the [GNU Tools for ARM Embedded Processors project][]. +Betaflight is built using the 4.9 series GCC compiler provided by the [GNU Tools for ARM Embedded Processors project][]. Hit the "all downloads" link on the right side of the GNU Tools for ARM page to view [the older releases][]. Grab the Mac installation tarball for the latest version in the 4.9 series (e.g. 4.9-2015q2). Move it somewhere useful @@ -73,29 +73,29 @@ If `arm-none-eabi-gcc` couldn't be found, go back and check that you entered the [GNU Tools for ARM Embedded Processors project]: https://launchpad.net/gcc-arm-embedded [the older releases]: https://launchpad.net/gcc-arm-embedded/+download -## Checkout CleanFlight sourcecode through git +## Checkout Betaflight sourcecode through git -Enter your development directory and clone the [Cleanflight repository][] using the "HTTPS clone URL" which is shown on -the right side of the Cleanflight GitHub page, like so: +Enter your development directory and clone the [Betaflight repository][] using the "HTTPS clone URL" which is shown on +the right side of the Betaflight GitHub page, like so: ``` -git clone https://github.com/cleanflight/cleanflight.git +git clone https://github.com/betaflight/betaflight.git ``` -This will download the entire Cleanflight repository for you into a new folder called "cleanflight". +This will download the entire betaflight repository for you into a new folder called "betaflight". -[CleanFlight repository]: https://github.com/cleanflight/cleanflight +[Betaflight repository]: https://github.com/betaflight/betaflight ## Build the code -Enter the cleanflight directory and run `make TARGET=NAZE` to build firmware for the Naze32. When the build completes, -the .hex firmware should be available as `obj/cleanflight_NAZE.hex` for you to flash using the Cleanflight +Enter the betaflight directory and run `make TARGET=NAZE` to build firmware for the Naze32. When the build completes, +the .hex firmware should be available as `obj/betaflight_NAZE.hex` for you to flash using the Betaflight Configurator. ## Updating to the latest source -If you want to erase your local changes and update to the latest version of the Cleanflight source, enter your -cleanflight directory and run these commands to first erase your local changes, fetch and merge the latest +If you want to erase your local changes and update to the latest version of the Betaflight source, enter your +betaflight directory and run these commands to first erase your local changes, fetch and merge the latest changes from the repository, then rebuild the firmware: ``` diff --git a/docs/development/Building in Ubuntu.md b/docs/development/Building in Ubuntu.md index 97af6b214f..664adc8131 100644 --- a/docs/development/Building in Ubuntu.md +++ b/docs/development/Building in Ubuntu.md @@ -1,7 +1,7 @@ # Building in Ubuntu Building for Ubuntu platform is remarkably easy. The only trick to understand is that the Ubuntu toolchain, -which they are downstreaming from Debian, is not compatible with Cleanflight. We suggest that you take an +which they are downstreaming from Debian, is not compatible with Betaflight. We suggest that you take an alternative PPA from Terry Guo, found here: https://launchpad.net/~terry.guo/+archive/ubuntu/gcc-arm-embedded @@ -44,23 +44,23 @@ sudo apt-get install gcc-arm-none-eabi=4.9.3.2014q4-0precise12 After the ARM toolchain from Terry is installed, you should be able to build from source. ```bash cd src -git clone git@github.com:cleanflight/cleanflight.git -cd cleanflight +git clone git@github.com:betaflight/betaflight.git +cd betaflight make TARGET=NAZE ``` You'll see a set of files being compiled, and finally linked, yielding both an ELF and then a HEX: ``` ... -arm-none-eabi-size ./obj/main/cleanflight_NAZE.elf +arm-none-eabi-size ./obj/main/betaflight_NAZE.elf text data bss dec hex filename - 97164 320 11080 108564 1a814 ./obj/main/cleanflight_NAZE.elf -arm-none-eabi-objcopy -O ihex --set-start 0x8000000 obj/main/cleanflight_NAZE.elf obj/cleanflight_NAZE.hex -$ ls -la obj/cleanflight_NAZE.hex --rw-rw-r-- 1 pim pim 274258 Jan 12 21:45 obj/cleanflight_NAZE.hex + 97164 320 11080 108564 1a814 ./obj/main/betaflight_NAZE.elf +arm-none-eabi-objcopy -O ihex --set-start 0x8000000 obj/main/betaflight_NAZE.elf obj/betaflight_NAZE.hex +$ ls -la obj/betaflight_NAZE.hex +-rw-rw-r-- 1 pim pim 274258 Jan 12 21:45 obj/betaflight_NAZE.hex ``` -You can use the Cleanflight-Configurator to flash the `obj/cleanflight_NAZE.hex` file. +You can use the Betaflight-Configurator to flash the `obj/betaflight_NAZE.hex` file. ## Bricked/Bad build? @@ -78,10 +78,10 @@ Make sure to remove `obj/` and `make clean`, before building again. ## Updating and rebuilding -Navigate to the local cleanflight repository and use the following steps to pull the latest changes and rebuild your version of cleanflight: +Navigate to the local betaflight repository and use the following steps to pull the latest changes and rebuild your version of betaflight: ```bash -cd src/cleanflight +cd src/betaflight git reset --hard git pull make clean TARGET=NAZE diff --git a/docs/development/Building in Windows.md b/docs/development/Building in Windows.md index e39274eeac..9f520a2346 100644 --- a/docs/development/Building in Windows.md +++ b/docs/development/Building in Windows.md @@ -5,7 +5,7 @@ A new feature in Windows 10 allows any developer to quickly and easily run an entire linux subsystem in windows and access it via a bash terminal. This gives developers full use of the entire linux OS and all of the great existing linux tools and programs. When Bash for Windows is up and running it feels like you sshed into a full linux box, except the linux distro is actually running alongside windows locally. -If you use Bash on Windows you can easily build cleanflight exactly as you would for Ubuntu. (the linux distro running on Windows is Ubuntu Trusty) +If you use Bash on Windows you can easily build betaflight exactly as you would for Ubuntu. (the linux distro running on Windows is Ubuntu Trusty) Setup for Bash on Windows is very easy and takes less than 5 minutes. [For instructions follow the official guide here.](https://msdn.microsoft.com/commandline/wsl/install_guide) @@ -61,24 +61,24 @@ add the "bin" subdirectory to the PATH Windows environment variable: ```%PATH%;C ![GNU ARM Toolchain Setup](assets/010.toolchain_path.png) -## Checkout and compile Cleanflight +## Checkout and compile Betaflight -Head over to the Cleanflight Github page and grab the URL of the GIT Repository: "https://github.com/cleanflight/cleanflight.git" +Head over to the Betaflight Github page and grab the URL of the GIT Repository: "https://github.com/betaflight/betaflight.git" Open the Cygwin-Terminal, navigate to your development folder and use the git commandline to checkout the repository: ```bash cd /cygdrive/c/dev -git clone https://github.com/cleanflight/cleanflight.git +git clone https://github.com/betaflight/betaflight.git ``` ![GIT Checkout](assets/011.git_checkout.png) ![GIT Checkout](assets/012.git_checkout.png) -To compile your Cleanflight binaries, enter the cleanflight directory and build the project using the make command. You can append TARGET=[HARDWARE] if you want to build anything other than the default NAZE target: +To compile your Betaflight binaries, enter the Betaflight directory and build the project using the make command. You can append TARGET=[HARDWARE] if you want to build anything other than the default NAZE target: ```bash -cd cleanflight +cd betaflight make TARGET=NAZE ``` @@ -88,20 +88,20 @@ within few moments you should have your binary ready: ```bash (...) -arm-none-eabi-size ./obj/main/cleanflight_NAZE.elf +arm-none-eabi-size ./obj/main/betaflight_NAZE.elf text data bss dec hex filename - 95388 308 10980 106676 1a0b4 ./obj/main/cleanflight_NAZE.elf -arm-none-eabi-objcopy -O ihex --set-start 0x8000000 obj/main/cleanflight_NAZE.elf obj/cleanflight_NAZE.hex + 95388 308 10980 106676 1a0b4 ./obj/main/betaflight_NAZE.elf +arm-none-eabi-objcopy -O ihex --set-start 0x8000000 obj/main/betaflight_NAZE.elf obj/betaflight_NAZE.hex ``` -You can use the Cleanflight-Configurator to flash the ```obj/cleanflight_NAZE.hex``` file. +You can use the Betaflight-Configurator to flash the ```obj/betaflight_NAZE.hex``` file. ## Updating and rebuilding -Navigate to the local cleanflight repository and use the following steps to pull the latest changes and rebuild your version of cleanflight: +Navigate to the local betaflight repository and use the following steps to pull the latest changes and rebuild your version of betaflight: ```bash -cd /cygdrive/c/dev/cleanflight +cd /cygdrive/c/dev/betaflight git reset --hard git pull make clean TARGET=NAZE -j16 -l diff --git a/docs/development/CodingStyle.md b/docs/development/CodingStyle.md index 7e915b1387..912cd109c6 100644 --- a/docs/development/CodingStyle.md +++ b/docs/development/CodingStyle.md @@ -1,15 +1,15 @@ -#General +# General This document overrides the original Baseflight style that was referenced before. This document has taken inspiration from that style, from Eclipse defaults and from Linux, as well as from some Cleanflight developers and existing code. There are not so many changes from the old style, if you managed to find it. -#Formatting style +# Formatting style -##Indentation +## Indentation K&R indent style with 4 space indent, NO hard tabs (all tabs replaced by spaces). -##Tool support +## Tool support Any of these tools can get you pretty close: Eclipse built in "K&R" style, after changing the indent to 4 spaces and change Braces after function declarations to Next line. @@ -26,21 +26,70 @@ Sometimes, for example, you may want other columns and line breaks so it looks l Note2: The Astyle settings have been tested and will produce a nice result. Many files will be changed, mostly to the better but maybe not always, so use with care. -##Curly Braces +## Curly Braces Functions shall have the opening brace at the beginning of the next line. +``` +int function(int x) +{ + body of function +} +``` All non-function statement blocks (if, switch, for) shall have the opening brace last on the same line, with the following statement on the next line. Closing braces shall be but on the line after the last statement in the block. +``` +if (x is true) { + we do y + ... +} +``` + +``` +switch (action) { +case ADD: + return "add"; +case REMOVE: + return "remove"; +case CHANGE: + return "change"; +default: + return NULL; +} +``` If it is followed by an `else` or `else if` that shall be on the same line, again with the opening brace on the same line. +``` +if (x is true) { + we do y + ... +} else { + we do z + ... +} +``` A single statement after an `if` or an `else` may omit the "unnecessary" braces only when ALL conditional branches have single statements AND you have strong reason to know it will always be that way. +``` +if (x is true) + we do y +else + we do z +``` + +``` +if (x is true) { + we do y + ... +} else { + we do z +} +``` If in doubt, do not omit such "unnecessary" braces. (Adding a statement to a branch will break the logic if the braces are forgotten and otherwise make the PR longer). -##Spaces +## Spaces Use a space after (most) keywords. The notable exceptions are sizeof, typeof, alignof, and __attribute__, which look somewhat like functions (and are usually used with parentheses). So use a space after these keywords: ``` @@ -76,7 +125,7 @@ and no space around the '.' and "->" structure member operators. '*' and '&', when used for pointer and reference, shall have no space between it and the following variable name. -#typedef +# typedef enums with a count should have that count declared as the last item in the enumeration list, so that it is automatically maintained, e.g.: ``` @@ -99,9 +148,9 @@ typedef struct motorMixer_s { ``` the motorMixer_s name is required. -#Variables +# Variables -##Naming +## Naming Generally, descriptive lowerCamelCase names are preferred for function names, variables, arguments, etc. For configuration variables that are user accessible via CLI or similar, all_lowercase with underscore is preferred. @@ -111,7 +160,7 @@ Simple temporary variables with a very small scope may be short where it aligns Such as "i" as a temporary counter in a `for` loop, like `for (int i = 0; i < 4; i++)`. Using "temporaryCounter" in that case would not improve readability. -##Declarations +## Declarations Avoid global variables. Variables should be declared at the top of the smallest scope where the variable is used. @@ -123,7 +172,7 @@ For example to limit variable scope to a single `case` branch. Variables with limited use may be declared at the point of first use. It makes PR-review easier (but that point is lost if the variable is used everywhere anyway). -##Initialisation +## Initialisation The pattern with "lazy initialisation" may be advantageous in the Configurator to speed up the start when the initialisation is "expensive" in some way. In the FC, however, it’s always better to use some milliseconds extra before take-off than to use them while flying. @@ -131,7 +180,7 @@ So don't use "lazy initialisation". An explicit "init" function, is preferred. -##Data types +## Data types Be aware of the data types you use and do not trust implicit type casting to be correct. Angles are sometimes represented as degrees in a float. Sometimes as decidegrees in a uint8_t. @@ -145,9 +194,9 @@ Instead of sin() and cos(), use sin_approx() and cos_approx() from common/math.h Float constants should be defined with "f" suffix, like 1.0f and 3.1415926f, otherwise double conversion might occur. -#Functions +# Functions -##Naming +## Naming Methods that return a boolean should be named as a question, and should not change any state. e.g. 'isOkToArm()'. Methods should have verb or verb-phrase names, like deletePage or save. Tell the system to 'do' something 'with' something. e.g. deleteAllPages(pageList). @@ -167,7 +216,7 @@ void newBiQuadLpf(...); boolean isBiQuadReady(); ``` -##Parameter order +## Parameter order Data should move from right to left, as in memcpy(void *dst, const void *src, size_t size). This also mimics the assignment operator (e.g. dst = src;) @@ -182,7 +231,7 @@ float biQuadFilterApply(float sample, biquad_t *state); void biQuadNewLpf(float filterCutFreq, biquad_t *state, uint32_t refreshRate); ``` -##Declarations +## Declarations Functions not used outside their containing .c file should be declared static (or STATIC_UNIT_TESTED so they can be used in unit tests). Non-static functions should have their declaration in a single .h file. @@ -199,7 +248,7 @@ In the module .c file, and in the test file but nowhere else, put `#define MODUL Note: You can get the same effect by putting the internals in a separate .h file. -##Implementation +## Implementation Keep functions short and distinctive. Think about unit test when you define your functions. Ideally you should implement the test cases before implementing the function. @@ -225,7 +274,7 @@ Use parentheses around each group in logical and mathematical statements, rather than relying on the implicit logic and operator priority. The compiler knows what it’s doing but it should be easy for people too. -#Includes +# Includes All files must include their own dependencies and not rely on includes from the included files or that some other file was included first. Do not include things you are not using. @@ -233,7 +282,7 @@ Do not include things you are not using. "[#pragma once](https://en.wikipedia.org/wiki/Pragma_once)" is preferred over "#include guards" to avoid multiple includes. -#Other details +# Other details No trailing whitespace at the end of lines or at blank lines. Stay within 120 columns, unless exceeding 120 columns significantly increases readability and does not hide information. diff --git a/docs/development/Development.md b/docs/development/Development.md index ebf3ac1fe3..81999d4f04 100644 --- a/docs/development/Development.md +++ b/docs/development/Development.md @@ -103,32 +103,32 @@ https://help.github.com/articles/creating-a-pull-request/ The main flow for a contributing is as follows: -1. Login to github, go to the cleanflight repository and press `fork`. +1. Login to github, go to the betaflight repository and press `fork`. 2. Then using the command line/terminal on your computer: `git clone ` -3. `cd cleanflight` +3. `cd betaflight` 4. `git checkout master` 5. `git checkout -b my-new-code` 6. Make changes 7. `git add ` 8. `git commit` 9. `git push origin my-new-code` -10. Create pull request using github UI to merge your changes from your new branch into `cleanflight/master` +10. Create pull request using github UI to merge your changes from your new branch into `betaflight/master` 11. Repeat from step 4 for new other changes. The primary thing to remember is that separate pull requests should be created for separate branches. Never create a pull request from your `master` branch. Once you have created the PR, -every new commit/push in your branch will propagate from your fork into the PR in the main github/cleanflight repo. +every new commit/push in your branch will propagate from your fork into the PR in the main github/betaflight repo. Checkout another branch first if you want something else. Push will often fail if you edit or squash commits in a branch already pushed. Never do such things after creating the PR. -Later, you can get the changes from the cleanflight repo into your `master` branch by adding cleanflight as a git remote and merging from it as follows: +Later, you can get the changes from the betaflight repo into your `master` branch by adding betaflight as a git remote and merging from it as follows: -1. `git remote add cleanflight https://github.com/cleanflight/cleanflight.git` +1. `git remote add betaflight https://github.com/betaflight/betaflight.git` 2. `git checkout master` -3. `git fetch cleanflight` -4. `git merge cleanflight/master` +3. `git fetch betaflight` +4. `git merge betaflight/master` 5. `git push origin master` is an optional step that will update your fork on github diff --git a/docs/development/assets/building-in-eclipse/checkout-cleanflight-001.PNG b/docs/development/assets/building-in-eclipse/checkout-betaflight-001.PNG similarity index 100% rename from docs/development/assets/building-in-eclipse/checkout-cleanflight-001.PNG rename to docs/development/assets/building-in-eclipse/checkout-betaflight-001.PNG diff --git a/docs/development/assets/building-in-eclipse/checkout-cleanflight-002.PNG b/docs/development/assets/building-in-eclipse/checkout-betaflight-002.PNG similarity index 100% rename from docs/development/assets/building-in-eclipse/checkout-cleanflight-002.PNG rename to docs/development/assets/building-in-eclipse/checkout-betaflight-002.PNG diff --git a/docs/development/assets/building-in-eclipse/checkout-cleanflight-003.PNG b/docs/development/assets/building-in-eclipse/checkout-betaflight-003.PNG similarity index 100% rename from docs/development/assets/building-in-eclipse/checkout-cleanflight-003.PNG rename to docs/development/assets/building-in-eclipse/checkout-betaflight-003.PNG diff --git a/docs/development/assets/building-in-eclipse/checkout-cleanflight-004.PNG b/docs/development/assets/building-in-eclipse/checkout-betaflight-004.PNG similarity index 100% rename from docs/development/assets/building-in-eclipse/checkout-cleanflight-004.PNG rename to docs/development/assets/building-in-eclipse/checkout-betaflight-004.PNG diff --git a/docs/development/assets/building-in-eclipse/checkout-cleanflight-005.PNG b/docs/development/assets/building-in-eclipse/checkout-betaflight-005.PNG similarity index 100% rename from docs/development/assets/building-in-eclipse/checkout-cleanflight-005.PNG rename to docs/development/assets/building-in-eclipse/checkout-betaflight-005.PNG diff --git a/docs/development/assets/building-in-eclipse/checkout-cleanflight-006.PNG b/docs/development/assets/building-in-eclipse/checkout-betaflight-006.PNG similarity index 100% rename from docs/development/assets/building-in-eclipse/checkout-cleanflight-006.PNG rename to docs/development/assets/building-in-eclipse/checkout-betaflight-006.PNG diff --git a/docs/development/assets/building-in-eclipse/checkout-cleanflight-007.PNG b/docs/development/assets/building-in-eclipse/checkout-betaflight-007.PNG similarity index 100% rename from docs/development/assets/building-in-eclipse/checkout-cleanflight-007.PNG rename to docs/development/assets/building-in-eclipse/checkout-betaflight-007.PNG diff --git a/docs/development/assets/building-in-eclipse/checkout-cleanflight-008.PNG b/docs/development/assets/building-in-eclipse/checkout-betaflight-008.PNG similarity index 100% rename from docs/development/assets/building-in-eclipse/checkout-cleanflight-008.PNG rename to docs/development/assets/building-in-eclipse/checkout-betaflight-008.PNG diff --git a/docs/development/assets/building-in-eclipse/checkout-cleanflight-009.PNG b/docs/development/assets/building-in-eclipse/checkout-betaflight-009.PNG similarity index 100% rename from docs/development/assets/building-in-eclipse/checkout-cleanflight-009.PNG rename to docs/development/assets/building-in-eclipse/checkout-betaflight-009.PNG diff --git a/docs/development/assets/building-in-eclipse/checkout-cleanflight-010.PNG b/docs/development/assets/building-in-eclipse/checkout-betaflight-010.PNG similarity index 100% rename from docs/development/assets/building-in-eclipse/checkout-cleanflight-010.PNG rename to docs/development/assets/building-in-eclipse/checkout-betaflight-010.PNG diff --git a/docs/development/assets/building-in-eclipse/checkout-cleanflight-011.PNG b/docs/development/assets/building-in-eclipse/checkout-betaflight-011.PNG similarity index 100% rename from docs/development/assets/building-in-eclipse/checkout-cleanflight-011.PNG rename to docs/development/assets/building-in-eclipse/checkout-betaflight-011.PNG diff --git a/docs/development/assets/building-in-eclipse/checkout-cleanflight-012.PNG b/docs/development/assets/building-in-eclipse/checkout-betaflight-012.PNG similarity index 100% rename from docs/development/assets/building-in-eclipse/checkout-cleanflight-012.PNG rename to docs/development/assets/building-in-eclipse/checkout-betaflight-012.PNG diff --git a/docs/development/assets/building-in-eclipse/checkout-cleanflight-013.PNG b/docs/development/assets/building-in-eclipse/checkout-betaflight-013.PNG similarity index 100% rename from docs/development/assets/building-in-eclipse/checkout-cleanflight-013.PNG rename to docs/development/assets/building-in-eclipse/checkout-betaflight-013.PNG diff --git a/docs/development/assets/building-in-eclipse/checkout-cleanflight-014.PNG b/docs/development/assets/building-in-eclipse/checkout-betaflight-014.PNG similarity index 100% rename from docs/development/assets/building-in-eclipse/checkout-cleanflight-014.PNG rename to docs/development/assets/building-in-eclipse/checkout-betaflight-014.PNG diff --git a/docs/development/assets/building-in-eclipse/checkout-cleanflight-015.PNG b/docs/development/assets/building-in-eclipse/checkout-betaflight-015.PNG similarity index 100% rename from docs/development/assets/building-in-eclipse/checkout-cleanflight-015.PNG rename to docs/development/assets/building-in-eclipse/checkout-betaflight-015.PNG diff --git a/docs/development/assets/building-in-eclipse/checkout-cleanflight-016.PNG b/docs/development/assets/building-in-eclipse/checkout-betaflight-016.PNG similarity index 100% rename from docs/development/assets/building-in-eclipse/checkout-cleanflight-016.PNG rename to docs/development/assets/building-in-eclipse/checkout-betaflight-016.PNG diff --git a/docs/development/assets/building-in-eclipse/checkout-cleanflight-017.PNG b/docs/development/assets/building-in-eclipse/checkout-betaflight-017.PNG similarity index 100% rename from docs/development/assets/building-in-eclipse/checkout-cleanflight-017.PNG rename to docs/development/assets/building-in-eclipse/checkout-betaflight-017.PNG diff --git a/docs/osd.md b/docs/osd.md new file mode 100644 index 0000000000..69aefdf010 --- /dev/null +++ b/docs/osd.md @@ -0,0 +1,165 @@ +# OSD Glyphs + +| PNG | Hex | Dec | Betaflight Name(s) | Notes | +| -------------------------------|------| --- | ------------------------------- | ----- | +| ![001.png](osd_images/001.png) | 0x01 | 001 | SYM_RSSI | Used in OSD.c | +| ![002.png](osd_images/002.png) | 0x02 | 002 | SYM_AH_RIGHT | Used in OSD.c | +| ![003.png](osd_images/003.png) | 0x03 | 003 | SYM_AH_LEFT SYM_CURSOR | Used in OSD.c | +| ![004.png](osd_images/004.png) | 0x04 | 004 | SYM_THR SYM_HOME | Used in OSD.c | +| ![005.png](osd_images/005.png) | 0x05 | 005 | SYM_THR1 SYM_AIRCRAFT | Used in OSD.c | +| ![006.png](osd_images/006.png) | 0x06 | 006 | SYM_VOLT | Used in OSD.c | +| ![007.png](osd_images/007.png) | 0x07 | 007 | SYM_MAH | Used in OSD.c | +| ![008.png](osd_images/008.png) | 0x08 | 008 | | | +| ![009.png](osd_images/009.png) | 0x09 | 009 | | | +| ![010.png](osd_images/010.png) | 0x0A | 010 | | | +| ![011.png](osd_images/011.png) | 0x0B | 011 | | | +| ![012.png](osd_images/012.png) | 0x0C | 012 | SYM_M | Used in OSD.c | +| ![013.png](osd_images/013.png) | 0x0D | 013 | SYM_TEMP_F | No longer used in OSD.c ? Using F instead | +| ![014.png](osd_images/014.png) | 0x0E | 014 | SYM_TEMP_C | No longer used in OSD.c ? Using C instead | +| ![015.png](osd_images/015.png) | 0x0F | 015 | SYM_FT | Used in OSD.c | +| ![016.png](osd_images/016.png) | 0x10 | 016 | | | +| ![017.png](osd_images/017.png) | 0x11 | 017 | | | +| ![018.png](osd_images/018.png) | 0x12 | 018 | | | +| ![019.png](osd_images/019.png) | 0x13 | 019 | SYM_AH_DECORATION | Used in OSD.c | +| ![020.png](osd_images/020.png) | 0x14 | 020 | | | +| ![021.png](osd_images/021.png) | 0x15 | 021 | | | +| ![022.png](osd_images/022.png) | 0x16 | 022 | | | +| ![023.png](osd_images/023.png) | 0x17 | 023 | | | +| ![024.png](osd_images/024.png) | 0x18 | 024 | SYM_HEADING_N | Used in OSD.c | +| ![025.png](osd_images/025.png) | 0x19 | 025 | SYM_HEADING_S | Used in OSD.c | +| ![026.png](osd_images/026.png) | 0x1A | 026 | SYM_HEADING_E | Used in OSD.c | +| ![027.png](osd_images/027.png) | 0x1B | 027 | SYM_HEADING_W | Used in OSD.c | +| ![028.png](osd_images/028.png) | 0x1C | 028 | SYM_HEADING_DIVIDED_LINE | Used in OSD.c | +| ![029.png](osd_images/029.png) | 0x1D | 029 | SYM_HEADING_LINE | Used in OSD.c | +| ![030.png](osd_images/030.png) | 0x1E | 030 | SYM_SAT_L | Used in OSD.c | +| ![031.png](osd_images/031.png) | 0x1F | 031 | SYM_SAT_R | Used in OSD.c | +| | 0x20 | 032 | SYM_BLANK | Used in OSD.c | +| ![033.png](osd_images/033.png) | 0x21 | 033 | | | +| ![034.png](osd_images/034.png) | 0x22 | 034 | | | +| ![035.png](osd_images/035.png) | 0x23 | 035 | | | +| ![036.png](osd_images/036.png) | 0x24 | 036 | | | +| ![037.png](osd_images/037.png) | 0x25 | 037 | | | +| ![038.png](osd_images/038.png) | 0x26 | 038 | SYM_AH_CENTER_LINE | Used in OSD.c | +| ![039.png](osd_images/039.png) | 0x27 | 039 | SYM_AH_CENTER_LINE_RIGHT | Used in OSD.c | +| ![040.png](osd_images/040.png) | 0x28 | 040 | | | +| ![041.png](osd_images/041.png) | 0x29 | 040 | | | +| ![042.png](osd_images/042.png) | 0x2A | 040 | | | +| ![043.png](osd_images/043.png) | 0x2B | 040 | | | +| ![044.png](osd_images/044.png) | 0x2C | 040 | | | +| ![045.png](osd_images/045.png) | 0x2D | 045 | SYM_COLON | Used in OSD.c | +| ![046.png](osd_images/046.png) | 0x2E | 046 | | | +| ![047.png](osd_images/047.png) | 0x2F | 047 | | | +| ![048.png](osd_images/048.png) | 0x30 | 048 | | | +| ![049.png](osd_images/049.png) | 0x31 | 049 | | | +| ![050.png](osd_images/050.png) | 0x32 | 050 | | | +| ![051.png](osd_images/051.png) | 0x33 | 051 | | | +| ![052.png](osd_images/052.png) | 0x34 | 052 | | | +| ![053.png](osd_images/053.png) | 0x35 | 053 | | | +| ![054.png](osd_images/054.png) | 0x36 | 054 | | | +| ![055.png](osd_images/055.png) | 0x37 | 055 | | | +| ![056.png](osd_images/056.png) | 0x38 | 056 | | | +| ![057.png](osd_images/057.png) | 0x39 | 057 | | | +| ![058.png](osd_images/058.png) | 0x3A | 058 | | | +| ![059.png](osd_images/059.png) | 0x3B | 059 | | | +| ![060.png](osd_images/060.png) | 0x3C | 060 | | | +| ![061.png](osd_images/061.png) | 0x3D | 061 | | | +| ![062.png](osd_images/062.png) | 0x3E | 062 | | | +| ![063.png](osd_images/063.png) | 0x3F | 063 | | | +| ![064.png](osd_images/064.png) | 0x40 | 064 | | | +| ![065.png](osd_images/065.png) | 0x41 | 065 | | | +| ![066.png](osd_images/066.png) | 0x42 | 066 | | | +| ![067.png](osd_images/067.png) | 0x43 | 067 | | | +| ![068.png](osd_images/068.png) | 0x44 | 068 | | | +| ![069.png](osd_images/069.png) | 0x45 | 069 | | | +| ![070.png](osd_images/070.png) | 0x46 | 070 | | | +| ![071.png](osd_images/071.png) | 0x47 | 071 | | | +| ![072.png](osd_images/072.png) | 0x48 | 072 | | | +| ![073.png](osd_images/073.png) | 0x49 | 073 | | | +| ![074.png](osd_images/074.png) | 0x4A | 074 | | | +| ![075.png](osd_images/075.png) | 0x4B | 075 | | | +| ![076.png](osd_images/076.png) | 0x4C | 076 | | | +| ![077.png](osd_images/077.png) | 0x4D | 077 | | | +| ![078.png](osd_images/078.png) | 0x4E | 078 | | | +| ![079.png](osd_images/079.png) | 0x4F | 079 | | | +| ![080.png](osd_images/080.png) | 0x50 | 080 | | | +| ![081.png](osd_images/081.png) | 0x51 | 081 | | | +| ![082.png](osd_images/082.png) | 0x52 | 082 | | | +| ![083.png](osd_images/083.png) | 0x53 | 083 | | | +| ![084.png](osd_images/084.png) | 0x54 | 084 | | | +| ![085.png](osd_images/085.png) | 0x55 | 085 | | | +| ![086.png](osd_images/086.png) | 0x56 | 086 | | | +| ![087.png](osd_images/087.png) | 0x57 | 087 | SYM_WATT | Used in OSD.c | +| ![088.png](osd_images/088.png) | 0x58 | 088 | | | +| ![089.png](osd_images/089.png) | 0x59 | 089 | | | +| ![090.png](osd_images/090.png) | 0x5A | 090 | | | +| ![091.png](osd_images/091.png) | 0x5B | 091 | | | +| ![092.png](osd_images/092.png) | 0x5C | 092 | | | +| ![093.png](osd_images/093.png) | 0x5D | 093 | | | +| ![094.png](osd_images/094.png) | 0x5E | 094 | | | +| ![095.png](osd_images/095.png) | 0x5F | 095 | | | +| ![096.png](osd_images/096.png) | 0x60 | 096 | SYM_ARROW_SOUTH | Used in OSD.c | +| ![097.png](osd_images/097.png) | 0x61 | 097 | SYM_ARROW_2 | Calculated from SYM_ARROW_SOUTH + heading | +| ![098.png](osd_images/098.png) | 0x62 | 098 | SYM_ARROW_3 | Calculated from SYM_ARROW_SOUTH + heading | +| ![099.png](osd_images/099.png) | 0x63 | 099 | SYM_ARROW_4 | Calculated from SYM_ARROW_SOUTH + heading | +| ![100.png](osd_images/100.png) | 0x64 | 100 | SYM_ARROW_EAST | Calculated from SYM_ARROW_SOUTH + heading | +| ![101.png](osd_images/101.png) | 0x65 | 101 | SYM_ARROW_6 | Calculated from SYM_ARROW_SOUTH + heading | +| ![102.png](osd_images/102.png) | 0x66 | 102 | SYM_ARROW_7 | Calculated from SYM_ARROW_SOUTH + heading | +| ![103.png](osd_images/103.png) | 0x67 | 103 | SYM_ARROW_8 | Calculated from SYM_ARROW_SOUTH + heading | +| ![104.png](osd_images/104.png) | 0x68 | 104 | SYM_ARROW_NORTH | Calculated from SYM_ARROW_SOUTH + heading | +| ![105.png](osd_images/105.png) | 0x69 | 105 | SYM_ARROW_10 | Calculated from SYM_ARROW_SOUTH + heading | +| ![106.png](osd_images/106.png) | 0x6A | 106 | SYM_ARROW_11 | Calculated from SYM_ARROW_SOUTH + heading | +| ![107.png](osd_images/107.png) | 0x6B | 107 | SYM_ARROW_12 | Calculated from SYM_ARROW_SOUTH + heading | +| ![108.png](osd_images/108.png) | 0x6C | 108 | SYM_ARROW_WEST | Calculated from SYM_ARROW_SOUTH + heading | +| ![109.png](osd_images/109.png) | 0x6D | 109 | SYM_ARROW_14 | Calculated from SYM_ARROW_SOUTH + heading | +| ![110.png](osd_images/110.png) | 0x6E | 110 | SYM_ARROW_15 | Calculated from SYM_ARROW_SOUTH + heading | +| ![111.png](osd_images/111.png) | 0x6F | 111 | SYM_ARROW_16 | Calculated from SYM_ARROW_SOUTH + heading | +| ![112.png](osd_images/112.png) | 0x70 | 112 | | | +| ![113.png](osd_images/113.png) | 0x71 | 113 | | | +| ![114.png](osd_images/114.png) | 0x72 | 114 | | | +| ![115.png](osd_images/115.png) | 0x73 | 115 | | | +| ![116.png](osd_images/116.png) | 0x74 | 116 | | | +| ![117.png](osd_images/117.png) | 0x75 | 117 | | | +| ![118.png](osd_images/118.png) | 0x76 | 118 | | | +| ![119.png](osd_images/119.png) | 0x77 | 119 | | | +| ![120.png](osd_images/120.png) | 0x78 | 120 | | | +| ![121.png](osd_images/121.png) | 0x79 | 121 | | | +| | 0x7A | 122 | | | +| ![123.png](osd_images/123.png) | 0x7B | 123 | | | +| ![124.png](osd_images/124.png) | 0x7C | 124 | | | +| ![125.png](osd_images/125.png) | 0x7D | 125 | | | +| ![126.png](osd_images/126.png) | 0x7E | 126 | SYM_AH_CENTER | Used in OSD.c | +| ![127.png](osd_images/127.png) | 0x7F | 127 | | | +| ![128.png](osd_images/128.png) | 0x80 | 128 | SYM_AH_BAR9_0 | Used in OSD.c | +| ![129.png](osd_images/129.png) | 0x81 | 129 | SYM_AH_BAR9_1 | Calculated in AH using SYM_AH_BAR9_0 as base | +| ![130.png](osd_images/130.png) | 0x82 | 130 | SYM_AH_BAR9_2 | Calculated in AH using SYM_AH_BAR9_0 as base | +| ![131.png](osd_images/131.png) | 0x83 | 131 | SYM_AH_BAR9_3 | Calculated in AH using SYM_AH_BAR9_0 as base | +| ![132.png](osd_images/132.png) | 0x84 | 132 | SYM_AH_BAR9_4 | Calculated in AH using SYM_AH_BAR9_0 as base | +| ![133.png](osd_images/133.png) | 0x85 | 133 | SYM_AH_BAR9_5 | Calculated in AH using SYM_AH_BAR9_0 as base | +| ![134.png](osd_images/134.png) | 0x86 | 134 | SYM_AH_BAR9_6 | Calculated in AH using SYM_AH_BAR9_0 as base | +| ![135.png](osd_images/135.png) | 0x87 | 135 | SYM_AH_BAR9_7 | Calculated in AH using SYM_AH_BAR9_0 as base | +| ![136.png](osd_images/136.png) | 0x88 | 136 | SYM_AH_BAR9_8 | Calculated in AH using SYM_AH_BAR9_0 as base | +| ![137.png](osd_images/137.png) | 0x89 | 137 | | | +| ![138.png](osd_images/138.png) | 0x8A | 138 | SYM_PB_START | Used in OSD.c | +| ![139.png](osd_images/139.png) | 0x8B | 139 | SYM_PB_FULL | Used in OSD.c | +| ![140.png](osd_images/140.png) | 0x8C | 140 | SYM_PB_HALF | | +| ![141.png](osd_images/141.png) | 0x8D | 141 | SYM_PB_EMPTY | Used in OSD.c | +| ![142.png](osd_images/142.png) | 0x8E | 142 | SYM_PB_END | Used in OSD.c | +| ![143.png](osd_images/143.png) | 0x8F | 143 | SYM_PB_CLOSE | Used in OSD.c | +| ![144.png](osd_images/144.png) | 0x90 | 144 | SYM_BATT_FULL | Calculated from SYM_BATT_EMPTY | +| ![145.png](osd_images/145.png) | 0x91 | 145 | SYM_BATT_5 | Calculated from SYM_BATT_EMPTY | +| ![146.png](osd_images/146.png) | 0x92 | 146 | SYM_BATT_4 | Calculated from SYM_BATT_EMPTY | +| ![147.png](osd_images/147.png) | 0x93 | 147 | SYM_BATT_3 | Calculated from SYM_BATT_EMPTY | +| ![148.png](osd_images/148.png) | 0x94 | 148 | SYM_BATT_2 | Calculated from SYM_BATT_EMPTY | +| ![149.png](osd_images/149.png) | 0x95 | 149 | SYM_BATT_1 | Calculated from SYM_BATT_EMPTY | +| ![150.png](osd_images/150.png) | 0x96 | 150 | SYM_BATT_EMPTY | Used in OSD.c | +| ![151.png](osd_images/151.png) | 0x97 | 151 | SYM_MAIN_BATT | Used in OSD.c | +| ![152.png](osd_images/152.png) | 0x98 | 152 | | | +| ![153.png](osd_images/153.png) | 0x99 | 153 | | | +| ![154.png](osd_images/154.png) | 0x9A | 154 | SYM_AMP | Used in OSD.c | +| ![155.png](osd_images/155.png) | 0x9B | 155 | SYM_ON_M | Used in OSD.c | +| ![156.png](osd_images/156.png) | 0x9C | 156 | SYM_FLY_M | Used in OSD.c | +| ![157.png](osd_images/157.png) | 0x9D | 157 | | | +| ![158.png](osd_images/158.png) | 0x9E | 158 | | | +| ![159.png](osd_images/159.png) | 0x9F | 159 | | | +| ![160.png](osd_images/160.png) | 0xA0 | 160 | | *Logo Starts Here* | +| | 0xFF | 255 | SYM_END_OF_FONT | | \ No newline at end of file diff --git a/docs/osd_images/001.png b/docs/osd_images/001.png new file mode 100644 index 0000000000..4b3ff795e7 Binary files /dev/null and b/docs/osd_images/001.png differ diff --git a/docs/osd_images/002.png b/docs/osd_images/002.png new file mode 100644 index 0000000000..a30af21211 Binary files /dev/null and b/docs/osd_images/002.png differ diff --git a/docs/osd_images/003.png b/docs/osd_images/003.png new file mode 100644 index 0000000000..d86ecee88b Binary files /dev/null and b/docs/osd_images/003.png differ diff --git a/docs/osd_images/004.png b/docs/osd_images/004.png new file mode 100644 index 0000000000..a3cf0c4f91 Binary files /dev/null and b/docs/osd_images/004.png differ diff --git a/docs/osd_images/005.png b/docs/osd_images/005.png new file mode 100644 index 0000000000..7750c6d7e9 Binary files /dev/null and b/docs/osd_images/005.png differ diff --git a/docs/osd_images/006.png b/docs/osd_images/006.png new file mode 100644 index 0000000000..006584f312 Binary files /dev/null and b/docs/osd_images/006.png differ diff --git a/docs/osd_images/007.png b/docs/osd_images/007.png new file mode 100644 index 0000000000..4f1f751729 Binary files /dev/null and b/docs/osd_images/007.png differ diff --git a/docs/osd_images/008.png b/docs/osd_images/008.png new file mode 100644 index 0000000000..b25a0413ed Binary files /dev/null and b/docs/osd_images/008.png differ diff --git a/docs/osd_images/009.png b/docs/osd_images/009.png new file mode 100644 index 0000000000..20b43ff16c Binary files /dev/null and b/docs/osd_images/009.png differ diff --git a/docs/osd_images/010.png b/docs/osd_images/010.png new file mode 100644 index 0000000000..ff2d03cbdc Binary files /dev/null and b/docs/osd_images/010.png differ diff --git a/docs/osd_images/011.png b/docs/osd_images/011.png new file mode 100644 index 0000000000..053198cb86 Binary files /dev/null and b/docs/osd_images/011.png differ diff --git a/docs/osd_images/012.png b/docs/osd_images/012.png new file mode 100644 index 0000000000..53ee533289 Binary files /dev/null and b/docs/osd_images/012.png differ diff --git a/docs/osd_images/013.png b/docs/osd_images/013.png new file mode 100644 index 0000000000..03b90e1aac Binary files /dev/null and b/docs/osd_images/013.png differ diff --git a/docs/osd_images/014.png b/docs/osd_images/014.png new file mode 100644 index 0000000000..b304c8e49d Binary files /dev/null and b/docs/osd_images/014.png differ diff --git a/docs/osd_images/015.png b/docs/osd_images/015.png new file mode 100644 index 0000000000..e8cf1127cd Binary files /dev/null and b/docs/osd_images/015.png differ diff --git a/docs/osd_images/016.png b/docs/osd_images/016.png new file mode 100644 index 0000000000..d8a9e0db51 Binary files /dev/null and b/docs/osd_images/016.png differ diff --git a/docs/osd_images/017.png b/docs/osd_images/017.png new file mode 100644 index 0000000000..5bb3695d7f Binary files /dev/null and b/docs/osd_images/017.png differ diff --git a/docs/osd_images/018.png b/docs/osd_images/018.png new file mode 100644 index 0000000000..0eab164b85 Binary files /dev/null and b/docs/osd_images/018.png differ diff --git a/docs/osd_images/019.png b/docs/osd_images/019.png new file mode 100644 index 0000000000..3dd0aed435 Binary files /dev/null and b/docs/osd_images/019.png differ diff --git a/docs/osd_images/020.png b/docs/osd_images/020.png new file mode 100644 index 0000000000..14377dedd2 Binary files /dev/null and b/docs/osd_images/020.png differ diff --git a/docs/osd_images/021.png b/docs/osd_images/021.png new file mode 100644 index 0000000000..39c0cdacdc Binary files /dev/null and b/docs/osd_images/021.png differ diff --git a/docs/osd_images/022.png b/docs/osd_images/022.png new file mode 100644 index 0000000000..308c53e4ff Binary files /dev/null and b/docs/osd_images/022.png differ diff --git a/docs/osd_images/023.png b/docs/osd_images/023.png new file mode 100644 index 0000000000..3000ef294e Binary files /dev/null and b/docs/osd_images/023.png differ diff --git a/docs/osd_images/024.png b/docs/osd_images/024.png new file mode 100644 index 0000000000..1fc49d89cf Binary files /dev/null and b/docs/osd_images/024.png differ diff --git a/docs/osd_images/025.png b/docs/osd_images/025.png new file mode 100644 index 0000000000..8c6a774018 Binary files /dev/null and b/docs/osd_images/025.png differ diff --git a/docs/osd_images/026.png b/docs/osd_images/026.png new file mode 100644 index 0000000000..1e41a414b0 Binary files /dev/null and b/docs/osd_images/026.png differ diff --git a/docs/osd_images/027.png b/docs/osd_images/027.png new file mode 100644 index 0000000000..44b7d63468 Binary files /dev/null and b/docs/osd_images/027.png differ diff --git a/docs/osd_images/028.png b/docs/osd_images/028.png new file mode 100644 index 0000000000..df43cc6fd1 Binary files /dev/null and b/docs/osd_images/028.png differ diff --git a/docs/osd_images/029.png b/docs/osd_images/029.png new file mode 100644 index 0000000000..2c432b9d5e Binary files /dev/null and b/docs/osd_images/029.png differ diff --git a/docs/osd_images/030.png b/docs/osd_images/030.png new file mode 100644 index 0000000000..db5aba491d Binary files /dev/null and b/docs/osd_images/030.png differ diff --git a/docs/osd_images/031.png b/docs/osd_images/031.png new file mode 100644 index 0000000000..1b48ca2bce Binary files /dev/null and b/docs/osd_images/031.png differ diff --git a/docs/osd_images/033.png b/docs/osd_images/033.png new file mode 100644 index 0000000000..a300dc2362 Binary files /dev/null and b/docs/osd_images/033.png differ diff --git a/docs/osd_images/034.png b/docs/osd_images/034.png new file mode 100644 index 0000000000..9402b171dd Binary files /dev/null and b/docs/osd_images/034.png differ diff --git a/docs/osd_images/035.png b/docs/osd_images/035.png new file mode 100644 index 0000000000..146e63c262 Binary files /dev/null and b/docs/osd_images/035.png differ diff --git a/docs/osd_images/036.png b/docs/osd_images/036.png new file mode 100644 index 0000000000..da701a3505 Binary files /dev/null and b/docs/osd_images/036.png differ diff --git a/docs/osd_images/037.png b/docs/osd_images/037.png new file mode 100644 index 0000000000..04bc665cb5 Binary files /dev/null and b/docs/osd_images/037.png differ diff --git a/docs/osd_images/038.png b/docs/osd_images/038.png new file mode 100644 index 0000000000..a4420839ca Binary files /dev/null and b/docs/osd_images/038.png differ diff --git a/docs/osd_images/039.png b/docs/osd_images/039.png new file mode 100644 index 0000000000..fb86c54704 Binary files /dev/null and b/docs/osd_images/039.png differ diff --git a/docs/osd_images/040.png b/docs/osd_images/040.png new file mode 100644 index 0000000000..355467c252 Binary files /dev/null and b/docs/osd_images/040.png differ diff --git a/docs/osd_images/041.png b/docs/osd_images/041.png new file mode 100644 index 0000000000..f0cc60cfc6 Binary files /dev/null and b/docs/osd_images/041.png differ diff --git a/docs/osd_images/042.png b/docs/osd_images/042.png new file mode 100644 index 0000000000..db99c8255f Binary files /dev/null and b/docs/osd_images/042.png differ diff --git a/docs/osd_images/043.png b/docs/osd_images/043.png new file mode 100644 index 0000000000..4352a813dc Binary files /dev/null and b/docs/osd_images/043.png differ diff --git a/docs/osd_images/044.png b/docs/osd_images/044.png new file mode 100644 index 0000000000..6e32fb88d5 Binary files /dev/null and b/docs/osd_images/044.png differ diff --git a/docs/osd_images/045.png b/docs/osd_images/045.png new file mode 100644 index 0000000000..b072d4480e Binary files /dev/null and b/docs/osd_images/045.png differ diff --git a/docs/osd_images/046.png b/docs/osd_images/046.png new file mode 100644 index 0000000000..85b81a7c8d Binary files /dev/null and b/docs/osd_images/046.png differ diff --git a/docs/osd_images/047.png b/docs/osd_images/047.png new file mode 100644 index 0000000000..8cb430010d Binary files /dev/null and b/docs/osd_images/047.png differ diff --git a/docs/osd_images/048.png b/docs/osd_images/048.png new file mode 100644 index 0000000000..6d14fac96c Binary files /dev/null and b/docs/osd_images/048.png differ diff --git a/docs/osd_images/049.png b/docs/osd_images/049.png new file mode 100644 index 0000000000..735b779ff0 Binary files /dev/null and b/docs/osd_images/049.png differ diff --git a/docs/osd_images/050.png b/docs/osd_images/050.png new file mode 100644 index 0000000000..ee444c8b85 Binary files /dev/null and b/docs/osd_images/050.png differ diff --git a/docs/osd_images/051.png b/docs/osd_images/051.png new file mode 100644 index 0000000000..a6c5e70c65 Binary files /dev/null and b/docs/osd_images/051.png differ diff --git a/docs/osd_images/052.png b/docs/osd_images/052.png new file mode 100644 index 0000000000..c52ad24921 Binary files /dev/null and b/docs/osd_images/052.png differ diff --git a/docs/osd_images/053.png b/docs/osd_images/053.png new file mode 100644 index 0000000000..7dc2f21a7f Binary files /dev/null and b/docs/osd_images/053.png differ diff --git a/docs/osd_images/054.png b/docs/osd_images/054.png new file mode 100644 index 0000000000..363dd40cfe Binary files /dev/null and b/docs/osd_images/054.png differ diff --git a/docs/osd_images/055.png b/docs/osd_images/055.png new file mode 100644 index 0000000000..7289a0aeee Binary files /dev/null and b/docs/osd_images/055.png differ diff --git a/docs/osd_images/056.png b/docs/osd_images/056.png new file mode 100644 index 0000000000..0dd0c529e5 Binary files /dev/null and b/docs/osd_images/056.png differ diff --git a/docs/osd_images/057.png b/docs/osd_images/057.png new file mode 100644 index 0000000000..a486e8862a Binary files /dev/null and b/docs/osd_images/057.png differ diff --git a/docs/osd_images/058.png b/docs/osd_images/058.png new file mode 100644 index 0000000000..e7b3a50dd2 Binary files /dev/null and b/docs/osd_images/058.png differ diff --git a/docs/osd_images/059.png b/docs/osd_images/059.png new file mode 100644 index 0000000000..c1eb839fae Binary files /dev/null and b/docs/osd_images/059.png differ diff --git a/docs/osd_images/060.png b/docs/osd_images/060.png new file mode 100644 index 0000000000..f7b13dd0f2 Binary files /dev/null and b/docs/osd_images/060.png differ diff --git a/docs/osd_images/061.png b/docs/osd_images/061.png new file mode 100644 index 0000000000..ae41460a40 Binary files /dev/null and b/docs/osd_images/061.png differ diff --git a/docs/osd_images/062.png b/docs/osd_images/062.png new file mode 100644 index 0000000000..81a1953980 Binary files /dev/null and b/docs/osd_images/062.png differ diff --git a/docs/osd_images/063.png b/docs/osd_images/063.png new file mode 100644 index 0000000000..a7e7000cce Binary files /dev/null and b/docs/osd_images/063.png differ diff --git a/docs/osd_images/064.png b/docs/osd_images/064.png new file mode 100644 index 0000000000..19715c3281 Binary files /dev/null and b/docs/osd_images/064.png differ diff --git a/docs/osd_images/065.png b/docs/osd_images/065.png new file mode 100644 index 0000000000..5a0fa2a084 Binary files /dev/null and b/docs/osd_images/065.png differ diff --git a/docs/osd_images/066.png b/docs/osd_images/066.png new file mode 100644 index 0000000000..8fbe82ff99 Binary files /dev/null and b/docs/osd_images/066.png differ diff --git a/docs/osd_images/067.png b/docs/osd_images/067.png new file mode 100644 index 0000000000..6e3ac17b7e Binary files /dev/null and b/docs/osd_images/067.png differ diff --git a/docs/osd_images/068.png b/docs/osd_images/068.png new file mode 100644 index 0000000000..704081d936 Binary files /dev/null and b/docs/osd_images/068.png differ diff --git a/docs/osd_images/069.png b/docs/osd_images/069.png new file mode 100644 index 0000000000..dd01b538f8 Binary files /dev/null and b/docs/osd_images/069.png differ diff --git a/docs/osd_images/070.png b/docs/osd_images/070.png new file mode 100644 index 0000000000..8e2d3c583b Binary files /dev/null and b/docs/osd_images/070.png differ diff --git a/docs/osd_images/071.png b/docs/osd_images/071.png new file mode 100644 index 0000000000..96ef73a5e3 Binary files /dev/null and b/docs/osd_images/071.png differ diff --git a/docs/osd_images/072.png b/docs/osd_images/072.png new file mode 100644 index 0000000000..59adbab6bd Binary files /dev/null and b/docs/osd_images/072.png differ diff --git a/docs/osd_images/073.png b/docs/osd_images/073.png new file mode 100644 index 0000000000..ee7c0a6f24 Binary files /dev/null and b/docs/osd_images/073.png differ diff --git a/docs/osd_images/074.png b/docs/osd_images/074.png new file mode 100644 index 0000000000..237205d48b Binary files /dev/null and b/docs/osd_images/074.png differ diff --git a/docs/osd_images/075.png b/docs/osd_images/075.png new file mode 100644 index 0000000000..a5cfc733af Binary files /dev/null and b/docs/osd_images/075.png differ diff --git a/docs/osd_images/076.png b/docs/osd_images/076.png new file mode 100644 index 0000000000..8470b30724 Binary files /dev/null and b/docs/osd_images/076.png differ diff --git a/docs/osd_images/077.png b/docs/osd_images/077.png new file mode 100644 index 0000000000..d1eaebc788 Binary files /dev/null and b/docs/osd_images/077.png differ diff --git a/docs/osd_images/078.png b/docs/osd_images/078.png new file mode 100644 index 0000000000..3de9804c77 Binary files /dev/null and b/docs/osd_images/078.png differ diff --git a/docs/osd_images/079.png b/docs/osd_images/079.png new file mode 100644 index 0000000000..28b4a42d44 Binary files /dev/null and b/docs/osd_images/079.png differ diff --git a/docs/osd_images/080.png b/docs/osd_images/080.png new file mode 100644 index 0000000000..e67c1a018a Binary files /dev/null and b/docs/osd_images/080.png differ diff --git a/docs/osd_images/081.png b/docs/osd_images/081.png new file mode 100644 index 0000000000..6219e7ac5d Binary files /dev/null and b/docs/osd_images/081.png differ diff --git a/docs/osd_images/082.png b/docs/osd_images/082.png new file mode 100644 index 0000000000..cb6292eada Binary files /dev/null and b/docs/osd_images/082.png differ diff --git a/docs/osd_images/083.png b/docs/osd_images/083.png new file mode 100644 index 0000000000..dfbbbe37b8 Binary files /dev/null and b/docs/osd_images/083.png differ diff --git a/docs/osd_images/084.png b/docs/osd_images/084.png new file mode 100644 index 0000000000..7ac0fc7ec6 Binary files /dev/null and b/docs/osd_images/084.png differ diff --git a/docs/osd_images/085.png b/docs/osd_images/085.png new file mode 100644 index 0000000000..c92f162df3 Binary files /dev/null and b/docs/osd_images/085.png differ diff --git a/docs/osd_images/086.png b/docs/osd_images/086.png new file mode 100644 index 0000000000..02fefa569f Binary files /dev/null and b/docs/osd_images/086.png differ diff --git a/docs/osd_images/087.png b/docs/osd_images/087.png new file mode 100644 index 0000000000..62a2e33df4 Binary files /dev/null and b/docs/osd_images/087.png differ diff --git a/docs/osd_images/088.png b/docs/osd_images/088.png new file mode 100644 index 0000000000..107670b104 Binary files /dev/null and b/docs/osd_images/088.png differ diff --git a/docs/osd_images/089.png b/docs/osd_images/089.png new file mode 100644 index 0000000000..eac32b2413 Binary files /dev/null and b/docs/osd_images/089.png differ diff --git a/docs/osd_images/090.png b/docs/osd_images/090.png new file mode 100644 index 0000000000..123198b7de Binary files /dev/null and b/docs/osd_images/090.png differ diff --git a/docs/osd_images/091.png b/docs/osd_images/091.png new file mode 100644 index 0000000000..7a52d5140c Binary files /dev/null and b/docs/osd_images/091.png differ diff --git a/docs/osd_images/092.png b/docs/osd_images/092.png new file mode 100644 index 0000000000..10561bc3ed Binary files /dev/null and b/docs/osd_images/092.png differ diff --git a/docs/osd_images/093.png b/docs/osd_images/093.png new file mode 100644 index 0000000000..045dadc646 Binary files /dev/null and b/docs/osd_images/093.png differ diff --git a/docs/osd_images/094.png b/docs/osd_images/094.png new file mode 100644 index 0000000000..7f19068d6b Binary files /dev/null and b/docs/osd_images/094.png differ diff --git a/docs/osd_images/095.png b/docs/osd_images/095.png new file mode 100644 index 0000000000..d36a7bd546 Binary files /dev/null and b/docs/osd_images/095.png differ diff --git a/docs/osd_images/096.png b/docs/osd_images/096.png new file mode 100644 index 0000000000..1d5c386f45 Binary files /dev/null and b/docs/osd_images/096.png differ diff --git a/docs/osd_images/097.png b/docs/osd_images/097.png new file mode 100644 index 0000000000..4da4737c9b Binary files /dev/null and b/docs/osd_images/097.png differ diff --git a/docs/osd_images/098.png b/docs/osd_images/098.png new file mode 100644 index 0000000000..ad41226a63 Binary files /dev/null and b/docs/osd_images/098.png differ diff --git a/docs/osd_images/099.png b/docs/osd_images/099.png new file mode 100644 index 0000000000..a65b63546c Binary files /dev/null and b/docs/osd_images/099.png differ diff --git a/docs/osd_images/100.png b/docs/osd_images/100.png new file mode 100644 index 0000000000..8ca9aceb5c Binary files /dev/null and b/docs/osd_images/100.png differ diff --git a/docs/osd_images/101.png b/docs/osd_images/101.png new file mode 100644 index 0000000000..1d3dde884a Binary files /dev/null and b/docs/osd_images/101.png differ diff --git a/docs/osd_images/102.png b/docs/osd_images/102.png new file mode 100644 index 0000000000..b803c914d1 Binary files /dev/null and b/docs/osd_images/102.png differ diff --git a/docs/osd_images/103.png b/docs/osd_images/103.png new file mode 100644 index 0000000000..1ade7a30e1 Binary files /dev/null and b/docs/osd_images/103.png differ diff --git a/docs/osd_images/104.png b/docs/osd_images/104.png new file mode 100644 index 0000000000..df1a263c6f Binary files /dev/null and b/docs/osd_images/104.png differ diff --git a/docs/osd_images/105.png b/docs/osd_images/105.png new file mode 100644 index 0000000000..b501901303 Binary files /dev/null and b/docs/osd_images/105.png differ diff --git a/docs/osd_images/106.png b/docs/osd_images/106.png new file mode 100644 index 0000000000..6e64998b7e Binary files /dev/null and b/docs/osd_images/106.png differ diff --git a/docs/osd_images/107.png b/docs/osd_images/107.png new file mode 100644 index 0000000000..74ee2ded69 Binary files /dev/null and b/docs/osd_images/107.png differ diff --git a/docs/osd_images/108.png b/docs/osd_images/108.png new file mode 100644 index 0000000000..06c3d9b2f5 Binary files /dev/null and b/docs/osd_images/108.png differ diff --git a/docs/osd_images/109.png b/docs/osd_images/109.png new file mode 100644 index 0000000000..404f32bfa7 Binary files /dev/null and b/docs/osd_images/109.png differ diff --git a/docs/osd_images/110.png b/docs/osd_images/110.png new file mode 100644 index 0000000000..0707015b83 Binary files /dev/null and b/docs/osd_images/110.png differ diff --git a/docs/osd_images/111.png b/docs/osd_images/111.png new file mode 100644 index 0000000000..8cd300650d Binary files /dev/null and b/docs/osd_images/111.png differ diff --git a/docs/osd_images/112.png b/docs/osd_images/112.png new file mode 100644 index 0000000000..b48db90c44 Binary files /dev/null and b/docs/osd_images/112.png differ diff --git a/docs/osd_images/113.png b/docs/osd_images/113.png new file mode 100644 index 0000000000..1814ff712f Binary files /dev/null and b/docs/osd_images/113.png differ diff --git a/docs/osd_images/114.png b/docs/osd_images/114.png new file mode 100644 index 0000000000..3628315876 Binary files /dev/null and b/docs/osd_images/114.png differ diff --git a/docs/osd_images/115.png b/docs/osd_images/115.png new file mode 100644 index 0000000000..69095abd3c Binary files /dev/null and b/docs/osd_images/115.png differ diff --git a/docs/osd_images/116.png b/docs/osd_images/116.png new file mode 100644 index 0000000000..e953dbc30a Binary files /dev/null and b/docs/osd_images/116.png differ diff --git a/docs/osd_images/117.png b/docs/osd_images/117.png new file mode 100644 index 0000000000..c9be062e39 Binary files /dev/null and b/docs/osd_images/117.png differ diff --git a/docs/osd_images/118.png b/docs/osd_images/118.png new file mode 100644 index 0000000000..544dc8c19c Binary files /dev/null and b/docs/osd_images/118.png differ diff --git a/docs/osd_images/119.png b/docs/osd_images/119.png new file mode 100644 index 0000000000..8a24a9287a Binary files /dev/null and b/docs/osd_images/119.png differ diff --git a/docs/osd_images/120.png b/docs/osd_images/120.png new file mode 100644 index 0000000000..65576d4b50 Binary files /dev/null and b/docs/osd_images/120.png differ diff --git a/docs/osd_images/121.png b/docs/osd_images/121.png new file mode 100644 index 0000000000..0b9dac5b67 Binary files /dev/null and b/docs/osd_images/121.png differ diff --git a/docs/osd_images/123.png b/docs/osd_images/123.png new file mode 100644 index 0000000000..c18a48cd72 Binary files /dev/null and b/docs/osd_images/123.png differ diff --git a/docs/osd_images/124.png b/docs/osd_images/124.png new file mode 100644 index 0000000000..85249519ab Binary files /dev/null and b/docs/osd_images/124.png differ diff --git a/docs/osd_images/125.png b/docs/osd_images/125.png new file mode 100644 index 0000000000..0c8e476b7d Binary files /dev/null and b/docs/osd_images/125.png differ diff --git a/docs/osd_images/126.png b/docs/osd_images/126.png new file mode 100644 index 0000000000..f947f0e230 Binary files /dev/null and b/docs/osd_images/126.png differ diff --git a/docs/osd_images/127.png b/docs/osd_images/127.png new file mode 100644 index 0000000000..405b704988 Binary files /dev/null and b/docs/osd_images/127.png differ diff --git a/docs/osd_images/128.png b/docs/osd_images/128.png new file mode 100644 index 0000000000..36351565b0 Binary files /dev/null and b/docs/osd_images/128.png differ diff --git a/docs/osd_images/129.png b/docs/osd_images/129.png new file mode 100644 index 0000000000..078b2e4119 Binary files /dev/null and b/docs/osd_images/129.png differ diff --git a/docs/osd_images/130.png b/docs/osd_images/130.png new file mode 100644 index 0000000000..fc8dc49f96 Binary files /dev/null and b/docs/osd_images/130.png differ diff --git a/docs/osd_images/131.png b/docs/osd_images/131.png new file mode 100644 index 0000000000..4b50a677fc Binary files /dev/null and b/docs/osd_images/131.png differ diff --git a/docs/osd_images/132.png b/docs/osd_images/132.png new file mode 100644 index 0000000000..edd4fd9bc2 Binary files /dev/null and b/docs/osd_images/132.png differ diff --git a/docs/osd_images/133.png b/docs/osd_images/133.png new file mode 100644 index 0000000000..9961bf5b40 Binary files /dev/null and b/docs/osd_images/133.png differ diff --git a/docs/osd_images/134.png b/docs/osd_images/134.png new file mode 100644 index 0000000000..34ff7d7607 Binary files /dev/null and b/docs/osd_images/134.png differ diff --git a/docs/osd_images/135.png b/docs/osd_images/135.png new file mode 100644 index 0000000000..9196231818 Binary files /dev/null and b/docs/osd_images/135.png differ diff --git a/docs/osd_images/136.png b/docs/osd_images/136.png new file mode 100644 index 0000000000..41f5762bed Binary files /dev/null and b/docs/osd_images/136.png differ diff --git a/docs/osd_images/137.png b/docs/osd_images/137.png new file mode 100644 index 0000000000..2440eda755 Binary files /dev/null and b/docs/osd_images/137.png differ diff --git a/docs/osd_images/138.png b/docs/osd_images/138.png new file mode 100644 index 0000000000..cc4c54669f Binary files /dev/null and b/docs/osd_images/138.png differ diff --git a/docs/osd_images/139.png b/docs/osd_images/139.png new file mode 100644 index 0000000000..82b1b20b3d Binary files /dev/null and b/docs/osd_images/139.png differ diff --git a/docs/osd_images/140.png b/docs/osd_images/140.png new file mode 100644 index 0000000000..8d8175f471 Binary files /dev/null and b/docs/osd_images/140.png differ diff --git a/docs/osd_images/141.png b/docs/osd_images/141.png new file mode 100644 index 0000000000..49d7199513 Binary files /dev/null and b/docs/osd_images/141.png differ diff --git a/docs/osd_images/142.png b/docs/osd_images/142.png new file mode 100644 index 0000000000..ff5f888876 Binary files /dev/null and b/docs/osd_images/142.png differ diff --git a/docs/osd_images/143.png b/docs/osd_images/143.png new file mode 100644 index 0000000000..fca530fd4e Binary files /dev/null and b/docs/osd_images/143.png differ diff --git a/docs/osd_images/144.png b/docs/osd_images/144.png new file mode 100644 index 0000000000..6d85ec2aa7 Binary files /dev/null and b/docs/osd_images/144.png differ diff --git a/docs/osd_images/145.png b/docs/osd_images/145.png new file mode 100644 index 0000000000..6241353bb6 Binary files /dev/null and b/docs/osd_images/145.png differ diff --git a/docs/osd_images/146.png b/docs/osd_images/146.png new file mode 100644 index 0000000000..99ee059c5b Binary files /dev/null and b/docs/osd_images/146.png differ diff --git a/docs/osd_images/147.png b/docs/osd_images/147.png new file mode 100644 index 0000000000..fd308af701 Binary files /dev/null and b/docs/osd_images/147.png differ diff --git a/docs/osd_images/148.png b/docs/osd_images/148.png new file mode 100644 index 0000000000..e51c407e44 Binary files /dev/null and b/docs/osd_images/148.png differ diff --git a/docs/osd_images/149.png b/docs/osd_images/149.png new file mode 100644 index 0000000000..316f88e4e0 Binary files /dev/null and b/docs/osd_images/149.png differ diff --git a/docs/osd_images/150.png b/docs/osd_images/150.png new file mode 100644 index 0000000000..48cbf1e245 Binary files /dev/null and b/docs/osd_images/150.png differ diff --git a/docs/osd_images/151.png b/docs/osd_images/151.png new file mode 100644 index 0000000000..73614b2509 Binary files /dev/null and b/docs/osd_images/151.png differ diff --git a/docs/osd_images/152.png b/docs/osd_images/152.png new file mode 100644 index 0000000000..ef694b10fb Binary files /dev/null and b/docs/osd_images/152.png differ diff --git a/docs/osd_images/153.png b/docs/osd_images/153.png new file mode 100644 index 0000000000..a30c7f7d05 Binary files /dev/null and b/docs/osd_images/153.png differ diff --git a/docs/osd_images/154.png b/docs/osd_images/154.png new file mode 100644 index 0000000000..1ab4a8bad5 Binary files /dev/null and b/docs/osd_images/154.png differ diff --git a/docs/osd_images/155.png b/docs/osd_images/155.png new file mode 100644 index 0000000000..51431a6908 Binary files /dev/null and b/docs/osd_images/155.png differ diff --git a/docs/osd_images/156.png b/docs/osd_images/156.png new file mode 100644 index 0000000000..57c3830b27 Binary files /dev/null and b/docs/osd_images/156.png differ diff --git a/docs/osd_images/157.png b/docs/osd_images/157.png new file mode 100644 index 0000000000..9cc05cd2d1 Binary files /dev/null and b/docs/osd_images/157.png differ diff --git a/docs/osd_images/158.png b/docs/osd_images/158.png new file mode 100644 index 0000000000..3d30950a3d Binary files /dev/null and b/docs/osd_images/158.png differ diff --git a/docs/osd_images/159.png b/docs/osd_images/159.png new file mode 100644 index 0000000000..9580b05425 Binary files /dev/null and b/docs/osd_images/159.png differ diff --git a/docs/osd_images/160.png b/docs/osd_images/160.png new file mode 100644 index 0000000000..74e4e3fbc1 Binary files /dev/null and b/docs/osd_images/160.png differ diff --git a/docs/osd_images/161.png b/docs/osd_images/161.png new file mode 100644 index 0000000000..1093983de8 Binary files /dev/null and b/docs/osd_images/161.png differ diff --git a/docs/osd_images/184.png b/docs/osd_images/184.png new file mode 100644 index 0000000000..b5c47af1fa Binary files /dev/null and b/docs/osd_images/184.png differ diff --git a/docs/osd_images/185.png b/docs/osd_images/185.png new file mode 100644 index 0000000000..d2ab878090 Binary files /dev/null and b/docs/osd_images/185.png differ diff --git a/docs/osd_images/186.png b/docs/osd_images/186.png new file mode 100644 index 0000000000..6595ff0bcd Binary files /dev/null and b/docs/osd_images/186.png differ diff --git a/docs/osd_images/187.png b/docs/osd_images/187.png new file mode 100644 index 0000000000..af83bf9138 Binary files /dev/null and b/docs/osd_images/187.png differ diff --git a/docs/osd_images/188.png b/docs/osd_images/188.png new file mode 100644 index 0000000000..61d80dd84d Binary files /dev/null and b/docs/osd_images/188.png differ diff --git a/docs/osd_images/191.png b/docs/osd_images/191.png new file mode 100644 index 0000000000..ff73a3db71 Binary files /dev/null and b/docs/osd_images/191.png differ diff --git a/docs/osd_images/193.png b/docs/osd_images/193.png new file mode 100644 index 0000000000..67fec0bcc4 Binary files /dev/null and b/docs/osd_images/193.png differ diff --git a/docs/osd_images/194.png b/docs/osd_images/194.png new file mode 100644 index 0000000000..8dc9234614 Binary files /dev/null and b/docs/osd_images/194.png differ diff --git a/docs/osd_images/196.png b/docs/osd_images/196.png new file mode 100644 index 0000000000..3f6350489e Binary files /dev/null and b/docs/osd_images/196.png differ diff --git a/docs/osd_images/197.png b/docs/osd_images/197.png new file mode 100644 index 0000000000..4a64358242 Binary files /dev/null and b/docs/osd_images/197.png differ diff --git a/docs/osd_images/198.png b/docs/osd_images/198.png new file mode 100644 index 0000000000..cddc1edfb9 Binary files /dev/null and b/docs/osd_images/198.png differ diff --git a/docs/osd_images/199.png b/docs/osd_images/199.png new file mode 100644 index 0000000000..e203133dac Binary files /dev/null and b/docs/osd_images/199.png differ diff --git a/docs/osd_images/200.png b/docs/osd_images/200.png new file mode 100644 index 0000000000..1160eb2332 Binary files /dev/null and b/docs/osd_images/200.png differ diff --git a/docs/osd_images/201.png b/docs/osd_images/201.png new file mode 100644 index 0000000000..918dc27573 Binary files /dev/null and b/docs/osd_images/201.png differ diff --git a/docs/osd_images/202.png b/docs/osd_images/202.png new file mode 100644 index 0000000000..d68838eed2 Binary files /dev/null and b/docs/osd_images/202.png differ diff --git a/docs/osd_images/203.png b/docs/osd_images/203.png new file mode 100644 index 0000000000..f34b2cf9c7 Binary files /dev/null and b/docs/osd_images/203.png differ diff --git a/docs/osd_images/204.png b/docs/osd_images/204.png new file mode 100644 index 0000000000..7684e17af8 Binary files /dev/null and b/docs/osd_images/204.png differ diff --git a/docs/osd_images/205.png b/docs/osd_images/205.png new file mode 100644 index 0000000000..8803d04ab9 Binary files /dev/null and b/docs/osd_images/205.png differ diff --git a/docs/osd_images/206.png b/docs/osd_images/206.png new file mode 100644 index 0000000000..4cc4dbeda9 Binary files /dev/null and b/docs/osd_images/206.png differ diff --git a/docs/osd_images/207.png b/docs/osd_images/207.png new file mode 100644 index 0000000000..87129e42d4 Binary files /dev/null and b/docs/osd_images/207.png differ diff --git a/docs/osd_images/209.png b/docs/osd_images/209.png new file mode 100644 index 0000000000..6fcf71fe52 Binary files /dev/null and b/docs/osd_images/209.png differ diff --git a/docs/osd_images/210.png b/docs/osd_images/210.png new file mode 100644 index 0000000000..98d5554419 Binary files /dev/null and b/docs/osd_images/210.png differ diff --git a/docs/osd_images/211.png b/docs/osd_images/211.png new file mode 100644 index 0000000000..3ffd8e7845 Binary files /dev/null and b/docs/osd_images/211.png differ diff --git a/docs/osd_images/212.png b/docs/osd_images/212.png new file mode 100644 index 0000000000..6f8e05eea2 Binary files /dev/null and b/docs/osd_images/212.png differ diff --git a/docs/osd_images/213.png b/docs/osd_images/213.png new file mode 100644 index 0000000000..2b65a5023e Binary files /dev/null and b/docs/osd_images/213.png differ diff --git a/docs/osd_images/214.png b/docs/osd_images/214.png new file mode 100644 index 0000000000..d86f564f96 Binary files /dev/null and b/docs/osd_images/214.png differ diff --git a/docs/osd_images/215.png b/docs/osd_images/215.png new file mode 100644 index 0000000000..52ddcfd54c Binary files /dev/null and b/docs/osd_images/215.png differ diff --git a/docs/osd_images/216.png b/docs/osd_images/216.png new file mode 100644 index 0000000000..fd0f3c718a Binary files /dev/null and b/docs/osd_images/216.png differ diff --git a/docs/osd_images/217.png b/docs/osd_images/217.png new file mode 100644 index 0000000000..bfbb32dfe7 Binary files /dev/null and b/docs/osd_images/217.png differ diff --git a/docs/osd_images/218.png b/docs/osd_images/218.png new file mode 100644 index 0000000000..a38d5e806e Binary files /dev/null and b/docs/osd_images/218.png differ diff --git a/docs/osd_images/219.png b/docs/osd_images/219.png new file mode 100644 index 0000000000..f2cf452ba6 Binary files /dev/null and b/docs/osd_images/219.png differ diff --git a/docs/osd_images/220.png b/docs/osd_images/220.png new file mode 100644 index 0000000000..4292af2630 Binary files /dev/null and b/docs/osd_images/220.png differ diff --git a/docs/osd_images/221.png b/docs/osd_images/221.png new file mode 100644 index 0000000000..965a233a3e Binary files /dev/null and b/docs/osd_images/221.png differ diff --git a/docs/osd_images/222.png b/docs/osd_images/222.png new file mode 100644 index 0000000000..16a9ec4ff6 Binary files /dev/null and b/docs/osd_images/222.png differ diff --git a/docs/osd_images/223.png b/docs/osd_images/223.png new file mode 100644 index 0000000000..8f8106c723 Binary files /dev/null and b/docs/osd_images/223.png differ diff --git a/docs/osd_images/224.png b/docs/osd_images/224.png new file mode 100644 index 0000000000..940647d5e7 Binary files /dev/null and b/docs/osd_images/224.png differ diff --git a/docs/osd_images/225.png b/docs/osd_images/225.png new file mode 100644 index 0000000000..2cea870056 Binary files /dev/null and b/docs/osd_images/225.png differ diff --git a/docs/osd_images/226.png b/docs/osd_images/226.png new file mode 100644 index 0000000000..e811d4dcb0 Binary files /dev/null and b/docs/osd_images/226.png differ diff --git a/docs/osd_images/227.png b/docs/osd_images/227.png new file mode 100644 index 0000000000..4b3a65a152 Binary files /dev/null and b/docs/osd_images/227.png differ diff --git a/docs/osd_images/228.png b/docs/osd_images/228.png new file mode 100644 index 0000000000..f0a337f1a3 Binary files /dev/null and b/docs/osd_images/228.png differ diff --git a/docs/osd_images/229.png b/docs/osd_images/229.png new file mode 100644 index 0000000000..b4b144811e Binary files /dev/null and b/docs/osd_images/229.png differ diff --git a/docs/osd_images/230.png b/docs/osd_images/230.png new file mode 100644 index 0000000000..e674ad5448 Binary files /dev/null and b/docs/osd_images/230.png differ diff --git a/docs/osd_images/231.png b/docs/osd_images/231.png new file mode 100644 index 0000000000..5735531a0f Binary files /dev/null and b/docs/osd_images/231.png differ diff --git a/docs/osd_images/234.png b/docs/osd_images/234.png new file mode 100644 index 0000000000..d768181d84 Binary files /dev/null and b/docs/osd_images/234.png differ diff --git a/docs/osd_images/235.png b/docs/osd_images/235.png new file mode 100644 index 0000000000..dfa2203a40 Binary files /dev/null and b/docs/osd_images/235.png differ diff --git a/docs/osd_images/236.png b/docs/osd_images/236.png new file mode 100644 index 0000000000..75e10bc99f Binary files /dev/null and b/docs/osd_images/236.png differ diff --git a/docs/osd_images/237.png b/docs/osd_images/237.png new file mode 100644 index 0000000000..b341e6822a Binary files /dev/null and b/docs/osd_images/237.png differ diff --git a/docs/osd_images/238.png b/docs/osd_images/238.png new file mode 100644 index 0000000000..7c9850a646 Binary files /dev/null and b/docs/osd_images/238.png differ diff --git a/docs/osd_images/239.png b/docs/osd_images/239.png new file mode 100644 index 0000000000..1bd053b22f Binary files /dev/null and b/docs/osd_images/239.png differ diff --git a/docs/osd_images/240.png b/docs/osd_images/240.png new file mode 100644 index 0000000000..c71e2caf58 Binary files /dev/null and b/docs/osd_images/240.png differ diff --git a/docs/osd_images/241.png b/docs/osd_images/241.png new file mode 100644 index 0000000000..4af2713cf3 Binary files /dev/null and b/docs/osd_images/241.png differ diff --git a/docs/osd_images/242.png b/docs/osd_images/242.png new file mode 100644 index 0000000000..978e01e8e6 Binary files /dev/null and b/docs/osd_images/242.png differ diff --git a/docs/osd_images/243.png b/docs/osd_images/243.png new file mode 100644 index 0000000000..4eafab5b0b Binary files /dev/null and b/docs/osd_images/243.png differ diff --git a/docs/osd_images/244.png b/docs/osd_images/244.png new file mode 100644 index 0000000000..5cbc4031f7 Binary files /dev/null and b/docs/osd_images/244.png differ diff --git a/docs/osd_images/245.png b/docs/osd_images/245.png new file mode 100644 index 0000000000..b2cd6d84d9 Binary files /dev/null and b/docs/osd_images/245.png differ diff --git a/docs/osd_images/246.png b/docs/osd_images/246.png new file mode 100644 index 0000000000..35f994571f Binary files /dev/null and b/docs/osd_images/246.png differ diff --git a/docs/osd_images/247.png b/docs/osd_images/247.png new file mode 100644 index 0000000000..72b8b0d225 Binary files /dev/null and b/docs/osd_images/247.png differ diff --git a/docs/osd_images/248.png b/docs/osd_images/248.png new file mode 100644 index 0000000000..f4f9f52cdd Binary files /dev/null and b/docs/osd_images/248.png differ diff --git a/docs/osd_images/249.png b/docs/osd_images/249.png new file mode 100644 index 0000000000..7316c4522f Binary files /dev/null and b/docs/osd_images/249.png differ diff --git a/docs/osd_images/250.png b/docs/osd_images/250.png new file mode 100644 index 0000000000..256920c50b Binary files /dev/null and b/docs/osd_images/250.png differ diff --git a/docs/osd_images/251.png b/docs/osd_images/251.png new file mode 100644 index 0000000000..4735dec762 Binary files /dev/null and b/docs/osd_images/251.png differ diff --git a/docs/osd_images/252.png b/docs/osd_images/252.png new file mode 100644 index 0000000000..02ac6e7d57 Binary files /dev/null and b/docs/osd_images/252.png differ diff --git a/docs/osd_images/253.png b/docs/osd_images/253.png new file mode 100644 index 0000000000..3f1f357509 Binary files /dev/null and b/docs/osd_images/253.png differ diff --git a/docs/osd_images/254.png b/docs/osd_images/254.png new file mode 100644 index 0000000000..8743c22e91 Binary files /dev/null and b/docs/osd_images/254.png differ diff --git a/make/mcu/STM32F3.mk b/make/mcu/STM32F3.mk index c0d96caace..4b60e91723 100644 --- a/make/mcu/STM32F3.mk +++ b/make/mcu/STM32F3.mk @@ -43,7 +43,14 @@ DEVICE_STDPERIPH_SRC := $(DEVICE_STDPERIPH_SRC)\ $(USBPERIPH_SRC) endif -ifneq ($(filter SDCARD, $(FEATURES)),) +ifneq ($(filter SDCARD_SPI, $(FEATURES)),) +INCLUDE_DIRS := $(INCLUDE_DIRS) \ + $(FATFS_DIR) \ + +VPATH := $(VPATH):$(FATFS_DIR) +endif + +ifneq ($(filter SDCARD_SDIO, $(FEATURES)),) INCLUDE_DIRS := $(INCLUDE_DIRS) \ $(FATFS_DIR) \ diff --git a/make/mcu/STM32F4.mk b/make/mcu/STM32F4.mk index f5bdcd5432..a7abfb7efd 100644 --- a/make/mcu/STM32F4.mk +++ b/make/mcu/STM32F4.mk @@ -137,7 +137,13 @@ INCLUDE_DIRS := $(INCLUDE_DIRS) \ $(ROOT)/src/main/vcpf4 endif -ifneq ($(filter SDCARD,$(FEATURES)),) +ifneq ($(filter SDCARD_SPI,$(FEATURES)),) +INCLUDE_DIRS := $(INCLUDE_DIRS) \ + $(FATFS_DIR) +VPATH := $(VPATH):$(FATFS_DIR) +endif + +ifneq ($(filter SDCARD_SDIO,$(FEATURES)),) INCLUDE_DIRS := $(INCLUDE_DIRS) \ $(FATFS_DIR) VPATH := $(VPATH):$(FATFS_DIR) @@ -202,12 +208,12 @@ MSC_SRC = \ msc/usbd_msc_desc.c \ msc/usbd_storage.c -ifneq ($(filter SDCARD,$(FEATURES)),) +ifneq ($(filter SDCARD_SPI,$(FEATURES)),) MSC_SRC += \ msc/usbd_storage_sd_spi.c endif -ifneq ($(filter SDIO,$(FEATURES)),) +ifneq ($(filter SDCARD_SDIO,$(FEATURES)),) MSC_SRC += \ msc/usbd_storage_sdio.c MCU_COMMON_SRC += \ diff --git a/make/mcu/STM32F7.mk b/make/mcu/STM32F7.mk index 9bcb643a92..2c5733415c 100644 --- a/make/mcu/STM32F7.mk +++ b/make/mcu/STM32F7.mk @@ -117,7 +117,13 @@ INCLUDE_DIRS := $(INCLUDE_DIRS) \ $(ROOT)/lib/main/STM32F7/Drivers/CMSIS/Device/ST/STM32F7xx/Include \ $(ROOT)/src/main/vcp_hal -ifneq ($(filter SDCARD,$(FEATURES)),) +ifneq ($(filter SDCARD_SPI,$(FEATURES)),) +INCLUDE_DIRS := $(INCLUDE_DIRS) \ + $(FATFS_DIR) +VPATH := $(VPATH):$(FATFS_DIR) +endif + +ifneq ($(filter SDCARD_SDIO,$(FEATURES)),) INCLUDE_DIRS := $(INCLUDE_DIRS) \ $(FATFS_DIR) VPATH := $(VPATH):$(FATFS_DIR) @@ -127,7 +133,12 @@ endif ARCH_FLAGS = -mthumb -mcpu=cortex-m7 -mfloat-abi=hard -mfpu=fpv5-sp-d16 -fsingle-precision-constant -Wdouble-promotion DEVICE_FLAGS = -DUSE_HAL_DRIVER -DUSE_FULL_LL_DRIVER -ifeq ($(TARGET),$(filter $(TARGET),$(F7X5XG_TARGETS))) +ifeq ($(TARGET),$(filter $(TARGET),$(F7X5XI_TARGETS))) +DEVICE_FLAGS += -DSTM32F765xx +LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f765.ld +STARTUP_SRC = startup_stm32f765xx.s +TARGET_FLASH = 2048 +else ifeq ($(TARGET),$(filter $(TARGET),$(F7X5XG_TARGETS))) DEVICE_FLAGS += -DSTM32F745xx LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f74x.ld STARTUP_SRC = startup_stm32f745xx.s @@ -184,14 +195,14 @@ MSC_SRC = \ drivers/usb_msc_f7xx.c \ msc/usbd_storage.c -ifneq ($(filter SDIO,$(FEATURES)),) +ifneq ($(filter SDCARD_SDIO,$(FEATURES)),) MCU_COMMON_SRC += \ drivers/sdio_f7xx.c MSC_SRC += \ msc/usbd_storage_sdio.c endif -ifneq ($(filter SDCARD,$(FEATURES)),) +ifneq ($(filter SDCARD_SPI,$(FEATURES)),) MSC_SRC += \ msc/usbd_storage_sd_spi.c endif diff --git a/make/source.mk b/make/source.mk index 4a80c8e5b4..c17feaaabf 100644 --- a/make/source.mk +++ b/make/source.mk @@ -38,9 +38,9 @@ COMMON_SRC = \ drivers/transponder_ir_erlt.c \ fc/board_info.c \ fc/config.c \ - fc/fc_dispatch.c \ - fc/fc_hardfaults.c \ - fc/fc_tasks.c \ + fc/dispatch.c \ + fc/hardfaults.c \ + fc/tasks.c \ fc/runtime_config.c \ interface/msp.c \ interface/msp_box.c \ @@ -51,21 +51,16 @@ COMMON_SRC = \ io/serial.c \ io/statusindicator.c \ io/transponder_ir.c \ + io/usb_cdc_hid.c \ + io/usb_msc.c \ msp/msp_serial.c \ scheduler/scheduler.c \ sensors/adcinternal.c \ sensors/battery.c \ sensors/current.c \ sensors/voltage.c \ - target/config_helper.c - -OSD_SLAVE_SRC = \ - io/displayport_max7456.c \ - osd_slave/osd_slave_init.c \ - io/osd_slave.c - -FC_SRC = \ - fc/fc_init.c \ + target/config_helper.c \ + fc/init.c \ fc/controlrate_profile.c \ drivers/camera_control.c \ drivers/accgyro/gyro_sync.c \ @@ -75,8 +70,8 @@ FC_SRC = \ drivers/rx/rx_xn297.c \ drivers/rx/rx_pwm.c \ drivers/serial_softserial.c \ - fc/fc_core.c \ - fc/fc_rc.c \ + fc/core.c \ + fc/rc.c \ fc/rc_adjustments.c \ fc/rc_controls.c \ fc/rc_modes.c \ @@ -140,12 +135,11 @@ FC_SRC = \ io/displayport_max7456.c \ io/displayport_msp.c \ io/displayport_oled.c \ - io/displayport_rcdevice.c \ io/displayport_srxl.c \ io/displayport_crsf.c \ + io/displayport_hott.c \ io/rcdevice_cam.c \ io/rcdevice.c \ - io/rcdevice_osd.c \ io/gps.c \ io/ledstrip.c \ io/osd.c \ @@ -176,12 +170,7 @@ COMMON_DEVICE_SRC = \ $(CMSIS_SRC) \ $(DEVICE_STDPERIPH_SRC) -ifeq ($(OSD_SLAVE),yes) -TARGET_FLAGS := -DUSE_OSD_SLAVE $(TARGET_FLAGS) -COMMON_SRC := $(COMMON_SRC) $(OSD_SLAVE_SRC) $(COMMON_DEVICE_SRC) -else -COMMON_SRC := $(COMMON_SRC) $(FC_SRC) $(COMMON_DEVICE_SRC) -endif +COMMON_SRC := $(COMMON_SRC) $(COMMON_DEVICE_SRC) ifeq ($(SIMULATOR_BUILD),yes) TARGET_FLAGS := -DSIMULATOR_BUILD $(TARGET_FLAGS) @@ -224,9 +213,9 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \ drivers/serial_uart.c \ drivers/system.c \ drivers/timer.c \ - fc/fc_core.c \ - fc/fc_tasks.c \ - fc/fc_rc.c \ + fc/core.c \ + fc/tasks.c \ + fc/rc.c \ fc/rc_controls.c \ fc/runtime_config.c \ flight/imu.c \ @@ -266,6 +255,7 @@ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \ drivers/compass/compass_fake.c \ drivers/compass/compass_hmc5883l.c \ drivers/compass/compass_qmc5883l.c \ + drivers/compass/compass_lis3mdl.c \ drivers/display_ug2864hsweg01.c \ drivers/inverter.c \ drivers/light_ws2811strip.c \ @@ -282,7 +272,7 @@ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \ drivers/vtx_rtc6705_soft_spi.c \ drivers/vtx_rtc6705.c \ drivers/vtx_common.c \ - fc/fc_init.c \ + fc/init.c \ fc/board_info.c \ config/config_eeprom.c \ config/feature.c \ @@ -297,6 +287,7 @@ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \ io/serial_4way_avrootloader.c \ io/serial_4way_stk500v2.c \ io/transponder_ir.c \ + io/usb_cdc_hid.c \ msp/msp_serial.c \ cms/cms.c \ cms/cms_menu_blackbox.c \ @@ -364,17 +355,19 @@ SRC += $(COMMON_SRC) #excludes SRC := $(filter-out $(MCU_EXCLUDES), $(SRC)) -ifneq ($(filter SDCARD,$(FEATURES)),) +ifneq ($(filter SDCARD_SPI,$(FEATURES)),) SRC += \ drivers/sdcard.c \ + drivers/sdcard_spi.c \ drivers/sdcard_standard.c \ io/asyncfatfs/asyncfatfs.c \ io/asyncfatfs/fat_standard.c \ $(MSC_SRC) endif -ifneq ($(filter SDIO,$(FEATURES)),) +ifneq ($(filter SDCARD_SDIO,$(FEATURES)),) SRC += \ + drivers/sdcard.c \ drivers/sdcard_sdio_baremetal.c \ drivers/sdcard_standard.c \ io/asyncfatfs/asyncfatfs.c \ diff --git a/make/targets.mk b/make/targets.mk index da17b64f93..99d9ee844e 100644 --- a/make/targets.mk +++ b/make/targets.mk @@ -1,161 +1,19 @@ -OFFICIAL_TARGETS = ALIENFLIGHTF3 ALIENFLIGHTF4 ANYFCF7 BETAFLIGHTF3 BLUEJAYF4 FURYF4 REVO SIRINFPV SPARKY SPRACINGF3 SPRACINGF3EVO SPRACINGF3NEO SPRACINGF4EVO SPRACINGF7DUAL STM32F3DISCOVERY -ALT_TARGETS = $(sort $(filter-out target, $(basename $(notdir $(wildcard $(ROOT)/src/main/target/*/*.mk))))) -NOBUILD_TARGETS = $(sort $(filter-out target, $(basename $(notdir $(wildcard $(ROOT)/src/main/target/*/*.nomk))))) -OPBL_TARGETS = $(filter %_OPBL, $(ALT_TARGETS)) -OSD_SLAVE_TARGETS = SPRACINGF3OSD - -VALID_TARGETS = $(dir $(wildcard $(ROOT)/src/main/target/*/target.mk)) -VALID_TARGETS := $(subst /,, $(subst ./src/main/target/,, $(VALID_TARGETS))) -VALID_TARGETS := $(VALID_TARGETS) $(ALT_TARGETS) -VALID_TARGETS := $(sort $(VALID_TARGETS)) -VALID_TARGETS := $(filter-out $(NOBUILD_TARGETS), $(VALID_TARGETS)) +include $(ROOT)/make/targets_list.mk ifeq ($(filter $(TARGET),$(NOBUILD_TARGETS)), $(TARGET)) ALTERNATES := $(sort $(filter-out target, $(basename $(notdir $(wildcard $(ROOT)/src/main/target/$(TARGET)/*.mk))))) $(error The target specified, $(TARGET), cannot be built. Use one of the ALT targets: $(ALTERNATES)) endif -UNSUPPORTED_TARGETS := \ - AFROMINI \ - ALIENFLIGHTF1 \ - BEEBRAIN \ - CC3D \ - CC3D_OPBL \ - CJMCU \ - MICROSCISKY \ - NAZE - -SUPPORTED_TARGETS := $(filter-out $(UNSUPPORTED_TARGETS), $(VALID_TARGETS)) - -GROUP_1_TARGETS := \ - AIORACERF3 \ - AIR32 \ - AIRBOTF4 \ - AIRBOTF4SD \ - AIRHEROF3 \ - ALIENFLIGHTF3 \ - ALIENFLIGHTF4 \ - ALIENFLIGHTNGF7 \ - ALIENWHOOPF4 \ - ALIENWHOOPF7 \ - ANYFCF7 \ - BEEBRAIN_V2D \ - BEEBRAIN_V2F \ - BEEROTORF4 \ - BETAFLIGHTF3 \ - BETAFLIGHTF4 \ - BLUEJAYF4 \ - CHEBUZZF3 \ - CLRACINGF4 \ - COLIBRI \ - COLIBRI_OPBL \ - COLIBRI_RACE \ - -GROUP_2_TARGETS := \ - DOGE \ - DYSF4PRO \ - EACHIF3 \ - ELLE0 \ - F4BY \ - FISHDRONEF4 \ - FLIP32F3OSD \ - FF_ACROWHOOPSP \ - FF_FORTINIF4 \ - FF_KOMBINI \ - FF_PIKOBLX \ - FF_PIKOF4 \ - FF_RADIANCE \ - FPVM_BETAFLIGHTF7 \ - FRSKYF3 \ - FRSKYF4 \ - FURYF3 \ - FURYF3OSD \ - FURYF4 \ - FURYF4OSD \ - FURYF7 \ - IMPULSERCF3 \ - IRCFUSIONF3 \ - ISHAPEDF3 \ - KAKUTEF4 \ - KAKUTEF7 \ - KISSCC \ - KISSFC \ - KIWIF4 \ - KIWIF4V2 \ - KROOZX - -GROUP_3_TARGETS := \ - LUX_RACE \ - LUXV2_RACE \ - LUXF4OSD \ - MLTEMPF4 \ - MLTYPHF4 \ - MOTOLAB \ - MULTIFLITEPICO \ - NERO \ - NUCLEOF7 \ - OMNIBUS \ - OMNIBUSF4 \ - OMNIBUSF4SD \ - OMNIBUSF7 \ - OMNIBUSF7V2 \ - OMNINXT4 \ - OMNINXT7 \ - PLUMF4 \ - PODIUMF4 \ - RACEBASE \ - RCEXPLORERF3 \ - RG_SSD_F3 \ - REVO \ - REVO_OPBL \ - REVOLT \ - REVONANO \ - RMDO - -GROUP_4_TARGETS := \ - SINGULARITY \ - SIRINFPV \ - SOULF4 \ - SPARKY \ - SPARKY2 \ - SPRACINGF3 \ - SPRACINGF3EVO \ - SPRACINGF3MINI \ - SPRACINGF3MQ \ - SPRACINGF3NEO \ - SPRACINGF3OSD \ - SPRACINGF4EVO \ - SPRACINGF4NEO \ - SPRACINGF7DUAL \ - STM32F3DISCOVERY \ - TINYBEEF3 \ - TINYFISH \ - VRRACE \ - XRACERF4 \ - X_RACERSPI \ - ZCOREF3 - -GROUP_OTHER_TARGETS := $(filter-out $(GROUP_1_TARGETS) $(GROUP_2_TARGETS) $(GROUP_3_TARGETS) $(GROUP_4_TARGETS), $(SUPPORTED_TARGETS)) - -ifeq ($(filter $(TARGET),$(ALT_TARGETS)), $(TARGET)) -BASE_TARGET := $(firstword $(subst /,, $(subst ./src/main/target/,, $(dir $(wildcard $(ROOT)/src/main/target/*/$(TARGET).mk))))) +BASE_TARGET := $(call get_base_target,$(TARGET)) +ifneq ($(TARGET),$(BASE_TARGET)) include $(ROOT)/src/main/target/$(BASE_TARGET)/$(TARGET).mk -else -BASE_TARGET := $(TARGET) endif ifeq ($(filter $(TARGET),$(OPBL_TARGETS)), $(TARGET)) OPBL = yes endif -ifeq ($(filter $(TARGET),$(OSD_SLAVE_TARGETS)), $(TARGET)) -# build an OSD SLAVE -OSD_SLAVE = yes -else -# build an FC -FC = yes -endif - # silently ignore if the file is not present. Allows for target specific. -include $(ROOT)/src/main/target/$(BASE_TARGET)/target.mk diff --git a/make/targets_list.mk b/make/targets_list.mk new file mode 100644 index 0000000000..fa2a59a496 --- /dev/null +++ b/make/targets_list.mk @@ -0,0 +1,67 @@ +OFFICIAL_TARGETS = \ + ALIENFLIGHTF3 \ + ALIENFLIGHTF4 \ + ANYFCF7 \ + BETAFLIGHTF3 \ + BLUEJAYF4 \ + FURYF4 REVO \ + SIRINFPV \ + SPARKY \ + SPRACINGF3 \ + SPRACINGF3EVO \ + SPRACINGF3NEO \ + SPRACINGF4EVO \ + SPRACINGF7DUAL \ + STM32F3DISCOVERY + +ALT_TARGET_PATHS = $(filter-out %/target,$(basename $(wildcard $(ROOT)/src/main/target/*/*.mk))) +ALT_TARGET_NAMES = $(notdir $(ALT_TARGET_PATHS)) +BASE_TARGET_NAMES = $(notdir $(patsubst %/,%,$(dir $(ALT_TARGET_PATHS)))) +BASE_ALT_PAIRS = $(join $(BASE_TARGET_NAMES:=/),$(ALT_TARGET_NAMES)) + +ALT_TARGETS = $(sort $(notdir $(BASE_ALT_PAIRS))) +BASE_TARGETS = $(sort $(notdir $(patsubst %/,%,$(dir $(wildcard $(ROOT)/src/main/target/*/target.mk))))) +NOBUILD_TARGETS = $(sort $(filter-out target,$(basename $(notdir $(wildcard $(ROOT)/src/main/target/*/*.nomk))))) +OPBL_TARGETS = $(sort $(filter %_OPBL,$(ALT_TARGETS))) + +VALID_TARGETS = $(sort $(filter-out $(NOBUILD_TARGETS),$(BASE_TARGETS) $(ALT_TARGETS))) + +# For alt targets, returns their base target name. +# For base targets, returns the (same) target name. +# param $1 = target name +find_target_pair = $(filter %/$(1),$(BASE_ALT_PAIRS)) +get_base_target = $(if $(call find_target_pair,$(1)),$(patsubst %/,%,$(dir $(call find_target_pair,$(1)))),$(1)) + +UNSUPPORTED_TARGETS := \ + AFROMINI \ + ALIENFLIGHTF1 \ + BEEBRAIN \ + CC3D \ + CC3D_OPBL \ + CJMCU \ + MICROSCISKY \ + NAZE + +SUPPORTED_TARGETS := $(filter-out $(UNSUPPORTED_TARGETS), $(VALID_TARGETS)) + +TARGETS_TOTAL := $(words $(SUPPORTED_TARGETS)) +TARGET_GROUPS := 5 +TARGETS_PER_GROUP := $(shell expr $(TARGETS_TOTAL) / $(TARGET_GROUPS) ) + +ST := 1 +ET := $(shell expr $(ST) + $(TARGETS_PER_GROUP)) +GROUP_1_TARGETS := $(wordlist $(ST), $(ET), $(SUPPORTED_TARGETS)) + +ST := $(shell expr $(ET) + 1) +ET := $(shell expr $(ST) + $(TARGETS_PER_GROUP)) +GROUP_2_TARGETS := $(wordlist $(ST), $(ET), $(SUPPORTED_TARGETS)) + +ST := $(shell expr $(ET) + 1) +ET := $(shell expr $(ST) + $(TARGETS_PER_GROUP)) +GROUP_3_TARGETS := $(wordlist $(ST), $(ET), $(SUPPORTED_TARGETS)) + +ST := $(shell expr $(ET) + 1) +ET := $(shell expr $(ST) + $(TARGETS_PER_GROUP)) +GROUP_4_TARGETS := $(wordlist $(ST), $(ET), $(SUPPORTED_TARGETS)) + +GROUP_OTHER_TARGETS := $(filter-out $(GROUP_1_TARGETS) $(GROUP_2_TARGETS) $(GROUP_3_TARGETS) $(GROUP_4_TARGETS), $(SUPPORTED_TARGETS)) diff --git a/make/tools.mk b/make/tools.mk index 52d71adb26..daa4878df4 100644 --- a/make/tools.mk +++ b/make/tools.mk @@ -14,9 +14,9 @@ ############################## # Set up ARM (STM32) SDK -ARM_SDK_DIR ?= $(TOOLS_DIR)/gcc-arm-none-eabi-7-2017-q4-major +ARM_SDK_DIR ?= $(TOOLS_DIR)/gcc-arm-none-eabi-7-2018-q2-update # Checked below, Should match the output of $(shell arm-none-eabi-gcc -dumpversion) -GCC_REQUIRED_VERSION ?= 7.2.1 +GCC_REQUIRED_VERSION ?= 7.3.1 .PHONY: arm_sdk_version @@ -26,7 +26,7 @@ arm_sdk_version: ## arm_sdk_install : Install Arm SDK .PHONY: arm_sdk_install -ARM_SDK_URL_BASE := https://developer.arm.com/-/media/Files/downloads/gnu-rm/7-2017q4/gcc-arm-none-eabi-7-2017-q4-major +ARM_SDK_URL_BASE := https://developer.arm.com/-/media/Files/downloads/gnu-rm/7-2018q2/gcc-arm-none-eabi-7-2018-q2-update # source: https://developer.arm.com/open-source/gnu-toolchain/gnu-rm/downloads ifdef LINUX diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 2950f9090e..7869a48ebf 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -53,7 +53,7 @@ #include "fc/config.h" #include "fc/controlrate_profile.h" -#include "fc/fc_rc.h" +#include "fc/rc.h" #include "fc/rc_controls.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" @@ -182,6 +182,9 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = { {"axisD", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_0)}, {"axisD", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_1)}, {"axisD", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_2)}, + {"axisF", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, + {"axisF", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, + {"axisF", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, /* rcCommands are encoded together as a group in P-frames: */ {"rcCommand", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)}, {"rcCommand", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)}, @@ -285,6 +288,7 @@ typedef struct blackboxMainState_s { int32_t axisPID_P[XYZ_AXIS_COUNT]; int32_t axisPID_I[XYZ_AXIS_COUNT]; int32_t axisPID_D[XYZ_AXIS_COUNT]; + int32_t axisPID_F[XYZ_AXIS_COUNT]; int16_t rcCommand[4]; int16_t gyroADC[XYZ_AXIS_COUNT]; @@ -444,7 +448,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition) #endif case FLIGHT_LOG_FIELD_CONDITION_RSSI: - return rxConfig()->rssi_channel > 0 || feature(FEATURE_RSSI_ADC); + return isRssiConfigured(); case FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME: return blackboxConfig()->p_ratio != 1; @@ -531,6 +535,8 @@ static void writeIntraframe(void) } } + blackboxWriteSignedVBArray(blackboxCurrent->axisPID_F, XYZ_AXIS_COUNT); + // Write roll, pitch and yaw first: blackboxWriteSigned16VBArray(blackboxCurrent->rcCommand, 3); @@ -662,6 +668,9 @@ static void writeInterframe(void) } } + arraySubInt32(deltas, blackboxCurrent->axisPID_F, blackboxLast->axisPID_F, XYZ_AXIS_COUNT); + blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT); + /* * RC tends to stay the same or fairly small for many frames at a time, so use an encoding that * can pack multiple values per byte: @@ -968,7 +977,7 @@ static void writeGPSFrame(timeUs_t currentTimeUs) blackboxWriteUnsignedVB(gpsSol.numSat); blackboxWriteSignedVB(gpsSol.llh.lat - gpsHistory.GPS_home[LAT]); blackboxWriteSignedVB(gpsSol.llh.lon - gpsHistory.GPS_home[LON]); - blackboxWriteUnsignedVB(gpsSol.llh.alt); + blackboxWriteUnsignedVB(gpsSol.llh.altCm / 10); // was originally designed to transport meters in int16, but +-3276.7m is a good compromise blackboxWriteUnsignedVB(gpsSol.groundSpeed); blackboxWriteUnsignedVB(gpsSol.groundCourse); @@ -992,6 +1001,7 @@ static void loadMainState(timeUs_t currentTimeUs) blackboxCurrent->axisPID_P[i] = pidData[i].P; blackboxCurrent->axisPID_I[i] = pidData[i].I; blackboxCurrent->axisPID_D[i] = pidData[i].D; + blackboxCurrent->axisPID_F[i] = pidData[i].F; blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i]); blackboxCurrent->accADC[i] = lrintf(acc.accADC[i]); #ifdef USE_MAG @@ -1244,6 +1254,9 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE("rates", "%d,%d,%d", currentControlRateProfile->rates[ROLL], currentControlRateProfile->rates[PITCH], currentControlRateProfile->rates[YAW]); + BLACKBOX_PRINT_HEADER_LINE("rate_limits", "%d,%d,%d", currentControlRateProfile->rate_limit[ROLL], + currentControlRateProfile->rate_limit[PITCH], + currentControlRateProfile->rate_limit[YAW]); BLACKBOX_PRINT_HEADER_LINE("rollPID", "%d,%d,%d", currentPidProfile->pid[PID_ROLL].P, currentPidProfile->pid[PID_ROLL].I, currentPidProfile->pid[PID_ROLL].D); @@ -1253,25 +1266,10 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE("yawPID", "%d,%d,%d", currentPidProfile->pid[PID_YAW].P, currentPidProfile->pid[PID_YAW].I, currentPidProfile->pid[PID_YAW].D); - BLACKBOX_PRINT_HEADER_LINE("altPID", "%d,%d,%d", currentPidProfile->pid[PID_ALT].P, - currentPidProfile->pid[PID_ALT].I, - currentPidProfile->pid[PID_ALT].D); - BLACKBOX_PRINT_HEADER_LINE("posPID", "%d,%d,%d", currentPidProfile->pid[PID_POS].P, - currentPidProfile->pid[PID_POS].I, - currentPidProfile->pid[PID_POS].D); - BLACKBOX_PRINT_HEADER_LINE("posrPID", "%d,%d,%d", currentPidProfile->pid[PID_POSR].P, - currentPidProfile->pid[PID_POSR].I, - currentPidProfile->pid[PID_POSR].D); - BLACKBOX_PRINT_HEADER_LINE("navrPID", "%d,%d,%d", currentPidProfile->pid[PID_NAVR].P, - currentPidProfile->pid[PID_NAVR].I, - currentPidProfile->pid[PID_NAVR].D); BLACKBOX_PRINT_HEADER_LINE("levelPID", "%d,%d,%d", currentPidProfile->pid[PID_LEVEL].P, currentPidProfile->pid[PID_LEVEL].I, currentPidProfile->pid[PID_LEVEL].D); BLACKBOX_PRINT_HEADER_LINE("magPID", "%d", currentPidProfile->pid[PID_MAG].P); - BLACKBOX_PRINT_HEADER_LINE("velPID", "%d,%d,%d", currentPidProfile->pid[PID_VEL].P, - currentPidProfile->pid[PID_VEL].I, - currentPidProfile->pid[PID_VEL].D); BLACKBOX_PRINT_HEADER_LINE("dterm_filter_type", "%d", currentPidProfile->dterm_filter_type); BLACKBOX_PRINT_HEADER_LINE("dterm_lowpass_hz", "%d", currentPidProfile->dterm_lowpass_hz); BLACKBOX_PRINT_HEADER_LINE("dterm_lowpass2_hz", "%d", currentPidProfile->dterm_lowpass2_hz); @@ -1283,10 +1281,15 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE("pidAtMinThrottle", "%d", currentPidProfile->pidAtMinThrottle); // Betaflight PID controller parameters + BLACKBOX_PRINT_HEADER_LINE("anti_gravity_mode", "%d", currentPidProfile->antiGravityMode); BLACKBOX_PRINT_HEADER_LINE("anti_gravity_threshold", "%d", currentPidProfile->itermThrottleThreshold); BLACKBOX_PRINT_HEADER_LINE("anti_gravity_gain", "%d", currentPidProfile->itermAcceleratorGain); - BLACKBOX_PRINT_HEADER_LINE("setpoint_relaxation_ratio", "%d", currentPidProfile->setpointRelaxRatio); - BLACKBOX_PRINT_HEADER_LINE("dterm_setpoint_weight", "%d", currentPidProfile->dtermSetpointWeight); + + BLACKBOX_PRINT_HEADER_LINE("feedforward_transition", "%d", currentPidProfile->feedForwardTransition); + BLACKBOX_PRINT_HEADER_LINE("feedforward_weight", "%d,%d,%d", currentPidProfile->pid[PID_ROLL].F, + currentPidProfile->pid[PID_PITCH].F, + currentPidProfile->pid[PID_YAW].F); + BLACKBOX_PRINT_HEADER_LINE("acc_limit_yaw", "%d", currentPidProfile->yawRateAccelLimit); BLACKBOX_PRINT_HEADER_LINE("acc_limit", "%d", currentPidProfile->rateAccelLimit); BLACKBOX_PRINT_HEADER_LINE("pidsum_limit", "%d", currentPidProfile->pidSumLimit); @@ -1315,6 +1318,7 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE("gyro_cal_on_first_arm", "%d", armingConfig()->gyro_cal_on_first_arm); BLACKBOX_PRINT_HEADER_LINE("rc_interpolation", "%d", rxConfig()->rcInterpolation); BLACKBOX_PRINT_HEADER_LINE("rc_interpolation_interval", "%d", rxConfig()->rcInterpolationInterval); + BLACKBOX_PRINT_HEADER_LINE("rc_interpolation_channels", "%d", rxConfig()->rcInterpolationChannels); BLACKBOX_PRINT_HEADER_LINE("airmode_activate_throttle", "%d", rxConfig()->airModeActivateThreshold); BLACKBOX_PRINT_HEADER_LINE("serialrx_provider", "%d", rxConfig()->serialrx_provider); BLACKBOX_PRINT_HEADER_LINE("use_unsynced_pwm", "%d", motorConfig()->dev.useUnsyncedPwm); @@ -1333,6 +1337,7 @@ static bool blackboxWriteSysinfo(void) rxConfig()->rc_smoothing_derivative_type); BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_active_cutoffs", "%d, %d", rcSmoothingGetValue(RC_SMOOTHING_VALUE_INPUT_ACTIVE), rcSmoothingGetValue(RC_SMOOTHING_VALUE_DERIVATIVE_ACTIVE)); + BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_rx_average", "%d", rcSmoothingGetValue(RC_SMOOTHING_VALUE_AVERAGE_FRAME)); #endif // USE_RC_SMOOTHING_FILTER @@ -1486,7 +1491,7 @@ STATIC_UNIT_TESTED void blackboxLogIteration(timeUs_t currentTimeUs) writeInterframe(); } #ifdef USE_GPS - if (feature(FEATURE_GPS)) { + if (featureIsEnabled(FEATURE_GPS)) { if (blackboxShouldLogGpsHomeFrame()) { writeGPSHomeFrame(); writeGPSFrame(currentTimeUs); @@ -1552,7 +1557,7 @@ void blackboxUpdate(timeUs_t currentTimeUs) if (!sendFieldDefinition('I', 'P', blackboxMainFields, blackboxMainFields + 1, ARRAYLEN(blackboxMainFields), &blackboxMainFields[0].condition, &blackboxMainFields[1].condition)) { #ifdef USE_GPS - if (feature(FEATURE_GPS)) { + if (featureIsEnabled(FEATURE_GPS)) { blackboxSetState(BLACKBOX_STATE_SEND_GPS_H_HEADER); } else #endif @@ -1669,16 +1674,15 @@ void blackboxUpdate(timeUs_t currentTimeUs) #ifdef USE_FLASHFS if (blackboxState != BLACKBOX_STATE_ERASING && blackboxState != BLACKBOX_STATE_START_ERASE - && blackboxState != BLACKBOX_STATE_ERASED) { + && blackboxState != BLACKBOX_STATE_ERASED) #endif + { blackboxSetState(BLACKBOX_STATE_STOPPED); // ensure we reset the test mode flag if we stop due to full memory card if (startedLoggingInTestMode) { startedLoggingInTestMode = false; } -#ifdef USE_FLASHFS } -#endif } else { // Only log in test mode if there is room! switch (blackboxConfig()->mode) { case BLACKBOX_MODE_MOTOR_TEST: diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index f6ebe68ae8..d0f7264b02 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/src/main/blackbox/blackbox_io.c @@ -41,6 +41,10 @@ #include "msp/msp_serial.h" +#ifdef USE_SDCARD +#include "drivers/sdcard.h" +#endif + #define BLACKBOX_SERIAL_PORT_MODE MODE_TX // How many bytes can we transmit per loop iteration when writing headers? @@ -259,7 +263,7 @@ bool blackboxDeviceOpen(void) break; #ifdef USE_FLASHFS case BLACKBOX_DEVICE_FLASH: - if (flashfsGetSize() == 0 || isBlackboxDeviceFull()) { + if (!flashfsIsSupported() || isBlackboxDeviceFull()) { return false; } @@ -538,6 +542,27 @@ bool isBlackboxDeviceFull(void) } } +bool isBlackboxDeviceWorking(void) +{ + switch (blackboxConfig()->device) { + case BLACKBOX_DEVICE_SERIAL: + return blackboxPort != NULL; + +#ifdef USE_SDCARD + case BLACKBOX_DEVICE_SDCARD: + return sdcard_isInserted() && sdcard_isFunctional() && (afatfs_getFilesystemState() == AFATFS_FILESYSTEM_STATE_READY); +#endif + +#ifdef USE_FLASHFS + case BLACKBOX_DEVICE_FLASH: + return flashfsIsReady(); +#endif + + default: + return false; + } +} + unsigned int blackboxGetLogNumber(void) { #ifdef USE_SDCARD diff --git a/src/main/blackbox/blackbox_io.h b/src/main/blackbox/blackbox_io.h index 3cd481d6a8..72e1a60422 100644 --- a/src/main/blackbox/blackbox_io.h +++ b/src/main/blackbox/blackbox_io.h @@ -56,6 +56,7 @@ bool blackboxDeviceBeginLog(void); bool blackboxDeviceEndLog(bool retainLog); bool isBlackboxDeviceFull(void); +bool isBlackboxDeviceWorking(void); unsigned int blackboxGetLogNumber(void); void blackboxReplenishHeaderBudget(void); diff --git a/src/main/build/build_config.h b/src/main/build/build_config.h index 517d51ed2c..c5bd1f8742 100644 --- a/src/main/build/build_config.h +++ b/src/main/build/build_config.h @@ -20,8 +20,6 @@ #pragma once -#define BUILD_BUG_ON(condition) ((void)sizeof(char[1 - 2*!!(condition)])) - #ifdef UNIT_TEST // make these visible to unit test #define STATIC_UNIT_TESTED @@ -35,8 +33,6 @@ #define UNIT_TESTED #endif -//#define SOFT_I2C // enable to test software i2c - #ifndef __CC_ARM #define REQUIRE_CC_ARM_PRINTF_SUPPORT #define REQUIRE_PRINTF_LONG_SUPPORT diff --git a/src/main/build/debug.c b/src/main/build/debug.c index 10c59fe9df..87237bdd4e 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -50,6 +50,7 @@ const char * const debugModeNames[DEBUG_COUNT] = { "FFT_TIME", "FFT_FREQ", "RX_FRSKY_SPI", + "RX_SFHSS_SPI", "GYRO_RAW", "DUAL_GYRO", "DUAL_GYRO_RAW", @@ -72,5 +73,7 @@ const char * const debugModeNames[DEBUG_COUNT] = { "ITERM_RELAX", "ACRO_TRAINER", "RC_SMOOTHING", - "RX_SIGNAL_LOSS", + "RX_SIGNAL_LOSS", + "RC_SMOOTHING_RATE", + "ANTI_GRAVITY", }; diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 43db3ee9b7..03111c48a4 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -68,6 +68,7 @@ typedef enum { DEBUG_FFT_TIME, DEBUG_FFT_FREQ, DEBUG_RX_FRSKY_SPI, + DEBUG_RX_SFHSS_SPI, DEBUG_GYRO_RAW, DEBUG_DUAL_GYRO, DEBUG_DUAL_GYRO_RAW, @@ -91,6 +92,8 @@ typedef enum { DEBUG_ACRO_TRAINER, DEBUG_RC_SMOOTHING, DEBUG_RX_SIGNAL_LOSS, + DEBUG_RC_SMOOTHING_RATE, + DEBUG_ANTI_GRAVITY, DEBUG_COUNT } debugType_e; diff --git a/src/main/build/version.h b/src/main/build/version.h index e11e4f0491..1c748f90ec 100644 --- a/src/main/build/version.h +++ b/src/main/build/version.h @@ -23,8 +23,8 @@ #include "common/utils.h" #define FC_FIRMWARE_NAME "Betaflight" -#define FC_VERSION_MAJOR 3 // increment when a major release is made (big new feature, etc) -#define FC_VERSION_MINOR 4 // increment when a minor release is made (small new feature, change etc) +#define FC_VERSION_MAJOR 4 // increment when a major release is made (big new feature, etc) +#define FC_VERSION_MINOR 0 // increment when a minor release is made (small new feature, change etc) #define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed #define FC_VERSION_STRING STR(FC_VERSION_MAJOR) "." STR(FC_VERSION_MINOR) "." STR(FC_VERSION_PATCH_LEVEL) diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index 3fcfe78e20..2cb14f2e57 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -160,6 +160,7 @@ static uint8_t leftMenuColumn; static uint8_t rightMenuColumn; static uint8_t maxMenuItems; static uint8_t linesPerMenuItem; +static cms_key_e externKey = CMS_KEY_NONE; bool cmsInMenu = false; @@ -764,18 +765,10 @@ long cmsMenuExit(displayPort_t *pDisplay, const void *ptr) #define IS_LO(X) (rcData[X] < 1250) #define IS_MID(X) (rcData[X] > 1250 && rcData[X] < 1750) -#define KEY_NONE 0 -#define KEY_UP 1 -#define KEY_DOWN 2 -#define KEY_LEFT 3 -#define KEY_RIGHT 4 -#define KEY_ESC 5 -#define KEY_MENU 6 - #define BUTTON_TIME 250 // msec #define BUTTON_PAUSE 500 // msec -STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) +STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, cms_key_e key) { uint16_t res = BUTTON_TIME; OSD_Entry *p; @@ -783,17 +776,17 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) if (!currentCtx.menu) return res; - if (key == KEY_MENU) { + if (key == CMS_KEY_MENU) { cmsMenuOpen(); return BUTTON_PAUSE; } - if (key == KEY_ESC) { + if (key == CMS_KEY_ESC) { cmsMenuBack(pDisplay); return BUTTON_PAUSE; } - if (key == KEY_DOWN) { + if (key == CMS_KEY_DOWN) { if (currentCtx.cursorRow < pageMaxRow) { currentCtx.cursorRow++; } else { @@ -802,7 +795,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) } } - if (key == KEY_UP) { + if (key == CMS_KEY_UP) { currentCtx.cursorRow--; // Skip non-title labels @@ -816,14 +809,14 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) } } - if (key == KEY_DOWN || key == KEY_UP) + if (key == CMS_KEY_DOWN || key == CMS_KEY_UP) return res; p = pageTop + currentCtx.cursorRow; switch (p->type) { case OME_Submenu: - if (key == KEY_RIGHT) { + if (key == CMS_KEY_RIGHT) { cmsMenuChange(pDisplay, p->data); res = BUTTON_PAUSE; } @@ -831,7 +824,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) case OME_Funcall:; long retval; - if (p->func && key == KEY_RIGHT) { + if (p->func && key == CMS_KEY_RIGHT) { retval = p->func(pDisplay, p->data); if (retval == MENU_CHAIN_BACK) cmsMenuBack(pDisplay); @@ -840,7 +833,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) break; case OME_OSD_Exit: - if (p->func && key == KEY_RIGHT) { + if (p->func && key == CMS_KEY_RIGHT) { p->func(pDisplay, p->data); res = BUTTON_PAUSE; } @@ -854,7 +847,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) case OME_Bool: if (p->data) { uint8_t *val = p->data; - if (key == KEY_RIGHT) + if (key == CMS_KEY_RIGHT) *val = 1; else *val = 0; @@ -867,7 +860,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) if (p->data) { uint16_t *val = (uint16_t *)p->data; - if (key == KEY_RIGHT) + if (key == CMS_KEY_RIGHT) *val |= VISIBLE_FLAG; else *val %= ~VISIBLE_FLAG; @@ -880,7 +873,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) case OME_FLOAT: if (p->data) { OSD_UINT8_t *ptr = p->data; - if (key == KEY_RIGHT) { + if (key == CMS_KEY_RIGHT) { if (*ptr->val < ptr->max) *ptr->val += ptr->step; } @@ -899,7 +892,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) if (p->type == OME_TAB) { OSD_TAB_t *ptr = p->data; - if (key == KEY_RIGHT) { + if (key == CMS_KEY_RIGHT) { if (*ptr->val < ptr->max) *ptr->val += 1; } @@ -916,7 +909,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) case OME_INT8: if (p->data) { OSD_INT8_t *ptr = p->data; - if (key == KEY_RIGHT) { + if (key == CMS_KEY_RIGHT) { if (*ptr->val < ptr->max) *ptr->val += ptr->step; } @@ -934,7 +927,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) case OME_UINT16: if (p->data) { OSD_UINT16_t *ptr = p->data; - if (key == KEY_RIGHT) { + if (key == CMS_KEY_RIGHT) { if (*ptr->val < ptr->max) *ptr->val += ptr->step; } @@ -952,7 +945,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) case OME_INT16: if (p->data) { OSD_INT16_t *ptr = p->data; - if (key == KEY_RIGHT) { + if (key == CMS_KEY_RIGHT) { if (*ptr->val < ptr->max) *ptr->val += ptr->step; } @@ -981,7 +974,13 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) return res; } -uint16_t cmsHandleKeyWithRepeat(displayPort_t *pDisplay, uint8_t key, int repeatCount) +void cmsSetExternKey(cms_key_e extKey) +{ + if (externKey == CMS_KEY_NONE) + externKey = extKey; +} + +uint16_t cmsHandleKeyWithRepeat(displayPort_t *pDisplay, cms_key_e key, int repeatCount) { uint16_t ret = 0; @@ -1026,72 +1025,77 @@ void cmsUpdate(uint32_t currentTimeUs) // Scan 'key' first // - uint8_t key = KEY_NONE; + cms_key_e key = CMS_KEY_NONE; - if (IS_MID(THROTTLE) && IS_LO(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) { - key = KEY_MENU; - } - else if (IS_HI(PITCH)) { - key = KEY_UP; - } - else if (IS_LO(PITCH)) { - key = KEY_DOWN; - } - else if (IS_LO(ROLL)) { - key = KEY_LEFT; - } - else if (IS_HI(ROLL)) { - key = KEY_RIGHT; - } - else if (IS_HI(YAW) || IS_LO(YAW)) - { - key = KEY_ESC; - } - - if (key == KEY_NONE) { - // No 'key' pressed, reset repeat control - holdCount = 1; - repeatCount = 1; - repeatBase = 0; + if (externKey != CMS_KEY_NONE) { + rcDelayMs = cmsHandleKey(pCurrentDisplay, externKey); + externKey = CMS_KEY_NONE; } else { - // The 'key' is being pressed; keep counting - ++holdCount; - } + if (IS_MID(THROTTLE) && IS_LO(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) { + key = CMS_KEY_MENU; + } + else if (IS_HI(PITCH)) { + key = CMS_KEY_UP; + } + else if (IS_LO(PITCH)) { + key = CMS_KEY_DOWN; + } + else if (IS_LO(ROLL)) { + key = CMS_KEY_LEFT; + } + else if (IS_HI(ROLL)) { + key = CMS_KEY_RIGHT; + } + else if (IS_HI(YAW) || IS_LO(YAW)) + { + key = CMS_KEY_ESC; + } - if (rcDelayMs > 0) { - rcDelayMs -= (currentTimeMs - lastCalledMs); - } else if (key) { - rcDelayMs = cmsHandleKeyWithRepeat(pCurrentDisplay, key, repeatCount); + if (key == CMS_KEY_NONE) { + // No 'key' pressed, reset repeat control + holdCount = 1; + repeatCount = 1; + repeatBase = 0; + } else { + // The 'key' is being pressed; keep counting + ++holdCount; + } - // Key repeat effect is implemented in two phases. - // First phldase is to decrease rcDelayMs reciprocal to hold time. - // When rcDelayMs reached a certain limit (scheduling interval), - // repeat rate will not raise anymore, so we call key handler - // multiple times (repeatCount). - // - // XXX Caveat: Most constants are adjusted pragmatically. - // XXX Rewrite this someday, so it uses actual hold time instead - // of holdCount, which depends on the scheduling interval. + if (rcDelayMs > 0) { + rcDelayMs -= (currentTimeMs - lastCalledMs); + } else if (key) { + rcDelayMs = cmsHandleKeyWithRepeat(pCurrentDisplay, key, repeatCount); - if (((key == KEY_LEFT) || (key == KEY_RIGHT)) && (holdCount > 20)) { + // Key repeat effect is implemented in two phases. + // First phldase is to decrease rcDelayMs reciprocal to hold time. + // When rcDelayMs reached a certain limit (scheduling interval), + // repeat rate will not raise anymore, so we call key handler + // multiple times (repeatCount). + // + // XXX Caveat: Most constants are adjusted pragmatically. + // XXX Rewrite this someday, so it uses actual hold time instead + // of holdCount, which depends on the scheduling interval. - // Decrease rcDelayMs reciprocally + if (((key == CMS_KEY_LEFT) || (key == CMS_KEY_RIGHT)) && (holdCount > 20)) { - rcDelayMs /= (holdCount - 20); + // Decrease rcDelayMs reciprocally - // When we reach the scheduling limit, + rcDelayMs /= (holdCount - 20); - if (rcDelayMs <= 50) { + // When we reach the scheduling limit, - // start calling handler multiple times. + if (rcDelayMs <= 50) { - if (repeatBase == 0) - repeatBase = holdCount; + // start calling handler multiple times. - repeatCount = repeatCount + (holdCount - repeatBase) / 5; + if (repeatBase == 0) + repeatBase = holdCount; - if (repeatCount > 5) { - repeatCount= 5; + repeatCount = repeatCount + (holdCount - repeatBase) / 5; + + if (repeatCount > 5) { + repeatCount= 5; + } } } } diff --git a/src/main/cms/cms.h b/src/main/cms/cms.h index 0e46fe34f5..18d73b4cd6 100644 --- a/src/main/cms/cms.h +++ b/src/main/cms/cms.h @@ -24,11 +24,22 @@ #include "common/time.h" +typedef enum { + CMS_KEY_NONE, + CMS_KEY_UP, + CMS_KEY_DOWN, + CMS_KEY_LEFT, + CMS_KEY_RIGHT, + CMS_KEY_ESC, + CMS_KEY_MENU +} cms_key_e; + extern bool cmsInMenu; // Device management bool cmsDisplayPortRegister(displayPort_t *pDisplay); -displayPort_t *pCurrentDisplay; + +extern displayPort_t *pCurrentDisplay; // For main.c and scheduler void cmsInit(void); @@ -39,6 +50,7 @@ void cmsMenuOpen(void); long cmsMenuChange(displayPort_t *pPort, const void *ptr); long cmsMenuExit(displayPort_t *pPort, const void *ptr); void cmsUpdate(uint32_t currentTimeUs); +void cmsSetExternKey(cms_key_e extKey); #define CMS_STARTUP_HELP_TEXT1 "MENU:THR MID" #define CMS_STARTUP_HELP_TEXT2 "+ YAW LEFT" diff --git a/src/main/cms/cms_menu_blackbox.c b/src/main/cms/cms_menu_blackbox.c index 0e2e78c844..f0e99df030 100644 --- a/src/main/cms/cms_menu_blackbox.c +++ b/src/main/cms/cms_menu_blackbox.c @@ -122,7 +122,7 @@ static void cmsx_Blackbox_GetDeviceStatus(void) case BLACKBOX_DEVICE_FLASH: unit = "KB"; - storageDeviceIsWorking = flashfsIsReady(); + storageDeviceIsWorking = flashfsIsSupported(); if (storageDeviceIsWorking) { tfp_sprintf(cmsx_BlackboxStatus, "READY"); @@ -150,6 +150,10 @@ static long cmsx_EraseFlash(displayPort_t *pDisplay, const void *ptr) { UNUSED(ptr); + if (!flashfsIsSupported()) { + return 0; + } + displayClearScreen(pDisplay); displayWrite(pDisplay, 5, 3, "ERASING FLASH..."); displayResync(pDisplay); // Was max7456RefreshAll(); Why at this timing? diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 801a9d5d78..b5c6ce58be 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -58,6 +58,7 @@ static uint8_t tmpPidProfileIndex; static uint8_t pidProfileIndex; static char pidProfileIndexString[] = " p"; static uint8_t tempPid[3][3]; +static uint16_t tempPidF[3]; static uint8_t tmpRateProfileIndex; static uint8_t rateProfileIndex; @@ -115,6 +116,7 @@ static long cmsx_PidRead(void) tempPid[i][0] = pidProfile->pid[i].P; tempPid[i][1] = pidProfile->pid[i].I; tempPid[i][2] = pidProfile->pid[i].D; + tempPidF[i] = pidProfile->pid[i].F; } return 0; @@ -137,6 +139,7 @@ static long cmsx_PidWriteback(const OSD_Entry *self) pidProfile->pid[i].P = tempPid[i][0]; pidProfile->pid[i].I = tempPid[i][1]; pidProfile->pid[i].D = tempPid[i][2]; + pidProfile->pid[i].F = tempPidF[i]; } pidInitConfig(currentPidProfile); @@ -150,14 +153,17 @@ static OSD_Entry cmsx_menuPidEntries[] = { "ROLL P", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_ROLL][0], 0, 200, 1 }, 0 }, { "ROLL I", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_ROLL][1], 0, 200, 1 }, 0 }, { "ROLL D", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_ROLL][2], 0, 200, 1 }, 0 }, + { "ROLL F", OME_UINT16, NULL, &(OSD_UINT16_t){ &tempPidF[PID_ROLL], 0, 2000, 1 }, 0 }, { "PITCH P", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_PITCH][0], 0, 200, 1 }, 0 }, { "PITCH I", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_PITCH][1], 0, 200, 1 }, 0 }, { "PITCH D", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_PITCH][2], 0, 200, 1 }, 0 }, + { "PITCH F", OME_UINT16, NULL, &(OSD_UINT16_t){ &tempPidF[PID_PITCH], 0, 2000, 1 }, 0 }, { "YAW P", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_YAW][0], 0, 200, 1 }, 0 }, { "YAW I", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_YAW][1], 0, 200, 1 }, 0 }, { "YAW D", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_YAW][2], 0, 200, 1 }, 0 }, + { "YAW F", OME_UINT16, NULL, &(OSD_UINT16_t){ &tempPidF[PID_YAW], 0, 2000, 1 }, 0 }, { "BACK", OME_Back, NULL, NULL, 0 }, { NULL, OME_END, NULL, NULL, 0 } @@ -237,11 +243,11 @@ static CMS_Menu cmsx_menuRateProfile = { .entries = cmsx_menuRateProfileEntries }; -static uint16_t cmsx_dtermSetpointWeight; -static uint8_t cmsx_setpointRelaxRatio; +static uint8_t cmsx_feedForwardTransition; static uint8_t cmsx_angleStrength; static uint8_t cmsx_horizonStrength; static uint8_t cmsx_horizonTransition; +static uint8_t cmsx_throttleBoost; static uint16_t cmsx_itermAcceleratorGain; static uint16_t cmsx_itermThrottleThreshold; @@ -250,8 +256,8 @@ static long cmsx_profileOtherOnEnter(void) pidProfileIndexString[1] = '0' + tmpPidProfileIndex; const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex); - cmsx_dtermSetpointWeight = pidProfile->dtermSetpointWeight; - cmsx_setpointRelaxRatio = pidProfile->setpointRelaxRatio; + + cmsx_feedForwardTransition = pidProfile->feedForwardTransition; cmsx_angleStrength = pidProfile->pid[PID_LEVEL].P; cmsx_horizonStrength = pidProfile->pid[PID_LEVEL].I; @@ -260,6 +266,8 @@ static long cmsx_profileOtherOnEnter(void) cmsx_itermAcceleratorGain = pidProfile->itermAcceleratorGain; cmsx_itermThrottleThreshold = pidProfile->itermThrottleThreshold; + cmsx_throttleBoost = pidProfile->throttle_boost; + return 0; } @@ -268,8 +276,7 @@ static long cmsx_profileOtherOnExit(const OSD_Entry *self) UNUSED(self); pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex); - pidProfile->dtermSetpointWeight = cmsx_dtermSetpointWeight; - pidProfile->setpointRelaxRatio = cmsx_setpointRelaxRatio; + pidProfile->feedForwardTransition = cmsx_feedForwardTransition; pidInitConfig(currentPidProfile); pidProfile->pid[PID_LEVEL].P = cmsx_angleStrength; @@ -279,19 +286,23 @@ static long cmsx_profileOtherOnExit(const OSD_Entry *self) pidProfile->itermAcceleratorGain = cmsx_itermAcceleratorGain; pidProfile->itermThrottleThreshold = cmsx_itermThrottleThreshold; + pidProfile->throttle_boost = cmsx_throttleBoost; + return 0; } static OSD_Entry cmsx_menuProfileOtherEntries[] = { { "-- OTHER PP --", OME_Label, NULL, pidProfileIndexString, 0 }, - { "D SETPT WT", OME_UINT16, NULL, &(OSD_UINT16_t) { &cmsx_dtermSetpointWeight, 0, 2000, 1 }, 0 }, - { "SETPT TRS", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_setpointRelaxRatio, 0, 100, 1, 10 }, 0 }, + { "FF TRANS", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_feedForwardTransition, 0, 100, 1, 10 }, 0 }, { "ANGLE STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_angleStrength, 0, 200, 1 } , 0 }, { "HORZN STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonStrength, 0, 200, 1 } , 0 }, { "HORZN TRS", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonTransition, 0, 200, 1 } , 0 }, { "AG GAIN", OME_UINT16, NULL, &(OSD_UINT16_t) { &cmsx_itermAcceleratorGain, 1000, 30000, 10 } , 0 }, { "AG THR", OME_UINT16, NULL, &(OSD_UINT16_t) { &cmsx_itermThrottleThreshold, 20, 1000, 1 } , 0 }, +#ifdef USE_THROTTLE_BOOST + { "THR BOOST", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_throttleBoost, 0, 100, 1 } , 0 }, +#endif { "BACK", OME_Back, NULL, NULL, 0 }, { NULL, OME_END, NULL, NULL, 0 } @@ -346,7 +357,9 @@ static OSD_Entry cmsx_menuFilterGlobalEntries[] = { "-- FILTER GLB --", OME_Label, NULL, NULL, 0 }, { "GYRO LPF", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lowpass_hz, 0, 16000, 1 }, 0 }, +#ifdef USE_GYRO_LPF2 { "GYRO LPF2", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lowpass2_hz, 0, 16000, 1 }, 0 }, +#endif { "GYRO NF1", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_hz_1, 0, 500, 1 }, 0 }, { "GYRO NF1C", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_cutoff_1, 0, 500, 1 }, 0 }, { "GYRO NF2", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_hz_2, 0, 500, 1 }, 0 }, @@ -367,6 +380,7 @@ static CMS_Menu cmsx_menuFilterGlobal = { }; static uint16_t cmsx_dterm_lowpass_hz; +static uint16_t cmsx_dterm_lowpass2_hz; static uint16_t cmsx_dterm_notch_hz; static uint16_t cmsx_dterm_notch_cutoff; static uint16_t cmsx_yaw_lowpass_hz; @@ -374,10 +388,12 @@ static uint16_t cmsx_yaw_lowpass_hz; static long cmsx_FilterPerProfileRead(void) { const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex); - cmsx_dterm_lowpass_hz = pidProfile->dterm_lowpass_hz; - cmsx_dterm_notch_hz = pidProfile->dterm_notch_hz; + + cmsx_dterm_lowpass_hz = pidProfile->dterm_lowpass_hz; + cmsx_dterm_lowpass2_hz = pidProfile->dterm_lowpass2_hz; + cmsx_dterm_notch_hz = pidProfile->dterm_notch_hz; cmsx_dterm_notch_cutoff = pidProfile->dterm_notch_cutoff; - cmsx_yaw_lowpass_hz = pidProfile->yaw_lowpass_hz; + cmsx_yaw_lowpass_hz = pidProfile->yaw_lowpass_hz; return 0; } @@ -387,10 +403,12 @@ static long cmsx_FilterPerProfileWriteback(const OSD_Entry *self) UNUSED(self); pidProfile_t *pidProfile = currentPidProfile; - pidProfile->dterm_lowpass_hz = cmsx_dterm_lowpass_hz; - pidProfile->dterm_notch_hz = cmsx_dterm_notch_hz; + + pidProfile->dterm_lowpass_hz = cmsx_dterm_lowpass_hz; + pidProfile->dterm_lowpass2_hz = cmsx_dterm_lowpass2_hz; + pidProfile->dterm_notch_hz = cmsx_dterm_notch_hz; pidProfile->dterm_notch_cutoff = cmsx_dterm_notch_cutoff; - pidProfile->yaw_lowpass_hz = cmsx_yaw_lowpass_hz; + pidProfile->yaw_lowpass_hz = cmsx_yaw_lowpass_hz; return 0; } @@ -400,6 +418,7 @@ static OSD_Entry cmsx_menuFilterPerProfileEntries[] = { "-- FILTER PP --", OME_Label, NULL, NULL, 0 }, { "DTERM LPF", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lowpass_hz, 0, 500, 1 }, 0 }, + { "DTERM LPF2", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lowpass2_hz, 0, 500, 1 }, 0 }, { "DTERM NF", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_notch_hz, 0, 500, 1 }, 0 }, { "DTERM NFCO", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_notch_cutoff, 0, 500, 1 }, 0 }, { "YAW LPF", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_yaw_lowpass_hz, 0, 500, 1 }, 0 }, @@ -492,7 +511,7 @@ CMS_Menu cmsx_menuCopyProfile = { static OSD_Entry cmsx_menuImuEntries[] = { - { "-- IMU --", OME_Label, NULL, NULL, 0}, + { "-- PROFILE --", OME_Label, NULL, NULL, 0}, {"PID PROF", OME_UINT8, cmsx_profileIndexOnChange, &(OSD_UINT8_t){ &tmpPidProfileIndex, 1, MAX_PROFILE_COUNT, 1}, 0}, {"PID", OME_Submenu, cmsMenuChange, &cmsx_menuPid, 0}, diff --git a/src/main/cms/cms_menu_ledstrip.c b/src/main/cms/cms_menu_ledstrip.c index 335e3972a9..b6e5423db0 100644 --- a/src/main/cms/cms_menu_ledstrip.c +++ b/src/main/cms/cms_menu_ledstrip.c @@ -48,7 +48,7 @@ static uint8_t cmsx_FeatureLedstrip; static long cmsx_Ledstrip_FeatureRead(void) { if (!featureRead) { - cmsx_FeatureLedstrip = feature(FEATURE_LED_STRIP) ? 1 : 0; + cmsx_FeatureLedstrip = featureIsEnabled(FEATURE_LED_STRIP) ? 1 : 0; featureRead = true; } @@ -60,9 +60,9 @@ static long cmsx_Ledstrip_FeatureWriteback(const OSD_Entry *self) UNUSED(self); if (featureRead) { if (cmsx_FeatureLedstrip) - featureSet(FEATURE_LED_STRIP); + featureEnable(FEATURE_LED_STRIP); else - featureClear(FEATURE_LED_STRIP); + featureDisable(FEATURE_LED_STRIP); } return 0; diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c index abbf80b0a0..0d92caebe9 100644 --- a/src/main/cms/cms_menu_osd.c +++ b/src/main/cms/cms_menu_osd.c @@ -101,7 +101,11 @@ OSD_Entry menuOsdActiveElemsEntries[] = {"PIT ANG", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_PITCH_ANGLE], 0}, {"ROL ANG", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_ROLL_ANGLE], 0}, {"HEADING", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_NUMERICAL_HEADING], 0}, +#ifdef USE_VARIO {"VARIO", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_NUMERICAL_VARIO], 0}, +#endif + {"G-FORCE", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_G_FORCE], 0}, + {"FLIP ARROW", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_FLIP_ARROW], 0}, {"BACK", OME_Back, NULL, NULL, 0}, {NULL, OME_END, NULL, NULL, 0} }; diff --git a/src/main/common/bitarray.c b/src/main/common/bitarray.c index fb3e82c091..1794e60e51 100644 --- a/src/main/common/bitarray.c +++ b/src/main/common/bitarray.c @@ -47,3 +47,12 @@ void bitArrayXor(void *dest, size_t size, void *op1, void *op2) ((uint8_t*)dest)[i] = ((uint8_t*)op1)[i] ^ ((uint8_t*)op2)[i]; } } + +void bitArrayCopy(void *array, unsigned from, unsigned to) +{ + if (bitArrayGet(array, from)) { + bitArraySet(array, to); + } else { + bitArrayClr(array, to); + } +} diff --git a/src/main/common/bitarray.h b/src/main/common/bitarray.h index 1492985893..13fafe5550 100644 --- a/src/main/common/bitarray.h +++ b/src/main/common/bitarray.h @@ -24,3 +24,4 @@ bool bitArrayGet(const void *array, unsigned bit); void bitArraySet(void *array, unsigned bit); void bitArrayClr(void *array, unsigned bit); void bitArrayXor(void *dest, size_t size, void *op1, void *op2); +void bitArrayCopy(void *array, unsigned from, unsigned to); diff --git a/src/main/common/filter.c b/src/main/common/filter.c index e61f1fcd2c..9b729cd5c3 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -56,6 +56,11 @@ void pt1FilterInit(pt1Filter_t *filter, float k) filter->k = k; } +void pt1FilterUpdateCutoff(pt1Filter_t *filter, float k) +{ + filter->k = k; +} + FAST_CODE float pt1FilterApply(pt1Filter_t *filter, float input) { filter->state = filter->state + filter->k * (input - filter->state); @@ -167,6 +172,11 @@ FAST_CODE void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint filter->y2 = y2; } +FAST_CODE void biquadFilterUpdateLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate) +{ + biquadFilterUpdate(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF); +} + /* Computes a biquadFilter_t filter on a sample (slightly less precise than df2 but works in dynamic mode) */ FAST_CODE float biquadFilterApplyDF1(biquadFilter_t *filter, float input) { diff --git a/src/main/common/filter.h b/src/main/common/filter.h index c6befb1537..f4de43d17d 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -67,6 +67,7 @@ float nullFilterApply(filter_t *filter, float input); void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate); void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType); void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType); +void biquadFilterUpdateLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate); float biquadFilterApplyDF1(biquadFilter_t *filter, float input); float biquadFilterApply(biquadFilter_t *filter, float input); @@ -77,6 +78,7 @@ float laggedMovingAverageUpdate(laggedMovingAverage_t *filter, float input); float pt1FilterGain(uint16_t f_cut, float dT); void pt1FilterInit(pt1Filter_t *filter, float k); +void pt1FilterUpdateCutoff(pt1Filter_t *filter, float k); float pt1FilterApply(pt1Filter_t *filter, float input); void slewFilterInit(slewFilter_t *filter, float slewLimit, float threshold); diff --git a/src/main/common/maths.c b/src/main/common/maths.c index 565684be82..00a0c51ecd 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -176,6 +176,12 @@ int scaleRange(int x, int srcFrom, int srcTo, int destFrom, int destTo) { return (a / b) + destFrom; } +float scaleRangef(float x, float srcFrom, float srcTo, float destFrom, float destTo) { + float a = (destTo - destFrom) * (x - srcFrom); + float b = srcTo - srcFrom; + return (a / b) + destFrom; +} + // Normalize a vector void normalizeV(struct fp_vector *src, struct fp_vector *dest) { diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 19842ebb40..1a506789b8 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -20,6 +20,8 @@ #pragma once +#include + #ifndef sq #define sq(x) ((x)*(x)) #endif @@ -33,13 +35,13 @@ #define M_PIf 3.14159265358979323846f #define RAD (M_PIf / 180.0f) -#define DEGREES_TO_DECIDEGREES(angle) (angle * 10) -#define DECIDEGREES_TO_DEGREES(angle) (angle / 10) -#define DECIDEGREES_TO_RADIANS(angle) ((angle / 10.0f) * 0.0174532925f) +#define DEGREES_TO_DECIDEGREES(angle) ((angle) * 10) +#define DECIDEGREES_TO_DEGREES(angle) ((angle) / 10) +#define DECIDEGREES_TO_RADIANS(angle) ((angle) / 10.0f * 0.0174532925f) #define DEGREES_TO_RADIANS(angle) ((angle) * 0.0174532925f) -#define CM_S_TO_KM_H(centimetersPerSecond) (centimetersPerSecond * 36 / 1000) -#define CM_S_TO_MPH(centimetersPerSecond) (((centimetersPerSecond * 10000) / 5080) / 88) +#define CM_S_TO_KM_H(centimetersPerSecond) ((centimetersPerSecond) * 36 / 1000) +#define CM_S_TO_MPH(centimetersPerSecond) ((centimetersPerSecond) * 10000 / 5080 / 88) #define MIN(a,b) \ __extension__ ({ __typeof__ (a) _a = (a); \ @@ -55,6 +57,8 @@ #define Q12 (1 << 12) +#define HZ_TO_INTERVAL_US(x) (1000000 / (x)) + typedef int32_t fix12_t; typedef struct stdev_s @@ -100,6 +104,7 @@ float devStandardDeviation(stdev_t *dev); float degreesToRadians(int16_t degrees); int scaleRange(int x, int srcFrom, int srcTo, int destFrom, int destTo); +float scaleRangef(float x, float srcFrom, float srcTo, float destFrom, float destTo); void normalizeV(struct fp_vector *src, struct fp_vector *dest); diff --git a/src/main/common/time.c b/src/main/common/time.c index 5940c4225f..3541f237e8 100644 --- a/src/main/common/time.c +++ b/src/main/common/time.c @@ -36,7 +36,11 @@ #ifdef USE_RTC_TIME -#define UNIX_REFERENCE_YEAR 1970 +// For the "modulo 4" arithmetic to work, we need a leap base year +#define REFERENCE_YEAR 2000 +// Offset (seconds) from the UNIX epoch (1970-01-01) to 2000-01-01 +#define EPOCH_2000_OFFSET 946684800 + #define MILLIS_PER_SECOND 1000 // rtcTime_t when the system was started. @@ -64,14 +68,14 @@ static rtcTime_t dateTimeToRtcTime(dateTime_t *dt) unsigned int hour = dt->hours; // 0-23 unsigned int day = dt->day - 1; // 0-30 unsigned int month = dt->month - 1; // 0-11 - unsigned int year = dt->year - UNIX_REFERENCE_YEAR; // 0-99 - int32_t unixTime = (((year / 4 * (365 * 4 + 1) + days[year % 4][month] + day) * 24 + hour) * 60 + minute) * 60 + second; + unsigned int year = dt->year - REFERENCE_YEAR; // 0-99 + int32_t unixTime = (((year / 4 * (365 * 4 + 1) + days[year % 4][month] + day) * 24 + hour) * 60 + minute) * 60 + second + EPOCH_2000_OFFSET; return rtcTimeMake(unixTime, dt->millis); } static void rtcTimeToDateTime(dateTime_t *dt, rtcTime_t t) { - int32_t unixTime = t / MILLIS_PER_SECOND; + int32_t unixTime = t / MILLIS_PER_SECOND - EPOCH_2000_OFFSET; dt->seconds = unixTime % 60; unixTime /= 60; dt->minutes = unixTime % 60; @@ -96,7 +100,7 @@ static void rtcTimeToDateTime(dateTime_t *dt, rtcTime_t t) } } - dt->year = years + year + UNIX_REFERENCE_YEAR; + dt->year = years + year + REFERENCE_YEAR; dt->month = month + 1; dt->day = unixTime - days[year][month] + 1; dt->millis = t % MILLIS_PER_SECOND; @@ -115,7 +119,7 @@ static void rtcGetDefaultDateTime(dateTime_t *dateTime) static bool rtcIsDateTimeValid(dateTime_t *dateTime) { - return (dateTime->year >= UNIX_REFERENCE_YEAR) && + return (dateTime->year >= REFERENCE_YEAR) && (dateTime->month >= 1 && dateTime->month <= 12) && (dateTime->day >= 1 && dateTime->day <= 31) && (dateTime->hours <= 23) && diff --git a/src/main/common/utils.h b/src/main/common/utils.h index 9bbf67d42a..076b4680f2 100644 --- a/src/main/common/utils.h +++ b/src/main/common/utils.h @@ -57,9 +57,8 @@ #if !defined(UNUSED) #define UNUSED(x) (void)(x) #endif -#define BUILD_BUG_ON(condition) ((void)sizeof(char[1 - 2*!!(condition)])) -#define STATIC_ASSERT(condition, name) \ - typedef char assert_failed_ ## name [(condition) ? 1 : -1 ] __attribute__((unused)) + +#define STATIC_ASSERT(condition, name) _Static_assert((condition), #name) #define BIT(x) (1 << (x)) diff --git a/src/main/config/config_eeprom.c b/src/main/config/config_eeprom.c index 2b8c571737..7c51f2fbcf 100644 --- a/src/main/config/config_eeprom.c +++ b/src/main/config/config_eeprom.c @@ -86,12 +86,12 @@ typedef struct { void initEEPROM(void) { // Verify that this architecture packs as expected. - BUILD_BUG_ON(offsetof(packingTest_t, byte) != 0); - BUILD_BUG_ON(offsetof(packingTest_t, word) != 1); - BUILD_BUG_ON(sizeof(packingTest_t) != 5); + STATIC_ASSERT(offsetof(packingTest_t, byte) == 0, byte_packing_test_failed); + STATIC_ASSERT(offsetof(packingTest_t, word) == 1, word_packing_test_failed); + STATIC_ASSERT(sizeof(packingTest_t) == 5, overall_packing_test_failed); - BUILD_BUG_ON(sizeof(configFooter_t) != 2); - BUILD_BUG_ON(sizeof(configRecord_t) != 6); + STATIC_ASSERT(sizeof(configFooter_t) == 2, footer_size_failed); + STATIC_ASSERT(sizeof(configRecord_t) == 6, record_size_failed); } bool isEEPROMVersionValid(void) diff --git a/src/main/config/config_eeprom.h b/src/main/config/config_eeprom.h index 35979b1b14..0c3364c2dc 100644 --- a/src/main/config/config_eeprom.h +++ b/src/main/config/config_eeprom.h @@ -23,7 +23,7 @@ #include #include -#define EEPROM_CONF_VERSION 169 +#define EEPROM_CONF_VERSION 171 bool isEEPROMVersionValid(void); bool isEEPROMStructureValid(void); diff --git a/src/main/config/config_streamer.c b/src/main/config/config_streamer.c index 3ed646504f..a47f1d4f9b 100644 --- a/src/main/config/config_streamer.c +++ b/src/main/config/config_streamer.c @@ -31,6 +31,7 @@ extern uint8_t __config_start; // configured via linker script when building b extern uint8_t __config_end; #endif +// @todo this is not strictly correct for F4/F7, where sector sizes are variable #if !defined(FLASH_PAGE_SIZE) // F1 # if defined(STM32F10X_MD) @@ -56,6 +57,8 @@ extern uint8_t __config_end; # define FLASH_PAGE_SIZE ((uint32_t)0x8000) // 32K sectors # elif defined(STM32F746xx) # define FLASH_PAGE_SIZE ((uint32_t)0x8000) +# elif defined(STM32F765xx) +# define FLASH_PAGE_SIZE ((uint32_t)0x8000) # elif defined(UNIT_TEST) # define FLASH_PAGE_SIZE (0x400) // SIMULATOR @@ -101,7 +104,7 @@ void config_streamer_start(config_streamer_t *c, uintptr_t base, int size) c->err = 0; } -#if defined(STM32F745xx) || defined(STM32F746xx) +#if defined(STM32F745xx) || defined(STM32F746xx) || defined(STM32F765xx) /* Sector 0 0x08000000 - 0x08007FFF 32 Kbytes Sector 1 0x08008000 - 0x0800FFFF 32 Kbytes @@ -111,6 +114,12 @@ Sector 4 0x08020000 - 0x0803FFFF 128 Kbytes Sector 5 0x08040000 - 0x0807FFFF 256 Kbytes Sector 6 0x08080000 - 0x080BFFFF 256 Kbytes Sector 7 0x080C0000 - 0x080FFFFF 256 Kbytes + +F7X5XI device with 2M flash +Sector 8 0x08100000 - 0x0813FFFF 256 Kbytes +Sector 9 0x08140000 - 0x0817FFFF 256 Kbytes +Sector 10 0x08180000 - 0x081BFFFF 256 Kbytes +Sector 11 0x081C0000 - 0x081FFFFF 256 Kbytes */ static uint32_t getFLASHSectorForEEPROM(void) @@ -131,6 +140,16 @@ static uint32_t getFLASHSectorForEEPROM(void) return FLASH_SECTOR_6; if ((uint32_t)&__config_start <= 0x080FFFFF) return FLASH_SECTOR_7; +#if defined(STM32F765xx) + if ((uint32_t)&__config_start <= 0x0813FFFF) + return FLASH_SECTOR_8; + if ((uint32_t)&__config_start <= 0x0817FFFF) + return FLASH_SECTOR_9; + if ((uint32_t)&__config_start <= 0x081BFFFF) + return FLASH_SECTOR_10; + if ((uint32_t)&__config_start <= 0x081FFFFF) + return FLASH_SECTOR_11; +#endif // Not good while (1) { diff --git a/src/main/config/config_unittest.h b/src/main/config/config_unittest.h index 65c5cb13e7..27a92ec681 100644 --- a/src/main/config/config_unittest.h +++ b/src/main/config/config_unittest.h @@ -50,9 +50,9 @@ bool unittest_outsideRealtimeGuardInterval; float unittest_pidLuxFloat_lastErrorForDelta[3]; float unittest_pidLuxFloat_delta1[3]; float unittest_pidLuxFloat_delta2[3]; -float unittest_pidLuxFloat_PTerm[3]; -float unittest_pidLuxFloat_ITerm[3]; -float unittest_pidLuxFloat_DTerm[3]; +float unittest_pidLuxFloat_pterm[3]; +float unittest_pidLuxFloat_iterm[3]; +float unittest_pidLuxFloat_dterm[3]; #define SET_PID_LUX_FLOAT_LOCALS(axis) \ { \ @@ -66,15 +66,15 @@ float unittest_pidLuxFloat_DTerm[3]; unittest_pidLuxFloat_lastErrorForDelta[axis] = lastErrorForDelta[axis]; \ unittest_pidLuxFloat_delta1[axis] = delta1[axis]; \ unittest_pidLuxFloat_delta2[axis] = delta2[axis]; \ - unittest_pidLuxFloat_PTerm[axis] = PTerm; \ - unittest_pidLuxFloat_ITerm[axis] = ITerm; \ - unittest_pidLuxFloat_DTerm[axis] = DTerm; \ + unittest_pidLuxFloat_pterm[axis] = pterm; \ + unittest_pidLuxFloat_iterm[axis] = iterm; \ + unittest_pidLuxFloat_dterm[axis] = dterm; \ } int32_t unittest_pidMultiWiiRewrite_lastErrorForDelta[3]; -int32_t unittest_pidMultiWiiRewrite_PTerm[3]; -int32_t unittest_pidMultiWiiRewrite_ITerm[3]; -int32_t unittest_pidMultiWiiRewrite_DTerm[3]; +int32_t unittest_pidMultiWiiRewrite_pterm[3]; +int32_t unittest_pidMultiWiiRewrite_iterm[3]; +int32_t unittest_pidMultiWiiRewrite_dterm[3]; #define SET_PID_MULTI_WII_REWRITE_LOCALS(axis) \ { \ @@ -84,9 +84,9 @@ int32_t unittest_pidMultiWiiRewrite_DTerm[3]; #define GET_PID_MULTI_WII_REWRITE_LOCALS(axis) \ { \ unittest_pidMultiWiiRewrite_lastErrorForDelta[axis] = lastErrorForDelta[axis]; \ - unittest_pidMultiWiiRewrite_PTerm[axis] = PTerm; \ - unittest_pidMultiWiiRewrite_ITerm[axis] = ITerm; \ - unittest_pidMultiWiiRewrite_DTerm[axis] = DTerm; \ + unittest_pidMultiWiiRewrite_pterm[axis] = pterm; \ + unittest_pidMultiWiiRewrite_iterm[axis] = iterm; \ + unittest_pidMultiWiiRewrite_dterm[axis] = dterm; \ } #else diff --git a/src/main/config/feature.c b/src/main/config/feature.c index f054c12bd8..9135d634ba 100644 --- a/src/main/config/feature.c +++ b/src/main/config/feature.c @@ -29,57 +29,40 @@ #include "pg/pg_ids.h" -static uint32_t activeFeaturesLatch = 0; - PG_REGISTER_WITH_RESET_TEMPLATE(featureConfig_t, featureConfig, PG_FEATURE_CONFIG, 0); PG_RESET_TEMPLATE(featureConfig_t, featureConfig, .enabledFeatures = DEFAULT_FEATURES | DEFAULT_RX_FEATURE | FEATURE_DYNAMIC_FILTER | FEATURE_ANTI_GRAVITY, ); -void intFeatureSet(uint32_t mask, uint32_t *features) +void featureSet(const uint32_t mask, uint32_t *features) { *features |= mask; } -void intFeatureClear(uint32_t mask, uint32_t *features) +void featureClear(const uint32_t mask, uint32_t *features) { *features &= ~(mask); } -void intFeatureClearAll(uint32_t *features) -{ - *features = 0; -} - -void latchActiveFeatures(void) -{ - activeFeaturesLatch = featureConfig()->enabledFeatures; -} - -bool featureConfigured(uint32_t mask) +bool featureIsEnabled(const uint32_t mask) { return featureConfig()->enabledFeatures & mask; } -bool feature(uint32_t mask) +void featureEnable(const uint32_t mask) { - return activeFeaturesLatch & mask; + featureSet(mask, &featureConfigMutable()->enabledFeatures); } -void featureSet(uint32_t mask) +void featureDisable(const uint32_t mask) { - intFeatureSet(mask, &featureConfigMutable()->enabledFeatures); + featureClear(mask, &featureConfigMutable()->enabledFeatures); } -void featureClear(uint32_t mask) +void featureDisableAll(void) { - intFeatureClear(mask, &featureConfigMutable()->enabledFeatures); -} - -void featureClearAll(void) -{ - intFeatureClearAll(&featureConfigMutable()->enabledFeatures); + featureConfigMutable()->enabledFeatures = 0; } uint32_t featureMask(void) diff --git a/src/main/config/feature.h b/src/main/config/feature.h index 56ee75a5b9..f8f9a16764 100644 --- a/src/main/config/feature.h +++ b/src/main/config/feature.h @@ -62,14 +62,11 @@ typedef struct featureConfig_s { PG_DECLARE(featureConfig_t, featureConfig); -void latchActiveFeatures(void); -bool featureConfigured(uint32_t mask); -bool feature(uint32_t mask); -void featureSet(uint32_t mask); -void featureClear(uint32_t mask); -void featureClearAll(void); +bool featureIsEnabled(const uint32_t mask); +void featureEnable(const uint32_t mask); +void featureDisable(const uint32_t mask); +void featureDisableAll(void); uint32_t featureMask(void); -void intFeatureClearAll(uint32_t *features); -void intFeatureSet(uint32_t mask, uint32_t *features); -void intFeatureClear(uint32_t mask, uint32_t *features); +void featureSet(const uint32_t mask, uint32_t *features); +void featureClear(const uint32_t mask, uint32_t *features); diff --git a/src/main/drivers/accgyro/accgyro.h b/src/main/drivers/accgyro/accgyro.h index fe7560e136..636dd05c80 100644 --- a/src/main/drivers/accgyro/accgyro.h +++ b/src/main/drivers/accgyro/accgyro.h @@ -27,7 +27,7 @@ #include "drivers/bus.h" #include "drivers/sensor.h" #include "drivers/accgyro/accgyro_mpu.h" -#include "sensors/gyro.h" + #pragma GCC diagnostic push #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD) #include @@ -35,16 +35,37 @@ #pragma GCC diagnostic warning "-Wpadded" #endif -#ifndef MPU_I2C_INSTANCE -#define MPU_I2C_INSTANCE I2C_DEVICE +typedef enum { + GYRO_NONE = 0, + GYRO_DEFAULT, + GYRO_MPU6050, + GYRO_L3G4200D, + GYRO_MPU3050, + GYRO_L3GD20, + GYRO_MPU6000, + GYRO_MPU6500, + GYRO_MPU9250, + GYRO_ICM20601, + GYRO_ICM20602, + GYRO_ICM20608G, + GYRO_ICM20649, + GYRO_ICM20689, + GYRO_BMI160, + GYRO_FAKE +} gyroHardware_e; + +typedef enum { + GYRO_HARDWARE_LPF_NORMAL, + GYRO_HARDWARE_LPF_1KHZ_SAMPLE, +#ifdef USE_GYRO_DLPF_EXPERIMENTAL + GYRO_HARDWARE_LPF_EXPERIMENTAL #endif +} gyroHardwareLpf_e; -#define GYRO_HARDWARE_LPF_NORMAL 0 -#define GYRO_HARDWARE_LPF_EXPERIMENTAL 1 -#define GYRO_HARDWARE_LPF_1KHZ_SAMPLE 2 - -#define GYRO_32KHZ_HARDWARE_LPF_NORMAL 0 -#define GYRO_32KHZ_HARDWARE_LPF_EXPERIMENTAL 1 +typedef enum { + GYRO_32KHZ_HARDWARE_LPF_NORMAL, + GYRO_32KHZ_HARDWARE_LPF_EXPERIMENTAL +} gyro32khzHardwareLpf; typedef enum { GYRO_RATE_1_kHz, @@ -71,7 +92,6 @@ typedef struct gyroDev_s { int32_t gyroADCRawPrevious[XYZ_AXIS_COUNT]; int16_t gyroADCRaw[XYZ_AXIS_COUNT]; int16_t temperature; - mpuConfiguration_t mpuConfiguration; mpuDetectionResult_t mpuDetectionResult; sensor_align_e gyroAlign; gyroRateKHz_e gyroRateKHz; @@ -82,13 +102,14 @@ typedef struct gyroDev_s { uint8_t mpuDividerDrops; ioTag_t mpuIntExtiTag; uint8_t gyroHasOverflowProtection; - gyroSensor_e gyroHardware; + gyroHardware_e gyroHardware; } gyroDev_t; typedef struct accDev_s { #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD) pthread_mutex_t lock; #endif + float acc_1G_rec; sensorAccInitFuncPtr initFn; // initialize function sensorAccReadFuncPtr readFn; // read 3 axis data function busDevice_t bus; diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index 9ae8fbfdd4..62350addb2 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -54,11 +54,8 @@ #include "drivers/accgyro/accgyro_spi_mpu9250.h" #include "drivers/accgyro/accgyro_mpu.h" -mpuResetFnPtr mpuResetFn; - -#ifndef MPU_I2C_INSTANCE -#define MPU_I2C_INSTANCE I2C_DEVICE -#endif +#include "pg/pg.h" +#include "pg/gyrodev.h" #ifndef MPU_ADDRESS #define MPU_ADDRESS 0x68 @@ -66,7 +63,7 @@ mpuResetFnPtr mpuResetFn; #define MPU_INQUIRY_MASK 0x7E -#ifdef USE_I2C +#ifdef USE_I2C_GYRO static void mpu6050FindRevision(gyroDev_t *gyro) { // There is a map of revision contained in the android source tree which is quite comprehensive and may help to understand this code @@ -105,7 +102,7 @@ static void mpu6050FindRevision(gyroDev_t *gyro) /* * Gyro interrupt service routine */ -#if defined(MPU_INT_EXTI) +#ifdef USE_GYRO_EXTI static void mpuIntExtiHandler(extiCallbackRec_t *cb) { #ifdef DEBUG_MPU_DATA_READY_INTERRUPT @@ -138,11 +135,11 @@ static void mpuIntExtiInit(gyroDev_t *gyro) #endif #if defined (STM32F7) - IOInit(mpuIntIO, OWNER_MPU_EXTI, 0); + IOInit(mpuIntIO, OWNER_GYRO_EXTI, 0); EXTIHandlerInit(&gyro->exti, mpuIntExtiHandler); EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, IO_CONFIG(GPIO_MODE_INPUT,0,GPIO_NOPULL)); // TODO - maybe pullup / pulldown ? #else - IOInit(mpuIntIO, OWNER_MPU_EXTI, 0); + IOInit(mpuIntIO, OWNER_GYRO_EXTI, 0); IOConfigGPIO(mpuIntIO, IOCFG_IN_FLOATING); // TODO - maybe pullup / pulldown ? EXTIHandlerInit(&gyro->exti, mpuIntExtiHandler); @@ -184,6 +181,7 @@ bool mpuGyroRead(gyroDev_t *gyro) return true; } +#ifdef USE_SPI_GYRO bool mpuGyroReadSPI(gyroDev_t *gyro) { static const uint8_t dataToSend[7] = {MPU_RA_GYRO_XOUT_H | 0x80, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; @@ -201,121 +199,81 @@ bool mpuGyroReadSPI(gyroDev_t *gyro) return true; } -#ifdef USE_SPI -static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) +typedef uint8_t (*gyroSpiDetectFn_t)(const busDevice_t *bus); + +static gyroSpiDetectFn_t gyroSpiDetectFnTable[] = { +#ifdef USE_GYRO_SPI_MPU6000 + mpu6000SpiDetect, +#endif +#ifdef USE_GYRO_SPI_MPU6500 + mpu6500SpiDetect, // some targets using MPU_9250_SPI, ICM_20608_SPI or ICM_20602_SPI state sensor is MPU_65xx_SPI +#endif +#ifdef USE_GYRO_SPI_MPU9250 + mpu9250SpiDetect, +#endif +#ifdef USE_GYRO_SPI_ICM20649 + icm20649SpiDetect, +#endif +#ifdef USE_GYRO_SPI_ICM20689 + icm20689SpiDetect, // icm20689SpiDetect detects ICM20602 and ICM20689 +#endif +#ifdef USE_ACCGYRO_BMI160 + bmi160Detect, +#endif + NULL // Avoid an empty array +}; + +static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro, const gyroDeviceConfig_t *config) { - UNUSED(gyro); // since there are FCs which have gyro on I2C but other devices on SPI + SPI_TypeDef *instance = spiInstanceByDevice(SPI_CFG_TO_DEV(config->spiBus)); + if (!instance) { + return false; + } + spiBusSetInstance(&gyro->bus, instance); + + gyro->bus.busdev_u.spi.csnPin = IOGetByTag(config->csnTag); + IOInit(gyro->bus.busdev_u.spi.csnPin, OWNER_GYRO_CS, RESOURCE_INDEX(config->index)); + IOConfigGPIO(gyro->bus.busdev_u.spi.csnPin, SPI_IO_CS_CFG); + IOHi(gyro->bus.busdev_u.spi.csnPin); // Ensure device is disabled, important when two devices are on the same bus. uint8_t sensor = MPU_NONE; - UNUSED(sensor); - // note, when USE_DUAL_GYRO is enabled the gyro->bus must already be initialised. + // It is hard to use hardware to optimize the detection loop here, + // as hardware type and detection function name doesn't match. + // May need a bitmap of hardware to detection function to do it right? -#ifdef USE_GYRO_SPI_MPU6000 -#ifndef USE_DUAL_GYRO - spiBusSetInstance(&gyro->bus, MPU6000_SPI_INSTANCE); -#endif -#ifdef MPU6000_CS_PIN - gyro->bus.busdev_u.spi.csnPin = gyro->bus.busdev_u.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU6000_CS_PIN)) : gyro->bus.busdev_u.spi.csnPin; -#endif - sensor = mpu6000SpiDetect(&gyro->bus); - if (sensor != MPU_NONE) { - gyro->mpuDetectionResult.sensor = sensor; - return true; + for (size_t index = 0 ; gyroSpiDetectFnTable[index] ; index++) { + sensor = (gyroSpiDetectFnTable[index])(&gyro->bus); + if (sensor != MPU_NONE) { + gyro->mpuDetectionResult.sensor = sensor; + return true; + } } -#endif -#ifdef USE_GYRO_SPI_MPU6500 -#ifndef USE_DUAL_GYRO - spiBusSetInstance(&gyro->bus, MPU6500_SPI_INSTANCE); -#endif -#ifdef MPU6500_CS_PIN - gyro->bus.busdev_u.spi.csnPin = gyro->bus.busdev_u.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU6500_CS_PIN)) : gyro->bus.busdev_u.spi.csnPin; -#endif - sensor = mpu6500SpiDetect(&gyro->bus); - // some targets using MPU_9250_SPI, ICM_20608_SPI or ICM_20602_SPI state sensor is MPU_65xx_SPI - if (sensor != MPU_NONE) { - gyro->mpuDetectionResult.sensor = sensor; - return true; - } -#endif - -#ifdef USE_GYRO_SPI_MPU9250 -#ifndef USE_DUAL_GYRO - spiBusSetInstance(&gyro->bus, MPU9250_SPI_INSTANCE); -#endif -#ifdef MPU9250_CS_PIN - gyro->bus.busdev_u.spi.csnPin = gyro->bus.busdev_u.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU9250_CS_PIN)) : gyro->bus.busdev_u.spi.csnPin; -#endif - sensor = mpu9250SpiDetect(&gyro->bus); - if (sensor != MPU_NONE) { - gyro->mpuDetectionResult.sensor = sensor; - gyro->mpuConfiguration.resetFn = mpu9250SpiResetGyro; - return true; - } -#endif - -#ifdef USE_GYRO_SPI_ICM20649 -#ifdef ICM20649_SPI_INSTANCE - spiBusSetInstance(&gyro->bus, ICM20649_SPI_INSTANCE); -#endif -#ifdef ICM20649_CS_PIN - gyro->bus.busdev_u.spi.csnPin = gyro->bus.busdev_u.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(ICM20649_CS_PIN)) : gyro->bus.busdev_u.spi.csnPin; -#endif - sensor = icm20649SpiDetect(&gyro->bus); - if (sensor != MPU_NONE) { - gyro->mpuDetectionResult.sensor = sensor; - return true; - } -#endif - -#ifdef USE_GYRO_SPI_ICM20689 -#ifndef USE_DUAL_GYRO - spiBusSetInstance(&gyro->bus, ICM20689_SPI_INSTANCE); -#endif -#ifdef ICM20689_CS_PIN - gyro->bus.busdev_u.spi.csnPin = gyro->bus.busdev_u.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(ICM20689_CS_PIN)) : gyro->bus.busdev_u.spi.csnPin; -#endif - sensor = icm20689SpiDetect(&gyro->bus); - // icm20689SpiDetect detects ICM20602 and ICM20689 - if (sensor != MPU_NONE) { - gyro->mpuDetectionResult.sensor = sensor; - return true; - } -#endif - -#ifdef USE_ACCGYRO_BMI160 -#ifndef USE_DUAL_GYRO - spiBusSetInstance(&gyro->bus, BMI160_SPI_INSTANCE); -#endif -#ifdef BMI160_CS_PIN - gyro->bus.busdev_u.spi.csnPin = gyro->bus.busdev_u.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(BMI160_CS_PIN)) : gyro->bus.busdev_u.spi.csnPin; -#endif - sensor = bmi160Detect(&gyro->bus); - if (sensor != MPU_NONE) { - gyro->mpuDetectionResult.sensor = sensor; - return true; - } -#endif + spiPreinitCsByTag(config->csnTag); return false; } #endif -void mpuDetect(gyroDev_t *gyro) +void mpuDetect(gyroDev_t *gyro, const gyroDeviceConfig_t *config) { // MPU datasheet specifies 30ms. delay(35); -#ifdef USE_I2C - if (gyro->bus.bustype == BUSTYPE_NONE) { - // if no bustype is selected try I2C first. - gyro->bus.bustype = BUSTYPE_I2C; + if (config->bustype == BUSTYPE_NONE) { + return; } + if (config->bustype == BUSTYPE_GYRO_AUTO) { + gyro->bus.bustype = BUSTYPE_I2C; + } else { + gyro->bus.bustype = config->bustype; + } + +#ifdef USE_I2C_GYRO if (gyro->bus.bustype == BUSTYPE_I2C) { - gyro->bus.busdev_u.i2c.device = MPU_I2C_INSTANCE; - gyro->bus.busdev_u.i2c.address = MPU_ADDRESS; + gyro->bus.busdev_u.i2c.address = config->i2cAddress ? config->i2cAddress : MPU_ADDRESS; uint8_t sig = 0; bool ack = busReadRegisterBuffer(&gyro->bus, MPU_RA_WHO_AM_I, &sig, 1); @@ -342,15 +300,15 @@ void mpuDetect(gyroDev_t *gyro) } #endif -#ifdef USE_SPI +#ifdef USE_SPI_GYRO gyro->bus.bustype = BUSTYPE_SPI; - detectSPISensorsAndUpdateDetectionResult(gyro); + detectSPISensorsAndUpdateDetectionResult(gyro, config); #endif } void mpuGyroInit(gyroDev_t *gyro) { -#ifdef MPU_INT_EXTI +#ifdef USE_GYRO_EXTI mpuIntExtiInit(gyro); #else UNUSED(gyro); @@ -359,20 +317,27 @@ void mpuGyroInit(gyroDev_t *gyro) uint8_t mpuGyroDLPF(gyroDev_t *gyro) { - uint8_t ret; - if (gyro->gyroRateKHz > GYRO_RATE_8_kHz) { - ret = 0; // If gyro is in 32KHz mode then the DLPF bits aren't used - set to 0 - } else { + uint8_t ret = 0; + + // If gyro is in 32KHz mode then the DLPF bits aren't used + if (gyro->gyroRateKHz <= GYRO_RATE_8_kHz) { switch (gyro->hardware_lpf) { - case GYRO_HARDWARE_LPF_NORMAL: - ret = 0; - break; +#ifdef USE_GYRO_DLPF_EXPERIMENTAL case GYRO_HARDWARE_LPF_EXPERIMENTAL: - ret = 7; + // experimental mode not supported for MPU60x0 family + if ((gyro->gyroHardware != GYRO_MPU6050) && (gyro->gyroHardware != GYRO_MPU6000)) { + ret = 7; + } else { + ret = 0; + } break; +#endif + case GYRO_HARDWARE_LPF_1KHZ_SAMPLE: ret = 1; break; + + case GYRO_HARDWARE_LPF_NORMAL: default: ret = 0; break; @@ -384,11 +349,15 @@ uint8_t mpuGyroDLPF(gyroDev_t *gyro) uint8_t mpuGyroFCHOICE(gyroDev_t *gyro) { if (gyro->gyroRateKHz > GYRO_RATE_8_kHz) { +#ifdef USE_GYRO_DLPF_EXPERIMENTAL if (gyro->hardware_32khz_lpf == GYRO_32KHZ_HARDWARE_LPF_EXPERIMENTAL) { return FCB_8800_32; } else { return FCB_3600_32; } +#else + return FCB_3600_32; +#endif } else { return FCB_DISABLED; // Not in 32KHz mode, set FCHOICE to select 8KHz sampling } diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h index d038ba5ed7..845401f6de 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.h +++ b/src/main/drivers/accgyro/accgyro_mpu.h @@ -140,14 +140,6 @@ // RF = Register Flag #define MPU_RF_DATA_RDY_EN (1 << 0) -typedef void (*mpuResetFnPtr)(void); - -extern mpuResetFnPtr mpuResetFn; - -typedef struct mpuConfiguration_s { - mpuResetFnPtr resetFn; -} mpuConfiguration_t; - enum gyro_fsr_e { INV_FSR_250DPS = 0, INV_FSR_500DPS, @@ -156,6 +148,14 @@ enum gyro_fsr_e { NUM_GYRO_FSR }; +enum icm_high_range_gyro_fsr_e { + ICM_HIGH_RANGE_FSR_500DPS = 0, + ICM_HIGH_RANGE_FSR_1000DPS, + ICM_HIGH_RANGE_FSR_2000DPS, + ICM_HIGH_RANGE_FSR_4000DPS, + NUM_ICM_HIGH_RANGE_GYRO_FSR +}; + enum fchoice_b { FCB_DISABLED = 0x00, FCB_8800_32 = 0x01, @@ -176,6 +176,14 @@ enum accel_fsr_e { NUM_ACCEL_FSR }; +enum icm_high_range_accel_fsr_e { + ICM_HIGH_RANGE_FSR_4G = 0, + ICM_HIGH_RANGE_FSR_8G, + ICM_HIGH_RANGE_FSR_16G, + ICM_HIGH_RANGE_FSR_32G, + NUM_ICM_HIGH_RANGE_ACCEL_FSR +}; + typedef enum { GYRO_OVERFLOW_NONE = 0x00, GYRO_OVERFLOW_X = 0x01, @@ -210,10 +218,11 @@ typedef struct mpuDetectionResult_s { } mpuDetectionResult_t; struct gyroDev_s; +struct gyroDeviceConfig_s; void mpuGyroInit(struct gyroDev_s *gyro); bool mpuGyroRead(struct gyroDev_s *gyro); bool mpuGyroReadSPI(struct gyroDev_s *gyro); -void mpuDetect(struct gyroDev_s *gyro); +void mpuDetect(struct gyroDev_s *gyro, const struct gyroDeviceConfig_s *config); uint8_t mpuGyroDLPF(struct gyroDev_s *gyro); uint8_t mpuGyroFCHOICE(struct gyroDev_s *gyro); uint8_t mpuGyroReadRegister(const busDevice_t *bus, uint8_t reg); diff --git a/src/main/drivers/accgyro/accgyro_mpu6500.c b/src/main/drivers/accgyro/accgyro_mpu6500.c index 7a7cac1854..dd534e392f 100644 --- a/src/main/drivers/accgyro/accgyro_mpu6500.c +++ b/src/main/drivers/accgyro/accgyro_mpu6500.c @@ -56,6 +56,14 @@ void mpu6500GyroInit(gyroDev_t *gyro) { mpuGyroInit(gyro); + int gyro_range = INV_FSR_2000DPS; + int accel_range = INV_FSR_16G; + + if (gyro->mpuDetectionResult.sensor == ICM_20601_SPI) { + gyro_range = gyro->gyro_high_fsr ? ICM_HIGH_RANGE_FSR_4000DPS : ICM_HIGH_RANGE_FSR_2000DPS; + accel_range = ICM_HIGH_RANGE_FSR_16G; + } + busWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET); delay(100); busWriteRegister(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, 0x07); @@ -64,9 +72,9 @@ void mpu6500GyroInit(gyroDev_t *gyro) delay(100); busWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL); delay(15); - busWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3 | mpuGyroFCHOICE(gyro)); + busWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, gyro_range << 3 | mpuGyroFCHOICE(gyro)); delay(15); - busWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); + busWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, accel_range << 3); delay(15); busWriteRegister(&gyro->bus, MPU_RA_CONFIG, mpuGyroDLPF(gyro)); delay(15); diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi160.c b/src/main/drivers/accgyro/accgyro_spi_bmi160.c index 643cfad57f..ac0af59796 100644 --- a/src/main/drivers/accgyro/accgyro_spi_bmi160.c +++ b/src/main/drivers/accgyro/accgyro_spi_bmi160.c @@ -97,11 +97,6 @@ uint8_t bmi160Detect(const busDevice_t *bus) return BMI_160_SPI; } -#ifndef USE_DUAL_GYRO - IOInit(bus->busdev_u.spi.csnPin, OWNER_MPU_CS, 0); - IOConfigGPIO(bus->busdev_u.spi.csnPin, SPI_IO_CS_CFG); - IOHi(bus->busdev_u.spi.csnPin); -#endif spiSetDivisor(bus->busdev_u.spi.instance, BMI160_SPI_DIVISOR); @@ -250,22 +245,18 @@ void bmi160ExtiHandler(extiCallbackRec_t *cb) static void bmi160IntExtiInit(gyroDev_t *gyro) { - static bool bmi160ExtiInitDone = false; - - if (bmi160ExtiInitDone) { + if (gyro->mpuIntExtiTag == IO_TAG_NONE) { return; } - IO_t mpuIntIO = IOGetByTag(IO_TAG(BMI160_INT_EXTI)); + IO_t mpuIntIO = IOGetByTag(gyro->mpuIntExtiTag); - IOInit(mpuIntIO, OWNER_MPU_EXTI, 0); + IOInit(mpuIntIO, OWNER_GYRO_EXTI, 0); IOConfigGPIO(mpuIntIO, IOCFG_IN_FLOATING); // TODO - maybe pullup / pulldown ? EXTIHandlerInit(&gyro->exti, bmi160ExtiHandler); EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, EXTI_Trigger_Rising); EXTIEnable(mpuIntIO, true); - - bmi160ExtiInitDone = true; } diff --git a/src/main/drivers/accgyro/accgyro_spi_icm20649.c b/src/main/drivers/accgyro/accgyro_spi_icm20649.c index 802e492342..6a809840f3 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm20649.c +++ b/src/main/drivers/accgyro/accgyro_spi_icm20649.c @@ -45,11 +45,6 @@ static void icm20649SpiInit(const busDevice_t *bus) return; } -#ifndef USE_DUAL_GYRO - IOInit(bus->busdev_u.spi.csnPin, OWNER_MPU_CS, 0); - IOConfigGPIO(bus->busdev_u.spi.csnPin, SPI_IO_CS_CFG); - IOHi(bus->busdev_u.spi.csnPin); -#endif // all registers can be read/written at full speed (7MHz +-10%) // TODO verify that this works at 9MHz and 10MHz on non F7 diff --git a/src/main/drivers/accgyro/accgyro_spi_icm20689.c b/src/main/drivers/accgyro/accgyro_spi_icm20689.c index af1407bf19..d2b51c43dd 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm20689.c +++ b/src/main/drivers/accgyro/accgyro_spi_icm20689.c @@ -45,11 +45,6 @@ static void icm20689SpiInit(const busDevice_t *bus) return; } -#ifndef USE_DUAL_GYRO - IOInit(bus->busdev_u.spi.csnPin, OWNER_MPU_CS, 0); - IOConfigGPIO(bus->busdev_u.spi.csnPin, SPI_IO_CS_CFG); - IOHi(bus->busdev_u.spi.csnPin); -#endif spiSetDivisor(bus->busdev_u.spi.instance, SPI_CLOCK_STANDARD); @@ -60,7 +55,7 @@ uint8_t icm20689SpiDetect(const busDevice_t *bus) { icm20689SpiInit(bus); - spiSetDivisor(bus->busdev_u.spi.instance, SPI_CLOCK_INITIALIZATON); //low speed + spiSetDivisor(bus->busdev_u.spi.instance, SPI_CLOCK_INITIALIZATION); //low speed spiBusWriteRegister(bus, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET); @@ -124,7 +119,7 @@ void icm20689GyroInit(gyroDev_t *gyro) { mpuGyroInit(gyro); - spiSetDivisor(gyro->bus.busdev_u.spi.instance, SPI_CLOCK_INITIALIZATON); + spiSetDivisor(gyro->bus.busdev_u.spi.instance, SPI_CLOCK_INITIALIZATION); spiBusWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET); delay(100); diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro/accgyro_spi_mpu6000.c index 6ca1a114a5..65e353ef63 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu6000.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu6000.c @@ -105,7 +105,7 @@ void mpu6000SpiGyroInit(gyroDev_t *gyro) mpu6000AccAndGyroInit(gyro); - spiSetDivisor(gyro->bus.busdev_u.spi.instance, SPI_CLOCK_INITIALIZATON); + spiSetDivisor(gyro->bus.busdev_u.spi.instance, SPI_CLOCK_INITIALIZATION); // Accel and Gyro DLPF Setting spiBusWriteRegister(&gyro->bus, MPU6000_CONFIG, mpuGyroDLPF(gyro)); @@ -127,13 +127,8 @@ void mpu6000SpiAccInit(accDev_t *acc) uint8_t mpu6000SpiDetect(const busDevice_t *bus) { -#ifndef USE_DUAL_GYRO - IOInit(bus->busdev_u.spi.csnPin, OWNER_MPU_CS, 0); - IOConfigGPIO(bus->busdev_u.spi.csnPin, SPI_IO_CS_CFG); - IOHi(bus->busdev_u.spi.csnPin); -#endif - spiSetDivisor(bus->busdev_u.spi.instance, SPI_CLOCK_INITIALIZATON); + spiSetDivisor(bus->busdev_u.spi.instance, SPI_CLOCK_INITIALIZATION); spiBusWriteRegister(bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET); @@ -180,7 +175,7 @@ static void mpu6000AccAndGyroInit(gyroDev_t *gyro) return; } - spiSetDivisor(gyro->bus.busdev_u.spi.instance, SPI_CLOCK_INITIALIZATON); + spiSetDivisor(gyro->bus.busdev_u.spi.instance, SPI_CLOCK_INITIALIZATION); // Device Reset spiBusWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET); diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro/accgyro_spi_mpu6500.c index cc63dab615..25e84822d4 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu6500.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu6500.c @@ -41,11 +41,6 @@ static void mpu6500SpiInit(const busDevice_t *bus) { -#ifndef USE_DUAL_GYRO - IOInit(bus->busdev_u.spi.csnPin, OWNER_MPU_CS, 0); - IOConfigGPIO(bus->busdev_u.spi.csnPin, SPI_IO_CS_CFG); - IOHi(bus->busdev_u.spi.csnPin); -#endif spiSetDivisor(bus->busdev_u.spi.instance, SPI_CLOCK_FAST); } @@ -108,6 +103,7 @@ bool mpu6500SpiAccDetect(accDev_t *acc) case MPU_9250_SPI: case ICM_20608_SPI: case ICM_20602_SPI: + case ICM_20601_SPI: break; default: return false; @@ -127,6 +123,11 @@ bool mpu6500SpiGyroDetect(gyroDev_t *gyro) case MPU_9250_SPI: case ICM_20608_SPI: case ICM_20602_SPI: + // 16.4 dps/lsb scalefactor + gyro->scale = 1.0f / 16.4f; + break; + case ICM_20601_SPI: + gyro->scale = 1.0f / (gyro->gyro_high_fsr ? 8.2f : 16.4f); break; default: return false; @@ -135,8 +136,5 @@ bool mpu6500SpiGyroDetect(gyroDev_t *gyro) gyro->initFn = mpu6500SpiGyroInit; gyro->readFn = mpuGyroReadSPI; - // 16.4 dps/lsb scalefactor - gyro->scale = 1.0f / 16.4f; - return true; } diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu9250.c b/src/main/drivers/accgyro/accgyro_spi_mpu9250.c index 3cbe1e5ca3..ade77ccf25 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu9250.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu9250.c @@ -75,19 +75,6 @@ static bool mpu9250SpiSlowReadRegisterBuffer(const busDevice_t *bus, uint8_t reg return true; } -void mpu9250SpiResetGyro(void) -{ -#if 0 -// XXX This doesn't work. Need a proper busDevice_t. - // Device Reset -#ifdef MPU9250_CS_PIN - busDevice_t bus = { .spi = { .csnPin = IOGetByTag(IO_TAG(MPU9250_CS_PIN)) } }; - mpu9250SpiWriteRegister(&bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); - delay(150); -#endif -#endif -} - void mpu9250SpiGyroInit(gyroDev_t *gyro) { mpuGyroInit(gyro); @@ -137,7 +124,7 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro) { return; } - spiSetDivisor(gyro->bus.busdev_u.spi.instance, SPI_CLOCK_INITIALIZATON); //low speed for writing to slow registers + spiSetDivisor(gyro->bus.busdev_u.spi.instance, SPI_CLOCK_INITIALIZATION); //low speed for writing to slow registers mpu9250SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); delay(50); @@ -164,13 +151,8 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro) { uint8_t mpu9250SpiDetect(const busDevice_t *bus) { -#ifndef USE_DUAL_GYRO - IOInit(bus->busdev_u.spi.csnPin, OWNER_MPU_CS, 0); - IOConfigGPIO(bus->busdev_u.spi.csnPin, SPI_IO_CS_CFG); - IOHi(bus->busdev_u.spi.csnPin); -#endif - spiSetDivisor(bus->busdev_u.spi.instance, SPI_CLOCK_INITIALIZATON); //low speed + spiSetDivisor(bus->busdev_u.spi.instance, SPI_CLOCK_INITIALIZATION); //low speed mpu9250SpiWriteRegister(bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); uint8_t attemptsRemaining = 20; diff --git a/src/main/drivers/accgyro/gyro_sync.c b/src/main/drivers/accgyro/gyro_sync.c index 74a754cbff..55b43f4ec3 100644 --- a/src/main/drivers/accgyro/gyro_sync.c +++ b/src/main/drivers/accgyro/gyro_sync.c @@ -51,10 +51,10 @@ uint32_t gyroSetSampleRate(gyroDev_t *gyro, uint8_t lpf, uint8_t gyroSyncDenomin { float gyroSamplePeriod; - if (lpf == GYRO_HARDWARE_LPF_NORMAL || lpf == GYRO_HARDWARE_LPF_EXPERIMENTAL) { + if (lpf != GYRO_HARDWARE_LPF_1KHZ_SAMPLE) { if (gyro_use_32khz) { gyro->gyroRateKHz = GYRO_RATE_32_kHz; - gyroSamplePeriod = 31.5f; + gyroSamplePeriod = 31.25f; } else { switch (gyro->mpuDetectionResult.sensor) { case BMI_160_SPI: diff --git a/src/main/drivers/accgyro_legacy/accgyro_l3gd20.c b/src/main/drivers/accgyro_legacy/accgyro_l3gd20.c index d78944cb33..b1ea9b168d 100644 --- a/src/main/drivers/accgyro_legacy/accgyro_l3gd20.c +++ b/src/main/drivers/accgyro_legacy/accgyro_l3gd20.c @@ -82,12 +82,6 @@ static void l3gd20SpiInit(SPI_TypeDef *SPIx) UNUSED(SPIx); // FIXME mpul3gd20CsPin = IOGetByTag(IO_TAG(L3GD20_CS_PIN)); -#ifndef USE_DUAL_GYRO - IOInit(mpul3gd20CsPin, OWNER_MPU_CS, 0); - IOConfigGPIO(mpul3gd20CsPin, SPI_IO_CS_CFG); - - DISABLE_L3GD20; -#endif spiSetDivisor(L3GD20_SPI, SPI_CLOCK_STANDARD); } diff --git a/src/main/drivers/accgyro_legacy/accgyro_mma845x.c b/src/main/drivers/accgyro_legacy/accgyro_mma845x.c index 25c869b6c6..69419bba93 100644 --- a/src/main/drivers/accgyro_legacy/accgyro_mma845x.c +++ b/src/main/drivers/accgyro_legacy/accgyro_mma845x.c @@ -86,7 +86,7 @@ static uint8_t device_id; static inline void mma8451ConfigureInterrupt(void) { #ifdef MMA8451_INT_PIN - IOInit(IOGetByTag(IO_TAG(MMA8451_INT_PIN)), OWNER_MPU_EXTI, 0); + IOInit(IOGetByTag(IO_TAG(MMA8451_INT_PIN)), OWNER_GYRO_EXTI, 0); // TODO - maybe pullup / pulldown ? IOConfigGPIO(IOGetByTag(IO_TAG(MMA8451_INT_PIN)), IOCFG_IN_FLOATING); #endif diff --git a/src/main/drivers/adc_stm32f30x.c b/src/main/drivers/adc_stm32f30x.c index 81eb6e6c4a..e6f536c0a0 100644 --- a/src/main/drivers/adc_stm32f30x.c +++ b/src/main/drivers/adc_stm32f30x.c @@ -118,9 +118,10 @@ void adcInit(const adcConfig_t *config) adcOperatingConfig[ADC_CURRENT].tag = config->current.ioTag; //CURRENT_METER_ADC_CHANNEL; } - ADCDevice device = adcDeviceByInstance(ADC_INSTANCE); - if (device == ADCINVALID) + ADCDevice device = ADC_CFG_TO_DEV(config->device); + if (device == ADCINVALID) { return; + } #ifdef ADC24_DMA_REMAP SYSCFG_DMAChannelRemapConfig(SYSCFG_DMARemap_ADC2ADC4, ENABLE); diff --git a/src/main/drivers/adc_stm32f4xx.c b/src/main/drivers/adc_stm32f4xx.c index c247384f5b..f3340f4484 100644 --- a/src/main/drivers/adc_stm32f4xx.c +++ b/src/main/drivers/adc_stm32f4xx.c @@ -211,8 +211,9 @@ void adcInit(const adcConfig_t *config) } ADCDevice device = ADC_CFG_TO_DEV(config->device); - if (device == ADCINVALID) + if (device == ADCINVALID) { return; + } adcDevice_t adc = adcHardware[device]; diff --git a/src/main/drivers/adc_stm32f7xx.c b/src/main/drivers/adc_stm32f7xx.c index 7ce1a08597..255e61061d 100644 --- a/src/main/drivers/adc_stm32f7xx.c +++ b/src/main/drivers/adc_stm32f7xx.c @@ -58,9 +58,8 @@ #define TEMPSENSOR_CAL_VREFANALOG ( 3300U) /* Analog voltage reference (Vref+) voltage with which temperature sensor has been calibrated in production (+-10 mV) (unit: mV). */ // These addresses are incorrectly defined in stm32f7xx_ll_adc.h - -#if defined(STM32F745xx) || defined(STM32F746xx) -// F745xx_F746xx +#if defined(STM32F745xx) || defined(STM32F746xx) || defined(STM32F765xx) +// F745xx_F746xx and F765xx_F767xx_F769xx #define VREFINT_CAL_ADDR ((uint16_t*) (0x1FF0F44A)) #define TEMPSENSOR_CAL1_ADDR ((uint16_t*) (0x1FF0F44C)) #define TEMPSENSOR_CAL2_ADDR ((uint16_t*) (0x1FF0F44E)) @@ -238,8 +237,9 @@ void adcInit(const adcConfig_t *config) } ADCDevice device = adcDeviceByInstance(ADC_INSTANCE); - if (device == ADCINVALID) + if (device == ADCINVALID) { return; + } adc = adcHardware[device]; diff --git a/src/main/drivers/barometer/barometer_qmp6988.c b/src/main/drivers/barometer/barometer_qmp6988.c index e014e6c6b4..60aa80c632 100755 --- a/src/main/drivers/barometer/barometer_qmp6988.c +++ b/src/main/drivers/barometer/barometer_qmp6988.c @@ -139,8 +139,8 @@ bool qmp6988Detect(baroDev_t *baro) int Coe_b12_; int Coe_b21_; int Coe_bp3_; - u16 lb=0,hb=0; - u32 lw=0,hw=0,temp1,temp2; + uint16_t lb=0,hb=0; + uint32_t lw=0,hw=0,temp1,temp2; delay(20); diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h index cd2195959e..1dad078006 100644 --- a/src/main/drivers/bus.h +++ b/src/main/drivers/bus.h @@ -29,7 +29,8 @@ typedef enum { BUSTYPE_NONE = 0, BUSTYPE_I2C, BUSTYPE_SPI, - BUSTYPE_MPU_SLAVE // Slave I2C on SPI master + BUSTYPE_MPU_SLAVE, // Slave I2C on SPI master + BUSTYPE_GYRO_AUTO // Only used by acc/gyro bus auto detection code } busType_e; typedef struct busDevice_s { diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index 4645d8e3b6..b5d6b34561 100644 --- a/src/main/drivers/bus_spi.c +++ b/src/main/drivers/bus_spi.c @@ -62,7 +62,7 @@ SPIDevice spiDeviceByInstance(SPI_TypeDef *instance) SPI_TypeDef *spiInstanceByDevice(SPIDevice device) { - if (device >= SPIDEV_COUNT) { + if (device == SPIINVALID || device >= SPIDEV_COUNT) { return NULL; } @@ -141,6 +141,28 @@ void spiResetErrorCounter(SPI_TypeDef *instance) } } +bool spiBusIsBusBusy(const busDevice_t *bus) +{ + return spiIsBusBusy(bus->busdev_u.spi.instance); +} + +uint8_t spiBusTransferByte(const busDevice_t *bus, uint8_t data) +{ + return spiTransferByte(bus->busdev_u.spi.instance, data); +} + +void spiBusWriteByte(const busDevice_t *bus, uint8_t data) +{ + IOLo(bus->busdev_u.spi.csnPin); + spiBusTransferByte(bus, data); + IOHi(bus->busdev_u.spi.csnPin); +} + +bool spiBusRawTransfer(const busDevice_t *bus, const uint8_t *txData, uint8_t *rxData, int len) +{ + return spiTransfer(bus->busdev_u.spi.instance, txData, rxData, len); +} + bool spiBusWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data) { IOLo(bus->busdev_u.spi.csnPin); @@ -151,27 +173,46 @@ bool spiBusWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data) return true; } -bool spiBusReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t *data, uint8_t length) +bool spiBusRawReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t *data, uint8_t length) + { IOLo(bus->busdev_u.spi.csnPin); - spiTransferByte(bus->busdev_u.spi.instance, reg | 0x80); // read transaction + spiTransferByte(bus->busdev_u.spi.instance, reg); spiTransfer(bus->busdev_u.spi.instance, NULL, data, length); IOHi(bus->busdev_u.spi.csnPin); return true; } -uint8_t spiBusReadRegister(const busDevice_t *bus, uint8_t reg) +bool spiBusReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t *data, uint8_t length) +{ + return spiBusRawReadRegisterBuffer(bus, reg | 0x80, data, length); +} + +void spiBusWriteRegisterBuffer(const busDevice_t *bus, uint8_t reg, const uint8_t *data, uint8_t length) +{ + IOLo(bus->busdev_u.spi.csnPin); + spiTransferByte(bus->busdev_u.spi.instance, reg); + spiTransfer(bus->busdev_u.spi.instance, data, NULL, length); + IOHi(bus->busdev_u.spi.csnPin); +} + +uint8_t spiBusRawReadRegister(const busDevice_t *bus, uint8_t reg) { uint8_t data; IOLo(bus->busdev_u.spi.csnPin); - spiTransferByte(bus->busdev_u.spi.instance, reg | 0x80); // read transaction + spiTransferByte(bus->busdev_u.spi.instance, reg); spiTransfer(bus->busdev_u.spi.instance, NULL, &data, 1); IOHi(bus->busdev_u.spi.csnPin); return data; } +uint8_t spiBusReadRegister(const busDevice_t *bus, uint8_t reg) +{ + return spiBusRawReadRegister(bus, reg | 0x80); +} + void spiBusSetInstance(busDevice_t *bus, SPI_TypeDef *instance) { bus->bustype = BUSTYPE_SPI; diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index 8f1ef5ed4e..13e2627500 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -50,7 +50,7 @@ Flash M25p16 tolerates 20mhz, SPI_CLOCK_FAST should sit around 20 or less. */ typedef enum { - SPI_CLOCK_INITIALIZATON = 256, + SPI_CLOCK_INITIALIZATION = 256, #if defined(STM32F4) SPI_CLOCK_SLOW = 128, //00.65625 MHz SPI_CLOCK_STANDARD = 8, //10.50000 MHz @@ -112,10 +112,19 @@ void spiResetErrorCounter(SPI_TypeDef *instance); SPIDevice spiDeviceByInstance(SPI_TypeDef *instance); SPI_TypeDef *spiInstanceByDevice(SPIDevice device); +bool spiBusIsBusBusy(const busDevice_t *bus); + bool spiBusTransfer(const busDevice_t *bus, const uint8_t *txData, uint8_t *rxData, int length); +uint8_t spiBusTransferByte(const busDevice_t *bus, uint8_t data); +void spiBusWriteByte(const busDevice_t *bus, uint8_t data); +bool spiBusRawTransfer(const busDevice_t *bus, const uint8_t *txData, uint8_t *rxData, int len); + bool spiBusWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data); +bool spiBusRawReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t *data, uint8_t length); bool spiBusReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t *data, uint8_t length); +void spiBusWriteRegisterBuffer(const busDevice_t *bus, uint8_t reg, const uint8_t *data, uint8_t length); +uint8_t spiBusRawReadRegister(const busDevice_t *bus, uint8_t reg); uint8_t spiBusReadRegister(const busDevice_t *bus, uint8_t reg); void spiBusSetInstance(busDevice_t *bus, SPI_TypeDef *instance); diff --git a/src/main/drivers/bus_spi_ll.c b/src/main/drivers/bus_spi_ll.c index 826be73092..573a29c61c 100644 --- a/src/main/drivers/bus_spi_ll.c +++ b/src/main/drivers/bus_spi_ll.c @@ -27,6 +27,7 @@ #if defined(USE_SPI) #include "common/utils.h" +#include "common/maths.h" #include "drivers/bus.h" #include "drivers/bus_spi.h" @@ -36,8 +37,6 @@ #include "drivers/nvic.h" #include "drivers/rcc.h" -spiDevice_t spiDevice[SPIDEV_COUNT]; - #ifndef SPI2_SCK_PIN #define SPI2_NSS_PIN PB12 #define SPI2_SCK_PIN PB13 @@ -78,6 +77,10 @@ void spiInitDevice(SPIDevice device) { spiDevice_t *spi = &(spiDevice[device]); + if (!spi->dev) { + return; + } + #ifdef SDCARD_SPI_INSTANCE if (spi->dev == SDCARD_SPI_INSTANCE) { spi->leadingEdge = true; @@ -224,8 +227,10 @@ void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor) } #endif + divisor = constrain(divisor, 2, 256); + LL_SPI_Disable(instance); - LL_SPI_SetBaudRatePrescaler(instance, divisor ? (ffs(divisor | 0x100) - 2) << SPI_CR1_BR_Pos : 0); + LL_SPI_SetBaudRatePrescaler(instance, (ffs(divisor) - 2) << SPI_CR1_BR_Pos); LL_SPI_Enable(instance); } #endif diff --git a/src/main/drivers/bus_spi_stdperiph.c b/src/main/drivers/bus_spi_stdperiph.c index beda822348..de7911ea51 100644 --- a/src/main/drivers/bus_spi_stdperiph.c +++ b/src/main/drivers/bus_spi_stdperiph.c @@ -26,6 +26,7 @@ #ifdef USE_SPI +#include "common/maths.h" #include "drivers/bus.h" #include "drivers/bus_spi.h" #include "drivers/bus_spi_impl.h" @@ -33,12 +34,14 @@ #include "drivers/io.h" #include "drivers/rcc.h" -spiDevice_t spiDevice[SPIDEV_COUNT]; - void spiInitDevice(SPIDevice device) { spiDevice_t *spi = &(spiDevice[device]); + if (!spi->dev) { + return; + } + #ifdef SDCARD_SPI_INSTANCE if (spi->dev == SDCARD_SPI_INSTANCE) { spi->leadingEdge = true; @@ -184,10 +187,12 @@ void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor) } #endif + divisor = constrain(divisor, 2, 256); + SPI_Cmd(instance, DISABLE); const uint16_t tempRegister = (instance->CR1 & ~BR_BITS); - instance->CR1 = tempRegister | (divisor ? ((ffs(divisor | 0x100) - 2) << 3) : 0); + instance->CR1 = tempRegister | ((ffs(divisor) - 2) << 3); SPI_Cmd(instance, ENABLE); diff --git a/src/main/drivers/camera_control.c b/src/main/drivers/camera_control.c index cd552e0702..589e69828f 100644 --- a/src/main/drivers/camera_control.c +++ b/src/main/drivers/camera_control.c @@ -55,24 +55,21 @@ #define CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE #include "timer.h" -#ifndef CAMERA_CONTROL_PIN -#define CAMERA_CONTROL_PIN NONE -#endif - #ifdef USE_OSD #include "io/osd.h" #endif -PG_REGISTER_WITH_RESET_TEMPLATE(cameraControlConfig_t, cameraControlConfig, PG_CAMERA_CONTROL_CONFIG, 0); +PG_REGISTER_WITH_RESET_FN(cameraControlConfig_t, cameraControlConfig, PG_CAMERA_CONTROL_CONFIG, 0); -PG_RESET_TEMPLATE(cameraControlConfig_t, cameraControlConfig, - .mode = CAMERA_CONTROL_MODE_HARDWARE_PWM, - .refVoltage = 330, - .keyDelayMs = 180, - .internalResistance = 470, - .ioTag = IO_TAG(CAMERA_CONTROL_PIN), - .inverted = 0, // Output is inverted externally -); +void pgResetFn_cameraControlConfig(cameraControlConfig_t *cameraControlConfig) +{ + cameraControlConfig->mode = CAMERA_CONTROL_MODE_HARDWARE_PWM; + cameraControlConfig->refVoltage = 330; + cameraControlConfig->keyDelayMs = 180; + cameraControlConfig->internalResistance = 470; + cameraControlConfig->ioTag = timerioTagGetByUsage(TIM_USE_CAMERA_CONTROL, 0); + cameraControlConfig->inverted = 0; // Output is inverted externally +} static struct { bool enabled; diff --git a/src/main/drivers/compass/compass_lis3mdl.c b/src/main/drivers/compass/compass_lis3mdl.c new file mode 100644 index 0000000000..b1e00ea0cf --- /dev/null +++ b/src/main/drivers/compass/compass_lis3mdl.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 . + */ + +#include +#include + +#include + +#include "platform.h" + +#if defined(USE_MAG_LIS3MDL) + +#include "compass.h" +#include "drivers/time.h" +#include "common/axis.h" + +#define LIS3MDL_MAG_I2C_ADDRESS 0x1E +#define LIS3MDL_DEVICE_ID 0x3D + +#define LIS3MDL_REG_WHO_AM_I 0x0F + +#define LIS3MDL_REG_CTRL_REG1 0x20 +#define LIS3MDL_REG_CTRL_REG2 0x21 +#define LIS3MDL_REG_CTRL_REG3 0x22 +#define LIS3MDL_REG_CTRL_REG4 0x23 +#define LIS3MDL_REG_CTRL_REG5 0x24 + +#define LIS3MDL_REG_STATUS_REG 0x27 + +#define LIS3MDL_REG_OUT_X_L 0x28 +#define LIS3MDL_REG_OUT_X_H 0x29 +#define LIS3MDL_REG_OUT_Y_L 0x2A +#define LIS3MDL_REG_OUT_Y_H 0x2B +#define LIS3MDL_REG_OUT_Z_L 0x2C +#define LIS3MDL_REG_OUT_Z_H 0x2D + +#define LIS3MDL_TEMP_OUT_L 0x2E +#define LIS3MDL_TEMP_OUT_H 0x2F + +#define LIS3MDL_INT_CFG 0x30 +#define LIS3MDL_INT_SRC 0x31 +#define LIS3MDL_THS_L 0x32 +#define LIS3MDL_THS_H 0x33 + +// CTRL_REG1 +#define LIS3MDL_TEMP_EN 0x80 // Default 0 +#define LIS3MDL_OM_LOW_POWER 0x00 // Default +#define LIS3MDL_OM_MED_PROF 0x20 +#define LIS3MDL_OM_HI_PROF 0x40 +#define LIS3MDL_OM_ULTRA_HI_PROF 0x60 +#define LIS3MDL_DO_0_625 0x00 +#define LIS3MDL_DO_1_25 0x04 +#define LIS3MDL_DO_2_5 0x08 +#define LIS3MDL_DO_5 0x0C +#define LIS3MDL_DO_10 0x10 // Default +#define LIS3MDL_DO_20 0x14 +#define LIS3MDL_DO_40 0x18 +#define LIS3MDL_DO_80 0x1C +#define LIS3MDL_FAST_ODR 0x02 + +// CTRL_REG2 +#define LIS3MDL_FS_4GAUSS 0x00 // Default +#define LIS3MDL_FS_8GAUSS 0x20 +#define LIS3MDL_FS_12GAUSS 0x40 +#define LIS3MDL_FS_16GAUSS 0x60 +#define LIS3MDL_REBOOT 0x08 +#define LIS3MDL_SOFT_RST 0x04 + +// CTRL_REG3 +#define LIS3MDL_LP 0x20 // Default 0 +#define LIS3MDL_SIM 0x04 // Default 0 +#define LIS3MDL_MD_CONTINUOUS 0x00 // Default +#define LIS3MDL_MD_SINGLE 0x01 +#define LIS3MDL_MD_POWERDOWN 0x03 + +// CTRL_REG4 +#define LIS3MDL_ZOM_LP 0x00 // Default +#define LIS3MDL_ZOM_MP 0x04 +#define LIS3MDL_ZOM_HP 0x08 +#define LIS3MDL_ZOM_UHP 0x0C +#define LIS3MDL_BLE 0x02 // Default 0 + +// CTRL_REG5 +#define LIS3MDL_FAST_READ 0x80 // Default 0 +#define LIS3MDL_BDU 0x40 // Default 0 + +static bool lis3mdlRead(magDev_t * mag, int16_t *magData) +{ + uint8_t buf[6]; + + busDevice_t *busdev = &mag->busdev; + + bool ack = busReadRegisterBuffer(busdev, LIS3MDL_REG_OUT_X_L, buf, 6); + + if (!ack) { + return false; + } + + magData[X] = (int16_t)(buf[1] << 8 | buf[0]) / 4; + magData[Y] = (int16_t)(buf[3] << 8 | buf[2]) / 4; + magData[Z] = (int16_t)(buf[5] << 8 | buf[4]) / 4; + + return true; +} + +static bool lis3mdlInit(magDev_t *mag) +{ + busDevice_t *busdev = &mag->busdev; + + busWriteRegister(busdev, LIS3MDL_REG_CTRL_REG2, LIS3MDL_FS_4GAUSS); + busWriteRegister(busdev, LIS3MDL_REG_CTRL_REG1, LIS3MDL_TEMP_EN | LIS3MDL_OM_ULTRA_HI_PROF | LIS3MDL_DO_80); + busWriteRegister(busdev, LIS3MDL_REG_CTRL_REG5, LIS3MDL_BDU); + busWriteRegister(busdev, LIS3MDL_REG_CTRL_REG4, LIS3MDL_ZOM_UHP); + busWriteRegister(busdev, LIS3MDL_REG_CTRL_REG3, 0x00); + + delay(100); + + return true; +} + +bool lis3mdlDetect(magDev_t * mag) +{ + busDevice_t *busdev = &mag->busdev; + + uint8_t sig = 0; + + if (busdev->bustype == BUSTYPE_I2C && busdev->busdev_u.i2c.address == 0) { + busdev->busdev_u.i2c.address = LIS3MDL_MAG_I2C_ADDRESS; + } + + bool ack = busReadRegisterBuffer(&mag->busdev, LIS3MDL_REG_WHO_AM_I, &sig, 1); + + if (!ack || sig != LIS3MDL_DEVICE_ID) { + return false; + } + + mag->init = lis3mdlInit; + mag->read = lis3mdlRead; + + return true; +} +#endif diff --git a/src/main/io/displayport_rcdevice.h b/src/main/drivers/compass/compass_lis3mdl.h similarity index 88% rename from src/main/io/displayport_rcdevice.h rename to src/main/drivers/compass/compass_lis3mdl.h index 78bfa6272a..52a0928bd9 100644 --- a/src/main/io/displayport_rcdevice.h +++ b/src/main/drivers/compass/compass_lis3mdl.h @@ -20,5 +20,6 @@ #pragma once -struct vcdProfile_s; -displayPort_t *rcdeviceDisplayPortInit(const struct vcdProfile_s *vcdProfile); \ No newline at end of file +#include "drivers/io_types.h" + +bool lis3mdlDetect(magDev_t* mag); diff --git a/src/main/drivers/compass/compass_qmc5883l.c b/src/main/drivers/compass/compass_qmc5883l.c index 116f45f268..0f463e63ef 100644 --- a/src/main/drivers/compass/compass_qmc5883l.c +++ b/src/main/drivers/compass/compass_qmc5883l.c @@ -135,6 +135,12 @@ bool qmc5883lDetect(magDev_t *magDev) uint8_t sig = 0; bool ack = busReadRegisterBuffer(busdev, QMC5883L_REG_ID, &sig, 1); if (ack && sig == QMC5883_ID_VAL) { + // Should be in standby mode after soft reset and sensor is really present + // Reading ChipID of 0xFF alone is not sufficient to be sure the QMC is present + ack = busReadRegisterBuffer(busdev, QMC5883L_REG_CONF1, &sig, 1); + if (ack && sig != QMC5883L_MODE_STANDBY) { + return false; + } magDev->init = qmc5883lInit; magDev->read = qmc5883lRead; return true; diff --git a/src/main/drivers/inverter.c b/src/main/drivers/inverter.c index 1366f25010..0ef2bd3103 100644 --- a/src/main/drivers/inverter.c +++ b/src/main/drivers/inverter.c @@ -23,6 +23,8 @@ #include "platform.h" +#ifdef USE_INVERTER + #include "io/serial.h" // For SERIAL_PORT_IDENTIFIER_TO_INDEX #include "drivers/io.h" #include "drivers/serial.h" @@ -32,8 +34,6 @@ #include "inverter.h" -#ifdef USE_INVERTER - static const serialPinConfig_t *pSerialPinConfig; static void inverterSet(int identifier, bool on) diff --git a/src/main/drivers/inverter.h b/src/main/drivers/inverter.h index 3257df3e22..97d2708def 100644 --- a/src/main/drivers/inverter.h +++ b/src/main/drivers/inverter.h @@ -20,10 +20,6 @@ #pragma once -#if defined(INVERTER_PIN_UART1) || defined(INVERTER_PIN_UART2) || defined(INVERTER_PIN_UART3) || defined(INVERTER_PIN_UART4) || defined(INVERTER_PIN_UART5) || defined(INVERTER_PIN_UART6) -#define USE_INVERTER -#endif - #include "drivers/serial.h" void initInverters(const serialPinConfig_t *serialPinConfigToUse); diff --git a/src/main/drivers/light_ws2811strip.c b/src/main/drivers/light_ws2811strip.c index d360644c87..36005fc4ce 100644 --- a/src/main/drivers/light_ws2811strip.c +++ b/src/main/drivers/light_ws2811strip.c @@ -38,15 +38,20 @@ #include "common/color.h" #include "common/colorconversion.h" -#include "dma.h" + +#include "drivers/dma.h" #include "drivers/io.h" + #include "light_ws2811strip.h" #if defined(STM32F1) || defined(STM32F3) uint8_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; +#elif defined(STM32F7) +FAST_RAM_ZERO_INIT uint32_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; #else uint32_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; #endif + volatile uint8_t ws2811LedDataTransferInProgress = 0; uint16_t BIT_COMPARE_1 = 0; diff --git a/src/main/drivers/light_ws2811strip.h b/src/main/drivers/light_ws2811strip.h index 0044222e8e..6dfc367749 100644 --- a/src/main/drivers/light_ws2811strip.h +++ b/src/main/drivers/light_ws2811strip.h @@ -21,7 +21,6 @@ #pragma once #include "drivers/io_types.h" -#include "io/ledstrip.h" #define WS2811_LED_STRIP_LENGTH 32 #define WS2811_BITS_PER_LED 24 @@ -35,6 +34,12 @@ #define WS2811_TIMER_MHZ 48 #define WS2811_CARRIER_HZ 800000 +// Enumeration to match the string options defined in lookupLedStripFormatRGB in settings.c +typedef enum { + LED_GRB, + LED_RGB +} ledStripFormatRGB_e; + void ws2811LedStripInit(ioTag_t ioTag); void ws2811LedStripHardwareInit(ioTag_t ioTag); diff --git a/src/main/drivers/light_ws2811strip_hal.c b/src/main/drivers/light_ws2811strip_hal.c index f3bbfd3ec7..f615baa76d 100644 --- a/src/main/drivers/light_ws2811strip_hal.c +++ b/src/main/drivers/light_ws2811strip_hal.c @@ -26,13 +26,15 @@ #ifdef USE_LED_STRIP #include "common/color.h" -#include "light_ws2811strip.h" -#include "drivers/nvic.h" -#include "dma.h" + +#include "drivers/dma.h" #include "drivers/io.h" +#include "drivers/nvic.h" +#include "drivers/rcc.h" #include "drivers/system.h" -#include "rcc.h" -#include "timer.h" +#include "drivers/timer.h" + +#include "light_ws2811strip.h" static IO_t ws2811IO = IO_NONE; bool ws2811Initialised = false; diff --git a/src/main/drivers/light_ws2811strip_stdperiph.c b/src/main/drivers/light_ws2811strip_stdperiph.c index 9c7951d7a3..627aebefeb 100644 --- a/src/main/drivers/light_ws2811strip_stdperiph.c +++ b/src/main/drivers/light_ws2811strip_stdperiph.c @@ -27,14 +27,15 @@ #include "build/debug.h" +#include "common/color.h" + +#include "drivers/dma.h" #include "drivers/io.h" #include "drivers/nvic.h" +#include "drivers/rcc.h" +#include "drivers/timer.h" -#include "common/color.h" #include "light_ws2811strip.h" -#include "dma.h" -#include "rcc.h" -#include "timer.h" static IO_t ws2811IO = IO_NONE; bool ws2811Initialised = false; diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index 98add73dfc..7d4a2ea161 100644 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -186,10 +186,6 @@ #define __spiBusTransactionEnd(busdev) IOHi((busdev)->busdev_u.spi.csnPin) #endif -#ifndef MAX7456_SPI_CLK -#define MAX7456_SPI_CLK (SPI_CLOCK_STANDARD) -#endif - busDevice_t max7456BusDevice; busDevice_t *busdev = &max7456BusDevice; @@ -415,18 +411,18 @@ void max7456ReInit(void) // Here we init only CS and try to init MAX for first time. // Also detect device type (MAX v.s. AT) -void max7456Init(const max7456Config_t *max7456Config, const vcdProfile_t *pVcdProfile, bool cpuOverclock) +bool max7456Init(const max7456Config_t *max7456Config, const vcdProfile_t *pVcdProfile, bool cpuOverclock) { max7456HardwareReset(); if (!max7456Config->csTag) { - return; + return false; } busdev->busdev_u.spi.csnPin = IOGetByTag(max7456Config->csTag); if (!IOIsFreeOrPreinit(busdev->busdev_u.spi.csnPin)) { - return; + return false; } IOInit(busdev->busdev_u.spi.csnPin, OWNER_OSD_CS, 0); @@ -439,6 +435,8 @@ void max7456Init(const max7456Config_t *max7456Config, const vcdProfile_t *pVcdP // Do this at half the speed for safety. spiSetDivisor(busdev->busdev_u.spi.instance, MAX7456_SPI_CLK * 2); + __spiBusTransactionBegin(busdev); + max7456Send(MAX7456ADD_CMAL, (1 << 6)); // CA[8] bit if (max7456Send(MAX7456ADD_CMAL|MAX7456ADD_READ, 0xff) & (1 << 6)) { @@ -447,6 +445,8 @@ void max7456Init(const max7456Config_t *max7456Config, const vcdProfile_t *pVcdP max7456DeviceType = MAX7456_DEVICE_TYPE_MAX; } + __spiBusTransactionEnd(busdev); + #if defined(USE_OVERCLOCK) // Determine SPI clock divisor based on config and the device type. @@ -489,6 +489,7 @@ void max7456Init(const max7456Config_t *max7456Config, const vcdProfile_t *pVcdP #endif // Real init will be made later when driver detect idle. + return true; } /** diff --git a/src/main/drivers/max7456.h b/src/main/drivers/max7456.h index 95b55695cf..c5327ddfaa 100644 --- a/src/main/drivers/max7456.h +++ b/src/main/drivers/max7456.h @@ -31,7 +31,7 @@ extern uint16_t maxScreenSize; struct vcdProfile_s; void max7456HardwareReset(void); struct max7456Config_s; -void max7456Init(const struct max7456Config_s *max7456Config, const struct vcdProfile_s *vcdProfile, bool cpuOverclock); +bool max7456Init(const struct max7456Config_s *max7456Config, const struct vcdProfile_s *vcdProfile, bool cpuOverclock); void max7456Invert(bool invert); void max7456Brightness(uint8_t black, uint8_t white); void max7456DrawScreen(void); diff --git a/src/main/drivers/max7456_symbols.h b/src/main/drivers/max7456_symbols.h index 6a141794fe..9929d9d294 100644 --- a/src/main/drivers/max7456_symbols.h +++ b/src/main/drivers/max7456_symbols.h @@ -21,215 +21,106 @@ #pragma once -#define SYM_END_OF_FONT 0xFF - -// Character Symbols -#define SYM_BLANK 0x20 - -// Satellite Graphics -#define SYM_SAT_L 0x1E -#define SYM_SAT_R 0x1F -// Not available: #define SYM_HDP_L 0xBD -// Not available: #define SYM_HDP_R 0xBE -//#define SYM_SAT 0x0F // Not used - -// Degrees Icon for HEADING/DIRECTION HOME -// Not available: #define SYM_DEGREES 0xBD - -// Direction arrows -#define SYM_ARROW_SOUTH 0x60 -#define SYM_ARROW_2 0x61 -#define SYM_ARROW_3 0x62 -#define SYM_ARROW_4 0x63 -#define SYM_ARROW_EAST 0x64 -#define SYM_ARROW_6 0x65 -#define SYM_ARROW_7 0x66 -#define SYM_ARROW_8 0x67 -#define SYM_ARROW_NORTH 0x68 -#define SYM_ARROW_10 0x69 -#define SYM_ARROW_11 0x6A -#define SYM_ARROW_12 0x6B -#define SYM_ARROW_WEST 0x6C -#define SYM_ARROW_14 0x6D -#define SYM_ARROW_15 0x6E -#define SYM_ARROW_16 0x6F - -// Heading Graphics -#define SYM_HEADING_N 0x18 -#define SYM_HEADING_S 0x19 -#define SYM_HEADING_E 0x1A -#define SYM_HEADING_W 0x1B -#define SYM_HEADING_DIVIDED_LINE 0x1C -#define SYM_HEADING_LINE 0x1D - -// FRSKY HUB -// Not available: #define SYM_CELL0 0xF0 -// Not available: #define SYM_CELL1 0xF1 -// Not available: #define SYM_CELL2 0xF2 -// Not available: #define SYM_CELL3 0xF3 -// Not available: #define SYM_CELL4 0xF4 -// Not available: #define SYM_CELL5 0xF5 -// Not available: #define SYM_CELL6 0xF6 -// Not available: #define SYM_CELL7 0xF7 -// Not available: #define SYM_CELL8 0xF8 -// Not available: #define SYM_CELL9 0xF9 -// Not available: #define SYM_CELLA 0xFA -// Not available: #define SYM_CELLB 0xFB -// Not available: #define SYM_CELLC 0xFC -// Not available: #define SYM_CELLD 0xFD -// Not available: #define SYM_CELLE 0xFE -// Not available: #define SYM_CELLF 0xC3 - -// Map mode -#define SYM_HOME 0x04 -#define SYM_AIRCRAFT 0x05 -#define SYM_RANGE_100 0x21 -#define SYM_RANGE_500 0x22 -#define SYM_RANGE_2500 0x23 -#define SYM_RANGE_MAX 0x24 -#define SYM_DIRECTION 0x72 - -// GPS Coordinates and Altitude -// Not available: #define SYM_LAT 0xCA -// Not available: #define SYM_LON 0xCB -// Not available: #define SYM_ALT 0xCC - -// GPS Mode and Autopilot -// Not available: #define SYM_3DFIX 0xDF -// Not available: #define SYM_HOLD 0xEF -// Not available: #define SYM_G_HOME 0xFF -#define SYM_GHOME 0x9D -#define SYM_GHOME1 0x9E -// Not available: #define SYM_GHOLD 0xCD -// Not available: #define SYM_GHOLD1 0xCE -// Not available: #define SYM_GMISSION 0xB5 -// Not available: #define SYM_GMISSION1 0xB6 -// Not available: #define SYM_GLAND 0xB7 -// Not available: #define SYM_GLAND1 0xB8 -// Not available: #define SYM_HOME_DIST 0xA0 - -// Gimbal active Mode -#define SYM_GIMBAL 0x16 -#define SYM_GIMBAL1 0x17 - -// Sensor´s Presence -// Not available: #define SYM_ACC 0xA0 -// Not available: #define SYM_MAG 0xA1 -// Not available: #define SYM_BAR 0xA2 -// Not available: #define SYM_GPS 0xA3 -// Not available: #define SYM_MAN 0xC0 -// Not available: #define SYM_MAN1 0xC1 -// Not available: #define SYM_MAN2 0xC2 -// Not available: #define SYM_CHECK 0xBE -// Not available: #define SYM_BARO10 0xB7 -// Not available: #define SYM_BARO11 0xB8 -// Not available: #define SYM_MAG10 0xB5 -// Not available: #define SYM_MAG11 0xB6 - -// AH Center screen Graphics -#define SYM_AH_CENTER_LINE 0x26 -#define SYM_AH_CENTER_LINE_RIGHT 0x27 -#define SYM_AH_CENTER 0x7E -#define SYM_AH_RIGHT 0x02 -#define SYM_AH_LEFT 0x03 -#define SYM_AH_DECORATION_UP 0xC9 -#define SYM_AH_DECORATION_DOWN 0xCF - -// AH Bars -#define SYM_AH_BAR9_0 0x80 - -// Temperature -#define SYM_TEMP_F 0x0D -#define SYM_TEMP_C 0x0E - -// Batt evolution -#define SYM_BATT_FULL 0x90 -#define SYM_BATT_5 0x91 -#define SYM_BATT_4 0x92 -#define SYM_BATT_3 0x93 -#define SYM_BATT_2 0x94 -#define SYM_BATT_1 0x95 -#define SYM_BATT_EMPTY 0x96 - -// Vario -#define SYM_VARIO 0x7F - -// Glidescope -// Not available: #define SYM_GLIDESCOPE 0xE0 - -// Batt Icon´s -#define SYM_MAIN_BATT 0x97 -// Not available: #define SYM_VID_BAT 0xBF - -// Unit Icon´s (Metric) -#define SYM_MS 0x9F -// Not available: #define SYM_KMH 0xA5 -// Not available: #define SYM_ALTM 0xA7 -// Not available: #define SYM_DISTHOME_M 0xBB -#define SYM_M 0x0C - -// Unit Icon´s (Imperial) -#define SYM_FTS 0x99 -// Not available: #define SYM_MPH 0xA6 -// Not available: #define SYM_ALTFT 0xA8 -// Not available: #define SYM_DISTHOME_FT 0xB9 -#define SYM_FT 0x0F - -// Voltage and amperage -#define SYM_VOLT 0x06 -#define SYM_AMP 0x9A -#define SYM_MAH 0x07 -#define SYM_WATT 0x57 - -// Flying Mode -// Not available: #define SYM_ACRO 0xAE -#define SYM_ACROGY 0x98 -// Not available: #define SYM_ACRO1 0xAF -// Not available: #define SYM_STABLE 0xAC -// Not available: #define SYM_STABLE1 0xAD -// Not available: #define SYM_HORIZON 0xC4 -// Not available: #define SYM_HORIZON1 0xC5 -// Not available: #define SYM_PASS 0xAA -// Not available: #define SYM_PASS1 0xAB -// Not available: #define SYM_AIR 0xEA -// Not available: #define SYM_AIR1 0xEB -#define SYM_PLUS 0x89 - -// Note, these change with scrolling enabled (scrolling is TODO) -//#define SYM_AH_DECORATION_LEFT 0x13 -//#define SYM_AH_DECORATION_RIGHT 0x13 -#define SYM_AH_DECORATION 0x13 - -// Time -#define SYM_ON_M 0x9B -#define SYM_FLY_M 0x9C -#define SYM_ON_H 0x70 -#define SYM_FLY_H 0x71 -// Not available: #define SYM_CLOCK 0xBC - -// Throttle Position (%) -#define SYM_THR 0x04 -#define SYM_THR1 0x05 +//Misc +#define SYM_END_OF_FONT 0xFF +#define SYM_BLANK 0x20 +#define SYM_COLON 0x2D // RSSI -#define SYM_RSSI 0x01 +#define SYM_RSSI 0x01 -// Menu cursor -#define SYM_CURSOR SYM_AH_LEFT +// Throttle Position (%) +#define SYM_THR 0x04 +#define SYM_THR1 0x05 -//Misc -#define SYM_COLON 0x2D -// Not available: #define SYM_ZERO_HALF_TRAILING_DOT 0xC0 -// Not available: #define SYM_ZERO_HALF_LEADING_DOT 0xD0 +// Map mode +#define SYM_HOME 0x04 +#define SYM_AIRCRAFT 0x05 -//sport -// Not available: #define SYM_MIN 0xB3 -// Not available: #define SYM_AVG 0xB4 +// Unit Icon´s (Metric) +#define SYM_M 0x0C + +// Unit Icon´s (Imperial) +#define SYM_FT 0x0F + +// Heading Graphics +#define SYM_HEADING_N 0x18 +#define SYM_HEADING_S 0x19 +#define SYM_HEADING_E 0x1A +#define SYM_HEADING_W 0x1B +#define SYM_HEADING_DIVIDED_LINE 0x1C +#define SYM_HEADING_LINE 0x1D + +// AH Center screen Graphics +#define SYM_AH_CENTER_LINE 0x26 +#define SYM_AH_CENTER_LINE_RIGHT 0x27 +#define SYM_AH_CENTER 0x7E +#define SYM_AH_RIGHT 0x02 +#define SYM_AH_LEFT 0x03 +#define SYM_AH_DECORATION 0x13 + +// Satellite Graphics +#define SYM_SAT_L 0x1E +#define SYM_SAT_R 0x1F + +// Direction arrows +#define SYM_ARROW_SOUTH 0x60 +#define SYM_ARROW_2 0x61 +#define SYM_ARROW_3 0x62 +#define SYM_ARROW_4 0x63 +#define SYM_ARROW_EAST 0x64 +#define SYM_ARROW_6 0x65 +#define SYM_ARROW_7 0x66 +#define SYM_ARROW_8 0x67 +#define SYM_ARROW_NORTH 0x68 +#define SYM_ARROW_10 0x69 +#define SYM_ARROW_11 0x6A +#define SYM_ARROW_12 0x6B +#define SYM_ARROW_WEST 0x6C +#define SYM_ARROW_14 0x6D +#define SYM_ARROW_15 0x6E +#define SYM_ARROW_16 0x6F + +// AH Bars +#define SYM_AH_BAR9_0 0x80 +#define SYM_AH_BAR9_1 0x81 +#define SYM_AH_BAR9_2 0x82 +#define SYM_AH_BAR9_3 0x83 +#define SYM_AH_BAR9_4 0x84 +#define SYM_AH_BAR9_5 0x85 +#define SYM_AH_BAR9_6 0x86 +#define SYM_AH_BAR9_7 0x87 +#define SYM_AH_BAR9_8 0x88 // Progress bar -#define SYM_PB_START 0x8A -#define SYM_PB_FULL 0x8B -#define SYM_PB_HALF 0x8C -#define SYM_PB_EMPTY 0x8D -#define SYM_PB_END 0x8E -#define SYM_PB_CLOSE 0x8F +#define SYM_PB_START 0x8A +#define SYM_PB_FULL 0x8B +#define SYM_PB_HALF 0x8C +#define SYM_PB_EMPTY 0x8D +#define SYM_PB_END 0x8E +#define SYM_PB_CLOSE 0x8F + +// Batt evolution +#define SYM_BATT_FULL 0x90 +#define SYM_BATT_5 0x91 +#define SYM_BATT_4 0x92 +#define SYM_BATT_3 0x93 +#define SYM_BATT_2 0x94 +#define SYM_BATT_1 0x95 +#define SYM_BATT_EMPTY 0x96 + +// Batt Icon´s +#define SYM_MAIN_BATT 0x97 + +// Voltage and amperage +#define SYM_VOLT 0x06 +#define SYM_AMP 0x9A +#define SYM_MAH 0x07 +#define SYM_WATT 0x57 + +// Time +#define SYM_ON_M 0x9B +#define SYM_FLY_M 0x9C + +// Menu cursor +#define SYM_CURSOR SYM_AH_LEFT \ No newline at end of file diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 166d5c8882..ddd1e5b0c0 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -37,18 +37,20 @@ static FAST_RAM_ZERO_INIT pwmCompleteWriteFn *pwmCompleteWrite = NULL; #ifdef USE_DSHOT FAST_RAM_ZERO_INIT loadDmaBufferFn *loadDmaBuffer; +#define DSHOT_INITIAL_DELAY_US 10000 #define DSHOT_COMMAND_DELAY_US 1000 -#define DSHOT_ESCINFO_DELAY_US 5000 +#define DSHOT_ESCINFO_DELAY_US 12000 #define DSHOT_BEEP_DELAY_US 100000 typedef struct dshotCommandControl_s { - timeUs_t nextCommandAt; - timeUs_t delayAfterCommand; + timeUs_t nextCommandAtUs; + timeUs_t delayAfterCommandUs; + bool waitingForIdle; + uint8_t repeats; uint8_t command[MAX_SUPPORTED_MOTORS]; - uint8_t repeats; } dshotCommandControl_t; -dshotCommandControl_t dshotCommandControl; +static dshotCommandControl_t dshotCommandControl; #endif #ifdef USE_SERVOS @@ -386,16 +388,39 @@ uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType) } } +bool allMotorsAreIdle(uint8_t motorCount) +{ + bool allMotorsIdle = true; + for (unsigned i = 0; i < motorCount; i++) { + const motorDmaOutput_t *motor = getMotorDmaOutput(i); + if (motor->value) { + allMotorsIdle = false; + } + } + + return allMotorsIdle; +} + +FAST_CODE bool pwmDshotCommandIsQueued(void) +{ + return dshotCommandControl.nextCommandAtUs; +} + +FAST_CODE bool pwmDshotCommandIsProcessing(void) +{ + return dshotCommandControl.nextCommandAtUs && !dshotCommandControl.waitingForIdle && dshotCommandControl.repeats > 0; +} + void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bool blocking) { timeUs_t timeNowUs = micros(); - if (!isMotorProtocolDshot() || (command > DSHOT_MAX_COMMAND)) { + if (!isMotorProtocolDshot() || (command > DSHOT_MAX_COMMAND) || pwmDshotCommandIsQueued()) { return; } uint8_t repeats = 1; - timeUs_t timeDelayUs = DSHOT_COMMAND_DELAY_US; + timeUs_t delayAfterCommandUs = DSHOT_COMMAND_DELAY_US; switch (command) { case DSHOT_CMD_SPIN_DIRECTION_1: @@ -406,24 +431,24 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bo case DSHOT_CMD_SPIN_DIRECTION_NORMAL: case DSHOT_CMD_SPIN_DIRECTION_REVERSED: repeats = 10; - timeDelayUs = DSHOT_COMMAND_DELAY_US; break; case DSHOT_CMD_BEACON1: case DSHOT_CMD_BEACON2: case DSHOT_CMD_BEACON3: case DSHOT_CMD_BEACON4: case DSHOT_CMD_BEACON5: - repeats = 1; - timeDelayUs = DSHOT_BEEP_DELAY_US; + delayAfterCommandUs = DSHOT_BEEP_DELAY_US; break; default: break; } if (blocking) { + delayMicroseconds(DSHOT_INITIAL_DELAY_US - DSHOT_COMMAND_DELAY_US); for (; repeats; repeats--) { + delayMicroseconds(DSHOT_COMMAND_DELAY_US); + for (uint8_t i = 0; i < motorCount; i++) { - delayMicroseconds(DSHOT_COMMAND_DELAY_US); if ((i == index) || (index == ALL_MOTORS)) { motorDmaOutput_t *const motor = getMotorDmaOutput(i); motor->requestTelemetry = true; @@ -433,24 +458,21 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bo pwmCompleteDshotMotorUpdate(0); } - delayMicroseconds(timeDelayUs); + delayMicroseconds(delayAfterCommandUs); } else { - if (!pwmIsProcessingDshotCommand()) { - for (uint8_t i = 0; i < motorCount; i++) { - if ((index == i) || (index == ALL_MOTORS)) { - dshotCommandControl.command[i] = command; - dshotCommandControl.repeats = repeats; - dshotCommandControl.nextCommandAt = timeNowUs + DSHOT_COMMAND_DELAY_US; - dshotCommandControl.delayAfterCommand = timeDelayUs; - } + dshotCommandControl.repeats = repeats; + dshotCommandControl.nextCommandAtUs = timeNowUs + DSHOT_INITIAL_DELAY_US; + dshotCommandControl.delayAfterCommandUs = delayAfterCommandUs; + for (unsigned i = 0; i < motorCount; i++) { + if (index == i || index == ALL_MOTORS) { + dshotCommandControl.command[i] = command; + } else { + dshotCommandControl.command[i] = command; } } - } -} -FAST_RAM bool pwmIsProcessingDshotCommand(void) -{ - return dshotCommandControl.nextCommandAt; + dshotCommandControl.waitingForIdle = !allMotorsAreIdle(motorCount); + } } uint8_t pwmGetDshotCommand(uint8_t index) @@ -458,34 +480,44 @@ uint8_t pwmGetDshotCommand(uint8_t index) return dshotCommandControl.command[index]; } -bool FAST_CODE_NOINLINE pwmProcessDshotCommand(uint8_t motorCount) +FAST_CODE_NOINLINE bool pwmDshotCommandOutputIsEnabled(uint8_t motorCount) { timeUs_t timeNowUs = micros(); - if (cmpTimeUs(timeNowUs, dshotCommandControl.nextCommandAt) < 0) { - return false; //Skip motor update because it isn't time yet for a new command + + if (dshotCommandControl.waitingForIdle) { + if (allMotorsAreIdle(motorCount)) { + dshotCommandControl.nextCommandAtUs = timeNowUs + DSHOT_INITIAL_DELAY_US; + dshotCommandControl.waitingForIdle = false; + } + + // Send normal motor output while waiting for motors to go idle + return true; + } + + if (cmpTimeUs(timeNowUs, dshotCommandControl.nextCommandAtUs) < 0) { + //Skip motor update because it isn't time yet for a new command + return false; } //Timed motor update happening with dshot command if (dshotCommandControl.repeats > 0) { dshotCommandControl.repeats--; - dshotCommandControl.nextCommandAt = timeNowUs + DSHOT_COMMAND_DELAY_US; - if (dshotCommandControl.repeats == 0) { - dshotCommandControl.nextCommandAt = timeNowUs + dshotCommandControl.delayAfterCommand; + + if (dshotCommandControl.repeats > 0) { + dshotCommandControl.nextCommandAtUs = timeNowUs + DSHOT_COMMAND_DELAY_US; + } else { + dshotCommandControl.nextCommandAtUs = timeNowUs + dshotCommandControl.delayAfterCommandUs; } } else { - for (uint8_t i = 0; i < motorCount; i++) { - dshotCommandControl.command[i] = 0; - } - dshotCommandControl.nextCommandAt = 0; - dshotCommandControl.delayAfterCommand = 0; + dshotCommandControl.nextCommandAtUs = 0; } return true; } -FAST_CODE uint16_t prepareDshotPacket(motorDmaOutput_t *const motor, const uint16_t value) +FAST_CODE uint16_t prepareDshotPacket(motorDmaOutput_t *const motor) { - uint16_t packet = (value << 1) | (motor->requestTelemetry ? 1 : 0); + uint16_t packet = (motor->value << 1) | (motor->requestTelemetry ? 1 : 0); motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row // compute checksum @@ -525,16 +557,18 @@ void servoDevInit(const servoDevConfig_t *servoConfig) IOInit(servos[servoIndex].io, OWNER_SERVO, RESOURCE_INDEX(servoIndex)); const timerHardware_t *timer = timerGetByTag(tag); -#if defined(USE_HAL_DRIVER) - IOConfigGPIOAF(servos[servoIndex].io, IOCFG_AF_PP, timer->alternateFunction); -#else - IOConfigGPIO(servos[servoIndex].io, IOCFG_AF_PP); -#endif if (timer == NULL) { /* flag failure and disable ability to arm */ break; } + +#if defined(STM32F1) + IOConfigGPIO(servos[servoIndex].io, IOCFG_AF_PP); +#else + IOConfigGPIOAF(servos[servoIndex].io, IOCFG_AF_PP, timer->alternateFunction); +#endif + pwmOutConfig(&servos[servoIndex].channel, timer, PWM_TIMER_1MHZ, PWM_TIMER_1MHZ / servoConfig->servoPwmRate, servoConfig->servoCenterPulse, 0); servos[servoIndex].enabled = true; } diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 74f1594632..a1dbe30ee9 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -191,7 +191,7 @@ bool isMotorProtocolDshot(void); #ifdef USE_DSHOT typedef uint8_t loadDmaBufferFn(uint32_t *dmaBuffer, int stride, uint16_t packet); // function pointer used to encode a digital motor value into the DMA buffer representation -uint16_t prepareDshotPacket(motorDmaOutput_t *const motor, uint16_t value); +uint16_t prepareDshotPacket(motorDmaOutput_t *const motor); extern loadDmaBufferFn *loadDmaBuffer; @@ -201,9 +201,11 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bo void pwmWriteDshotInt(uint8_t index, uint16_t value); void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output); void pwmCompleteDshotMotorUpdate(uint8_t motorCount); -bool pwmIsProcessingDshotCommand(void); + +bool pwmDshotCommandIsQueued(void); +bool pwmDshotCommandIsProcessing(void); uint8_t pwmGetDshotCommand(uint8_t index); -bool pwmProcessDshotCommand(uint8_t motorCount); +bool pwmDshotCommandOutputIsEnabled(uint8_t motorCount); #endif diff --git a/src/main/drivers/pwm_output_dshot.c b/src/main/drivers/pwm_output_dshot.c index 43b94480fc..ee81dffb60 100644 --- a/src/main/drivers/pwm_output_dshot.c +++ b/src/main/drivers/pwm_output_dshot.c @@ -70,12 +70,16 @@ void pwmWriteDshotInt(uint8_t index, uint16_t value) } /*If there is a command ready to go overwrite the value and send that instead*/ - if (pwmIsProcessingDshotCommand()) { + if (pwmDshotCommandIsProcessing()) { value = pwmGetDshotCommand(index); - motor->requestTelemetry = true; + if (value) { + motor->requestTelemetry = true; + } } - - uint16_t packet = prepareDshotPacket(motor, value); + + motor->value = value; + + uint16_t packet = prepareDshotPacket(motor); uint8_t bufferSize; #ifdef USE_DSHOT_DMAR @@ -97,8 +101,10 @@ void pwmCompleteDshotMotorUpdate(uint8_t motorCount) UNUSED(motorCount); /* If there is a dshot command loaded up, time it correctly with motor update*/ - if (!pwmProcessDshotCommand(motorCount)) { - return; //Skip motor update + if (pwmDshotCommandIsQueued()) { + if (!pwmDshotCommandOutputIsEnabled(motorCount)) { + return; + } } for (int i = 0; i < dmaMotorTimerCount; i++) { diff --git a/src/main/drivers/pwm_output_dshot_hal.c b/src/main/drivers/pwm_output_dshot_hal.c index e0ba048639..6c62baff06 100644 --- a/src/main/drivers/pwm_output_dshot_hal.c +++ b/src/main/drivers/pwm_output_dshot_hal.c @@ -63,12 +63,16 @@ FAST_CODE void pwmWriteDshotInt(uint8_t index, uint16_t value) } /*If there is a command ready to go overwrite the value and send that instead*/ - if (pwmIsProcessingDshotCommand()) { + if (pwmDshotCommandIsProcessing()) { value = pwmGetDshotCommand(index); - motor->requestTelemetry = true; + if (value) { + motor->requestTelemetry = true; + } } - uint16_t packet = prepareDshotPacket(motor, value); + motor->value = value; + + uint16_t packet = prepareDshotPacket(motor); uint8_t bufferSize; #ifdef USE_DSHOT_DMAR @@ -90,8 +94,8 @@ FAST_CODE void pwmCompleteDshotMotorUpdate(uint8_t motorCount) UNUSED(motorCount); /* If there is a dshot command loaded up, time it correctly with motor update*/ - if (pwmIsProcessingDshotCommand()) { - if (!pwmProcessDshotCommand(motorCount)) { + if (pwmDshotCommandIsQueued()) { + if (!pwmDshotCommandOutputIsEnabled(motorCount)) { return; } } diff --git a/src/main/drivers/resource.c b/src/main/drivers/resource.c index 91161e028f..c1bab637dc 100644 --- a/src/main/drivers/resource.c +++ b/src/main/drivers/resource.c @@ -49,11 +49,11 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = { "SDCARD_DETECT", "FLASH_CS", "BARO_CS", - "MPU_CS", + "GYRO_CS", "OSD_CS", "RX_SPI_CS", "SPI_CS", - "MPU_EXTI", + "GYRO_EXTI", "BARO_EXTI", "COMPASS_EXTI", "USB", diff --git a/src/main/drivers/resource.h b/src/main/drivers/resource.h index 8b8daaf6a8..54c66da9da 100644 --- a/src/main/drivers/resource.h +++ b/src/main/drivers/resource.h @@ -49,11 +49,11 @@ typedef enum { OWNER_SDCARD_DETECT, OWNER_FLASH_CS, OWNER_BARO_CS, - OWNER_MPU_CS, + OWNER_GYRO_CS, OWNER_OSD_CS, OWNER_RX_SPI_CS, OWNER_SPI_CS, - OWNER_MPU_EXTI, + OWNER_GYRO_EXTI, OWNER_BARO_EXTI, OWNER_COMPASS_EXTI, OWNER_USB, diff --git a/src/main/drivers/rx/rx_cc2500.c b/src/main/drivers/rx/rx_cc2500.c index 39d576d67f..ace926d147 100644 --- a/src/main/drivers/rx/rx_cc2500.c +++ b/src/main/drivers/rx/rx_cc2500.c @@ -43,30 +43,28 @@ #define NOP 0xFF -uint8_t cc2500ReadFifo(uint8_t *dpbuffer, uint8_t len) +void cc2500ReadFifo(uint8_t *dpbuffer, uint8_t len) { - return rxSpiReadCommandMulti(CC2500_3F_RXFIFO | CC2500_READ_BURST, NOP, dpbuffer, len); + rxSpiReadCommandMulti(CC2500_3F_RXFIFO | CC2500_READ_BURST, NOP, dpbuffer, len); } -uint8_t cc2500WriteFifo(uint8_t *dpbuffer, uint8_t len) +void cc2500WriteFifo(uint8_t *dpbuffer, uint8_t len) { - uint8_t ret; cc2500Strobe(CC2500_SFTX); // 0x3B SFTX - ret = rxSpiWriteCommandMulti(CC2500_3F_TXFIFO | CC2500_WRITE_BURST, + rxSpiWriteCommandMulti(CC2500_3F_TXFIFO | CC2500_WRITE_BURST, dpbuffer, len); cc2500Strobe(CC2500_STX); // 0x35 - return ret; } -uint8_t cc2500ReadRegisterMulti(uint8_t address, uint8_t *data, uint8_t length) +void cc2500ReadRegisterMulti(uint8_t address, uint8_t *data, uint8_t length) { - return rxSpiReadCommandMulti(address, NOP, data, length); + rxSpiReadCommandMulti(address, NOP, data, length); } -uint8_t cc2500WriteRegisterMulti(uint8_t address, uint8_t *data, +void cc2500WriteRegisterMulti(uint8_t address, uint8_t *data, uint8_t length) { - return rxSpiWriteCommandMulti(address, data, length); + rxSpiWriteCommandMulti(address, data, length); } uint8_t cc2500ReadReg(uint8_t reg) @@ -76,9 +74,9 @@ uint8_t cc2500ReadReg(uint8_t reg) void cc2500Strobe(uint8_t address) { rxSpiWriteByte(address); } -uint8_t cc2500WriteReg(uint8_t address, uint8_t data) +void cc2500WriteReg(uint8_t address, uint8_t data) { - return rxSpiWriteCommand(address, data); + rxSpiWriteCommand(address, data); } void cc2500SetPower(uint8_t power) diff --git a/src/main/drivers/rx/rx_cc2500.h b/src/main/drivers/rx/rx_cc2500.h index 69bdf98296..aa2edeb8c3 100644 --- a/src/main/drivers/rx/rx_cc2500.h +++ b/src/main/drivers/rx/rx_cc2500.h @@ -157,16 +157,16 @@ enum { #define CC2500_LQI_CRC_OK_BM 0x80 #define CC2500_LQI_EST_BM 0x7F -uint8_t cc2500ReadFifo(uint8_t *dpbuffer, uint8_t len); -uint8_t cc2500WriteFifo(uint8_t *dpbuffer, uint8_t len); +void cc2500ReadFifo(uint8_t *dpbuffer, uint8_t len); +void cc2500WriteFifo(uint8_t *dpbuffer, uint8_t len); -uint8_t cc2500ReadRegisterMulti(uint8_t address, uint8_t *data, +void cc2500ReadRegisterMulti(uint8_t address, uint8_t *data, uint8_t length); -uint8_t cc2500WriteRegisterMulti(uint8_t address, uint8_t *data, +void cc2500WriteRegisterMulti(uint8_t address, uint8_t *data, uint8_t length); uint8_t cc2500ReadReg(uint8_t reg); void cc2500Strobe(uint8_t address); -uint8_t cc2500WriteReg(uint8_t address, uint8_t data); +void cc2500WriteReg(uint8_t address, uint8_t data); void cc2500SetPower(uint8_t power); uint8_t cc2500Reset(void); diff --git a/src/main/drivers/rx/rx_nrf24l01.c b/src/main/drivers/rx/rx_nrf24l01.c index 8c5d3b9c7c..33e84ebf9e 100644 --- a/src/main/drivers/rx/rx_nrf24l01.c +++ b/src/main/drivers/rx/rx_nrf24l01.c @@ -69,14 +69,14 @@ static void NRF24L01_InitGpio(void) NRF24_CE_LO(); } -uint8_t NRF24L01_WriteReg(uint8_t reg, uint8_t data) +void NRF24L01_WriteReg(uint8_t reg, uint8_t data) { - return rxSpiWriteCommand(W_REGISTER | (REGISTER_MASK & reg), data); + rxSpiWriteCommand(W_REGISTER | (REGISTER_MASK & reg), data); } -uint8_t NRF24L01_WriteRegisterMulti(uint8_t reg, const uint8_t *data, uint8_t length) +void NRF24L01_WriteRegisterMulti(uint8_t reg, const uint8_t *data, uint8_t length) { - return rxSpiWriteCommandMulti(W_REGISTER | ( REGISTER_MASK & reg), data, length); + rxSpiWriteCommandMulti(W_REGISTER | ( REGISTER_MASK & reg), data, length); } /* @@ -84,14 +84,14 @@ uint8_t NRF24L01_WriteRegisterMulti(uint8_t reg, const uint8_t *data, uint8_t le * Packets in the TX FIFO are transmitted when the * nRF24L01 next enters TX mode */ -uint8_t NRF24L01_WritePayload(const uint8_t *data, uint8_t length) +void NRF24L01_WritePayload(const uint8_t *data, uint8_t length) { - return rxSpiWriteCommandMulti(W_TX_PAYLOAD, data, length); + rxSpiWriteCommandMulti(W_TX_PAYLOAD, data, length); } -uint8_t NRF24L01_WriteAckPayload(const uint8_t *data, uint8_t length, uint8_t pipe) +void NRF24L01_WriteAckPayload(const uint8_t *data, uint8_t length, uint8_t pipe) { - return rxSpiWriteCommandMulti(W_ACK_PAYLOAD | (pipe & 0x07), data, length); + rxSpiWriteCommandMulti(W_ACK_PAYLOAD | (pipe & 0x07), data, length); } uint8_t NRF24L01_ReadReg(uint8_t reg) @@ -99,17 +99,17 @@ uint8_t NRF24L01_ReadReg(uint8_t reg) return rxSpiReadCommand(R_REGISTER | (REGISTER_MASK & reg), NOP); } -uint8_t NRF24L01_ReadRegisterMulti(uint8_t reg, uint8_t *data, uint8_t length) +void NRF24L01_ReadRegisterMulti(uint8_t reg, uint8_t *data, uint8_t length) { - return rxSpiReadCommandMulti(R_REGISTER | (REGISTER_MASK & reg), NOP, data, length); + rxSpiReadCommandMulti(R_REGISTER | (REGISTER_MASK & reg), NOP, data, length); } /* * Read a packet from the nRF24L01 RX FIFO. */ -uint8_t NRF24L01_ReadPayload(uint8_t *data, uint8_t length) +void NRF24L01_ReadPayload(uint8_t *data, uint8_t length) { - return rxSpiReadCommandMulti(R_RX_PAYLOAD, NOP, data, length); + rxSpiReadCommandMulti(R_RX_PAYLOAD, NOP, data, length); } /* @@ -128,9 +128,9 @@ void NRF24L01_FlushRx(void) rxSpiWriteByte(FLUSH_RX); } -uint8_t NRF24L01_Activate(uint8_t code) +void NRF24L01_Activate(uint8_t code) { - return rxSpiWriteCommand(ACTIVATE, code); + rxSpiWriteCommand(ACTIVATE, code); } // standby configuration, used to simplify switching between RX, TX, and Standby modes diff --git a/src/main/drivers/rx/rx_nrf24l01.h b/src/main/drivers/rx/rx_nrf24l01.h index ae252be6e6..64ba5ffffc 100644 --- a/src/main/drivers/rx/rx_nrf24l01.h +++ b/src/main/drivers/rx/rx_nrf24l01.h @@ -179,20 +179,20 @@ enum { }; void NRF24L01_Initialize(uint8_t baseConfig); -uint8_t NRF24L01_WriteReg(uint8_t reg, uint8_t data); -uint8_t NRF24L01_WriteRegisterMulti(uint8_t reg, const uint8_t *data, uint8_t length); -uint8_t NRF24L01_WritePayload(const uint8_t *data, uint8_t length); -uint8_t NRF24L01_WriteAckPayload(const uint8_t *data, uint8_t length, uint8_t pipe); +void NRF24L01_WriteReg(uint8_t reg, uint8_t data); +void NRF24L01_WriteRegisterMulti(uint8_t reg, const uint8_t *data, uint8_t length); +void NRF24L01_WritePayload(const uint8_t *data, uint8_t length); +void NRF24L01_WriteAckPayload(const uint8_t *data, uint8_t length, uint8_t pipe); uint8_t NRF24L01_ReadReg(uint8_t reg); -uint8_t NRF24L01_ReadRegisterMulti(uint8_t reg, uint8_t *data, uint8_t length); -uint8_t NRF24L01_ReadPayload(uint8_t *data, uint8_t length); +void NRF24L01_ReadRegisterMulti(uint8_t reg, uint8_t *data, uint8_t length); +void NRF24L01_ReadPayload(uint8_t *data, uint8_t length); // Utility functions void NRF24L01_FlushTx(void); void NRF24L01_FlushRx(void); -uint8_t NRF24L01_Activate(uint8_t code); +void NRF24L01_Activate(uint8_t code); void NRF24L01_SetupBasic(void); void NRF24L01_SetStandbyMode(void); diff --git a/src/main/drivers/rx/rx_spi.c b/src/main/drivers/rx/rx_spi.c index b4120a9f5c..39a9545233 100644 --- a/src/main/drivers/rx/rx_spi.c +++ b/src/main/drivers/rx/rx_spi.c @@ -41,12 +41,12 @@ #include "rx_spi.h" +#define ENABLE_RX() IOLo(busdev->busdev_u.spi.csnPin) +#define DISABLE_RX() IOHi(busdev->busdev_u.spi.csnPin) + static busDevice_t rxSpiDevice; static busDevice_t *busdev = &rxSpiDevice; -#define DISABLE_RX() {IOHi(busdev->busdev_u.spi.csnPin);} -#define ENABLE_RX() {IOLo(busdev->busdev_u.spi.csnPin);} - bool rxSpiDeviceInit(const rxSpiConfig_t *rxSpiConfig) { if (!rxSpiConfig->spibus) { @@ -60,7 +60,7 @@ bool rxSpiDeviceInit(const rxSpiConfig_t *rxSpiConfig) IOConfigGPIO(rxCsPin, SPI_IO_CS_CFG); busdev->busdev_u.spi.csnPin = rxCsPin; - DISABLE_RX(); + IOHi(rxCsPin); spiSetDivisor(busdev->busdev_u.spi.instance, SPI_CLOCK_STANDARD); @@ -69,54 +69,33 @@ bool rxSpiDeviceInit(const rxSpiConfig_t *rxSpiConfig) uint8_t rxSpiTransferByte(uint8_t data) { - return spiTransferByte(busdev->busdev_u.spi.instance, data); + return spiBusTransferByte(busdev, data); } -uint8_t rxSpiWriteByte(uint8_t data) +void rxSpiWriteByte(uint8_t data) { - ENABLE_RX(); - const uint8_t ret = rxSpiTransferByte(data); - DISABLE_RX(); - return ret; + spiBusWriteByte(busdev, data); } -uint8_t rxSpiWriteCommand(uint8_t command, uint8_t data) +void rxSpiWriteCommand(uint8_t command, uint8_t data) { - ENABLE_RX(); - const uint8_t ret = rxSpiTransferByte(command); - rxSpiTransferByte(data); - DISABLE_RX(); - return ret; + spiBusWriteRegister(busdev, command, data); } -uint8_t rxSpiWriteCommandMulti(uint8_t command, const uint8_t *data, uint8_t length) +void rxSpiWriteCommandMulti(uint8_t command, const uint8_t *data, uint8_t length) { - ENABLE_RX(); - const uint8_t ret = rxSpiTransferByte(command); - for (uint8_t i = 0; i < length; i++) { - rxSpiTransferByte(data[i]); - } - DISABLE_RX(); - return ret; + spiBusWriteRegisterBuffer(busdev, command, data, length); } uint8_t rxSpiReadCommand(uint8_t command, uint8_t data) { - ENABLE_RX(); - rxSpiTransferByte(command); - const uint8_t ret = rxSpiTransferByte(data); - DISABLE_RX(); - return ret; + UNUSED(data); + return spiBusRawReadRegister(busdev, command); } -uint8_t rxSpiReadCommandMulti(uint8_t command, uint8_t commandData, uint8_t *retData, uint8_t length) +void rxSpiReadCommandMulti(uint8_t command, uint8_t commandData, uint8_t *retData, uint8_t length) { - ENABLE_RX(); - const uint8_t ret = rxSpiTransferByte(command); - for (uint8_t i = 0; i < length; i++) { - retData[i] = rxSpiTransferByte(commandData); - } - DISABLE_RX(); - return ret; + UNUSED(commandData); + spiBusRawReadRegisterBuffer(busdev, command, retData, length); } #endif diff --git a/src/main/drivers/rx/rx_spi.h b/src/main/drivers/rx/rx_spi.h index f54a50693d..d3250ac018 100644 --- a/src/main/drivers/rx/rx_spi.h +++ b/src/main/drivers/rx/rx_spi.h @@ -28,8 +28,8 @@ struct rxSpiConfig_s; bool rxSpiDeviceInit(const struct rxSpiConfig_s *rxSpiConfig); uint8_t rxSpiTransferByte(uint8_t data); -uint8_t rxSpiWriteByte(uint8_t data); -uint8_t rxSpiWriteCommand(uint8_t command, uint8_t data); -uint8_t rxSpiWriteCommandMulti(uint8_t command, const uint8_t *data, uint8_t length); +void rxSpiWriteByte(uint8_t data); +void rxSpiWriteCommand(uint8_t command, uint8_t data); +void rxSpiWriteCommandMulti(uint8_t command, const uint8_t *data, uint8_t length); uint8_t rxSpiReadCommand(uint8_t command, uint8_t commandData); -uint8_t rxSpiReadCommandMulti(uint8_t command, uint8_t commandData, uint8_t *retData, uint8_t length); +void rxSpiReadCommandMulti(uint8_t command, uint8_t commandData, uint8_t *retData, uint8_t length); diff --git a/src/main/drivers/rx/rx_xn297.c b/src/main/drivers/rx/rx_xn297.c index 782413ba90..a925d7668c 100644 --- a/src/main/drivers/rx/rx_xn297.c +++ b/src/main/drivers/rx/rx_xn297.c @@ -26,6 +26,8 @@ #include "platform.h" +#if defined(USE_RX_XN297) + #include "common/crc.h" #include "pg/rx.h" @@ -77,7 +79,7 @@ uint16_t XN297_UnscramblePayload(uint8_t *data, int len, const uint8_t *rxAddr) return crc; } -uint8_t XN297_WritePayload(uint8_t *data, int len, const uint8_t *rxAddr) +void XN297_WritePayload(uint8_t *data, int len, const uint8_t *rxAddr) { uint8_t packet[NRF24L01_MAX_PAYLOAD_SIZE]; uint16_t crc = 0xb5d2; @@ -93,5 +95,6 @@ uint8_t XN297_WritePayload(uint8_t *data, int len, const uint8_t *rxAddr) crc ^= xn297_crc_xorout[len]; packet[RX_TX_ADDR_LEN + len] = crc >> 8; packet[RX_TX_ADDR_LEN + len + 1] = crc & 0xff; - return NRF24L01_WritePayload(packet, RX_TX_ADDR_LEN + len + 2); + NRF24L01_WritePayload(packet, RX_TX_ADDR_LEN + len + 2); } +#endif diff --git a/src/main/drivers/rx/rx_xn297.h b/src/main/drivers/rx/rx_xn297.h index eeea848791..c16bb5f0c0 100644 --- a/src/main/drivers/rx/rx_xn297.h +++ b/src/main/drivers/rx/rx_xn297.h @@ -24,4 +24,4 @@ #include uint16_t XN297_UnscramblePayload(uint8_t* data, int len, const uint8_t *rxAddr); -uint8_t XN297_WritePayload(uint8_t *data, int len, const uint8_t *rxAddr); +void XN297_WritePayload(uint8_t *data, int len, const uint8_t *rxAddr); diff --git a/src/main/drivers/sdcard.c b/src/main/drivers/sdcard.c index 1664ea22a2..d27fb4cfe5 100644 --- a/src/main/drivers/sdcard.c +++ b/src/main/drivers/sdcard.c @@ -32,16 +32,16 @@ #include "drivers/bus_spi.h" #include "drivers/time.h" +#include "pg/sdcard.h" + #include "sdcard.h" +#include "sdcard_impl.h" #include "sdcard_standard.h" #ifdef AFATFS_USE_INTROSPECTIVE_LOGGING #define SDCARD_PROFILING #endif -#define SET_CS_HIGH IOHi(sdcard.chipSelectPin) -#define SET_CS_LOW IOLo(sdcard.chipSelectPin) - #define SDCARD_INIT_NUM_DUMMY_BYTES 10 #define SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY 8 // Chosen so that CMD8 will have the same CRC as CMD0: @@ -50,70 +50,18 @@ #define SDCARD_TIMEOUT_INIT_MILLIS 200 #define SDCARD_MAX_CONSECUTIVE_FAILURES 8 +/* SPI_CLOCK_INITIALIZATION (256) is the slowest (Spec calls for under 400KHz) */ +#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER SPI_CLOCK_INITIALIZATION + +/* Operational speed <= 25MHz */ +#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER SPI_CLOCK_FAST + /* Break up 512-byte SD card sectors into chunks of this size when writing without DMA to reduce the peak overhead * per call to sdcard_poll(). */ #define SDCARD_NON_DMA_CHUNK_SIZE 256 -typedef enum { - // In these states we run at the initialization 400kHz clockspeed: - SDCARD_STATE_NOT_PRESENT = 0, - SDCARD_STATE_RESET, - SDCARD_STATE_CARD_INIT_IN_PROGRESS, - SDCARD_STATE_INITIALIZATION_RECEIVE_CID, - - // In these states we run at full clock speed - SDCARD_STATE_READY, - SDCARD_STATE_READING, - SDCARD_STATE_SENDING_WRITE, - SDCARD_STATE_WAITING_FOR_WRITE, - SDCARD_STATE_WRITING_MULTIPLE_BLOCKS, - SDCARD_STATE_STOPPING_MULTIPLE_BLOCK_WRITE -} sdcardState_e; - -typedef struct sdcard_t { - struct { - uint8_t *buffer; - uint32_t blockIndex; - uint8_t chunkIndex; - - sdcard_operationCompleteCallback_c callback; - uint32_t callbackData; - -#ifdef SDCARD_PROFILING - uint32_t profileStartTime; -#endif - } pendingOperation; - - uint32_t operationStartTime; - - uint8_t failureCount; - - uint8_t version; - bool highCapacity; - - uint32_t multiWriteNextBlock; - uint32_t multiWriteBlocksRemain; - - sdcardState_e state; - - sdcardMetadata_t metadata; - sdcardCSD_t csd; - -#ifdef SDCARD_PROFILING - sdcard_profilerCallback_c profiler; -#endif - SPI_TypeDef *instance; - bool enabled; - bool detectionInverted; - bool useDMAForTx; - IO_t cardDetectPin; - IO_t chipSelectPin; - dmaChannelDescriptor_t * dma; - uint8_t dmaChannel; -} sdcard_t; - -static sdcard_t sdcard; +sdcard_t sdcard; STATIC_ASSERT(sizeof(sdcardCSD_t) == 16, sdcard_csd_bitfields_didnt_pack_properly); @@ -149,1019 +97,75 @@ bool sdcard_isInserted(void) } /** - * Returns true if the card has already been, or is currently, initializing and hasn't encountered enough errors to - * trip our error threshold and be disabled (i.e. our card is in and working!) + * Dispatch */ -bool sdcard_isFunctional(void) -{ - return sdcard.state != SDCARD_STATE_NOT_PRESENT; -} +sdcardVTable_t *sdcardVTable; -static void sdcard_select(void) -{ - SET_CS_LOW; -} - -static void sdcard_deselect(void) -{ - // As per the SD-card spec, give the card 8 dummy clocks so it can finish its operation - //spiTransferByte(sdcard.instance, 0xFF); - - while (spiIsBusBusy(sdcard.instance)) { - } - - SET_CS_HIGH; -} - -/** - * Handle a failure of an SD card operation by resetting the card back to its initialization phase. - * - * Increments the failure counter, and when the failure threshold is reached, disables the card until - * the next call to sdcard_init(). - */ -static void sdcard_reset(void) -{ - if (!sdcard_isInserted()) { - sdcard.state = SDCARD_STATE_NOT_PRESENT; - return; - } - - if (sdcard.state >= SDCARD_STATE_READY) { - spiSetDivisor(sdcard.instance, SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER); - } - - sdcard.failureCount++; - if (sdcard.failureCount >= SDCARD_MAX_CONSECUTIVE_FAILURES) { - sdcard.state = SDCARD_STATE_NOT_PRESENT; - } else { - sdcard.operationStartTime = millis(); - sdcard.state = SDCARD_STATE_RESET; - } -} - -/** - * The SD card spec requires 8 clock cycles to be sent by us on the bus after most commands so it can finish its - * processing of that command. The easiest way for us to do this is to just wait for the bus to become idle before - * we transmit a command, sending at least 8-bits onto the bus when we do so. - */ -static bool sdcard_waitForIdle(int maxBytesToWait) -{ - while (maxBytesToWait > 0) { - uint8_t b = spiTransferByte(sdcard.instance, 0xFF); - if (b == 0xFF) { - return true; - } - maxBytesToWait--; - } - - return false; -} - -/** - * Wait for up to maxDelay 0xFF idle bytes to arrive from the card, returning the first non-idle byte found. - * - * Returns 0xFF on failure. - */ -static uint8_t sdcard_waitForNonIdleByte(int maxDelay) -{ - for (int i = 0; i < maxDelay + 1; i++) { // + 1 so we can wait for maxDelay '0xFF' bytes before reading a response byte afterwards - uint8_t response = spiTransferByte(sdcard.instance, 0xFF); - - if (response != 0xFF) { - return response; - } - } - - return 0xFF; -} - -/** - * Waits up to SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY bytes for the card to become ready, send a command to the card - * with the given argument, waits up to SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY bytes for a reply, and returns the - * first non-0xFF byte of the reply. - * - * You must select the card first with sdcard_select() and deselect it afterwards with sdcard_deselect(). - * - * Upon failure, 0xFF is returned. - */ -static uint8_t sdcard_sendCommand(uint8_t commandCode, uint32_t commandArgument) -{ - const uint8_t command[6] = { - 0x40 | commandCode, - commandArgument >> 24, - commandArgument >> 16, - commandArgument >> 8, - commandArgument, - 0x95 /* Static CRC. This CRC is valid for CMD0 with a 0 argument, and CMD8 with 0x1AB argument, which are the only - commands that require a CRC */ - }; - - // Go ahead and send the command even if the card isn't idle if this is the reset command - if (!sdcard_waitForIdle(SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY) && commandCode != SDCARD_COMMAND_GO_IDLE_STATE) - return 0xFF; - - spiTransfer(sdcard.instance, command, NULL, sizeof(command)); - - /* - * The card can take up to SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY bytes to send the response, in the meantime - * it'll transmit 0xFF filler bytes. - */ - return sdcard_waitForNonIdleByte(SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY); -} - -static uint8_t sdcard_sendAppCommand(uint8_t commandCode, uint32_t commandArgument) -{ - sdcard_sendCommand(SDCARD_COMMAND_APP_CMD, 0); - - return sdcard_sendCommand(commandCode, commandArgument); -} - -/** - * Sends an IF_COND message to the card to check its version and validate its voltage requirements. Sets the global - * sdCardVersion with the detected version (0, 1, or 2) and returns true if the card is compatible. - */ -static bool sdcard_validateInterfaceCondition(void) -{ - uint8_t ifCondReply[4]; - - sdcard.version = 0; - - sdcard_select(); - - uint8_t status = sdcard_sendCommand(SDCARD_COMMAND_SEND_IF_COND, (SDCARD_VOLTAGE_ACCEPTED_2_7_to_3_6 << 8) | SDCARD_IF_COND_CHECK_PATTERN); - - // Don't deselect the card right away, because we'll want to read the rest of its reply if it's a V2 card - - if (status == (SDCARD_R1_STATUS_BIT_ILLEGAL_COMMAND | SDCARD_R1_STATUS_BIT_IDLE)) { - // V1 cards don't support this command - sdcard.version = 1; - } else if (status == SDCARD_R1_STATUS_BIT_IDLE) { - spiTransfer(sdcard.instance, NULL, ifCondReply, sizeof(ifCondReply)); - - /* - * We don't bother to validate the SDCard's operating voltage range since the spec requires it to accept our - * 3.3V, but do check that it echoed back our check pattern properly. - */ - if (ifCondReply[3] == SDCARD_IF_COND_CHECK_PATTERN) { - sdcard.version = 2; - } - } - - sdcard_deselect(); - - return sdcard.version > 0; -} - -static bool sdcard_readOCRRegister(uint32_t *result) -{ - sdcard_select(); - - uint8_t status = sdcard_sendCommand(SDCARD_COMMAND_READ_OCR, 0); - - uint8_t response[4]; - - spiTransfer(sdcard.instance, NULL, response, sizeof(response)); - - if (status == 0) { - sdcard_deselect(); - - *result = (response[0] << 24) | (response[1] << 16) | (response[2] << 8) | response[3]; - - return true; - } else { - sdcard_deselect(); - - return false; - } -} - -typedef enum { - SDCARD_RECEIVE_SUCCESS, - SDCARD_RECEIVE_BLOCK_IN_PROGRESS, - SDCARD_RECEIVE_ERROR -} sdcardReceiveBlockStatus_e; - -/** - * Attempt to receive a data block from the SD card. - * - * Return true on success, otherwise the card has not responded yet and you should retry later. - */ -static sdcardReceiveBlockStatus_e sdcard_receiveDataBlock(uint8_t *buffer, int count) -{ - uint8_t dataToken = sdcard_waitForNonIdleByte(8); - - if (dataToken == 0xFF) { - return SDCARD_RECEIVE_BLOCK_IN_PROGRESS; - } - - if (dataToken != SDCARD_SINGLE_BLOCK_READ_START_TOKEN) { - return SDCARD_RECEIVE_ERROR; - } - - spiTransfer(sdcard.instance, NULL, buffer, count); - - // Discard trailing CRC, we don't care - spiTransferByte(sdcard.instance, 0xFF); - spiTransferByte(sdcard.instance, 0xFF); - - return SDCARD_RECEIVE_SUCCESS; -} - -static bool sdcard_sendDataBlockFinish(void) -{ -#ifdef USE_HAL_DRIVER - // Drain anything left in the Rx FIFO (we didn't read it during the write) - //This is necessary here as when using msc there is timing issue - while (LL_SPI_IsActiveFlag_RXNE(sdcard.instance)) { - sdcard.instance->DR; - } -#endif - - // Send a dummy CRC - spiTransferByte(sdcard.instance, 0x00); - spiTransferByte(sdcard.instance, 0x00); - - uint8_t dataResponseToken = spiTransferByte(sdcard.instance, 0xFF); - - /* - * Check if the card accepted the write (no CRC error / no address error) - * - * The lower 5 bits are structured as follows: - * | 0 | Status | 1 | - * | 0 | x x x | 1 | - * - * Statuses: - * 010 - Data accepted - * 101 - CRC error - * 110 - Write error - */ - return (dataResponseToken & 0x1F) == 0x05; -} - -/** - * Begin sending a buffer of SDCARD_BLOCK_SIZE bytes to the SD card. - */ -static void sdcard_sendDataBlockBegin(const uint8_t *buffer, bool multiBlockWrite) -{ - // Card wants 8 dummy clock cycles between the write command's response and a data block beginning: - spiTransferByte(sdcard.instance, 0xFF); - - spiTransferByte(sdcard.instance, multiBlockWrite ? SDCARD_MULTIPLE_BLOCK_WRITE_START_TOKEN : SDCARD_SINGLE_BLOCK_WRITE_START_TOKEN); - - if (sdcard.useDMAForTx) { -#if defined(USE_HAL_DRIVER) - LL_DMA_InitTypeDef init; - - LL_DMA_StructInit(&init); - - init.Channel = dmaGetChannel(sdcard.dmaChannel); - init.Mode = LL_DMA_MODE_NORMAL; - init.Direction = LL_DMA_DIRECTION_MEMORY_TO_PERIPH; - - init.PeriphOrM2MSrcAddress = (uint32_t)&sdcard.instance->DR; - init.Priority = LL_DMA_PRIORITY_LOW; - init.PeriphOrM2MSrcIncMode = LL_DMA_PERIPH_NOINCREMENT; - init.PeriphOrM2MSrcDataSize = LL_DMA_PDATAALIGN_BYTE; - - init.MemoryOrM2MDstAddress = (uint32_t)buffer; - init.MemoryOrM2MDstIncMode = LL_DMA_MEMORY_INCREMENT; - init.MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_BYTE; - - init.NbData = SDCARD_BLOCK_SIZE; - - LL_DMA_DeInit(sdcard.dma->dma, sdcard.dma->stream); - LL_DMA_Init(sdcard.dma->dma, sdcard.dma->stream, &init); - - LL_DMA_EnableStream(sdcard.dma->dma, sdcard.dma->stream); - - LL_SPI_EnableDMAReq_TX(sdcard.instance); - -#else - - DMA_InitTypeDef init; - - DMA_StructInit(&init); -#ifdef STM32F4 - init.DMA_Channel = dmaGetChannel(sdcard.dmaChannel); - init.DMA_Memory0BaseAddr = (uint32_t) buffer; - init.DMA_DIR = DMA_DIR_MemoryToPeripheral; -#else - init.DMA_M2M = DMA_M2M_Disable; - init.DMA_MemoryBaseAddr = (uint32_t) buffer; - init.DMA_DIR = DMA_DIR_PeripheralDST; -#endif - init.DMA_PeripheralBaseAddr = (uint32_t) &sdcard.instance->DR; - init.DMA_Priority = DMA_Priority_Low; - init.DMA_PeripheralInc = DMA_PeripheralInc_Disable; - init.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; - - init.DMA_MemoryInc = DMA_MemoryInc_Enable; - init.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; - - init.DMA_BufferSize = SDCARD_BLOCK_SIZE; - init.DMA_Mode = DMA_Mode_Normal; - - DMA_DeInit(sdcard.dma->ref); - DMA_Init(sdcard.dma->ref, &init); - - DMA_Cmd(sdcard.dma->ref, ENABLE); - - SPI_I2S_DMACmd(sdcard.instance, SPI_I2S_DMAReq_Tx, ENABLE); -#endif - } else { - // Send the first chunk now - spiTransfer(sdcard.instance, buffer, NULL, SDCARD_NON_DMA_CHUNK_SIZE); - } -} - -static bool sdcard_receiveCID(void) -{ - uint8_t cid[16]; - - if (sdcard_receiveDataBlock(cid, sizeof(cid)) != SDCARD_RECEIVE_SUCCESS) { - return false; - } - - sdcard.metadata.manufacturerID = cid[0]; - sdcard.metadata.oemID = (cid[1] << 8) | cid[2]; - sdcard.metadata.productName[0] = cid[3]; - sdcard.metadata.productName[1] = cid[4]; - sdcard.metadata.productName[2] = cid[5]; - sdcard.metadata.productName[3] = cid[6]; - sdcard.metadata.productName[4] = cid[7]; - sdcard.metadata.productRevisionMajor = cid[8] >> 4; - sdcard.metadata.productRevisionMinor = cid[8] & 0x0F; - sdcard.metadata.productSerial = (cid[9] << 24) | (cid[10] << 16) | (cid[11] << 8) | cid[12]; - sdcard.metadata.productionYear = (((cid[13] & 0x0F) << 4) | (cid[14] >> 4)) + 2000; - sdcard.metadata.productionMonth = cid[14] & 0x0F; - - return true; -} - -static bool sdcard_fetchCSD(void) -{ - uint32_t readBlockLen, blockCount, blockCountMult; - uint64_t capacityBytes; - - sdcard_select(); - - /* The CSD command's data block should always arrive within 8 idle clock cycles (SD card spec). This is because - * the information about card latency is stored in the CSD register itself, so we can't use that yet! - */ - bool success = - sdcard_sendCommand(SDCARD_COMMAND_SEND_CSD, 0) == 0 - && sdcard_receiveDataBlock((uint8_t*) &sdcard.csd, sizeof(sdcard.csd)) == SDCARD_RECEIVE_SUCCESS - && SDCARD_GET_CSD_FIELD(sdcard.csd, 1, TRAILER) == 1; - - if (success) { - switch (SDCARD_GET_CSD_FIELD(sdcard.csd, 1, CSD_STRUCTURE_VER)) { - case SDCARD_CSD_STRUCTURE_VERSION_1: - // Block size in bytes (doesn't have to be 512) - readBlockLen = 1 << SDCARD_GET_CSD_FIELD(sdcard.csd, 1, READ_BLOCK_LEN); - blockCountMult = 1 << (SDCARD_GET_CSD_FIELD(sdcard.csd, 1, CSIZE_MULT) + 2); - blockCount = (SDCARD_GET_CSD_FIELD(sdcard.csd, 1, CSIZE) + 1) * blockCountMult; - - // We could do this in 32 bits but it makes the 2GB case awkward - capacityBytes = (uint64_t) blockCount * readBlockLen; - - // Re-express that capacity (max 2GB) in our standard 512-byte block size - sdcard.metadata.numBlocks = capacityBytes / SDCARD_BLOCK_SIZE; - break; - case SDCARD_CSD_STRUCTURE_VERSION_2: - sdcard.metadata.numBlocks = (SDCARD_GET_CSD_FIELD(sdcard.csd, 2, CSIZE) + 1) * 1024; - break; - default: - success = false; - } - } - - sdcard_deselect(); - - return success; -} - -/** - * Check if the SD Card has completed its startup sequence. Must be called with sdcard.state == SDCARD_STATE_INITIALIZATION. - * - * Returns true if the card has finished its init process. - */ -static bool sdcard_checkInitDone(void) -{ - sdcard_select(); - - uint8_t status = sdcard_sendAppCommand(SDCARD_ACOMMAND_SEND_OP_COND, sdcard.version == 2 ? 1 << 30 /* We support high capacity cards */ : 0); - - sdcard_deselect(); - - // When card init is complete, the idle bit in the response becomes zero. - return status == 0x00; -} - -/** - * Begin the initialization process for the SD card. This must be called first before any other sdcard_ routine. - */ void sdcard_init(const sdcardConfig_t *config) { - sdcard.enabled = config->enabled; - if (!sdcard.enabled) { - sdcard.state = SDCARD_STATE_NOT_PRESENT; - return; - } - - sdcard.instance = spiInstanceByDevice(config->device); - - sdcard.useDMAForTx = config->useDma; - if (sdcard.useDMAForTx) { -#if defined(STM32F4) || defined(STM32F7) - sdcard.dmaChannel = config->dmaChannel; + switch (config->mode) { +#ifdef USE_SDCARD_SPI + case SDCARD_MODE_SPI: + sdcardVTable = &sdcardSpiVTable; + break; #endif - sdcard.dma = dmaGetDescriptorByIdentifier(config->dmaIdentifier); - dmaInit(config->dmaIdentifier, OWNER_SDCARD, 0); +#ifdef USE_SDCARD_SDIO + case SDCARD_MODE_SDIO: + sdcardVTable = &sdcardSdioVTable; + break; +#endif + default: + break; } - if (config->chipSelectTag) { - sdcard.chipSelectPin = IOGetByTag(config->chipSelectTag); - IOInit(sdcard.chipSelectPin, OWNER_SDCARD_CS, 0); - IOConfigGPIO(sdcard.chipSelectPin, SPI_IO_CS_CFG); - } else { - sdcard.chipSelectPin = IO_NONE; - } - - if (config->cardDetectTag) { - sdcard.cardDetectPin = IOGetByTag(config->cardDetectTag); - sdcard.detectionInverted = config->cardDetectInverted; - } else { - sdcard.cardDetectPin = IO_NONE; - sdcard.detectionInverted = false; - } - - // Max frequency is initially 400kHz - spiSetDivisor(sdcard.instance, SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER); - - // SDCard wants 1ms minimum delay after power is applied to it - delay(1000); - - // Transmit at least 74 dummy clock cycles with CS high so the SD card can start up - SET_CS_HIGH; - - spiTransfer(sdcard.instance, NULL, NULL, SDCARD_INIT_NUM_DUMMY_BYTES); - - // Wait for that transmission to finish before we enable the SDCard, so it receives the required number of cycles: - int time = 100000; - while (spiIsBusBusy(sdcard.instance)) { - if (time-- == 0) { - sdcard.state = SDCARD_STATE_NOT_PRESENT; - sdcard.failureCount++; - return; - } - } - - sdcard.operationStartTime = millis(); - sdcard.state = SDCARD_STATE_RESET; - sdcard.failureCount = 0; -} - -static bool sdcard_setBlockLength(uint32_t blockLen) -{ - sdcard_select(); - - uint8_t status = sdcard_sendCommand(SDCARD_COMMAND_SET_BLOCKLEN, blockLen); - - sdcard_deselect(); - - return status == 0; -} - -/* - * Returns true if the card is ready to accept read/write commands. - */ -static bool sdcard_isReady(void) -{ - return sdcard.state == SDCARD_STATE_READY || sdcard.state == SDCARD_STATE_WRITING_MULTIPLE_BLOCKS; -} - -/** - * Send the stop-transmission token to complete a multi-block write. - * - * Returns: - * SDCARD_OPERATION_IN_PROGRESS - We're now waiting for that stop to complete, the card will enter - * the SDCARD_STATE_STOPPING_MULTIPLE_BLOCK_WRITE state. - * SDCARD_OPERATION_SUCCESS - The multi-block write finished immediately, the card will enter - * the SDCARD_READY state. - * - */ -static sdcardOperationStatus_e sdcard_endWriteBlocks(void) -{ - sdcard.multiWriteBlocksRemain = 0; - - // 8 dummy clocks to guarantee N_WR clocks between the last card response and this token - spiTransferByte(sdcard.instance, 0xFF); - - spiTransferByte(sdcard.instance, SDCARD_MULTIPLE_BLOCK_WRITE_STOP_TOKEN); - - // Card may choose to raise a busy (non-0xFF) signal after at most N_BR (1 byte) delay - if (sdcard_waitForNonIdleByte(1) == 0xFF) { - sdcard.state = SDCARD_STATE_READY; - return SDCARD_OPERATION_SUCCESS; - } else { - sdcard.state = SDCARD_STATE_STOPPING_MULTIPLE_BLOCK_WRITE; - sdcard.operationStartTime = millis(); - - return SDCARD_OPERATION_IN_PROGRESS; + if (sdcardVTable) { + sdcardVTable->sdcard_init(config); } } -/** - * Call periodically for the SD card to perform in-progress transfers. - * - * Returns true if the card is ready to accept commands. - */ -bool sdcard_poll(void) -{ - if (!sdcard.enabled) { - sdcard.state = SDCARD_STATE_NOT_PRESENT; - return false; - } - - uint8_t initStatus; - bool sendComplete; - -#ifdef SDCARD_PROFILING - bool profilingComplete; -#endif - - doMore: - switch (sdcard.state) { - case SDCARD_STATE_RESET: - sdcard_select(); - - initStatus = sdcard_sendCommand(SDCARD_COMMAND_GO_IDLE_STATE, 0); - - sdcard_deselect(); - - if (initStatus == SDCARD_R1_STATUS_BIT_IDLE) { - // Check card voltage and version - if (sdcard_validateInterfaceCondition()) { - - sdcard.state = SDCARD_STATE_CARD_INIT_IN_PROGRESS; - goto doMore; - } else { - // Bad reply/voltage, we ought to refrain from accessing the card. - sdcard.state = SDCARD_STATE_NOT_PRESENT; - } - } - break; - - case SDCARD_STATE_CARD_INIT_IN_PROGRESS: - if (sdcard_checkInitDone()) { - if (sdcard.version == 2) { - // Check for high capacity card - uint32_t ocr; - - if (!sdcard_readOCRRegister(&ocr)) { - sdcard_reset(); - goto doMore; - } - - sdcard.highCapacity = (ocr & (1 << 30)) != 0; - } else { - // Version 1 cards are always low-capacity - sdcard.highCapacity = false; - } - - // Now fetch the CSD and CID registers - if (sdcard_fetchCSD()) { - sdcard_select(); - - uint8_t status = sdcard_sendCommand(SDCARD_COMMAND_SEND_CID, 0); - - if (status == 0) { - // Keep the card selected to receive the response block - sdcard.state = SDCARD_STATE_INITIALIZATION_RECEIVE_CID; - goto doMore; - } else { - sdcard_deselect(); - - sdcard_reset(); - goto doMore; - } - } - } - break; - case SDCARD_STATE_INITIALIZATION_RECEIVE_CID: - if (sdcard_receiveCID()) { - sdcard_deselect(); - - /* The spec is a little iffy on what the default block size is for Standard Size cards (it can be changed on - * standard size cards) so let's just set it to 512 explicitly so we don't have a problem. - */ - if (!sdcard.highCapacity && !sdcard_setBlockLength(SDCARD_BLOCK_SIZE)) { - sdcard_reset(); - goto doMore; - } - - // Now we're done with init and we can switch to the full speed clock (<25MHz) - spiSetDivisor(sdcard.instance, SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER); - - sdcard.multiWriteBlocksRemain = 0; - - sdcard.state = SDCARD_STATE_READY; - goto doMore; - } // else keep waiting for the CID to arrive - break; - case SDCARD_STATE_SENDING_WRITE: - // Have we finished sending the write yet? - sendComplete = false; - -#if defined(USE_HAL_DRIVER) - if (sdcard.useDMAForTx && DMA_GET_FLAG_STATUS(sdcard.dma, DMA_IT_TCIF)) { - //Clear both flags after transfer - DMA_CLEAR_FLAG(sdcard.dma, DMA_IT_TCIF); - DMA_CLEAR_FLAG(sdcard.dma, DMA_IT_HTIF); - // Drain anything left in the Rx FIFO (we didn't read it during the write) - while (LL_SPI_IsActiveFlag_RXNE(sdcard.instance)) { - sdcard.instance->DR; - } - - // Wait for the final bit to be transmitted - while (spiIsBusBusy(sdcard.instance)) { - } - - LL_SPI_DisableDMAReq_TX(sdcard.instance); - - sendComplete = true; - } -#else -#ifdef STM32F4 - if (sdcard.useDMAForTx && DMA_GetFlagStatus(sdcard.dma->ref, sdcard.dma->completeFlag) == SET) { - DMA_ClearFlag(sdcard.dma->ref, sdcard.dma->completeFlag); -#else - if (sdcard.useDMAForTx && DMA_GetFlagStatus(sdcard.dma->completeFlag) == SET) { - DMA_ClearFlag(sdcard.dma->completeFlag); -#endif - - DMA_Cmd(sdcard.dma->ref, DISABLE); - - // Drain anything left in the Rx FIFO (we didn't read it during the write) - while (SPI_I2S_GetFlagStatus(sdcard.instance, SPI_I2S_FLAG_RXNE) == SET) { - sdcard.instance->DR; - } - - // Wait for the final bit to be transmitted - while (spiIsBusBusy(sdcard.instance)) { - } - - SPI_I2S_DMACmd(sdcard.instance, SPI_I2S_DMAReq_Tx, DISABLE); - - sendComplete = true; - } -#endif - if (!sdcard.useDMAForTx) { - // Send another chunk - spiTransfer(sdcard.instance, sdcard.pendingOperation.buffer + SDCARD_NON_DMA_CHUNK_SIZE * sdcard.pendingOperation.chunkIndex, NULL, SDCARD_NON_DMA_CHUNK_SIZE); - - sdcard.pendingOperation.chunkIndex++; - - sendComplete = sdcard.pendingOperation.chunkIndex == SDCARD_BLOCK_SIZE / SDCARD_NON_DMA_CHUNK_SIZE; - } - - if (sendComplete) { - // Finish up by sending the CRC and checking the SD-card's acceptance/rejectance - if (sdcard_sendDataBlockFinish()) { - // The SD card is now busy committing that write to the card - sdcard.state = SDCARD_STATE_WAITING_FOR_WRITE; - sdcard.operationStartTime = millis(); - - // Since we've transmitted the buffer we can go ahead and tell the caller their operation is complete - if (sdcard.pendingOperation.callback) { - sdcard.pendingOperation.callback(SDCARD_BLOCK_OPERATION_WRITE, sdcard.pendingOperation.blockIndex, sdcard.pendingOperation.buffer, sdcard.pendingOperation.callbackData); - } - } else { - /* Our write was rejected! This could be due to a bad address but we hope not to attempt that, so assume - * the card is broken and needs reset. - */ - sdcard_reset(); - - // Announce write failure: - if (sdcard.pendingOperation.callback) { - sdcard.pendingOperation.callback(SDCARD_BLOCK_OPERATION_WRITE, sdcard.pendingOperation.blockIndex, NULL, sdcard.pendingOperation.callbackData); - } - - goto doMore; - } - } - break; - case SDCARD_STATE_WAITING_FOR_WRITE: - if (sdcard_waitForIdle(SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY)) { -#ifdef SDCARD_PROFILING - profilingComplete = true; -#endif - - sdcard.failureCount = 0; // Assume the card is good if it can complete a write - - // Still more blocks left to write in a multi-block chain? - if (sdcard.multiWriteBlocksRemain > 1) { - sdcard.multiWriteBlocksRemain--; - sdcard.multiWriteNextBlock++; - sdcard.state = SDCARD_STATE_WRITING_MULTIPLE_BLOCKS; - } else if (sdcard.multiWriteBlocksRemain == 1) { - // This function changes the sd card state for us whether immediately succesful or delayed: - if (sdcard_endWriteBlocks() == SDCARD_OPERATION_SUCCESS) { - sdcard_deselect(); - } else { -#ifdef SDCARD_PROFILING - // Wait for the multi-block write to be terminated before finishing timing - profilingComplete = false; -#endif - } - } else { - sdcard.state = SDCARD_STATE_READY; - sdcard_deselect(); - } - -#ifdef SDCARD_PROFILING - if (profilingComplete && sdcard.profiler) { - sdcard.profiler(SDCARD_BLOCK_OPERATION_WRITE, sdcard.pendingOperation.blockIndex, micros() - sdcard.pendingOperation.profileStartTime); - } -#endif - } else if (millis() > sdcard.operationStartTime + SDCARD_TIMEOUT_WRITE_MSEC) { - /* - * The caller has already been told that their write has completed, so they will have discarded - * their buffer and have no hope of retrying the operation. But this should be very rare and it allows - * them to reuse their buffer milliseconds faster than they otherwise would. - */ - sdcard_reset(); - goto doMore; - } - break; - case SDCARD_STATE_READING: - switch (sdcard_receiveDataBlock(sdcard.pendingOperation.buffer, SDCARD_BLOCK_SIZE)) { - case SDCARD_RECEIVE_SUCCESS: - sdcard_deselect(); - - sdcard.state = SDCARD_STATE_READY; - sdcard.failureCount = 0; // Assume the card is good if it can complete a read - -#ifdef SDCARD_PROFILING - if (sdcard.profiler) { - sdcard.profiler(SDCARD_BLOCK_OPERATION_READ, sdcard.pendingOperation.blockIndex, micros() - sdcard.pendingOperation.profileStartTime); - } -#endif - - if (sdcard.pendingOperation.callback) { - sdcard.pendingOperation.callback( - SDCARD_BLOCK_OPERATION_READ, - sdcard.pendingOperation.blockIndex, - sdcard.pendingOperation.buffer, - sdcard.pendingOperation.callbackData - ); - } - break; - case SDCARD_RECEIVE_BLOCK_IN_PROGRESS: - if (millis() <= sdcard.operationStartTime + SDCARD_TIMEOUT_READ_MSEC) { - break; // Timeout not reached yet so keep waiting - } - // Timeout has expired, so fall through to convert to a fatal error - FALLTHROUGH; - - case SDCARD_RECEIVE_ERROR: - sdcard_deselect(); - - sdcard_reset(); - - if (sdcard.pendingOperation.callback) { - sdcard.pendingOperation.callback( - SDCARD_BLOCK_OPERATION_READ, - sdcard.pendingOperation.blockIndex, - NULL, - sdcard.pendingOperation.callbackData - ); - } - - goto doMore; - break; - } - break; - case SDCARD_STATE_STOPPING_MULTIPLE_BLOCK_WRITE: - if (sdcard_waitForIdle(SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY)) { - sdcard_deselect(); - - sdcard.state = SDCARD_STATE_READY; - -#ifdef SDCARD_PROFILING - if (sdcard.profiler) { - sdcard.profiler(SDCARD_BLOCK_OPERATION_WRITE, sdcard.pendingOperation.blockIndex, micros() - sdcard.pendingOperation.profileStartTime); - } -#endif - } else if (millis() > sdcard.operationStartTime + SDCARD_TIMEOUT_WRITE_MSEC) { - sdcard_reset(); - goto doMore; - } - break; - case SDCARD_STATE_NOT_PRESENT: - default: - ; - } - - // Is the card's initialization taking too long? - if (sdcard.state >= SDCARD_STATE_RESET && sdcard.state < SDCARD_STATE_READY - && millis() - sdcard.operationStartTime > SDCARD_TIMEOUT_INIT_MILLIS) { - sdcard_reset(); - } - - return sdcard_isReady(); -} - -/** - * Write the 512-byte block from the given buffer into the block with the given index. - * - * If the write does not complete immediately, your callback will be called later. If the write was successful, the - * buffer pointer will be the same buffer you originally passed in, otherwise the buffer will be set to NULL. - * - * Returns: - * SDCARD_OPERATION_IN_PROGRESS - Your buffer is currently being transmitted to the card and your callback will be - * called later to report the completion. The buffer pointer must remain valid until - * that time. - * SDCARD_OPERATION_SUCCESS - Your buffer has been transmitted to the card now. - * SDCARD_OPERATION_BUSY - The card is already busy and cannot accept your write - * SDCARD_OPERATION_FAILURE - Your write was rejected by the card, card will be reset - */ -sdcardOperationStatus_e sdcard_writeBlock(uint32_t blockIndex, uint8_t *buffer, sdcard_operationCompleteCallback_c callback, uint32_t callbackData) -{ - uint8_t status; - -#ifdef SDCARD_PROFILING - sdcard.pendingOperation.profileStartTime = micros(); -#endif - - doMore: - switch (sdcard.state) { - case SDCARD_STATE_WRITING_MULTIPLE_BLOCKS: - // Do we need to cancel the previous multi-block write? - if (blockIndex != sdcard.multiWriteNextBlock) { - if (sdcard_endWriteBlocks() == SDCARD_OPERATION_SUCCESS) { - // Now we've entered the ready state, we can try again - goto doMore; - } else { - return SDCARD_OPERATION_BUSY; - } - } - - // We're continuing a multi-block write - break; - case SDCARD_STATE_READY: - // We're not continuing a multi-block write so we need to send a single-block write command - sdcard_select(); - - // Standard size cards use byte addressing, high capacity cards use block addressing - status = sdcard_sendCommand(SDCARD_COMMAND_WRITE_BLOCK, sdcard.highCapacity ? blockIndex : blockIndex * SDCARD_BLOCK_SIZE); - - if (status != 0) { - sdcard_deselect(); - - sdcard_reset(); - - return SDCARD_OPERATION_FAILURE; - } - break; - default: - return SDCARD_OPERATION_BUSY; - } - - sdcard_sendDataBlockBegin(buffer, sdcard.state == SDCARD_STATE_WRITING_MULTIPLE_BLOCKS); - - sdcard.pendingOperation.buffer = buffer; - sdcard.pendingOperation.blockIndex = blockIndex; - sdcard.pendingOperation.callback = callback; - sdcard.pendingOperation.callbackData = callbackData; - sdcard.pendingOperation.chunkIndex = 1; // (for non-DMA transfers) we've sent chunk #0 already - sdcard.state = SDCARD_STATE_SENDING_WRITE; - - return SDCARD_OPERATION_IN_PROGRESS; -} - -/** - * Begin writing a series of consecutive blocks beginning at the given block index. This will allow (but not require) - * the SD card to pre-erase the number of blocks you specifiy, which can allow the writes to complete faster. - * - * Afterwards, just call sdcard_writeBlock() as normal to write those blocks consecutively. - * - * It's okay to abort the multi-block write at any time by writing to a non-consecutive address, or by performing a read. - * - * Returns: - * SDCARD_OPERATION_SUCCESS - Multi-block write has been queued - * SDCARD_OPERATION_BUSY - The card is already busy and cannot accept your write - * SDCARD_OPERATION_FAILURE - A fatal error occured, card will be reset - */ -sdcardOperationStatus_e sdcard_beginWriteBlocks(uint32_t blockIndex, uint32_t blockCount) -{ - if (sdcard.state != SDCARD_STATE_READY) { - if (sdcard.state == SDCARD_STATE_WRITING_MULTIPLE_BLOCKS) { - if (blockIndex == sdcard.multiWriteNextBlock) { - // Assume that the caller wants to continue the multi-block write they already have in progress! - return SDCARD_OPERATION_SUCCESS; - } else if (sdcard_endWriteBlocks() != SDCARD_OPERATION_SUCCESS) { - return SDCARD_OPERATION_BUSY; - } // Else we've completed the previous multi-block write and can fall through to start the new one - } else { - return SDCARD_OPERATION_BUSY; - } - } - - sdcard_select(); - - if ( - sdcard_sendAppCommand(SDCARD_ACOMMAND_SET_WR_BLOCK_ERASE_COUNT, blockCount) == 0 - && sdcard_sendCommand(SDCARD_COMMAND_WRITE_MULTIPLE_BLOCK, sdcard.highCapacity ? blockIndex : blockIndex * SDCARD_BLOCK_SIZE) == 0 - ) { - sdcard.state = SDCARD_STATE_WRITING_MULTIPLE_BLOCKS; - sdcard.multiWriteBlocksRemain = blockCount; - sdcard.multiWriteNextBlock = blockIndex; - - // Leave the card selected - return SDCARD_OPERATION_SUCCESS; - } else { - sdcard_deselect(); - - sdcard_reset(); - - return SDCARD_OPERATION_FAILURE; - } -} - -/** - * Read the 512-byte block with the given index into the given 512-byte buffer. - * - * When the read completes, your callback will be called. If the read was successful, the buffer pointer will be the - * same buffer you originally passed in, otherwise the buffer will be set to NULL. - * - * You must keep the pointer to the buffer valid until the operation completes! - * - * Returns: - * true - The operation was successfully queued for later completion, your callback will be called later - * false - The operation could not be started due to the card being busy (try again later). - */ bool sdcard_readBlock(uint32_t blockIndex, uint8_t *buffer, sdcard_operationCompleteCallback_c callback, uint32_t callbackData) { - if (sdcard.state != SDCARD_STATE_READY) { - if (sdcard.state == SDCARD_STATE_WRITING_MULTIPLE_BLOCKS) { - if (sdcard_endWriteBlocks() != SDCARD_OPERATION_SUCCESS) { - return false; - } - } else { - return false; - } - } + return sdcardVTable->sdcard_readBlock(blockIndex, buffer, callback, callbackData); +} -#ifdef SDCARD_PROFILING - sdcard.pendingOperation.profileStartTime = micros(); -#endif +sdcardOperationStatus_e sdcard_beginWriteBlocks(uint32_t blockIndex, uint32_t blockCount) +{ + return sdcardVTable->sdcard_beginWriteBlocks(blockIndex, blockCount); +} - sdcard_select(); +sdcardOperationStatus_e sdcard_writeBlock(uint32_t blockIndex, uint8_t *buffer, sdcard_operationCompleteCallback_c callback, uint32_t callbackData) +{ + return sdcardVTable->sdcard_writeBlock(blockIndex, buffer, callback, callbackData); +} - // Standard size cards use byte addressing, high capacity cards use block addressing - uint8_t status = sdcard_sendCommand(SDCARD_COMMAND_READ_SINGLE_BLOCK, sdcard.highCapacity ? blockIndex : blockIndex * SDCARD_BLOCK_SIZE); - - if (status == 0) { - sdcard.pendingOperation.buffer = buffer; - sdcard.pendingOperation.blockIndex = blockIndex; - sdcard.pendingOperation.callback = callback; - sdcard.pendingOperation.callbackData = callbackData; - - sdcard.state = SDCARD_STATE_READING; - - sdcard.operationStartTime = millis(); - - // Leave the card selected for the whole transaction - - return true; +bool sdcard_poll(void) +{ + // sdcard_poll is called from taskMain() via afatfs_poll() and for USE_SDCARD. + if (sdcardVTable) { + return sdcardVTable->sdcard_poll(); + } else { + return false; + } +} + +bool sdcard_isFunctional(void) +{ + // sdcard_isFunctional is called from multiple places, including the case of hardware implementation + // without a detect pin in which case sdcard_isInserted() always returns true. + if (sdcardVTable) { + return sdcardVTable->sdcard_isFunctional(); } else { - sdcard_deselect(); - return false; } } -/** - * Returns true if the SD card has successfully completed its startup procedures. - */ bool sdcard_isInitialized(void) { - return sdcard.state >= SDCARD_STATE_READY; + return sdcardVTable->sdcard_isInitialized(); } const sdcardMetadata_t* sdcard_getMetadata(void) { - return &sdcard.metadata; + return sdcardVTable->sdcard_getMetadata(); } - -#ifdef SDCARD_PROFILING - -void sdcard_setProfilerCallback(sdcard_profilerCallback_c callback) -{ - sdcard.profiler = callback; -} - -#endif - #endif diff --git a/src/main/drivers/sdcard_impl.h b/src/main/drivers/sdcard_impl.h new file mode 100644 index 0000000000..98a6dbd093 --- /dev/null +++ b/src/main/drivers/sdcard_impl.h @@ -0,0 +1,133 @@ +/* + * 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 + +#include "drivers/nvic.h" +#include "drivers/io.h" +#include "dma.h" + +#include "drivers/bus_spi.h" +#include "drivers/time.h" + +#include "sdcard.h" +#include "sdcard_standard.h" + +#ifdef AFATFS_USE_INTROSPECTIVE_LOGGING + #define SDCARD_PROFILING +#endif + +#define SDCARD_TIMEOUT_INIT_MILLIS 200 +#define SDCARD_MAX_CONSECUTIVE_FAILURES 8 + +typedef enum { + // In these states we run at the initialization 400kHz clockspeed: + SDCARD_STATE_NOT_PRESENT = 0, + SDCARD_STATE_RESET, + SDCARD_STATE_CARD_INIT_IN_PROGRESS, + SDCARD_STATE_INITIALIZATION_RECEIVE_CID, + + // In these states we run at full clock speed + SDCARD_STATE_READY, + SDCARD_STATE_READING, + SDCARD_STATE_SENDING_WRITE, + SDCARD_STATE_WAITING_FOR_WRITE, + SDCARD_STATE_WRITING_MULTIPLE_BLOCKS, + SDCARD_STATE_STOPPING_MULTIPLE_BLOCK_WRITE +} sdcardState_e; + +typedef struct sdcard_t { + struct { + uint8_t *buffer; + uint32_t blockIndex; + uint8_t chunkIndex; + + sdcard_operationCompleteCallback_c callback; + uint32_t callbackData; + +#ifdef SDCARD_PROFILING + uint32_t profileStartTime; +#endif + } pendingOperation; + + uint32_t operationStartTime; + + uint8_t failureCount; + + uint8_t version; + bool highCapacity; + + uint32_t multiWriteNextBlock; + uint32_t multiWriteBlocksRemain; + + sdcardState_e state; + + sdcardMetadata_t metadata; + sdcardCSD_t csd; + +#ifdef SDCARD_PROFILING + sdcard_profilerCallback_c profiler; +#endif + bool enabled; + bool detectionInverted; + IO_t cardDetectPin; + +#ifdef USE_SDCARD_SPI + busDevice_t busdev; + bool useDMAForTx; + dmaChannelDescriptor_t * dma; +#endif + +#ifdef USE_SDCARD_SDIO + dmaIdentifier_e dmaIdentifier; + uint8_t useCache; +#endif + + uint8_t dmaChannel; +} sdcard_t; + +extern sdcard_t sdcard; + +STATIC_ASSERT(sizeof(sdcardCSD_t) == 16, sdcard_csd_bitfields_didnt_pack_properly); + +void sdcardInsertionDetectInit(void); +void sdcardInsertionDetectDeinit(void); +bool sdcard_isInserted(void); + +typedef struct sdcardVTable_s { + void (*sdcard_init)(const sdcardConfig_t *config); + bool (*sdcard_readBlock)(uint32_t blockIndex, uint8_t *buffer, sdcard_operationCompleteCallback_c callback, uint32_t callbackData); + sdcardOperationStatus_e (*sdcard_beginWriteBlocks)(uint32_t blockIndex, uint32_t blockCount); + sdcardOperationStatus_e (*sdcard_writeBlock)(uint32_t blockIndex, uint8_t *buffer, sdcard_operationCompleteCallback_c callback, uint32_t callbackData); + bool (*sdcard_poll)(void); + bool (*sdcard_isFunctional)(void); + bool (*sdcard_isInitialized)(void); + const sdcardMetadata_t* (*sdcard_getMetadata)(void); +#ifdef SDCARD_PROFILING + void (*sdcardSdio_setProfilerCallback)(sdcard_profilerCallback_c callback); +#endif +} sdcardVTable_t; + +#ifdef USE_SDCARD_SPI +extern sdcardVTable_t sdcardSpiVTable; +#endif +#ifdef USE_SDCARD_SDIO +extern sdcardVTable_t sdcardSdioVTable; +#endif diff --git a/src/main/drivers/sdcard_sdio_baremetal.c b/src/main/drivers/sdcard_sdio_baremetal.c index 3e65845878..32f253ed69 100644 --- a/src/main/drivers/sdcard_sdio_baremetal.c +++ b/src/main/drivers/sdcard_sdio_baremetal.c @@ -26,7 +26,6 @@ #include "platform.h" -#define USE_SDCARD_SDIO #ifdef USE_SDCARD_SDIO #include "drivers/nvic.h" @@ -36,6 +35,7 @@ #include "drivers/time.h" #include "drivers/sdcard.h" +#include "drivers/sdcard_impl.h" #include "drivers/sdcard_standard.h" #include "drivers/sdmmc_sdio.h" @@ -43,13 +43,6 @@ #include "pg/pg.h" #include "pg/sdio.h" -#ifdef AFATFS_USE_INTROSPECTIVE_LOGGING - #define SDCARD_PROFILING -#endif - -#define SDCARD_TIMEOUT_INIT_MILLIS 200 -#define SDCARD_MAX_CONSECUTIVE_FAILURES 8 - // Use this to speed up writing to SDCARD... asyncfatfs has limited support for multiblock write #define FATFS_BLOCK_CACHE_SIZE 16 uint8_t writeCache[512 * FATFS_BLOCK_CACHE_SIZE] __attribute__ ((aligned (4))); @@ -75,98 +68,11 @@ void cache_reset(void) cacheCount = 0; } -typedef enum { - // In these states we run at the initialization 400kHz clockspeed: - SDCARD_STATE_NOT_PRESENT = 0, - SDCARD_STATE_RESET, - SDCARD_STATE_CARD_INIT_IN_PROGRESS, - SDCARD_STATE_INITIALIZATION_RECEIVE_CID, - - // In these states we run at full clock speed - SDCARD_STATE_READY, - SDCARD_STATE_READING, - SDCARD_STATE_SENDING_WRITE, - SDCARD_STATE_WAITING_FOR_WRITE, - SDCARD_STATE_WRITING_MULTIPLE_BLOCKS, - SDCARD_STATE_STOPPING_MULTIPLE_BLOCK_WRITE -} sdcardState_e; - -typedef struct sdcard_t { - struct { - uint8_t *buffer; - uint32_t blockIndex; - uint8_t chunkIndex; - - sdcard_operationCompleteCallback_c callback; - uint32_t callbackData; - -#ifdef SDCARD_PROFILING - uint32_t profileStartTime; -#endif - } pendingOperation; - - uint32_t operationStartTime; - - uint8_t failureCount; - - uint8_t version; - bool highCapacity; - - uint32_t multiWriteNextBlock; - uint32_t multiWriteBlocksRemain; - - sdcardState_e state; - - sdcardMetadata_t metadata; - sdcardCSD_t csd; - -#ifdef SDCARD_PROFILING - sdcard_profilerCallback_c profiler; -#endif - bool enabled; - IO_t cardDetectPin; - dmaIdentifier_e dma; - uint8_t dmaChannel; - uint8_t useCache; -} sdcard_t; - -static sdcard_t sdcard; - -STATIC_ASSERT(sizeof(sdcardCSD_t) == 16, sdcard_csd_bitfields_didnt_pack_properly); - -void sdcardInsertionDetectDeinit(void) -{ - if (sdcard.cardDetectPin) { - IOInit(sdcard.cardDetectPin, OWNER_FREE, 0); - IOConfigGPIO(sdcard.cardDetectPin, IOCFG_IN_FLOATING); - } -} - -void sdcardInsertionDetectInit(void) -{ - if (sdcard.cardDetectPin) { - IOInit(sdcard.cardDetectPin, OWNER_SDCARD_DETECT, 0); - IOConfigGPIO(sdcard.cardDetectPin, IOCFG_IPU); - } -} - -/** - * Detect if a SD card is physically present in the memory slot. - */ -bool sdcard_isInserted(void) -{ - bool ret = true; - if (sdcard.cardDetectPin) { - ret = IORead(sdcard.cardDetectPin) == 0; - } - return ret; -} - /** * Returns true if the card has already been, or is currently, initializing and hasn't encountered enough errors to * trip our error threshold and be disabled (i.e. our card is in and working!) */ -bool sdcard_isFunctional(void) +static bool sdcardSdio_isFunctional(void) { return sdcard.state != SDCARD_STATE_NOT_PRESENT; } @@ -281,15 +187,15 @@ static bool sdcard_checkInitDone(void) /** * Begin the initialization process for the SD card. This must be called first before any other sdcard_ routine. */ -void sdcard_init(const sdcardConfig_t *config) +static void sdcardSdio_init(const sdcardConfig_t *config) { - sdcard.enabled = config->enabled; + sdcard.enabled = config->mode; if (!sdcard.enabled) { sdcard.state = SDCARD_STATE_NOT_PRESENT; return; } - sdcard.dma = config->dmaIdentifier; - if (sdcard.dma == 0) { + sdcard.dmaIdentifier = config->dmaIdentifier; + if (sdcard.dmaIdentifier == 0) { sdcard.state = SDCARD_STATE_NOT_PRESENT; return; } @@ -298,12 +204,15 @@ void sdcard_init(const sdcardConfig_t *config) } else { sdcard.cardDetectPin = IO_NONE; } + if (config->cardDetectInverted) { + sdcard.detectionInverted = 1; + } if (sdioConfig()->useCache) { sdcard.useCache = 1; } else { sdcard.useCache = 0; } - SD_Initialize_LL(dmaGetRefByIdentifier(sdcard.dma)); + SD_Initialize_LL(dmaGetRefByIdentifier(sdcard.dmaIdentifier)); if (SD_IsDetected()) { if (SD_Init() != 0) { sdcard.state = SDCARD_STATE_NOT_PRESENT; @@ -364,7 +273,7 @@ static sdcardOperationStatus_e sdcard_endWriteBlocks() * * Returns true if the card is ready to accept commands. */ -bool sdcard_poll(void) +static bool sdcardSdio_poll(void) { if (!sdcard.enabled) { sdcard.state = SDCARD_STATE_NOT_PRESENT; @@ -539,7 +448,7 @@ bool sdcard_poll(void) * SDCARD_OPERATION_BUSY - The card is already busy and cannot accept your write * SDCARD_OPERATION_FAILURE - Your write was rejected by the card, card will be reset */ -sdcardOperationStatus_e sdcard_writeBlock(uint32_t blockIndex, uint8_t *buffer, sdcard_operationCompleteCallback_c callback, uint32_t callbackData) +static sdcardOperationStatus_e sdcardSdio_writeBlock(uint32_t blockIndex, uint8_t *buffer, sdcard_operationCompleteCallback_c callback, uint32_t callbackData) { #ifdef SDCARD_PROFILING @@ -622,7 +531,7 @@ sdcardOperationStatus_e sdcard_writeBlock(uint32_t blockIndex, uint8_t *buffer, * SDCARD_OPERATION_BUSY - The card is already busy and cannot accept your write * SDCARD_OPERATION_FAILURE - A fatal error occured, card will be reset */ -sdcardOperationStatus_e sdcard_beginWriteBlocks(uint32_t blockIndex, uint32_t blockCount) +static sdcardOperationStatus_e sdcardSdio_beginWriteBlocks(uint32_t blockIndex, uint32_t blockCount) { if (sdcard.state != SDCARD_STATE_READY) { if (sdcard.state == SDCARD_STATE_WRITING_MULTIPLE_BLOCKS) { @@ -655,7 +564,7 @@ sdcardOperationStatus_e sdcard_beginWriteBlocks(uint32_t blockIndex, uint32_t bl * true - The operation was successfully queued for later completion, your callback will be called later * false - The operation could not be started due to the card being busy (try again later). */ -bool sdcard_readBlock(uint32_t blockIndex, uint8_t *buffer, sdcard_operationCompleteCallback_c callback, uint32_t callbackData) +static bool sdcardSdio_readBlock(uint32_t blockIndex, uint8_t *buffer, sdcard_operationCompleteCallback_c callback, uint32_t callbackData) { if (sdcard.state != SDCARD_STATE_READY) { if (sdcard.state == SDCARD_STATE_WRITING_MULTIPLE_BLOCKS) { @@ -702,23 +611,36 @@ bool sdcard_readBlock(uint32_t blockIndex, uint8_t *buffer, sdcard_operationComp /** * Returns true if the SD card has successfully completed its startup procedures. */ -bool sdcard_isInitialized(void) +static bool sdcardSdio_isInitialized(void) { return sdcard.state >= SDCARD_STATE_READY; } -const sdcardMetadata_t* sdcard_getMetadata(void) +static const sdcardMetadata_t* sdcardSdio_getMetadata(void) { return &sdcard.metadata; } #ifdef SDCARD_PROFILING -void sdcard_setProfilerCallback(sdcard_profilerCallback_c callback) +static void sdcardSdio_setProfilerCallback(sdcard_profilerCallback_c callback) { sdcard.profiler = callback; } #endif +sdcardVTable_t sdcardSdioVTable = { + sdcardSdio_init, + sdcardSdio_readBlock, + sdcardSdio_beginWriteBlocks, + sdcardSdio_writeBlock, + sdcardSdio_poll, + sdcardSdio_isFunctional, + sdcardSdio_isInitialized, + sdcardSdio_getMetadata, +#ifdef SDCARD_PROFILING + sdcardSdio_setProfilerCallback, +#endif +}; #endif diff --git a/src/main/drivers/sdcard_spi.c b/src/main/drivers/sdcard_spi.c new file mode 100644 index 0000000000..6a31513812 --- /dev/null +++ b/src/main/drivers/sdcard_spi.c @@ -0,0 +1,1091 @@ +/* + * 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 . + */ + +#include +#include + +#include "platform.h" + +#ifdef USE_SDCARD_SPI + +#include "drivers/nvic.h" +#include "drivers/io.h" +#include "dma.h" + +#include "drivers/bus_spi.h" +#include "drivers/time.h" + +#include "sdcard.h" +#include "sdcard_impl.h" +#include "sdcard_standard.h" + +#ifdef AFATFS_USE_INTROSPECTIVE_LOGGING + #define SDCARD_PROFILING +#endif + +#define SDCARD_INIT_NUM_DUMMY_BYTES 10 +#define SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY 8 +// Chosen so that CMD8 will have the same CRC as CMD0: +#define SDCARD_IF_COND_CHECK_PATTERN 0xAB + +/* SPI_CLOCK_INITIALIZATION (256) is the slowest (Spec calls for under 400KHz) */ +#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER SPI_CLOCK_INITIALIZATION + +/* Operational speed <= 25MHz */ +#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER SPI_CLOCK_FAST + +/* Break up 512-byte SD card sectors into chunks of this size when writing without DMA to reduce the peak overhead + * per call to sdcard_poll(). + */ +#define SDCARD_NON_DMA_CHUNK_SIZE 256 + +/** + * Returns true if the card has already been, or is currently, initializing and hasn't encountered enough errors to + * trip our error threshold and be disabled (i.e. our card is in and working!) + */ +static bool sdcardSpi_isFunctional(void) +{ + return sdcard.state != SDCARD_STATE_NOT_PRESENT; +} + +static void sdcard_select(void) +{ + IOLo(sdcard.busdev.busdev_u.spi.csnPin); +} + +static void sdcard_deselect(void) +{ + // As per the SD-card spec, give the card 8 dummy clocks so it can finish its operation + //spiBusTransferByte(&sdcard.busdev, 0xFF); + + while (spiBusIsBusBusy(&sdcard.busdev)) { + } + + IOHi(sdcard.busdev.busdev_u.spi.csnPin); +} + +/** + * Handle a failure of an SD card operation by resetting the card back to its initialization phase. + * + * Increments the failure counter, and when the failure threshold is reached, disables the card until + * the next call to sdcard_init(). + */ +static void sdcard_reset(void) +{ + if (!sdcard_isInserted()) { + sdcard.state = SDCARD_STATE_NOT_PRESENT; + return; + } + + if (sdcard.state >= SDCARD_STATE_READY) { + spiSetDivisor(sdcard.busdev.busdev_u.spi.instance, SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER); + } + + sdcard.failureCount++; + if (sdcard.failureCount >= SDCARD_MAX_CONSECUTIVE_FAILURES) { + sdcard.state = SDCARD_STATE_NOT_PRESENT; + } else { + sdcard.operationStartTime = millis(); + sdcard.state = SDCARD_STATE_RESET; + } +} + +/** + * The SD card spec requires 8 clock cycles to be sent by us on the bus after most commands so it can finish its + * processing of that command. The easiest way for us to do this is to just wait for the bus to become idle before + * we transmit a command, sending at least 8-bits onto the bus when we do so. + */ +static bool sdcard_waitForIdle(int maxBytesToWait) +{ + while (maxBytesToWait > 0) { + uint8_t b = spiBusTransferByte(&sdcard.busdev, 0xFF); + if (b == 0xFF) { + return true; + } + maxBytesToWait--; + } + + return false; +} + +/** + * Wait for up to maxDelay 0xFF idle bytes to arrive from the card, returning the first non-idle byte found. + * + * Returns 0xFF on failure. + */ +static uint8_t sdcard_waitForNonIdleByte(int maxDelay) +{ + for (int i = 0; i < maxDelay + 1; i++) { // + 1 so we can wait for maxDelay '0xFF' bytes before reading a response byte afterwards + uint8_t response = spiBusTransferByte(&sdcard.busdev, 0xFF); + + if (response != 0xFF) { + return response; + } + } + + return 0xFF; +} + +/** + * Waits up to SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY bytes for the card to become ready, send a command to the card + * with the given argument, waits up to SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY bytes for a reply, and returns the + * first non-0xFF byte of the reply. + * + * You must select the card first with sdcard_select() and deselect it afterwards with sdcard_deselect(). + * + * Upon failure, 0xFF is returned. + */ +static uint8_t sdcard_sendCommand(uint8_t commandCode, uint32_t commandArgument) +{ + const uint8_t command[6] = { + 0x40 | commandCode, + commandArgument >> 24, + commandArgument >> 16, + commandArgument >> 8, + commandArgument, + 0x95 /* Static CRC. This CRC is valid for CMD0 with a 0 argument, and CMD8 with 0x1AB argument, which are the only + commands that require a CRC */ + }; + + // Go ahead and send the command even if the card isn't idle if this is the reset command + if (!sdcard_waitForIdle(SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY) && commandCode != SDCARD_COMMAND_GO_IDLE_STATE) + return 0xFF; + + spiBusRawTransfer(&sdcard.busdev, command, NULL, sizeof(command)); + + /* + * The card can take up to SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY bytes to send the response, in the meantime + * it'll transmit 0xFF filler bytes. + */ + return sdcard_waitForNonIdleByte(SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY); +} + +static uint8_t sdcard_sendAppCommand(uint8_t commandCode, uint32_t commandArgument) +{ + sdcard_sendCommand(SDCARD_COMMAND_APP_CMD, 0); + + return sdcard_sendCommand(commandCode, commandArgument); +} + +/** + * Sends an IF_COND message to the card to check its version and validate its voltage requirements. Sets the global + * sdCardVersion with the detected version (0, 1, or 2) and returns true if the card is compatible. + */ +static bool sdcard_validateInterfaceCondition(void) +{ + uint8_t ifCondReply[4]; + + sdcard.version = 0; + + sdcard_select(); + + uint8_t status = sdcard_sendCommand(SDCARD_COMMAND_SEND_IF_COND, (SDCARD_VOLTAGE_ACCEPTED_2_7_to_3_6 << 8) | SDCARD_IF_COND_CHECK_PATTERN); + + // Don't deselect the card right away, because we'll want to read the rest of its reply if it's a V2 card + + if (status == (SDCARD_R1_STATUS_BIT_ILLEGAL_COMMAND | SDCARD_R1_STATUS_BIT_IDLE)) { + // V1 cards don't support this command + sdcard.version = 1; + } else if (status == SDCARD_R1_STATUS_BIT_IDLE) { + spiBusRawTransfer(&sdcard.busdev, NULL, ifCondReply, sizeof(ifCondReply)); + + /* + * We don't bother to validate the SDCard's operating voltage range since the spec requires it to accept our + * 3.3V, but do check that it echoed back our check pattern properly. + */ + if (ifCondReply[3] == SDCARD_IF_COND_CHECK_PATTERN) { + sdcard.version = 2; + } + } + + sdcard_deselect(); + + return sdcard.version > 0; +} + +static bool sdcard_readOCRRegister(uint32_t *result) +{ + sdcard_select(); + + uint8_t status = sdcard_sendCommand(SDCARD_COMMAND_READ_OCR, 0); + + uint8_t response[4]; + + spiBusRawTransfer(&sdcard.busdev, NULL, response, sizeof(response)); + + if (status == 0) { + sdcard_deselect(); + + *result = (response[0] << 24) | (response[1] << 16) | (response[2] << 8) | response[3]; + + return true; + } else { + sdcard_deselect(); + + return false; + } +} + +typedef enum { + SDCARD_RECEIVE_SUCCESS, + SDCARD_RECEIVE_BLOCK_IN_PROGRESS, + SDCARD_RECEIVE_ERROR +} sdcardReceiveBlockStatus_e; + +/** + * Attempt to receive a data block from the SD card. + * + * Return true on success, otherwise the card has not responded yet and you should retry later. + */ +static sdcardReceiveBlockStatus_e sdcard_receiveDataBlock(uint8_t *buffer, int count) +{ + uint8_t dataToken = sdcard_waitForNonIdleByte(8); + + if (dataToken == 0xFF) { + return SDCARD_RECEIVE_BLOCK_IN_PROGRESS; + } + + if (dataToken != SDCARD_SINGLE_BLOCK_READ_START_TOKEN) { + return SDCARD_RECEIVE_ERROR; + } + + spiBusRawTransfer(&sdcard.busdev, NULL, buffer, count); + + // Discard trailing CRC, we don't care + spiBusTransferByte(&sdcard.busdev, 0xFF); + spiBusTransferByte(&sdcard.busdev, 0xFF); + + return SDCARD_RECEIVE_SUCCESS; +} + +static bool sdcard_sendDataBlockFinish(void) +{ +#ifdef USE_HAL_DRIVER + // Drain anything left in the Rx FIFO (we didn't read it during the write) + //This is necessary here as when using msc there is timing issue + while (LL_SPI_IsActiveFlag_RXNE(sdcard.busdev.busdev_u.spi.instance)) { + sdcard.busdev.busdev_u.spi.instance->DR; + } +#endif + + // Send a dummy CRC + spiBusTransferByte(&sdcard.busdev, 0x00); + spiBusTransferByte(&sdcard.busdev, 0x00); + + uint8_t dataResponseToken = spiBusTransferByte(&sdcard.busdev, 0xFF); + + /* + * Check if the card accepted the write (no CRC error / no address error) + * + * The lower 5 bits are structured as follows: + * | 0 | Status | 1 | + * | 0 | x x x | 1 | + * + * Statuses: + * 010 - Data accepted + * 101 - CRC error + * 110 - Write error + */ + return (dataResponseToken & 0x1F) == 0x05; +} + +/** + * Begin sending a buffer of SDCARD_BLOCK_SIZE bytes to the SD card. + */ +static void sdcard_sendDataBlockBegin(const uint8_t *buffer, bool multiBlockWrite) +{ + // Card wants 8 dummy clock cycles between the write command's response and a data block beginning: + spiBusTransferByte(&sdcard.busdev, 0xFF); + + spiBusTransferByte(&sdcard.busdev, multiBlockWrite ? SDCARD_MULTIPLE_BLOCK_WRITE_START_TOKEN : SDCARD_SINGLE_BLOCK_WRITE_START_TOKEN); + + if (sdcard.useDMAForTx) { +#if defined(USE_HAL_DRIVER) + LL_DMA_InitTypeDef init; + + LL_DMA_StructInit(&init); + + init.Channel = dmaGetChannel(sdcard.dmaChannel); + init.Mode = LL_DMA_MODE_NORMAL; + init.Direction = LL_DMA_DIRECTION_MEMORY_TO_PERIPH; + + init.PeriphOrM2MSrcAddress = (uint32_t)&sdcard.busdev.busdev_u.spi.instance->DR; + init.Priority = LL_DMA_PRIORITY_LOW; + init.PeriphOrM2MSrcIncMode = LL_DMA_PERIPH_NOINCREMENT; + init.PeriphOrM2MSrcDataSize = LL_DMA_PDATAALIGN_BYTE; + + init.MemoryOrM2MDstAddress = (uint32_t)buffer; + init.MemoryOrM2MDstIncMode = LL_DMA_MEMORY_INCREMENT; + init.MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_BYTE; + + init.NbData = SDCARD_BLOCK_SIZE; + + LL_DMA_DeInit(sdcard.dma->dma, sdcard.dma->stream); + LL_DMA_Init(sdcard.dma->dma, sdcard.dma->stream, &init); + + LL_DMA_EnableStream(sdcard.dma->dma, sdcard.dma->stream); + + LL_SPI_EnableDMAReq_TX(sdcard.busdev.busdev_u.spi.instance); + +#else + + DMA_InitTypeDef init; + + DMA_StructInit(&init); +#ifdef STM32F4 + init.DMA_Channel = dmaGetChannel(sdcard.dmaChannel); + init.DMA_Memory0BaseAddr = (uint32_t) buffer; + init.DMA_DIR = DMA_DIR_MemoryToPeripheral; +#else + init.DMA_M2M = DMA_M2M_Disable; + init.DMA_MemoryBaseAddr = (uint32_t) buffer; + init.DMA_DIR = DMA_DIR_PeripheralDST; +#endif + init.DMA_PeripheralBaseAddr = (uint32_t) &sdcard.busdev.busdev_u.spi.instance->DR; + init.DMA_Priority = DMA_Priority_Low; + init.DMA_PeripheralInc = DMA_PeripheralInc_Disable; + init.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; + + init.DMA_MemoryInc = DMA_MemoryInc_Enable; + init.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; + + init.DMA_BufferSize = SDCARD_BLOCK_SIZE; + init.DMA_Mode = DMA_Mode_Normal; + + DMA_DeInit(sdcard.dma->ref); + DMA_Init(sdcard.dma->ref, &init); + + DMA_Cmd(sdcard.dma->ref, ENABLE); + + SPI_I2S_DMACmd(sdcard.busdev.busdev_u.spi.instance, SPI_I2S_DMAReq_Tx, ENABLE); +#endif + } else { + // Send the first chunk now + spiBusRawTransfer(&sdcard.busdev, buffer, NULL, SDCARD_NON_DMA_CHUNK_SIZE); + } +} + +static bool sdcard_receiveCID(void) +{ + uint8_t cid[16]; + + if (sdcard_receiveDataBlock(cid, sizeof(cid)) != SDCARD_RECEIVE_SUCCESS) { + return false; + } + + sdcard.metadata.manufacturerID = cid[0]; + sdcard.metadata.oemID = (cid[1] << 8) | cid[2]; + sdcard.metadata.productName[0] = cid[3]; + sdcard.metadata.productName[1] = cid[4]; + sdcard.metadata.productName[2] = cid[5]; + sdcard.metadata.productName[3] = cid[6]; + sdcard.metadata.productName[4] = cid[7]; + sdcard.metadata.productRevisionMajor = cid[8] >> 4; + sdcard.metadata.productRevisionMinor = cid[8] & 0x0F; + sdcard.metadata.productSerial = (cid[9] << 24) | (cid[10] << 16) | (cid[11] << 8) | cid[12]; + sdcard.metadata.productionYear = (((cid[13] & 0x0F) << 4) | (cid[14] >> 4)) + 2000; + sdcard.metadata.productionMonth = cid[14] & 0x0F; + + return true; +} + +static bool sdcard_fetchCSD(void) +{ + uint32_t readBlockLen, blockCount, blockCountMult; + uint64_t capacityBytes; + + sdcard_select(); + + /* The CSD command's data block should always arrive within 8 idle clock cycles (SD card spec). This is because + * the information about card latency is stored in the CSD register itself, so we can't use that yet! + */ + bool success = + sdcard_sendCommand(SDCARD_COMMAND_SEND_CSD, 0) == 0 + && sdcard_receiveDataBlock((uint8_t*) &sdcard.csd, sizeof(sdcard.csd)) == SDCARD_RECEIVE_SUCCESS + && SDCARD_GET_CSD_FIELD(sdcard.csd, 1, TRAILER) == 1; + + if (success) { + switch (SDCARD_GET_CSD_FIELD(sdcard.csd, 1, CSD_STRUCTURE_VER)) { + case SDCARD_CSD_STRUCTURE_VERSION_1: + // Block size in bytes (doesn't have to be 512) + readBlockLen = 1 << SDCARD_GET_CSD_FIELD(sdcard.csd, 1, READ_BLOCK_LEN); + blockCountMult = 1 << (SDCARD_GET_CSD_FIELD(sdcard.csd, 1, CSIZE_MULT) + 2); + blockCount = (SDCARD_GET_CSD_FIELD(sdcard.csd, 1, CSIZE) + 1) * blockCountMult; + + // We could do this in 32 bits but it makes the 2GB case awkward + capacityBytes = (uint64_t) blockCount * readBlockLen; + + // Re-express that capacity (max 2GB) in our standard 512-byte block size + sdcard.metadata.numBlocks = capacityBytes / SDCARD_BLOCK_SIZE; + break; + case SDCARD_CSD_STRUCTURE_VERSION_2: + sdcard.metadata.numBlocks = (SDCARD_GET_CSD_FIELD(sdcard.csd, 2, CSIZE) + 1) * 1024; + break; + default: + success = false; + } + } + + sdcard_deselect(); + + return success; +} + +/** + * Check if the SD Card has completed its startup sequence. Must be called with sdcard.state == SDCARD_STATE_INITIALIZATION. + * + * Returns true if the card has finished its init process. + */ +static bool sdcard_checkInitDone(void) +{ + sdcard_select(); + + uint8_t status = sdcard_sendAppCommand(SDCARD_ACOMMAND_SEND_OP_COND, sdcard.version == 2 ? 1 << 30 /* We support high capacity cards */ : 0); + + sdcard_deselect(); + + // When card init is complete, the idle bit in the response becomes zero. + return status == 0x00; +} + +/** + * Begin the initialization process for the SD card. This must be called first before any other sdcard_ routine. + */ +static void sdcardSpi_init(const sdcardConfig_t *config) +{ + sdcard.enabled = config->mode; + if (!sdcard.enabled) { + sdcard.state = SDCARD_STATE_NOT_PRESENT; + return; + } + + spiBusSetInstance(&sdcard.busdev, spiInstanceByDevice(SPI_CFG_TO_DEV(config->device))); + + sdcard.useDMAForTx = config->useDma; + if (sdcard.useDMAForTx) { +#if defined(STM32F4) || defined(STM32F7) + sdcard.dmaChannel = config->dmaChannel; +#endif + sdcard.dma = dmaGetDescriptorByIdentifier(config->dmaIdentifier); + dmaInit(config->dmaIdentifier, OWNER_SDCARD, 0); + } + + IO_t chipSelectIO; + if (config->chipSelectTag) { + chipSelectIO = IOGetByTag(config->chipSelectTag); + IOInit(chipSelectIO, OWNER_SDCARD_CS, 0); + IOConfigGPIO(chipSelectIO, SPI_IO_CS_CFG); + } else { + chipSelectIO = IO_NONE; + } + sdcard.busdev.busdev_u.spi.csnPin = chipSelectIO; + + if (config->cardDetectTag) { + sdcard.cardDetectPin = IOGetByTag(config->cardDetectTag); + sdcard.detectionInverted = config->cardDetectInverted; + } else { + sdcard.cardDetectPin = IO_NONE; + sdcard.detectionInverted = false; + } + + // Max frequency is initially 400kHz + spiSetDivisor(sdcard.busdev.busdev_u.spi.instance, SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER); + + // SDCard wants 1ms minimum delay after power is applied to it + delay(1000); + + // Transmit at least 74 dummy clock cycles with CS high so the SD card can start up + IOHi(sdcard.busdev.busdev_u.spi.csnPin); + + spiBusRawTransfer(&sdcard.busdev, NULL, NULL, SDCARD_INIT_NUM_DUMMY_BYTES); + + // Wait for that transmission to finish before we enable the SDCard, so it receives the required number of cycles: + int time = 100000; + while (spiBusIsBusBusy(&sdcard.busdev)) { + if (time-- == 0) { + sdcard.state = SDCARD_STATE_NOT_PRESENT; + sdcard.failureCount++; + return; + } + } + + sdcard.operationStartTime = millis(); + sdcard.state = SDCARD_STATE_RESET; + sdcard.failureCount = 0; +} + +static bool sdcard_setBlockLength(uint32_t blockLen) +{ + sdcard_select(); + + uint8_t status = sdcard_sendCommand(SDCARD_COMMAND_SET_BLOCKLEN, blockLen); + + sdcard_deselect(); + + return status == 0; +} + +/* + * Returns true if the card is ready to accept read/write commands. + */ +static bool sdcard_isReady(void) +{ + return sdcard.state == SDCARD_STATE_READY || sdcard.state == SDCARD_STATE_WRITING_MULTIPLE_BLOCKS; +} + +/** + * Send the stop-transmission token to complete a multi-block write. + * + * Returns: + * SDCARD_OPERATION_IN_PROGRESS - We're now waiting for that stop to complete, the card will enter + * the SDCARD_STATE_STOPPING_MULTIPLE_BLOCK_WRITE state. + * SDCARD_OPERATION_SUCCESS - The multi-block write finished immediately, the card will enter + * the SDCARD_READY state. + * + */ +static sdcardOperationStatus_e sdcard_endWriteBlocks(void) +{ + sdcard.multiWriteBlocksRemain = 0; + + // 8 dummy clocks to guarantee N_WR clocks between the last card response and this token + spiBusTransferByte(&sdcard.busdev, 0xFF); + + spiBusTransferByte(&sdcard.busdev, SDCARD_MULTIPLE_BLOCK_WRITE_STOP_TOKEN); + + // Card may choose to raise a busy (non-0xFF) signal after at most N_BR (1 byte) delay + if (sdcard_waitForNonIdleByte(1) == 0xFF) { + sdcard.state = SDCARD_STATE_READY; + return SDCARD_OPERATION_SUCCESS; + } else { + sdcard.state = SDCARD_STATE_STOPPING_MULTIPLE_BLOCK_WRITE; + sdcard.operationStartTime = millis(); + + return SDCARD_OPERATION_IN_PROGRESS; + } +} + +/** + * Call periodically for the SD card to perform in-progress transfers. + * + * Returns true if the card is ready to accept commands. + */ +static bool sdcardSpi_poll(void) +{ + if (!sdcard.enabled) { + sdcard.state = SDCARD_STATE_NOT_PRESENT; + return false; + } + + uint8_t initStatus; + bool sendComplete; + +#ifdef SDCARD_PROFILING + bool profilingComplete; +#endif + + doMore: + switch (sdcard.state) { + case SDCARD_STATE_RESET: + sdcard_select(); + + initStatus = sdcard_sendCommand(SDCARD_COMMAND_GO_IDLE_STATE, 0); + + sdcard_deselect(); + + if (initStatus == SDCARD_R1_STATUS_BIT_IDLE) { + // Check card voltage and version + if (sdcard_validateInterfaceCondition()) { + + sdcard.state = SDCARD_STATE_CARD_INIT_IN_PROGRESS; + goto doMore; + } else { + // Bad reply/voltage, we ought to refrain from accessing the card. + sdcard.state = SDCARD_STATE_NOT_PRESENT; + } + } + break; + + case SDCARD_STATE_CARD_INIT_IN_PROGRESS: + if (sdcard_checkInitDone()) { + if (sdcard.version == 2) { + // Check for high capacity card + uint32_t ocr; + + if (!sdcard_readOCRRegister(&ocr)) { + sdcard_reset(); + goto doMore; + } + + sdcard.highCapacity = (ocr & (1 << 30)) != 0; + } else { + // Version 1 cards are always low-capacity + sdcard.highCapacity = false; + } + + // Now fetch the CSD and CID registers + if (sdcard_fetchCSD()) { + sdcard_select(); + + uint8_t status = sdcard_sendCommand(SDCARD_COMMAND_SEND_CID, 0); + + if (status == 0) { + // Keep the card selected to receive the response block + sdcard.state = SDCARD_STATE_INITIALIZATION_RECEIVE_CID; + goto doMore; + } else { + sdcard_deselect(); + + sdcard_reset(); + goto doMore; + } + } + } + break; + case SDCARD_STATE_INITIALIZATION_RECEIVE_CID: + if (sdcard_receiveCID()) { + sdcard_deselect(); + + /* The spec is a little iffy on what the default block size is for Standard Size cards (it can be changed on + * standard size cards) so let's just set it to 512 explicitly so we don't have a problem. + */ + if (!sdcard.highCapacity && !sdcard_setBlockLength(SDCARD_BLOCK_SIZE)) { + sdcard_reset(); + goto doMore; + } + + // Now we're done with init and we can switch to the full speed clock (<25MHz) + spiSetDivisor(sdcard.busdev.busdev_u.spi.instance, SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER); + + sdcard.multiWriteBlocksRemain = 0; + + sdcard.state = SDCARD_STATE_READY; + goto doMore; + } // else keep waiting for the CID to arrive + break; + case SDCARD_STATE_SENDING_WRITE: + // Have we finished sending the write yet? + sendComplete = false; + +#if defined(USE_HAL_DRIVER) + if (sdcard.useDMAForTx && DMA_GET_FLAG_STATUS(sdcard.dma, DMA_IT_TCIF)) { + //Clear both flags after transfer + DMA_CLEAR_FLAG(sdcard.dma, DMA_IT_TCIF); + DMA_CLEAR_FLAG(sdcard.dma, DMA_IT_HTIF); + // Drain anything left in the Rx FIFO (we didn't read it during the write) + while (LL_SPI_IsActiveFlag_RXNE(sdcard.busdev.busdev_u.spi.instance)) { + sdcard.busdev.busdev_u.spi.instance->DR; + } + + // Wait for the final bit to be transmitted + while (spiBusIsBusBusy(&sdcard.busdev)) { + } + + LL_SPI_DisableDMAReq_TX(sdcard.busdev.busdev_u.spi.instance); + + sendComplete = true; + } +#else +#ifdef STM32F4 + if (sdcard.useDMAForTx && DMA_GetFlagStatus(sdcard.dma->ref, sdcard.dma->completeFlag) == SET) { + DMA_ClearFlag(sdcard.dma->ref, sdcard.dma->completeFlag); +#else + if (sdcard.useDMAForTx && DMA_GetFlagStatus(sdcard.dma->completeFlag) == SET) { + DMA_ClearFlag(sdcard.dma->completeFlag); +#endif + + DMA_Cmd(sdcard.dma->ref, DISABLE); + + // Drain anything left in the Rx FIFO (we didn't read it during the write) + while (SPI_I2S_GetFlagStatus(sdcard.busdev.busdev_u.spi.instance, SPI_I2S_FLAG_RXNE) == SET) { + sdcard.busdev.busdev_u.spi.instance->DR; + } + + // Wait for the final bit to be transmitted + while (spiBusIsBusBusy(&sdcard.busdev)) { + } + + SPI_I2S_DMACmd(sdcard.busdev.busdev_u.spi.instance, SPI_I2S_DMAReq_Tx, DISABLE); + + sendComplete = true; + } +#endif + if (!sdcard.useDMAForTx) { + // Send another chunk + spiBusRawTransfer(&sdcard.busdev, sdcard.pendingOperation.buffer + SDCARD_NON_DMA_CHUNK_SIZE * sdcard.pendingOperation.chunkIndex, NULL, SDCARD_NON_DMA_CHUNK_SIZE); + + sdcard.pendingOperation.chunkIndex++; + + sendComplete = sdcard.pendingOperation.chunkIndex == SDCARD_BLOCK_SIZE / SDCARD_NON_DMA_CHUNK_SIZE; + } + + if (sendComplete) { + // Finish up by sending the CRC and checking the SD-card's acceptance/rejectance + if (sdcard_sendDataBlockFinish()) { + // The SD card is now busy committing that write to the card + sdcard.state = SDCARD_STATE_WAITING_FOR_WRITE; + sdcard.operationStartTime = millis(); + + // Since we've transmitted the buffer we can go ahead and tell the caller their operation is complete + if (sdcard.pendingOperation.callback) { + sdcard.pendingOperation.callback(SDCARD_BLOCK_OPERATION_WRITE, sdcard.pendingOperation.blockIndex, sdcard.pendingOperation.buffer, sdcard.pendingOperation.callbackData); + } + } else { + /* Our write was rejected! This could be due to a bad address but we hope not to attempt that, so assume + * the card is broken and needs reset. + */ + sdcard_reset(); + + // Announce write failure: + if (sdcard.pendingOperation.callback) { + sdcard.pendingOperation.callback(SDCARD_BLOCK_OPERATION_WRITE, sdcard.pendingOperation.blockIndex, NULL, sdcard.pendingOperation.callbackData); + } + + goto doMore; + } + } + break; + case SDCARD_STATE_WAITING_FOR_WRITE: + if (sdcard_waitForIdle(SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY)) { +#ifdef SDCARD_PROFILING + profilingComplete = true; +#endif + + sdcard.failureCount = 0; // Assume the card is good if it can complete a write + + // Still more blocks left to write in a multi-block chain? + if (sdcard.multiWriteBlocksRemain > 1) { + sdcard.multiWriteBlocksRemain--; + sdcard.multiWriteNextBlock++; + sdcard.state = SDCARD_STATE_WRITING_MULTIPLE_BLOCKS; + } else if (sdcard.multiWriteBlocksRemain == 1) { + // This function changes the sd card state for us whether immediately succesful or delayed: + if (sdcard_endWriteBlocks() == SDCARD_OPERATION_SUCCESS) { + sdcard_deselect(); + } else { +#ifdef SDCARD_PROFILING + // Wait for the multi-block write to be terminated before finishing timing + profilingComplete = false; +#endif + } + } else { + sdcard.state = SDCARD_STATE_READY; + sdcard_deselect(); + } + +#ifdef SDCARD_PROFILING + if (profilingComplete && sdcard.profiler) { + sdcard.profiler(SDCARD_BLOCK_OPERATION_WRITE, sdcard.pendingOperation.blockIndex, micros() - sdcard.pendingOperation.profileStartTime); + } +#endif + } else if (millis() > sdcard.operationStartTime + SDCARD_TIMEOUT_WRITE_MSEC) { + /* + * The caller has already been told that their write has completed, so they will have discarded + * their buffer and have no hope of retrying the operation. But this should be very rare and it allows + * them to reuse their buffer milliseconds faster than they otherwise would. + */ + sdcard_reset(); + goto doMore; + } + break; + case SDCARD_STATE_READING: + switch (sdcard_receiveDataBlock(sdcard.pendingOperation.buffer, SDCARD_BLOCK_SIZE)) { + case SDCARD_RECEIVE_SUCCESS: + sdcard_deselect(); + + sdcard.state = SDCARD_STATE_READY; + sdcard.failureCount = 0; // Assume the card is good if it can complete a read + +#ifdef SDCARD_PROFILING + if (sdcard.profiler) { + sdcard.profiler(SDCARD_BLOCK_OPERATION_READ, sdcard.pendingOperation.blockIndex, micros() - sdcard.pendingOperation.profileStartTime); + } +#endif + + if (sdcard.pendingOperation.callback) { + sdcard.pendingOperation.callback( + SDCARD_BLOCK_OPERATION_READ, + sdcard.pendingOperation.blockIndex, + sdcard.pendingOperation.buffer, + sdcard.pendingOperation.callbackData + ); + } + break; + case SDCARD_RECEIVE_BLOCK_IN_PROGRESS: + if (millis() <= sdcard.operationStartTime + SDCARD_TIMEOUT_READ_MSEC) { + break; // Timeout not reached yet so keep waiting + } + // Timeout has expired, so fall through to convert to a fatal error + FALLTHROUGH; + + case SDCARD_RECEIVE_ERROR: + sdcard_deselect(); + + sdcard_reset(); + + if (sdcard.pendingOperation.callback) { + sdcard.pendingOperation.callback( + SDCARD_BLOCK_OPERATION_READ, + sdcard.pendingOperation.blockIndex, + NULL, + sdcard.pendingOperation.callbackData + ); + } + + goto doMore; + break; + } + break; + case SDCARD_STATE_STOPPING_MULTIPLE_BLOCK_WRITE: + if (sdcard_waitForIdle(SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY)) { + sdcard_deselect(); + + sdcard.state = SDCARD_STATE_READY; + +#ifdef SDCARD_PROFILING + if (sdcard.profiler) { + sdcard.profiler(SDCARD_BLOCK_OPERATION_WRITE, sdcard.pendingOperation.blockIndex, micros() - sdcard.pendingOperation.profileStartTime); + } +#endif + } else if (millis() > sdcard.operationStartTime + SDCARD_TIMEOUT_WRITE_MSEC) { + sdcard_reset(); + goto doMore; + } + break; + case SDCARD_STATE_NOT_PRESENT: + default: + ; + } + + // Is the card's initialization taking too long? + if (sdcard.state >= SDCARD_STATE_RESET && sdcard.state < SDCARD_STATE_READY + && millis() - sdcard.operationStartTime > SDCARD_TIMEOUT_INIT_MILLIS) { + sdcard_reset(); + } + + return sdcard_isReady(); +} + +/** + * Write the 512-byte block from the given buffer into the block with the given index. + * + * If the write does not complete immediately, your callback will be called later. If the write was successful, the + * buffer pointer will be the same buffer you originally passed in, otherwise the buffer will be set to NULL. + * + * Returns: + * SDCARD_OPERATION_IN_PROGRESS - Your buffer is currently being transmitted to the card and your callback will be + * called later to report the completion. The buffer pointer must remain valid until + * that time. + * SDCARD_OPERATION_SUCCESS - Your buffer has been transmitted to the card now. + * SDCARD_OPERATION_BUSY - The card is already busy and cannot accept your write + * SDCARD_OPERATION_FAILURE - Your write was rejected by the card, card will be reset + */ +static sdcardOperationStatus_e sdcardSpi_writeBlock(uint32_t blockIndex, uint8_t *buffer, sdcard_operationCompleteCallback_c callback, uint32_t callbackData) +{ + uint8_t status; + +#ifdef SDCARD_PROFILING + sdcard.pendingOperation.profileStartTime = micros(); +#endif + + doMore: + switch (sdcard.state) { + case SDCARD_STATE_WRITING_MULTIPLE_BLOCKS: + // Do we need to cancel the previous multi-block write? + if (blockIndex != sdcard.multiWriteNextBlock) { + if (sdcard_endWriteBlocks() == SDCARD_OPERATION_SUCCESS) { + // Now we've entered the ready state, we can try again + goto doMore; + } else { + return SDCARD_OPERATION_BUSY; + } + } + + // We're continuing a multi-block write + break; + case SDCARD_STATE_READY: + // We're not continuing a multi-block write so we need to send a single-block write command + sdcard_select(); + + // Standard size cards use byte addressing, high capacity cards use block addressing + status = sdcard_sendCommand(SDCARD_COMMAND_WRITE_BLOCK, sdcard.highCapacity ? blockIndex : blockIndex * SDCARD_BLOCK_SIZE); + + if (status != 0) { + sdcard_deselect(); + + sdcard_reset(); + + return SDCARD_OPERATION_FAILURE; + } + break; + default: + return SDCARD_OPERATION_BUSY; + } + + sdcard_sendDataBlockBegin(buffer, sdcard.state == SDCARD_STATE_WRITING_MULTIPLE_BLOCKS); + + sdcard.pendingOperation.buffer = buffer; + sdcard.pendingOperation.blockIndex = blockIndex; + sdcard.pendingOperation.callback = callback; + sdcard.pendingOperation.callbackData = callbackData; + sdcard.pendingOperation.chunkIndex = 1; // (for non-DMA transfers) we've sent chunk #0 already + sdcard.state = SDCARD_STATE_SENDING_WRITE; + + return SDCARD_OPERATION_IN_PROGRESS; +} + +/** + * Begin writing a series of consecutive blocks beginning at the given block index. This will allow (but not require) + * the SD card to pre-erase the number of blocks you specifiy, which can allow the writes to complete faster. + * + * Afterwards, just call sdcard_writeBlock() as normal to write those blocks consecutively. + * + * It's okay to abort the multi-block write at any time by writing to a non-consecutive address, or by performing a read. + * + * Returns: + * SDCARD_OPERATION_SUCCESS - Multi-block write has been queued + * SDCARD_OPERATION_BUSY - The card is already busy and cannot accept your write + * SDCARD_OPERATION_FAILURE - A fatal error occured, card will be reset + */ +static sdcardOperationStatus_e sdcardSpi_beginWriteBlocks(uint32_t blockIndex, uint32_t blockCount) +{ + if (sdcard.state != SDCARD_STATE_READY) { + if (sdcard.state == SDCARD_STATE_WRITING_MULTIPLE_BLOCKS) { + if (blockIndex == sdcard.multiWriteNextBlock) { + // Assume that the caller wants to continue the multi-block write they already have in progress! + return SDCARD_OPERATION_SUCCESS; + } else if (sdcard_endWriteBlocks() != SDCARD_OPERATION_SUCCESS) { + return SDCARD_OPERATION_BUSY; + } // Else we've completed the previous multi-block write and can fall through to start the new one + } else { + return SDCARD_OPERATION_BUSY; + } + } + + sdcard_select(); + + if ( + sdcard_sendAppCommand(SDCARD_ACOMMAND_SET_WR_BLOCK_ERASE_COUNT, blockCount) == 0 + && sdcard_sendCommand(SDCARD_COMMAND_WRITE_MULTIPLE_BLOCK, sdcard.highCapacity ? blockIndex : blockIndex * SDCARD_BLOCK_SIZE) == 0 + ) { + sdcard.state = SDCARD_STATE_WRITING_MULTIPLE_BLOCKS; + sdcard.multiWriteBlocksRemain = blockCount; + sdcard.multiWriteNextBlock = blockIndex; + + // Leave the card selected + return SDCARD_OPERATION_SUCCESS; + } else { + sdcard_deselect(); + + sdcard_reset(); + + return SDCARD_OPERATION_FAILURE; + } +} + +/** + * Read the 512-byte block with the given index into the given 512-byte buffer. + * + * When the read completes, your callback will be called. If the read was successful, the buffer pointer will be the + * same buffer you originally passed in, otherwise the buffer will be set to NULL. + * + * You must keep the pointer to the buffer valid until the operation completes! + * + * Returns: + * true - The operation was successfully queued for later completion, your callback will be called later + * false - The operation could not be started due to the card being busy (try again later). + */ +static bool sdcardSpi_readBlock(uint32_t blockIndex, uint8_t *buffer, sdcard_operationCompleteCallback_c callback, uint32_t callbackData) +{ + if (sdcard.state != SDCARD_STATE_READY) { + if (sdcard.state == SDCARD_STATE_WRITING_MULTIPLE_BLOCKS) { + if (sdcard_endWriteBlocks() != SDCARD_OPERATION_SUCCESS) { + return false; + } + } else { + return false; + } + } + +#ifdef SDCARD_PROFILING + sdcard.pendingOperation.profileStartTime = micros(); +#endif + + sdcard_select(); + + // Standard size cards use byte addressing, high capacity cards use block addressing + uint8_t status = sdcard_sendCommand(SDCARD_COMMAND_READ_SINGLE_BLOCK, sdcard.highCapacity ? blockIndex : blockIndex * SDCARD_BLOCK_SIZE); + + if (status == 0) { + sdcard.pendingOperation.buffer = buffer; + sdcard.pendingOperation.blockIndex = blockIndex; + sdcard.pendingOperation.callback = callback; + sdcard.pendingOperation.callbackData = callbackData; + + sdcard.state = SDCARD_STATE_READING; + + sdcard.operationStartTime = millis(); + + // Leave the card selected for the whole transaction + + return true; + } else { + sdcard_deselect(); + + return false; + } +} + +/** + * Returns true if the SD card has successfully completed its startup procedures. + */ +static bool sdcardSpi_isInitialized(void) +{ + return sdcard.state >= SDCARD_STATE_READY; +} + +static const sdcardMetadata_t* sdcardSpi_getMetadata(void) +{ + return &sdcard.metadata; +} + +#ifdef SDCARD_PROFILING + +static void sdcardSpi_setProfilerCallback(sdcard_profilerCallback_c callback) +{ + sdcard.profiler = callback; +} + +#endif + +sdcardVTable_t sdcardSpiVTable = { + sdcardSpi_init, + sdcardSpi_readBlock, + sdcardSpi_beginWriteBlocks, + sdcardSpi_writeBlock, + sdcardSpi_poll, + sdcardSpi_isFunctional, + sdcardSpi_isInitialized, + sdcardSpi_getMetadata, +#ifdef SDCARD_PROFILING + sdcardSpi_setProfilerCallback, +#endif +}; + +#endif diff --git a/src/main/drivers/sdio_f4xx.c b/src/main/drivers/sdio_f4xx.c index c42141a13e..3a8653cdbf 100644 --- a/src/main/drivers/sdio_f4xx.c +++ b/src/main/drivers/sdio_f4xx.c @@ -31,9 +31,11 @@ #include "stdbool.h" #include -#include "sdmmc_sdio.h" - #include "platform.h" + +#ifdef USE_SDCARD_SDIO + +#include "sdmmc_sdio.h" #include "stm32f4xx_gpio.h" #include "pg/pg.h" @@ -1899,3 +1901,4 @@ void SDIO_DMA_ST6_IRQHandler(dmaChannelDescriptor_t *dma) } /* ------------------------------------------------------------------------------------------------------------------*/ +#endif diff --git a/src/main/drivers/sdio_f7xx.c b/src/main/drivers/sdio_f7xx.c index 7b9984a005..e33906c618 100644 --- a/src/main/drivers/sdio_f7xx.c +++ b/src/main/drivers/sdio_f7xx.c @@ -28,9 +28,11 @@ #include "stdbool.h" #include -#include "sdmmc_sdio.h" - #include "platform.h" + +#ifdef USE_SDCARD_SDIO + +#include "sdmmc_sdio.h" #include "stm32f7xx.h" #include "drivers/io.h" @@ -1899,3 +1901,4 @@ void SDMMC_DMA_ST6_IRQHandler(dmaChannelDescriptor_t *dma) } /* ------------------------------------------------------------------------------------------------------------------*/ +#endif diff --git a/src/main/drivers/serial_pinconfig.c b/src/main/drivers/serial_pinconfig.c index 7f2f149a14..c28f3c415a 100644 --- a/src/main/drivers/serial_pinconfig.c +++ b/src/main/drivers/serial_pinconfig.c @@ -24,6 +24,8 @@ #include "platform.h" +#ifdef USE_UART + #include "build/build_config.h" #include "io/serial.h" @@ -273,3 +275,4 @@ void pgResetFn_serialPinConfig(serialPinConfig_t *serialPinConfig) } } #endif +#endif diff --git a/src/main/drivers/serial_uart.c b/src/main/drivers/serial_uart.c index 660af4f4ef..826f879135 100644 --- a/src/main/drivers/serial_uart.c +++ b/src/main/drivers/serial_uart.c @@ -30,6 +30,8 @@ #include "platform.h" +#ifdef USE_UART + #include "build/build_config.h" #include "build/atomic.h" @@ -356,3 +358,4 @@ void UART8_IRQHandler(void) uartIrqHandler(s); } #endif +#endif diff --git a/src/main/drivers/serial_uart.h b/src/main/drivers/serial_uart.h index e7550c0357..ee22b64750 100644 --- a/src/main/drivers/serial_uart.h +++ b/src/main/drivers/serial_uart.h @@ -27,10 +27,6 @@ // Various serial routines return the buffer occupied size as uint8_t which would need to be extended in order to // increase size further. -#if defined(USE_UART1) || defined(USE_UART2) || defined(USE_UART3) || defined(USE_UART4) || defined(USE_UART5) || defined(USE_UART6) || defined(USE_UART7) || defined(USE_UART8) -#define USE_UART -#endif - typedef enum { UARTDEV_1 = 0, UARTDEV_2 = 1, diff --git a/src/main/drivers/serial_uart_hal.c b/src/main/drivers/serial_uart_hal.c index ab420b7c1a..668c696a9c 100644 --- a/src/main/drivers/serial_uart_hal.c +++ b/src/main/drivers/serial_uart_hal.c @@ -30,7 +30,10 @@ #include "platform.h" +#ifdef USE_UART + #include "build/build_config.h" +#include "build/atomic.h" #include "common/utils.h" #include "drivers/io.h" @@ -230,26 +233,40 @@ void uartSetMode(serialPort_t *instance, portMode_e mode) uartReconfigure(uartPort); } -void uartStartTxDMA(uartPort_t *s) +void uartTryStartTxDMA(uartPort_t *s) { - uint16_t size = 0; - uint32_t fromwhere=0; - HAL_UART_StateTypeDef state = HAL_UART_GetState(&s->Handle); - if ((state & HAL_UART_STATE_BUSY_TX) == HAL_UART_STATE_BUSY_TX) - return; + ATOMIC_BLOCK(NVIC_PRIO_SERIALUART_TXDMA) { + if (s->txDMAStream->CR & DMA_SxCR_EN) { + // DMA is already in progress + return; + } - if (s->port.txBufferHead > s->port.txBufferTail) { - size = s->port.txBufferHead - s->port.txBufferTail; - fromwhere = s->port.txBufferTail; - s->port.txBufferTail = s->port.txBufferHead; - } else { - size = s->port.txBufferSize - s->port.txBufferTail; - fromwhere = s->port.txBufferTail; - s->port.txBufferTail = 0; + HAL_UART_StateTypeDef state = HAL_UART_GetState(&s->Handle); + if ((state & HAL_UART_STATE_BUSY_TX) == HAL_UART_STATE_BUSY_TX) { + return; + } + + if (s->port.txBufferHead == s->port.txBufferTail) { + // No more data to transmit + s->txDMAEmpty = true; + return; + } + + uint16_t size; + uint32_t fromWhere = s->port.txBufferTail; + + if (s->port.txBufferHead > s->port.txBufferTail) { + size = s->port.txBufferHead - s->port.txBufferTail; + s->port.txBufferTail = s->port.txBufferHead; + } else { + size = s->port.txBufferSize - s->port.txBufferTail; + s->port.txBufferTail = 0; + } + + s->txDMAEmpty = false; + + HAL_UART_Transmit_DMA(&s->Handle, (uint8_t *)&s->port.txBuffer[fromWhere], size); } - s->txDMAEmpty = false; - //HAL_CLEANCACHE((uint8_t *)&s->port.txBuffer[fromwhere],size); - HAL_UART_Transmit_DMA(&s->Handle, (uint8_t *)&s->port.txBuffer[fromwhere], size); } uint32_t uartTotalRxBytesWaiting(const serialPort_t *instance) @@ -349,8 +366,7 @@ void uartWrite(serialPort_t *instance, uint8_t ch) } if (s->txDMAStream) { - if (!(s->txDMAStream->CR & 1)) - uartStartTxDMA(s); + uartTryStartTxDMA(s); } else { __HAL_UART_ENABLE_IT(&s->Handle, UART_IT_TXE); } @@ -444,3 +460,4 @@ void UART8_IRQHandler(void) uartIrqHandler(s); } #endif +#endif diff --git a/src/main/drivers/serial_uart_impl.h b/src/main/drivers/serial_uart_impl.h index ef8a50c6b9..b549ecf6ac 100644 --- a/src/main/drivers/serial_uart_impl.h +++ b/src/main/drivers/serial_uart_impl.h @@ -51,7 +51,7 @@ #endif #elif defined(STM32F7) #define UARTDEV_COUNT_MAX 8 -#define UARTHARDWARE_MAX_PINS 3 +#define UARTHARDWARE_MAX_PINS 4 #ifndef UART_RX_BUFFER_SIZE #define UART_RX_BUFFER_SIZE 128 #endif @@ -114,6 +114,13 @@ #define UARTDEV_COUNT (UARTDEV_COUNT_1 + UARTDEV_COUNT_2 + UARTDEV_COUNT_3 + UARTDEV_COUNT_4 + UARTDEV_COUNT_5 + UARTDEV_COUNT_6 + UARTDEV_COUNT_7 + UARTDEV_COUNT_8) +typedef struct uartPinDef_s { + ioTag_t pin; +#if defined(STM32F7) + uint8_t af; +#endif +} uartPinDef_t; + typedef struct uartHardware_s { UARTDevice_e device; // XXX Not required for full allocation USART_TypeDef* reg; @@ -125,8 +132,8 @@ typedef struct uartHardware_s { DMA_Stream_TypeDef *txDMAStream; DMA_Stream_TypeDef *rxDMAStream; #endif - ioTag_t rxPins[UARTHARDWARE_MAX_PINS]; - ioTag_t txPins[UARTHARDWARE_MAX_PINS]; + uartPinDef_t rxPins[UARTHARDWARE_MAX_PINS]; + uartPinDef_t txPins[UARTHARDWARE_MAX_PINS]; #if defined(STM32F7) uint32_t rcc_ahb1; rccPeriphTag_t rcc_apb2; @@ -134,9 +141,10 @@ typedef struct uartHardware_s { #else rccPeriphTag_t rcc; #endif +#if !defined(STM32F7) uint8_t af; +#endif #if defined(STM32F7) - uint8_t txIrq; uint8_t rxIrq; #else uint8_t irqn; @@ -153,8 +161,8 @@ extern const uartHardware_t uartHardware[]; typedef struct uartDevice_s { uartPort_t port; const uartHardware_t *hardware; - ioTag_t rx; - ioTag_t tx; + uartPinDef_t rx; + uartPinDef_t tx; volatile uint8_t rxBuffer[UART_RX_BUFFER_SIZE]; volatile uint8_t txBuffer[UART_TX_BUFFER_SIZE]; } uartDevice_t; @@ -163,11 +171,7 @@ extern uartDevice_t *uartDevmap[]; extern const struct serialPortVTable uartVTable[]; -#ifdef USE_HAL_DRIVER -void uartStartTxDMA(uartPort_t *s); -#else void uartTryStartTxDMA(uartPort_t *s); -#endif uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_e mode, portOptions_e options); diff --git a/src/main/drivers/serial_uart_init.c b/src/main/drivers/serial_uart_init.c index 6936cacbec..5b8d23140f 100644 --- a/src/main/drivers/serial_uart_init.c +++ b/src/main/drivers/serial_uart_init.c @@ -34,6 +34,8 @@ #include "platform.h" +#ifdef USE_UART + #include "build/build_config.h" #include "common/utils.h" @@ -245,3 +247,4 @@ serialPort_t *uartOpen(UARTDevice_e device, serialReceiveCallbackPtr rxCallback, return (serialPort_t *)s; } +#endif diff --git a/src/main/drivers/serial_uart_pinconfig.c b/src/main/drivers/serial_uart_pinconfig.c index d9d6f9ad90..57df5bd965 100644 --- a/src/main/drivers/serial_uart_pinconfig.c +++ b/src/main/drivers/serial_uart_pinconfig.c @@ -32,6 +32,8 @@ #include "platform.h" +#ifdef USE_UART + #include "build/build_config.h" #include "drivers/rcc.h" @@ -52,16 +54,19 @@ void uartPinConfigure(const serialPinConfig_t *pSerialPinConfig) const UARTDevice_e device = hardware->device; for (int pindex = 0 ; pindex < UARTHARDWARE_MAX_PINS ; pindex++) { - if (hardware->rxPins[pindex] && (hardware->rxPins[pindex] == pSerialPinConfig->ioTagRx[device])) - uartdev->rx = pSerialPinConfig->ioTagRx[device]; + if (hardware->rxPins[pindex].pin == pSerialPinConfig->ioTagRx[device]) { + uartdev->rx = hardware->rxPins[pindex]; + } - if (hardware->txPins[pindex] && (hardware->txPins[pindex] == pSerialPinConfig->ioTagTx[device])) - uartdev->tx = pSerialPinConfig->ioTagTx[device]; + if (hardware->txPins[pindex].pin == pSerialPinConfig->ioTagTx[device]) { + uartdev->tx = hardware->txPins[pindex]; + } } - if (uartdev->rx || uartdev->tx) { + if (uartdev->rx.pin || uartdev->tx.pin) { uartdev->hardware = hardware; uartDevmap[device] = uartdev++; } } } +#endif diff --git a/src/main/drivers/serial_uart_stm32f10x.c b/src/main/drivers/serial_uart_stm32f10x.c index b6a7ac3c9d..64f4dbd0ba 100644 --- a/src/main/drivers/serial_uart_stm32f10x.c +++ b/src/main/drivers/serial_uart_stm32f10x.c @@ -31,6 +31,8 @@ #include "platform.h" +#ifdef USE_UART + #include "drivers/system.h" #include "drivers/io.h" #include "drivers/nvic.h" @@ -41,8 +43,6 @@ #include "drivers/serial_uart.h" #include "drivers/serial_uart_impl.h" -#ifdef USE_UART - #ifdef USE_UART1_RX_DMA # define UART1_RX_DMA_CHANNEL DMA1_Channel5 #else @@ -67,8 +67,8 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { .reg = USART1, .rxDMAChannel = UART1_RX_DMA_CHANNEL, .txDMAChannel = UART1_TX_DMA_CHANNEL, - .rxPins = { DEFIO_TAG_E(PA10), DEFIO_TAG_E(PB7), IO_TAG_NONE }, - .txPins = { DEFIO_TAG_E(PA9), DEFIO_TAG_E(PB6), IO_TAG_NONE }, + .rxPins = { { DEFIO_TAG_E(PA10) }, { DEFIO_TAG_E(PB7) } }, + .txPins = { { DEFIO_TAG_E(PA9) }, { DEFIO_TAG_E(PB6) } }, //.af = GPIO_AF_USART1, .rcc = RCC_APB2(USART1), .irqn = USART1_IRQn, @@ -82,8 +82,8 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { .reg = USART2, .rxDMAChannel = UART2_RX_DMA_CHANNEL, .txDMAChannel = UART2_TX_DMA_CHANNEL, - .rxPins = { DEFIO_TAG_E(PA3), DEFIO_TAG_E(PD6), IO_TAG_NONE }, - .txPins = { DEFIO_TAG_E(PA2), DEFIO_TAG_E(PD5), IO_TAG_NONE }, + .rxPins = { { DEFIO_TAG_E(PA3) }, { DEFIO_TAG_E(PD6) } }, + .txPins = { { DEFIO_TAG_E(PA2) }, { DEFIO_TAG_E(PD5) } }, //.af = GPIO_AF_USART2, .rcc = RCC_APB1(USART2), .irqn = USART2_IRQn, @@ -97,8 +97,8 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { .reg = USART3, .rxDMAChannel = UART3_RX_DMA_CHANNEL, .txDMAChannel = UART3_TX_DMA_CHANNEL, - .rxPins = { DEFIO_TAG_E(PB11), DEFIO_TAG_E(PD9), DEFIO_TAG_E(PC11) }, - .txPins = { DEFIO_TAG_E(PB10), DEFIO_TAG_E(PD8), DEFIO_TAG_E(PC10) }, + .rxPins = { { DEFIO_TAG_E(PB11) }, { DEFIO_TAG_E(PD9) }, { DEFIO_TAG_E(PC11) } }, + .txPins = { { DEFIO_TAG_E(PB10) }, { DEFIO_TAG_E(PD8) }, { DEFIO_TAG_E(PC10) } }, //.af = GPIO_AF_USART3, .rcc = RCC_APB1(USART3), .irqn = USART3_IRQn, @@ -157,8 +157,8 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_e mode, s->txDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->DR; } - IO_t rxIO = IOGetByTag(uartdev->rx); - IO_t txIO = IOGetByTag(uartdev->tx); + IO_t rxIO = IOGetByTag(uartdev->rx.pin); + IO_t txIO = IOGetByTag(uartdev->tx.pin); if (options & SERIAL_BIDIR) { IOInit(txIO, OWNER_SERIAL_TX, RESOURCE_INDEX(device)); diff --git a/src/main/drivers/serial_uart_stm32f30x.c b/src/main/drivers/serial_uart_stm32f30x.c index 68574d5de3..65aedd3c44 100644 --- a/src/main/drivers/serial_uart_stm32f30x.c +++ b/src/main/drivers/serial_uart_stm32f30x.c @@ -32,6 +32,8 @@ #include "platform.h" +#ifdef USE_UART + #include "drivers/system.h" #include "drivers/io.h" #include "drivers/nvic.h" @@ -42,8 +44,6 @@ #include "drivers/serial_uart.h" #include "drivers/serial_uart_impl.h" -#ifdef USE_UART - // XXX Will DMA eventually be configurable? // XXX Do these belong here? @@ -90,8 +90,8 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { .reg = USART1, .rxDMAChannel = UART1_RX_DMA, .txDMAChannel = UART1_TX_DMA, - .rxPins = { DEFIO_TAG_E(PA10), DEFIO_TAG_E(PB7), DEFIO_TAG_E(PC5), DEFIO_TAG_E(PE1) }, - .txPins = { DEFIO_TAG_E(PA9), DEFIO_TAG_E(PB6), DEFIO_TAG_E(PC4), DEFIO_TAG_E(PE0) }, + .rxPins = { { DEFIO_TAG_E(PA10) }, { DEFIO_TAG_E(PB7) }, { DEFIO_TAG_E(PC5) }, { DEFIO_TAG_E(PE1) } }, + .txPins = { { DEFIO_TAG_E(PA9) }, { DEFIO_TAG_E(PB6) }, { DEFIO_TAG_E(PC4) }, { DEFIO_TAG_E(PE0) } }, .rcc = RCC_APB2(USART1), .af = GPIO_AF_7, .irqn = USART1_IRQn, @@ -106,8 +106,8 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { .reg = USART2, .rxDMAChannel = UART2_RX_DMA, .txDMAChannel = UART2_TX_DMA, - .rxPins = { DEFIO_TAG_E(PA15), DEFIO_TAG_E(PA3), DEFIO_TAG_E(PB4), DEFIO_TAG_E(PD6) }, - .txPins = { DEFIO_TAG_E(PA14), DEFIO_TAG_E(PA2), DEFIO_TAG_E(PB3), DEFIO_TAG_E(PD5) }, + .rxPins = { { DEFIO_TAG_E(PA15) }, { DEFIO_TAG_E(PA3) }, { DEFIO_TAG_E(PB4) }, { DEFIO_TAG_E(PD6) } }, + .txPins = { { DEFIO_TAG_E(PA14) }, { DEFIO_TAG_E(PA2) }, { DEFIO_TAG_E(PB3) }, { DEFIO_TAG_E(PD5) } }, .rcc = RCC_APB1(USART2), .af = GPIO_AF_7, .irqn = USART2_IRQn, @@ -122,8 +122,8 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { .reg = USART3, .rxDMAChannel = UART3_RX_DMA, .txDMAChannel = UART3_TX_DMA, - .rxPins = { DEFIO_TAG_E(PB11), DEFIO_TAG_E(PC11), DEFIO_TAG_E(PD9), IO_TAG_NONE }, - .txPins = { DEFIO_TAG_E(PB10), DEFIO_TAG_E(PC10), DEFIO_TAG_E(PD8), IO_TAG_NONE }, + .rxPins = { { DEFIO_TAG_E(PB11) }, { DEFIO_TAG_E(PC11) }, { DEFIO_TAG_E(PD9) } }, + .txPins = { { DEFIO_TAG_E(PB10) }, { DEFIO_TAG_E(PC10) }, { DEFIO_TAG_E(PD8) } }, .rcc = RCC_APB1(USART3), .af = GPIO_AF_7, .irqn = USART3_IRQn, @@ -139,8 +139,8 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { .reg = UART4, .rxDMAChannel = 0, // XXX UART4_RX_DMA !? .txDMAChannel = 0, // XXX UART4_TX_DMA !? - .rxPins = { DEFIO_TAG_E(PC11), IO_TAG_NONE, IO_TAG_NONE, IO_TAG_NONE }, - .txPins = { DEFIO_TAG_E(PC10), IO_TAG_NONE, IO_TAG_NONE, IO_TAG_NONE }, + .rxPins = { { DEFIO_TAG_E(PC11) } }, + .txPins = { { DEFIO_TAG_E(PC10) } }, .rcc = RCC_APB1(UART4), .af = GPIO_AF_5, .irqn = UART4_IRQn, @@ -156,8 +156,8 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { .reg = UART5, .rxDMAChannel = 0, .txDMAChannel = 0, - .rxPins = { DEFIO_TAG_E(PD2), IO_TAG_NONE, IO_TAG_NONE, IO_TAG_NONE }, - .txPins = { DEFIO_TAG_E(PC12), IO_TAG_NONE, IO_TAG_NONE, IO_TAG_NONE }, + .rxPins = { { DEFIO_TAG_E(PD2) } }, + .txPins = { { DEFIO_TAG_E(PC12) } }, .rcc = RCC_APB1(UART5), .af = GPIO_AF_5, .irqn = UART5_IRQn, @@ -242,7 +242,7 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_e mode, s->txDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->TDR; } - serialUARTInitIO(IOGetByTag(uartDev->tx), IOGetByTag(uartDev->rx), mode, options, hardware->af, device); + serialUARTInitIO(IOGetByTag(uartDev->tx.pin), IOGetByTag(uartDev->rx.pin), mode, options, hardware->af, device); if (!s->rxDMAChannel || !s->txDMAChannel) { NVIC_InitTypeDef NVIC_InitStructure; diff --git a/src/main/drivers/serial_uart_stm32f4xx.c b/src/main/drivers/serial_uart_stm32f4xx.c index 61ff2b49e9..e8cfd66560 100644 --- a/src/main/drivers/serial_uart_stm32f4xx.c +++ b/src/main/drivers/serial_uart_stm32f4xx.c @@ -27,6 +27,8 @@ #include "platform.h" +#ifdef USE_UART + #include "drivers/system.h" #include "drivers/io.h" #include "drivers/dma.h" @@ -37,8 +39,6 @@ #include "drivers/serial_uart.h" #include "drivers/serial_uart_impl.h" -#ifdef USE_UART - const uartHardware_t uartHardware[UARTDEV_COUNT] = { #ifdef USE_UART1 { @@ -51,8 +51,8 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { #ifdef USE_UART1_TX_DMA .txDMAStream = DMA2_Stream7, #endif - .rxPins = { DEFIO_TAG_E(PA10), DEFIO_TAG_E(PB7), IO_TAG_NONE }, - .txPins = { DEFIO_TAG_E(PA9), DEFIO_TAG_E(PB6), IO_TAG_NONE }, + .rxPins = { { DEFIO_TAG_E(PA10) }, { DEFIO_TAG_E(PB7) } }, + .txPins = { { DEFIO_TAG_E(PA9) }, { DEFIO_TAG_E(PB6) } }, .af = GPIO_AF_USART1, .rcc = RCC_APB2(USART1), .irqn = USART1_IRQn, @@ -72,8 +72,8 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { #ifdef USE_UART2_TX_DMA .txDMAStream = DMA1_Stream6, #endif - .rxPins = { DEFIO_TAG_E(PA3), DEFIO_TAG_E(PD6), IO_TAG_NONE }, - .txPins = { DEFIO_TAG_E(PA2), DEFIO_TAG_E(PD5), IO_TAG_NONE }, + .rxPins = { { DEFIO_TAG_E(PA3) }, { DEFIO_TAG_E(PD6) } }, + .txPins = { { DEFIO_TAG_E(PA2) }, { DEFIO_TAG_E(PD5) } }, .af = GPIO_AF_USART2, .rcc = RCC_APB1(USART2), .irqn = USART2_IRQn, @@ -93,8 +93,8 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { #ifdef USE_UART3_TX_DMA .txDMAStream = DMA1_Stream3, #endif - .rxPins = { DEFIO_TAG_E(PB11), DEFIO_TAG_E(PC11), DEFIO_TAG_E(PD9) }, - .txPins = { DEFIO_TAG_E(PB10), DEFIO_TAG_E(PC10), DEFIO_TAG_E(PD8) }, + .rxPins = { { DEFIO_TAG_E(PB11) }, { DEFIO_TAG_E(PC11) }, { DEFIO_TAG_E(PD9) } }, + .txPins = { { DEFIO_TAG_E(PB10) }, { DEFIO_TAG_E(PC10) }, { DEFIO_TAG_E(PD8) } }, .af = GPIO_AF_USART3, .rcc = RCC_APB1(USART3), .irqn = USART3_IRQn, @@ -114,8 +114,8 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { #ifdef USE_UART4_TX_DMA .txDMAStream = DMA1_Stream4, #endif - .rxPins = { DEFIO_TAG_E(PA1), DEFIO_TAG_E(PC11), IO_TAG_NONE }, - .txPins = { DEFIO_TAG_E(PA0), DEFIO_TAG_E(PC10), IO_TAG_NONE }, + .rxPins = { { DEFIO_TAG_E(PA1) }, { DEFIO_TAG_E(PC11) } }, + .txPins = { { DEFIO_TAG_E(PA0) }, { DEFIO_TAG_E(PC10) } }, .af = GPIO_AF_UART4, .rcc = RCC_APB1(UART4), .irqn = UART4_IRQn, @@ -135,8 +135,8 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { #ifdef USE_UART5_TX_DMA .txDMAStream = DMA1_Stream7, #endif - .rxPins = { DEFIO_TAG_E(PD2), IO_TAG_NONE, IO_TAG_NONE }, - .txPins = { DEFIO_TAG_E(PC12), IO_TAG_NONE, IO_TAG_NONE }, + .rxPins = { { DEFIO_TAG_E(PD2) } }, + .txPins = { { DEFIO_TAG_E(PC12) } }, .af = GPIO_AF_UART5, .rcc = RCC_APB1(UART5), .irqn = UART5_IRQn, @@ -156,8 +156,8 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { #ifdef USE_UART6_TX_DMA .txDMAStream = DMA2_Stream6, #endif - .rxPins = { DEFIO_TAG_E(PC7), DEFIO_TAG_E(PG9), IO_TAG_NONE }, - .txPins = { DEFIO_TAG_E(PC6), DEFIO_TAG_E(PG14), IO_TAG_NONE }, + .rxPins = { { DEFIO_TAG_E(PC7) }, { DEFIO_TAG_E(PG9) } }, + .txPins = { { DEFIO_TAG_E(PC6) }, { DEFIO_TAG_E(PG14) } }, .af = GPIO_AF_USART6, .rcc = RCC_APB2(USART6), .irqn = USART6_IRQn, @@ -234,8 +234,8 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_e mode, s->txDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->DR; } - IO_t txIO = IOGetByTag(uart->tx); - IO_t rxIO = IOGetByTag(uart->rx); + IO_t txIO = IOGetByTag(uart->tx.pin); + IO_t rxIO = IOGetByTag(uart->rx.pin); if (hardware->rcc) { RCC_ClockCmd(hardware->rcc, ENABLE); diff --git a/src/main/drivers/serial_uart_stm32f7xx.c b/src/main/drivers/serial_uart_stm32f7xx.c index 6cc4517be6..0154d255be 100644 --- a/src/main/drivers/serial_uart_stm32f7xx.c +++ b/src/main/drivers/serial_uart_stm32f7xx.c @@ -27,6 +27,8 @@ #include "platform.h" +#ifdef USE_UART + #include "drivers/system.h" #include "drivers/dma.h" #include "drivers/io.h" @@ -37,8 +39,6 @@ #include "drivers/serial_uart.h" #include "drivers/serial_uart_impl.h" -#ifdef USE_UART - static void handleUsartTxDma(uartPort_t *s); const uartHardware_t uartHardware[UARTDEV_COUNT] = { @@ -53,14 +53,24 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { #ifdef USE_UART1_TX_DMA .txDMAStream = DMA2_Stream7, #endif - .rxPins = { DEFIO_TAG_E(PA10), DEFIO_TAG_E(PB7), IO_TAG_NONE }, - .txPins = { DEFIO_TAG_E(PA9), DEFIO_TAG_E(PB6), IO_TAG_NONE }, - .af = GPIO_AF7_USART1, + .rxPins = { + { DEFIO_TAG_E(PA10), GPIO_AF7_USART1 }, + { DEFIO_TAG_E(PB7), GPIO_AF7_USART1 }, +#ifdef STM32F765xx + { DEFIO_TAG_E(PB15), GPIO_AF4_USART1 } +#endif + }, + .txPins = { + { DEFIO_TAG_E(PA9), GPIO_AF7_USART1 }, + { DEFIO_TAG_E(PB6), GPIO_AF7_USART1 }, +#ifdef STM32F765xx + { DEFIO_TAG_E(PB14), GPIO_AF4_USART1 } +#endif + }, #ifdef UART1_AHB1_PERIPHERALS .rcc_ahb1 = UART1_AHB1_PERIPHERALS, #endif .rcc_apb2 = RCC_APB2(USART1), - .txIrq = DMA2_ST7_HANDLER, .rxIrq = USART1_IRQn, .txPriority = NVIC_PRIO_SERIALUART1_TXDMA, .rxPriority = NVIC_PRIO_SERIALUART1 @@ -78,14 +88,18 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { #ifdef USE_UART2_TX_DMA .txDMAStream = DMA1_Stream6, #endif - .rxPins = { DEFIO_TAG_E(PA3), DEFIO_TAG_E(PD6), IO_TAG_NONE }, - .txPins = { DEFIO_TAG_E(PA2), DEFIO_TAG_E(PD5), IO_TAG_NONE }, - .af = GPIO_AF7_USART2, + .rxPins = { + { DEFIO_TAG_E(PA3), GPIO_AF7_USART2 }, + { DEFIO_TAG_E(PD6), GPIO_AF7_USART2 } + }, + .txPins = { + { DEFIO_TAG_E(PA2), GPIO_AF7_USART2 }, + { DEFIO_TAG_E(PD5), GPIO_AF7_USART2 } + }, #ifdef UART2_AHB1_PERIPHERALS .rcc_ahb1 = UART2_AHB1_PERIPHERALS, #endif .rcc_apb1 = RCC_APB1(USART2), - .txIrq = DMA1_ST6_HANDLER, .rxIrq = USART2_IRQn, .txPriority = NVIC_PRIO_SERIALUART2_TXDMA, .rxPriority = NVIC_PRIO_SERIALUART2 @@ -103,14 +117,20 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { #ifdef USE_UART3_TX_DMA .txDMAStream = DMA1_Stream3, #endif - .rxPins = { DEFIO_TAG_E(PB11), DEFIO_TAG_E(PC11), DEFIO_TAG_E(PD9) }, - .txPins = { DEFIO_TAG_E(PB10), DEFIO_TAG_E(PC10), DEFIO_TAG_E(PD8) }, - .af = GPIO_AF7_USART3, + .rxPins = { + { DEFIO_TAG_E(PB11), GPIO_AF7_USART3 }, + { DEFIO_TAG_E(PC11), GPIO_AF7_USART3 }, + { DEFIO_TAG_E(PD9), GPIO_AF7_USART3 } + }, + .txPins = { + { DEFIO_TAG_E(PB10), GPIO_AF7_USART3 }, + { DEFIO_TAG_E(PC10), GPIO_AF7_USART3 }, + { DEFIO_TAG_E(PD8), GPIO_AF7_USART3 } + }, #ifdef UART3_AHB1_PERIPHERALS .rcc_ahb1 = UART3_AHB1_PERIPHERALS, #endif .rcc_apb1 = RCC_APB1(USART3), - .txIrq = DMA1_ST3_HANDLER, .rxIrq = USART3_IRQn, .txPriority = NVIC_PRIO_SERIALUART3_TXDMA, .rxPriority = NVIC_PRIO_SERIALUART3 @@ -128,14 +148,26 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { #ifdef USE_UART4_TX_DMA .txDMAStream = DMA1_Stream4, #endif - .rxPins = { DEFIO_TAG_E(PA1), DEFIO_TAG_E(PC11), IO_TAG_NONE }, - .txPins = { DEFIO_TAG_E(PA0), DEFIO_TAG_E(PC10), IO_TAG_NONE }, - .af = GPIO_AF8_UART4, + .rxPins = { + { DEFIO_TAG_E(PA1), GPIO_AF8_UART4 }, + { DEFIO_TAG_E(PC11), GPIO_AF8_UART4 }, +#ifdef STM32F765xx + { DEFIO_TAG_E(PA11), GPIO_AF6_UART4 }, + { DEFIO_TAG_E(PD0), GPIO_AF8_UART4 } +#endif + }, + .txPins = { + { DEFIO_TAG_E(PA0), GPIO_AF8_UART4 }, + { DEFIO_TAG_E(PC10), GPIO_AF8_UART4 }, +#ifdef STM32F765xx + { DEFIO_TAG_E(PA12), GPIO_AF6_UART4 }, + { DEFIO_TAG_E(PD1), GPIO_AF8_UART4 } +#endif + }, #ifdef UART4_AHB1_PERIPHERALS .rcc_ahb1 = UART4_AHB1_PERIPHERALS, #endif .rcc_apb1 = RCC_APB1(UART4), - .txIrq = DMA1_ST4_HANDLER, .rxIrq = UART4_IRQn, .txPriority = NVIC_PRIO_SERIALUART4_TXDMA, .rxPriority = NVIC_PRIO_SERIALUART4 @@ -153,14 +185,26 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { #ifdef USE_UART5_TX_DMA .txDMAStream = DMA1_Stream7, #endif - .rxPins = { DEFIO_TAG_E(PD2), IO_TAG_NONE, IO_TAG_NONE }, - .txPins = { DEFIO_TAG_E(PC12), IO_TAG_NONE, IO_TAG_NONE }, - .af = GPIO_AF8_UART5, + .rxPins = { + { DEFIO_TAG_E(PD2), GPIO_AF8_UART5 }, +#ifdef STM32F765xx + { DEFIO_TAG_E(PB5), GPIO_AF1_UART5 }, + { DEFIO_TAG_E(PB8), GPIO_AF7_UART5 }, + { DEFIO_TAG_E(PB12), GPIO_AF8_UART5 } +#endif + }, + .txPins = { + { DEFIO_TAG_E(PC12), GPIO_AF8_UART5 }, +#ifdef STM32F765xx + { DEFIO_TAG_E(PB6), GPIO_AF1_UART5 }, + { DEFIO_TAG_E(PB9), GPIO_AF7_UART5 }, + { DEFIO_TAG_E(PB13), GPIO_AF8_UART5 } +#endif + }, #ifdef UART5_AHB1_PERIPHERALS .rcc_ahb1 = UART5_AHB1_PERIPHERALS, #endif .rcc_apb1 = RCC_APB1(UART5), - .txIrq = DMA1_ST7_HANDLER, .rxIrq = UART5_IRQn, .txPriority = NVIC_PRIO_SERIALUART5_TXDMA, .rxPriority = NVIC_PRIO_SERIALUART5 @@ -178,14 +222,18 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { #ifdef USE_UART6_TX_DMA .txDMAStream = DMA2_Stream6, #endif - .rxPins = { DEFIO_TAG_E(PC7), DEFIO_TAG_E(PG9), IO_TAG_NONE }, - .txPins = { DEFIO_TAG_E(PC6), DEFIO_TAG_E(PG14), IO_TAG_NONE }, - .af = GPIO_AF8_USART6, + .rxPins = { + { DEFIO_TAG_E(PC7), GPIO_AF8_USART6 }, + { DEFIO_TAG_E(PG9), GPIO_AF8_USART6 } + }, + .txPins = { + { DEFIO_TAG_E(PC6), GPIO_AF8_USART6 }, + { DEFIO_TAG_E(PG14), GPIO_AF8_USART6 } + }, #ifdef UART6_AHB1_PERIPHERALS .rcc_ahb1 = UART6_AHB1_PERIPHERALS, #endif .rcc_apb2 = RCC_APB2(USART6), - .txIrq = DMA2_ST6_HANDLER, .rxIrq = USART6_IRQn, .txPriority = NVIC_PRIO_SERIALUART6_TXDMA, .rxPriority = NVIC_PRIO_SERIALUART6 @@ -203,14 +251,26 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { #ifdef USE_UART7_TX_DMA .txDMAStream = DMA1_Stream1, #endif - .rxPins = { DEFIO_TAG_E(PE7), DEFIO_TAG_E(PF6), IO_TAG_NONE }, - .txPins = { DEFIO_TAG_E(PE8), DEFIO_TAG_E(PF7), IO_TAG_NONE }, - .af = GPIO_AF8_UART7, + .rxPins = { + { DEFIO_TAG_E(PE7), GPIO_AF8_UART7 }, + { DEFIO_TAG_E(PF6), GPIO_AF8_UART7 }, +#ifdef STM32F765xx + { DEFIO_TAG_E(PA8), GPIO_AF12_UART7 }, + { DEFIO_TAG_E(PB3), GPIO_AF12_UART7 } +#endif + }, + .txPins = { + { DEFIO_TAG_E(PE8), GPIO_AF8_UART7 }, + { DEFIO_TAG_E(PF7), GPIO_AF8_UART7 }, +#ifdef STM32F765xx + { DEFIO_TAG_E(PA15), GPIO_AF12_UART7 }, + { DEFIO_TAG_E(PB4), GPIO_AF12_UART7 } +#endif + }, #ifdef UART7_AHB1_PERIPHERALS .rcc_ahb1 = UART7_AHB1_PERIPHERALS, #endif .rcc_apb1 = RCC_APB1(UART7), - .txIrq = DMA1_ST1_HANDLER, .rxIrq = UART7_IRQn, .txPriority = NVIC_PRIO_SERIALUART7_TXDMA, .rxPriority = NVIC_PRIO_SERIALUART7 @@ -228,14 +288,16 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { #ifdef USE_UART8_TX_DMA .txDMAStream = DMA1_Stream0, #endif - .rxPins = { DEFIO_TAG_E(PE0), IO_TAG_NONE, IO_TAG_NONE }, - .txPins = { DEFIO_TAG_E(PE1), IO_TAG_NONE, IO_TAG_NONE }, - .af = GPIO_AF8_UART8, + .rxPins = { + { DEFIO_TAG_E(PE0), GPIO_AF8_UART8 } + }, + .txPins = { + { DEFIO_TAG_E(PE1), GPIO_AF8_UART8 } + }, #ifdef UART8_AHB1_PERIPHERALS .rcc_ahb1 = UART8_AHB1_PERIPHERALS, #endif .rcc_apb1 = RCC_APB1(UART8), - .txIrq = DMA1_ST0_HANDLER, .rxIrq = UART8_IRQn, .txPriority = NVIC_PRIO_SERIALUART8_TXDMA, .rxPriority = NVIC_PRIO_SERIALUART8 @@ -314,12 +376,7 @@ void uartIrqHandler(uartPort_t *s) static void handleUsartTxDma(uartPort_t *s) { - if (s->port.txBufferHead != s->port.txBufferTail) - uartStartTxDMA(s); - else - { - s->txDMAEmpty = true; - } + uartTryStartTxDMA(s); } void dmaIRQHandler(dmaChannelDescriptor_t* descriptor) @@ -362,8 +419,9 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_e mode, s->txDMAStream = hardware->txDMAStream; // DMA TX Interrupt - dmaInit(hardware->txIrq, OWNER_SERIAL_TX, RESOURCE_INDEX(device)); - dmaSetHandler(hardware->txIrq, dmaIRQHandler, hardware->txPriority, (uint32_t)uartdev); + dmaIdentifier_e identifier = dmaGetIdentifier(hardware->txDMAStream); + dmaInit(identifier, OWNER_SERIAL_TX, RESOURCE_INDEX(device)); + dmaSetHandler(identifier, dmaIRQHandler, hardware->txPriority, (uint32_t)uartdev); } s->txDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->TDR; @@ -371,8 +429,8 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_e mode, s->Handle.Instance = hardware->reg; - IO_t txIO = IOGetByTag(uartdev->tx); - IO_t rxIO = IOGetByTag(uartdev->rx); + IO_t txIO = IOGetByTag(uartdev->tx.pin); + IO_t rxIO = IOGetByTag(uartdev->rx.pin); if ((options & SERIAL_BIDIR) && txIO) { ioConfig_t ioCfg = IO_CONFIG( @@ -382,17 +440,17 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_e mode, ); IOInit(txIO, OWNER_SERIAL_TX, RESOURCE_INDEX(device)); - IOConfigGPIOAF(txIO, ioCfg, hardware->af); + IOConfigGPIOAF(txIO, ioCfg, uartdev->tx.af); } else { if ((mode & MODE_TX) && txIO) { IOInit(txIO, OWNER_SERIAL_TX, RESOURCE_INDEX(device)); - IOConfigGPIOAF(txIO, IOCFG_AF_PP, hardware->af); + IOConfigGPIOAF(txIO, IOCFG_AF_PP, uartdev->tx.af); } if ((mode & MODE_RX) && rxIO) { IOInit(rxIO, OWNER_SERIAL_RX, RESOURCE_INDEX(device)); - IOConfigGPIOAF(rxIO, IOCFG_AF_PP, hardware->af); + IOConfigGPIOAF(rxIO, IOCFG_AF_PP, uartdev->rx.af); } } diff --git a/src/main/drivers/serial_usb_vcp.c b/src/main/drivers/serial_usb_vcp.c index 712e9a7c28..c11a819125 100644 --- a/src/main/drivers/serial_usb_vcp.c +++ b/src/main/drivers/serial_usb_vcp.c @@ -28,15 +28,16 @@ #include "build/build_config.h" #include "common/utils.h" + #include "drivers/io.h" +#include "pg/pg.h" +#include "pg/usb.h" #if defined(STM32F4) #include "usb_core.h" #include "usbd_cdc_vcp.h" #ifdef USE_USB_CDC_HID #include "usbd_hid_cdc_wrapper.h" -#include "pg/pg.h" -#include "pg/usb.h" #endif #include "usb_io.h" #elif defined(STM32F7) @@ -44,8 +45,6 @@ #include "usb_io.h" #ifdef USE_USB_CDC_HID #include "usbd_cdc_hid.h" -#include "pg/pg.h" -#include "pg/usb.h" #endif USBD_HandleTypeDef USBD_Device; #else @@ -219,36 +218,38 @@ serialPort_t *usbVcpOpen(void) { vcpPort_t *s; + IOInit(IOGetByTag(IO_TAG(PA11)), OWNER_USB, 0); + IOInit(IOGetByTag(IO_TAG(PA12)), OWNER_USB, 0); + #if defined(STM32F4) usbGenerateDisconnectPulse(); - IOInit(IOGetByTag(IO_TAG(PA11)), OWNER_USB, 0); - IOInit(IOGetByTag(IO_TAG(PA12)), OWNER_USB, 0); + switch (usbDevConfig()->type) { #ifdef USE_USB_CDC_HID - if (usbDevConfig()->type == COMPOSITE) { + case COMPOSITE: USBD_Init(&USB_OTG_dev, USB_OTG_FS_CORE_ID, &USR_desc, &USBD_HID_CDC_cb, &USR_cb); - } else { + break; #endif + default: USBD_Init(&USB_OTG_dev, USB_OTG_FS_CORE_ID, &USR_desc, &USBD_CDC_cb, &USR_cb); -#ifdef USE_USB_CDC_HID + break; } -#endif #elif defined(STM32F7) usbGenerateDisconnectPulse(); - IOInit(IOGetByTag(IO_TAG(PA11)), OWNER_USB, 0); - IOInit(IOGetByTag(IO_TAG(PA12)), OWNER_USB, 0); /* Init Device Library */ USBD_Init(&USBD_Device, &VCP_Desc, 0); /* Add Supported Class */ + switch (usbDevConfig()->type) { #ifdef USE_USB_CDC_HID - if (usbDevConfig()->type == COMPOSITE) { + case COMPOSITE: USBD_RegisterClass(&USBD_Device, USBD_HID_CDC_CLASS); - } else + break; #endif - { + default: USBD_RegisterClass(&USBD_Device, USBD_CDC_CLASS); + break; } /* HID Interface doesn't have any callbacks... */ diff --git a/src/main/drivers/serial_usb_vcp.h b/src/main/drivers/serial_usb_vcp.h index daeaa2ce22..18cfeb929c 100644 --- a/src/main/drivers/serial_usb_vcp.h +++ b/src/main/drivers/serial_usb_vcp.h @@ -22,6 +22,14 @@ #include "drivers/serial.h" +#if defined(STM32F7) +#include "common/maths.h" + +#include "usbd_cdc.h" + +extern USBD_HandleTypeDef USBD_Device; +#endif + typedef struct { serialPort_t port; diff --git a/src/main/drivers/stm32f7xx_ll_ex.h b/src/main/drivers/stm32f7xx_ll_ex.h index d1e7ecd4e0..12895e5cf2 100644 --- a/src/main/drivers/stm32f7xx_ll_ex.h +++ b/src/main/drivers/stm32f7xx_ll_ex.h @@ -37,12 +37,12 @@ __STATIC_INLINE DMA_TypeDef *LL_EX_DMA_Stream_to_DMA(DMA_Stream_TypeDef *DMAx_St __STATIC_INLINE uint32_t LL_EX_DMA_Stream_to_Stream(DMA_Stream_TypeDef *DMAx_Streamy) { + STATIC_ASSERT(DMA1_Stream0_BASE - DMA1_BASE == sizeof(DMA_TypeDef), DMA_TypeDef_has_padding); + STATIC_ASSERT(DMA1_Stream1_BASE - DMA1_Stream0_BASE == sizeof(DMA_Stream_TypeDef), DMA_Stream_TypeDef_has_padding); + const size_t firstStreamOffset = sizeof(DMA_TypeDef); const size_t streamSize = sizeof(DMA_Stream_TypeDef); - STATIC_ASSERT(DMA1_Stream0_BASE - DMA1_BASE == firstStreamOffset, DMA_TypeDef_has_padding); - STATIC_ASSERT(DMA1_Stream1_BASE - DMA1_Stream0_BASE == streamSize, DMA_Stream_TypeDef_has_padding); - return (((uint32_t) DMAx_Streamy & DMA_STREAM_MASK) - firstStreamOffset) / streamSize; } diff --git a/src/main/drivers/system.c b/src/main/drivers/system.c index 982a7fb66b..e620687726 100644 --- a/src/main/drivers/system.c +++ b/src/main/drivers/system.c @@ -35,6 +35,7 @@ static uint32_t usTicks = 0; // current uptime for 1kHz systick timer. will rollover after 49 days. hopefully we won't care. static volatile uint32_t sysTickUptime = 0; +static volatile uint32_t sysTickValStamp = 0; // cached value of RCC->CSR uint32_t cachedRccCsrValue; @@ -57,6 +58,7 @@ void SysTick_Handler(void) { ATOMIC_BLOCK(NVIC_PRIO_MAX) { sysTickUptime++; + sysTickValStamp = SysTick->VAL; sysTickPending = 0; (void)(SysTick->CTRL); } @@ -108,12 +110,7 @@ uint32_t micros(void) do { ms = sysTickUptime; cycle_cnt = SysTick->VAL; - /* - * If the SysTick timer expired during the previous instruction, we need to give it a little time for that - * interrupt to be delivered before we can recheck sysTickUptime: - */ - asm volatile("\tnop\n"); - } while (ms != sysTickUptime); + } while (ms != sysTickUptime || cycle_cnt > sysTickValStamp); return (ms * 1000) + (usTicks * 1000 - cycle_cnt) / usTicks; } diff --git a/src/main/drivers/system_stm32f4xx.c b/src/main/drivers/system_stm32f4xx.c index 3a2d536987..535445170b 100644 --- a/src/main/drivers/system_stm32f4xx.c +++ b/src/main/drivers/system_stm32f4xx.c @@ -34,10 +34,6 @@ void SetSysClock(void); void systemReset(void) { - if (mpuResetFn) { - mpuResetFn(); - } - __disable_irq(); NVIC_SystemReset(); } @@ -47,10 +43,6 @@ PERSISTENT uint32_t bootloaderRequest = 0; void systemResetToBootloader(void) { - if (mpuResetFn) { - mpuResetFn(); - } - bootloaderRequest = BOOTLOADER_REQUEST_COOKIE; __disable_irq(); diff --git a/src/main/drivers/system_stm32f7xx.c b/src/main/drivers/system_stm32f7xx.c index 3f042498ab..c5bed7df7d 100644 --- a/src/main/drivers/system_stm32f7xx.c +++ b/src/main/drivers/system_stm32f7xx.c @@ -30,26 +30,20 @@ #include "drivers/nvic.h" #include "drivers/system.h" +#include "stm32f7xx_ll_cortex.h" + #define AIRCR_VECTKEY_MASK ((uint32_t)0x05FA0000) void SystemClock_Config(void); void systemReset(void) { - if (mpuResetFn) { - mpuResetFn(); - } - __disable_irq(); NVIC_SystemReset(); } void systemResetToBootloader(void) { - if (mpuResetFn) { - mpuResetFn(); - } - (*(__IO uint32_t *) (BKPSRAM_BASE + 4)) = 0xDEADBEEF; // flag that will be readable after reboot __disable_irq(); @@ -162,6 +156,10 @@ void systemInit(void) { checkForBootLoaderRequest(); + // Mark ITCM-RAM as read-only + LL_MPU_ConfigRegion(LL_MPU_REGION_NUMBER0, 0, RAMITCM_BASE, LL_MPU_REGION_SIZE_16KB | LL_MPU_REGION_PRIV_RO_URO); + LL_MPU_Enable(LL_MPU_CTRL_PRIVILEGED_DEFAULT); + //SystemClock_Config(); // Configure NVIC preempt/priority groups diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index 8ac2adb220..cde75e0654 100644 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -783,17 +783,6 @@ void timerInit(void) RCC_ClockCmd(timerRCC(timerHardware[i].tim), ENABLE); } -#if defined(STM32F3) || defined(STM32F4) - for (unsigned timerIndex = 0; timerIndex < USABLE_TIMER_CHANNEL_COUNT; timerIndex++) { - const timerHardware_t *timerHardwarePtr = &timerHardware[timerIndex]; - if (timerHardwarePtr->usageFlags == TIM_USE_NONE) { - continue; - } - // XXX IOConfigGPIOAF in timerInit should eventually go away. - IOConfigGPIOAF(IOGetByTag(timerHardwarePtr->tag), IOCFG_AF_PP, timerHardwarePtr->alternateFunction); - } -#endif - // initialize timer channel structures for (unsigned i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { timerChannelInfo[i].type = TYPE_FREE; diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index 32f28b4a3d..bc80fcea9a 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -63,15 +63,16 @@ typedef uint32_t timCNT_t; #endif typedef enum { - TIM_USE_ANY = 0x0, - TIM_USE_NONE = 0x0, - TIM_USE_PPM = 0x1, - TIM_USE_PWM = 0x2, - TIM_USE_MOTOR = 0x4, - TIM_USE_SERVO = 0x8, - TIM_USE_LED = 0x10, - TIM_USE_TRANSPONDER = 0x20, - TIM_USE_BEEPER = 0x40 + TIM_USE_ANY = 0x0, + TIM_USE_NONE = 0x0, + TIM_USE_PPM = 0x1, + TIM_USE_PWM = 0x2, + TIM_USE_MOTOR = 0x4, + TIM_USE_SERVO = 0x8, + TIM_USE_LED = 0x10, + TIM_USE_TRANSPONDER = 0x20, + TIM_USE_BEEPER = 0x40, + TIM_USE_CAMERA_CONTROL = 0x80, } timerUsageFlag_e; // use different types from capture and overflow - multiple overflow handlers are implemented as linked list diff --git a/src/main/drivers/timer_def.h b/src/main/drivers/timer_def.h index b138f4b071..72d11a5953 100644 --- a/src/main/drivers/timer_def.h +++ b/src/main/drivers/timer_def.h @@ -120,8 +120,8 @@ IO_TAG(pin), \ DEF_TIM_CHANNEL(CH_ ## chan), \ flags, \ - (DEF_TIM_OUTPUT(CH_ ## chan) | out), \ - DEF_TIM_DMA_COND( \ + (DEF_TIM_OUTPUT(CH_ ## chan) | out) \ + DEF_TIM_DMA_COND(/* add comma */ , \ DEF_TIM_DMA_CHANNEL(TCH_## tim ## _ ## chan), \ DEF_TIM_DMA_HANDLER(TCH_## tim ## _ ## chan) \ ) \ @@ -169,12 +169,12 @@ DEF_TIM_CHANNEL(CH_ ## chan), \ flags, \ (DEF_TIM_OUTPUT(CH_ ## chan) | out), \ - DEF_TIM_AF(TCH_## tim ## _ ## chan, pin), \ - DEF_TIM_DMA_COND( \ + DEF_TIM_AF(TCH_## tim ## _ ## chan, pin) \ + DEF_TIM_DMA_COND(/* add comma */ , \ DEF_TIM_DMA_CHANNEL(TCH_## tim ## _ ## chan), \ DEF_TIM_DMA_HANDLER(TCH_## tim ## _ ## chan) \ - ), \ - DEF_TIM_DMA_COND( \ + ) \ + DEF_TIM_DMA_COND(/* add comma */ , \ DEF_TIM_DMA_CHANNEL(TCH_## tim ## _UP), \ DEF_TIM_DMA_HANDLER(TCH_## tim ## _UP) \ ) \ @@ -386,13 +386,13 @@ DEF_TIM_CHANNEL(CH_ ## chan), \ flags, \ (DEF_TIM_OUTPUT(CH_ ## chan) | out), \ - DEF_TIM_AF(TIM_ ## tim), \ - DEF_TIM_DMA_COND( \ + DEF_TIM_AF(TIM_ ## tim) \ + DEF_TIM_DMA_COND(/* add comma */ , \ DEF_TIM_DMA_STREAM(dmaopt, TCH_## tim ## _ ## chan), \ DEF_TIM_DMA_CHANNEL(dmaopt, TCH_## tim ## _ ## chan), \ DEF_TIM_DMA_HANDLER(dmaopt, TCH_## tim ## _ ## chan) \ - ), \ - DEF_TIM_DMA_COND( \ + ) \ + DEF_TIM_DMA_COND(/* add comma */ , \ DEF_TIM_DMA_STREAM(0, TCH_## tim ## _UP), \ DEF_TIM_DMA_CHANNEL(0, TCH_## tim ## _UP), \ DEF_TIM_DMA_HANDLER(0, TCH_## tim ## _UP) \ @@ -492,13 +492,13 @@ DEF_TIM_CHANNEL(CH_ ## chan), \ flags, \ (DEF_TIM_OUTPUT(CH_ ## chan) | out), \ - DEF_TIM_AF(TCH_## tim ## _ ## chan, pin), \ - DEF_TIM_DMA_COND( \ + DEF_TIM_AF(TCH_## tim ## _ ## chan, pin) \ + DEF_TIM_DMA_COND(/* add comma */ , \ DEF_TIM_DMA_STREAM(dmaopt, TCH_## tim ## _ ## chan), \ DEF_TIM_DMA_CHANNEL(dmaopt, TCH_## tim ## _ ## chan), \ DEF_TIM_DMA_HANDLER(dmaopt, TCH_## tim ## _ ## chan) \ - ), \ - DEF_TIM_DMA_COND( \ + ) \ + DEF_TIM_DMA_COND(/* add comma */ , \ DEF_TIM_DMA_STREAM(0, TCH_## tim ## _UP), \ DEF_TIM_DMA_CHANNEL(0, TCH_## tim ## _UP), \ DEF_TIM_DMA_HANDLER(0, TCH_## tim ## _UP) \ @@ -553,9 +553,9 @@ #define DEF_TIM_DMA__BTCH_TIM5_CH3 D(1, 0, 6) #define DEF_TIM_DMA__BTCH_TIM5_CH4 D(1, 1, 6),D(1, 3, 6) -#define DEF_TIM_DMA__BTCH_TIM8_CH1 D(2, 2, 7),D(2, 2, 0) -#define DEF_TIM_DMA__BTCH_TIM8_CH2 D(2, 3, 7),D(2, 2, 0) -#define DEF_TIM_DMA__BTCH_TIM8_CH3 D(2, 4, 7),D(2, 2, 0) +#define DEF_TIM_DMA__BTCH_TIM8_CH1 D(2, 2, 0),D(2, 2, 7) +#define DEF_TIM_DMA__BTCH_TIM8_CH2 D(2, 2, 0),D(2, 3, 7) +#define DEF_TIM_DMA__BTCH_TIM8_CH3 D(2, 2, 0),D(2, 4, 7) #define DEF_TIM_DMA__BTCH_TIM8_CH4 D(2, 7, 7) #define DEF_TIM_DMA__BTCH_TIM4_CH4 NONE diff --git a/src/main/drivers/usb_io.c b/src/main/drivers/usb_io.c index 85944babe5..f08f835300 100644 --- a/src/main/drivers/usb_io.c +++ b/src/main/drivers/usb_io.c @@ -30,9 +30,8 @@ #include "usb_io.h" #include "sdcard.h" - #ifdef USE_USB_DETECT -static IO_t usbDetectPin = IO_NONE; +static IO_t usbDetectPin; #endif void usbCableDetectDeinit(void) @@ -47,9 +46,6 @@ void usbCableDetectDeinit(void) void usbCableDetectInit(void) { #ifdef USE_USB_DETECT -#ifndef USB_DETECT_PIN -#define USB_DETECT_PIN NONE -#endif usbDetectPin = IOGetByTag(IO_TAG(USB_DETECT_PIN)); IOInit(usbDetectPin, OWNER_USB_DETECT, 0); diff --git a/src/main/drivers/usb_msc.h b/src/main/drivers/usb_msc.h index 3995b3c170..d2467894a0 100644 --- a/src/main/drivers/usb_msc.h +++ b/src/main/drivers/usb_msc.h @@ -31,3 +31,4 @@ bool mscCheckBoot(void); uint8_t mscStart(void); bool mscCheckButton(void); void mscWaitForButton(void); +void systemResetToMsc(void); diff --git a/src/main/drivers/usb_msc_f4xx.c b/src/main/drivers/usb_msc_f4xx.c index 920be99fe6..a98c017268 100644 --- a/src/main/drivers/usb_msc_f4xx.c +++ b/src/main/drivers/usb_msc_f4xx.c @@ -44,21 +44,13 @@ #include "drivers/time.h" #include "drivers/usb_msc.h" +#include "drivers/accgyro/accgyro_mpu.h" + #include "pg/usb.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" @@ -152,4 +144,12 @@ void mscWaitForButton(void) } } } + +void systemResetToMsc(void) +{ + *((uint32_t *)0x2001FFF0) = MSC_MAGIC; + + __disable_irq(); + NVIC_SystemReset(); +} #endif diff --git a/src/main/drivers/usb_msc_f7xx.c b/src/main/drivers/usb_msc_f7xx.c index 65af03f351..a0c47a586f 100644 --- a/src/main/drivers/usb_msc_f7xx.c +++ b/src/main/drivers/usb_msc_f7xx.c @@ -39,9 +39,12 @@ #include "drivers/io.h" #include "drivers/light_led.h" #include "drivers/nvic.h" +#include "drivers/serial_usb_vcp.h" #include "drivers/time.h" #include "drivers/usb_msc.h" +#include "drivers/accgyro/accgyro_mpu.h" + #include "pg/usb.h" #include "vcp_hal/usbd_cdc_interface.h" @@ -49,8 +52,6 @@ #include "usbd_msc.h" #include "msc/usbd_storage.h" -USBD_HandleTypeDef USBD_Device; - #define DEBOUNCE_TIME_MS 20 static IO_t mscButton; @@ -148,4 +149,12 @@ void mscWaitForButton(void) } } } + +void systemResetToMsc(void) +{ + *((__IO uint32_t*) BKPSRAM_BASE + 16) = MSC_MAGIC; + + __disable_irq(); + NVIC_SystemReset(); +} #endif diff --git a/src/main/drivers/vtx_common.c b/src/main/drivers/vtx_common.c index 07fb85e9aa..1baaba6930 100644 --- a/src/main/drivers/vtx_common.c +++ b/src/main/drivers/vtx_common.c @@ -57,6 +57,15 @@ vtxDevType_e vtxCommonGetDeviceType(const vtxDevice_t *vtxDevice) return vtxDevice->vTable->getDeviceType(vtxDevice); } +bool vtxCommonDeviceIsReady(const vtxDevice_t *vtxDevice) +{ + if (vtxDevice && vtxDevice->vTable->isReady) { + return vtxDevice->vTable->isReady(vtxDevice); + } + + return false; +} + void vtxCommonProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs) { if (vtxDevice) { diff --git a/src/main/drivers/vtx_common.h b/src/main/drivers/vtx_common.h index 94c6747cc3..7b28e0af27 100644 --- a/src/main/drivers/vtx_common.h +++ b/src/main/drivers/vtx_common.h @@ -40,7 +40,6 @@ #define VTX_SETTINGS_DEFAULT_CHANNEL 1 #define VTX_SETTINGS_DEFAULT_FREQ 5740 #define VTX_SETTINGS_DEFAULT_PITMODE_FREQ 0 -#define VTX_SETTINGS_DEFAULT_LOW_POWER_DISARM 0 #define VTX_SETTINGS_MAX_FREQUENCY_MHZ 5999 //max freq (in MHz) for 'vtx_freq' setting @@ -166,7 +165,8 @@ vtxDevice_t *vtxCommonDevice(void); // VTable functions void vtxCommonProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs); -uint8_t vtxCommonGetDeviceType(const vtxDevice_t *vtxDevice); +vtxDevType_e vtxCommonGetDeviceType(const vtxDevice_t *vtxDevice); +bool vtxCommonDeviceIsReady(const vtxDevice_t *vtxDevice); void vtxCommonSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel); void vtxCommonSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t level); void vtxCommonSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff); diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 55ddccd9b6..164f68e0f1 100644 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -34,8 +34,8 @@ #include "fc/config.h" #include "fc/controlrate_profile.h" -#include "fc/fc_core.h" -#include "fc/fc_rc.h" +#include "fc/core.h" +#include "fc/rc.h" #include "fc/rc_adjustments.h" #include "fc/rc_controls.h" @@ -48,6 +48,7 @@ #include "io/beeper.h" #include "io/ledstrip.h" #include "io/serial.h" +#include "io/gps.h" #include "pg/beeper.h" #include "pg/beeper_dev.h" @@ -61,14 +62,14 @@ #include "sensors/battery.h" #include "sensors/gyro.h" -#ifndef USE_OSD_SLAVE pidProfile_t *currentPidProfile; -#endif #ifndef RX_SPI_DEFAULT_PROTOCOL #define RX_SPI_DEFAULT_PROTOCOL 0 #endif +#define DYNAMIC_FILTER_MAX_SUPPORTED_LOOP_TIME HZ_TO_INTERVAL_US(2000) + PG_REGISTER_WITH_RESET_TEMPLATE(pilotConfig_t, pilotConfig, PG_PILOT_CONFIG, 0); PG_RESET_TEMPLATE(pilotConfig_t, pilotConfig, @@ -87,7 +88,6 @@ PG_RESET_TEMPLATE(systemConfig_t, systemConfig, .boardIdentifier = TARGET_BOARD_IDENTIFIER ); -#ifndef USE_OSD_SLAVE uint8_t getCurrentPidProfileIndex(void) { return systemConfig()->pidProfileIndex; @@ -107,7 +107,6 @@ uint16_t getCurrentMinthrottle(void) { return motorConfig()->minthrottle; } -#endif // USE_OSD_SLAVE void resetConfigs(void) { @@ -120,7 +119,6 @@ void resetConfigs(void) static void activateConfig(void) { -#ifndef USE_OSD_SLAVE loadPidProfile(); loadControlRateProfile(); @@ -136,8 +134,7 @@ static void activateConfig(void) setAccelerationTrims(&accelerometerConfigMutable()->accZero); accInitFilters(); - imuConfigure(throttleCorrectionConfig()->throttle_correction_angle); -#endif // USE_OSD_SLAVE + imuConfigure(throttleCorrectionConfig()->throttle_correction_angle, throttleCorrectionConfig()->throttle_correction_value); #ifdef USE_LED_STRIP reevaluateLedConfig(); @@ -146,7 +143,7 @@ static void activateConfig(void) static void validateAndFixConfig(void) { -#if !defined(USE_QUAD_MIXER_ONLY) && !defined(USE_OSD_SLAVE) +#if !defined(USE_QUAD_MIXER_ONLY) // Reset unsupported mixer mode to default. // This check will be gone when motor/servo mixers are loaded dynamically // by configurator as a part of configuration procedure. @@ -163,7 +160,24 @@ static void validateAndFixConfig(void) } #endif -#ifndef USE_OSD_SLAVE + if (!isSerialConfigValid(serialConfig())) { + pgResetFn_serialConfig(serialConfigMutable()); + } + +#if defined(USE_GPS) + serialPortConfig_t *gpsSerial = findSerialPortConfig(FUNCTION_GPS); + if (gpsConfig()->provider == GPS_MSP && gpsSerial) { + serialRemovePort(gpsSerial->identifier); + } +#endif + if ( +#if defined(USE_GPS) + gpsConfig()->provider != GPS_MSP && !gpsSerial && +#endif + true) { + featureDisable(FEATURE_GPS); + } + if (systemConfig()->activeRateProfile >= CONTROL_RATE_PROFILE_COUNT) { systemConfigMutable()->activeRateProfile = 0; } @@ -179,8 +193,12 @@ static void validateAndFixConfig(void) currentPidProfile->dterm_notch_hz = 0; } - if ((motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) && (motorConfig()->mincommand < 1000)) { - motorConfigMutable()->mincommand = 1000; + if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) { + featureDisable(FEATURE_3D); + + if (motorConfig()->mincommand < 1000) { + motorConfigMutable()->mincommand = 1000; + } } if ((motorConfig()->dev.motorPwmProtocol == PWM_TYPE_STANDARD) && (motorConfig()->dev.motorPwmRate > BRUSHLESS_MOTORS_PWM_RATE)) { @@ -189,50 +207,40 @@ static void validateAndFixConfig(void) validateAndFixGyroConfig(); - if (!(featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_PPM) || featureConfigured(FEATURE_RX_SERIAL) || featureConfigured(FEATURE_RX_MSP) || featureConfigured(FEATURE_RX_SPI))) { - featureSet(DEFAULT_RX_FEATURE); + if (!(featureIsEnabled(FEATURE_RX_PARALLEL_PWM) || featureIsEnabled(FEATURE_RX_PPM) || featureIsEnabled(FEATURE_RX_SERIAL) || featureIsEnabled(FEATURE_RX_MSP) || featureIsEnabled(FEATURE_RX_SPI))) { + featureEnable(DEFAULT_RX_FEATURE); } - if (featureConfigured(FEATURE_RX_PPM)) { - featureClear(FEATURE_RX_SERIAL | FEATURE_RX_PARALLEL_PWM | FEATURE_RX_MSP | FEATURE_RX_SPI); + if (featureIsEnabled(FEATURE_RX_PPM)) { + featureDisable(FEATURE_RX_SERIAL | FEATURE_RX_PARALLEL_PWM | FEATURE_RX_MSP | FEATURE_RX_SPI); } - if (featureConfigured(FEATURE_RX_MSP)) { - featureClear(FEATURE_RX_SERIAL | FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM | FEATURE_RX_SPI); + if (featureIsEnabled(FEATURE_RX_MSP)) { + featureDisable(FEATURE_RX_SERIAL | FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM | FEATURE_RX_SPI); } - if (featureConfigured(FEATURE_RX_SERIAL)) { - featureClear(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_MSP | FEATURE_RX_PPM | FEATURE_RX_SPI); + if (featureIsEnabled(FEATURE_RX_SERIAL)) { + featureDisable(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_MSP | FEATURE_RX_PPM | FEATURE_RX_SPI); } #ifdef USE_RX_SPI - if (featureConfigured(FEATURE_RX_SPI)) { - featureClear(FEATURE_RX_SERIAL | FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM | FEATURE_RX_MSP); + if (featureIsEnabled(FEATURE_RX_SPI)) { + featureDisable(FEATURE_RX_SERIAL | FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM | FEATURE_RX_MSP); } #endif // USE_RX_SPI - if (featureConfigured(FEATURE_RX_PARALLEL_PWM)) { - featureClear(FEATURE_RX_SERIAL | FEATURE_RX_MSP | FEATURE_RX_PPM | FEATURE_RX_SPI); -#if defined(STM32F10X) - // rssi adc needs the same ports - featureClear(FEATURE_RSSI_ADC); - // current meter needs the same ports - if (batteryConfig()->currentMeterSource == CURRENT_METER_ADC) { - batteryConfigMutable()->currentMeterSource = CURRENT_METER_NONE; - } -#endif // STM32F10X - // software serial needs free PWM ports - featureClear(FEATURE_SOFTSERIAL); + if (featureIsEnabled(FEATURE_RX_PARALLEL_PWM)) { + featureDisable(FEATURE_RX_SERIAL | FEATURE_RX_MSP | FEATURE_RX_PPM | FEATURE_RX_SPI); } #ifdef USE_SOFTSPI - if (featureConfigured(FEATURE_SOFTSPI)) { - featureClear(FEATURE_RX_PPM | FEATURE_RX_PARALLEL_PWM | FEATURE_SOFTSERIAL); + if (featureIsEnabled(FEATURE_SOFTSPI)) { + featureDisable(FEATURE_RX_PPM | FEATURE_RX_PARALLEL_PWM | FEATURE_SOFTSERIAL); batteryConfigMutable()->voltageMeterSource = VOLTAGE_METER_NONE; #if defined(STM32F10X) - featureClear(FEATURE_LED_STRIP); + featureDisable(FEATURE_LED_STRIP); // rssi adc needs the same ports - featureClear(FEATURE_RSSI_ADC); + featureDisable(FEATURE_RSSI_ADC); // current meter needs the same ports if (batteryConfig()->currentMeterSource == CURRENT_METER_ADC) { batteryConfigMutable()->currentMeterSource = CURRENT_METER_NONE; @@ -242,31 +250,38 @@ static void validateAndFixConfig(void) #endif // USE_SOFTSPI #if defined(USE_ADC) - if (feature(FEATURE_RSSI_ADC)) { + if (featureIsEnabled(FEATURE_RSSI_ADC)) { rxConfigMutable()->rssi_channel = 0; rxConfigMutable()->rssi_src_frame_errors = false; } else #endif if (rxConfigMutable()->rssi_channel #if defined(USE_PWM) || defined(USE_PPM) - || feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) + || featureIsEnabled(FEATURE_RX_PPM) || featureIsEnabled(FEATURE_RX_PARALLEL_PWM) #endif ) { rxConfigMutable()->rssi_src_frame_errors = false; } - if (( -#if defined(USE_RC_SMOOTHING_FILTER) - rxConfig()->rc_smoothing_type == RC_SMOOTHING_TYPE_INTERPOLATION && -#endif - rxConfig()->rcInterpolation == RC_SMOOTHING_OFF) || rxConfig()->rcInterpolationChannels == INTERPOLATION_CHANNELS_T) { + if (!rcSmoothingIsEnabled() || rxConfig()->rcInterpolationChannels == INTERPOLATION_CHANNELS_T) { for (unsigned i = 0; i < MAX_PROFILE_COUNT; i++) { - pidProfilesMutable(i)->dtermSetpointWeight = 0; + pidProfilesMutable(i)->pid[PID_ROLL].F = 0; + pidProfilesMutable(i)->pid[PID_PITCH].F = 0; + } + } + + if (!rcSmoothingIsEnabled() || + (rxConfig()->rcInterpolationChannels != INTERPOLATION_CHANNELS_RPY && + rxConfig()->rcInterpolationChannels != INTERPOLATION_CHANNELS_RPYT)) { + + for (unsigned i = 0; i < MAX_PROFILE_COUNT; i++) { + pidProfilesMutable(i)->pid[PID_YAW].F = 0; } } #if defined(USE_THROTTLE_BOOST) - if (!(rxConfig()->rcInterpolationChannels == INTERPOLATION_CHANNELS_RPYT + if (!rcSmoothingIsEnabled() || + !(rxConfig()->rcInterpolationChannels == INTERPOLATION_CHANNELS_RPYT || rxConfig()->rcInterpolationChannels == INTERPOLATION_CHANNELS_T || rxConfig()->rcInterpolationChannels == INTERPOLATION_CHANNELS_RPT)) { for (unsigned i = 0; i < MAX_PROFILE_COUNT; i++) { @@ -274,15 +289,25 @@ static void validateAndFixConfig(void) } } #endif -#endif // USE_OSD_SLAVE - if (!isSerialConfigValid(serialConfig())) { - pgResetFn_serialConfig(serialConfigMutable()); + if ( + featureIsEnabled(FEATURE_3D) || !featureIsEnabled(FEATURE_GPS) +#if !defined(USE_GPS) || !defined(USE_GPS_RESCUE) + || true +#endif + ) { + if (failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_GPS_RESCUE) { + failsafeConfigMutable()->failsafe_procedure = FAILSAFE_PROCEDURE_DROP_IT; + } + + if (isModeActivationConditionPresent(BOXGPSRESCUE)) { + removeModeActivationCondition(BOXGPSRESCUE); + } } #if defined(USE_ESC_SENSOR) if (!findSerialPortConfig(FUNCTION_ESC_SENSOR)) { - featureClear(FEATURE_ESC_SENSOR); + featureDisable(FEATURE_ESC_SENSOR); } #endif @@ -290,75 +315,71 @@ static void validateAndFixConfig(void) // I have kept them all here in one place, some could be moved to sections of code above. #ifndef USE_PPM - featureClear(FEATURE_RX_PPM); + featureDisable(FEATURE_RX_PPM); #endif #ifndef USE_SERIAL_RX - featureClear(FEATURE_RX_SERIAL); + featureDisable(FEATURE_RX_SERIAL); #endif #if !defined(USE_SOFTSERIAL1) && !defined(USE_SOFTSERIAL2) - featureClear(FEATURE_SOFTSERIAL); -#endif - -#ifndef USE_GPS - featureClear(FEATURE_GPS); + featureDisable(FEATURE_SOFTSERIAL); #endif #ifndef USE_RANGEFINDER - featureClear(FEATURE_RANGEFINDER); + featureDisable(FEATURE_RANGEFINDER); #endif #ifndef USE_TELEMETRY - featureClear(FEATURE_TELEMETRY); + featureDisable(FEATURE_TELEMETRY); #endif #ifndef USE_PWM - featureClear(FEATURE_RX_PARALLEL_PWM); + featureDisable(FEATURE_RX_PARALLEL_PWM); #endif #ifndef USE_RX_MSP - featureClear(FEATURE_RX_MSP); + featureDisable(FEATURE_RX_MSP); #endif #ifndef USE_LED_STRIP - featureClear(FEATURE_LED_STRIP); + featureDisable(FEATURE_LED_STRIP); #endif #ifndef USE_DASHBOARD - featureClear(FEATURE_DASHBOARD); + featureDisable(FEATURE_DASHBOARD); #endif #ifndef USE_OSD - featureClear(FEATURE_OSD); + featureDisable(FEATURE_OSD); #endif #ifndef USE_SERVOS - featureClear(FEATURE_SERVO_TILT | FEATURE_CHANNEL_FORWARDING); + featureDisable(FEATURE_SERVO_TILT | FEATURE_CHANNEL_FORWARDING); #endif #ifndef USE_TRANSPONDER - featureClear(FEATURE_TRANSPONDER); + featureDisable(FEATURE_TRANSPONDER); #endif #ifndef USE_RX_SPI - featureClear(FEATURE_RX_SPI); + featureDisable(FEATURE_RX_SPI); #endif #ifndef USE_SOFTSPI - featureClear(FEATURE_SOFTSPI); + featureDisable(FEATURE_SOFTSPI); #endif #ifndef USE_ESC_SENSOR - featureClear(FEATURE_ESC_SENSOR); + featureDisable(FEATURE_ESC_SENSOR); #endif #ifndef USE_GYRO_DATA_ANALYSE - featureClear(FEATURE_DYNAMIC_FILTER); + featureDisable(FEATURE_DYNAMIC_FILTER); #endif #if !defined(USE_ADC) - featureClear(FEATURE_RSSI_ADC); + featureDisable(FEATURE_RSSI_ADC); #endif #if defined(USE_BEEPER) @@ -387,9 +408,15 @@ static void validateAndFixConfig(void) #endif } -#ifndef USE_OSD_SLAVE void validateAndFixGyroConfig(void) { +#ifdef USE_GYRO_DATA_ANALYSE + // Disable dynamic filter if gyro loop is less than 2KHz + if (gyro.targetLooptime > DYNAMIC_FILTER_MAX_SUPPORTED_LOOP_TIME) { + featureDisable(FEATURE_DYNAMIC_FILTER); + } +#endif + // Prevent invalid notch cutoff if (gyroConfig()->gyro_soft_notch_cutoff_1 >= gyroConfig()->gyro_soft_notch_hz_1) { gyroConfigMutable()->gyro_soft_notch_hz_1 = 0; @@ -398,7 +425,7 @@ void validateAndFixGyroConfig(void) gyroConfigMutable()->gyro_soft_notch_hz_2 = 0; } - if (gyroConfig()->gyro_hardware_lpf != GYRO_HARDWARE_LPF_NORMAL && gyroConfig()->gyro_hardware_lpf != GYRO_HARDWARE_LPF_EXPERIMENTAL) { + if (gyroConfig()->gyro_hardware_lpf == GYRO_HARDWARE_LPF_1KHZ_SAMPLE) { pidConfigMutable()->pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed gyroConfigMutable()->gyro_sync_denom = 1; gyroConfigMutable()->gyro_use_32khz = false; @@ -429,7 +456,7 @@ void validateAndFixGyroConfig(void) samplingTime = 0.000125f; break; } - if (gyroConfig()->gyro_hardware_lpf != GYRO_HARDWARE_LPF_NORMAL && gyroConfig()->gyro_hardware_lpf != GYRO_HARDWARE_LPF_EXPERIMENTAL) { + if (gyroConfig()->gyro_hardware_lpf == GYRO_HARDWARE_LPF_1KHZ_SAMPLE) { switch (gyroMpuDetectionResult()->sensor) { case ICM_20649_SPI: samplingTime = 1.0f / 1100.0f; @@ -482,13 +509,10 @@ void validateAndFixGyroConfig(void) } } } -#endif // USE_OSD_SLAVE bool readEEPROM(void) { -#ifndef USE_OSD_SLAVE - suspendRxSignal(); -#endif + suspendRxPwmPpmSignal(); // Sanity check, read flash bool success = loadEEPROM(); @@ -497,9 +521,7 @@ bool readEEPROM(void) activateConfig(); -#ifndef USE_OSD_SLAVE - resumeRxSignal(); -#endif + resumeRxPwmPpmSignal(); return success; } @@ -508,15 +530,19 @@ void writeEEPROM(void) { validateAndFixConfig(); -#ifndef USE_OSD_SLAVE - suspendRxSignal(); -#endif + suspendRxPwmPpmSignal(); writeConfigToEEPROM(); -#ifndef USE_OSD_SLAVE - resumeRxSignal(); -#endif + resumeRxPwmPpmSignal(); +} + +void writeEEPROMWithFeatures(uint32_t features) +{ + featureDisableAll(); + featureEnable(features); + + writeEEPROM(); } void resetEEPROM(void) @@ -543,7 +569,6 @@ void saveConfigAndNotify(void) beeperConfirmationBeeps(1); } -#ifndef USE_OSD_SLAVE void changePidProfile(uint8_t pidProfileIndex) { if (pidProfileIndex < MAX_PROFILE_COUNT) { @@ -553,4 +578,3 @@ void changePidProfile(uint8_t pidProfileIndex) beeperConfirmationBeeps(pidProfileIndex + 1); } -#endif diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 062163d3af..5a665b691d 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -53,6 +53,7 @@ void initEEPROM(void); void resetEEPROM(void); bool readEEPROM(void); void writeEEPROM(void); +void writeEEPROMWithFeatures(uint32_t features); void ensureEEPROMStructureIsValid(void); void saveConfigAndNotify(void); diff --git a/src/main/fc/controlrate_profile.c b/src/main/fc/controlrate_profile.c index 1b60d95a4a..0d6b330343 100644 --- a/src/main/fc/controlrate_profile.c +++ b/src/main/fc/controlrate_profile.c @@ -32,7 +32,8 @@ #include "fc/config.h" #include "fc/controlrate_profile.h" -#include "fc/fc_rc.h" +#include "fc/rc.h" +#include "fc/rc_controls.h" controlRateConfig_t *currentControlRateProfile; @@ -57,7 +58,10 @@ void pgResetFn_controlRateProfiles(controlRateConfig_t *controlRateConfig) .rates[FD_PITCH] = 70, .rates[FD_YAW] = 70, .throttle_limit_type = THROTTLE_LIMIT_TYPE_OFF, - .throttle_limit_percent = 100 + .throttle_limit_percent = 100, + .rate_limit[FD_ROLL] = CONTROL_RATE_CONFIG_RATE_LIMIT_MAX, + .rate_limit[FD_PITCH] = CONTROL_RATE_CONFIG_RATE_LIMIT_MAX, + .rate_limit[FD_YAW] = CONTROL_RATE_CONFIG_RATE_LIMIT_MAX ); } } @@ -78,7 +82,7 @@ void changeControlRateProfile(uint8_t controlRateProfileIndex) } void copyControlRateProfile(const uint8_t dstControlRateProfileIndex, const uint8_t srcControlRateProfileIndex) { - if ((dstControlRateProfileIndex < CONTROL_RATE_PROFILE_COUNT-1 && srcControlRateProfileIndex < CONTROL_RATE_PROFILE_COUNT-1) + if ((dstControlRateProfileIndex < CONTROL_RATE_PROFILE_COUNT && srcControlRateProfileIndex < CONTROL_RATE_PROFILE_COUNT) && dstControlRateProfileIndex != srcControlRateProfileIndex ) { memcpy(controlRateProfilesMutable(dstControlRateProfileIndex), controlRateProfilesMutable(srcControlRateProfileIndex), sizeof(controlRateConfig_t)); diff --git a/src/main/fc/controlrate_profile.h b/src/main/fc/controlrate_profile.h index ab4e4c2f12..865d1787b8 100644 --- a/src/main/fc/controlrate_profile.h +++ b/src/main/fc/controlrate_profile.h @@ -48,6 +48,7 @@ typedef struct controlRateConfig_s { uint16_t tpa_breakpoint; // Breakpoint where TPA is activated uint8_t throttle_limit_type; // Sets the throttle limiting type - off, scale or clip uint8_t throttle_limit_percent; // Sets the maximum pilot commanded throttle limit + uint16_t rate_limit[3]; // Sets the maximum rate for the axes } controlRateConfig_t; PG_DECLARE_ARRAY(controlRateConfig_t, CONTROL_RATE_PROFILE_COUNT, controlRateProfiles); diff --git a/src/main/fc/fc_core.c b/src/main/fc/core.c similarity index 89% rename from src/main/fc/fc_core.c rename to src/main/fc/core.c index 4d03c0ebb5..8ff491b2f1 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/core.c @@ -40,12 +40,10 @@ #include "pg/rx.h" #include "drivers/light_led.h" -#include "drivers/serial_usb_vcp.h" #include "drivers/sound_beeper.h" #include "drivers/system.h" #include "drivers/time.h" #include "drivers/transponder_ir.h" -#include "drivers/usb_io.h" #include "sensors/acceleration.h" #include "sensors/barometer.h" @@ -56,9 +54,8 @@ #include "fc/config.h" #include "fc/controlrate_profile.h" -#include "fc/fc_core.h" -#include "fc/fc_dispatch.h" -#include "fc/fc_rc.h" +#include "fc/core.h" +#include "fc/rc.h" #include "fc/rc_adjustments.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" @@ -67,7 +64,6 @@ #include "interface/cli.h" -#include "io/asyncfatfs/asyncfatfs.h" #include "io/beeper.h" #include "io/gps.h" #include "io/motors.h" @@ -102,6 +98,11 @@ enum { ALIGN_MAG = 2 }; +enum { + ARMING_DELAYED_DISARMED = 0, + ARMING_DELAYED_NORMAL = 1, + ARMING_DELAYED_CRASHFLIP = 2 +}; #define GYRO_WATCHDOG_DELAY 80 // delay for gyro sync @@ -123,20 +124,18 @@ enum { #define DEBUG_RUNAWAY_TAKEOFF_FALSE 0 #endif -#define PARALYZE_PREVENT_MODE_CHANGES_DELAY_US 100000 // Delay after paralyze mode activates to let other mode changes propagate - #if defined(USE_GPS) || defined(USE_MAG) int16_t magHold; #endif static bool flipOverAfterCrashMode = false; -static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero +static timeUs_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero bool isRXDataNew; static int lastArmingDisabledReason = 0; static timeUs_t lastDisarmTimeUs; -static bool tryingToArm; +static int tryingToArm = ARMING_DELAYED_DISARMED; #ifdef USE_RUNAWAY_TAKEOFF static timeUs_t runawayTakeoffDeactivateUs = 0; @@ -146,15 +145,6 @@ static timeUs_t runawayTakeoffTriggerUs = 0; static bool runawayTakeoffTemporarilyDisabled = false; #endif -static bool paralyzeModeAllowed = false; - -void preventModeChangesDispatch(dispatchEntry_t *self) { - UNUSED(self); - preventModeChanges(); -} - -static dispatchEntry_t preventModeChangesDispatchEntry = { .dispatch = preventModeChangesDispatch}; - PG_REGISTER_WITH_RESET_TEMPLATE(throttleCorrectionConfig_t, throttleCorrectionConfig, PG_THROTTLE_CORRECTION_CONFIG, 0); PG_RESET_TEMPLATE(throttleCorrectionConfig_t, throttleCorrectionConfig, @@ -260,17 +250,16 @@ void updateArmingStatus(void) #ifdef USE_GPS_RESCUE if (isModeActivationConditionPresent(BOXGPSRESCUE)) { - if (rescueState.sensor.numSat < gpsRescueConfig()->minSats) { - setArmingDisabled(ARMING_DISABLED_GPS); - } else { + if (!gpsRescueConfig()->minSats || STATE(GPS_FIX_HOME) || ARMING_FLAG(WAS_EVER_ARMED)) { unsetArmingDisabled(ARMING_DISABLED_GPS); + } else { + setArmingDisabled(ARMING_DISABLED_GPS); } } #endif - if (IS_RC_MODE_ACTIVE(BOXPARALYZE) && paralyzeModeAllowed) { + if (IS_RC_MODE_ACTIVE(BOXPARALYZE)) { setArmingDisabled(ARMING_DISABLED_PARALYZE); - dispatchAdd(&preventModeChangesDispatchEntry, PARALYZE_PREVENT_MODE_CHANGES_DELAY_US); } if (!isUsingSticksForArming()) { @@ -279,7 +268,7 @@ void updateArmingStatus(void) && !(getArmingDisableFlags() & ~(ARMING_DISABLED_ARM_SWITCH | ARMING_DISABLED_CALIBRATING)); /* Ignore ARMING_DISABLED_THROTTLE (once arm switch is on) if we are in 3D mode */ - bool ignoreThrottle = feature(FEATURE_3D) + bool ignoreThrottle = featureIsEnabled(FEATURE_3D) && !IS_RC_MODE_ACTIVE(BOX3D) && !flight3DConfig()->switched_mode3d && !(getArmingDisableFlags() & ~(ARMING_DISABLED_ARM_SWITCH | ARMING_DISABLED_THROTTLE)); @@ -309,11 +298,6 @@ void updateArmingStatus(void) warningLedUpdate(); } - - // Check if entering into paralyze mode can be allowed regardless of arming status - if (!IS_RC_MODE_ACTIVE(BOXPARALYZE) && !paralyzeModeAllowed) { - paralyzeModeAllowed = true; - } } void disarm(void) @@ -329,13 +313,12 @@ void disarm(void) #endif BEEP_OFF; #ifdef USE_DSHOT - if (isMotorProtocolDshot() && isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH) && !feature(FEATURE_3D)) { - flipOverAfterCrashMode = false; - if (!feature(FEATURE_3D)) { - pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL, false); - } + if (isMotorProtocolDshot() && flipOverAfterCrashMode && !featureIsEnabled(FEATURE_3D)) { + pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL, false); } #endif + flipOverAfterCrashMode = false; + // if ARMING_DISABLED_RUNAWAY_TAKEOFF is set then we want to play it's beep pattern instead if (!(getArmingDisableFlags() & ARMING_DISABLED_RUNAWAY_TAKEOFF)) { beeper(BEEPER_DISARMING); // emit disarm tone @@ -355,15 +338,24 @@ void tryArm(void) if (ARMING_FLAG(ARMED)) { return; } + + const timeUs_t currentTimeUs = micros(); + #ifdef USE_DSHOT - if (micros() - getLastDshotBeaconCommandTimeUs() < DSHOT_BEACON_GUARD_DELAY_US) { - tryingToArm = true; + if (currentTimeUs - getLastDshotBeaconCommandTimeUs() < DSHOT_BEACON_GUARD_DELAY_US) { + if (tryingToArm == ARMING_DELAYED_DISARMED) { + if (isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH) && IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) { + tryingToArm = ARMING_DELAYED_CRASHFLIP; + } else { + tryingToArm = ARMING_DELAYED_NORMAL; + } + } return; } if (isMotorProtocolDshot() && isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH)) { - if (!IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) { + if (!(IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH) || (tryingToArm == ARMING_DELAYED_CRASHFLIP))) { flipOverAfterCrashMode = false; - if (!feature(FEATURE_3D)) { + if (!featureIsEnabled(FEATURE_3D)) { pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL, false); } } else { @@ -371,7 +363,7 @@ void tryArm(void) #ifdef USE_RUNAWAY_TAKEOFF runawayTakeoffCheckDisabled = false; #endif - if (!feature(FEATURE_3D)) { + if (!featureIsEnabled(FEATURE_3D)) { pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_REVERSED, false); } } @@ -392,13 +384,13 @@ void tryArm(void) } imuQuaternionHeadfreeOffsetSet(); - disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000; // start disarm timeout, will be extended when throttle is nonzero + disarmAt = currentTimeUs + armingConfig()->auto_disarm_delay * 1e6; // start disarm timeout, will be extended when throttle is nonzero lastArmingDisabledReason = 0; //beep to indicate arming #ifdef USE_GPS - if (feature(FEATURE_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5) { + if (featureIsEnabled(FEATURE_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5) { beeper(BEEPER_ARMING_GPS_FIX); } else { beeper(BEEPER_ARMING); @@ -528,7 +520,7 @@ void runawayTakeoffTemporaryDisable(uint8_t disableFlag) uint8_t calculateThrottlePercent(void) { uint8_t ret = 0; - if (feature(FEATURE_3D) + if (featureIsEnabled(FEATURE_3D) && !IS_RC_MODE_ACTIVE(BOX3D) && !flight3DConfig()->switched_mode3d) { @@ -549,7 +541,7 @@ uint8_t calculateThrottlePercent(void) static bool airmodeIsActivated; -bool isAirmodeActivated() +bool isAirmodeActivated() { return airmodeIsActivated; } @@ -562,14 +554,16 @@ bool isAirmodeActivated() bool processRx(timeUs_t currentTimeUs) { static bool armedBeeperOn = false; +#ifdef USE_TELEMETRY static bool sharedPortTelemetryEnabled = false; +#endif if (!calculateRxChannelsAndUpdateFailsafe(currentTimeUs)) { return false; } // in 3D mode, we need to be able to disarm by switch at any time - if (feature(FEATURE_3D)) { + if (featureIsEnabled(FEATURE_3D)) { if (!IS_RC_MODE_ACTIVE(BOXARM)) disarm(); } @@ -584,18 +578,18 @@ bool processRx(timeUs_t currentTimeUs) const throttleStatus_e throttleStatus = calculateThrottleStatus(); const uint8_t throttlePercent = calculateThrottlePercent(); - if (isAirmodeActive() && ARMING_FLAG(ARMED)) { + if (airmodeIsEnabled() && ARMING_FLAG(ARMED)) { if (throttlePercent >= rxConfig()->airModeActivateThreshold) { - airmodeIsActivated = true; // Prevent Iterm from being reset + airmodeIsActivated = true; // Prevent iterm from being reset } } else { airmodeIsActivated = false; } - /* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered. - This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air */ + /* In airmode iterm should be prevented to grow when Low thottle and Roll + Pitch Centered. + This is needed to prevent iterm winding on the ground, but keep full stabilisation on 0 throttle while in air */ if (throttleStatus == THROTTLE_LOW && !airmodeIsActivated) { - pidResetITerm(); + pidResetIterm(); if (currentPidProfile->pidAtMinThrottle) pidStabilisationState(PID_STABILISATION_ON); else @@ -624,7 +618,7 @@ bool processRx(timeUs_t currentTimeUs) // - sticks are active and have deflection greater than runaway_takeoff_deactivate_stick_percent // - pidSum on all axis is less then runaway_takeoff_deactivate_pidlimit bool inStableFlight = false; - if (!feature(FEATURE_MOTOR_STOP) || isAirmodeActive() || (throttleStatus != THROTTLE_LOW)) { // are motors running? + if (!featureIsEnabled(FEATURE_MOTOR_STOP) || airmodeIsEnabled() || (throttleStatus != THROTTLE_LOW)) { // are motors running? const uint8_t lowThrottleLimit = pidConfig()->runaway_takeoff_deactivate_throttle; const uint8_t midThrottleLimit = constrain(lowThrottleLimit * 2, lowThrottleLimit * 2, RUNAWAY_TAKEOFF_HIGH_THROTTLE_PERCENT); if ((((throttlePercent >= lowThrottleLimit) && areSticksActive(RUNAWAY_TAKEOFF_DEACTIVATE_STICK_PERCENT)) || (throttlePercent >= midThrottleLimit)) @@ -675,17 +669,17 @@ bool processRx(timeUs_t currentTimeUs) // When armed and motors aren't spinning, do beeps and then disarm // board after delay so users without buzzer won't lose fingers. // mixTable constrains motor commands, so checking throttleStatus is enough + const timeUs_t autoDisarmDelayUs = armingConfig()->auto_disarm_delay * 1e6; if (ARMING_FLAG(ARMED) - && feature(FEATURE_MOTOR_STOP) + && featureIsEnabled(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING) - && !feature(FEATURE_3D) - && !isAirmodeActive() + && !featureIsEnabled(FEATURE_3D) + && !airmodeIsEnabled() + && !FLIGHT_MODE(GPS_RESCUE_MODE) // disable auto-disarm when GPS Rescue is active ) { if (isUsingSticksForArming()) { if (throttleStatus == THROTTLE_LOW) { - if (armingConfig()->auto_disarm_delay != 0 - && (int32_t)(disarmAt - millis()) < 0 - ) { + if ((autoDisarmDelayUs > 0) && (currentTimeUs > disarmAt)) { // auto-disarm configured and delay is over disarm(); armedBeeperOn = false; @@ -695,11 +689,8 @@ bool processRx(timeUs_t currentTimeUs) armedBeeperOn = true; } } else { - // throttle is not low - if (armingConfig()->auto_disarm_delay != 0) { - // extend disarm time - disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000; - } + // throttle is not low - extend disarm time + disarmAt = currentTimeUs + autoDisarmDelayUs; if (armedBeeperOn) { beeperSilence(); @@ -716,11 +707,13 @@ bool processRx(timeUs_t currentTimeUs) armedBeeperOn = false; } } + } else { + disarmAt = currentTimeUs + autoDisarmDelayUs; // extend auto-disarm timer } processRcStickPositions(); - if (feature(FEATURE_INFLIGHT_ACC_CAL)) { + if (featureIsEnabled(FEATURE_INFLIGHT_ACC_CAL)) { updateInflightCalibrationState(); } @@ -734,7 +727,6 @@ bool processRx(timeUs_t currentTimeUs) #endif if (!cliMode) { - updateAdjustmentStates(); processRcAdjustments(currentControlRateProfile); } @@ -762,6 +754,7 @@ bool processRx(timeUs_t currentTimeUs) DISABLE_FLIGHT_MODE(HORIZON_MODE); } +#ifdef USE_GPS_RESCUE if (IS_RC_MODE_ACTIVE(BOXGPSRESCUE) || (failsafeIsActive() && failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_GPS_RESCUE)) { if (!FLIGHT_MODE(GPS_RESCUE_MODE)) { ENABLE_FLIGHT_MODE(GPS_RESCUE_MODE); @@ -769,6 +762,7 @@ bool processRx(timeUs_t currentTimeUs) } else { DISABLE_FLIGHT_MODE(GPS_RESCUE_MODE); } +#endif if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { LED1_ON; @@ -809,7 +803,7 @@ bool processRx(timeUs_t currentTimeUs) } } #endif - + if (IS_RC_MODE_ACTIVE(BOXPASSTHRU)) { ENABLE_FLIGHT_MODE(PASSTHRU_MODE); } else { @@ -821,7 +815,7 @@ bool processRx(timeUs_t currentTimeUs) } #ifdef USE_TELEMETRY - if (feature(FEATURE_TELEMETRY)) { + if (featureIsEnabled(FEATURE_TELEMETRY)) { bool enableSharedPortTelemetry = (!isModeActivationConditionPresent(BOXTELEMETRY) && ARMING_FLAG(ARMED)) || (isModeActivationConditionPresent(BOXTELEMETRY) && IS_RC_MODE_ACTIVE(BOXTELEMETRY)); if (enableSharedPortTelemetry && !sharedPortTelemetryEnabled) { mspSerialReleaseSharedTelemetryPorts(); @@ -850,6 +844,14 @@ bool processRx(timeUs_t currentTimeUs) pidSetAcroTrainerState(IS_RC_MODE_ACTIVE(BOXACROTRAINER) && sensors(SENSOR_ACC)); #endif // USE_ACRO_TRAINER +#ifdef USE_RC_SMOOTHING_FILTER + if (ARMING_FLAG(ARMED) && !rcSmoothingInitializationComplete()) { + beeper(BEEPER_RC_SMOOTHING_INIT_FAIL); + } +#endif + + pidSetAntiGravityState(IS_RC_MODE_ACTIVE(BOXANTIGRAVITY) || featureIsEnabled(FEATURE_ANTI_GRAVITY)); + return true; } @@ -871,7 +873,7 @@ static FAST_CODE void subTaskPidController(timeUs_t currentTimeUs) && !runawayTakeoffCheckDisabled && !flipOverAfterCrashMode && !runawayTakeoffTemporarilyDisabled - && (!feature(FEATURE_MOTOR_STOP) || isAirmodeActive() || (calculateThrottleStatus() != THROTTLE_LOW))) { + && (!featureIsEnabled(FEATURE_MOTOR_STOP) || airmodeIsEnabled() || (calculateThrottleStatus() != THROTTLE_LOW))) { if (((fabsf(pidData[FD_PITCH].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD) || (fabsf(pidData[FD_ROLL].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD) @@ -906,37 +908,19 @@ static FAST_CODE void subTaskPidController(timeUs_t currentTimeUs) #endif } -static FAST_CODE_NOINLINE void subTaskMainSubprocesses(timeUs_t currentTimeUs) +static FAST_CODE_NOINLINE void subTaskPidSubprocesses(timeUs_t currentTimeUs) { uint32_t startTime = 0; if (debugMode == DEBUG_PIDLOOP) { startTime = micros(); } - // Read out gyro temperature if used for telemmetry - if (feature(FEATURE_TELEMETRY)) { - gyroReadTemperature(); - } - #ifdef USE_MAG if (sensors(SENSOR_MAG)) { updateMagHold(); } #endif -#ifdef USE_GPS_RESCUE - updateGPSRescueState(); -#endif - -#ifdef USE_SDCARD - afatfs_poll(); -#endif - -#if defined(USE_VCP) - DEBUG_SET(DEBUG_USB, 0, usbCableIsInserted()); - DEBUG_SET(DEBUG_USB, 1, usbVcpIsConnected()); -#endif - #ifdef USE_BLACKBOX if (!cliMode && blackboxConfig()->device) { blackboxUpdate(currentTimeUs); @@ -945,12 +929,19 @@ static FAST_CODE_NOINLINE void subTaskMainSubprocesses(timeUs_t currentTimeUs) UNUSED(currentTimeUs); #endif -#ifdef USE_TRANSPONDER - transponderUpdate(currentTimeUs); -#endif DEBUG_SET(DEBUG_PIDLOOP, 3, micros() - startTime); } +#ifdef USE_TELEMETRY +void subTaskTelemetryPollSensors(timeUs_t currentTimeUs) +{ + UNUSED(currentTimeUs); + + // Read out gyro temperature if used for telemmetry + gyroReadTemperature(); +} +#endif + static FAST_CODE void subTaskMotorUpdate(timeUs_t currentTimeUs) { uint32_t startTime = 0; @@ -981,6 +972,7 @@ static FAST_CODE void subTaskMotorUpdate(timeUs_t currentTimeUs) static FAST_CODE_NOINLINE void subTaskRcCommand(timeUs_t currentTimeUs) { + UNUSED(currentTimeUs); // If we're armed, at minimum throttle, and we do arming via the // sticks, do not process yaw input from the rx. We do this so the @@ -998,12 +990,8 @@ static FAST_CODE_NOINLINE void subTaskRcCommand(timeUs_t currentTimeUs) resetYawAxis(); } - if (throttleCorrectionConfig()->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { - rcCommand[THROTTLE] += calculateThrottleAngleCorrection(throttleCorrectionConfig()->throttle_correction_value); - } - processRcCommand(); - UNUSED(currentTimeUs); + } // Function for loop trigger @@ -1019,7 +1007,7 @@ FAST_CODE void taskMainPidLoop(timeUs_t currentTimeUs) // 0 - gyroUpdate() // 1 - subTaskPidController() // 2 - subTaskMotorUpdate() - // 3 - subTaskMainSubprocesses() + // 3 - subTaskPidSubprocesses() gyroUpdate(currentTimeUs); DEBUG_SET(DEBUG_PIDLOOP, 0, micros() - currentTimeUs); @@ -1027,7 +1015,7 @@ FAST_CODE void taskMainPidLoop(timeUs_t currentTimeUs) subTaskRcCommand(currentTimeUs); subTaskPidController(currentTimeUs); subTaskMotorUpdate(currentTimeUs); - subTaskMainSubprocesses(currentTimeUs); + subTaskPidSubprocesses(currentTimeUs); } if (debugMode == DEBUG_CYCLETIME) { @@ -1049,11 +1037,10 @@ timeUs_t getLastDisarmTimeUs(void) bool isTryingToArm() { - return tryingToArm; + return (tryingToArm != ARMING_DELAYED_DISARMED); } void resetTryingToArm() { - tryingToArm = false; + tryingToArm = ARMING_DELAYED_DISARMED; } - diff --git a/src/main/fc/fc_core.h b/src/main/fc/core.h similarity index 96% rename from src/main/fc/fc_core.h rename to src/main/fc/core.h index 2da50dc574..f67319445e 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/core.h @@ -56,3 +56,5 @@ bool isAirmodeActivated(); timeUs_t getLastDisarmTimeUs(void); bool isTryingToArm(); void resetTryingToArm(); + +void subTaskTelemetryPollSensors(timeUs_t currentTimeUs); diff --git a/src/main/fc/fc_dispatch.c b/src/main/fc/dispatch.c similarity index 91% rename from src/main/fc/fc_dispatch.c rename to src/main/fc/dispatch.c index b6fdfeef93..1e105fb797 100644 --- a/src/main/fc/fc_dispatch.c +++ b/src/main/fc/dispatch.c @@ -28,7 +28,7 @@ #include "drivers/time.h" -#include "fc/fc_dispatch.h" +#include "fc/dispatch.h" static dispatchEntry_t *head = NULL; static bool dispatchEnabled = false; @@ -51,6 +51,7 @@ void dispatchProcess(uint32_t currentTime) // unlink entry first, so handler can replan self dispatchEntry_t *current = *p; *p = (*p)->next; + current->inQue = false; (*current->dispatch)(current); } } @@ -58,10 +59,17 @@ void dispatchProcess(uint32_t currentTime) void dispatchAdd(dispatchEntry_t *entry, int delayUs) { uint32_t delayedUntil = micros() + delayUs; - entry->delayedUntil = delayedUntil; dispatchEntry_t **p = &head; + + if (entry->inQue) { + return; // Allready in Queue, abort + } + while (*p && cmp32((*p)->delayedUntil, delayedUntil) < 0) p = &(*p)->next; + entry->next = *p; + entry->delayedUntil = delayedUntil; + entry->inQue = true; *p = entry; } diff --git a/src/main/fc/fc_dispatch.h b/src/main/fc/dispatch.h similarity index 98% rename from src/main/fc/fc_dispatch.h rename to src/main/fc/dispatch.h index ce40918a86..18b2d49dca 100644 --- a/src/main/fc/fc_dispatch.h +++ b/src/main/fc/dispatch.h @@ -27,6 +27,7 @@ typedef struct dispatchEntry_s { dispatchFunc *dispatch; uint32_t delayedUntil; struct dispatchEntry_s *next; + bool inQue; } dispatchEntry_t; bool dispatchIsEnabled(void); diff --git a/src/main/fc/fc_hardfaults.c b/src/main/fc/hardfaults.c similarity index 84% rename from src/main/fc/fc_hardfaults.c rename to src/main/fc/hardfaults.c index 97a47effef..9bd4932a60 100644 --- a/src/main/fc/fc_hardfaults.c +++ b/src/main/fc/hardfaults.c @@ -27,10 +27,41 @@ #include "drivers/time.h" #include "drivers/transponder_ir.h" -#include "fc/fc_init.h" +#include "fc/init.h" #include "flight/mixer.h" +#ifdef STM32F7 +void MemManage_Handler(void) +{ + LED2_ON; + + // fall out of the sky + uint8_t requiredStateForMotors = SYSTEM_STATE_CONFIG_LOADED | SYSTEM_STATE_MOTORS_READY; + if ((systemState & requiredStateForMotors) == requiredStateForMotors) { + stopMotors(); + } + +#ifdef USE_TRANSPONDER + // prevent IR LEDs from burning out. + uint8_t requiredStateForTransponder = SYSTEM_STATE_CONFIG_LOADED | SYSTEM_STATE_TRANSPONDER_ENABLED; + if ((systemState & requiredStateForTransponder) == requiredStateForTransponder) { + transponderIrDisable(); + } +#endif + + LED1_OFF; + LED0_OFF; + + while (1) { + delay(500); + LED2_TOGGLE; + delay(50); + LED2_TOGGLE; + } +} +#endif + #ifdef DEBUG_HARDFAULTS //from: https://mcuoneclipse.com/2012/11/24/debugging-hard-faults-on-arm-cortex-m/ /** @@ -96,13 +127,11 @@ void HardFault_Handler(void) { LED2_ON; -#ifndef USE_OSD_SLAVE // fall out of the sky uint8_t requiredStateForMotors = SYSTEM_STATE_CONFIG_LOADED | SYSTEM_STATE_MOTORS_READY; if ((systemState & requiredStateForMotors) == requiredStateForMotors) { stopMotors(); } -#endif #ifdef USE_TRANSPONDER // prevent IR LEDs from burning out. @@ -116,10 +145,8 @@ void HardFault_Handler(void) LED0_OFF; while (1) { -#ifdef LED2 delay(50); LED2_TOGGLE; -#endif } } #endif diff --git a/src/main/fc/fc_init.c b/src/main/fc/init.c similarity index 87% rename from src/main/fc/fc_init.c rename to src/main/fc/init.c index c4c9e38d48..7bc5ab5f53 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/init.c @@ -80,8 +80,8 @@ #include "fc/board_info.h" #include "fc/config.h" -#include "fc/fc_init.h" -#include "fc/fc_tasks.h" +#include "fc/init.h" +#include "fc/tasks.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" @@ -111,9 +111,7 @@ #include "io/beeper.h" #include "io/displayport_max7456.h" -#include "io/displayport_rcdevice.h" #include "io/displayport_srxl.h" -#include "io/displayport_crsf.h" #include "io/serial.h" #include "io/flashfs.h" #include "io/gps.h" @@ -125,7 +123,6 @@ #include "io/asyncfatfs/asyncfatfs.h" #include "io/transponder_ir.h" #include "io/osd.h" -#include "io/osd_slave.h" #include "io/pidaudio.h" #include "io/piniobox.h" #include "io/displayport_msp.h" @@ -258,9 +255,6 @@ void init(void) debugMode = systemConfig()->debug_mode; - // Latch active features to be used for feature() in the remainder of init(). - latchActiveFeatures(); - #ifdef TARGET_PREINIT targetPreInit(); #endif @@ -302,7 +296,7 @@ void init(void) #endif #if defined(USE_SPEKTRUM_BIND) - if (feature(FEATURE_RX_SERIAL)) { + if (featureIsEnabled(FEATURE_RX_SERIAL)) { switch (rxConfig()->serialrx_provider) { case SERIALRX_SPEKTRUM1024: case SERIALRX_SPEKTRUM2048: @@ -333,27 +327,26 @@ void init(void) #endif #if defined(AVOID_UART1_FOR_PWM_PPM) - serialInit(feature(FEATURE_SOFTSERIAL), - feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART1 : SERIAL_PORT_NONE); + serialInit(featureIsEnabled(FEATURE_SOFTSERIAL), + featureIsEnabled(FEATURE_RX_PPM) || featureIsEnabled(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART1 : SERIAL_PORT_NONE); #elif defined(AVOID_UART2_FOR_PWM_PPM) - serialInit(feature(FEATURE_SOFTSERIAL), - feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART2 : SERIAL_PORT_NONE); + serialInit(featureIsEnabled(FEATURE_SOFTSERIAL), + featureIsEnabled(FEATURE_RX_PPM) || featureIsEnabled(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART2 : SERIAL_PORT_NONE); #elif defined(AVOID_UART3_FOR_PWM_PPM) - serialInit(feature(FEATURE_SOFTSERIAL), - feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART3 : SERIAL_PORT_NONE); + serialInit(featureIsEnabled(FEATURE_SOFTSERIAL), + featureIsEnabled(FEATURE_RX_PPM) || featureIsEnabled(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART3 : SERIAL_PORT_NONE); #else - serialInit(feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE); + serialInit(featureIsEnabled(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE); #endif mixerInit(mixerConfig()->mixerMode); mixerConfigureOutput(); uint16_t idlePulse = motorConfig()->mincommand; - if (feature(FEATURE_3D)) { + if (featureIsEnabled(FEATURE_3D)) { idlePulse = flight3DConfig()->neutral3d; } if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) { - featureClear(FEATURE_3D); idlePulse = 0; // brushed motors } /* Motors needs to be initialized soon as posible because hardware initialization @@ -364,12 +357,12 @@ void init(void) if (0) {} #if defined(USE_PPM) - else if (feature(FEATURE_RX_PPM)) { + else if (featureIsEnabled(FEATURE_RX_PPM)) { ppmRxInit(ppmConfig()); } #endif #if defined(USE_PWM) - else if (feature(FEATURE_RX_PARALLEL_PWM)) { + else if (featureIsEnabled(FEATURE_RX_PARALLEL_PWM)) { pwmRxInit(pwmConfig()); } #endif @@ -456,13 +449,13 @@ void init(void) // XXX These kind of code should goto target/config.c? // XXX And these no longer work properly as FEATURE_RANGEFINDER does control HCSR04 runtime configuration. #if defined(RANGEFINDER_HCSR04_SOFTSERIAL2_EXCLUSIVE) && defined(USE_RANGEFINDER_HCSR04) && defined(USE_SOFTSERIAL2) - if (feature(FEATURE_RANGEFINDER) && feature(FEATURE_SOFTSERIAL)) { + if (featureIsEnabled(FEATURE_RANGEFINDER) && featureIsEnabled(FEATURE_SOFTSERIAL)) { serialRemovePort(SERIAL_PORT_SOFTSERIAL2); } #endif #if defined(RANGEFINDER_HCSR04_SOFTSERIAL1_EXCLUSIVE) && defined(USE_RANGEFINDER_HCSR04) && defined(USE_SOFTSERIAL1) - if (feature(FEATURE_RANGEFINDER) && feature(FEATURE_SOFTSERIAL)) { + if (featureIsEnabled(FEATURE_RANGEFINDER) && featureIsEnabled(FEATURE_SOFTSERIAL)) { serialRemovePort(SERIAL_PORT_SOFTSERIAL1); } #endif @@ -472,9 +465,9 @@ void init(void) adcConfigMutable()->current.enabled = (batteryConfig()->currentMeterSource == CURRENT_METER_ADC); // The FrSky D SPI RX sends RSSI_ADC_PIN (if configured) as A2 - adcConfigMutable()->rssi.enabled = feature(FEATURE_RSSI_ADC); + adcConfigMutable()->rssi.enabled = featureIsEnabled(FEATURE_RSSI_ADC); #ifdef USE_RX_SPI - adcConfigMutable()->rssi.enabled |= (feature(FEATURE_RX_SPI) && rxSpiConfig()->rx_spi_protocol == RX_SPI_FRSKY_D); + adcConfigMutable()->rssi.enabled |= (featureIsEnabled(FEATURE_RX_SPI) && rxSpiConfig()->rx_spi_protocol == RX_SPI_FRSKY_D); #endif adcInit(adcConfig()); #endif @@ -503,7 +496,7 @@ void init(void) servosInit(); servoConfigureOutput(); if (isMixerUsingServos()) { - //pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING); + //pwm_params.useChannelForwarding = featureIsEnabled(FEATURE_CHANNEL_FORWARDING); servoDevInit(&servoConfig()->dev); } servosFilterInit(); @@ -558,18 +551,18 @@ void init(void) cmsInit(); #endif -#if (defined(USE_OSD) || (defined(USE_MSP_DISPLAYPORT) && defined(USE_CMS)) || defined(USE_OSD_SLAVE)) +#if (defined(USE_OSD) || (defined(USE_MSP_DISPLAYPORT) && defined(USE_CMS))) displayPort_t *osdDisplayPort = NULL; #endif -#if defined(USE_OSD) && !defined(USE_OSD_SLAVE) +#if defined(USE_OSD) //The OSD need to be initialised after GYRO to avoid GYRO initialisation failure on some targets - if (feature(FEATURE_OSD)) { + if (featureIsEnabled(FEATURE_OSD)) { #if defined(USE_MAX7456) // If there is a max7456 chip for the OSD then use it osdDisplayPort = max7456DisplayPortInit(vcdProfile()); -#elif defined(USE_OSD_OVER_MSP_DISPLAYPORT) // OSD over MSP; not supported (yet) +#elif defined(USE_CMS) && defined(USE_MSP_DISPLAYPORT) && defined(USE_OSD_OVER_MSP_DISPLAYPORT) // OSD over MSP; not supported (yet) osdDisplayPort = displayPortMspInit(); #endif // osdInit will register with CMS by itself. @@ -577,15 +570,6 @@ void init(void) } #endif -#if defined(USE_OSD_SLAVE) && !defined(USE_OSD) -#if defined(USE_MAX7456) - // If there is a max7456 chip for the OSD then use it - osdDisplayPort = max7456DisplayPortInit(vcdProfile()); - // osdInit will register with CMS by itself. - osdSlaveInit(osdDisplayPort); -#endif -#endif - #if defined(USE_CMS) && defined(USE_MSP_DISPLAYPORT) // If BFOSD is not active, then register MSP_DISPLAYPORT as a CMS device. if (!osdDisplayPort) @@ -594,7 +578,7 @@ void init(void) #ifdef USE_DASHBOARD // Dashbord will register with CMS by itself. - if (feature(FEATURE_DASHBOARD)) { + if (featureIsEnabled(FEATURE_DASHBOARD)) { dashboardInit(); } #endif @@ -604,12 +588,8 @@ void init(void) cmsDisplayPortRegister(displayPortSrxlInit()); #endif -#if defined(USE_CMS) && defined(USE_CRSF_CMS_TELEMETRY) && defined(USE_TELEMETRY) - cmsDisplayPortRegister(displayPortCrsfInit()); -#endif - #ifdef USE_GPS - if (feature(FEATURE_GPS)) { + if (featureIsEnabled(FEATURE_GPS)) { gpsInit(); } #endif @@ -617,19 +597,19 @@ void init(void) #ifdef USE_LED_STRIP ledStripInit(); - if (feature(FEATURE_LED_STRIP)) { + if (featureIsEnabled(FEATURE_LED_STRIP)) { ledStripEnable(); } #endif #ifdef USE_TELEMETRY - if (feature(FEATURE_TELEMETRY)) { + if (featureIsEnabled(FEATURE_TELEMETRY)) { telemetryInit(); } #endif #ifdef USE_ESC_SENSOR - if (feature(FEATURE_ESC_SENSOR)) { + if (featureIsEnabled(FEATURE_ESC_SENSOR)) { escSensorInit(); } #endif @@ -639,7 +619,7 @@ void init(void) #endif #ifdef USE_TRANSPONDER - if (feature(FEATURE_TRANSPONDER)) { + if (featureIsEnabled(FEATURE_TRANSPONDER)) { transponderInit(); transponderStartRepeating(); systemState |= SYSTEM_STATE_TRANSPONDER_ENABLED; @@ -655,7 +635,7 @@ void init(void) #ifdef USE_SDCARD if (blackboxConfig()->device == BLACKBOX_DEVICE_SDCARD) { - if (sdcardConfig()->enabled) { + if (sdcardConfig()->mode) { sdcardInsertionDetectInit(); sdcard_init(sdcardConfig()); afatfs_init(); @@ -722,7 +702,7 @@ void init(void) batteryInit(); // always needs doing, regardless of features. #ifdef USE_DASHBOARD - if (feature(FEATURE_DASHBOARD)) { + if (featureIsEnabled(FEATURE_DASHBOARD)) { #ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY dashboardShowFixedPage(PAGE_GPS); #else @@ -736,8 +716,6 @@ void init(void) rcdeviceInit(); #endif // USE_RCDEVICE - // Latch active features AGAIN since some may be modified by init(). - latchActiveFeatures(); pwmEnableMotors(); setArmingDisabled(ARMING_DISABLED_BOOT_GRACE_TIME); diff --git a/src/main/fc/fc_init.h b/src/main/fc/init.h similarity index 100% rename from src/main/fc/fc_init.h rename to src/main/fc/init.h diff --git a/src/main/fc/fc_rc.c b/src/main/fc/rc.c similarity index 54% rename from src/main/fc/fc_rc.c rename to src/main/fc/rc.c index 57aec5a681..e76d0834d5 100644 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/rc.c @@ -35,8 +35,8 @@ #include "fc/config.h" #include "fc/controlrate_profile.h" #include "drivers/time.h" -#include "fc/fc_core.h" -#include "fc/fc_rc.h" +#include "fc/core.h" +#include "fc/rc.h" #include "fc/rc_controls.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" @@ -69,16 +69,16 @@ enum { }; #ifdef USE_RC_SMOOTHING_FILTER -#define RC_SMOOTHING_IDENTITY_FREQUENCY 80 // Used in the formula to convert a BIQUAD cutoff frequency to PT1 -#define RC_SMOOTHING_FILTER_STARTUP_DELAY_MS 5000 // Time to wait after power to let the PID loop stabilize before starting average frame rate calculation -#define RC_SMOOTHING_FILTER_TRAINING_DELAY_MS 1000 // Additional time to wait after receiving first valid rx frame before training starts -#define RC_SMOOTHING_FILTER_TRAINING_SAMPLES 50 +#define RC_SMOOTHING_IDENTITY_FREQUENCY 80 // Used in the formula to convert a BIQUAD cutoff frequency to PT1 +#define RC_SMOOTHING_FILTER_STARTUP_DELAY_MS 5000 // Time to wait after power to let the PID loop stabilize before starting average frame rate calculation +#define RC_SMOOTHING_FILTER_TRAINING_SAMPLES 50 // Number of rx frame rate samples to average +#define RC_SMOOTHING_FILTER_TRAINING_DELAY_MS 1000 // Additional time to wait after receiving first valid rx frame before initial training starts +#define RC_SMOOTHING_FILTER_RETRAINING_DELAY_MS 2000 // Guard time to wait after retraining to prevent retraining again too quickly +#define RC_SMOOTHING_RX_RATE_CHANGE_PERCENT 20 // Look for samples varying this much from the current detected frame rate to initiate retraining +#define RC_SMOOTHING_RX_RATE_MIN_US 1000 // 1ms +#define RC_SMOOTHING_RX_RATE_MAX_US 50000 // 50ms or 20hz -static FAST_RAM_ZERO_INIT uint16_t defaultInputCutoffFrequency; -static FAST_RAM_ZERO_INIT uint16_t defaultDerivativeCutoffFrequency; -static FAST_RAM_ZERO_INIT uint16_t filterCutoffFrequency; -static FAST_RAM_ZERO_INIT uint16_t derivativeCutoffFrequency; -static FAST_RAM_ZERO_INIT uint16_t calculatedFrameTimeAverageUs; +static FAST_RAM_ZERO_INIT rcSmoothingFilter_t rcSmoothingData; #endif // USE_RC_SMOOTHING_FILTER float getSetpointRate(int axis) @@ -111,7 +111,9 @@ static int16_t rcLookupThrottle(int32_t tmp) return lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; } -#define SETPOINT_RATE_LIMIT 1998.0f +#define SETPOINT_RATE_LIMIT 1998 +STATIC_ASSERT(CONTROL_RATE_CONFIG_RATE_LIMIT_MAX <= SETPOINT_RATE_LIMIT, CONTROL_RATE_CONFIG_RATE_LIMIT_MAX_too_large); + #define RC_RATE_INCREMENTAL 14.54f float applyBetaflightRates(const int axis, float rcCommandf, const float rcCommandfAbs) @@ -147,17 +149,32 @@ float applyRaceFlightRates(const int axis, float rcCommandf, const float rcComma static void calculateSetpointRate(int axis) { - // scale rcCommandf to range [-1.0, 1.0] - float rcCommandf = rcCommand[axis] / 500.0f; - rcDeflection[axis] = rcCommandf; - const float rcCommandfAbs = ABS(rcCommandf); - rcDeflectionAbs[axis] = rcCommandfAbs; + float angleRate; + +#ifdef USE_GPS_RESCUE + if ((axis == FD_YAW) && FLIGHT_MODE(GPS_RESCUE_MODE)) { + // If GPS Rescue is active then override the setpointRate used in the + // pid controller with the value calculated from the desired heading logic. + angleRate = gpsRescueGetYawRate(); - float angleRate = applyRates(axis, rcCommandf, rcCommandfAbs); + // Treat the stick input as centered to avoid any stick deflection base modifications (like acceleration limit) + rcDeflection[axis] = 0; + rcDeflectionAbs[axis] = 0; + } else +#endif + { + // scale rcCommandf to range [-1.0, 1.0] + float rcCommandf = rcCommand[axis] / 500.0f; + rcDeflection[axis] = rcCommandf; + const float rcCommandfAbs = ABS(rcCommandf); + rcDeflectionAbs[axis] = rcCommandfAbs; + + angleRate = applyRates(axis, rcCommandf, rcCommandfAbs); + } + // Rate limit from profile (deg/sec) + setpointRate[axis] = constrainf(angleRate, -1.0f * currentControlRateProfile->rate_limit[axis], 1.0f * currentControlRateProfile->rate_limit[axis]); DEBUG_SET(DEBUG_ANGLERATE, axis, angleRate); - - setpointRate[axis] = constrainf(angleRate, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); // Rate limit protection (deg/sec) } static void scaleRcCommandToFpvCamAngle(void) @@ -175,8 +192,8 @@ static void scaleRcCommandToFpvCamAngle(void) float roll = setpointRate[ROLL]; float yaw = setpointRate[YAW]; - setpointRate[ROLL] = constrainf(roll * cosFactor - yaw * sinFactor, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); - setpointRate[YAW] = constrainf(yaw * cosFactor + roll * sinFactor, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); + setpointRate[ROLL] = constrainf(roll * cosFactor - yaw * sinFactor, -SETPOINT_RATE_LIMIT * 1.0f, SETPOINT_RATE_LIMIT * 1.0f); + setpointRate[YAW] = constrainf(yaw * cosFactor + roll * sinFactor, -SETPOINT_RATE_LIMIT * 1.0f, SETPOINT_RATE_LIMIT * 1.0f); } #define THROTTLE_BUFFER_MAX 20 @@ -189,7 +206,7 @@ static void checkForThrottleErrorResetState(uint16_t rxRefreshRate) const int rxRefreshRateMs = rxRefreshRate / 1000; const int indexMax = constrain(THROTTLE_DELTA_MS / rxRefreshRateMs, 1, THROTTLE_BUFFER_MAX); - const int16_t throttleVelocityThreshold = (feature(FEATURE_3D)) ? currentPidProfile->itermThrottleThreshold / 2 : currentPidProfile->itermThrottleThreshold; + const int16_t throttleVelocityThreshold = (featureIsEnabled(FEATURE_3D)) ? currentPidProfile->itermThrottleThreshold / 2 : currentPidProfile->itermThrottleThreshold; rcCommandThrottlePrevious[index++] = rcCommand[THROTTLE]; if (index >= indexMax) { @@ -198,10 +215,12 @@ static void checkForThrottleErrorResetState(uint16_t rxRefreshRate) const int16_t rcCommandSpeed = rcCommand[THROTTLE] - rcCommandThrottlePrevious[index]; - if (ABS(rcCommandSpeed) > throttleVelocityThreshold) { - pidSetItermAccelerator(CONVERT_PARAMETER_TO_FLOAT(currentPidProfile->itermAcceleratorGain)); - } else { - pidSetItermAccelerator(1.0f); + if (currentPidProfile->antiGravityMode == ANTI_GRAVITY_STEP) { + if (ABS(rcCommandSpeed) > throttleVelocityThreshold) { + pidSetItermAccelerator(CONVERT_PARAMETER_TO_FLOAT(currentPidProfile->itermAcceleratorGain)); + } else { + pidSetItermAccelerator(1.0f); + } } } @@ -264,11 +283,14 @@ FAST_CODE uint8_t processRcInterpolation(void) } #ifdef USE_RC_SMOOTHING_FILTER -int calcRcSmoothingCutoff(float avgRxFrameTime, bool pt1) +// Determine a cutoff frequency based on filter type and the calculated +// average rx frame time +FAST_CODE_NOINLINE int calcRcSmoothingCutoff(int avgRxFrameTimeUs, bool pt1) { - if (avgRxFrameTime > 0) { - float cutoff = (1 / avgRxFrameTime) / 2; // calculate the nyquist frequency + if (avgRxFrameTimeUs > 0) { + float cutoff = (1 / (avgRxFrameTimeUs * 1e-6f)) / 2; // calculate the nyquist frequency cutoff = cutoff * 0.90f; // Use 90% of the calculated nyquist frequency + if (pt1) { cutoff = sq(cutoff) / RC_SMOOTHING_IDENTITY_FREQUENCY; // convert to a cutoff for pt1 that has similar characteristics } @@ -278,107 +300,230 @@ int calcRcSmoothingCutoff(float avgRxFrameTime, bool pt1) } } +// Preforms a reasonableness check on the rx frame time to avoid bad data +// skewing the average. +FAST_CODE bool rcSmoothingRxRateValid(int currentRxRefreshRate) +{ + return (currentRxRefreshRate >= RC_SMOOTHING_RX_RATE_MIN_US && currentRxRefreshRate <= RC_SMOOTHING_RX_RATE_MAX_US); +} + +// Initialize or update the filters base on either the manually selected cutoff, or +// the auto-calculated cutoff frequency based on detected rx frame rate. +FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothingData) +{ + const float dT = targetPidLooptime * 1e-6f; + uint16_t oldCutoff = smoothingData->inputCutoffFrequency; + + if (rxConfig()->rc_smoothing_input_cutoff == 0) { + smoothingData->inputCutoffFrequency = calcRcSmoothingCutoff(smoothingData->averageFrameTimeUs, (rxConfig()->rc_smoothing_input_type == RC_SMOOTHING_INPUT_PT1)); + } + + // initialize or update the input filter + if ((smoothingData->inputCutoffFrequency != oldCutoff) || !smoothingData->filterInitialized) { + for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) { + if ((1 << i) & interpolationChannels) { // only update channels specified by rc_interp_ch + switch (rxConfig()->rc_smoothing_input_type) { + + case RC_SMOOTHING_INPUT_PT1: + if (!smoothingData->filterInitialized) { + pt1FilterInit((pt1Filter_t*) &smoothingData->filter[i], pt1FilterGain(smoothingData->inputCutoffFrequency, dT)); + } else { + pt1FilterUpdateCutoff((pt1Filter_t*) &smoothingData->filter[i], pt1FilterGain(smoothingData->inputCutoffFrequency, dT)); + } + break; + + case RC_SMOOTHING_INPUT_BIQUAD: + default: + if (!smoothingData->filterInitialized) { + biquadFilterInitLPF((biquadFilter_t*) &smoothingData->filter[i], smoothingData->inputCutoffFrequency, targetPidLooptime); + } else { + biquadFilterUpdateLPF((biquadFilter_t*) &smoothingData->filter[i], smoothingData->inputCutoffFrequency, targetPidLooptime); + } + break; + } + } + } + } + + // update or initialize the derivative filter + oldCutoff = smoothingData->derivativeCutoffFrequency; + if ((rxConfig()->rc_smoothing_derivative_cutoff == 0) && (rxConfig()->rc_smoothing_derivative_type != RC_SMOOTHING_DERIVATIVE_OFF)) { + smoothingData->derivativeCutoffFrequency = calcRcSmoothingCutoff(smoothingData->averageFrameTimeUs, (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_PT1)); + } + + if (!smoothingData->filterInitialized) { + pidInitSetpointDerivativeLpf(smoothingData->derivativeCutoffFrequency, rxConfig()->rc_smoothing_debug_axis, rxConfig()->rc_smoothing_derivative_type); + } else if (smoothingData->derivativeCutoffFrequency != oldCutoff) { + pidUpdateSetpointDerivativeLpf(smoothingData->derivativeCutoffFrequency); + } +} + +FAST_CODE_NOINLINE void rcSmoothingResetAccumulation(rcSmoothingFilter_t *smoothingData) +{ + smoothingData->training.sum = 0; + smoothingData->training.count = 0; + smoothingData->training.min = UINT16_MAX; + smoothingData->training.max = 0; +} + +// Accumulate the rx frame time samples. Once we've collected enough samples calculate the +// average and return true. +FAST_CODE bool rcSmoothingAccumulateSample(rcSmoothingFilter_t *smoothingData, int rxFrameTimeUs) +{ + smoothingData->training.sum += rxFrameTimeUs; + smoothingData->training.count++; + smoothingData->training.max = MAX(smoothingData->training.max, rxFrameTimeUs); + smoothingData->training.min = MIN(smoothingData->training.min, rxFrameTimeUs); + + // if we've collected enough samples then calculate the average and reset the accumulation + if (smoothingData->training.count >= RC_SMOOTHING_FILTER_TRAINING_SAMPLES) { + smoothingData->training.sum = smoothingData->training.sum - smoothingData->training.min - smoothingData->training.max; // Throw out high and low samples + smoothingData->averageFrameTimeUs = lrintf(smoothingData->training.sum / (smoothingData->training.count - 2)); + rcSmoothingResetAccumulation(smoothingData); + return true; + } + return false; +} + +// Determine if we need to caclulate filter cutoffs. If not then we can avoid +// examining the rx frame times completely +FAST_CODE_NOINLINE bool rcSmoothingAutoCalculate(void) +{ + bool ret = false; + + // if the input cutoff is 0 (auto) then we need to calculate cutoffs + if (rxConfig()->rc_smoothing_input_cutoff == 0) { + ret = true; + } + + // if the derivative type isn't OFF and the cutoff is 0 then we need to calculate + if (rxConfig()->rc_smoothing_derivative_type != RC_SMOOTHING_DERIVATIVE_OFF) { + if (rxConfig()->rc_smoothing_derivative_cutoff == 0) { + ret = true; + } + } + return ret; +} + FAST_CODE uint8_t processRcSmoothingFilter(void) { uint8_t updatedChannel = 0; - static FAST_RAM_ZERO_INIT float lastRxData[4]; - static FAST_RAM_ZERO_INIT pt1Filter_t rcCommandFilterPt1[4]; - static FAST_RAM_ZERO_INIT biquadFilter_t rcCommandFilterBiquad[4]; static FAST_RAM_ZERO_INIT bool initialized; - static FAST_RAM_ZERO_INIT bool filterInitialized; - static FAST_RAM_ZERO_INIT float rxFrameTimeSum; - static FAST_RAM_ZERO_INIT int rxFrameCount; - static FAST_RAM uint16_t minRxFrameInterval = UINT16_MAX; - static FAST_RAM_ZERO_INIT uint16_t maxRxFrameInterval; static FAST_RAM_ZERO_INIT timeMs_t validRxFrameTimeMs; + static FAST_RAM_ZERO_INIT bool calculateCutoffs; + // first call initialization if (!initialized) { initialized = true; - filterCutoffFrequency = rxConfig()->rc_smoothing_input_cutoff; - derivativeCutoffFrequency = rxConfig()->rc_smoothing_derivative_cutoff; + rcSmoothingData.filterInitialized = false; + rcSmoothingData.averageFrameTimeUs = 0; + rcSmoothingResetAccumulation(&rcSmoothingData); + + rcSmoothingData.inputCutoffFrequency = rxConfig()->rc_smoothing_input_cutoff; + + if (rxConfig()->rc_smoothing_derivative_type != RC_SMOOTHING_DERIVATIVE_OFF) { + rcSmoothingData.derivativeCutoffFrequency = rxConfig()->rc_smoothing_derivative_cutoff; + } + + calculateCutoffs = rcSmoothingAutoCalculate(); + + // if we don't need to calculate cutoffs dynamically then the filters can be initialized now + if (!calculateCutoffs) { + rcSmoothingSetFilterCutoffs(&rcSmoothingData); + rcSmoothingData.filterInitialized = true; + } } if (isRXDataNew) { + + // store the new raw channel values for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) { if ((1 << i) & interpolationChannels) { lastRxData[i] = rcCommand[i]; } } - // If the filter cutoffs are set to auto and we have good rx data, then determine the average rx frame rate - // and use that to calculate the filter cutoff frequencies - if (!filterInitialized) { + + // for dynamically calculated filters we need to examine each rx frame interval + if (calculateCutoffs) { const timeMs_t currentTimeMs = millis(); - if (rxIsReceivingSignal() && (targetPidLooptime > 0) && (currentTimeMs > RC_SMOOTHING_FILTER_STARTUP_DELAY_MS)) { - if (validRxFrameTimeMs == 0) { - validRxFrameTimeMs = currentTimeMs; - } else if ((currentTimeMs - validRxFrameTimeMs) > RC_SMOOTHING_FILTER_TRAINING_DELAY_MS) { - rxFrameTimeSum += currentRxRefreshRate; - rxFrameCount++; - maxRxFrameInterval = MAX(maxRxFrameInterval, currentRxRefreshRate); - minRxFrameInterval = MIN(minRxFrameInterval, currentRxRefreshRate); - DEBUG_SET(DEBUG_RC_SMOOTHING, 0, rxFrameCount); // log the step count during training - DEBUG_SET(DEBUG_RC_SMOOTHING, 3, currentRxRefreshRate); // log each frame interval during training - if (rxFrameCount >= RC_SMOOTHING_FILTER_TRAINING_SAMPLES) { - rxFrameTimeSum = rxFrameTimeSum - minRxFrameInterval - maxRxFrameInterval; // Throw out high and low samples - calculatedFrameTimeAverageUs = lrintf(rxFrameTimeSum / (rxFrameCount - 2)); - const float avgRxFrameTime = (rxFrameTimeSum / (rxFrameCount - 2)) * 1e-6f; + int sampleState = 0; - defaultInputCutoffFrequency = calcRcSmoothingCutoff(avgRxFrameTime, (rxConfig()->rc_smoothing_input_type == RC_SMOOTHING_INPUT_PT1)); - filterCutoffFrequency = (filterCutoffFrequency == 0) ? defaultInputCutoffFrequency : filterCutoffFrequency; + // If the filter cutoffs are set to auto and we have good rx data, then determine the average rx frame rate + // and use that to calculate the filter cutoff frequencies + if ((currentTimeMs > RC_SMOOTHING_FILTER_STARTUP_DELAY_MS) && (targetPidLooptime > 0)) { // skip during FC initialization + if (rxIsReceivingSignal() && rcSmoothingRxRateValid(currentRxRefreshRate)) { - if (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_OFF) { - derivativeCutoffFrequency = 0; - } else { - defaultDerivativeCutoffFrequency = calcRcSmoothingCutoff(avgRxFrameTime, (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_PT1)); - derivativeCutoffFrequency = (derivativeCutoffFrequency == 0) ? defaultDerivativeCutoffFrequency : derivativeCutoffFrequency; - } + // set the guard time expiration if it's not set + if (validRxFrameTimeMs == 0) { + validRxFrameTimeMs = currentTimeMs + (rcSmoothingData.filterInitialized ? RC_SMOOTHING_FILTER_RETRAINING_DELAY_MS : RC_SMOOTHING_FILTER_TRAINING_DELAY_MS); + } else { + sampleState = 1; + } - const float dT = targetPidLooptime * 1e-6f; - for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) { - if ((1 << i) & interpolationChannels) { - switch (rxConfig()->rc_smoothing_input_type) { - case RC_SMOOTHING_INPUT_PT1: - pt1FilterInit(&rcCommandFilterPt1[i], pt1FilterGain(filterCutoffFrequency, dT)); - break; - case RC_SMOOTHING_INPUT_BIQUAD: - default: - biquadFilterInitLPF(&rcCommandFilterBiquad[i], filterCutoffFrequency, targetPidLooptime); - break; - } + // if the guard time has expired then process the rx frame time + if (currentTimeMs > validRxFrameTimeMs) { + sampleState = 2; + bool accumulateSample = true; + + // During initial training process all samples. + // During retraining check samples to determine if they vary by more than the limit percentage. + if (rcSmoothingData.filterInitialized) { + const float percentChange = (ABS(currentRxRefreshRate - rcSmoothingData.averageFrameTimeUs) / (float)rcSmoothingData.averageFrameTimeUs) * 100; + if (percentChange < RC_SMOOTHING_RX_RATE_CHANGE_PERCENT) { + // We received a sample that wasn't more than the limit percent so reset the accumulation + // During retraining we need a contiguous block of samples that are all significantly different than the current average + rcSmoothingResetAccumulation(&rcSmoothingData); + accumulateSample = false; } } - pidInitSetpointDerivativeLpf(derivativeCutoffFrequency, rxConfig()->rc_smoothing_debug_axis, rxConfig()->rc_smoothing_derivative_type); - filterInitialized = true; + + // accumlate the sample into the average + if (accumulateSample) { + if (rcSmoothingAccumulateSample(&rcSmoothingData, currentRxRefreshRate)) { + // the required number of samples were collected so set the filter cutoffs + rcSmoothingSetFilterCutoffs(&rcSmoothingData); + rcSmoothingData.filterInitialized = true; + validRxFrameTimeMs = 0; + } + } + } + } else { + // we have either stopped receiving rx samples (failsafe?) or the sample time is unreasonable so reset the accumulation + rcSmoothingResetAccumulation(&rcSmoothingData); } - } else { - rxFrameTimeSum = 0; - rxFrameCount = 0; - validRxFrameTimeMs = 0; - minRxFrameInterval = UINT16_MAX; - maxRxFrameInterval = 0; + } + + // rx frame rate training blackbox debugging + if (debugMode == DEBUG_RC_SMOOTHING_RATE) { + DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 0, currentRxRefreshRate); // log each rx frame interval + DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 1, rcSmoothingData.training.count); // log the training step count + DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 2, rcSmoothingData.averageFrameTimeUs);// the current calculated average + DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 3, sampleState); // indicates whether guard time is active } } } - if (filterInitialized && (debugMode == DEBUG_RC_SMOOTHING)) { + if (rcSmoothingData.filterInitialized && (debugMode == DEBUG_RC_SMOOTHING)) { // after training has completed then log the raw rc channel and the calculated // average rx frame rate that was used to calculate the automatic filter cutoffs DEBUG_SET(DEBUG_RC_SMOOTHING, 0, lrintf(lastRxData[rxConfig()->rc_smoothing_debug_axis])); - DEBUG_SET(DEBUG_RC_SMOOTHING, 3, calculatedFrameTimeAverageUs); + DEBUG_SET(DEBUG_RC_SMOOTHING, 3, rcSmoothingData.averageFrameTimeUs); } + // each pid loop continue to apply the last received channel value to the filter for (updatedChannel = 0; updatedChannel < PRIMARY_CHANNEL_COUNT; updatedChannel++) { - if ((1 << updatedChannel) & interpolationChannels) { - if (filterInitialized) { + if ((1 << updatedChannel) & interpolationChannels) { // only smooth selected channels base on the rc_interp_ch value + if (rcSmoothingData.filterInitialized) { switch (rxConfig()->rc_smoothing_input_type) { case RC_SMOOTHING_INPUT_PT1: - rcCommand[updatedChannel] = pt1FilterApply(&rcCommandFilterPt1[updatedChannel], lastRxData[updatedChannel]); + rcCommand[updatedChannel] = pt1FilterApply((pt1Filter_t*) &rcSmoothingData.filter[updatedChannel], lastRxData[updatedChannel]); break; + case RC_SMOOTHING_INPUT_BIQUAD: default: - rcCommand[updatedChannel] = biquadFilterApply(&rcCommandFilterBiquad[updatedChannel], lastRxData[updatedChannel]); + rcCommand[updatedChannel] = biquadFilterApplyDF1((biquadFilter_t*) &rcSmoothingData.filter[updatedChannel], lastRxData[updatedChannel]); break; } } else { @@ -396,7 +541,7 @@ FAST_CODE void processRcCommand(void) { uint8_t updatedChannel; - if (isRXDataNew && isAntiGravityModeActive()) { + if (isRXDataNew && pidAntiGravityEnabled()) { checkForThrottleErrorResetState(currentRxRefreshRate); } @@ -479,7 +624,7 @@ FAST_CODE FAST_CODE_NOINLINE void updateRcCommands(void) } int32_t tmp; - if (feature(FEATURE_3D)) { + if (featureIsEnabled(FEATURE_3D)) { tmp = constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX); tmp = (uint32_t)(tmp - PWM_RANGE_MIN); } else { @@ -493,7 +638,7 @@ FAST_CODE FAST_CODE_NOINLINE void updateRcCommands(void) rcCommand[THROTTLE] = rcLookupThrottle(tmp); - if (feature(FEATURE_3D) && !failsafeIsActive()) { + if (featureIsEnabled(FEATURE_3D) && !failsafeIsActive()) { if (!flight3DConfig()->switched_mode3d) { if (IS_RC_MODE_ACTIVE(BOX3D)) { fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000); @@ -528,10 +673,6 @@ FAST_CODE FAST_CODE_NOINLINE void updateRcCommands(void) rcCommand[YAW] = rcCommandBuff.Z; } } - - if (FLIGHT_MODE(GPS_RESCUE_MODE)) { - rcCommand[THROTTLE] = rescueThrottle; - } } void resetYawAxis(void) @@ -595,22 +736,31 @@ void initRcProcessing(void) } } +bool rcSmoothingIsEnabled(void) +{ + return !( +#if defined(USE_RC_SMOOTHING_FILTER) + rxConfig()->rc_smoothing_type == RC_SMOOTHING_TYPE_INTERPOLATION && +#endif + rxConfig()->rcInterpolation == RC_SMOOTHING_OFF); +} + #ifdef USE_RC_SMOOTHING_FILTER int rcSmoothingGetValue(int whichValue) { switch (whichValue) { - case RC_SMOOTHING_VALUE_INPUT_AUTO: - return defaultInputCutoffFrequency; case RC_SMOOTHING_VALUE_INPUT_ACTIVE: - return filterCutoffFrequency; - case RC_SMOOTHING_VALUE_DERIVATIVE_AUTO: - return defaultDerivativeCutoffFrequency; + return rcSmoothingData.inputCutoffFrequency; case RC_SMOOTHING_VALUE_DERIVATIVE_ACTIVE: - return derivativeCutoffFrequency; + return rcSmoothingData.derivativeCutoffFrequency; case RC_SMOOTHING_VALUE_AVERAGE_FRAME: - return calculatedFrameTimeAverageUs; + return rcSmoothingData.averageFrameTimeUs; default: return 0; } } + +bool rcSmoothingInitializationComplete(void) { + return (rxConfig()->rc_smoothing_type != RC_SMOOTHING_TYPE_FILTER) || rcSmoothingData.filterInitialized; +} #endif // USE_RC_SMOOTHING_FILTER diff --git a/src/main/fc/fc_rc.h b/src/main/fc/rc.h similarity index 92% rename from src/main/fc/fc_rc.h rename to src/main/fc/rc.h index 64567da0ef..ae9586bf51 100644 --- a/src/main/fc/fc_rc.h +++ b/src/main/fc/rc.h @@ -39,4 +39,7 @@ void updateRcCommands(void); void resetYawAxis(void); void initRcProcessing(void); bool isMotorsReversed(void); +bool rcSmoothingIsEnabled(void); int rcSmoothingGetValue(int whichValue); +bool rcSmoothingAutoCalculate(void); +bool rcSmoothingInitializationComplete(void); diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index b8e1a0d897..2197625933 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -52,11 +52,13 @@ #include "fc/controlrate_profile.h" #include "fc/rc_adjustments.h" #include "fc/rc_controls.h" -#include "fc/fc_rc.h" +#include "fc/rc.h" #include "rx/rx.h" -PG_REGISTER_ARRAY(adjustmentRange_t, MAX_ADJUSTMENT_RANGE_COUNT, adjustmentRanges, PG_ADJUSTMENT_RANGE_CONFIG, 0); +#define ADJUSTMENT_RANGE_COUNT_INVALID -1 + +PG_REGISTER_ARRAY(adjustmentRange_t, MAX_ADJUSTMENT_RANGE_COUNT, adjustmentRanges, PG_ADJUSTMENT_RANGE_CONFIG, 1); uint8_t pidAudioPositionToModeMap[7] = { // on a pot with a center detent, it's easy to have center area for off/default, then three positions to the left and three to the right. @@ -75,6 +77,11 @@ uint8_t pidAudioPositionToModeMap[7] = { static pidProfile_t *pidProfile; +static int activeAdjustmentCount = ADJUSTMENT_RANGE_COUNT_INVALID; +static uint8_t activeAdjustmentArray[MAX_ADJUSTMENT_RANGE_COUNT]; +static int activeAbsoluteAdjustmentCount; +static uint8_t activeAbsoluteAdjustmentArray[MAX_ADJUSTMENT_RANGE_COUNT]; + static void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFunction, int32_t newValue) { #ifndef USE_BLACKBOX @@ -203,11 +210,11 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU .mode = ADJUSTMENT_MODE_STEP, .data = { .step = 1 } }, { - .adjustmentFunction = ADJUSTMENT_D_SETPOINT, + .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_F, .mode = ADJUSTMENT_MODE_STEP, .data = { .step = 1 } }, { - .adjustmentFunction = ADJUSTMENT_D_SETPOINT_TRANSITION, + .adjustmentFunction = ADJUSTMENT_FEEDFORWARD_TRANSITION, .mode = ADJUSTMENT_MODE_STEP, .data = { .step = 1 } }, { @@ -218,6 +225,18 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU .adjustmentFunction = ADJUSTMENT_PID_AUDIO, .mode = ADJUSTMENT_MODE_SELECT, .data = { .switchPositions = ARRAYLEN(pidAudioPositionToModeMap) } + }, { + .adjustmentFunction = ADJUSTMENT_PITCH_F, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .step = 1 } + }, { + .adjustmentFunction = ADJUSTMENT_ROLL_F, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .step = 1 } + }, { + .adjustmentFunction = ADJUSTMENT_YAW_F, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .step = 1 } } }; @@ -244,14 +263,21 @@ static const char * const adjustmentLabels[] = { "ROLL I", "ROLL D", "RC RATE YAW", - "D SETPOINT", - "D SETPOINT TRANSITION", + "PITCH/ROLL F", + "FF TRANSITION", "HORIZON STRENGTH", + "ROLL RC RATE", + "PITCH RC RATE", + "ROLL RC EXPO", + "PITCH RC EXPO", "PID AUDIO", + "PITCH F", + "ROLL F", + "YAW F" }; -const char *adjustmentRangeName; -int adjustmentRangeValue = -1; +static int adjustmentRangeNameIndex = 0; +static int adjustmentRangeValue = -1; #endif #define ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET 1 @@ -401,15 +427,31 @@ static int applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t a controlRateConfig->rcRates[FD_YAW] = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE_YAW, newValue); break; - case ADJUSTMENT_D_SETPOINT: - newValue = constrain((int)pidProfile->dtermSetpointWeight + delta, 0, 2000); // FIXME magic numbers repeated in cli.c - pidProfile->dtermSetpointWeight = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT, newValue); + case ADJUSTMENT_PITCH_ROLL_F: + case ADJUSTMENT_PITCH_F: + newValue = constrain(pidProfile->pid[PID_PITCH].F + delta, 0, 2000); + pidProfile->pid[PID_PITCH].F = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_F, newValue); + + if (adjustmentFunction == ADJUSTMENT_PITCH_F) { + break; + } + // fall through for combined ADJUSTMENT_PITCH_ROLL_F + FALLTHROUGH; + case ADJUSTMENT_ROLL_F: + newValue = constrain(pidProfile->pid[PID_ROLL].F + delta, 0, 2000); + pidProfile->pid[PID_ROLL].F = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_F, newValue); break; - case ADJUSTMENT_D_SETPOINT_TRANSITION: - newValue = constrain((int)pidProfile->setpointRelaxRatio + delta, 1, 100); // FIXME magic numbers repeated in cli.c - pidProfile->setpointRelaxRatio = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT_TRANSITION, newValue); + case ADJUSTMENT_YAW_F: + newValue = constrain(pidProfile->pid[PID_YAW].F + delta, 0, 2000); + pidProfile->pid[PID_YAW].F = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_F, newValue); + break; + case ADJUSTMENT_FEEDFORWARD_TRANSITION: + newValue = constrain(pidProfile->feedForwardTransition + delta, 1, 100); // FIXME magic numbers repeated in cli.c + pidProfile->feedForwardTransition = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_FEEDFORWARD_TRANSITION, newValue); break; default: newValue = -1; @@ -550,15 +592,31 @@ static int applyAbsoluteAdjustment(controlRateConfig_t *controlRateConfig, adjus controlRateConfig->rcRates[FD_YAW] = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE_YAW, newValue); break; - case ADJUSTMENT_D_SETPOINT: - newValue = constrain(value, 0, 2000); // FIXME magic numbers repeated in cli.c - pidProfile->dtermSetpointWeight = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT, newValue); + case ADJUSTMENT_PITCH_ROLL_F: + case ADJUSTMENT_PITCH_F: + newValue = constrain(value, 0, 2000); + pidProfile->pid[PID_PITCH].F = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_F, newValue); + + if (adjustmentFunction == ADJUSTMENT_PITCH_F) { + break; + } + // fall through for combined ADJUSTMENT_PITCH_ROLL_F + FALLTHROUGH; + case ADJUSTMENT_ROLL_F: + newValue = constrain(value, 0, 2000); + pidProfile->pid[PID_ROLL].F = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_F, newValue); break; - case ADJUSTMENT_D_SETPOINT_TRANSITION: + case ADJUSTMENT_YAW_F: + newValue = constrain(value, 0, 2000); + pidProfile->pid[PID_YAW].F = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_F, newValue); + break; + case ADJUSTMENT_FEEDFORWARD_TRANSITION: newValue = constrain(value, 1, 100); // FIXME magic numbers repeated in cli.c - pidProfile->setpointRelaxRatio = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT_TRANSITION, newValue); + pidProfile->feedForwardTransition = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_FEEDFORWARD_TRANSITION, newValue); break; default: newValue = -1; @@ -613,6 +671,38 @@ static uint8_t applySelectAdjustment(adjustmentFunction_e adjustmentFunction, ui return position; } +static void calcActiveAdjustmentRanges(void) +{ + adjustmentRange_t defaultAdjustmentRange; + memset(&defaultAdjustmentRange, 0, sizeof(defaultAdjustmentRange)); + + activeAdjustmentCount = 0; + activeAbsoluteAdjustmentCount = 0; + for (int i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) { + const adjustmentRange_t * const adjustmentRange = adjustmentRanges(i); + if (memcmp(adjustmentRange, &defaultAdjustmentRange, sizeof(defaultAdjustmentRange)) != 0) { + if (adjustmentRange->adjustmentCenter == 0) { + activeAdjustmentArray[activeAdjustmentCount++] = i; + } else { + activeAbsoluteAdjustmentArray[activeAbsoluteAdjustmentCount++] = i; + } + } + } +} + +static void updateAdjustmentStates(void) +{ + for (int index = 0; index < activeAdjustmentCount; index++) { + const adjustmentRange_t * const adjustmentRange = adjustmentRanges(activeAdjustmentArray[index]); + // Only use slots if center value has not been specified, otherwise apply values directly (scaled) from aux channel + if (isRangeActive(adjustmentRange->auxChannelIndex, &adjustmentRange->range) && + (adjustmentRange->adjustmentCenter == 0)) { + const adjustmentConfig_t *adjustmentConfig = &defaultAdjustmentConfigs[adjustmentRange->adjustmentFunction - ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET]; + configureAdjustment(adjustmentRange->adjustmentIndex, adjustmentRange->auxSwitchChannelIndex, adjustmentConfig); + } + } +} + #define RESET_FREQUENCY_2HZ (1000 / 2) void processRcAdjustments(controlRateConfig_t *controlRateConfig) @@ -623,6 +713,13 @@ void processRcAdjustments(controlRateConfig_t *controlRateConfig) const bool canUseRxData = rxIsReceivingSignal(); + // Recalculate the new active adjustments if required + if (activeAdjustmentCount == ADJUSTMENT_RANGE_COUNT_INVALID) { + calcActiveAdjustmentRanges(); + } + + updateAdjustmentStates(); + // Process Increment/Decrement adjustments for (int adjustmentIndex = 0; adjustmentIndex < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT; adjustmentIndex++) { adjustmentState_t *adjustmentState = &adjustmentStates[adjustmentIndex]; @@ -680,7 +777,7 @@ void processRcAdjustments(controlRateConfig_t *controlRateConfig) #if defined(USE_OSD) && defined(USE_OSD_ADJUSTMENTS) if (newValue != -1 && adjustmentState->config->adjustmentFunction != ADJUSTMENT_RATE_PROFILE) { // Rate profile already has an OSD element - adjustmentRangeName = &adjustmentLabels[adjustmentFunction - 1][0]; + adjustmentRangeNameIndex = adjustmentFunction; adjustmentRangeValue = newValue; } #else @@ -690,9 +787,9 @@ void processRcAdjustments(controlRateConfig_t *controlRateConfig) } // Process Absolute adjustments - for (int index = 0; index < MAX_ADJUSTMENT_RANGE_COUNT; index++) { + for (int i = 0; i < activeAbsoluteAdjustmentCount; i++) { static int16_t lastRcData[MAX_ADJUSTMENT_RANGE_COUNT] = { 0 }; - + int index = activeAbsoluteAdjustmentArray[i]; const adjustmentRange_t * const adjustmentRange = adjustmentRanges(index); const uint8_t channelIndex = NON_AUX_CHANNEL_COUNT + adjustmentRange->auxSwitchChannelIndex; const adjustmentConfig_t *adjustmentConfig = &defaultAdjustmentConfigs[adjustmentRange->adjustmentFunction - ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET]; @@ -716,20 +813,28 @@ void resetAdjustmentStates(void) memset(adjustmentStates, 0, sizeof(adjustmentStates)); } -void updateAdjustmentStates(void) -{ - for (int index = 0; index < MAX_ADJUSTMENT_RANGE_COUNT; index++) { - const adjustmentRange_t * const adjustmentRange = adjustmentRanges(index); - // Only use slots if center value has not been specified, otherwise apply values directly (scaled) from aux channel - if (isRangeActive(adjustmentRange->auxChannelIndex, &adjustmentRange->range) && - (adjustmentRange->adjustmentCenter == 0)) { - const adjustmentConfig_t *adjustmentConfig = &defaultAdjustmentConfigs[adjustmentRange->adjustmentFunction - ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET]; - configureAdjustment(adjustmentRange->adjustmentIndex, adjustmentRange->auxSwitchChannelIndex, adjustmentConfig); - } - } -} - void useAdjustmentConfig(pidProfile_t *pidProfileToUse) { pidProfile = pidProfileToUse; } + +#if defined(USE_OSD) && defined(USE_OSD_ADJUSTMENTS) +const char *getAdjustmentsRangeName(void) +{ + if (adjustmentRangeNameIndex > 0) { + return &adjustmentLabels[adjustmentRangeNameIndex - 1][0]; + } else { + return NULL; + } +} + +int getAdjustmentsRangeValue(void) +{ + return adjustmentRangeValue; +} +#endif + +void activeAdjustmentRangeReset(void) +{ + activeAdjustmentCount = ADJUSTMENT_RANGE_COUNT_INVALID; +} diff --git a/src/main/fc/rc_adjustments.h b/src/main/fc/rc_adjustments.h index 78fcc67846..36f21ec590 100644 --- a/src/main/fc/rc_adjustments.h +++ b/src/main/fc/rc_adjustments.h @@ -47,14 +47,17 @@ typedef enum { ADJUSTMENT_ROLL_I, ADJUSTMENT_ROLL_D, ADJUSTMENT_RC_RATE_YAW, - ADJUSTMENT_D_SETPOINT, - ADJUSTMENT_D_SETPOINT_TRANSITION, + ADJUSTMENT_PITCH_ROLL_F, + ADJUSTMENT_FEEDFORWARD_TRANSITION, ADJUSTMENT_HORIZON_STRENGTH, ADJUSTMENT_ROLL_RC_RATE, ADJUSTMENT_PITCH_RC_RATE, ADJUSTMENT_ROLL_RC_EXPO, ADJUSTMENT_PITCH_RC_EXPO, ADJUSTMENT_PID_AUDIO, + ADJUSTMENT_PITCH_F, + ADJUSTMENT_ROLL_F, + ADJUSTMENT_YAW_F, ADJUSTMENT_FUNCTION_COUNT } adjustmentFunction_e; @@ -74,7 +77,7 @@ typedef struct adjustmentConfig_s { adjustmentData_t data; } adjustmentConfig_t; -#define MAX_ADJUSTMENT_RANGE_COUNT 15 +#define MAX_ADJUSTMENT_RANGE_COUNT 30 typedef struct adjustmentRange_s { // when aux channel is in range... @@ -105,12 +108,11 @@ typedef struct adjustmentState_s { #define MAX_SIMULTANEOUS_ADJUSTMENT_COUNT 4 // enough for 4 x 3position switches / 4 aux channel #endif -extern const char *adjustmentRangeName; -extern int adjustmentRangeValue; - void resetAdjustmentStates(void); -void updateAdjustmentStates(void); struct controlRateConfig_s; void processRcAdjustments(struct controlRateConfig_s *controlRateConfig); struct pidProfile_s; void useAdjustmentConfig(struct pidProfile_s *pidProfileToUse); +const char *getAdjustmentsRangeName(void); +int getAdjustmentsRangeValue(void); +void activeAdjustmentRangeReset(void); diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 902a8ad944..b5b46ebfe4 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -43,9 +43,9 @@ #include "drivers/camera_control.h" #include "fc/config.h" -#include "fc/fc_core.h" +#include "fc/core.h" #include "fc/rc_controls.h" -#include "fc/fc_rc.h" +#include "fc/rc.h" #include "fc/runtime_config.h" #include "io/gps.h" @@ -113,7 +113,7 @@ bool areSticksInApModePosition(uint16_t ap_mode) throttleStatus_e calculateThrottleStatus(void) { - if (feature(FEATURE_3D)) { + if (featureIsEnabled(FEATURE_3D)) { if (IS_RC_MODE_ACTIVE(BOX3D) || flight3DConfig()->switched_mode3d) { if (rcData[THROTTLE] < rxConfig()->mincheck) { return THROTTLE_LOW; @@ -216,7 +216,8 @@ void processRcStickPositions() if (!ARMING_FLAG(ARMED)) { // Arm via YAW tryArm(); - if (isTryingToArm()) { + if (isTryingToArm() || + ((getArmingDisableFlags() == ARMING_DISABLED_CALIBRATING) && armingConfig()->gyro_cal_on_first_arm)) { doNotRepeat = false; } } else { @@ -240,7 +241,7 @@ void processRcStickPositions() gyroStartCalibration(false); #ifdef USE_GPS - if (feature(FEATURE_GPS)) { + if (featureIsEnabled(FEATURE_GPS)) { GPS_reset_home_position(); } #endif @@ -253,7 +254,7 @@ void processRcStickPositions() return; } - if (feature(FEATURE_INFLIGHT_ACC_CAL) && (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_HI)) { + if (featureIsEnabled(FEATURE_INFLIGHT_ACC_CAL) && (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_HI)) { // Inflight ACC Calibration handleInflightCalibrationStickPosition(); return; diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index 8820de8683..557aef58d1 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -22,6 +22,7 @@ #include +#include "common/filter.h" #include "pg/pg.h" typedef enum rc_alias { @@ -77,9 +78,7 @@ typedef enum { } rcSmoothingDerivativeFilter_e; typedef enum { - RC_SMOOTHING_VALUE_INPUT_AUTO, RC_SMOOTHING_VALUE_INPUT_ACTIVE, - RC_SMOOTHING_VALUE_DERIVATIVE_AUTO, RC_SMOOTHING_VALUE_DERIVATIVE_ACTIVE, RC_SMOOTHING_VALUE_AVERAGE_FRAME } rcSmoothingInfoType_e; @@ -101,6 +100,9 @@ typedef enum { #define CONTROL_RATE_CONFIG_RC_RATES_MAX 255 +#define CONTROL_RATE_CONFIG_RATE_LIMIT_MIN 200 +#define CONTROL_RATE_CONFIG_RATE_LIMIT_MAX 1998 + // (Super) rates are constrained to [0, 100] for Betaflight rates, so values higher than 100 won't make a difference. Range extended for RaceFlight rates. #define CONTROL_RATE_CONFIG_RATE_MAX 255 @@ -108,6 +110,27 @@ typedef enum { extern float rcCommand[4]; +typedef struct rcSmoothingFilterTraining_s { + float sum; + int count; + uint16_t min; + uint16_t max; +} rcSmoothingFilterTraining_t; + +typedef union rcSmoothingFilterTypes_u { + pt1Filter_t pt1Filter; + biquadFilter_t biquadFilter; +} rcSmoothingFilterTypes_t; + +typedef struct rcSmoothingFilter_s { + bool filterInitialized; + rcSmoothingFilterTypes_t filter[4]; + uint16_t inputCutoffFrequency; + uint16_t derivativeCutoffFrequency; + int averageFrameTimeUs; + rcSmoothingFilterTraining_t training; +} rcSmoothingFilter_t; + typedef struct rcControlsConfig_s { uint8_t deadband; // introduce a deadband around the stick center for pitch and roll axis. Must be greater than zero. uint8_t yaw_deadband; // introduce a deadband around the stick center for yaw axis. Must be greater than zero. diff --git a/src/main/fc/rc_modes.c b/src/main/fc/rc_modes.c index 8293488e74..aabe64b90a 100644 --- a/src/main/fc/rc_modes.c +++ b/src/main/fc/rc_modes.c @@ -28,6 +28,7 @@ #include "common/bitarray.h" #include "common/maths.h" +#include "drivers/time.h" #include "config/feature.h" #include "pg/pg.h" @@ -39,11 +40,14 @@ #include "rx/rx.h" -boxBitmask_t rcModeActivationMask; // one bit per mode defined in boxId_e -static bool modeChangesDisabled = false; +#define STICKY_MODE_BOOT_DELAY_US 5e6 -PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions, - PG_MODE_ACTIVATION_PROFILE, 1); +boxBitmask_t rcModeActivationMask; // one bit per mode defined in boxId_e +static boxBitmask_t stickyModesEverDisabled; + +static bool airmodeEnabled; + +PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions, PG_MODE_ACTIVATION_PROFILE, 2); bool IS_RC_MODE_ACTIVE(boxId_e boxId) { @@ -55,16 +59,8 @@ void rcModeUpdate(boxBitmask_t *newState) rcModeActivationMask = *newState; } -void preventModeChanges(void) { - modeChangesDisabled = true; -} - -bool isAirmodeActive(void) { - return (IS_RC_MODE_ACTIVE(BOXAIRMODE) || feature(FEATURE_AIRMODE)); -} - -bool isAntiGravityModeActive(void) { - return (IS_RC_MODE_ACTIVE(BOXANTIGRAVITY) || feature(FEATURE_ANTI_GRAVITY)); +bool airmodeIsEnabled(void) { + return airmodeEnabled; } bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range) { @@ -77,40 +73,72 @@ bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range) { channelValue < 900 + (range->endStep * 25)); } +void updateMasksForMac(const modeActivationCondition_t *mac, boxBitmask_t *andMask, boxBitmask_t *newMask) +{ + bool bAnd = (mac->modeLogic == MODELOGIC_AND) || bitArrayGet(andMask, mac->modeId); + bool bAct = isRangeActive(mac->auxChannelIndex, &mac->range); + if (bAnd) + bitArraySet(andMask, mac->modeId); + if (bAnd != bAct) + bitArraySet(newMask, mac->modeId); +} + +void updateMasksForStickyModes(const modeActivationCondition_t *mac, boxBitmask_t *andMask, boxBitmask_t *newMask) +{ + if (IS_RC_MODE_ACTIVE(mac->modeId)) { + bitArrayClr(andMask, mac->modeId); + bitArraySet(newMask, mac->modeId); + } else { + if (bitArrayGet(&stickyModesEverDisabled, mac->modeId)) { + updateMasksForMac(mac, andMask, newMask); + } else { + if (micros() >= STICKY_MODE_BOOT_DELAY_US && !isRangeActive(mac->auxChannelIndex, &mac->range)) { + bitArraySet(&stickyModesEverDisabled, mac->modeId); + } + } + } +} void updateActivatedModes(void) { - boxBitmask_t newMask, andMask; + boxBitmask_t newMask, andMask, stickyModes; memset(&andMask, 0, sizeof(andMask)); - - if (!modeChangesDisabled) { - memset(&newMask, 0, sizeof(newMask)); - } else { - memcpy(&newMask, &rcModeActivationMask, sizeof(newMask)); - } + memset(&newMask, 0, sizeof(newMask)); + memset(&stickyModes, 0, sizeof(stickyModes)); + bitArraySet(&stickyModes, BOXPARALYZE); // determine which conditions set/clear the mode for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { const modeActivationCondition_t *mac = modeActivationConditions(i); - boxId_e mode = mac->modeId; - - if (modeChangesDisabled && mode != BOXBEEPERON) { + // Skip linked macs for now to fully determine target states + if (mac->linkedTo) { continue; } - if (mode < CHECKBOX_ITEM_COUNT) { - bool bAnd = (mac->modeLogic == MODELOGIC_AND) || bitArrayGet(&andMask, mode); - bool bAct = isRangeActive(mac->auxChannelIndex, &mac->range); - if (bAnd) - bitArraySet(&andMask, mode); - if (bAnd != bAct) - bitArraySet(&newMask, mode); + if (bitArrayGet(&stickyModes, mac->modeId)) { + updateMasksForStickyModes(mac, &andMask, &newMask); + } else if (mac->modeId < CHECKBOX_ITEM_COUNT) { + updateMasksForMac(mac, &andMask, &newMask); } } + bitArrayXor(&newMask, sizeof(&newMask), &newMask, &andMask); + // Update linked modes + for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { + const modeActivationCondition_t *mac = modeActivationConditions(i); + + if (!mac->linkedTo) { + continue; + } + + bitArrayCopy(&newMask, mac->linkedTo, mac->modeId); + } + rcModeUpdate(&newMask); + + airmodeEnabled = featureIsEnabled(FEATURE_AIRMODE) || IS_RC_MODE_ACTIVE(BOXAIRMODE); } bool isModeActivationConditionPresent(boxId_e modeId) @@ -118,10 +146,34 @@ bool isModeActivationConditionPresent(boxId_e modeId) for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { const modeActivationCondition_t *mac = modeActivationConditions(i); - if (mac->modeId == modeId && IS_RANGE_USABLE(&mac->range)) { + if (mac->modeId == modeId && (IS_RANGE_USABLE(&mac->range) || mac->linkedTo)) { return true; } } return false; } + +void removeModeActivationCondition(const boxId_e modeId) +{ + unsigned offset = 0; + for (unsigned i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { + modeActivationCondition_t *mac = modeActivationConditionsMutable(i); + + if (mac->modeId == modeId && !offset) { + offset++; + } + + if (offset) { + while (i + offset < MAX_MODE_ACTIVATION_CONDITION_COUNT && modeActivationConditions(i + offset)->modeId == modeId) { + offset++; + } + + if (i + offset < MAX_MODE_ACTIVATION_CONDITION_COUNT) { + memcpy(mac, modeActivationConditions(i + offset), sizeof(modeActivationCondition_t)); + } else { + memset(mac, 0, sizeof(modeActivationCondition_t)); + } + } + } +} diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index f5b0b4814b..68925a9ae5 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -33,12 +33,8 @@ typedef enum { BOXANGLE, BOXHORIZON, BOXMAG, - BOXBARO, - BOXGPSHOME, - BOXGPSHOLD, BOXHEADFREE, BOXPASSTHRU, - BOXRANGEFINDER, BOXFAILSAFE, BOXGPSRESCUE, BOXID_FLIGHTMODE_LAST = BOXGPSRESCUE, @@ -49,16 +45,11 @@ typedef enum { BOXANTIGRAVITY, BOXHEADADJ, BOXCAMSTAB, - BOXCAMTRIG, BOXBEEPERON, - BOXLEDMAX, BOXLEDLOW, - BOXLLIGHTS, BOXCALIB, - BOXGOV, BOXOSD, BOXTELEMETRY, - BOXGTUNE, BOXSERVO1, BOXSERVO2, BOXSERVO3, @@ -81,6 +72,7 @@ typedef enum { BOXUSER4, BOXPIDAUDIO, BOXACROTRAINER, + BOXVTXCONTROLDISABLE, CHECKBOX_ITEM_COUNT } boxId_e; @@ -117,6 +109,7 @@ typedef struct modeActivationCondition_s { uint8_t auxChannelIndex; channelRange_t range; modeLogic_e modeLogic; + boxId_e linkedTo; } modeActivationCondition_t; PG_DECLARE_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions); @@ -129,11 +122,10 @@ typedef struct modeActivationProfile_s { bool IS_RC_MODE_ACTIVE(boxId_e boxId); void rcModeUpdate(boxBitmask_t *newState); -void preventModeChanges(void); -bool isAirmodeActive(void); -bool isAntiGravityModeActive(void); +bool airmodeIsEnabled(void); bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range); void updateActivatedModes(void); bool isModeActivationConditionPresent(boxId_e modeId); +void removeModeActivationCondition(boxId_e modeId); diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index b4a9ab291e..fb0f787dc3 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -73,13 +73,13 @@ typedef enum { ANGLE_MODE = (1 << 0), HORIZON_MODE = (1 << 1), MAG_MODE = (1 << 2), - BARO_MODE = (1 << 3), - GPS_HOME_MODE = (1 << 4), - GPS_HOLD_MODE = (1 << 5), +// BARO_MODE = (1 << 3), +// GPS_HOME_MODE = (1 << 4), +// GPS_HOLD_MODE = (1 << 5), HEADFREE_MODE = (1 << 6), - UNUSED_MODE = (1 << 7), // old autotune +// UNUSED_MODE = (1 << 7), // old autotune PASSTHRU_MODE = (1 << 8), - RANGEFINDER_MODE= (1 << 9), +// RANGEFINDER_MODE= (1 << 9), FAILSAFE_MODE = (1 << 10), GPS_RESCUE_MODE = (1 << 11) } flightModeFlags_e; @@ -96,12 +96,8 @@ extern uint16_t flightModeFlags; [BOXANGLE] = LOG2(ANGLE_MODE), \ [BOXHORIZON] = LOG2(HORIZON_MODE), \ [BOXMAG] = LOG2(MAG_MODE), \ - [BOXBARO] = LOG2(BARO_MODE), \ - [BOXGPSHOME] = LOG2(GPS_HOME_MODE), \ - [BOXGPSHOLD] = LOG2( GPS_HOLD_MODE), \ [BOXHEADFREE] = LOG2(HEADFREE_MODE), \ [BOXPASSTHRU] = LOG2(PASSTHRU_MODE), \ - [BOXRANGEFINDER] = LOG2(RANGEFINDER_MODE), \ [BOXFAILSAFE] = LOG2(FAILSAFE_MODE), \ [BOXGPSRESCUE] = LOG2(GPS_RESCUE_MODE), \ } \ diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/tasks.c similarity index 84% rename from src/main/fc/fc_tasks.c rename to src/main/fc/tasks.c index 4b781fc0a8..19e016a377 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/tasks.c @@ -38,15 +38,16 @@ #include "drivers/compass/compass.h" #include "drivers/sensor.h" #include "drivers/serial.h" +#include "drivers/serial_usb_vcp.h" #include "drivers/stack_check.h" #include "drivers/transponder_ir.h" +#include "drivers/usb_io.h" #include "drivers/vtx_common.h" #include "fc/config.h" -#include "fc/fc_core.h" -#include "fc/fc_rc.h" -#include "fc/fc_dispatch.h" -#include "fc/fc_tasks.h" +#include "fc/core.h" +#include "fc/rc.h" +#include "fc/dispatch.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" @@ -58,17 +59,18 @@ #include "interface/cli.h" #include "interface/msp.h" +#include "io/asyncfatfs/asyncfatfs.h" #include "io/beeper.h" #include "io/dashboard.h" #include "io/gps.h" #include "io/ledstrip.h" #include "io/osd.h" -#include "io/osd_slave.h" #include "io/piniobox.h" #include "io/serial.h" #include "io/transponder_ir.h" #include "io/vtx_tramp.h" // Will be gone #include "io/rcdevice_cam.h" +#include "io/usb_cdc_hid.h" #include "io/vtx.h" #include "msp/msp_serial.h" @@ -91,6 +93,10 @@ #include "telemetry/telemetry.h" +#ifdef USE_BST +#include "i2c_bst.h" +#endif + #ifdef USE_USB_CDC_HID //TODO: Make it platform independent in the future #ifdef STM32F4 @@ -102,21 +108,26 @@ #endif #endif -#ifdef USE_BST -void taskBstMasterProcess(timeUs_t currentTimeUs); -#endif +#include "tasks.h" -bool taskSerialCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs) +static void taskMain(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); - UNUSED(currentDeltaTimeUs); - return mspSerialWaiting(); +#ifdef USE_SDCARD + afatfs_poll(); +#endif } static void taskHandleSerial(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); + +#if defined(USE_VCP) + DEBUG_SET(DEBUG_USB, 0, usbCableIsInserted()); + DEBUG_SET(DEBUG_USB, 1, usbVcpIsConnected()); +#endif + #ifdef USE_CLI // in cli mode, all serial stuff goes to here. enter cli mode by sending # if (cliMode) { @@ -124,15 +135,11 @@ static void taskHandleSerial(timeUs_t currentTimeUs) return; } #endif -#ifndef OSD_SLAVE bool evaluateMspData = ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA; -#else - bool evaluateMspData = osdSlaveIsLocked ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA;; -#endif mspSerialProcess(evaluateMspData, mspFcProcessCommand, mspFcProcessReply); } -void taskBatteryAlerts(timeUs_t currentTimeUs) +static void taskBatteryAlerts(timeUs_t currentTimeUs) { if (!ARMING_FLAG(ARMED)) { // the battery *might* fall out in flight, but if that happens the FC will likely be off too unless the user has battery backup. @@ -142,7 +149,6 @@ void taskBatteryAlerts(timeUs_t currentTimeUs) batteryUpdateAlarms(); } -#ifndef USE_OSD_SLAVE static void taskUpdateAccelerometer(timeUs_t currentTimeUs) { accUpdate(currentTimeUs, &accelerometerConfigMutable()->accelerometerTrims); @@ -155,22 +161,13 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs) } static timeUs_t lastRxTimeUs; - currentRxRefreshRate = constrain(currentTimeUs - lastRxTimeUs, 1000, 20000); + currentRxRefreshRate = constrain(currentTimeUs - lastRxTimeUs, 1000, 30000); lastRxTimeUs = currentTimeUs; isRXDataNew = true; #ifdef USE_USB_CDC_HID if (!ARMING_FLAG(ARMED)) { - int8_t report[8]; - for (int i = 0; i < 8; i++) { - report[i] = scaleRange(constrain(rcData[i], 1000, 2000), 1000, 2000, -127, 127); - } -#ifdef STM32F4 - USBD_HID_SendReport(&USB_OTG_dev, (uint8_t*)report, sizeof(report)); -#elif defined(STM32F7) - extern USBD_HandleTypeDef USBD_Device; - USBD_HID_SendReport(&USBD_Device, (uint8_t*)report, sizeof(report)); -#endif + sendRcDataToHid(); } #endif @@ -178,7 +175,6 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs) updateRcCommands(); updateArmingStatus(); } -#endif #ifdef USE_BARO static void taskUpdateBaro(timeUs_t currentTimeUs) @@ -204,14 +200,16 @@ static void taskCalculateAltitude(timeUs_t currentTimeUs) #ifdef USE_TELEMETRY static void taskTelemetry(timeUs_t currentTimeUs) { - if (!cliMode && feature(FEATURE_TELEMETRY)) { + if (!cliMode && featureIsEnabled(FEATURE_TELEMETRY)) { + subTaskTelemetryPollSensors(currentTimeUs); + telemetryProcess(currentTimeUs); } } #endif #ifdef USE_CAMERA_CONTROL -void taskCameraControl(uint32_t currentTime) +static void taskCameraControl(uint32_t currentTime) { if (ARMING_FLAG(ARMED)) { return; @@ -224,6 +222,9 @@ void taskCameraControl(uint32_t currentTime) void fcTasksInit(void) { schedulerInit(); + + setTaskEnabled(TASK_MAIN, true); + setTaskEnabled(TASK_SERIAL, true); rescheduleTask(TASK_SERIAL, TASK_PERIOD_HZ(serialConfig()->serial_update_rate_hz)); @@ -231,24 +232,17 @@ void fcTasksInit(void) setTaskEnabled(TASK_BATTERY_VOLTAGE, useBatteryVoltage); const bool useBatteryCurrent = batteryConfig()->currentMeterSource != CURRENT_METER_NONE; setTaskEnabled(TASK_BATTERY_CURRENT, useBatteryCurrent); -#ifdef USE_OSD_SLAVE - const bool useBatteryAlerts = batteryConfig()->useVBatAlerts || batteryConfig()->useConsumptionAlerts; -#else - const bool useBatteryAlerts = batteryConfig()->useVBatAlerts || batteryConfig()->useConsumptionAlerts || feature(FEATURE_OSD); -#endif + const bool useBatteryAlerts = batteryConfig()->useVBatAlerts || batteryConfig()->useConsumptionAlerts || featureIsEnabled(FEATURE_OSD); setTaskEnabled(TASK_BATTERY_ALERTS, (useBatteryVoltage || useBatteryCurrent) && useBatteryAlerts); #ifdef USE_TRANSPONDER - setTaskEnabled(TASK_TRANSPONDER, feature(FEATURE_TRANSPONDER)); + setTaskEnabled(TASK_TRANSPONDER, featureIsEnabled(FEATURE_TRANSPONDER)); #endif #ifdef STACK_CHECK setTaskEnabled(TASK_STACK_CHECK, true); #endif -#ifdef USE_OSD_SLAVE - setTaskEnabled(TASK_OSD_SLAVE, true); -#else if (sensors(SENSOR_GYRO)) { rescheduleTask(TASK_GYROPID, gyro.targetLooptime); setTaskEnabled(TASK_GYROPID, true); @@ -267,23 +261,29 @@ void fcTasksInit(void) #ifdef USE_BEEPER setTaskEnabled(TASK_BEEPER, true); #endif + #ifdef USE_GPS - setTaskEnabled(TASK_GPS, feature(FEATURE_GPS)); + setTaskEnabled(TASK_GPS, featureIsEnabled(FEATURE_GPS)); #endif + #ifdef USE_MAG setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG)); #endif + #ifdef USE_BARO setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO)); #endif + #if defined(USE_BARO) || defined(USE_GPS) - setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || feature(FEATURE_GPS)); + setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || featureIsEnabled(FEATURE_GPS)); #endif + #ifdef USE_DASHBOARD - setTaskEnabled(TASK_DASHBOARD, feature(FEATURE_DASHBOARD)); + setTaskEnabled(TASK_DASHBOARD, featureIsEnabled(FEATURE_DASHBOARD)); #endif + #ifdef USE_TELEMETRY - if (feature(FEATURE_TELEMETRY)) { + if (featureIsEnabled(FEATURE_TELEMETRY)) { setTaskEnabled(TASK_TELEMETRY, true); if (rxConfig()->serialrx_provider == SERIALRX_JETIEXBUS) { // Reschedule telemetry to 500hz for Jeti Exbus @@ -294,67 +294,80 @@ void fcTasksInit(void) } } #endif + #ifdef USE_LED_STRIP - setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP)); + setTaskEnabled(TASK_LEDSTRIP, featureIsEnabled(FEATURE_LED_STRIP)); #endif + #ifdef USE_TRANSPONDER - setTaskEnabled(TASK_TRANSPONDER, feature(FEATURE_TRANSPONDER)); + setTaskEnabled(TASK_TRANSPONDER, featureIsEnabled(FEATURE_TRANSPONDER)); #endif + #ifdef USE_OSD - setTaskEnabled(TASK_OSD, feature(FEATURE_OSD)); + setTaskEnabled(TASK_OSD, featureIsEnabled(FEATURE_OSD) && osdInitialized()); #endif + #ifdef USE_BST setTaskEnabled(TASK_BST_MASTER_PROCESS, true); #endif + #ifdef USE_ESC_SENSOR - setTaskEnabled(TASK_ESC_SENSOR, feature(FEATURE_ESC_SENSOR)); + setTaskEnabled(TASK_ESC_SENSOR, featureIsEnabled(FEATURE_ESC_SENSOR)); #endif + #ifdef USE_ADC_INTERNAL setTaskEnabled(TASK_ADC_INTERNAL, true); #endif + #ifdef USE_PINIOBOX setTaskEnabled(TASK_PINIOBOX, true); #endif + #ifdef USE_CMS #ifdef USE_MSP_DISPLAYPORT setTaskEnabled(TASK_CMS, true); #else - setTaskEnabled(TASK_CMS, feature(FEATURE_OSD) || feature(FEATURE_DASHBOARD)); + setTaskEnabled(TASK_CMS, featureIsEnabled(FEATURE_OSD) || featureIsEnabled(FEATURE_DASHBOARD)); #endif #endif + #ifdef USE_VTX_CONTROL #if defined(USE_VTX_RTC6705) || defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) setTaskEnabled(TASK_VTXCTRL, true); #endif #endif + #ifdef USE_CAMERA_CONTROL setTaskEnabled(TASK_CAMCTRL, true); #endif + #ifdef USE_RCDEVICE setTaskEnabled(TASK_RCDEVICE, rcdeviceIsEnabled()); #endif -#endif } cfTask_t cfTasks[TASK_COUNT] = { [TASK_SYSTEM] = { .taskName = "SYSTEM", - .taskFunc = taskSystem, + .subTaskName = "LOAD", + .taskFunc = taskSystemLoad, .desiredPeriod = TASK_PERIOD_HZ(10), // 10Hz, every 100 ms .staticPriority = TASK_PRIORITY_MEDIUM_HIGH, }, + [TASK_MAIN] = { + .taskName = "SYSTEM", + .subTaskName = "UPDATE", + .taskFunc = taskMain, + .desiredPeriod = TASK_PERIOD_HZ(1000), + .staticPriority = TASK_PRIORITY_MEDIUM_HIGH, + }, + [TASK_SERIAL] = { .taskName = "SERIAL", .taskFunc = taskHandleSerial, -#ifdef USE_OSD_SLAVE - .checkFunc = taskSerialCheck, - .desiredPeriod = TASK_PERIOD_HZ(100), - .staticPriority = TASK_PRIORITY_REALTIME, -#else .desiredPeriod = TASK_PERIOD_HZ(100), // 100 Hz should be enough to flush up to 115 bytes @ 115200 baud .staticPriority = TASK_PRIORITY_LOW, -#endif }, [TASK_BATTERY_ALERTS] = { @@ -395,17 +408,6 @@ cfTask_t cfTasks[TASK_COUNT] = { }, #endif -#ifdef USE_OSD_SLAVE - [TASK_OSD_SLAVE] = { - .taskName = "OSD_SLAVE", - .checkFunc = osdSlaveCheck, - .taskFunc = osdSlaveUpdate, - .desiredPeriod = TASK_PERIOD_HZ(60), // 60 Hz - .staticPriority = TASK_PRIORITY_HIGH, - }, - -#else - [TASK_GYROPID] = { .taskName = "PID", .subTaskName = "GYRO", @@ -432,7 +434,7 @@ cfTask_t cfTasks[TASK_COUNT] = { .taskName = "RX", .checkFunc = rxUpdateCheck, .taskFunc = taskUpdateRxMain, - .desiredPeriod = TASK_PERIOD_HZ(50), // If event-based scheduling doesn't work, fallback to periodic scheduling + .desiredPeriod = TASK_PERIOD_HZ(33), // If event-based scheduling doesn't work, fallback to periodic scheduling .staticPriority = TASK_PRIORITY_HIGH, }, @@ -564,7 +566,7 @@ cfTask_t cfTasks[TASK_COUNT] = { [TASK_RCDEVICE] = { .taskName = "RCDEVICE", .taskFunc = rcdeviceUpdate, - .desiredPeriod = TASK_PERIOD_HZ(10), // 10 Hz, 100ms + .desiredPeriod = TASK_PERIOD_HZ(20), .staticPriority = TASK_PRIORITY_MEDIUM, }, #endif @@ -595,5 +597,4 @@ cfTask_t cfTasks[TASK_COUNT] = { .staticPriority = TASK_PRIORITY_IDLE }, #endif -#endif }; diff --git a/src/main/fc/fc_tasks.h b/src/main/fc/tasks.h similarity index 100% rename from src/main/fc/fc_tasks.h rename to src/main/fc/tasks.h diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 32ff5e01b6..c59a64f30f 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -34,7 +34,7 @@ #include "drivers/time.h" #include "fc/config.h" -#include "fc/fc_core.h" +#include "fc/core.h" #include "fc/rc_controls.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" @@ -221,7 +221,7 @@ void failsafeUpdateState(void) failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_1_SECONDS; // require 1 seconds of valid rxData reprocessState = true; } else if (!receivingRxData) { - if (millis() > failsafeState.throttleLowPeriod) { + if (millis() > failsafeState.throttleLowPeriod && failsafeConfig()->failsafe_procedure != FAILSAFE_PROCEDURE_GPS_RESCUE) { // JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds failsafeActivate(); failsafeState.phase = FAILSAFE_LANDED; // skip auto-landing procedure diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index 5d6fa1e037..d44c4a945f 100644 --- a/src/main/flight/gps_rescue.c +++ b/src/main/flight/gps_rescue.c @@ -32,8 +32,9 @@ #include "io/gps.h" #include "fc/config.h" -#include "fc/fc_core.h" +#include "fc/core.h" #include "fc/rc_controls.h" +#include "fc/rc_modes.h" #include "fc/runtime_config.h" #include "flight/failsafe.h" @@ -50,12 +51,74 @@ #include "gps_rescue.h" -PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 0); +typedef enum { + RESCUE_SANITY_OFF = 0, + RESCUE_SANITY_ON, + RESCUE_SANITY_FS_ONLY +} gpsRescueSanity_e; + +typedef enum { + RESCUE_IDLE, + RESCUE_INITIALIZE, + RESCUE_ATTAIN_ALT, + RESCUE_CROSSTRACK, + RESCUE_LANDING_APPROACH, + RESCUE_LANDING, + RESCUE_ABORT, + RESCUE_COMPLETE +} rescuePhase_e; + +typedef enum { + RESCUE_HEALTHY, + RESCUE_FLYAWAY, + RESCUE_CRASH_DETECTED, + RESCUE_TOO_CLOSE +} rescueFailureState_e; + +typedef struct { + int32_t targetAltitudeCm; + int32_t targetGroundspeed; + uint8_t minAngleDeg; + uint8_t maxAngleDeg; + bool crosstrack; +} rescueIntent_s; + +typedef struct { + int32_t maxAltitudeCm; + int32_t currentAltitudeCm; + uint16_t distanceToHomeM; + uint16_t maxDistanceToHomeM; + int16_t directionToHome; + uint16_t groundSpeed; + uint8_t numSat; + float zVelocity; // Up/down movement in cm/s + float zVelocityAvg; // Up/down average in cm/s + float accMagnitude; + float accMagnitudeAvg; +} rescueSensorData_s; + +typedef struct { + bool bumpDetection; + bool convergenceDetection; +} rescueSanityFlags; + +typedef struct { + rescuePhase_e phase; + rescueFailureState_e failure; + rescueSensorData_s sensor; + rescueIntent_s intent; + bool isFailsafe; +} rescueState_s; + +#define GPS_RESCUE_MAX_YAW_RATE 360 // deg/sec max yaw rate +#define GPS_RESCUE_RATE_SCALE_DEGREES 45 // Scale the commanded yaw rate when the error is less then this angle + +PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 1); PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, .angle = 32, - .initialAltitude = 50, - .descentDistance = 200, + .initialAltitudeM = 50, + .descentDistanceM = 200, .rescueGroundspeed = 2000, .throttleP = 150, .throttleI = 20, @@ -67,10 +130,14 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, .throttleMin = 1200, .throttleMax = 1600, .throttleHover = 1280, - .sanityChecks = 0, - .minSats = 8 + .sanityChecks = RESCUE_SANITY_ON, + .minSats = 8, + .minRescueDth = 100 ); +static uint16_t rescueThrottle; +static float rescueYaw; + int32_t gpsRescueAngle[ANGLE_INDEX_COUNT] = { 0, 0 }; uint16_t hoverThrottle = 0; float averageThrottle = 0.0; @@ -92,140 +159,143 @@ void rescueNewGpsData(void) newGPSData = true; } -/* - Determine what phase we are in, determine if all criteria are met to move to the next phase -*/ -void updateGPSRescueState(void) +static void rescueStart() { - if (!FLIGHT_MODE(GPS_RESCUE_MODE)) { - rescueStop(); - } else if (FLIGHT_MODE(GPS_RESCUE_MODE) && rescueState.phase == RESCUE_IDLE) { - rescueStart(); - } - - rescueState.isFailsafe = failsafeIsActive(); - - sensorUpdate(); - - switch (rescueState.phase) { - case RESCUE_IDLE: - idleTasks(); - break; - case RESCUE_INITIALIZE: - if (hoverThrottle == 0) { //no actual throttle data yet, let's use the default. - hoverThrottle = gpsRescueConfig()->throttleHover; - } - - rescueState.phase = RESCUE_ATTAIN_ALT; - FALLTHROUGH; - case RESCUE_ATTAIN_ALT: - // Get to a safe altitude at a low velocity ASAP - if (ABS(rescueState.intent.targetAltitude - rescueState.sensor.currentAltitude) < 1000) { - rescueState.phase = RESCUE_CROSSTRACK; - } - - rescueState.intent.targetGroundspeed = 500; - rescueState.intent.targetAltitude = MAX(gpsRescueConfig()->initialAltitude * 100, rescueState.sensor.maxAltitude + 1500); - rescueState.intent.crosstrack = true; - rescueState.intent.minAngleDeg = 10; - rescueState.intent.maxAngleDeg = 15; - break; - case RESCUE_CROSSTRACK: - if (rescueState.sensor.distanceToHome < gpsRescueConfig()->descentDistance) { - rescueState.phase = RESCUE_LANDING_APPROACH; - } - - // We can assume at this point that we are at or above our RTH height, so we need to try and point to home and tilt while maintaining alt - // Is our altitude way off? We should probably kick back to phase RESCUE_ATTAIN_ALT - rescueState.intent.targetGroundspeed = gpsRescueConfig()->rescueGroundspeed; - rescueState.intent.targetAltitude = MAX(gpsRescueConfig()->initialAltitude * 100, rescueState.sensor.maxAltitude + 1500); - rescueState.intent.crosstrack = true; - rescueState.intent.minAngleDeg = 15; - rescueState.intent.maxAngleDeg = gpsRescueConfig()->angle; - break; - case RESCUE_LANDING_APPROACH: - // We are getting close to home in the XY plane, get Z where it needs to be to move to landing phase - if (rescueState.sensor.distanceToHome < 10 && rescueState.sensor.currentAltitude <= 1000) { - rescueState.phase = RESCUE_LANDING; - } - - // Only allow new altitude and new speed to be equal or lower than the current values (to prevent parabolic movement on overshoot) - int32_t newAlt = gpsRescueConfig()->initialAltitude * 100 * rescueState.sensor.distanceToHome / gpsRescueConfig()->descentDistance; - int32_t newSpeed = gpsRescueConfig()->rescueGroundspeed * rescueState.sensor.distanceToHome / gpsRescueConfig()->descentDistance; - - rescueState.intent.targetAltitude = constrain(newAlt, 100, rescueState.intent.targetAltitude); - rescueState.intent.targetGroundspeed = constrain(newSpeed, 100, rescueState.intent.targetGroundspeed); - rescueState.intent.crosstrack = true; - rescueState.intent.minAngleDeg = 10; - rescueState.intent.maxAngleDeg = 20; - break; - case RESCUE_LANDING: - // We have reached the XYZ envelope to be considered at "home". We need to land gently and check our accelerometer for abnormal data. - // At this point, do not let the target altitude go up anymore, so if we overshoot, we dont' move in a parabolic trajectory - - // If we are over 120% of average magnitude, just disarm since we're pretty much home - if (rescueState.sensor.accMagnitude > rescueState.sensor.accMagnitudeAvg * 1.5) { - disarm(); - rescueState.phase = RESCUE_COMPLETE; - } - - rescueState.intent.targetGroundspeed = 0; - rescueState.intent.targetAltitude = 0; - rescueState.intent.crosstrack = true; - rescueState.intent.minAngleDeg = 0; - rescueState.intent.maxAngleDeg = 15; - break; - case RESCUE_COMPLETE: - rescueStop(); - break; - case RESCUE_ABORT: - disarm(); - rescueStop(); - break; - } - - performSanityChecks(); - - if (rescueState.phase != RESCUE_IDLE) { - rescueAttainPosition(); - } - - newGPSData = false; + rescueState.phase = RESCUE_INITIALIZE; } -void sensorUpdate() +static void rescueStop() { - rescueState.sensor.currentAltitude = getEstimatedAltitude(); - - // Calculate altitude velocity - static uint32_t previousTimeUs; - static int32_t previousAltitude; - - const uint32_t currentTimeUs = micros(); - const float dTime = currentTimeUs - previousTimeUs; - - if (newGPSData) { // Calculate velocity at lowest common denominator - rescueState.sensor.distanceToHome = GPS_distanceToHome; - rescueState.sensor.directionToHome = GPS_directionToHome; - rescueState.sensor.numSat = gpsSol.numSat; - rescueState.sensor.groundSpeed = gpsSol.groundSpeed; - - rescueState.sensor.zVelocity = (rescueState.sensor.currentAltitude - previousAltitude) * 1000000.0f / dTime; - rescueState.sensor.zVelocityAvg = 0.8f * rescueState.sensor.zVelocityAvg + rescueState.sensor.zVelocity * 0.2f; - - previousAltitude = rescueState.sensor.currentAltitude; - previousTimeUs = currentTimeUs; - } - - rescueState.sensor.accMagnitude = (float) sqrt(sq(acc.accADC[Z]) + sq(acc.accADC[X]) + sq(acc.accADC[Y]) / sq(acc.dev.acc_1G)); - rescueState.sensor.accMagnitudeAvg = (rescueState.sensor.accMagnitudeAvg * 0.998f) + (rescueState.sensor.accMagnitude * 0.002f); + rescueState.phase = RESCUE_IDLE; } -void performSanityChecks() +// Things that need to run regardless of GPS rescue mode being enabled or not +static void idleTasks() +{ + // Do not calculate any of the idle task values when we are not flying + if (!ARMING_FLAG(ARMED)) { + return; + } + + // Don't update any rescue flight statistics if we haven't applied a proper altitude offset yet + if (!isAltitudeOffset()) { + return; + } + + gpsRescueAngle[AI_PITCH] = 0; + gpsRescueAngle[AI_ROLL] = 0; + + // Store the max altitude we see not during RTH so we know our fly-back minimum alt + rescueState.sensor.maxAltitudeCm = MAX(rescueState.sensor.currentAltitudeCm, rescueState.sensor.maxAltitudeCm); + // Store the max distance to home during normal flight so we know if a flyaway is happening + rescueState.sensor.maxDistanceToHomeM = MAX(rescueState.sensor.distanceToHomeM, rescueState.sensor.maxDistanceToHomeM); + + rescueThrottle = rcCommand[THROTTLE]; + + //to do: have a default value for hoverThrottle + + // FIXME: GPS Rescue throttle handling should take into account min_check as the + // active throttle is from min_check through PWM_RANGE_MAX. Currently adjusting for this + // in gpsRescueGetThrottle() but it would be better handled here. + + float ct = getCosTiltAngle(); + if (ct > 0.5 && ct < 0.96 && throttleSamples < 1E6 && rescueThrottle > 1070) { //5 to 45 degrees tilt + //TO DO: only sample when acceleration is low + uint16_t adjustedThrottle = 1000 + (rescueThrottle - PWM_RANGE_MIN) * ct; + if (throttleSamples == 0) { + averageThrottle = adjustedThrottle; + } else { + averageThrottle += (adjustedThrottle - averageThrottle) / (throttleSamples + 1); + } + hoverThrottle = lrintf(averageThrottle); + throttleSamples++; + } +} + +// Very similar to maghold function on betaflight/cleanflight +static void setBearing(int16_t desiredHeading) +{ + float errorAngle = (attitude.values.yaw / 10.0f) - desiredHeading; + + // Determine the most efficient direction to rotate + if (errorAngle <= -180) { + errorAngle += 360; + } else if (errorAngle > 180) { + errorAngle -= 360; + } + + errorAngle *= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed); + + // Calculate a desired yaw rate based on a maximum limit beyond + // an error window and then scale the requested rate down inside + // the window as error approaches 0. + rescueYaw = -constrainf(errorAngle / GPS_RESCUE_RATE_SCALE_DEGREES * GPS_RESCUE_MAX_YAW_RATE, -GPS_RESCUE_MAX_YAW_RATE, GPS_RESCUE_MAX_YAW_RATE); +} + +static void rescueAttainPosition() +{ + // Point to home if that is in our intent + if (rescueState.intent.crosstrack) { + setBearing(rescueState.sensor.directionToHome); + } + + if (!newGPSData) { + return; + } + + /** + Speed controller + */ + static float previousSpeedError = 0; + static int16_t speedIntegral = 0; + + const int16_t speedError = (rescueState.intent.targetGroundspeed - rescueState.sensor.groundSpeed) / 100; + const int16_t speedDerivative = speedError - previousSpeedError; + + speedIntegral = constrain(speedIntegral + speedError, -100, 100); + + previousSpeedError = speedError; + + int16_t angleAdjustment = gpsRescueConfig()->velP * speedError + (gpsRescueConfig()->velI * speedIntegral) / 100 + gpsRescueConfig()->velD * speedDerivative; + + gpsRescueAngle[AI_PITCH] = constrain(gpsRescueAngle[AI_PITCH] + MIN(angleAdjustment, 80), rescueState.intent.minAngleDeg * 100, rescueState.intent.maxAngleDeg * 100); + + float ct = cos(DECIDEGREES_TO_RADIANS(gpsRescueAngle[AI_PITCH] / 10)); + + /** + Altitude controller + */ + static float previousAltitudeError = 0; + static int16_t altitudeIntegral = 0; + + const int16_t altitudeError = (rescueState.intent.targetAltitudeCm - rescueState.sensor.currentAltitudeCm) / 100; // Error in meters + const int16_t altitudeDerivative = altitudeError - previousAltitudeError; + + // Only allow integral windup within +-15m absolute altitude error + if (ABS(altitudeError) < 25) { + altitudeIntegral = constrain(altitudeIntegral + altitudeError, -250, 250); + } else { + altitudeIntegral = 0; + } + + previousAltitudeError = altitudeError; + + int16_t altitudeAdjustment = (gpsRescueConfig()->throttleP * altitudeError + (gpsRescueConfig()->throttleI * altitudeIntegral) / 10 * + gpsRescueConfig()->throttleD * altitudeDerivative) / ct / 20; + int16_t hoverAdjustment = (hoverThrottle - 1000) / ct; + + rescueThrottle = constrain(1000 + altitudeAdjustment + hoverAdjustment, gpsRescueConfig()->throttleMin, gpsRescueConfig()->throttleMax); + + DEBUG_SET(DEBUG_RTH, 0, rescueThrottle); + DEBUG_SET(DEBUG_RTH, 1, gpsRescueAngle[AI_PITCH]); + DEBUG_SET(DEBUG_RTH, 2, altitudeAdjustment); + DEBUG_SET(DEBUG_RTH, 3, rescueState.failure); +} + +static void performSanityChecks() { if (rescueState.phase == RESCUE_IDLE) { rescueState.failure = RESCUE_HEALTHY; - + return; } @@ -273,131 +343,173 @@ void performSanityChecks() if (msI == 5) { rescueState.failure = RESCUE_FLYAWAY; } +} - // Minimum distance detection (100m) - if (rescueState.sensor.distanceToHome < 100) { - rescueState.failure = RESCUE_TOO_CLOSE; +static void sensorUpdate() +{ + rescueState.sensor.currentAltitudeCm = getEstimatedAltitudeCm(); + + // Calculate altitude velocity + static uint32_t previousTimeUs; + static int32_t previousAltitudeCm; + + const uint32_t currentTimeUs = micros(); + const float dTime = currentTimeUs - previousTimeUs; + + if (newGPSData) { // Calculate velocity at lowest common denominator + rescueState.sensor.distanceToHomeM = GPS_distanceToHome; + rescueState.sensor.directionToHome = GPS_directionToHome; + rescueState.sensor.numSat = gpsSol.numSat; + rescueState.sensor.groundSpeed = gpsSol.groundSpeed; + + rescueState.sensor.zVelocity = (rescueState.sensor.currentAltitudeCm - previousAltitudeCm) * 1000000.0f / dTime; + rescueState.sensor.zVelocityAvg = 0.8f * rescueState.sensor.zVelocityAvg + rescueState.sensor.zVelocity * 0.2f; + + rescueState.sensor.accMagnitude = (float) sqrtf(sq(acc.accADC[Z]) + sq(acc.accADC[X]) + sq(acc.accADC[Y])) * acc.dev.acc_1G_rec; + rescueState.sensor.accMagnitudeAvg = (rescueState.sensor.accMagnitudeAvg * 0.8f) + (rescueState.sensor.accMagnitude * 0.2f); + + previousAltitudeCm = rescueState.sensor.currentAltitudeCm; + previousTimeUs = currentTimeUs; } } -void rescueStart() +/* + Determine what phase we are in, determine if all criteria are met to move to the next phase +*/ +void updateGPSRescueState(void) { - rescueState.phase = RESCUE_INITIALIZE; -} - -void rescueStop() -{ - rescueState.phase = RESCUE_IDLE; -} - -// Things that need to run regardless of GPS rescue mode being enabled or not -void idleTasks() -{ - // Do not calculate any of the idle task values when we are not flying - if (!ARMING_FLAG(ARMED)) { - return; + if (!FLIGHT_MODE(GPS_RESCUE_MODE)) { + rescueStop(); + } else if (FLIGHT_MODE(GPS_RESCUE_MODE) && rescueState.phase == RESCUE_IDLE) { + rescueStart(); } - gpsRescueAngle[AI_PITCH] = 0; - gpsRescueAngle[AI_ROLL] = 0; + rescueState.isFailsafe = failsafeIsActive(); - // Store the max altitude we see not during RTH so we know our fly-back minimum alt - rescueState.sensor.maxAltitude = MAX(rescueState.sensor.currentAltitude, rescueState.sensor.maxAltitude); - // Store the max distance to home during normal flight so we know if a flyaway is happening - rescueState.sensor.maxDistanceToHome = MAX(rescueState.sensor.distanceToHome, rescueState.sensor.maxDistanceToHome); + sensorUpdate(); - rescueThrottle = rcCommand[THROTTLE]; - - //to do: have a default value for hoverThrottle - float ct = getCosTiltAngle(); - if (ct > 0.5 && ct < 0.96 && throttleSamples < 1E6 && rescueThrottle > 1070) { //5 to 45 degrees tilt - //TO DO: only sample when acceleration is low - uint16_t adjustedThrottle = 1000 + (rescueThrottle - PWM_RANGE_MIN) * ct; - if (throttleSamples == 0) { - averageThrottle = adjustedThrottle; - } else { - averageThrottle += (adjustedThrottle - averageThrottle) / (throttleSamples + 1); + switch (rescueState.phase) { + case RESCUE_IDLE: + idleTasks(); + break; + case RESCUE_INITIALIZE: + if (hoverThrottle == 0) { //no actual throttle data yet, let's use the default. + hoverThrottle = gpsRescueConfig()->throttleHover; } - hoverThrottle = lrintf(averageThrottle); - throttleSamples++; + + // Minimum distance detection. Disarm regardless of sanity check configuration. Rescue too close is never a good idea. + if (rescueState.sensor.distanceToHomeM < gpsRescueConfig()->minRescueDth) { + // Never allow rescue mode to engage as a failsafe when too close or when disarmed. + if (rescueState.isFailsafe || !ARMING_FLAG(ARMED)) { + rescueState.failure = RESCUE_TOO_CLOSE; + setArmingDisabled(ARMING_DISABLED_ARM_SWITCH); + disarm(); + } else { + // Leave it up to the sanity check setting + rescueState.failure = RESCUE_TOO_CLOSE; + } + } + + rescueState.phase = RESCUE_ATTAIN_ALT; + FALLTHROUGH; + case RESCUE_ATTAIN_ALT: + // Get to a safe altitude at a low velocity ASAP + if (ABS(rescueState.intent.targetAltitudeCm - rescueState.sensor.currentAltitudeCm) < 1000) { + rescueState.phase = RESCUE_CROSSTRACK; + } + + rescueState.intent.targetGroundspeed = 500; + rescueState.intent.targetAltitudeCm = MAX(gpsRescueConfig()->initialAltitudeM * 100, rescueState.sensor.maxAltitudeCm + 1500); + rescueState.intent.crosstrack = true; + rescueState.intent.minAngleDeg = 10; + rescueState.intent.maxAngleDeg = 15; + break; + case RESCUE_CROSSTRACK: + if (rescueState.sensor.distanceToHomeM < gpsRescueConfig()->descentDistanceM) { + rescueState.phase = RESCUE_LANDING_APPROACH; + } + + // We can assume at this point that we are at or above our RTH height, so we need to try and point to home and tilt while maintaining alt + // Is our altitude way off? We should probably kick back to phase RESCUE_ATTAIN_ALT + rescueState.intent.targetGroundspeed = gpsRescueConfig()->rescueGroundspeed; + rescueState.intent.targetAltitudeCm = MAX(gpsRescueConfig()->initialAltitudeM * 100, rescueState.sensor.maxAltitudeCm + 1500); + rescueState.intent.crosstrack = true; + rescueState.intent.minAngleDeg = 15; + rescueState.intent.maxAngleDeg = gpsRescueConfig()->angle; + break; + case RESCUE_LANDING_APPROACH: + // We are getting close to home in the XY plane, get Z where it needs to be to move to landing phase + if (rescueState.sensor.distanceToHomeM < 10 && rescueState.sensor.currentAltitudeCm <= 1000) { + rescueState.phase = RESCUE_LANDING; + } + + // Only allow new altitude and new speed to be equal or lower than the current values (to prevent parabolic movement on overshoot) + int32_t newAlt = gpsRescueConfig()->initialAltitudeM * 100 * rescueState.sensor.distanceToHomeM / gpsRescueConfig()->descentDistanceM; + int32_t newSpeed = gpsRescueConfig()->rescueGroundspeed * rescueState.sensor.distanceToHomeM / gpsRescueConfig()->descentDistanceM; + + rescueState.intent.targetAltitudeCm = constrain(newAlt, 100, rescueState.intent.targetAltitudeCm); + rescueState.intent.targetGroundspeed = constrain(newSpeed, 100, rescueState.intent.targetGroundspeed); + rescueState.intent.crosstrack = true; + rescueState.intent.minAngleDeg = 10; + rescueState.intent.maxAngleDeg = 20; + break; + case RESCUE_LANDING: + // We have reached the XYZ envelope to be considered at "home". We need to land gently and check our accelerometer for abnormal data. + // At this point, do not let the target altitude go up anymore, so if we overshoot, we dont' move in a parabolic trajectory + + // If we are over 120% of average magnitude, just disarm since we're pretty much home + if (rescueState.sensor.accMagnitude > rescueState.sensor.accMagnitudeAvg * 1.5) { + setArmingDisabled(ARMING_DISABLED_ARM_SWITCH); + disarm(); + rescueState.phase = RESCUE_COMPLETE; + } + + rescueState.intent.targetGroundspeed = 0; + rescueState.intent.targetAltitudeCm = 0; + rescueState.intent.crosstrack = true; + rescueState.intent.minAngleDeg = 0; + rescueState.intent.maxAngleDeg = 15; + break; + case RESCUE_COMPLETE: + rescueStop(); + break; + case RESCUE_ABORT: + setArmingDisabled(ARMING_DISABLED_ARM_SWITCH); + disarm(); + rescueStop(); + break; + default: + break; } + performSanityChecks(); + + if (rescueState.phase != RESCUE_IDLE) { + rescueAttainPosition(); + } + + newGPSData = false; } -void rescueAttainPosition() +float gpsRescueGetYawRate(void) { - // Point to home if that is in our intent - if (rescueState.intent.crosstrack) { - setBearing(rescueState.sensor.directionToHome); - } - - if (!newGPSData) { - return; - } + return rescueYaw; +} +float gpsRescueGetThrottle(void) +{ + // Calculated a desired commanded throttle scaled from 0.0 to 1.0 for use in the mixer. + // We need to compensate for min_check since the throttle value set by gps rescue + // is based on the raw rcCommand value commanded by the pilot. + float commandedThrottle = scaleRangef(rescueThrottle, MAX(rxConfig()->mincheck, PWM_RANGE_MIN), PWM_RANGE_MAX, 0.0f, 1.0f); + commandedThrottle = constrainf(commandedThrottle, 0.0f, 1.0f); - /** - Speed controller - */ - static float previousSpeedError = 0; - static int16_t speedIntegral = 0; - - const int16_t speedError = (rescueState.intent.targetGroundspeed - rescueState.sensor.groundSpeed) / 100; - const int16_t speedDerivative = speedError - previousSpeedError; - - speedIntegral = constrain(speedIntegral + speedError, -100, 100); - - previousSpeedError = speedError; - - int16_t angleAdjustment = gpsRescueConfig()->velP * speedError + (gpsRescueConfig()->velI * speedIntegral) / 100 + gpsRescueConfig()->velD * speedDerivative; - - gpsRescueAngle[AI_PITCH] = constrain(gpsRescueAngle[AI_PITCH] + MIN(angleAdjustment, 80), rescueState.intent.minAngleDeg * 100, rescueState.intent.maxAngleDeg * 100); - - float ct = cos(DECIDEGREES_TO_RADIANS(gpsRescueAngle[AI_PITCH] / 10)); - - /** - Altitude controller - */ - static float previousAltitudeError = 0; - static int16_t altitudeIntegral = 0; - - const int16_t altitudeError = (rescueState.intent.targetAltitude - rescueState.sensor.currentAltitude) / 100; // Error in meters - const int16_t altitudeDerivative = altitudeError - previousAltitudeError; - - // Only allow integral windup within +-15m absolute altitude error - if (ABS(altitudeError) < 25) { - altitudeIntegral = constrain(altitudeIntegral + altitudeError, -250, 250); - } else { - altitudeIntegral = 0; - } - - previousAltitudeError = altitudeError; - - int16_t altitudeAdjustment = (gpsRescueConfig()->throttleP * altitudeError + (gpsRescueConfig()->throttleI * altitudeIntegral) / 10 * + gpsRescueConfig()->throttleD * altitudeDerivative) / ct / 20; - int16_t hoverAdjustment = (hoverThrottle - 1000) / ct; - - rescueThrottle = constrain(1000 + altitudeAdjustment + hoverAdjustment, gpsRescueConfig()->throttleMin, gpsRescueConfig()->throttleMax); - - DEBUG_SET(DEBUG_RTH, 0, rescueThrottle); - DEBUG_SET(DEBUG_RTH, 1, gpsRescueAngle[AI_PITCH]); - DEBUG_SET(DEBUG_RTH, 2, altitudeAdjustment); - DEBUG_SET(DEBUG_RTH, 3, rescueState.failure); + return commandedThrottle; } -// Very similar to maghold function on betaflight/cleanflight -void setBearing(int16_t deg) +bool gpsRescueIsConfigured(void) { - int16_t dif = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - deg; - - if (dif <= -180) - dif += 360; - if (dif >= +180) - dif -= 360; - - dif *= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed); - - rcCommand[YAW] = - (dif * gpsRescueConfig()->yawP / 20); + return failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_GPS_RESCUE || isModeActivationConditionPresent(BOXGPSRESCUE); } - #endif diff --git a/src/main/flight/gps_rescue.h b/src/main/flight/gps_rescue.h index 29128b67fe..d77de91da1 100644 --- a/src/main/flight/gps_rescue.h +++ b/src/main/flight/gps_rescue.h @@ -19,16 +19,10 @@ #include "pg/pg.h" -typedef enum { - RESCUE_SANITY_OFF = 0, - RESCUE_SANITY_ON, - RESCUE_SANITY_FS_ONLY -} gpsRescueSanity_e; - typedef struct gpsRescue_s { uint16_t angle; //degrees - uint16_t initialAltitude; //meters - uint16_t descentDistance; //meters + uint16_t initialAltitudeM; //meters + uint16_t descentDistanceM; //meters uint16_t rescueGroundspeed; // centimeters per second uint16_t throttleP, throttleI, throttleD; uint16_t yawP; @@ -37,76 +31,17 @@ typedef struct gpsRescue_s { uint16_t throttleHover; uint16_t velP, velI, velD; uint8_t minSats; - gpsRescueSanity_e sanityChecks; + uint16_t minRescueDth; //meters + uint8_t sanityChecks; } gpsRescueConfig_t; PG_DECLARE(gpsRescueConfig_t, gpsRescueConfig); -uint16_t rescueThrottle; - -typedef enum { - RESCUE_IDLE, - RESCUE_INITIALIZE, - RESCUE_ATTAIN_ALT, - RESCUE_CROSSTRACK, - RESCUE_LANDING_APPROACH, - RESCUE_LANDING, - RESCUE_ABORT, - RESCUE_COMPLETE -} rescuePhase_e; - -typedef enum { - RESCUE_HEALTHY, - RESCUE_FLYAWAY, - RESCUE_CRASH_DETECTED, - RESCUE_TOO_CLOSE -} rescueFailureState_e; - -typedef struct { - int32_t targetAltitude; - int32_t targetGroundspeed; - uint8_t minAngleDeg; - uint8_t maxAngleDeg; - bool crosstrack; -} rescueIntent_s; - -typedef struct { - int32_t maxAltitude; - int32_t currentAltitude; - uint16_t distanceToHome; - uint16_t maxDistanceToHome; - int16_t directionToHome; - uint16_t groundSpeed; - uint8_t numSat; - float zVelocity; // Up/down movement in cm/s - float zVelocityAvg; // Up/down average in cm/s - float accMagnitude; - float accMagnitudeAvg; -} rescueSensorData_s; - -typedef struct { - bool bumpDetection; - bool convergenceDetection; -} rescueSanityFlags; - -typedef struct { - rescuePhase_e phase; - rescueFailureState_e failure; - rescueSensorData_s sensor; - rescueIntent_s intent; - bool isFailsafe; -} rescueState_s; - extern int32_t gpsRescueAngle[ANGLE_INDEX_COUNT]; //NOTE: ANGLES ARE IN CENTIDEGREES -extern rescueState_s rescueState; void updateGPSRescueState(void); void rescueNewGpsData(void); -void idleTasks(void); -void rescueStop(void); -void rescueStart(void); -void setBearing(int16_t deg); -void performSanityChecks(void); -void sensorUpdate(void); -void rescueAttainPosition(void); +float gpsRescueGetYawRate(void); +float gpsRescueGetThrottle(void); +bool gpsRescueIsConfigured(void); diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 8a579ec45d..98291b5deb 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -85,6 +85,7 @@ static bool imuUpdated = false; #define GPS_COG_MIN_GROUNDSPEED 500 // 500cm/s minimum groundspeed for a gps heading to be considered valid int32_t accSum[XYZ_AXIS_COUNT]; +float accAverage[XYZ_AXIS_COUNT]; uint32_t accTimeSum = 0; // keep track for integration of acc int accSumCount = 0; @@ -92,6 +93,7 @@ float accVelScale; bool canUseGPSHeading = true; static float throttleAngleScale; +static int throttleAngleValue; static float fc_acc; static float smallAngleCosZ = 0; @@ -109,14 +111,12 @@ quaternion offset = QUATERNION_INITIALIZE; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800 attitudeEulerAngles_t attitude = EULER_INITIALIZE; -PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 0); +PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 1); PG_RESET_TEMPLATE(imuConfig_t, imuConfig, .dcm_kp = 2500, // 1.0 * 10000 .dcm_ki = 0, // 0.003 * 10000 .small_angle = 25, - .accDeadband = {.xy = 40, .z= 40}, - .acc_unarmedcal = 1 ); STATIC_UNIT_TESTED void imuComputeRotationMatrix(void){ @@ -134,7 +134,7 @@ STATIC_UNIT_TESTED void imuComputeRotationMatrix(void){ rMat[2][1] = 2.0f * (qP.yz - -qP.wx); rMat[2][2] = 1.0f - 2.0f * qP.xx - 2.0f * qP.yy; -#if defined(SIMULATOR_BUILD) && defined(SKIP_IMU_CALC) && !defined(SET_IMU_FROM_EULER) +#if defined(SIMULATOR_BUILD) && !defined(USE_IMU_CALC) && !defined(SET_IMU_FROM_EULER) rMat[1][0] = -2.0f * (qP.xy - -qP.wz); rMat[2][0] = -2.0f * (qP.xz + -qP.wy); #endif @@ -153,21 +153,22 @@ static float calculateThrottleAngleScale(uint16_t throttle_correction_angle) return (1800.0f / M_PIf) * (900.0f / throttle_correction_angle); } -void imuConfigure(uint16_t throttle_correction_angle) +void imuConfigure(uint16_t throttle_correction_angle, uint8_t throttle_correction_value) { imuRuntimeConfig.dcm_kp = imuConfig()->dcm_kp / 10000.0f; imuRuntimeConfig.dcm_ki = imuConfig()->dcm_ki / 10000.0f; - imuRuntimeConfig.acc_unarmedcal = imuConfig()->acc_unarmedcal; imuRuntimeConfig.small_angle = imuConfig()->small_angle; fc_acc = calculateAccZLowPassFilterRCTimeConstant(5.0f); // Set to fix value throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle); + + throttleAngleValue = throttle_correction_value; } void imuInit(void) { smallAngleCosZ = cos_approx(degreesToRadians(imuRuntimeConfig.small_angle)); - accVelScale = 9.80665f / acc.dev.acc_1G / 10000.0f; + accVelScale = 9.80665f * acc.dev.acc_1G_rec / 10000.0f; #ifdef USE_GPS canUseGPSHeading = true; @@ -351,16 +352,16 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void) static bool imuIsAccelerometerHealthy(float *accAverage) { - float accMagnitude = 0; + float accMagnitudeSq = 0; for (int axis = 0; axis < 3; axis++) { const float a = accAverage[axis]; - accMagnitude += a * a; + accMagnitudeSq += a * a; } - accMagnitude = accMagnitude * 100 / (sq((int32_t)acc.dev.acc_1G)); + accMagnitudeSq = accMagnitudeSq * sq(acc.dev.acc_1G_rec); - // Accept accel readings only in range 0.90g - 1.10g - return (81 < accMagnitude) && (accMagnitude < 121); + // Accept accel readings only in range 0.9g - 1.1g + return (0.81f < accMagnitudeSq) && (accMagnitudeSq < 1.21f); } // Calculate the dcmKpGain to use. When armed, the gain is imuRuntimeConfig.dcm_kp * 1.0 scaling. @@ -394,11 +395,11 @@ float imuCalcKpGain(timeUs_t currentTimeUs, bool useAcc, float *gyroAverage) if ((fabsf(gyroAverage[X]) > ATTITUDE_RESET_GYRO_LIMIT) || (fabsf(gyroAverage[Y]) > ATTITUDE_RESET_GYRO_LIMIT) || (fabsf(gyroAverage[Z]) > ATTITUDE_RESET_GYRO_LIMIT) - || (!useAcc)) { + || (!useAcc)) { gyroQuietPeriodTimeEnd = currentTimeUs + ATTITUDE_RESET_QUIET_TIME; attitudeResetTimeEnd = 0; - } + } } if (attitudeResetTimeEnd > 0) { // Resetting the attitude estimation if (currentTimeUs >= attitudeResetTimeEnd) { @@ -408,14 +409,14 @@ float imuCalcKpGain(timeUs_t currentTimeUs, bool useAcc, float *gyroAverage) } else { attitudeResetActive = true; } - } else if ((gyroQuietPeriodTimeEnd > 0) && (currentTimeUs >= gyroQuietPeriodTimeEnd)) { + } else if ((gyroQuietPeriodTimeEnd > 0) && (currentTimeUs >= gyroQuietPeriodTimeEnd)) { // Start the high gain period to bring the estimation into convergence attitudeResetTimeEnd = currentTimeUs + ATTITUDE_RESET_ACTIVE_TIME; gyroQuietPeriodTimeEnd = 0; } } lastArmState = armState; - + if (attitudeResetActive) { ret = ATTITUDE_RESET_KP_GAIN; } else { @@ -451,7 +452,7 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs) courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse); useCOG = true; } else { - // If GPS rescue mode is active and we can use it, go for it. When we're close to home we will + // If GPS rescue mode is active and we can use it, go for it. When we're close to home we will // probably stop re calculating GPS heading data. Other future modes can also use this extern if (canUseGPSHeading) { @@ -471,7 +472,7 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs) } #endif -#if defined(SIMULATOR_BUILD) && defined(SKIP_IMU_CALC) +#if defined(SIMULATOR_BUILD) && !defined(USE_IMU_CALC) UNUSED(imuMahonyAHRSupdate); UNUSED(imuIsAccelerometerHealthy); UNUSED(useAcc); @@ -488,7 +489,6 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs) #endif float gyroAverage[XYZ_AXIS_COUNT]; gyroGetAccumulationAverage(gyroAverage); - float accAverage[XYZ_AXIS_COUNT]; if (accGetAccumulationAverage(accAverage)) { useAcc = imuIsAccelerometerHealthy(accAverage); } @@ -503,6 +503,22 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs) #endif } +int calculateThrottleAngleCorrection(void) +{ + /* + * Use 0 as the throttle angle correction if we are inverted, vertical or with a + * small angle < 0.86 deg + * TODO: Define this small angle in config. + */ + if (rMat[2][2] <= 0.015f) { + return 0; + } + int angle = lrintf(acos_approx(rMat[2][2]) * throttleAngleScale); + if (angle > 900) + angle = 900; + return lrintf(throttleAngleValue * sin_approx(angle / (900.0f * M_PIf / 2.0f))); +} + void imuUpdateAttitude(timeUs_t currentTimeUs) { if (sensors(SENSOR_ACC) && acc.isAccelUpdatedAtLeastOnce) { @@ -516,6 +532,14 @@ void imuUpdateAttitude(timeUs_t currentTimeUs) #endif imuCalculateEstimatedAttitude(currentTimeUs); IMU_UNLOCK; + + // Update the throttle correction for angle and supply it to the mixer + int throttleAngleCorrection = 0; + if (throttleAngleValue && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && ARMING_FLAG(ARMED)) { + throttleAngleCorrection = calculateThrottleAngleCorrection(); + } + mixerSetThrottleAngleCorrection(throttleAngleCorrection); + } else { acc.accADC[X] = 0; acc.accADC[Y] = 0; @@ -532,7 +556,7 @@ bool shouldInitializeGPSHeading() return true; } - + return false; } @@ -549,22 +573,6 @@ void getQuaternion(quaternion *quat) quat->z = q.z; } -int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value) -{ - /* - * Use 0 as the throttle angle correction if we are inverted, vertical or with a - * small angle < 0.86 deg - * TODO: Define this small angle in config. - */ - if (rMat[2][2] <= 0.015f) { - return 0; - } - int angle = lrintf(acos_approx(rMat[2][2]) * throttleAngleScale); - if (angle > 900) - angle = 900; - return lrintf(throttle_correction_value * sin_approx(angle / (900.0f * M_PIf / 2.0f))); -} - void imuComputeQuaternionFromRPY(quaternionProducts *quatProd, int16_t initialRoll, int16_t initialPitch, int16_t initialYaw) { if (initialRoll > 1800) { diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 5bec324109..6d8472a5a1 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -31,6 +31,7 @@ extern int accSumCount; extern float accVelScale; extern int32_t accSum[XYZ_AXIS_COUNT]; extern bool canUseGPSHeading; +extern float accAverage[XYZ_AXIS_COUNT]; typedef struct { float w,x,y,z; @@ -55,17 +56,10 @@ typedef union { extern attitudeEulerAngles_t attitude; -typedef struct accDeadband_s { - uint8_t xy; // set the acc deadband for xy-Axis - uint8_t z; // set the acc deadband for z-Axis, this ignores small accelerations -} accDeadband_t; - typedef struct imuConfig_s { uint16_t dcm_kp; // DCM filter proportional gain ( x 10000) uint16_t dcm_ki; // DCM filter integral gain ( x 10000) uint8_t small_angle; - uint8_t acc_unarmedcal; // turn automatic acc compensation on/off - accDeadband_t accDeadband; } imuConfig_t; PG_DECLARE(imuConfig_t, imuConfig); @@ -73,17 +67,14 @@ PG_DECLARE(imuConfig_t, imuConfig); typedef struct imuRuntimeConfig_s { float dcm_ki; float dcm_kp; - uint8_t acc_unarmedcal; uint8_t small_angle; - accDeadband_t accDeadband; } imuRuntimeConfig_t; -void imuConfigure(uint16_t throttle_correction_angle); +void imuConfigure(uint16_t throttle_correction_angle, uint8_t throttle_correction_value); float getCosTiltAngle(void); void getQuaternion(quaternion * q); void imuUpdateAttitude(timeUs_t currentTimeUs); -int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value); void imuResetAccelerationSum(void); void imuInit(void); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 70b4d5841f..7e601f14f3 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -49,11 +49,12 @@ #include "fc/rc_controls.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" -#include "fc/fc_core.h" -#include "fc/fc_rc.h" +#include "fc/core.h" +#include "fc/rc.h" #include "flight/failsafe.h" #include "flight/imu.h" +#include "flight/gps_rescue.h" #include "flight/mixer.h" #include "flight/mixer_tricopter.h" #include "flight/pid.h" @@ -125,6 +126,8 @@ float motor_disarmed[MAX_SUPPORTED_MOTORS]; mixerMode_e currentMixerMode; static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; +static FAST_RAM_ZERO_INIT int throttleAngleCorrection; + static const motorMixer_t mixerQuadX[] = { { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R @@ -255,10 +258,10 @@ static const motorMixer_t mixerVtail4[] = { }; static const motorMixer_t mixerAtail4[] = { - { 1.0f, 0.0f, 1.0f, 1.0f }, // REAR_R - { 1.0f, -1.0f, -1.0f, 0.0f }, // FRONT_R - { 1.0f, 0.0f, 1.0f, -1.0f }, // REAR_L - { 1.0f, 1.0f, -1.0f, -0.0f }, // FRONT_L + { 1.0f, -0.58f, 0.58f, -1.0f }, // REAR_R + { 1.0f, -0.46f, -0.39f, 0.5f }, // FRONT_R + { 1.0f, 0.58f, 0.58f, 1.0f }, // REAR_L + { 1.0f, 0.46f, -0.39f, -0.5f }, // FRONT_L }; #if defined(USE_UNCOMMON_MIXERS) @@ -282,6 +285,8 @@ static const motorMixer_t mixerQuadX1234[] = { }; // Keep synced with mixerMode_e +// Some of these entries are bogus when servos (USE_SERVOS) are not configured, +// but left untouched to keep ordinals synced with mixerMode_e (and configurator). const mixer_t mixers[] = { // motors, use servo, motor mixer { 0, false, NULL }, // entry 0 @@ -317,9 +322,7 @@ const mixer_t mixers[] = { FAST_RAM_ZERO_INIT float motorOutputHigh, motorOutputLow; static FAST_RAM_ZERO_INIT float disarmMotorOutput, deadbandMotor3dHigh, deadbandMotor3dLow; -static FAST_RAM_ZERO_INIT uint16_t rcCommand3dDeadBandLow; -static FAST_RAM_ZERO_INIT uint16_t rcCommand3dDeadBandHigh; -static FAST_RAM_ZERO_INIT float rcCommandThrottleRange, rcCommandThrottleRange3dLow, rcCommandThrottleRange3dHigh; +static FAST_RAM_ZERO_INIT float rcCommandThrottleRange; uint8_t getMotorCount(void) { @@ -349,23 +352,12 @@ bool areMotorsRunning(void) return motorsRunning; } +#ifdef USE_SERVOS bool mixerIsTricopter(void) { -#ifdef USE_SERVOS return (currentMixerMode == MIXER_TRI || currentMixerMode == MIXER_CUSTOM_TRI); -#else - return false; +} #endif -} - -bool mixerIsOutputSaturated(int axis, float errorRate) -{ - if (axis == FD_YAW && mixerIsTricopter()) { - return mixerTricopterIsServoSaturated(errorRate); - } - - return motorMixRange >= 1.0f; -} // All PWM motor scaling is done to standard PWM range of 1000-2000 for easier tick conversion with legacy code / configurator // DSHOT scaling is done to the actual dshot range @@ -380,7 +372,7 @@ void initEscEndpoints(void) case PWM_TYPE_DSHOT300: case PWM_TYPE_DSHOT150: disarmMotorOutput = DSHOT_DISARM_COMMAND; - if (feature(FEATURE_3D)) { + if (featureIsEnabled(FEATURE_3D)) { motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_3D_DEADBAND_LOW - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue); } else { motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue); @@ -392,7 +384,7 @@ void initEscEndpoints(void) break; #endif default: - if (feature(FEATURE_3D)) { + if (featureIsEnabled(FEATURE_3D)) { disarmMotorOutput = flight3DConfig()->neutral3d; motorOutputLow = flight3DConfig()->limit3d_low; motorOutputHigh = flight3DConfig()->limit3d_high; @@ -407,12 +399,6 @@ void initEscEndpoints(void) } rcCommandThrottleRange = PWM_RANGE_MAX - rxConfig()->mincheck; - - rcCommand3dDeadBandLow = rxConfig()->midrc - flight3DConfig()->deadband3d_throttle; - rcCommand3dDeadBandHigh = rxConfig()->midrc + flight3DConfig()->deadband3d_throttle; - - rcCommandThrottleRange3dLow = rcCommand3dDeadBandLow - PWM_RANGE_MIN; - rcCommandThrottleRange3dHigh = PWM_RANGE_MAX - rcCommand3dDeadBandHigh; } void mixerInit(mixerMode_e mixerMode) @@ -420,9 +406,11 @@ void mixerInit(mixerMode_e mixerMode) currentMixerMode = mixerMode; initEscEndpoints(); +#ifdef USE_SERVOS if (mixerIsTricopter()) { mixerTricopterInit(); } +#endif } #ifndef USE_QUAD_MIXER_ONLY @@ -533,11 +521,29 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) static timeUs_t reversalTimeUs = 0; // time when motors last reversed in 3D mode float currentThrottleInputRange = 0; - if (feature(FEATURE_3D)) { + if (featureIsEnabled(FEATURE_3D)) { + uint16_t rcCommand3dDeadBandLow; + uint16_t rcCommand3dDeadBandHigh; + if (!ARMING_FLAG(ARMED)) { rcThrottlePrevious = rxConfig()->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming. } + if (IS_RC_MODE_ACTIVE(BOX3D) || flight3DConfig()->switched_mode3d) { + // The min_check range is halved because the output throttle is scaled to 500us. + // So by using half of min_check we maintain the same low-throttle deadband + // stick travel as normal non-3D mode. + const int mincheckOffset = (rxConfig()->mincheck - PWM_RANGE_MIN) / 2; + rcCommand3dDeadBandLow = rxConfig()->midrc - mincheckOffset; + rcCommand3dDeadBandHigh = rxConfig()->midrc + mincheckOffset; + } else { + rcCommand3dDeadBandLow = rxConfig()->midrc - flight3DConfig()->deadband3d_throttle; + rcCommand3dDeadBandHigh = rxConfig()->midrc + flight3DConfig()->deadband3d_throttle; + } + + const float rcCommandThrottleRange3dLow = rcCommand3dDeadBandLow - PWM_RANGE_MIN; + const float rcCommandThrottleRange3dHigh = PWM_RANGE_MAX - rcCommand3dDeadBandHigh; + if (rcCommand[THROTTLE] <= rcCommand3dDeadBandLow) { // INVERTED motorRangeMin = motorOutputLow; @@ -602,11 +608,11 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) currentThrottleInputRange = rcCommandThrottleRange3dHigh; } if (currentTimeUs - reversalTimeUs < 250000) { - // keep ITerm zero for 250ms after motor reversal - pidResetITerm(); + // keep iterm zero for 250ms after motor reversal + pidResetIterm(); } } else { - throttle = rcCommand[THROTTLE] - rxConfig()->mincheck; + throttle = rcCommand[THROTTLE] - rxConfig()->mincheck + throttleAngleCorrection; currentThrottleInputRange = rcCommandThrottleRange; motorRangeMin = motorOutputLow; motorRangeMax = motorOutputHigh; @@ -693,9 +699,11 @@ static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS]) // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips. for (int i = 0; i < motorCount; i++) { float motorOutput = motorOutputMin + (motorOutputRange * (motorOutputMixSign * motorMix[i] + throttle * currentMixer[i].throttle)); +#ifdef USE_SERVOS if (mixerIsTricopter()) { motorOutput += mixerTricopterMotorCorrection(i); } +#endif if (failsafeIsActive()) { if (isMotorProtocolDshot()) { motorOutput = (motorOutput < motorRangeMin) ? disarmMotorOutput : motorOutput; // Prevent getting into special reserved range @@ -704,12 +712,6 @@ static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS]) } else { motorOutput = constrain(motorOutput, motorRangeMin, motorRangeMax); } - // Motor stop handling - if (feature(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !feature(FEATURE_3D) && !isAirmodeActive()) { - if (((rcData[THROTTLE]) < rxConfig()->mincheck)) { - motorOutput = disarmMotorOutput; - } - } motor[i] = motorOutput; } @@ -736,6 +738,13 @@ float applyThrottleLimit(float throttle) return throttle; } +void applyMotorStop(void) +{ + for (int i = 0; i < motorCount; i++) { + motor[i] = disarmMotorOutput; + } +} + FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensation) { if (isFlipOverAfterCrashMode()) { @@ -776,10 +785,12 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa throttle = applyThrottleLimit(throttle); } + const bool airmodeEnabled = airmodeIsEnabled(); + #ifdef USE_YAW_SPIN_RECOVERY // 50% throttle provides the maximum authority for yaw recovery when airmode is not active. // When airmode is active the throttle setting doesn't impact recovery authority. - if (yawSpinDetected && !isAirmodeActive()) { + if (yawSpinDetected && !airmodeEnabled) { throttle = 0.5f; // } #endif // USE_YAW_SPIN_RECOVERY @@ -803,10 +814,24 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa motorMix[i] = mix; } + pidUpdateAntiGravityThrottleFilter(throttle); +#ifdef USE_DYN_LPF + dynLpfGyroUpdate(throttle); + dynLpfDTermUpdate(throttle); +#endif + #if defined(USE_THROTTLE_BOOST) if (throttleBoost > 0.0f) { - float throttlehpf = throttle - pt1FilterApply(&throttleLpf, throttle); - throttle = constrainf(throttle + throttleBoost * throttlehpf, 0.0f, 1.0f); + const float throttleHpf = throttle - pt1FilterApply(&throttleLpf, throttle); + throttle = constrainf(throttle + throttleBoost * throttleHpf, 0.0f, 1.0f); + } +#endif + +#ifdef USE_GPS_RESCUE + // If gps rescue is active then override the throttle. This prevents things + // like throttle boost or throttle limit from negatively affecting the throttle. + if (FLIGHT_MODE(GPS_RESCUE_MODE)) { + throttle = gpsRescueGetThrottle(); } #endif @@ -816,18 +841,27 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa motorMix[i] /= motorMixRange; } // Get the maximum correction by setting offset to center when airmode enabled - if (isAirmodeActive()) { + if (airmodeEnabled) { throttle = 0.5f; } } else { - if (isAirmodeActive() || throttle > 0.5f) { // Only automatically adjust throttle when airmode enabled. Airmode logic is always active on high throttle - const float throttleLimitOffset = motorMixRange / 2.0f; - throttle = constrainf(throttle, 0.0f + throttleLimitOffset, 1.0f - throttleLimitOffset); + if (airmodeEnabled || throttle > 0.5f) { // Only automatically adjust throttle when airmode enabled. Airmode logic is always active on high throttle + throttle = constrainf(throttle, -motorMixMin, 1.0f - motorMixMax); } } - // Apply the mix to motor endpoints - applyMixToMotors(motorMix); + if (featureIsEnabled(FEATURE_MOTOR_STOP) + && ARMING_FLAG(ARMED) + && !featureIsEnabled(FEATURE_3D) + && !airmodeEnabled + && !FLIGHT_MODE(GPS_RESCUE_MODE) // disable motor_stop while GPS Rescue is active + && (rcData[THROTTLE] < rxConfig()->mincheck)) { + // motor_stop handling + applyMotorStop(); + } else { + // Apply the mix to motor endpoints + applyMixToMotors(motorMix); + } } float convertExternalToMotor(uint16_t externalValue) @@ -838,7 +872,7 @@ float convertExternalToMotor(uint16_t externalValue) case true: externalValue = constrain(externalValue, PWM_RANGE_MIN, PWM_RANGE_MAX); - if (feature(FEATURE_3D)) { + if (featureIsEnabled(FEATURE_3D)) { if (externalValue == PWM_RANGE_MID) { motorValue = DSHOT_DISARM_COMMAND; } else if (externalValue < PWM_RANGE_MID) { @@ -867,7 +901,7 @@ uint16_t convertMotorToExternal(float motorValue) switch ((int)isMotorProtocolDshot()) { #ifdef USE_DSHOT case true: - if (feature(FEATURE_3D)) { + if (featureIsEnabled(FEATURE_3D)) { if (motorValue == DSHOT_DISARM_COMMAND || motorValue < DSHOT_MIN_THROTTLE) { externalValue = PWM_RANGE_MID; } else if (motorValue <= DSHOT_3D_DEADBAND_LOW) { @@ -888,3 +922,8 @@ uint16_t convertMotorToExternal(float motorValue) return externalValue; } + +void mixerSetThrottleAngleCorrection(int correctionValue) +{ + throttleAngleCorrection = correctionValue; +} diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index b0f4939593..e235a9d6b7 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -117,7 +117,6 @@ struct rxConfig_s; uint8_t getMotorCount(void); float getMotorMixRange(void); bool areMotorsRunning(void); -bool mixerIsOutputSaturated(int axis, float errorRate); void mixerLoadMix(int index, motorMixer_t *customMixers); void mixerInit(mixerMode_e mixerMode); @@ -134,3 +133,6 @@ void stopPwmAllMotors(void); float convertExternalToMotor(uint16_t externalValue); uint16_t convertMotorToExternal(float motorValue); bool mixerIsTricopter(void); + +void mixerSetThrottleAngleCorrection(int correctionValue); + diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 7c2ab9e077..823d4362fa 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -39,8 +39,8 @@ #include "drivers/sound_beeper.h" #include "drivers/time.h" -#include "fc/fc_core.h" -#include "fc/fc_rc.h" +#include "fc/core.h" +#include "fc/rc.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" @@ -56,6 +56,13 @@ #include "sensors/acceleration.h" +const char pidNames[] = + "ROLL;" + "PITCH;" + "YAW;" + "LEVEL;" + "MAG;"; + FAST_RAM_ZERO_INIT uint32_t targetPidLooptime; FAST_RAM_ZERO_INIT pidAxisData_t pidData[XYZ_AXIS_COUNT]; @@ -66,6 +73,12 @@ static FAST_RAM_ZERO_INIT bool inCrashRecoveryMode = false; static FAST_RAM_ZERO_INIT float dT; static FAST_RAM_ZERO_INIT float pidFrequency; +static FAST_RAM_ZERO_INIT uint8_t antiGravityMode; +static FAST_RAM_ZERO_INIT float antiGravityThrottleHpf; +static FAST_RAM_ZERO_INIT uint16_t itermAcceleratorGain; +static FAST_RAM float antiGravityOsdCutoff = 1.0f; +static FAST_RAM_ZERO_INIT bool antiGravityEnabled; + PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 2); #ifdef STM32F10X @@ -94,41 +107,33 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig, #define ACRO_TRAINER_SETPOINT_LIMIT 1000.0f // Limit the correcting setpoint #endif // USE_ACRO_TRAINER -PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 4); +#define ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF 15 // The anti gravity throttle highpass filter cutoff + +PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 5); void resetPidProfile(pidProfile_t *pidProfile) { RESET_CONFIG(pidProfile_t, pidProfile, .pid = { - [PID_ROLL] = { 46, 45, 25 }, - [PID_PITCH] = { 50, 50, 27 }, - [PID_YAW] = { 65, 45, 20 }, - [PID_ALT] = { 50, 0, 0 }, - [PID_POS] = { 15, 0, 0 }, // POSHOLD_P * 100, POSHOLD_I * 100, - [PID_POSR] = { 34, 14, 53 }, // POSHOLD_RATE_P * 10, POSHOLD_RATE_I * 100, POSHOLD_RATE_D * 1000, - [PID_NAVR] = { 25, 33, 83 }, // NAV_P * 10, NAV_I * 100, NAV_D * 1000 - [PID_LEVEL] = { 50, 50, 75 }, - [PID_MAG] = { 40, 0, 0 }, - [PID_VEL] = { 55, 55, 75 } + [PID_ROLL] = { 46, 45, 25, 60 }, + [PID_PITCH] = { 50, 50, 27, 60 }, + [PID_YAW] = { 45, 100, 0, 100 }, + [PID_LEVEL] = { 50, 50, 75, 0 }, + [PID_MAG] = { 40, 0, 0, 0 }, }, - .pidSumLimit = PIDSUM_LIMIT, .pidSumLimitYaw = PIDSUM_LIMIT_YAW, .yaw_lowpass_hz = 0, - .dterm_lowpass_hz = 100, // dual PT1 filtering ON by default - .dterm_lowpass2_hz = 200, // second Dterm LPF ON by default .dterm_notch_hz = 0, - .dterm_notch_cutoff = 160, - .dterm_filter_type = FILTER_PT1, + .dterm_notch_cutoff = 0, .itermWindupPointPercent = 40, .vbatPidCompensation = 0, .pidAtMinThrottle = PID_STABILISATION_ON, .levelAngleLimit = 55, - .setpointRelaxRatio = 0, - .dtermSetpointWeight = 60, - .yawRateAccelLimit = 100, + .feedForwardTransition = 0, + .yawRateAccelLimit = 0, .rateAccelLimit = 0, - .itermThrottleThreshold = 350, + .itermThrottleThreshold = 250, .itermAcceleratorGain = 5000, .crash_time = 500, // ms .crash_delay = 0, // ms @@ -141,14 +146,14 @@ void resetPidProfile(pidProfile_t *pidProfile) .horizon_tilt_effect = 75, .horizon_tilt_expert_mode = false, .crash_limit_yaw = 200, - .itermLimit = 150, + .itermLimit = 300, .throttle_boost = 5, .throttle_boost_cutoff = 15, .iterm_rotation = true, .smart_feedforward = false, - .iterm_relax = ITERM_RELAX_OFF, + .iterm_relax = ITERM_RELAX_RP, .iterm_relax_cutoff = 11, - .iterm_relax_type = ITERM_RELAX_GYRO, + .iterm_relax_type = ITERM_RELAX_SETPOINT, .acro_trainer_angle_limit = 20, .acro_trainer_lookahead_ms = 50, .acro_trainer_debug_axis = FD_ROLL, @@ -156,7 +161,20 @@ void resetPidProfile(pidProfile_t *pidProfile) .abs_control_gain = 0, .abs_control_limit = 90, .abs_control_error_limit = 20, + .antiGravityMode = ANTI_GRAVITY_SMOOTH, + .dyn_lpf_dterm_max_hz = 200, + .dyn_lpf_dterm_idle = 20, + .dterm_lowpass_hz = 100, // dual PT1 filtering ON by default + .dterm_lowpass2_hz = 200, // second Dterm LPF ON by default + .dterm_filter_type = FILTER_PT1, + .dterm_filter2_type = FILTER_PT1, ); +#ifdef USE_DYN_LPF + pidProfile->dterm_lowpass_hz = 120; + pidProfile->dterm_lowpass2_hz = 180; + pidProfile->dterm_filter_type = FILTER_BIQUAD; + pidProfile->dterm_filter2_type = FILTER_BIQUAD; +#endif } void pgResetFn_pidProfiles(pidProfile_t *pidProfiles) @@ -180,9 +198,9 @@ void pidSetItermAccelerator(float newItermAccelerator) itermAccelerator = newItermAccelerator; } -float pidItermAccelerator(void) +bool pidOsdAntiGravityActive(void) { - return itermAccelerator; + return (itermAccelerator > antiGravityOsdCutoff); } void pidStabilisationState(pidStabilisationState_e pidControllerState) @@ -198,11 +216,11 @@ typedef union dtermLowpass_u { } dtermLowpass_t; static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermNotchApplyFn; -static FAST_RAM_ZERO_INIT biquadFilter_t dtermNotch[2]; +static FAST_RAM_ZERO_INIT biquadFilter_t dtermNotch[XYZ_AXIS_COUNT]; static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermLowpassApplyFn; -static FAST_RAM_ZERO_INIT dtermLowpass_t dtermLowpass[2]; +static FAST_RAM_ZERO_INIT dtermLowpass_t dtermLowpass[XYZ_AXIS_COUNT]; static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermLowpass2ApplyFn; -static FAST_RAM_ZERO_INIT pt1Filter_t dtermLowpass2[2]; +static FAST_RAM_ZERO_INIT dtermLowpass_t dtermLowpass2[XYZ_AXIS_COUNT]; static FAST_RAM_ZERO_INIT filterApplyFnPtr ptermYawLowpassApplyFn; static FAST_RAM_ZERO_INIT pt1Filter_t ptermYawLowpass; #if defined(USE_ITERM_RELAX) @@ -213,16 +231,18 @@ static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoff; #endif #ifdef USE_RC_SMOOTHING_FILTER -static FAST_RAM_ZERO_INIT pt1Filter_t setpointDerivativePt1[2]; -static FAST_RAM_ZERO_INIT biquadFilter_t setpointDerivativeBiquad[2]; +static FAST_RAM_ZERO_INIT pt1Filter_t setpointDerivativePt1[XYZ_AXIS_COUNT]; +static FAST_RAM_ZERO_INIT biquadFilter_t setpointDerivativeBiquad[XYZ_AXIS_COUNT]; static FAST_RAM_ZERO_INIT bool setpointDerivativeLpfInitialized; static FAST_RAM_ZERO_INIT uint8_t rcSmoothingDebugAxis; static FAST_RAM_ZERO_INIT uint8_t rcSmoothingFilterType; #endif // USE_RC_SMOOTHING_FILTER +static FAST_RAM_ZERO_INIT pt1Filter_t antiGravityThrottleLpf; + void pidInitFilters(const pidProfile_t *pidProfile) { - BUILD_BUG_ON(FD_YAW != 2); // only setting up Dterm filters on roll and pitch axes, so ensure yaw axis is 2 + STATIC_ASSERT(FD_YAW == 2, FD_YAW_incorrect); // ensure yaw axis is 2 if (targetPidLooptime == 0) { // no looptime set, so set all the filters to null @@ -248,42 +268,60 @@ void pidInitFilters(const pidProfile_t *pidProfile) if (dTermNotchHz != 0 && pidProfile->dterm_notch_cutoff != 0) { dtermNotchApplyFn = (filterApplyFnPtr)biquadFilterApply; const float notchQ = filterGetNotchQ(dTermNotchHz, pidProfile->dterm_notch_cutoff); - for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { + for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { biquadFilterInit(&dtermNotch[axis], dTermNotchHz, targetPidLooptime, notchQ, FILTER_NOTCH); } } else { dtermNotchApplyFn = nullFilterApply; } - //2nd Dterm Lowpass Filter - if (pidProfile->dterm_lowpass2_hz == 0 || pidProfile->dterm_lowpass2_hz > pidFrequencyNyquist) { - dtermLowpass2ApplyFn = nullFilterApply; - } else { - dtermLowpass2ApplyFn = (filterApplyFnPtr)pt1FilterApply; - for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { - pt1FilterInit(&dtermLowpass2[axis], pt1FilterGain(pidProfile->dterm_lowpass2_hz, dT)); - } - } - + //1st Dterm Lowpass Filter if (pidProfile->dterm_lowpass_hz == 0 || pidProfile->dterm_lowpass_hz > pidFrequencyNyquist) { dtermLowpassApplyFn = nullFilterApply; } else { switch (pidProfile->dterm_filter_type) { - default: - dtermLowpassApplyFn = nullFilterApply; - break; case FILTER_PT1: dtermLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply; - for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { + for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { pt1FilterInit(&dtermLowpass[axis].pt1Filter, pt1FilterGain(pidProfile->dterm_lowpass_hz, dT)); } break; case FILTER_BIQUAD: +#ifdef USE_DYN_LPF + dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; +#else dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApply; - for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { +#endif + for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { biquadFilterInitLPF(&dtermLowpass[axis].biquadFilter, pidProfile->dterm_lowpass_hz, targetPidLooptime); } break; + default: + dtermLowpassApplyFn = nullFilterApply; + break; + } + } + + //2nd Dterm Lowpass Filter + if (pidProfile->dterm_lowpass2_hz == 0 || pidProfile->dterm_lowpass2_hz > pidFrequencyNyquist) { + dtermLowpass2ApplyFn = nullFilterApply; + } else { + switch (pidProfile->dterm_filter2_type) { + case FILTER_PT1: + dtermLowpass2ApplyFn = (filterApplyFnPtr)pt1FilterApply; + for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { + pt1FilterInit(&dtermLowpass2[axis].pt1Filter, pt1FilterGain(pidProfile->dterm_lowpass2_hz, dT)); + } + break; + case FILTER_BIQUAD: + dtermLowpass2ApplyFn = (filterApplyFnPtr)biquadFilterApply; + for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { + biquadFilterInitLPF(&dtermLowpass2[axis].biquadFilter, pidProfile->dterm_lowpass2_hz, targetPidLooptime); + } + break; + default: + dtermLowpass2ApplyFn = nullFilterApply; + break; } } @@ -304,6 +342,8 @@ void pidInitFilters(const pidProfile_t *pidProfile) } } #endif + + pt1FilterInit(&antiGravityThrottleLpf, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, dT)); } #ifdef USE_RC_SMOOTHING_FILTER @@ -313,7 +353,7 @@ void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint rcSmoothingFilterType = filterType; if ((filterCutoff > 0) && (rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) { setpointDerivativeLpfInitialized = true; - for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { + for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { switch (rcSmoothingFilterType) { case RC_SMOOTHING_DERIVATIVE_PT1: pt1FilterInit(&setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, dT)); @@ -325,20 +365,36 @@ void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint } } } + +void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff) +{ + if ((filterCutoff > 0) && (rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) { + for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { + switch (rcSmoothingFilterType) { + case RC_SMOOTHING_DERIVATIVE_PT1: + pt1FilterUpdateCutoff(&setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, dT)); + break; + case RC_SMOOTHING_DERIVATIVE_BIQUAD: + biquadFilterUpdateLPF(&setpointDerivativeBiquad[axis], filterCutoff, targetPidLooptime); + break; + } + } + } +} #endif // USE_RC_SMOOTHING_FILTER typedef struct pidCoefficient_s { float Kp; float Ki; float Kd; + float Kf; } pidCoefficient_t; -static FAST_RAM_ZERO_INIT pidCoefficient_t pidCoefficient[3]; -static FAST_RAM_ZERO_INIT float maxVelocity[3]; -static FAST_RAM_ZERO_INIT float relaxFactor; -static FAST_RAM_ZERO_INIT float dtermSetpointWeight; +static FAST_RAM_ZERO_INIT pidCoefficient_t pidCoefficient[XYZ_AXIS_COUNT]; +static FAST_RAM_ZERO_INIT float maxVelocity[XYZ_AXIS_COUNT]; +static FAST_RAM_ZERO_INIT float feedForwardTransition; static FAST_RAM_ZERO_INIT float levelGain, horizonGain, horizonTransition, horizonCutoffDegrees, horizonFactorRatio; -static FAST_RAM_ZERO_INIT float ITermWindupPointInv; +static FAST_RAM_ZERO_INIT float itermWindupPointInv; static FAST_RAM_ZERO_INIT uint8_t horizonTiltExpertMode; static FAST_RAM_ZERO_INIT timeDelta_t crashTimeLimitUs; static FAST_RAM_ZERO_INIT timeDelta_t crashTimeDelayUs; @@ -359,13 +415,13 @@ static FAST_RAM_ZERO_INIT bool itermRotation; static FAST_RAM_ZERO_INIT bool smartFeedforward; #endif #if defined(USE_ABSOLUTE_CONTROL) -static FAST_RAM_ZERO_INIT float axisError[XYZ_AXIS_COUNT]; +STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT float axisError[XYZ_AXIS_COUNT]; static FAST_RAM_ZERO_INIT float acGain; static FAST_RAM_ZERO_INIT float acLimit; static FAST_RAM_ZERO_INIT float acErrorLimit; #endif -void pidResetITerm(void) +void pidResetIterm(void) { for (int axis = 0; axis < 3; axis++) { pidData[axis].I = 0.0f; @@ -384,20 +440,35 @@ static FAST_RAM_ZERO_INIT int acroTrainerAxisState[2]; // only need roll and pi static FAST_RAM_ZERO_INIT float acroTrainerGain; #endif // USE_ACRO_TRAINER +void pidUpdateAntiGravityThrottleFilter(float throttle) +{ + if (antiGravityMode == ANTI_GRAVITY_SMOOTH) { + antiGravityThrottleHpf = throttle - pt1FilterApply(&antiGravityThrottleLpf, throttle); + } +} + +#ifdef USE_DYN_LPF +static FAST_RAM int8_t dynLpfFilter = DYN_LPF_NONE; +static FAST_RAM_ZERO_INIT float dynLpfIdle; +static FAST_RAM_ZERO_INIT float dynLpfIdlePoint; +static FAST_RAM_ZERO_INIT float dynLpfInvIdlePointScaled; +static FAST_RAM_ZERO_INIT uint16_t dynLpfMin; +#endif + void pidInitConfig(const pidProfile_t *pidProfile) { + if (pidProfile->feedForwardTransition == 0) { + feedForwardTransition = 0; + } else { + feedForwardTransition = 100.0f / pidProfile->feedForwardTransition; + } for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { pidCoefficient[axis].Kp = PTERM_SCALE * pidProfile->pid[axis].P; pidCoefficient[axis].Ki = ITERM_SCALE * pidProfile->pid[axis].I; pidCoefficient[axis].Kd = DTERM_SCALE * pidProfile->pid[axis].D; + pidCoefficient[axis].Kf = FEEDFORWARD_SCALE * (pidProfile->pid[axis].F / 100.0f); } - dtermSetpointWeight = pidProfile->dtermSetpointWeight / 100.0f; - if (pidProfile->setpointRelaxRatio == 0) { - relaxFactor = 0; - } else { - relaxFactor = 100.0f / pidProfile->setpointRelaxRatio; - } levelGain = pidProfile->pid[PID_LEVEL].P / 10.0f; horizonGain = pidProfile->pid[PID_LEVEL].I / 10.0f; horizonTransition = (float)pidProfile->pid[PID_LEVEL].D; @@ -406,8 +477,12 @@ void pidInitConfig(const pidProfile_t *pidProfile) horizonFactorRatio = (100 - pidProfile->horizon_tilt_effect) * 0.01f; maxVelocity[FD_ROLL] = maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 100 * dT; maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 100 * dT; - const float ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f; - ITermWindupPointInv = 1.0f / (1.0f - ITermWindupPoint); + itermWindupPointInv = 1.0f; + if (pidProfile->itermWindupPointPercent < 100) { + const float itermWindupPoint = pidProfile->itermWindupPointPercent / 100.0f; + itermWindupPointInv = 1.0f / (1.0f - itermWindupPoint); + } + itermAcceleratorGain = pidProfile->itermAcceleratorGain; crashTimeLimitUs = pidProfile->crash_time * 1000; crashTimeDelayUs = pidProfile->crash_delay * 1000; crashRecoveryAngleDeciDegrees = pidProfile->crash_recovery_angle * 10; @@ -421,6 +496,18 @@ void pidInitConfig(const pidProfile_t *pidProfile) throttleBoost = pidProfile->throttle_boost * 0.1f; #endif itermRotation = pidProfile->iterm_rotation; + antiGravityMode = pidProfile->antiGravityMode; + + // Calculate the anti-gravity value that will trigger the OSD display. + // For classic AG it's either 1.0 for off and > 1.0 for on. + // For the new AG it's a continuous floating value so we want to trigger the OSD + // display when it exceeds 25% of its possible range. This gives a useful indication + // of AG activity without excessive display. + antiGravityOsdCutoff = 1.0f; + if (antiGravityMode == ANTI_GRAVITY_SMOOTH) { + antiGravityOsdCutoff += ((itermAcceleratorGain - 1000) / 1000.0f) * 0.25f; + } + #if defined(USE_SMART_FEEDFORWARD) smartFeedforward = pidProfile->smart_feedforward; #endif @@ -442,6 +529,30 @@ void pidInitConfig(const pidProfile_t *pidProfile) acLimit = (float)pidProfile->abs_control_limit; acErrorLimit = (float)pidProfile->abs_control_error_limit; #endif + +#ifdef USE_DYN_LPF + if (pidProfile->dyn_lpf_dterm_idle > 0 && pidProfile->dyn_lpf_dterm_max_hz > 0) { + if (pidProfile->dterm_lowpass_hz > 0 ) { + dynLpfMin = pidProfile->dterm_lowpass_hz; + switch (pidProfile->dterm_filter_type) { + case FILTER_PT1: + dynLpfFilter = DYN_LPF_PT1; + break; + case FILTER_BIQUAD: + dynLpfFilter = DYN_LPF_BIQUAD; + break; + default: + dynLpfFilter = DYN_LPF_NONE; + break; + } + } + } else { + dynLpfFilter = DYN_LPF_NONE; + } + dynLpfIdle = pidProfile->dyn_lpf_dterm_idle / 100.0f; + dynLpfIdlePoint = (dynLpfIdle - (dynLpfIdle * dynLpfIdle * dynLpfIdle) / 3.0f) * 1.5f; + dynLpfInvIdlePointScaled = 1 / (1 - dynLpfIdlePoint) * (pidProfile->dyn_lpf_dterm_max_hz - dynLpfMin); +#endif } void pidInit(const pidProfile_t *pidProfile) @@ -469,7 +580,7 @@ void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex) } // calculates strength of horizon leveling; 0 = none, 1.0 = most leveling -static float calcHorizonLevelStrength(void) +STATIC_UNIT_TESTED float calcHorizonLevelStrength(void) { // start with 1.0 at center stick, 0.0 at max stick deflection: float horizonLevelStrength = 1.0f - MAX(getRcDeflectionAbs(FD_ROLL), getRcDeflectionAbs(FD_PITCH)); @@ -524,7 +635,7 @@ static float calcHorizonLevelStrength(void) return constrainf(horizonLevelStrength, 0, 1); } -static float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint) { +STATIC_UNIT_TESTED float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint) { // calculate error angle and limit the angle to the max inclination // rcDeflection is in range [-1.0, 1.0] float angle = pidProfile->levelAngleLimit * getRcDeflection(axis); @@ -547,7 +658,7 @@ static float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPit static float accelerationLimit(int axis, float currentPidSetpoint) { - static float previousSetpoint[3]; + static float previousSetpoint[XYZ_AXIS_COUNT]; const float currentVelocity = currentPidSetpoint - previousSetpoint[axis]; if (ABS(currentVelocity) > maxVelocity[axis]) { @@ -579,8 +690,8 @@ static void handleCrashRecovery( *errorRate = *currentPidSetpoint - gyroRate; } } - // reset ITerm, since accumulated error before crash is now meaningless - // and ITerm windup during crash recovery can be extreme, especially on yaw axis + // reset iterm, since accumulated error before crash is now meaningless + // and iterm windup during crash recovery can be extreme, especially on yaw axis pidData[axis].I = 0.0f; if (cmpTimeUs(currentTimeUs, crashDetectedAtUs) > crashTimeLimitUs || (getMotorMixRange() < 1.0f @@ -629,7 +740,7 @@ static void detectAndSetCrashRecovery( } } -static void rotateVector(float v[XYZ_AXIS_COUNT], float rotation[XYZ_AXIS_COUNT]) +static void rotateVector(float v[XYZ_AXIS_COUNT], float rotation[XYZ_AXIS_COUNT]) { // rotate v around rotation vector rotation // rotation in radians, all elements must be small @@ -642,7 +753,7 @@ static void rotateVector(float v[XYZ_AXIS_COUNT], float rotation[XYZ_AXIS_COUNT] } } -static void rotateITermAndAxisError() +STATIC_UNIT_TESTED void rotateItermAndAxisError() { if (itermRotation #if defined(USE_ABSOLUTE_CONTROL) @@ -650,7 +761,7 @@ static void rotateITermAndAxisError() #endif ) { const float gyroToAngle = dT * RAD; - float rotationRads[3]; + float rotationRads[XYZ_AXIS_COUNT]; for (int i = FD_ROLL; i <= FD_YAW; i++) { rotationRads[i] = gyro.gyroADCf[i] * gyroToAngle; } @@ -660,7 +771,7 @@ static void rotateITermAndAxisError() } #endif if (itermRotation) { - float v[3]; + float v[XYZ_AXIS_COUNT]; for (int i = 0; i < XYZ_AXIS_COUNT; i++) { v[i] = pidData[i].I; } @@ -746,45 +857,173 @@ static FAST_CODE_NOINLINE float applyAcroTrainer(int axis, const rollAndPitchTri } #endif // USE_ACRO_TRAINER +#ifdef USE_RC_SMOOTHING_FILTER +float FAST_CODE applyRcSmoothingDerivativeFilter(int axis, float pidSetpointDelta) +{ + float ret = pidSetpointDelta; + if (axis == rcSmoothingDebugAxis) { + DEBUG_SET(DEBUG_RC_SMOOTHING, 1, lrintf(pidSetpointDelta * 100.0f)); + } + if (setpointDerivativeLpfInitialized) { + switch (rcSmoothingFilterType) { + case RC_SMOOTHING_DERIVATIVE_PT1: + ret = pt1FilterApply(&setpointDerivativePt1[axis], pidSetpointDelta); + break; + case RC_SMOOTHING_DERIVATIVE_BIQUAD: + ret = biquadFilterApplyDF1(&setpointDerivativeBiquad[axis], pidSetpointDelta); + break; + } + if (axis == rcSmoothingDebugAxis) { + DEBUG_SET(DEBUG_RC_SMOOTHING, 2, lrintf(ret * 100.0f)); + } + } + return ret; +} +#endif // USE_RC_SMOOTHING_FILTER + +#ifdef USE_SMART_FEEDFORWARD +void FAST_CODE applySmartFeedforward(int axis) +{ + if (smartFeedforward) { + if (pidData[axis].P * pidData[axis].F > 0) { + if (ABS(pidData[axis].F) > ABS(pidData[axis].P)) { + pidData[axis].P = 0; + } else { + pidData[axis].F = 0; + } + } + } +} +#endif // USE_SMART_FEEDFORWARD + +#if defined(USE_ABSOLUTE_CONTROL) +STATIC_UNIT_TESTED void applyAbsoluteControl(const int axis, const float gyroRate, const bool itermRelaxIsEnabled, + const float setpointLpf, const float setpointHpf, + float *currentPidSetpoint, float *itermErrorRate) +{ + if (acGain > 0) { + float acErrorRate = 0; + if (itermRelaxIsEnabled) { + const float gmaxac = setpointLpf + 2 * setpointHpf; + const float gminac = setpointLpf - 2 * setpointHpf; + if (gyroRate >= gminac && gyroRate <= gmaxac) { + const float acErrorRate1 = gmaxac - gyroRate; + const float acErrorRate2 = gminac - gyroRate; + if (acErrorRate1 * axisError[axis] < 0) { + acErrorRate = acErrorRate1; + } else { + acErrorRate = acErrorRate2; + } + if (fabsf(acErrorRate * dT) > fabsf(axisError[axis]) ) { + acErrorRate = -axisError[axis] * pidFrequency; + } + } else { + acErrorRate = (gyroRate > gmaxac ? gmaxac : gminac ) - gyroRate; + } + } else { + acErrorRate = *itermErrorRate; + } + + if (isAirmodeActivated()) { + axisError[axis] = constrainf(axisError[axis] + acErrorRate * dT, + -acErrorLimit, acErrorLimit); + const float acCorrection = constrainf(axisError[axis] * acGain, -acLimit, acLimit); + *currentPidSetpoint += acCorrection; + *itermErrorRate += acCorrection; + if (axis == FD_ROLL) { + DEBUG_SET(DEBUG_ITERM_RELAX, 3, lrintf(axisError[axis] * 10)); + } + } + } +} +#endif + +#if defined(USE_ITERM_RELAX) +STATIC_UNIT_TESTED void applyItermRelax(const int axis, const float iterm, + const float gyroRate, float *itermErrorRate, float *currentPidSetpoint) +{ + const float setpointLpf = pt1FilterApply(&windupLpf[axis], *currentPidSetpoint); + const float setpointHpf = fabsf(*currentPidSetpoint - setpointLpf); + bool itermRelaxIsEnabled = false; + + if (itermRelax && + (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY || + itermRelax == ITERM_RELAX_RPY_INC)) { + itermRelaxIsEnabled = true; + const float itermRelaxFactor = 1 - setpointHpf / ITERM_RELAX_SETPOINT_THRESHOLD; + + const bool isDecreasingI = + ((iterm > 0) && (*itermErrorRate < 0)) || ((iterm < 0) && (*itermErrorRate > 0)); + if ((itermRelax >= ITERM_RELAX_RP_INC) && isDecreasingI) { + // Do Nothing, use the precalculed itermErrorRate + } else if (itermRelaxType == ITERM_RELAX_SETPOINT && setpointHpf < ITERM_RELAX_SETPOINT_THRESHOLD) { + *itermErrorRate *= itermRelaxFactor; + } else if (itermRelaxType == ITERM_RELAX_GYRO ) { + *itermErrorRate = fapplyDeadband(setpointLpf - gyroRate, setpointHpf); + } else { + *itermErrorRate = 0.0f; + } + + if (axis == FD_ROLL) { + DEBUG_SET(DEBUG_ITERM_RELAX, 0, lrintf(setpointHpf)); + DEBUG_SET(DEBUG_ITERM_RELAX, 1, lrintf(itermRelaxFactor * 100.0f)); + DEBUG_SET(DEBUG_ITERM_RELAX, 2, lrintf(*itermErrorRate)); + } + } else { +#if defined(USE_ABSOLUTE_CONTROL) + applyAbsoluteControl(axis, gyroRate, setpointLpf, setpointHpf, itermRelaxIsEnabled, currentPidSetpoint, itermErrorRate); +#else + UNUSED(itermRelaxIsEnabled); +#endif + } +} +#endif + // Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage. // Based on 2DOF reference design (matlab) void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, timeUs_t currentTimeUs) { - static float previousGyroRateDterm[2]; - static float previousPidSetpoint[2]; + static float previousGyroRateDterm[XYZ_AXIS_COUNT]; + static float previousPidSetpoint[XYZ_AXIS_COUNT]; const float tpaFactor = getThrottlePIDAttenuation(); - const float motorMixRange = getMotorMixRange(); #ifdef USE_YAW_SPIN_RECOVERY const bool yawSpinActive = gyroYawSpinDetected(); #endif // Dynamic i component, - // gradually scale back integration when above windup point - const float dynCi = MIN((1.0f - motorMixRange) * ITermWindupPointInv, 1.0f) * dT * itermAccelerator; + if ((antiGravityMode == ANTI_GRAVITY_SMOOTH) && antiGravityEnabled) { + itermAccelerator = 1 + fabsf(antiGravityThrottleHpf) * 0.01f * (itermAcceleratorGain - 1000); + DEBUG_SET(DEBUG_ANTI_GRAVITY, 1, lrintf(antiGravityThrottleHpf * 1000)); + } + DEBUG_SET(DEBUG_ANTI_GRAVITY, 0, lrintf(itermAccelerator * 1000)); - // Dynamic d component, enable 2-DOF PID controller only for rate mode - const float dynCd = flightModeFlags ? 0.0f : dtermSetpointWeight; + // gradually scale back integration when above windup point + float dynCi = dT * itermAccelerator; + if (itermWindupPointInv > 1.0f) { + dynCi *= constrainf((1.0f - getMotorMixRange()) * itermWindupPointInv, 0.0f, 1.0f); + } // Precalculate gyro deta for D-term here, this allows loop unrolling - float gyroRateDterm[2]; - for (int axis = FD_ROLL; axis < FD_YAW; ++axis) { + float gyroRateDterm[XYZ_AXIS_COUNT]; + for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) { gyroRateDterm[axis] = dtermNotchApplyFn((filter_t *) &dtermNotch[axis], gyro.gyroADCf[axis]); gyroRateDterm[axis] = dtermLowpassApplyFn((filter_t *) &dtermLowpass[axis], gyroRateDterm[axis]); gyroRateDterm[axis] = dtermLowpass2ApplyFn((filter_t *) &dtermLowpass2[axis], gyroRateDterm[axis]); } - rotateITermAndAxisError(); + rotateItermAndAxisError(); // ----------PID controller---------- for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) { + float currentPidSetpoint = getSetpointRate(axis); if (maxVelocity[axis]) { currentPidSetpoint = accelerationLimit(axis, currentPidSetpoint); } // Yaw control is GYRO based, direct sticks control is applied to rate PID - if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || FLIGHT_MODE(GPS_RESCUE_MODE)) && axis != YAW) { + if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || FLIGHT_MODE(GPS_RESCUE_MODE)) && axis != FD_YAW) { currentPidSetpoint = pidLevel(axis, pidProfile, angleTrim, currentPidSetpoint); } @@ -804,98 +1043,33 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT // -----calculate error rate const float gyroRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec - -#ifdef USE_ABSOLUTE_CONTROL - float acCorrection = 0; - float acErrorRate; -#endif - float itermErrorRate = 0.0f; - -#if defined(USE_ITERM_RELAX) - if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY )) { - const float setpointLpf = pt1FilterApply(&windupLpf[axis], currentPidSetpoint); - const float setpointHpf = fabsf(currentPidSetpoint - setpointLpf); - const float itermRelaxFactor = 1 - setpointHpf / 30.0f; - if (itermRelaxType == ITERM_RELAX_SETPOINT && setpointHpf < 30) { - itermErrorRate = itermRelaxFactor * (currentPidSetpoint - gyroRate); - } else if (itermRelaxType == ITERM_RELAX_GYRO ) { - itermErrorRate = fapplyDeadband(setpointLpf - gyroRate, setpointHpf); - } - - if (axis == FD_ROLL) { - DEBUG_SET(DEBUG_ITERM_RELAX, 0, lrintf(setpointHpf)); - DEBUG_SET(DEBUG_ITERM_RELAX, 1, lrintf(itermRelaxFactor * 100.0f)); - DEBUG_SET(DEBUG_ITERM_RELAX, 2, lrintf(itermErrorRate)); - } - -#if defined(USE_ABSOLUTE_CONTROL) - const float gmaxac = setpointLpf + 2 * setpointHpf; - const float gminac = setpointLpf - 2 * setpointHpf; - if (gyroRate >= gminac && gyroRate <= gmaxac) { - float acErrorRate1 = gmaxac - gyroRate; - float acErrorRate2 = gminac - gyroRate; - if (acErrorRate1 * axisError[axis] < 0) { - acErrorRate = acErrorRate1; - } else { - acErrorRate = acErrorRate2; - } - if (fabsf(acErrorRate * dT) > fabsf(axisError[axis]) ) { - acErrorRate = -axisError[axis] / dT; - } - } else { - acErrorRate = (gyroRate > gmaxac ? gmaxac : gminac ) - gyroRate; - } -#endif // USE_ABSOLUTE_CONTROL - } else -#endif // USE_ITERM_RELAX - { - itermErrorRate = currentPidSetpoint - gyroRate; -#if defined(USE_ABSOLUTE_CONTROL) - acErrorRate = itermErrorRate; -#endif // USE_ABSOLUTE_CONTROL - } - -#if defined(USE_ABSOLUTE_CONTROL) - if (acGain > 0 && isAirmodeActivated()) { - axisError[axis] = constrainf(axisError[axis] + acErrorRate * dT, -acErrorLimit, acErrorLimit); - acCorrection = constrainf(axisError[axis] * acGain, -acLimit, acLimit); - currentPidSetpoint += acCorrection; - itermErrorRate += acCorrection; - if (axis == FD_ROLL) { - DEBUG_SET(DEBUG_ITERM_RELAX, 3, lrintf(axisError[axis] * 10)); - } - } -#endif - float errorRate = currentPidSetpoint - gyroRate; // r - y handleCrashRecovery( pidProfile->crash_recovery, angleTrim, axis, currentTimeUs, gyroRate, ¤tPidSetpoint, &errorRate); + const float iterm = pidData[axis].I; + float itermErrorRate = errorRate; + +#if defined(USE_ITERM_RELAX) + applyItermRelax(axis, iterm, gyroRate, &itermErrorRate, ¤tPidSetpoint); +#endif + // --------low-level gyro-based PID based on 2DOF PID controller. ---------- // 2-DOF PID controller with optional filter on derivative term. - // b = 1 and only c (dtermSetpointWeight) can be tuned (amount derivative on measurement or error). + // b = 1 and only c (feedforward weight) can be tuned (amount derivative on measurement or error). - // -----calculate P component and add Dynamic Part based on stick input + // -----calculate P component pidData[axis].P = pidCoefficient[axis].Kp * errorRate * tpaFactor; if (axis == FD_YAW) { pidData[axis].P = ptermYawLowpassApplyFn((filter_t *) &ptermYawLowpass, pidData[axis].P); } // -----calculate I component - - const float ITerm = pidData[axis].I; - const float ITermNew = constrainf(ITerm + pidCoefficient[axis].Ki * itermErrorRate * dynCi, -itermLimit, itermLimit); - const bool outputSaturated = mixerIsOutputSaturated(axis, errorRate); - if (outputSaturated == false || ABS(ITermNew) < ABS(ITerm)) { - // Only increase ITerm if output is not saturated - pidData[axis].I = ITermNew; - } + pidData[axis].I = constrainf(iterm + pidCoefficient[axis].Ki * itermErrorRate * dynCi, -itermLimit, itermLimit); // -----calculate D component - if (axis != FD_YAW) { - // no transition if relaxFactor == 0 - float transition = relaxFactor > 0 ? MIN(1.f, getRcDeflectionAbs(axis) * relaxFactor) : 1; + if (pidCoefficient[axis].Kd > 0) { // Divide rate change by dT to get differential (ie dr/dt). // dT is fixed and calculated from the target PID loop time @@ -908,73 +1082,52 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT detectAndSetCrashRecovery(pidProfile->crash_recovery, axis, currentTimeUs, delta, errorRate); pidData[axis].D = pidCoefficient[axis].Kd * delta * tpaFactor; + } else { + pidData[axis].D = 0; + } + previousGyroRateDterm[axis] = gyroRateDterm[axis]; + + // -----calculate feedforward component + + // Only enable feedforward for rate mode + const float feedforwardGain = flightModeFlags ? 0.0f : pidCoefficient[axis].Kf; + + if (feedforwardGain > 0) { + + // no transition if feedForwardTransition == 0 + float transition = feedForwardTransition > 0 ? MIN(1.f, getRcDeflectionAbs(axis) * feedForwardTransition) : 1; float pidSetpointDelta = currentPidSetpoint - previousPidSetpoint[axis]; #ifdef USE_RC_SMOOTHING_FILTER - if (axis == rcSmoothingDebugAxis) { - DEBUG_SET(DEBUG_RC_SMOOTHING, 1, lrintf(pidSetpointDelta * 100.0f)); - } - if ((dynCd != 0) && setpointDerivativeLpfInitialized) { - switch (rcSmoothingFilterType) { - case RC_SMOOTHING_DERIVATIVE_PT1: - pidSetpointDelta = pt1FilterApply(&setpointDerivativePt1[axis], pidSetpointDelta); - break; - case RC_SMOOTHING_DERIVATIVE_BIQUAD: - pidSetpointDelta = biquadFilterApply(&setpointDerivativeBiquad[axis], pidSetpointDelta); - break; - } - if (axis == rcSmoothingDebugAxis) { - DEBUG_SET(DEBUG_RC_SMOOTHING, 2, lrintf(pidSetpointDelta * 100.0f)); - } - } + pidSetpointDelta = applyRcSmoothingDerivativeFilter(axis, pidSetpointDelta); #endif // USE_RC_SMOOTHING_FILTER - const float pidFeedForward = - pidCoefficient[axis].Kd * dynCd * transition * pidSetpointDelta * tpaFactor * pidFrequency; + pidData[axis].F = feedforwardGain * transition * pidSetpointDelta * pidFrequency; + #if defined(USE_SMART_FEEDFORWARD) - bool addFeedforward = true; - if (smartFeedforward) { - if (pidData[axis].P * pidFeedForward > 0) { - if (ABS(pidFeedForward) > ABS(pidData[axis].P)) { - pidData[axis].P = 0; - } else { - addFeedforward = false; - } - } - } - if (addFeedforward) + applySmartFeedforward(axis); #endif - { - pidData[axis].D += pidFeedForward; - } - previousGyroRateDterm[axis] = gyroRateDterm[axis]; - previousPidSetpoint[axis] = currentPidSetpoint; + } else { + pidData[axis].F = 0; + } + previousPidSetpoint[axis] = currentPidSetpoint; #ifdef USE_YAW_SPIN_RECOVERY - if (yawSpinActive) { + if (yawSpinActive) { + pidData[axis].I = 0; // in yaw spin always disable I + if (axis <= FD_PITCH) { // zero PIDs on pitch and roll leaving yaw P to correct spin pidData[axis].P = 0; - pidData[axis].I = 0; pidData[axis].D = 0; + pidData[axis].F = 0; } -#endif // USE_YAW_SPIN_RECOVERY } - } - - // calculating the PID sum - pidData[FD_ROLL].Sum = pidData[FD_ROLL].P + pidData[FD_ROLL].I + pidData[FD_ROLL].D; - pidData[FD_PITCH].Sum = pidData[FD_PITCH].P + pidData[FD_PITCH].I + pidData[FD_PITCH].D; - -#ifdef USE_YAW_SPIN_RECOVERY - if (yawSpinActive) { - // yaw P alone to correct spin - pidData[FD_YAW].I = 0; - } #endif // USE_YAW_SPIN_RECOVERY - // YAW has no D - pidData[FD_YAW].Sum = pidData[FD_YAW].P + pidData[FD_YAW].I; + // calculating the PID sum + pidData[axis].Sum = pidData[axis].P + pidData[axis].I + pidData[axis].D + pidData[axis].F; + } // Disable PID control if at zero throttle or if gyro overflow detected // This may look very innefficient, but it is done on purpose to always show real CPU usage as in flight @@ -983,6 +1136,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT pidData[axis].P = 0; pidData[axis].I = 0; pidData[axis].D = 0; + pidData[axis].F = 0; pidData[axis].Sum = 0; } @@ -1005,3 +1159,40 @@ void pidSetAcroTrainerState(bool newState) } } #endif // USE_ACRO_TRAINER + +void pidSetAntiGravityState(bool newState) +{ + if (newState != antiGravityEnabled) { + // reset the accelerator on state changes + itermAccelerator = 1.0f; + } + antiGravityEnabled = newState; +} + +bool pidAntiGravityEnabled(void) +{ + return antiGravityEnabled; +} + +#ifdef USE_DYN_LPF +void dynLpfDTermUpdate(float throttle) +{ + if (dynLpfFilter != DYN_LPF_NONE) { + uint16_t cutoffFreq = dynLpfMin; + if (throttle > dynLpfIdle) { + const float dynThrottle = (throttle - (throttle * throttle * throttle) / 3.0f) * 1.5f; + cutoffFreq += (dynThrottle - dynLpfIdlePoint) * dynLpfInvIdlePointScaled; + } + + if (dynLpfFilter == DYN_LPF_PT1) { + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + pt1FilterUpdateCutoff(&dtermLowpass[axis].pt1Filter, pt1FilterGain(cutoffFreq, dT)); + } + } else { + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + biquadFilterUpdateLPF(&dtermLowpass[axis].biquadFilter, cutoffFreq, targetPidLooptime); + } + } + } +} +#endif diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index d99e870824..d5c6536883 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -23,6 +23,7 @@ #include #include "common/time.h" #include "common/filter.h" +#include "common/axis.h" #include "pg/pg.h" #define MAX_PID_PROCESS_DENOM 16 @@ -39,17 +40,18 @@ #define ITERM_SCALE 0.244381f #define DTERM_SCALE 0.000529f +// The constant scale factor to replace the Kd component of the feedforward calculation. +// This value gives the same "feel" as the previous Kd default of 26 (26 * DTERM_SCALE) +#define FEEDFORWARD_SCALE 0.013754f + +#define ITERM_RELAX_SETPOINT_THRESHOLD 30.0f + typedef enum { PID_ROLL, PID_PITCH, PID_YAW, - PID_ALT, - PID_POS, - PID_POSR, - PID_NAVR, PID_LEVEL, PID_MAG, - PID_VEL, PID_ITEM_COUNT } pidIndex_e; @@ -70,16 +72,24 @@ typedef enum { PID_CRASH_RECOVERY_BEEP } pidCrashRecovery_e; -typedef struct pid8_s { +typedef struct pidf_s { uint8_t P; uint8_t I; uint8_t D; -} pid8_t; + uint16_t F; +} pidf_t; + +typedef enum { + ANTI_GRAVITY_SMOOTH, + ANTI_GRAVITY_STEP +} antiGravityMode_e; typedef enum { ITERM_RELAX_OFF, ITERM_RELAX_RP, - ITERM_RELAX_RPY + ITERM_RELAX_RPY, + ITERM_RELAX_RP_INC, + ITERM_RELAX_RPY_INC } itermRelax_e; typedef enum { @@ -93,10 +103,10 @@ typedef struct pidProfile_s { uint16_t dterm_notch_hz; // Biquad dterm notch hz uint16_t dterm_notch_cutoff; // Biquad dterm notch low cutoff - pid8_t pid[PID_ITEM_COUNT]; + pidf_t pid[PID_ITEM_COUNT]; uint8_t dterm_filter_type; // Filter selection for dterm - uint8_t itermWindupPointPercent; // Experimental ITerm windup threshold, percent motor saturation + uint8_t itermWindupPointPercent; // iterm windup threshold, percent motor saturation uint16_t pidSumLimit; uint16_t pidSumLimitYaw; uint8_t pidAtMinThrottle; // Disable/Enable pids on zero throttle. Normally even without airmode P and D would be active. @@ -106,9 +116,9 @@ typedef struct pidProfile_s { uint8_t horizon_tilt_expert_mode; // OFF or ON // Betaflight PID controller parameters + uint8_t antiGravityMode; // type of anti gravity method uint16_t itermThrottleThreshold; // max allowed throttle delta before iterm accelerated in ms uint16_t itermAcceleratorGain; // Iterm Accelerator Gain when itermThrottlethreshold is hit - uint16_t dtermSetpointWeight; // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > aggressive derivative) uint16_t yawRateAccelLimit; // yaw accel limiter for deg/sec/ms uint16_t rateAccelLimit; // accel limiter roll/pitch deg/sec/ms uint16_t crash_dthreshold; // dterm crash value @@ -119,15 +129,15 @@ typedef struct pidProfile_s { uint8_t crash_recovery_angle; // degrees uint8_t crash_recovery_rate; // degree/second uint8_t vbatPidCompensation; // Scale PIDsum to battery voltage - uint8_t setpointRelaxRatio; // Setpoint weight relaxation effect + uint8_t feedForwardTransition; // Feed forward weight transition uint16_t crash_limit_yaw; // limits yaw errorRate, so crashes don't cause huge throttle increase uint16_t itermLimit; uint16_t dterm_lowpass2_hz; // Extra PT1 Filter on D in hz uint8_t crash_recovery; // off, on, on and beeps when it is in crash recovery mode uint8_t throttle_boost; // how much should throttle be boosted during transient changes 0-100, 100 adds 10x hpf filtered throttle uint8_t throttle_boost_cutoff; // Which cutoff frequency to use for throttle boost. higher cutoffs keep the boost on for shorter. Specified in hz. - uint8_t iterm_rotation; // rotates iterm to translate world errors to local coordinate system - uint8_t smart_feedforward; // takes only the larger of P and the D weight feed forward term if they have the same sign. + uint8_t iterm_rotation; // rotates iterm to translate world errors to local coordinate system + uint8_t smart_feedforward; // takes only the larger of P and the D weight feed forward term if they have the same sign. uint8_t iterm_relax_type; // Specifies type of relax algorithm uint8_t iterm_relax_cutoff; // This cutoff frequency specifies a low pass filter which predicts average response of the quad to setpoint uint8_t iterm_relax; // Enable iterm suppression during stick input @@ -138,11 +148,12 @@ typedef struct pidProfile_s { uint8_t abs_control_gain; // How strongly should the absolute accumulated error be corrected for uint8_t abs_control_limit; // Limit to the correction uint8_t abs_control_error_limit; // Limit to the accumulated error + uint8_t dterm_filter2_type; // Filter selection for 2nd dterm + uint16_t dyn_lpf_dterm_max_hz; + uint8_t dyn_lpf_dterm_idle; } pidProfile_t; -#ifndef USE_OSD_SLAVE PG_DECLARE_ARRAY(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles); -#endif typedef struct pidConfig_s { uint8_t pid_process_denom; // Processing denominator for PID controller vs gyro sampling rate @@ -160,10 +171,13 @@ typedef struct pidAxisData_s { float P; float I; float D; + float F; float Sum; } pidAxisData_t; +extern const char pidNames[]; + extern pidAxisData_t pidData[3]; extern uint32_t targetPidLooptime; @@ -171,10 +185,9 @@ extern uint32_t targetPidLooptime; extern float throttleBoost; extern pt1Filter_t throttleLpf; -void pidResetITerm(void); +void pidResetIterm(void); void pidStabilisationState(pidStabilisationState_e pidControllerState); void pidSetItermAccelerator(float newItermAccelerator); -float pidItermAccelerator(void); void pidInitFilters(const pidProfile_t *pidProfile); void pidInitConfig(const pidProfile_t *pidProfile); void pidInit(const pidProfile_t *pidProfile); @@ -183,3 +196,25 @@ bool crashRecoveryModeActive(void); void pidAcroTrainerInit(void); void pidSetAcroTrainerState(bool newState); void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint8_t filterType); +void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff); +void pidUpdateAntiGravityThrottleFilter(float throttle); +bool pidOsdAntiGravityActive(void); +bool pidOsdAntiGravityMode(void); +void pidSetAntiGravityState(bool newState); +bool pidAntiGravityEnabled(void); + +#ifdef UNIT_TEST +#include "sensors/acceleration.h" +extern float axisError[XYZ_AXIS_COUNT]; +void applyItermRelax(const int axis, const float iterm, + const float gyroRate, float *itermErrorRate, float *currentPidSetpoint); +void applyAbsoluteControl(const int axis, const float gyroRate, const bool itermRelaxIsEnabled, + const float setpointLpf, const float setpointHpf, + float *currentPidSetpoint, float *itermErrorRate); +void rotateItermAndAxisError(); +float pidLevel(int axis, const pidProfile_t *pidProfile, + const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint); +float calcHorizonLevelStrength(void); +#endif +void dynLpfDTermUpdate(float throttle); + diff --git a/src/main/flight/position.c b/src/main/flight/position.c index 8d510e7f15..99b2818d32 100644 --- a/src/main/flight/position.c +++ b/src/main/flight/position.c @@ -22,6 +22,7 @@ #include #include #include +#include #include "build/debug.h" @@ -38,18 +39,41 @@ #include "sensors/sensors.h" #include "sensors/barometer.h" -static int32_t estimatedAltitude = 0; // in cm +static int32_t estimatedAltitudeCm = 0; // in cm #define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25) +#ifdef USE_VARIO +static int16_t estimatedVario = 0; // in cm/s + +int16_t calculateEstimatedVario(int32_t baroAlt, const uint32_t dTime) { + static float vel = 0; + static int32_t lastBaroAlt = 0; + + int32_t baroVel = 0; + + baroVel = (baroAlt - lastBaroAlt) * 1000000.0f / dTime; + lastBaroAlt = baroAlt; + + baroVel = constrain(baroVel, -1500.0f, 1500.0f); + baroVel = applyDeadband(baroVel, 10.0f); + + vel = vel * CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_vel) + baroVel * (1.0f - CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_vel)); + int32_t vel_tmp = lrintf(vel); + vel_tmp = applyDeadband(vel_tmp, 5.0f); + + return constrain(vel_tmp, SHRT_MIN, SHRT_MAX); +} +#endif #if defined(USE_BARO) || defined(USE_GPS) +static bool altitudeOffsetSet = false; + void calculateEstimatedAltitude(timeUs_t currentTimeUs) { static timeUs_t previousTimeUs = 0; static int32_t baroAltOffset = 0; static int32_t gpsAltOffset = 0; - static bool altitudeOffsetSet = false; const uint32_t dTime = currentTimeUs - previousTimeUs; if (dTime < BARO_UPDATE_FREQUENCY_40HZ) { @@ -76,7 +100,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs) #ifdef USE_GPS if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { - gpsAlt = gpsSol.llh.alt; + gpsAlt = gpsSol.llh.altCm; haveGpsAlt = true; if (gpsSol.hdop != 0) { @@ -98,27 +122,41 @@ if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { gpsAlt -= gpsAltOffset; if (haveGpsAlt && haveBaroAlt) { - estimatedAltitude = gpsAlt * gpsTrust + baroAlt * (1 - gpsTrust); + estimatedAltitudeCm = gpsAlt * gpsTrust + baroAlt * (1 - gpsTrust); +#ifdef USE_VARIO + estimatedVario = calculateEstimatedVario(baroAlt, dTime); +#endif } else if (haveGpsAlt) { - estimatedAltitude = gpsAlt; + estimatedAltitudeCm = gpsAlt; } else if (haveBaroAlt) { - estimatedAltitude = baroAlt; + estimatedAltitudeCm = baroAlt; +#ifdef USE_VARIO + estimatedVario = calculateEstimatedVario(baroAlt, dTime); +#endif } DEBUG_SET(DEBUG_ALTITUDE, 0, (int32_t)(100 * gpsTrust)); DEBUG_SET(DEBUG_ALTITUDE, 1, baroAlt); DEBUG_SET(DEBUG_ALTITUDE, 2, gpsAlt); } + +bool isAltitudeOffset(void) +{ + return altitudeOffsetSet; +} #endif - -int32_t getEstimatedAltitude(void) +int32_t getEstimatedAltitudeCm(void) { - return estimatedAltitude; + return estimatedAltitudeCm; } // This should be removed or fixed, but it would require changing a lot of other things to get rid of. int16_t getEstimatedVario(void) { +#ifdef USE_VARIO + return estimatedVario; +#else return 0; +#endif } diff --git a/src/main/flight/position.h b/src/main/flight/position.h index af7a3b3eca..bf821ca6a7 100644 --- a/src/main/flight/position.h +++ b/src/main/flight/position.h @@ -22,6 +22,7 @@ #include "common/time.h" +bool isAltitudeOffset(void); void calculateEstimatedAltitude(timeUs_t currentTimeUs); -int32_t getEstimatedAltitude(void); -int16_t getEstimatedVario(void); \ No newline at end of file +int32_t getEstimatedAltitudeCm(void); +int16_t getEstimatedVario(void); diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index d1e75d0b1a..82ac0df4f3 100644 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -224,7 +224,7 @@ void servosInit(void) // enable servos for mixes that require them. note, this shifts motor counts. useServo = mixers[currentMixerMode].useServo; // if we want camstab/trig, that also enables servos, even if mixer doesn't - if (feature(FEATURE_SERVO_TILT) || feature(FEATURE_CHANNEL_FORWARDING)) { + if (featureIsEnabled(FEATURE_SERVO_TILT) || featureIsEnabled(FEATURE_CHANNEL_FORWARDING)) { useServo = 1; } @@ -377,13 +377,13 @@ void writeServos(void) } // Two servos for SERVO_TILT, if enabled - if (feature(FEATURE_SERVO_TILT) || currentMixerMode == MIXER_GIMBAL) { + if (featureIsEnabled(FEATURE_SERVO_TILT) || currentMixerMode == MIXER_GIMBAL) { updateGimbalServos(servoIndex); servoIndex += 2; } // forward AUX to remaining servo outputs (not constrained) - if (feature(FEATURE_CHANNEL_FORWARDING)) { + if (featureIsEnabled(FEATURE_CHANNEL_FORWARDING)) { forwardAuxChannelsToServos(servoIndex); servoIndex += MAX_AUX_CHANNEL_COUNT; } @@ -406,7 +406,7 @@ void servoMixer(void) input[INPUT_STABILIZED_YAW] = pidData[FD_YAW].Sum * PID_SERVO_MIXER_SCALING; // Reverse yaw servo when inverted in 3D mode - if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig()->midrc)) { + if (featureIsEnabled(FEATURE_3D) && (rcData[THROTTLE] < rxConfig()->midrc)) { input[INPUT_STABILIZED_YAW] *= -1; } } @@ -498,7 +498,7 @@ static void servoTable(void) } // camera stabilization - if (feature(FEATURE_SERVO_TILT)) { + if (featureIsEnabled(FEATURE_SERVO_TILT)) { // center at fixed position, or vary either pitch or roll by RC channel servo[SERVO_GIMBAL_PITCH] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH); servo[SERVO_GIMBAL_ROLL] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL); diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h index 409b8c3177..683918663c 100644 --- a/src/main/flight/servos.h +++ b/src/main/flight/servos.h @@ -41,7 +41,7 @@ enum { INPUT_GIMBAL_PITCH, INPUT_GIMBAL_ROLL, INPUT_SOURCE_COUNT -} inputSource_e; +}; // target servo channels typedef enum { diff --git a/src/main/interface/cli.c b/src/main/interface/cli.c index 175e378999..dc3f000b94 100644 --- a/src/main/interface/cli.c +++ b/src/main/interface/cli.c @@ -11,7 +11,7 @@ * 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. * @@ -62,34 +62,34 @@ extern uint8_t __config_end; #include "drivers/adc.h" #include "drivers/buf_writer.h" #include "drivers/bus_spi.h" +#include "drivers/camera_control.h" #include "drivers/compass/compass.h" #include "drivers/display.h" #include "drivers/dma.h" #include "drivers/flash.h" +#include "drivers/inverter.h" #include "drivers/io.h" #include "drivers/io_impl.h" -#include "drivers/inverter.h" +#include "drivers/light_led.h" +#include "drivers/rangefinder/rangefinder_hcsr04.h" #include "drivers/sdcard.h" #include "drivers/sensor.h" #include "drivers/serial.h" #include "drivers/serial_escserial.h" -#include "drivers/rangefinder/rangefinder_hcsr04.h" #include "drivers/sound_beeper.h" #include "drivers/stack_check.h" #include "drivers/system.h" -#include "drivers/transponder_ir.h" #include "drivers/time.h" #include "drivers/timer.h" -#include "drivers/light_led.h" -#include "drivers/camera_control.h" -#include "drivers/vtx_common.h" +#include "drivers/transponder_ir.h" #include "drivers/usb_msc.h" +#include "drivers/vtx_common.h" #include "fc/board_info.h" #include "fc/config.h" #include "fc/controlrate_profile.h" -#include "fc/fc_core.h" -#include "fc/fc_rc.h" +#include "fc/core.h" +#include "fc/rc.h" #include "fc/rc_adjustments.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" @@ -116,6 +116,7 @@ extern uint8_t __config_end; #include "io/osd.h" #include "io/serial.h" #include "io/transponder_ir.h" +#include "io/usb_msc.h" #include "io/vtx_control.h" #include "io/vtx.h" @@ -125,6 +126,7 @@ extern uint8_t __config_end; #include "pg/board.h" #include "pg/bus_i2c.h" #include "pg/bus_spi.h" +#include "pg/gyrodev.h" #include "pg/max7456.h" #include "pg/pinio.h" #include "pg/pg.h" @@ -139,6 +141,7 @@ extern uint8_t __config_end; #include "rx/spektrum.h" #include "rx/cc2500_frsky_common.h" #include "rx/cc2500_frsky_x.h" +#include "rx/cc2500_common.h" #include "scheduler/scheduler.h" @@ -178,6 +181,9 @@ static bool configIsInCopy = false; static int8_t pidProfileIndexToUse = CURRENT_PROFILE_INDEX; static int8_t rateProfileIndexToUse = CURRENT_PROFILE_INDEX; +static bool featureMaskIsCopied = false; +static uint32_t featureMaskCopy; + #if defined(USE_BOARD_INFO) static bool boardInformationUpdated = false; #if defined(USE_SIGNATURE) @@ -372,14 +378,18 @@ static void cliPrintLinef(const char *format, ...) static void cliPrintErrorLinef(const char *format, ...) { - cliPrint("###ERROR### "); + cliPrint("###"); va_list va; va_start(va, format); cliPrintfva(format, va); va_end(va); - cliPrintLinefeed(); + cliPrintLine("###"); } +static void cliPrintCorruptMessage(int value) +{ + cliPrintf("%d ###CORRUPTED CONFIG###", value); +} static void printValuePointer(const clivalue_t *var, const void *valuePointer, bool full) { @@ -435,13 +445,21 @@ static void printValuePointer(const clivalue_t *var, const void *valuePointer, b switch (var->type & VALUE_MODE_MASK) { case MODE_DIRECT: - cliPrintf("%d", value); - if (full) { - cliPrintf(" %d %d", var->config.minmax.min, var->config.minmax.max); + if ((value < var->config.minmax.min) || (value > var->config.minmax.max)) { + cliPrintCorruptMessage(value); + } else { + cliPrintf("%d", value); + if (full) { + cliPrintf(" %d %d", var->config.minmax.min, var->config.minmax.max); + } } break; case MODE_LOOKUP: - cliPrint(lookupTables[var->config.lookup.tableIndex].values[value]); + if (value < lookupTables[var->config.lookup.tableIndex].valueCount) { + cliPrint(lookupTables[var->config.lookup.tableIndex].values[value]); + } else { + cliPrintCorruptMessage(value); + } break; case MODE_BITSET: if (value & 1 << var->config.bitpos) { @@ -684,12 +702,12 @@ static void cliPrompt(void) static void cliShowParseError(void) { - cliPrintErrorLinef("Parse error"); + cliPrintErrorLinef("PARSE ERROR"); } static void cliShowArgumentRangeError(char *name, int min, int max) { - cliPrintErrorLinef("%s not between %d and %d", name, min, max); + cliPrintErrorLinef("%s NOT BETWEEN %d AND %d", name, min, max); } static const char *nextArg(const char *currentArg) @@ -735,8 +753,7 @@ static void printRxFailsafe(uint8_t dumpMask, const rxFailsafeChannelConfig_t *r for (uint32_t channel = 0; channel < MAX_SUPPORTED_RC_CHANNEL_COUNT; channel++) { const rxFailsafeChannelConfig_t *channelFailsafeConfig = &rxFailsafeChannelConfigs[channel]; const rxFailsafeChannelConfig_t *defaultChannelFailsafeConfig = &defaultRxFailsafeChannelConfigs[channel]; - const bool equalsDefault = channelFailsafeConfig->mode == defaultChannelFailsafeConfig->mode - && channelFailsafeConfig->step == defaultChannelFailsafeConfig->step; + const bool equalsDefault = !memcmp(channelFailsafeConfig, defaultChannelFailsafeConfig, sizeof(*channelFailsafeConfig)); const bool requireValue = channelFailsafeConfig->mode == RX_FAILSAFE_MODE_SET; if (requireValue) { const char *format = "rxfail %u %c %d"; @@ -841,26 +858,23 @@ static void cliRxFailsafe(char *cmdline) ); } } else { - cliShowArgumentRangeError("channel", 0, MAX_SUPPORTED_RC_CHANNEL_COUNT - 1); + cliShowArgumentRangeError("CHANNEL", 0, MAX_SUPPORTED_RC_CHANNEL_COUNT - 1); } } } static void printAux(uint8_t dumpMask, const modeActivationCondition_t *modeActivationConditions, const modeActivationCondition_t *defaultModeActivationConditions) { - const char *format = "aux %u %u %u %u %u %u"; + const char *format = "aux %u %u %u %u %u %u %u"; // print out aux channel settings for (uint32_t i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { const modeActivationCondition_t *mac = &modeActivationConditions[i]; bool equalsDefault = false; if (defaultModeActivationConditions) { const modeActivationCondition_t *macDefault = &defaultModeActivationConditions[i]; - equalsDefault = mac->modeId == macDefault->modeId - && mac->auxChannelIndex == macDefault->auxChannelIndex - && mac->range.startStep == macDefault->range.startStep - && mac->range.endStep == macDefault->range.endStep - && mac->modeLogic == macDefault->modeLogic; + equalsDefault = !memcmp(mac, macDefault, sizeof(*mac)); const box_t *box = findBoxByBoxId(macDefault->modeId); + const box_t *linkedTo = findBoxByBoxId(macDefault->linkedTo); if (box) { cliDefaultPrintLinef(dumpMask, equalsDefault, format, i, @@ -868,11 +882,13 @@ static void printAux(uint8_t dumpMask, const modeActivationCondition_t *modeActi macDefault->auxChannelIndex, MODE_STEP_TO_CHANNEL_VALUE(macDefault->range.startStep), MODE_STEP_TO_CHANNEL_VALUE(macDefault->range.endStep), - macDefault->modeLogic + macDefault->modeLogic, + linkedTo ? linkedTo->permanentId : 0 ); } } const box_t *box = findBoxByBoxId(mac->modeId); + const box_t *linkedTo = findBoxByBoxId(mac->linkedTo); if (box) { cliDumpPrintLinef(dumpMask, equalsDefault, format, i, @@ -880,7 +896,8 @@ static void printAux(uint8_t dumpMask, const modeActivationCondition_t *modeActi mac->auxChannelIndex, MODE_STEP_TO_CHANNEL_VALUE(mac->range.startStep), MODE_STEP_TO_CHANNEL_VALUE(mac->range.endStep), - mac->modeLogic + mac->modeLogic, + linkedTo ? linkedTo->permanentId : 0 ); } } @@ -925,21 +942,33 @@ static void cliAux(char *cmdline) validArgumentCount++; } } + ptr = nextArg(ptr); + if (ptr) { + val = atoi(ptr); + const box_t *box = findBoxByPermanentId(val); + if (box) { + mac->linkedTo = box->boxId; + validArgumentCount++; + } + } if (validArgumentCount == 4) { // for backwards compatibility mac->modeLogic = MODELOGIC_OR; - } else if (validArgumentCount != 5) { + } else if (validArgumentCount == 5) { // for backwards compatibility + mac->linkedTo = 0; + } else if (validArgumentCount != 6) { memset(mac, 0, sizeof(modeActivationCondition_t)); } - cliPrintLinef( "aux %u %u %u %u %u %u", + cliPrintLinef( "aux %u %u %u %u %u %u %u", i, mac->modeId, mac->auxChannelIndex, MODE_STEP_TO_CHANNEL_VALUE(mac->range.startStep), MODE_STEP_TO_CHANNEL_VALUE(mac->range.endStep), - mac->modeLogic + mac->modeLogic, + mac->linkedTo ); } else { - cliShowArgumentRangeError("index", 0, MAX_MODE_ACTIVATION_CONDITION_COUNT - 1); + cliShowArgumentRangeError("INDEX", 0, MAX_MODE_ACTIVATION_CONDITION_COUNT - 1); } } } @@ -953,12 +982,7 @@ static void printSerial(uint8_t dumpMask, const serialConfig_t *serialConfig, co }; bool equalsDefault = false; if (serialConfigDefault) { - equalsDefault = serialConfig->portConfigs[i].identifier == serialConfigDefault->portConfigs[i].identifier - && serialConfig->portConfigs[i].functionMask == serialConfigDefault->portConfigs[i].functionMask - && serialConfig->portConfigs[i].msp_baudrateIndex == serialConfigDefault->portConfigs[i].msp_baudrateIndex - && serialConfig->portConfigs[i].gps_baudrateIndex == serialConfigDefault->portConfigs[i].gps_baudrateIndex - && serialConfig->portConfigs[i].telemetry_baudrateIndex == serialConfigDefault->portConfigs[i].telemetry_baudrateIndex - && serialConfig->portConfigs[i].blackbox_baudrateIndex == serialConfigDefault->portConfigs[i].blackbox_baudrateIndex; + equalsDefault = !memcmp(&serialConfig->portConfigs[i], &serialConfigDefault->portConfigs[i], sizeof(serialConfig->portConfigs[i])); cliDefaultPrintLinef(dumpMask, equalsDefault, format, serialConfigDefault->portConfigs[i].identifier, serialConfigDefault->portConfigs[i].functionMask, @@ -1070,7 +1094,7 @@ static void cliSerial(char *cmdline) } -#ifndef SKIP_SERIAL_PASSTHROUGH +#if defined(USE_SERIAL_PASSTHROUGH) #ifdef USE_PINIO static void cbCtrlLine(void *context, uint16_t ctrl) { @@ -1207,14 +1231,7 @@ static void printAdjustmentRange(uint8_t dumpMask, const adjustmentRange_t *adju bool equalsDefault = false; if (defaultAdjustmentRanges) { const adjustmentRange_t *arDefault = &defaultAdjustmentRanges[i]; - equalsDefault = ar->auxChannelIndex == arDefault->auxChannelIndex - && ar->range.startStep == arDefault->range.startStep - && ar->range.endStep == arDefault->range.endStep - && ar->adjustmentFunction == arDefault->adjustmentFunction - && ar->auxSwitchChannelIndex == arDefault->auxSwitchChannelIndex - && ar->adjustmentIndex == arDefault->adjustmentIndex - && ar->adjustmentCenter == arDefault->adjustmentCenter - && ar->adjustmentScale == arDefault->adjustmentScale; + equalsDefault = !memcmp(ar, arDefault, sizeof(*ar)); cliDefaultPrintLinef(dumpMask, equalsDefault, format, i, arDefault->adjustmentIndex, @@ -1314,6 +1331,9 @@ static void cliAdjustmentRange(char *cmdline) ar->adjustmentScale = val; validArgumentCount++; } + + activeAdjustmentRangeReset(); + cliDumpPrintLinef(0, false, format, i, ar->adjustmentIndex, @@ -1327,7 +1347,7 @@ static void cliAdjustmentRange(char *cmdline) ); } else { - cliShowArgumentRangeError("index", 0, MAX_ADJUSTMENT_RANGE_COUNT - 1); + cliShowArgumentRangeError("INDEX", 0, MAX_ADJUSTMENT_RANGE_COUNT - 1); } } } @@ -1394,7 +1414,7 @@ static void cliMotorMix(char *cmdline) len = strlen(ptr); for (uint32_t i = 0; ; i++) { if (mixerNames[i] == NULL) { - cliPrintErrorLinef("Invalid name"); + cliPrintErrorLinef("INVALID NAME"); break; } if (strncasecmp(ptr, mixerNames[i], len) == 0) { @@ -1435,7 +1455,7 @@ static void cliMotorMix(char *cmdline) printMotorMix(DUMP_MASTER, customMotorMixer(0), NULL); } } else { - cliShowArgumentRangeError("index", 0, MAX_SUPPORTED_MOTORS - 1); + cliShowArgumentRangeError("INDEX", 0, MAX_SUPPORTED_MOTORS - 1); } } #endif @@ -1447,8 +1467,7 @@ static void printRxRange(uint8_t dumpMask, const rxChannelRangeConfig_t *channel for (uint32_t i = 0; i < NON_AUX_CHANNEL_COUNT; i++) { bool equalsDefault = false; if (defaultChannelRangeConfigs) { - equalsDefault = channelRangeConfigs[i].min == defaultChannelRangeConfigs[i].min - && channelRangeConfigs[i].max == defaultChannelRangeConfigs[i].max; + equalsDefault = !memcmp(&channelRangeConfigs[i], &defaultChannelRangeConfigs[i], sizeof(channelRangeConfigs[i])); cliDefaultPrintLinef(dumpMask, equalsDefault, format, i, defaultChannelRangeConfigs[i].min, @@ -1507,7 +1526,7 @@ static void cliRxRange(char *cmdline) } } else { - cliShowArgumentRangeError("channel", 0, NON_AUX_CHANNEL_COUNT - 1); + cliShowArgumentRangeError("CHANNEL", 0, NON_AUX_CHANNEL_COUNT - 1); } } } @@ -1553,7 +1572,7 @@ static void cliLed(char *cmdline) cliShowParseError(); } } else { - cliShowArgumentRangeError("index", 0, LED_MAX_STRIP_LENGTH - 1); + cliShowArgumentRangeError("INDEX", 0, LED_MAX_STRIP_LENGTH - 1); } } } @@ -1566,9 +1585,7 @@ static void printColor(uint8_t dumpMask, const hsvColor_t *colors, const hsvColo bool equalsDefault = false; if (defaultColors) { const hsvColor_t *colorDefault = &defaultColors[i]; - equalsDefault = color->h == colorDefault->h - && color->s == colorDefault->s - && color->v == colorDefault->v; + equalsDefault = !memcmp(color, colorDefault, sizeof(*color)); cliDefaultPrintLinef(dumpMask, equalsDefault, format, i,colorDefault->h, colorDefault->s, colorDefault->v); } cliDumpPrintLinef(dumpMask, equalsDefault, format, i, color->h, color->s, color->v); @@ -1592,7 +1609,7 @@ static void cliColor(char *cmdline) cliShowParseError(); } } else { - cliShowArgumentRangeError("index", 0, LED_CONFIGURABLE_COLOR_COUNT - 1); + cliShowArgumentRangeError("INDEX", 0, LED_CONFIGURABLE_COLOR_COUNT - 1); } } } @@ -1677,11 +1694,7 @@ static void printServo(uint8_t dumpMask, const servoParam_t *servoParams, const bool equalsDefault = false; if (defaultServoParams) { const servoParam_t *defaultServoConf = &defaultServoParams[i]; - equalsDefault = servoConf->min == defaultServoConf->min - && servoConf->max == defaultServoConf->max - && servoConf->middle == defaultServoConf->middle - && servoConf->rate == defaultServoConf->rate - && servoConf->forwardFromChannel == defaultServoConf->forwardFromChannel; + equalsDefault = !memcmp(servoConf, defaultServoConf, sizeof(*servoConf)); cliDefaultPrintLinef(dumpMask, equalsDefault, format, i, defaultServoConf->min, @@ -1823,13 +1836,7 @@ static void printServoMix(uint8_t dumpMask, const servoMixer_t *customServoMixer bool equalsDefault = false; if (defaultCustomServoMixers) { servoMixer_t customServoMixerDefault = defaultCustomServoMixers[i]; - equalsDefault = customServoMixer.targetChannel == customServoMixerDefault.targetChannel - && customServoMixer.inputSource == customServoMixerDefault.inputSource - && customServoMixer.rate == customServoMixerDefault.rate - && customServoMixer.speed == customServoMixerDefault.speed - && customServoMixer.min == customServoMixerDefault.min - && customServoMixer.max == customServoMixerDefault.max - && customServoMixer.box == customServoMixerDefault.box; + equalsDefault = !memcmp(&customServoMixer, &customServoMixerDefault, sizeof(customServoMixer)); cliDefaultPrintLinef(dumpMask, equalsDefault, format, i, @@ -1876,7 +1883,7 @@ static void cliServoMix(char *cmdline) len = strlen(ptr); for (uint32_t i = 0; ; i++) { if (mixerNames[i] == NULL) { - cliPrintErrorLinef("Invalid name"); + cliPrintErrorLinef("INVALID NAME"); break; } if (strncasecmp(ptr, mixerNames[i], len) == 0) { @@ -1891,8 +1898,7 @@ static void cliServoMix(char *cmdline) enum {SERVO = 0, INPUT, REVERSE, ARGS_COUNT}; char *ptr = strchr(cmdline, ' '); - len = strlen(ptr); - if (len == 0) { + if (ptr == NULL) { cliPrintf("s"); for (uint32_t inputSource = 0; inputSource < INPUT_SOURCE_COUNT; inputSource++) cliPrintf("\ti%d", inputSource); @@ -1900,8 +1906,9 @@ static void cliServoMix(char *cmdline) for (uint32_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) { cliPrintf("%d", servoIndex); - for (uint32_t inputSource = 0; inputSource < INPUT_SOURCE_COUNT; inputSource++) + for (uint32_t inputSource = 0; inputSource < INPUT_SOURCE_COUNT; inputSource++) { cliPrintf("\t%s ", (servoParams(servoIndex)->reversedSources & (1 << inputSource)) ? "r" : "n"); + } cliPrintLinefeed(); } return; @@ -1922,12 +1929,15 @@ static void cliServoMix(char *cmdline) if (args[SERVO] >= 0 && args[SERVO] < MAX_SUPPORTED_SERVOS && args[INPUT] >= 0 && args[INPUT] < INPUT_SOURCE_COUNT && (*ptr == 'r' || *ptr == 'n')) { - if (*ptr == 'r') + if (*ptr == 'r') { servoParamsMutable(args[SERVO])->reversedSources |= 1 << args[INPUT]; - else + } else { servoParamsMutable(args[SERVO])->reversedSources &= ~(1 << args[INPUT]); - } else + } + } else { cliShowParseError(); + return; + } cliServoMix("reverse"); } else { @@ -1990,7 +2000,7 @@ static void cliSdInfo(char *cmdline) return; } - if (!sdcard_isInitialized()) { + if (!sdcard_isFunctional() || !sdcard_isInitialized()) { cliPrintLine("Startup failed"); return; } @@ -2057,6 +2067,10 @@ static void cliFlashErase(char *cmdline) { UNUSED(cmdline); + if (!flashfsIsSupported()) { + return; + } + #ifndef MINIMAL_CLI uint32_t i = 0; cliPrintLine("Erasing, please wait ... "); @@ -2148,11 +2162,7 @@ static void printVtx(uint8_t dumpMask, const vtxConfig_t *vtxConfig, const vtxCo const vtxChannelActivationCondition_t *cac = &vtxConfig->vtxChannelActivationConditions[i]; if (vtxConfigDefault) { const vtxChannelActivationCondition_t *cacDefault = &vtxConfigDefault->vtxChannelActivationConditions[i]; - equalsDefault = cac->auxChannelIndex == cacDefault->auxChannelIndex - && cac->band == cacDefault->band - && cac->channel == cacDefault->channel - && cac->range.startStep == cacDefault->range.startStep - && cac->range.endStep == cacDefault->range.endStep; + equalsDefault = !memcmp(cac, cacDefault, sizeof(*cac)); cliDefaultPrintLinef(dumpMask, equalsDefault, format, i, cacDefault->auxChannelIndex, @@ -2229,7 +2239,7 @@ static void cliVtx(char *cmdline) ); } } else { - cliShowArgumentRangeError("index", 0, MAX_CHANNEL_ACTIVATION_CONDITION_COUNT - 1); + cliShowArgumentRangeError("INDEX", 0, MAX_CHANNEL_ACTIVATION_CONDITION_COUNT - 1); } } } @@ -2256,13 +2266,13 @@ static void cliName(char *cmdline) #if defined(USE_BOARD_INFO) -#define ERROR_MESSAGE "%s cannot be changed. Current value: '%s'" +#define ERROR_MESSAGE "%s CANNOT BE CHANGED. CURRENT VALUE: '%s'" static void cliBoardName(char *cmdline) { const unsigned int len = strlen(cmdline); if (len > 0 && boardInformationIsSet() && (len != strlen(getBoardName()) || strncmp(getBoardName(), cmdline, len))) { - cliPrintErrorLinef(ERROR_MESSAGE, "board_name", getBoardName()); + cliPrintErrorLinef(ERROR_MESSAGE, "BOARD_NAME", getBoardName()); } else { if (len > 0) { setBoardName(cmdline); @@ -2276,7 +2286,7 @@ static void cliManufacturerId(char *cmdline) { const unsigned int len = strlen(cmdline); if (len > 0 && boardInformationIsSet() && (len != strlen(getManufacturerId()) || strncmp(getManufacturerId(), cmdline, len))) { - cliPrintErrorLinef(ERROR_MESSAGE, "manufacturer_id", getManufacturerId()); + cliPrintErrorLinef(ERROR_MESSAGE, "MANUFACTURER_ID", getManufacturerId()); } else { if (len > 0) { setManufacturerId(cmdline); @@ -2301,7 +2311,7 @@ static void cliSignature(char *cmdline) uint8_t signature[SIGNATURE_LENGTH] = {0}; if (len > 0) { if (len != 2 * SIGNATURE_LENGTH) { - cliPrintErrorLinef("Invalid length: %d (expected: %d)", len, 2 * SIGNATURE_LENGTH); + cliPrintErrorLinef("INVALID LENGTH: %d (EXPECTED: %d)", len, 2 * SIGNATURE_LENGTH); return; } @@ -2316,7 +2326,7 @@ static void cliSignature(char *cmdline) if (end == &temp[BLOCK_SIZE]) { signature[i] = result; } else { - cliPrintErrorLinef("Invalid character found: %c", end[0]); + cliPrintErrorLinef("INVALID CHARACTER FOUND: %c", end[0]); return; } @@ -2327,7 +2337,7 @@ static void cliSignature(char *cmdline) char signatureStr[SIGNATURE_LENGTH * 2 + 1] = {0}; if (len > 0 && signatureIsSet() && memcmp(signature, getSignature(), SIGNATURE_LENGTH)) { writeSignature(signatureStr, getSignature()); - cliPrintErrorLinef(ERROR_MESSAGE, "signature", signatureStr); + cliPrintErrorLinef(ERROR_MESSAGE, "SIGNATURE", signatureStr); } else { if (len > 0) { setSignature(signature); @@ -2355,9 +2365,18 @@ static void cliMcuId(char *cmdline) cliPrintLinef("mcu_id %08x%08x%08x", U_ID_0, U_ID_1, U_ID_2); } +static uint32_t getFeatureMask(const uint32_t featureMask) +{ + if (featureMaskIsCopied) { + return featureMaskCopy; + } else { + return featureMask; + } +} + static void printFeature(uint8_t dumpMask, const featureConfig_t *featureConfig, const featureConfig_t *featureConfigDefault) { - const uint32_t mask = featureConfig->enabledFeatures; + const uint32_t mask = getFeatureMask(featureConfig->enabledFeatures); const uint32_t defaultMask = featureConfigDefault->enabledFeatures; for (uint32_t i = 0; featureNames[i]; i++) { // disabled features first if (strcmp(featureNames[i], emptyString) != 0) { //Skip unused @@ -2382,15 +2401,16 @@ static void printFeature(uint8_t dumpMask, const featureConfig_t *featureConfig, static void cliFeature(char *cmdline) { uint32_t len = strlen(cmdline); - uint32_t mask = featureMask(); - + const uint32_t mask = getFeatureMask(featureMask()); if (len == 0) { cliPrint("Enabled: "); for (uint32_t i = 0; ; i++) { - if (featureNames[i] == NULL) + if (featureNames[i] == NULL) { break; - if (mask & (1 << i)) + } + if (mask & (1 << i)) { cliPrintf("%s ", featureNames[i]); + } } cliPrintLinefeed(); } else if (strncasecmp(cmdline, "list", len) == 0) { @@ -2404,6 +2424,12 @@ static void cliFeature(char *cmdline) cliPrintLinefeed(); return; } else { + if (!featureMaskIsCopied) { + featureMaskCopy = featureMask(); + featureMaskIsCopied = true; + } + uint32_t feature; + bool remove = false; if (cmdline[0] == '-') { // remove feature @@ -2414,30 +2440,29 @@ static void cliFeature(char *cmdline) for (uint32_t i = 0; ; i++) { if (featureNames[i] == NULL) { - cliPrintErrorLinef("Invalid name"); + cliPrintErrorLinef("INVALID NAME"); break; } if (strncasecmp(cmdline, featureNames[i], len) == 0) { - - mask = 1 << i; + feature = 1 << i; #ifndef USE_GPS - if (mask & FEATURE_GPS) { + if (feature & FEATURE_GPS) { cliPrintLine("unavailable"); break; } #endif #ifndef USE_RANGEFINDER - if (mask & FEATURE_RANGEFINDER) { + if (feature & FEATURE_RANGEFINDER) { cliPrintLine("unavailable"); break; } #endif if (remove) { - featureClear(mask); + featureClear(feature, &featureMaskCopy); cliPrint("Disabled"); } else { - featureSet(mask); + featureSet(feature, &featureMaskCopy); cliPrint("Enabled"); } cliPrintLinef(" %s", featureNames[i]); @@ -2451,7 +2476,7 @@ static void cliFeature(char *cmdline) static void printBeeper(uint8_t dumpMask, const uint32_t offFlags, const uint32_t offFlagsDefault, const char *name) { const uint8_t beeperCount = beeperTableEntryCount(); - for (int32_t i = 0; i < beeperCount - 2; i++) { + for (int32_t i = 0; i < beeperCount - 1; i++) { const char *formatOff = "%s -%s"; const char *formatOn = "%s %s"; const uint32_t beeperModeMask = beeperModeMaskForTableIndex(i); @@ -2496,7 +2521,7 @@ static void processBeeperCommand(char *cmdline, uint32_t *offFlags, const uint32 for (uint32_t i = 0; ; i++) { if (i == beeperCount) { - cliPrintErrorLinef("Invalid name"); + cliPrintErrorLinef("INVALID NAME"); break; } if (strncasecmp(cmdline, beeperNameForTableIndex(i), len) == 0 && beeperModeMaskForTableIndex(i) & (allowedFlags | BEEPER_GET_FLAG(BEEPER_ALL))) { @@ -2536,17 +2561,18 @@ static void cliBeeper(char *cmdline) } #endif -#ifdef USE_RX_FRSKY_SPI -void cliFrSkyBind(char *cmdline){ +#ifdef USE_RX_SPI +void cliRxBind(char *cmdline){ UNUSED(cmdline); switch (rxSpiConfig()->rx_spi_protocol) { +#ifdef USE_RX_CC2500_BIND case RX_SPI_FRSKY_D: case RX_SPI_FRSKY_X: - frSkySpiBind(); - + case RX_SPI_SFHSS: + cc2500SpiBind(); cliPrint("Binding..."); - break; +#endif default: cliPrint("Not supported."); @@ -2693,7 +2719,7 @@ static void cliPrintGyroRegisters(uint8_t whichSensor) static void cliDumpGyroRegisters(char *cmdline) { -#ifdef USE_DUAL_GYRO +#ifdef USE_MULTI_GYRO if ((gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_1) || (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH)) { cliPrintLinef("\r\n# Gyro 1"); cliPrintGyroRegisters(GYRO_CONFIG_USE_GYRO_1); @@ -2704,7 +2730,7 @@ static void cliDumpGyroRegisters(char *cmdline) } #else cliPrintGyroRegisters(GYRO_CONFIG_USE_GYRO_1); -#endif // USE_DUAL_GYRO +#endif UNUSED(cmdline); } #endif @@ -2717,7 +2743,7 @@ static int parseOutputIndex(char *pch, bool allowAllEscs) { } else if (allowAllEscs && outputIndex == ALL_MOTORS) { cliPrintLinef("Using all outputs."); } else { - cliPrintErrorLinef("Invalid output number. Range: 0 %d.", getMotorCount() - 1); + cliPrintErrorLinef("INVALID OUTPUT NUMBER. RANGE: 0 - %d.", getMotorCount() - 1); return -1; } @@ -2888,7 +2914,7 @@ void printEscInfo(const uint8_t *escInfoBuffer, uint8_t bytesRead) } } } else { - cliPrintErrorLinef("Checksum Error."); + cliPrintErrorLinef("CHECKSUM ERROR."); } } } @@ -2956,7 +2982,7 @@ static void cliDshotProg(char *cmdline) pwmWriteDshotCommand(escIndex, getMotorCount(), command, true); } else { #if defined(USE_ESC_SENSOR) && defined(USE_ESC_SENSOR_INFO) - if (feature(FEATURE_ESC_SENSOR)) { + if (featureIsEnabled(FEATURE_ESC_SENSOR)) { if (escIndex != ALL_MOTORS) { executeEscInfoCommand(escIndex); } else { @@ -2974,7 +3000,7 @@ static void cliDshotProg(char *cmdline) cliPrintLinef("Command Sent: %d", command); } else { - cliPrintErrorLinef("Invalid command. Range: 1 - %d.", DSHOT_MIN_THROTTLE - 1); + cliPrintErrorLinef("INVALID COMMAND. RANGE: 1 - %d.", DSHOT_MIN_THROTTLE - 1); } } @@ -3066,7 +3092,7 @@ static void cliMixer(char *cmdline) for (uint32_t i = 0; ; i++) { if (mixerNames[i] == NULL) { - cliPrintErrorLinef("Invalid name"); + cliPrintErrorLinef("INVALID NAME"); return; } if (strncasecmp(cmdline, mixerNames[i], len) == 0) { @@ -3113,7 +3139,7 @@ static void cliMotor(char *cmdline) if (index == 2) { if (motorValue < PWM_RANGE_MIN || motorValue > PWM_RANGE_MAX) { - cliShowArgumentRangeError("value", 1000, 2000); + cliShowArgumentRangeError("VALUE", 1000, 2000); } else { uint32_t motorOutputValue = convertExternalToMotor(motorValue); @@ -3150,7 +3176,7 @@ static void cliPlaySound(char *cmdline) if ((name=beeperNameForTableIndex(i)) != NULL) break; //if name OK then play sound below if (i == lastSoundIdx + 1) { //prevent infinite loop - cliPrintErrorLinef("Error playing sound"); + cliPrintErrorLinef("ERROR PLAYING SOUND"); return; } } @@ -3248,7 +3274,11 @@ static void cliSave(char *cmdline) #endif #endif // USE_BOARD_INFO - writeEEPROM(); + if (featureMaskIsCopied) { + writeEEPROMWithFeatures(featureMaskCopy); + } else { + writeEEPROM(); + } cliReboot(); } @@ -3336,7 +3366,7 @@ STATIC_UNIT_TESTED void cliGet(char *cmdline) return; } - cliPrintErrorLinef("Invalid name"); + cliPrintErrorLinef("INVALID NAME"); } static uint8_t getWordLength(char *bufBegin, char *bufEnd) @@ -3484,14 +3514,14 @@ STATIC_UNIT_TESTED void cliSet(char *cmdline) cliPrintf("%s set to ", val->name); cliPrintVar(val, 0); } else { - cliPrintErrorLinef("Invalid value"); + cliPrintErrorLinef("INVALID VALUE"); cliPrintVarRange(val); } return; } } - cliPrintErrorLinef("Invalid name"); + cliPrintErrorLinef("INVALID NAME"); } else { // no equals, check for matching variables. cliGet(cmdline); @@ -3578,7 +3608,7 @@ static void cliStatus(char *cmdline) cliPrintLinefeed(); } -#ifndef SKIP_TASK_STATISTICS +#if defined(USE_TASK_STATISTICS) static void cliTasks(char *cmdline) { UNUSED(cmdline); @@ -3627,6 +3657,8 @@ static void cliTasks(char *cmdline) if (taskId == TASK_GYROPID && pidConfig()->pid_process_denom > 1) { cliPrintLinef(" - (%15s) %6d", taskInfo.subTaskName, subTaskFrequency); } + + schedulerResetTaskMaxExecutionTime(taskId); } } if (systemConfig()->task_statistics) { @@ -3662,20 +3694,24 @@ static void cliRcSmoothing(char *cmdline) if (rxConfig()->rc_smoothing_type == RC_SMOOTHING_TYPE_FILTER) { cliPrintLine("FILTER"); uint16_t avgRxFrameMs = rcSmoothingGetValue(RC_SMOOTHING_VALUE_AVERAGE_FRAME); - cliPrint("# Detected RX frame rate: "); - if (avgRxFrameMs == 0) { - cliPrintLine("NO SIGNAL"); - } else { - cliPrintLinef("%d.%dms", avgRxFrameMs / 1000, avgRxFrameMs % 1000); + if (rcSmoothingAutoCalculate()) { + cliPrint("# Detected RX frame rate: "); + if (avgRxFrameMs == 0) { + cliPrintLine("NO SIGNAL"); + } else { + cliPrintLinef("%d.%dms", avgRxFrameMs / 1000, avgRxFrameMs % 1000); + } } - cliPrintLinef("# Auto input cutoff: %dhz", rcSmoothingGetValue(RC_SMOOTHING_VALUE_INPUT_AUTO)); + cliPrint("# Input filter type: "); + cliPrintLinef(lookupTables[TABLE_RC_SMOOTHING_INPUT_TYPE].values[rxConfig()->rc_smoothing_input_type]); cliPrintf("# Active input cutoff: %dhz ", rcSmoothingGetValue(RC_SMOOTHING_VALUE_INPUT_ACTIVE)); if (rxConfig()->rc_smoothing_input_cutoff == 0) { cliPrintLine("(auto)"); } else { cliPrintLine("(manual)"); } - cliPrintLinef("# Auto derivative cutoff: %dhz", rcSmoothingGetValue(RC_SMOOTHING_VALUE_DERIVATIVE_AUTO)); + cliPrint("# Derivative filter type: "); + cliPrintLinef(lookupTables[TABLE_RC_SMOOTHING_DERIVATIVE_TYPE].values[rxConfig()->rc_smoothing_derivative_type]); cliPrintf("# Active derivative cutoff: %dhz (", rcSmoothingGetValue(RC_SMOOTHING_VALUE_DERIVATIVE_ACTIVE)); if (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_OFF) { cliPrintLine("off)"); @@ -3726,8 +3762,10 @@ const cliResourceValue_t resourceTable[] = { #ifdef USE_SERVOS DEFA( OWNER_SERVO, PG_SERVO_CONFIG, servoConfig_t, dev.ioTags[0], MAX_SUPPORTED_SERVOS ), #endif -#if defined(USE_PWM) || defined(USE_PPM) +#if defined(USE_PPM) DEFS( OWNER_PPMINPUT, PG_PPM_CONFIG, ppmConfig_t, ioTag ), +#endif +#if defined(USE_PWM) DEFA( OWNER_PWMINPUT, PG_PWM_CONFIG, pwmConfig_t, ioTags[0], PWM_INPUT_PORT_COUNT ), #endif #ifdef USE_RANGEFINDER_HCSR04 @@ -3737,8 +3775,10 @@ const cliResourceValue_t resourceTable[] = { #ifdef USE_LED_STRIP DEFS( OWNER_LED_STRIP, PG_LED_STRIP_CONFIG, ledStripConfig_t, ioTag ), #endif +#ifdef USE_UART DEFA( OWNER_SERIAL_TX, PG_SERIAL_PIN_CONFIG, serialPinConfig_t, ioTagTx[0], SERIAL_PORT_MAX_INDEX ), DEFA( OWNER_SERIAL_RX, PG_SERIAL_PIN_CONFIG, serialPinConfig_t, ioTagRx[0], SERIAL_PORT_MAX_INDEX ), +#endif #ifdef USE_INVERTER DEFA( OWNER_INVERTER, PG_SERIAL_PIN_CONFIG, serialPinConfig_t, ioTagInverter[0], SERIAL_PORT_MAX_INDEX ), #endif @@ -3780,8 +3820,10 @@ const cliResourceValue_t resourceTable[] = { DEFS( OWNER_COMPASS_EXTI, PG_COMPASS_CONFIG, compassConfig_t, interruptTag ), #endif #endif -#ifdef USE_SDCARD +#ifdef USE_SDCARD_SPI DEFS( OWNER_SDCARD_CS, PG_SDCARD_CONFIG, sdcardConfig_t, chipSelectTag ), +#endif +#ifdef USE_SDCARD DEFS( OWNER_SDCARD_DETECT, PG_SDCARD_CONFIG, sdcardConfig_t, cardDetectTag ), #endif #ifdef USE_PINIO @@ -3803,6 +3845,13 @@ const cliResourceValue_t resourceTable[] = { #ifdef USE_RX_SPI DEFS( OWNER_RX_SPI_CS, PG_RX_SPI_CONFIG, rxSpiConfig_t, csnTag ), #endif +#ifdef USE_GYRO_EXTI + DEFW( OWNER_GYRO_EXTI, PG_GYRO_DEVICE_CONFIG, gyroDeviceConfig_t, extiTag, 2 ), +#endif + DEFW( OWNER_GYRO_CS, PG_GYRO_DEVICE_CONFIG, gyroDeviceConfig_t, csnTag, 2 ), +#ifdef USE_USB_DETECT + DEFS( OWNER_USB_DETECT, PG_USB_CONFIG, usbDev_t, detectPin ), +#endif }; #undef DEFS @@ -3960,7 +4009,7 @@ static void cliResource(char *cmdline) pch = strtok_r(cmdline, " ", &saveptr); for (resourceIndex = 0; ; resourceIndex++) { if (resourceIndex >= ARRAYLEN(resourceTable)) { - cliPrintErrorLinef("Invalid"); + cliPrintErrorLinef("INVALID"); return; } @@ -3974,7 +4023,7 @@ static void cliResource(char *cmdline) if (resourceTable[resourceIndex].maxIndex > 0 || index > 0) { if (index <= 0 || index > MAX_RESOURCE_INDEX(resourceTable[resourceIndex].maxIndex)) { - cliShowArgumentRangeError("index", 1, MAX_RESOURCE_INDEX(resourceTable[resourceIndex].maxIndex)); + cliShowArgumentRangeError("INDEX", 1, MAX_RESOURCE_INDEX(resourceTable[resourceIndex].maxIndex)); return; } index -= 1; @@ -4069,7 +4118,7 @@ static void printTimer(uint8_t dumpMask) cliDumpPrintLinef(dumpMask, false, format, IO_GPIOPortIdxByTag(ioTag) + 'A', IO_GPIOPinIdxByTag(ioTag), - timerIndex + timerIndex - 1 ); } } @@ -4111,7 +4160,7 @@ static void cliTimer(char *cmdline) } if (timerIOIndex < 0) { - cliPrintErrorLinef("Index out of range."); + cliPrintErrorLinef("INDEX OUT OF RANGE."); return; } @@ -4120,13 +4169,13 @@ static void cliTimer(char *cmdline) if (pch) { if (strcasecmp(pch, "list") == 0) { /* output the list of available options */ - uint8_t index = 1; + uint8_t index = 0; for (unsigned i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { if (timerHardware[i].tag == ioTag) { cliPrintLinef("# %d. TIM%d CH%d", index, timerGetTIMNumber(timerHardware[i].tim), - CC_INDEX_FROM_CHANNEL(timerHardware[i].channel) + CC_INDEX_FROM_CHANNEL(timerHardware[i].channel) + 1 ); index++; } @@ -4135,7 +4184,7 @@ static void cliTimer(char *cmdline) } else if (strcasecmp(pch, "none") == 0) { goto success; } else { - timerIndex = atoi(pch); + timerIndex = atoi(pch) + 1; } } else { goto error; @@ -4334,40 +4383,21 @@ static void cliDiff(char *cmdline) printConfig(cmdline, true); } -#ifdef USE_USB_MSC +#if defined(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"); + if (mscCheckFilesystemReady()) { + cliPrintHashLine("Restarting in mass storage mode"); cliPrint("\r\nRebooting"); bufWriterFlush(cliWriter); - delay(1000); waitForSerialPortToFinishTransmitting(cliPort); stopPwmAllMotors(); - if (mpuResetFn) { - mpuResetFn(); - } -#ifdef STM32F7 - *((__IO uint32_t*) BKPSRAM_BASE + 16) = MSC_MAGIC; -#elif defined(STM32F4) - *((uint32_t *)0x2001FFF0) = MSC_MAGIC; -#endif - - __disable_irq(); - NVIC_SystemReset(); + systemResetToMsc(); } else { - cliPrint("\r\nStorage not present or failed to initialize!"); - bufWriterFlush(cliWriter); + cliPrintHashLine("Storage not present or failed to initialize!"); } } #endif @@ -4443,8 +4473,8 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("flash_write", NULL, "
", cliFlashWrite), #endif #endif -#ifdef USE_RX_FRSKY_SPI - CLI_COMMAND_DEF("frsky_bind", "initiate binding for FrSky SPI RX", NULL, cliFrSkyBind), +#ifdef USE_RX_CC2500_BIND + CLI_COMMAND_DEF("bind", "initiate binding for RX", NULL, cliRxBind), #endif CLI_COMMAND_DEF("get", "get variable value", "[name]", cliGet), #ifdef USE_GPS @@ -4492,7 +4522,7 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("sd_info", "sdcard info", NULL, cliSdInfo), #endif CLI_COMMAND_DEF("serial", "configure serial ports", NULL, cliSerial), -#ifndef SKIP_SERIAL_PASSTHROUGH +#if defined(USE_SERIAL_PASSTHROUGH) CLI_COMMAND_DEF("serialpassthrough", "passthrough serial data to port", " [baud] [mode] [DTR PINIO]: passthrough to serial", cliSerialPassthrough), #endif #ifdef USE_SERVOS @@ -4509,7 +4539,7 @@ const clicmd_t cmdTable[] = { "\treverse r|n", cliServoMix), #endif CLI_COMMAND_DEF("status", "show status", NULL, cliStatus), -#ifndef SKIP_TASK_STATISTICS +#if defined(USE_TASK_STATISTICS) CLI_COMMAND_DEF("tasks", "show task stats", NULL, cliTasks), #endif #ifdef USE_TIMER_MGMT @@ -4663,9 +4693,9 @@ void cliEnter(serialPort_t *serialPort) #else cliPrintLine("\r\nCLI"); #endif - cliPrompt(); - setArmingDisabled(ARMING_DISABLED_CLI); + + cliPrompt(); } void cliInit(const serialConfig_t *serialConfig) diff --git a/src/main/interface/crsf_protocol.h b/src/main/interface/crsf_protocol.h index 547044df55..32b0714915 100644 --- a/src/main/interface/crsf_protocol.h +++ b/src/main/interface/crsf_protocol.h @@ -92,6 +92,9 @@ enum { CRSF_FRAME_ORIGIN_DEST_SIZE = 2, }; +// Clashes with CRSF_ADDRESS_FLIGHT_CONTROLLER +#define CRSF_SYNC_BYTE 0XC8 + typedef enum { CRSF_ADDRESS_BROADCAST = 0x00, CRSF_ADDRESS_USB = 0x10, diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c index 559ef6b762..8407de303a 100644 --- a/src/main/interface/msp.c +++ b/src/main/interface/msp.c @@ -23,6 +23,7 @@ #include #include #include +#include #include "platform.h" @@ -35,20 +36,17 @@ #include "common/axis.h" #include "common/bitarray.h" #include "common/color.h" +#include "common/huffman.h" #include "common/maths.h" #include "common/streambuf.h" -#include "common/huffman.h" +#include "common/utils.h" #include "config/config_eeprom.h" #include "config/feature.h" -#include "pg/pg.h" -#include "pg/pg_ids.h" -#include "pg/beeper.h" -#include "pg/rx.h" -#include "pg/rx_spi.h" #include "drivers/accgyro/accgyro.h" #include "drivers/bus_i2c.h" +#include "drivers/camera_control.h" #include "drivers/compass/compass.h" #include "drivers/flash.h" #include "drivers/io.h" @@ -58,15 +56,15 @@ #include "drivers/serial.h" #include "drivers/serial_escserial.h" #include "drivers/system.h" -#include "drivers/vtx_common.h" #include "drivers/transponder_ir.h" -#include "drivers/camera_control.h" +#include "drivers/usb_msc.h" +#include "drivers/vtx_common.h" #include "fc/board_info.h" #include "fc/config.h" #include "fc/controlrate_profile.h" -#include "fc/fc_core.h" -#include "fc/fc_rc.h" +#include "fc/core.h" +#include "fc/rc.h" #include "fc/rc_adjustments.h" #include "fc/rc_controls.h" #include "fc/rc_modes.h" @@ -74,6 +72,7 @@ #include "flight/position.h" #include "flight/failsafe.h" +#include "flight/gps_rescue.h" #include "flight/imu.h" #include "flight/mixer.h" #include "flight/pid.h" @@ -91,18 +90,24 @@ #include "io/ledstrip.h" #include "io/motors.h" #include "io/osd.h" -#include "io/osd_slave.h" #include "io/serial.h" #include "io/serial_4way.h" #include "io/servos.h" #include "io/transponder_ir.h" +#include "io/usb_msc.h" #include "io/vtx_control.h" #include "io/vtx.h" #include "io/vtx_string.h" #include "msp/msp_serial.h" +#include "pg/beeper.h" #include "pg/board.h" +#include "pg/pg.h" +#include "pg/pg_ids.h" +#include "pg/rx.h" +#include "pg/rx_spi.h" +#include "pg/usb.h" #include "pg/vcd.h" #include "rx/rx.h" @@ -129,19 +134,14 @@ static const char * const flightControllerIdentifier = BETAFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller. -#ifndef USE_OSD_SLAVE +enum { + MSP_REBOOT_FIRMWARE = 0, + MSP_REBOOT_BOOTLOADER, + MSP_REBOOT_MSC, + MSP_REBOOT_COUNT, +}; -static const char pidnames[] = - "ROLL;" - "PITCH;" - "YAW;" - "ALT;" - "Pos;" - "PosR;" - "NavR;" - "LEVEL;" - "MAG;" - "VEL;"; +static uint8_t rebootMode; typedef enum { MSP_SDCARD_STATE_NOT_PRESENT = 0, @@ -161,10 +161,21 @@ typedef enum { } mspFlashFsFlags_e; #define RATEPROFILE_MASK (1 << 7) -#endif //USE_OSD_SLAVE #define RTC_NOT_SUPPORTED 0xff +static bool featureMaskIsCopied = false; +static uint32_t featureMaskCopy; + +static uint32_t getFeatureMask(void) +{ + if (featureMaskIsCopied) { + return featureMaskCopy; + } else { + return featureMask(); + } +} + #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE #define ESC_4WAY 0xff @@ -229,16 +240,32 @@ static void mspRebootFn(serialPort_t *serialPort) { UNUSED(serialPort); -#ifndef USE_OSD_SLAVE stopPwmAllMotors(); + + switch (rebootMode) { + case MSP_REBOOT_FIRMWARE: + systemReset(); + + break; + case MSP_REBOOT_BOOTLOADER: + systemResetToBootloader(); + + break; +#if defined(USE_USB_MSC) + case MSP_REBOOT_MSC: + systemResetToMsc(); + + break; #endif - systemReset(); + default: + + break; + } // control should never return here. while (true) ; } -#ifndef USE_OSD_SLAVE static void serializeSDCardSummaryReply(sbuf_t *dst) { #ifdef USE_SDCARD @@ -277,8 +304,13 @@ static void serializeSDCardSummaryReply(sbuf_t *dst) sbufWriteU8(dst, state); sbufWriteU8(dst, afatfs_getLastError()); // Write free space and total space in kilobytes - sbufWriteU32(dst, afatfs_getContiguousFreeSpace() / 1024); - sbufWriteU32(dst, sdcard_getMetadata()->numBlocks / 2); // Block size is half a kilobyte + if (state == MSP_SDCARD_STATE_READY) { + sbufWriteU32(dst, afatfs_getContiguousFreeSpace() / 1024); + sbufWriteU32(dst, sdcard_getMetadata()->numBlocks / 2); // Block size is half a kilobyte + } else { + sbufWriteU32(dst, 0); + sbufWriteU32(dst, 0); + } #else sbufWriteU8(dst, 0); sbufWriteU8(dst, 0); @@ -308,7 +340,7 @@ static void serializeDataflashSummaryReply(sbuf_t *dst) sbufWriteU32(dst, 0); sbufWriteU32(dst, 0); sbufWriteU32(dst, 0); - } + } } #ifdef USE_FLASHFS @@ -319,7 +351,7 @@ enum compressionType_e { static void serializeDataflashReadReply(sbuf_t *dst, uint32_t address, const uint16_t size, bool useLegacyFormat, bool allowCompression) { - BUILD_BUG_ON(MSP_PORT_DATAFLASH_INFO_SIZE < 16); + STATIC_ASSERT(MSP_PORT_DATAFLASH_INFO_SIZE >= 16, MSP_PORT_DATAFLASH_INFO_SIZE_invalid); uint16_t readLen = size; const int bytesRemainingInBuf = sbufBytesRemaining(dst) - MSP_PORT_DATAFLASH_INFO_SIZE; @@ -402,7 +434,6 @@ static void serializeDataflashReadReply(sbuf_t *dst, uint32_t address, const uin } } #endif // USE_FLASHFS -#endif // USE_OSD_SLAVE /* * Returns true if the command was processd, false otherwise. @@ -410,6 +441,8 @@ static void serializeDataflashReadReply(sbuf_t *dst, uint32_t address, const uin */ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn) { + UNUSED(mspPostProcessFn); + switch (cmdMSP) { case MSP_API_VERSION: sbufWriteU8(dst, MSP_PROTOCOL_VERSION); @@ -435,14 +468,10 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce #else sbufWriteU16(dst, 0); // No other build targets currently have hardware revision detection. #endif -#ifdef USE_OSD_SLAVE - sbufWriteU8(dst, 1); // 1 == OSD -#else #if defined(USE_OSD) && defined(USE_MAX7456) sbufWriteU8(dst, 2); // 2 == FC with OSD #else sbufWriteU8(dst, 0); // 0 == FC -#endif #endif // Board communication capabilities (uint8) // Bit 0: 1 iff the board has VCP @@ -486,27 +515,14 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce sbufWriteData(dst, shortGitRevision, GIT_SHORT_REVISION_LENGTH); break; - case MSP_REBOOT: - if (mspPostProcessFn) { - *mspPostProcessFn = mspRebootFn; - } - break; - case MSP_ANALOG: sbufWriteU8(dst, (uint8_t)constrain(getBatteryVoltage(), 0, 255)); sbufWriteU16(dst, (uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF)); // milliamp hours drawn from battery -#ifdef USE_OSD_SLAVE - sbufWriteU16(dst, 0); // rssi -#else sbufWriteU16(dst, getRssi()); -#endif sbufWriteU16(dst, (int16_t)constrain(getAmperage(), -0x8000, 0x7FFF)); // send current in 0.01 A steps, range is -320A to 320A break; case MSP_DEBUG: - // output some useful QA statistics - // debug[x] = ((hse_value / 1000000) * 1000) + (SystemCoreClock / 1000000); // XX0YY [crystal clock : core clock] - for (int i = 0; i < DEBUG16_VALUE_COUNT; i++) { sbufWriteU16(dst, debug[i]); // 4 variables are here for general monitoring purpose } @@ -519,7 +535,7 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce break; case MSP_FEATURE_CONFIG: - sbufWriteU32(dst, featureMask()); + sbufWriteU32(dst, getFeatureMask()); break; #ifdef USE_BEEPER @@ -584,25 +600,27 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce } case MSP_VOLTAGE_METER_CONFIG: - // by using a sensor type and a sub-frame length it's possible to configure any type of voltage meter, - // e.g. an i2c/spi/can sensor or any sensor not built directly into the FC such as ESC/RX/SPort/SBus that has - // different configuration requirements. - BUILD_BUG_ON(VOLTAGE_SENSOR_ADC_VBAT != 0); // VOLTAGE_SENSOR_ADC_VBAT should be the first index, - sbufWriteU8(dst, MAX_VOLTAGE_SENSOR_ADC); // voltage meters in payload - for (int i = VOLTAGE_SENSOR_ADC_VBAT; i < MAX_VOLTAGE_SENSOR_ADC; i++) { - const uint8_t adcSensorSubframeLength = 1 + 1 + 1 + 1 + 1; // length of id, type, vbatscale, vbatresdivval, vbatresdivmultipler, in bytes - sbufWriteU8(dst, adcSensorSubframeLength); // ADC sensor sub-frame length + { + // by using a sensor type and a sub-frame length it's possible to configure any type of voltage meter, + // e.g. an i2c/spi/can sensor or any sensor not built directly into the FC such as ESC/RX/SPort/SBus that has + // different configuration requirements. + STATIC_ASSERT(VOLTAGE_SENSOR_ADC_VBAT == 0, VOLTAGE_SENSOR_ADC_VBAT_incorrect); // VOLTAGE_SENSOR_ADC_VBAT should be the first index + sbufWriteU8(dst, MAX_VOLTAGE_SENSOR_ADC); // voltage meters in payload + for (int i = VOLTAGE_SENSOR_ADC_VBAT; i < MAX_VOLTAGE_SENSOR_ADC; i++) { + const uint8_t adcSensorSubframeLength = 1 + 1 + 1 + 1 + 1; // length of id, type, vbatscale, vbatresdivval, vbatresdivmultipler, in bytes + sbufWriteU8(dst, adcSensorSubframeLength); // ADC sensor sub-frame length - sbufWriteU8(dst, voltageMeterADCtoIDMap[i]); // id of the sensor - sbufWriteU8(dst, VOLTAGE_SENSOR_TYPE_ADC_RESISTOR_DIVIDER); // indicate the type of sensor that the next part of the payload is for + sbufWriteU8(dst, voltageMeterADCtoIDMap[i]); // id of the sensor + sbufWriteU8(dst, VOLTAGE_SENSOR_TYPE_ADC_RESISTOR_DIVIDER); // indicate the type of sensor that the next part of the payload is for - sbufWriteU8(dst, voltageSensorADCConfig(i)->vbatscale); - sbufWriteU8(dst, voltageSensorADCConfig(i)->vbatresdivval); - sbufWriteU8(dst, voltageSensorADCConfig(i)->vbatresdivmultiplier); + sbufWriteU8(dst, voltageSensorADCConfig(i)->vbatscale); + sbufWriteU8(dst, voltageSensorADCConfig(i)->vbatresdivval); + sbufWriteU8(dst, voltageSensorADCConfig(i)->vbatresdivmultiplier); + } + // if we had any other voltage sensors, this is where we would output any needed configuration } - // if we had any other voltage sensors, this is where we would output any needed configuration - break; + break; case MSP_CURRENT_METER_CONFIG: { // the ADC and VIRTUAL sensors have the same configuration requirements, however this API reflects // that this situation may change and allows us to support configuration of any current sensor with @@ -672,7 +690,7 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce case MSP_OSD_CONFIG: { #define OSD_FLAGS_OSD_FEATURE (1 << 0) -#define OSD_FLAGS_OSD_SLAVE (1 << 1) +//#define OSD_FLAGS_OSD_SLAVE (1 << 1) #define OSD_FLAGS_RESERVED_1 (1 << 2) #define OSD_FLAGS_RESERVED_2 (1 << 3) #define OSD_FLAGS_OSD_HARDWARE_MAX_7456 (1 << 4) @@ -681,9 +699,6 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce #if defined(USE_OSD) osdFlags |= OSD_FLAGS_OSD_FEATURE; #endif -#if defined(USE_OSD_SLAVE) - osdFlags |= OSD_FLAGS_OSD_SLAVE; -#endif #ifdef USE_MAX7456 osdFlags |= OSD_FLAGS_OSD_HARDWARE_MAX_7456; #endif @@ -742,38 +757,6 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce return true; } -#ifdef USE_OSD_SLAVE -static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) -{ - switch (cmdMSP) { - case MSP_STATUS_EX: - case MSP_STATUS: - sbufWriteU16(dst, getTaskDeltaTime(TASK_SERIAL)); -#ifdef USE_I2C - sbufWriteU16(dst, i2cGetErrorCounter()); -#else - sbufWriteU16(dst, 0); -#endif - sbufWriteU16(dst, 0); // sensors - sbufWriteU32(dst, 0); // flight modes - sbufWriteU8(dst, 0); // profile - sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100)); - if (cmdMSP == MSP_STATUS_EX) { - sbufWriteU8(dst, 1); // max profiles - sbufWriteU8(dst, 0); // control rate profile - } else { - sbufWriteU16(dst, 0); // gyro cycle time - } - break; - - default: - return false; - } - return true; -} - -#else - static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) { bool unsupportedCommand = false; @@ -908,11 +891,15 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) case MSP_ALTITUDE: #if defined(USE_BARO) || defined(USE_RANGEFINDER) - sbufWriteU32(dst, getEstimatedAltitude()); + sbufWriteU32(dst, getEstimatedAltitudeCm()); #else sbufWriteU32(dst, 0); #endif +#ifdef USE_VARIO sbufWriteU16(dst, getEstimatedVario()); +#else + sbufWriteU16(dst, 0); +#endif break; case MSP_SONAR_ALTITUDE: @@ -960,7 +947,7 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) break; case MSP_PIDNAMES: - for (const char *c = pidnames; *c; c++) { + for (const char *c = pidNames; *c; c++) { sbufWriteU8(dst, *c); } break; @@ -1006,7 +993,7 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) #if defined(USE_ESC_SENSOR) case MSP_ESC_SENSOR_DATA: - if (feature(FEATURE_ESC_SENSOR)) { + if (featureIsEnabled(FEATURE_ESC_SENSOR)) { sbufWriteU8(dst, getMotorCount()); for (int i = 0; i < getMotorCount(); i++) { const escSensorData_t *escData = getEscSensorData(i); @@ -1033,7 +1020,7 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) sbufWriteU8(dst, gpsSol.numSat); sbufWriteU32(dst, gpsSol.llh.lat); sbufWriteU32(dst, gpsSol.llh.lon); - sbufWriteU16(dst, gpsSol.llh.alt); + sbufWriteU16(dst, (uint16_t)constrain(gpsSol.llh.altCm / 100, 0, UINT16_MAX)); // alt changed from 1m to 0.01m per lsb since MSP API 1.39 by RTH. To maintain backwards compatibility compensate to 1m per lsb in MSP again. sbufWriteU16(dst, gpsSol.groundSpeed); sbufWriteU16(dst, gpsSol.groundCourse); break; @@ -1053,6 +1040,31 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) sbufWriteU8(dst, GPS_svinfo_cno[i]); } break; + +#ifdef USE_GPS_RESCUE + case MSP_GPS_RESCUE: + sbufWriteU16(dst, gpsRescueConfig()->angle); + sbufWriteU16(dst, gpsRescueConfig()->initialAltitudeM); + sbufWriteU16(dst, gpsRescueConfig()->descentDistanceM); + sbufWriteU16(dst, gpsRescueConfig()->rescueGroundspeed); + sbufWriteU16(dst, gpsRescueConfig()->throttleMin); + sbufWriteU16(dst, gpsRescueConfig()->throttleMax); + sbufWriteU16(dst, gpsRescueConfig()->throttleHover); + sbufWriteU16(dst, gpsRescueConfig()->throttleMax); + sbufWriteU8(dst, gpsRescueConfig()->sanityChecks); + sbufWriteU8(dst, gpsRescueConfig()->minSats); + break; + + case MSP_GPS_RESCUE_PIDS: + sbufWriteU16(dst, gpsRescueConfig()->throttleP); + sbufWriteU16(dst, gpsRescueConfig()->throttleI); + sbufWriteU16(dst, gpsRescueConfig()->throttleD); + sbufWriteU16(dst, gpsRescueConfig()->velP); + sbufWriteU16(dst, gpsRescueConfig()->velI); + sbufWriteU16(dst, gpsRescueConfig()->velD); + sbufWriteU16(dst, gpsRescueConfig()->yawP); + break; +#endif #endif case MSP_ACC_TRIM: @@ -1086,8 +1098,27 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) sbufWriteU8(dst, 0); #endif sbufWriteU8(dst, rxConfig()->fpvCamAngleDegrees); - break; + sbufWriteU8(dst, rxConfig()->rcInterpolationChannels); +#if defined(USE_RC_SMOOTHING_FILTER) + sbufWriteU8(dst, rxConfig()->rc_smoothing_type); + sbufWriteU8(dst, rxConfig()->rc_smoothing_input_cutoff); + sbufWriteU8(dst, rxConfig()->rc_smoothing_derivative_cutoff); + sbufWriteU8(dst, rxConfig()->rc_smoothing_input_type); + sbufWriteU8(dst, rxConfig()->rc_smoothing_derivative_type); +#else + sbufWriteU8(dst, 0); + sbufWriteU8(dst, 0); + sbufWriteU8(dst, 0); + sbufWriteU8(dst, 0); + sbufWriteU8(dst, 0); +#endif +#if defined(USE_USB_CDC_HID) + sbufWriteU8(dst, usbDevConfig()->type); +#else + sbufWriteU8(dst, 0); +#endif + break; case MSP_FAILSAFE_CONFIG: sbufWriteU8(dst, failsafeConfig()->failsafe_delay); sbufWriteU8(dst, failsafeConfig()->failsafe_off_delay); @@ -1112,21 +1143,6 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) sbufWriteData(dst, rxConfig()->rcmap, RX_MAPPABLE_CHANNEL_COUNT); break; - case MSP_BF_CONFIG: - sbufWriteU8(dst, mixerConfig()->mixerMode); - - sbufWriteU32(dst, featureMask()); - - sbufWriteU8(dst, rxConfig()->serialrx_provider); - - sbufWriteU16(dst, boardAlignment()->rollDegrees); - sbufWriteU16(dst, boardAlignment()->pitchDegrees); - sbufWriteU16(dst, boardAlignment()->yawDegrees); - - sbufWriteU16(dst, 0); // was currentMeterScale, see MSP_CURRENT_METER_CONFIG - sbufWriteU16(dst, 0); //was currentMeterOffset, see MSP_CURRENT_METER_CONFIG - break; - case MSP_CF_SERIAL_CONFIG: for (int i = 0; i < SERIAL_PORT_COUNT; i++) { if (!serialIsPortAvailable(serialConfig()->portConfigs[i].identifier)) { @@ -1265,8 +1281,8 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) sbufWriteU16(dst, 0); // was pidProfile.yaw_p_limit sbufWriteU8(dst, 0); // reserved sbufWriteU8(dst, currentPidProfile->vbatPidCompensation); - sbufWriteU8(dst, currentPidProfile->setpointRelaxRatio); - sbufWriteU8(dst, MIN(currentPidProfile->dtermSetpointWeight, 255)); + sbufWriteU8(dst, currentPidProfile->feedForwardTransition); + sbufWriteU8(dst, 0); // was low byte of currentPidProfile->dtermSetpointWeight sbufWriteU8(dst, 0); // reserved sbufWriteU8(dst, 0); // reserved sbufWriteU8(dst, 0); // reserved @@ -1276,9 +1292,42 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) sbufWriteU8(dst, 0); // was pidProfile.levelSensitivity sbufWriteU16(dst, currentPidProfile->itermThrottleThreshold); sbufWriteU16(dst, currentPidProfile->itermAcceleratorGain); - sbufWriteU16(dst, currentPidProfile->dtermSetpointWeight); - break; + sbufWriteU16(dst, 0); // was currentPidProfile->dtermSetpointWeight + sbufWriteU8(dst, currentPidProfile->iterm_rotation); +#if defined(USE_SMART_FEEDFORWARD) + sbufWriteU8(dst, currentPidProfile->smart_feedforward); +#else + sbufWriteU8(dst, 0); +#endif +#if defined(USE_ITERM_RELAX) + sbufWriteU8(dst, currentPidProfile->iterm_relax); + sbufWriteU8(dst, currentPidProfile->iterm_relax_type); +#else + sbufWriteU8(dst, 0); + sbufWriteU8(dst, 0); +#endif +#if defined(USE_ABSOLUTE_CONTROL) + sbufWriteU8(dst, currentPidProfile->abs_control_gain); +#else + sbufWriteU8(dst, 0); +#endif +#if defined(USE_THROTTLE_BOOST) + sbufWriteU8(dst, currentPidProfile->throttle_boost); +#else + sbufWriteU8(dst, 0); +#endif +#if defined(USE_ACRO_TRAINER) + sbufWriteU8(dst, currentPidProfile->acro_trainer_angle_limit); +#else + sbufWriteU8(dst, 0); +#endif + sbufWriteU16(dst, currentPidProfile->pid[PID_ROLL].F); + sbufWriteU16(dst, currentPidProfile->pid[PID_PITCH].F); + sbufWriteU16(dst, currentPidProfile->pid[PID_YAW].F); + sbufWriteU8(dst, currentPidProfile->antiGravityMode); + + break; case MSP_SENSOR_CONFIG: sbufWriteU8(dst, accelerometerConfig()->acc_hardware); sbufWriteU8(dst, barometerConfig()->baro_hardware); @@ -1289,21 +1338,24 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) case MSP_VTX_CONFIG: { const vtxDevice_t *vtxDevice = vtxCommonDevice(); + uint8_t pitmode = 0; + vtxDevType_e vtxType = VTXDEV_UNKNOWN; + uint8_t deviceIsReady = 0; if (vtxDevice) { - uint8_t pitmode=0; vtxCommonGetPitMode(vtxDevice, &pitmode); - sbufWriteU8(dst, vtxCommonGetDeviceType(vtxDevice)); - sbufWriteU8(dst, vtxSettingsConfig()->band); - sbufWriteU8(dst, vtxSettingsConfig()->channel); - sbufWriteU8(dst, vtxSettingsConfig()->power); - sbufWriteU8(dst, pitmode); - sbufWriteU16(dst, vtxSettingsConfig()->freq); - // future extensions here... - } else { - sbufWriteU8(dst, VTXDEV_UNKNOWN); // no VTX detected + vtxType = vtxCommonGetDeviceType(vtxDevice); + deviceIsReady = vtxCommonDeviceIsReady(vtxDevice) ? 1 : 0; } + sbufWriteU8(dst, vtxType); + sbufWriteU8(dst, vtxSettingsConfig()->band); + sbufWriteU8(dst, vtxSettingsConfig()->channel); + sbufWriteU8(dst, vtxSettingsConfig()->power); + sbufWriteU8(dst, pitmode); + sbufWriteU16(dst, vtxSettingsConfig()->freq); + sbufWriteU8(dst, deviceIsReady); + sbufWriteU8(dst, vtxSettingsConfig()->lowPowerDisarm); + // future extensions here... } - break; #endif @@ -1344,27 +1396,95 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) return !unsupportedCommand; } -static mspResult_e mspFcProcessOutCommandWithArg(uint8_t cmdMSP, sbuf_t *arg, sbuf_t *dst) +static mspResult_e mspFcProcessOutCommandWithArg(uint8_t cmdMSP, sbuf_t *src, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn) { + switch (cmdMSP) { case MSP_BOXNAMES: { - const int page = sbufBytesRemaining(arg) ? sbufReadU8(arg) : 0; + const int page = sbufBytesRemaining(src) ? sbufReadU8(src) : 0; serializeBoxReply(dst, page, &serializeBoxNameFn); } break; case MSP_BOXIDS: { - const int page = sbufBytesRemaining(arg) ? sbufReadU8(arg) : 0; + const int page = sbufBytesRemaining(src) ? sbufReadU8(src) : 0; serializeBoxReply(dst, page, &serializeBoxPermanentIdFn); } break; + case MSP_REBOOT: + if (sbufBytesRemaining(src)) { + rebootMode = sbufReadU8(src); + + if (rebootMode >= MSP_REBOOT_COUNT +#if !defined(USE_USB_MSC) + || rebootMode == MSP_REBOOT_MSC +#endif + ) { + return MSP_RESULT_ERROR; + } + } else { + rebootMode = MSP_REBOOT_FIRMWARE; + } + + sbufWriteU8(dst, rebootMode); + +#if defined(USE_USB_MSC) + if (rebootMode == MSP_REBOOT_MSC) { + if (mscCheckFilesystemReady()) { + sbufWriteU8(dst, 1); + } else { + sbufWriteU8(dst, 0); + + return MSP_RESULT_ACK; + } + } +#endif + + if (mspPostProcessFn) { + *mspPostProcessFn = mspRebootFn; + } + + break; + case MSP_MULTIPLE_MSP: + { + uint8_t maxMSPs = 0; + if (sbufBytesRemaining(src) == 0) { + return MSP_RESULT_ERROR; + } + int bytesRemaining = sbufBytesRemaining(dst) - 1; // need to keep one byte for checksum + mspPacket_t packetIn, packetOut; + sbufInit(&packetIn.buf, src->end, src->end); + uint8_t* resetInputPtr = src->ptr; + while (sbufBytesRemaining(src) && bytesRemaining > 0) { + uint8_t newMSP = sbufReadU8(src); + sbufInit(&packetOut.buf, dst->ptr, dst->end); + packetIn.cmd = newMSP; + mspFcProcessCommand(&packetIn, &packetOut, NULL); + uint8_t mspSize = sbufPtr(&packetOut.buf) - dst->ptr; + mspSize++; // need to add length information for each MSP + bytesRemaining -= mspSize; + if (bytesRemaining >= 0) { + maxMSPs++; + } + } + src->ptr = resetInputPtr; + sbufInit(&packetOut.buf, dst->ptr, dst->end); + for (int i = 0; i < maxMSPs; i++) { + uint8_t* sizePtr = sbufPtr(&packetOut.buf); + sbufWriteU8(&packetOut.buf, 0); // dummy + packetIn.cmd = sbufReadU8(src); + mspFcProcessCommand(&packetIn, &packetOut, NULL); + (*sizePtr) = sbufPtr(&packetOut.buf) - (sizePtr + 1); + } + dst->ptr = packetOut.buf.ptr; + } + break; default: return MSP_RESULT_CMD_UNKNOWN; } return MSP_RESULT_ACK; } -#endif // USE_OSD_SLAVE #ifdef USE_FLASHFS static void mspFcDataFlashReadCommand(sbuf_t *dst, sbuf_t *src) @@ -1389,30 +1509,6 @@ static void mspFcDataFlashReadCommand(sbuf_t *dst, sbuf_t *src) } #endif -#ifdef USE_OSD_SLAVE -static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) -{ - UNUSED(cmdMSP); - UNUSED(src); - - switch(cmdMSP) { - case MSP_RESET_CONF: - resetEEPROM(); - readEEPROM(); - break; - case MSP_EEPROM_WRITE: - writeEEPROM(); - readEEPROM(); - break; - default: - // we do not know how to handle the (valid) message, indicate error MSP $M! - return MSP_RESULT_ERROR; - } - return MSP_RESULT_ACK; -} - -#else - static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) { uint32_t i; @@ -1532,6 +1628,7 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) } else { return MSP_RESULT_ERROR; } + activeAdjustmentRangeReset(); } else { return MSP_RESULT_ERROR; } @@ -1596,6 +1693,30 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) gpsConfigMutable()->autoConfig = sbufReadU8(src); gpsConfigMutable()->autoBaud = sbufReadU8(src); break; + +#ifdef USE_GPS_RESCUE + case MSP_SET_GPS_RESCUE: + gpsRescueConfigMutable()->angle = sbufReadU16(src); + gpsRescueConfigMutable()->initialAltitudeM = sbufReadU16(src); + gpsRescueConfigMutable()->descentDistanceM = sbufReadU16(src); + gpsRescueConfigMutable()->rescueGroundspeed = sbufReadU16(src); + gpsRescueConfigMutable()->throttleMin = sbufReadU16(src); + gpsRescueConfigMutable()->throttleMax = sbufReadU16(src); + gpsRescueConfigMutable()->throttleHover = sbufReadU16(src); + gpsRescueConfigMutable()->sanityChecks = sbufReadU8(src); + gpsRescueConfigMutable()->minSats = sbufReadU8(src); + break; + + case MSP_SET_GPS_RESCUE_PIDS: + gpsRescueConfigMutable()->throttleP = sbufReadU16(src); + gpsRescueConfigMutable()->throttleI = sbufReadU16(src); + gpsRescueConfigMutable()->throttleD = sbufReadU16(src); + gpsRescueConfigMutable()->velP = sbufReadU16(src); + gpsRescueConfigMutable()->velI = sbufReadU16(src); + gpsRescueConfigMutable()->velD = sbufReadU16(src); + gpsRescueConfigMutable()->yawP = sbufReadU16(src); + break; +#endif #endif #ifdef USE_MAG @@ -1739,8 +1860,8 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) sbufReadU16(src); // was pidProfile.yaw_p_limit sbufReadU8(src); // reserved currentPidProfile->vbatPidCompensation = sbufReadU8(src); - currentPidProfile->setpointRelaxRatio = sbufReadU8(src); - currentPidProfile->dtermSetpointWeight = sbufReadU8(src); + currentPidProfile->feedForwardTransition = sbufReadU8(src); + sbufReadU8(src); // was low byte of currentPidProfile->dtermSetpointWeight sbufReadU8(src); // reserved sbufReadU8(src); // reserved sbufReadU8(src); // reserved @@ -1755,11 +1876,48 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) currentPidProfile->itermAcceleratorGain = sbufReadU16(src); } if (sbufBytesRemaining(src) >= 2) { - currentPidProfile->dtermSetpointWeight = sbufReadU16(src); + sbufReadU16(src); // was currentPidProfile->dtermSetpointWeight + } + if (sbufBytesRemaining(src) >= 14) { + // Added in MSP API 1.40 + currentPidProfile->iterm_rotation = sbufReadU8(src); +#if defined(USE_SMART_FEEDFORWARD) + currentPidProfile->smart_feedforward = sbufReadU8(src); +#else + sbufReadU8(src); +#endif +#if defined(USE_ITERM_RELAX) + currentPidProfile->iterm_relax = sbufReadU8(src); + currentPidProfile->iterm_relax_type = sbufReadU8(src); +#else + sbufReadU8(src); + sbufReadU8(src); +#endif +#if defined(USE_ABSOLUTE_CONTROL) + currentPidProfile->abs_control_gain = sbufReadU8(src); +#else + sbufReadU8(src); +#endif +#if defined(USE_THROTTLE_BOOST) + currentPidProfile->throttle_boost = sbufReadU8(src); +#else + sbufReadU8(src); +#endif +#if defined(USE_ACRO_TRAINER) + currentPidProfile->acro_trainer_angle_limit = sbufReadU8(src); +#else + sbufReadU8(src); +#endif + // PID controller feedforward terms + currentPidProfile->pid[PID_ROLL].F = sbufReadU16(src); + currentPidProfile->pid[PID_PITCH].F = sbufReadU16(src); + currentPidProfile->pid[PID_YAW].F = sbufReadU16(src); + + currentPidProfile->antiGravityMode = sbufReadU8(src); } pidInitConfig(currentPidProfile); - break; + break; case MSP_SET_SENSOR_CONFIG: accelerometerConfigMutable()->acc_hardware = sbufReadU8(src); barometerConfigMutable()->baro_hardware = sbufReadU8(src); @@ -1787,7 +1945,12 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) if (ARMING_FLAG(ARMED)) { return MSP_RESULT_ERROR; } - writeEEPROM(); + + if (featureMaskIsCopied) { + writeEEPROMWithFeatures(featureMaskCopy); + } else { + writeEEPROM(); + } readEEPROM(); break; @@ -1813,29 +1976,36 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) case MSP_SET_VTX_CONFIG: { vtxDevice_t *vtxDevice = vtxCommonDevice(); + vtxDevType_e vtxType = VTXDEV_UNKNOWN; if (vtxDevice) { - if (vtxCommonGetDeviceType(vtxDevice) != VTXDEV_UNKNOWN) { - uint16_t newFrequency = sbufReadU16(src); - if (newFrequency <= VTXCOMMON_MSP_BANDCHAN_CHKVAL) { //value is band and channel - const uint8_t newBand = (newFrequency / 8) + 1; - const uint8_t newChannel = (newFrequency % 8) + 1; - vtxSettingsConfigMutable()->band = newBand; - vtxSettingsConfigMutable()->channel = newChannel; - vtxSettingsConfigMutable()->freq = vtx58_Bandchan2Freq(newBand, newChannel); - } else { //value is frequency in MHz - vtxSettingsConfigMutable()->band = 0; - vtxSettingsConfigMutable()->freq = newFrequency; + vtxType = vtxCommonGetDeviceType(vtxDevice); + } + uint16_t newFrequency = sbufReadU16(src); + if (newFrequency <= VTXCOMMON_MSP_BANDCHAN_CHKVAL) { // Value is band and channel + const uint8_t newBand = (newFrequency / 8) + 1; + const uint8_t newChannel = (newFrequency % 8) + 1; + vtxSettingsConfigMutable()->band = newBand; + vtxSettingsConfigMutable()->channel = newChannel; + vtxSettingsConfigMutable()->freq = vtx58_Bandchan2Freq(newBand, newChannel); + } else if (newFrequency <= VTX_SETTINGS_MAX_FREQUENCY_MHZ) { // Value is frequency in MHz + vtxSettingsConfigMutable()->band = 0; + vtxSettingsConfigMutable()->channel = 0; + vtxSettingsConfigMutable()->freq = newFrequency; + } + + if (sbufBytesRemaining(src) >= 2) { + vtxSettingsConfigMutable()->power = sbufReadU8(src); + if (vtxType != VTXDEV_UNKNOWN) { + // Delegate pitmode to vtx directly + const uint8_t newPitmode = sbufReadU8(src); + uint8_t currentPitmode = 0; + vtxCommonGetPitMode(vtxDevice, ¤tPitmode); + if (currentPitmode != newPitmode) { + vtxCommonSetPitMode(vtxDevice, newPitmode); } - if (sbufBytesRemaining(src) > 1) { - vtxSettingsConfigMutable()->power = sbufReadU8(src); - // Delegate pitmode to vtx directly - const uint8_t newPitmode = sbufReadU8(src); - uint8_t currentPitmode = 0; - vtxCommonGetPitMode(vtxDevice, ¤tPitmode); - if (currentPitmode != newPitmode) { - vtxCommonSetPitMode(vtxDevice, newPitmode); - } + if (sbufBytesRemaining(src)) { + vtxSettingsConfigMutable()->lowPowerDisarm = sbufReadU8(src); } } } @@ -1899,14 +2069,17 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) gpsSol.numSat = sbufReadU8(src); gpsSol.llh.lat = sbufReadU32(src); gpsSol.llh.lon = sbufReadU32(src); - gpsSol.llh.alt = sbufReadU16(src); + gpsSol.llh.altCm = sbufReadU16(src) * 100; // alt changed from 1m to 0.01m per lsb since MSP API 1.39 by RTH. Received MSP altitudes in 1m per lsb have to upscaled. gpsSol.groundSpeed = sbufReadU16(src); - GPS_update |= 2; // New data signalisation to GPS functions // FIXME Magic Numbers + GPS_update |= GPS_MSP_UPDATE; // MSP data signalisation to GPS functions break; #endif // USE_GPS case MSP_SET_FEATURE_CONFIG: - featureClearAll(); - featureSet(sbufReadU32(src)); // features bitmap + featureMaskCopy = sbufReadU32(src); + if (!featureMaskIsCopied) { + featureMaskIsCopied = true; + } + break; #ifdef USE_BEEPER @@ -1967,8 +2140,34 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) if (sbufBytesRemaining(src) >= 1) { rxConfigMutable()->fpvCamAngleDegrees = sbufReadU8(src); } - break; + if (sbufBytesRemaining(src) >= 6) { + // Added in MSP API 1.40 + rxConfigMutable()->rcInterpolationChannels = sbufReadU8(src); +#if defined(USE_RC_SMOOTHING_FILTER) + rxConfigMutable()->rc_smoothing_type = sbufReadU8(src); + rxConfigMutable()->rc_smoothing_input_cutoff = sbufReadU8(src); + rxConfigMutable()->rc_smoothing_derivative_cutoff = sbufReadU8(src); + rxConfigMutable()->rc_smoothing_input_type = sbufReadU8(src); + rxConfigMutable()->rc_smoothing_derivative_type = sbufReadU8(src); +#else + sbufReadU8(src); + sbufReadU8(src); + sbufReadU8(src); + sbufReadU8(src); + sbufReadU8(src); +#endif + } + if (sbufBytesRemaining(src) >= 1) { + // Added in MSP API 1.40 + // Kept separate from the section above to work around missing Configurator support in version < 10.4.2 +#if defined(USE_USB_CDC_HID) + usbDevConfigMutable()->type = sbufReadU8(src); +#else + sbufReadU8(src); +#endif + } + break; case MSP_SET_FAILSAFE_CONFIG: failsafeConfigMutable()->failsafe_delay = sbufReadU8(src); failsafeConfigMutable()->failsafe_off_delay = sbufReadU8(src); @@ -1998,25 +2197,6 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) } break; - case MSP_SET_BF_CONFIG: -#ifdef USE_QUAD_MIXER_ONLY - sbufReadU8(src); // mixerMode ignored -#else - mixerConfigMutable()->mixerMode = sbufReadU8(src); // mixerMode -#endif - - featureClearAll(); - featureSet(sbufReadU32(src)); // features bitmap - - rxConfigMutable()->serialrx_provider = sbufReadU8(src); // serialrx_type - - boardAlignmentMutable()->rollDegrees = sbufReadU16(src); // board_align_roll - boardAlignmentMutable()->pitchDegrees = sbufReadU16(src); // board_align_pitch - boardAlignmentMutable()->yawDegrees = sbufReadU16(src); // board_align_ - sbufReadU16(src); // was currentMeterScale, see MSP_SET_CURRENT_METER_CONFIG - sbufReadU16(src); // was currentMeterOffset see MSP_SET_CURRENT_METER_CONFIG - break; - case MSP_SET_CF_SERIAL_CONFIG: { uint8_t portConfigSize = sizeof(uint8_t) + sizeof(uint16_t) + (sizeof(uint8_t) * 4); @@ -2089,15 +2269,13 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) #ifdef USE_RTC_TIME case MSP_SET_RTC: { - dateTime_t dt; - dt.year = sbufReadU16(src); - dt.month = sbufReadU8(src); - dt.day = sbufReadU8(src); - dt.hours = sbufReadU8(src); - dt.minutes = sbufReadU8(src); - dt.seconds = sbufReadU8(src); - dt.millis = 0; - rtcSetDateTime(&dt); + // Use seconds and milliseconds to make senders + // easier to implement. Generating a 64 bit value + // might not be trivial in some platforms. + int32_t secs = (int32_t)sbufReadU32(src); + uint16_t millis = sbufReadU16(src); + rtcTime_t t = rtcTimeMake(secs, millis); + rtcSet(&t); } break; @@ -2154,10 +2332,10 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) } return MSP_RESULT_ACK; } -#endif // USE_OSD_SLAVE -static mspResult_e mspCommonProcessInCommand(uint8_t cmdMSP, sbuf_t *src) +static mspResult_e mspCommonProcessInCommand(uint8_t cmdMSP, sbuf_t *src, mspPostProcessFnPtr *mspPostProcessFn) { + UNUSED(mspPostProcessFn); const unsigned int dataSize = sbufBytesRemaining(src); UNUSED(dataSize); // maybe unused due to compiler options @@ -2257,7 +2435,7 @@ static mspResult_e mspCommonProcessInCommand(uint8_t cmdMSP, sbuf_t *src) batteryConfigMutable()->currentMeterSource = sbufReadU8(src); break; -#if defined(USE_OSD) || defined (USE_OSD_SLAVE) +#if defined(USE_OSD) case MSP_SET_OSD_CONFIG: { const uint8_t addr = sbufReadU8(src); @@ -2331,7 +2509,7 @@ static mspResult_e mspCommonProcessInCommand(uint8_t cmdMSP, sbuf_t *src) #else return MSP_RESULT_ERROR; #endif -#endif // OSD || USE_OSD_SLAVE +#endif // OSD default: return mspProcessInCommand(cmdMSP, src); @@ -2355,10 +2533,8 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro ret = MSP_RESULT_ACK; } else if (mspProcessOutCommand(cmdMSP, dst)) { ret = MSP_RESULT_ACK; -#ifndef USE_OSD_SLAVE - } else if ((ret = mspFcProcessOutCommandWithArg(cmdMSP, src, dst)) != MSP_RESULT_CMD_UNKNOWN) { + } else if ((ret = mspFcProcessOutCommandWithArg(cmdMSP, src, dst, mspPostProcessFn)) != MSP_RESULT_CMD_UNKNOWN) { /* ret */; -#endif #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE } else if (cmdMSP == MSP_SET_4WAY_IF) { mspFc4waySerialCommand(dst, src, mspPostProcessFn); @@ -2370,7 +2546,7 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro ret = MSP_RESULT_ACK; #endif } else { - ret = mspCommonProcessInCommand(cmdMSP, src); + ret = mspCommonProcessInCommand(cmdMSP, src, mspPostProcessFn); } reply->result = ret; return ret; @@ -2382,7 +2558,6 @@ void mspFcProcessReply(mspPacket_t *reply) UNUSED(src); // potentially unused depending on compile options. switch (reply->cmd) { -#ifndef OSD_SLAVE case MSP_ANALOG: { uint8_t batteryVoltage = sbufReadU8(src); @@ -2400,54 +2575,10 @@ void mspFcProcessReply(mspPacket_t *reply) #endif } break; -#endif - -#ifdef USE_OSD_SLAVE - case MSP_DISPLAYPORT: - { - osdSlaveIsLocked = true; // lock it as soon as a MSP_DISPLAYPORT message is received to prevent accidental CLI/DFU mode. - - const int subCmd = sbufReadU8(src); - - switch (subCmd) { - case 0: // HEARTBEAT - //debug[0]++; - osdSlaveHeartbeat(); - break; - case 1: // RELEASE - //debug[1]++; - break; - case 2: // CLEAR - //debug[2]++; - osdSlaveClearScreen(); - break; - case 3: - { - //debug[3]++; -#define MSP_OSD_MAX_STRING_LENGTH 30 // FIXME move this - const uint8_t y = sbufReadU8(src); // row - const uint8_t x = sbufReadU8(src); // column - sbufReadU8(src); // reserved - char buf[MSP_OSD_MAX_STRING_LENGTH + 1]; - const int len = MIN(sbufBytesRemaining(src), MSP_OSD_MAX_STRING_LENGTH); - sbufReadData(src, &buf, len); - buf[len] = 0; - osdSlaveWrite(x, y, buf); - } - break; - case 4: - osdSlaveDrawScreen(); - break; - } - } - break; -#endif } } void mspInit(void) { -#ifndef USE_OSD_SLAVE initActiveBoxIds(); -#endif } diff --git a/src/main/interface/msp_box.c b/src/main/interface/msp_box.c index 54ef9cec1c..2084b343e5 100644 --- a/src/main/interface/msp_box.c +++ b/src/main/interface/msp_box.c @@ -44,32 +44,31 @@ #include "pg/piniobox.h" -#ifndef USE_OSD_SLAVE // permanent IDs must uniquely identify BOX meaning, DO NOT REUSE THEM! static const box_t boxes[CHECKBOX_ITEM_COUNT] = { { BOXARM, "ARM", 0 }, { BOXANGLE, "ANGLE", 1 }, { BOXHORIZON, "HORIZON", 2 }, - { BOXBARO, "BARO", 3 }, +// { BOXBARO, "BARO", 3 }, { BOXANTIGRAVITY, "ANTI GRAVITY", 4 }, { BOXMAG, "MAG", 5 }, { BOXHEADFREE, "HEADFREE", 6 }, { BOXHEADADJ, "HEADADJ", 7 }, { BOXCAMSTAB, "CAMSTAB", 8 }, - { BOXCAMTRIG, "CAMTRIG", 9 }, - { BOXGPSHOME, "GPS HOME", 10 }, - { BOXGPSHOLD, "GPS HOLD", 11 }, +// { BOXCAMTRIG, "CAMTRIG", 9 }, +// { BOXGPSHOME, "GPS HOME", 10 }, +// { BOXGPSHOLD, "GPS HOLD", 11 }, { BOXPASSTHRU, "PASSTHRU", 12 }, { BOXBEEPERON, "BEEPER", 13 }, - { BOXLEDMAX, "LEDMAX", 14 }, +// { BOXLEDMAX, "LEDMAX", 14 }, (removed) { BOXLEDLOW, "LEDLOW", 15 }, - { BOXLLIGHTS, "LLIGHTS", 16 }, +// { BOXLLIGHTS, "LLIGHTS", 16 }, (removed) { BOXCALIB, "CALIB", 17 }, - { BOXGOV, "GOVERNOR", 18 }, +// { BOXGOV, "GOVERNOR", 18 }, (removed) { BOXOSD, "OSD DISABLE SW", 19 }, { BOXTELEMETRY, "TELEMETRY", 20 }, - { BOXGTUNE, "GTUNE", 21 }, - { BOXRANGEFINDER, "RANGEFINDER", 22 }, +// { BOXGTUNE, "GTUNE", 21 }, (removed) +// { BOXRANGEFINDER, "RANGEFINDER", 22 }, (removed) { BOXSERVO1, "SERVO1", 23 }, { BOXSERVO2, "SERVO2", 24 }, { BOXSERVO3, "SERVO3", 25 }, @@ -95,6 +94,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT] = { { BOXPARALYZE, "PARALYZE", 45 }, { BOXGPSRESCUE, "GPS RESCUE", 46 }, { BOXACROTRAINER, "ACRO TRAINER", 47 }, + { BOXVTXCONTROLDISABLE, "DISABLE VTX CONTROL", 48}, }; // mask of enabled IDs, calculated on startup based on enabled features. boxId_e is used as bit index @@ -166,11 +166,11 @@ void initActiveBoxIds(void) #define BME(boxId) do { bitArraySet(&ena, boxId); } while (0) BME(BOXARM); BME(BOXPREARM); - if (!feature(FEATURE_AIRMODE)) { + if (!featureIsEnabled(FEATURE_AIRMODE)) { BME(BOXAIRMODE); } - if (!feature(FEATURE_ANTI_GRAVITY)) { + if (!featureIsEnabled(FEATURE_ANTI_GRAVITY)) { BME(BOXANTIGRAVITY); } @@ -181,12 +181,6 @@ void initActiveBoxIds(void) BME(BOXHEADADJ); } -#ifdef USE_BARO - if (sensors(SENSOR_BARO)) { - BME(BOXBARO); - } -#endif - #ifdef USE_MAG if (sensors(SENSOR_MAG)) { BME(BOXMAG); @@ -194,17 +188,13 @@ void initActiveBoxIds(void) #endif #ifdef USE_GPS - if (feature(FEATURE_GPS)) { - BME(BOXGPSHOME); - BME(BOXGPSHOLD); - BME(BOXGPSRESCUE); - BME(BOXBEEPGPSCOUNT); - } + if (featureIsEnabled(FEATURE_GPS)) { +#ifdef USE_GPS_RESCUE + if (!featureIsEnabled(FEATURE_3D)) { + BME(BOXGPSRESCUE); + } #endif - -#ifdef USE_RANGEFINDER - if (feature(FEATURE_RANGEFINDER)) { // XXX && sensors(SENSOR_RANGEFINDER)? - BME(BOXRANGEFINDER); + BME(BOXBEEPGPSCOUNT); } #endif @@ -217,7 +207,7 @@ void initActiveBoxIds(void) BME(BOXBEEPERON); #ifdef USE_LED_STRIP - if (feature(FEATURE_LED_STRIP)) { + if (featureIsEnabled(FEATURE_LED_STRIP)) { BME(BOXLEDLOW); } #endif @@ -231,7 +221,7 @@ void initActiveBoxIds(void) BME(BOXFPVANGLEMIX); - if (feature(FEATURE_3D)) { + if (featureIsEnabled(FEATURE_3D)) { BME(BOX3D); } @@ -239,18 +229,18 @@ void initActiveBoxIds(void) BME(BOXFLIPOVERAFTERCRASH); } - if (feature(FEATURE_SERVO_TILT)) { + if (featureIsEnabled(FEATURE_SERVO_TILT)) { BME(BOXCAMSTAB); } - if (feature(FEATURE_INFLIGHT_ACC_CAL)) { + if (featureIsEnabled(FEATURE_INFLIGHT_ACC_CAL)) { BME(BOXCALIB); } BME(BOXOSD); #ifdef USE_TELEMETRY - if (feature(FEATURE_TELEMETRY)) { + if (featureIsEnabled(FEATURE_TELEMETRY)) { BME(BOXTELEMETRY); } #endif @@ -271,6 +261,7 @@ void initActiveBoxIds(void) #if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) BME(BOXVTXPITMODE); + BME(BOXVTXCONTROLDISABLE); #endif BME(BOXPARALYZE); @@ -353,4 +344,3 @@ int packFlightModeFlags(boxBitmask_t *mspFlightModeFlags) // return count of used bits return mspBoxIdx; } -#endif // USE_OSD_SLAVE diff --git a/src/main/interface/msp_protocol.h b/src/main/interface/msp_protocol.h index bae04e0c1d..59cac16f4b 100644 --- a/src/main/interface/msp_protocol.h +++ b/src/main/interface/msp_protocol.h @@ -62,7 +62,7 @@ #define MSP_PROTOCOL_VERSION 0 #define API_VERSION_MAJOR 1 // increment when major changes are made -#define API_VERSION_MINOR 39 // increment after a release, to set the version for all changes to go into the following release (if no changes to MSP are made between the releases, this can be reverted before the release) +#define API_VERSION_MINOR 41 // increment after a release, to set the version for all changes to go into the following release (if no changes to MSP are made between the releases, this can be reverted before the release) #define API_VERSION_LENGTH 2 @@ -161,10 +161,9 @@ #define MSP_RX_MAP 64 //out message get channel map (also returns number of channels total) #define MSP_SET_RX_MAP 65 //in message set rx map, numchannels to set comes from MSP_RX_MAP -// FIXME - Provided for backwards compatibility with configurator code until configurator is updated. // DEPRECATED - DO NOT USE "MSP_BF_CONFIG" and MSP_SET_BF_CONFIG. In Cleanflight, isolated commands already exist and should be used instead. -#define MSP_BF_CONFIG 66 //out message baseflight-specific settings that aren't covered elsewhere -#define MSP_SET_BF_CONFIG 67 //in message baseflight-specific settings save +// DEPRECATED - #define MSP_BF_CONFIG 66 //out message baseflight-specific settings that aren't covered elsewhere +// DEPRECATED - #define MSP_SET_BF_CONFIG 67 //in message baseflight-specific settings save #define MSP_REBOOT 68 //in message reboot settings @@ -280,6 +279,8 @@ #define MSP_GPS_CONFIG 132 //out message GPS configuration #define MSP_COMPASS_CONFIG 133 //out message Compass configuration #define MSP_ESC_SENSOR_DATA 134 //out message Extra ESC data from 32-Bit ESCs (Temperature, RPM) +#define MSP_GPS_RESCUE 135 //out message GPS Rescues's angle, initialAltitude, descentDistance, rescueGroundSpeed, sanityChecks and minSats +#define MSP_GPS_RESCUE_PIDS 136 //out message GPS Rescues's throttleP and velocity PIDS + yaw P #define MSP_SET_RAW_RC 200 //in message 8 rc chan #define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed @@ -306,6 +307,8 @@ #define MSP_SET_MOTOR_CONFIG 222 //out message Motor configuration (min/max throttle, etc) #define MSP_SET_GPS_CONFIG 223 //out message GPS configuration #define MSP_SET_COMPASS_CONFIG 224 //out message Compass configuration +#define MSP_SET_GPS_RESCUE 225 //in message GPS Rescues's angle, initialAltitude, descentDistance, rescueGroundSpeed, sanityChecks and minSats +#define MSP_SET_GPS_RESCUE_PIDS 226 //in message GPS Rescues's throttleP and velocity PIDS + yaw P // #define MSP_BIND 240 //in message no param // #define MSP_ALARMS 242 @@ -322,6 +325,7 @@ #define MSP_UID 160 //out message Unique device ID #define MSP_GPSSVINFO 164 //out message get Signal Strength (only U-Blox) #define MSP_GPSSTATISTICS 166 //out message get GPS debugging data +#define MSP_MULTIPLE_MSP 230 //out message request multiple MSPs in one request - limit is the TX buffer; returns each MSP in the order they were requested starting with length of MSP; MSPs with input arguments are not supported #define MSP_ACC_TRIM 240 //out message get acc angle trim values #define MSP_SET_ACC_TRIM 239 //in message set acc angle trim values #define MSP_SERVO_MIX_RULES 241 //out message Returns servo mixer configuration diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index e86edc331d..50d60ad3ed 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -41,7 +41,7 @@ #include "fc/config.h" #include "fc/controlrate_profile.h" -#include "fc/fc_core.h" +#include "fc/core.h" #include "fc/rc_adjustments.h" #include "fc/rc_controls.h" @@ -70,6 +70,7 @@ #include "pg/beeper_dev.h" #include "pg/dashboard.h" #include "pg/flash.h" +#include "pg/gyrodev.h" #include "pg/max7456.h" #include "pg/pg.h" #include "pg/pg_ids.h" @@ -82,9 +83,11 @@ #include "pg/vcd.h" #include "pg/usb.h" #include "pg/sdio.h" +#include "pg/rcdevice.h" #include "rx/rx.h" #include "rx/cc2500_frsky_common.h" +#include "rx/cc2500_sfhss.h" #include "rx/spektrum.h" #include "sensors/acceleration.h" @@ -124,7 +127,7 @@ const char * const lookupTableBaroHardware[] = { #if defined(USE_SENSOR_NAMES) || defined(USE_MAG) // sync with magSensor_e const char * const lookupTableMagHardware[] = { - "AUTO", "NONE", "HMC5883", "AK8975", "AK8963", "QMC5883" + "AUTO", "NONE", "HMC5883", "AK8975", "AK8963", "QMC5883", "LIS3MDL" }; #endif #if defined(USE_SENSOR_NAMES) || defined(USE_RANGEFINDER) @@ -157,7 +160,7 @@ static const char * const lookupTableAlignment[] = { "CW270FLIP" }; -#ifdef USE_DUAL_GYRO +#ifdef USE_MULTI_GYRO static const char * const lookupTableGyro[] = { "FIRST", "SECOND", "BOTH" }; @@ -165,7 +168,7 @@ static const char * const lookupTableGyro[] = { #ifdef USE_GPS static const char * const lookupTableGPSProvider[] = { - "NMEA", "UBLOX" + "NMEA", "UBLOX", "MSP" }; static const char * const lookupTableGPSSBASMode[] = { @@ -222,17 +225,20 @@ static const char * const lookupTableRxSpi[] = { "FRSKY_X", "FLYSKY", "FLYSKY_2A", - "KN" + "KN", + "SFHSS" }; #endif static const char * const lookupTableGyroHardwareLpf[] = { "NORMAL", - "EXPERIMENTAL", - "1KHZ_SAMPLING" + "1KHZ_SAMPLING", +#ifdef USE_GYRO_DLPF_EXPERIMENTAL + "EXPERIMENTAL" +#endif }; -#ifdef USE_32K_CAPABLE_GYRO +#if defined(USE_32K_CAPABLE_GYRO) && defined(USE_GYRO_DLPF_EXPERIMENTAL) static const char * const lookupTableGyro32khzHardwareLpf[] = { "NORMAL", "EXPERIMENTAL" @@ -272,6 +278,10 @@ static const char * const lookupTableDtermLowpassType[] = { "BIQUAD", }; +static const char * const lookupTableAntiGravityMode[] = { + "SMOOTH", + "STEP", +}; static const char * const lookupTableFailsafe[] = { "AUTO-LAND", "DROP", "GPS-RESCUE" @@ -282,7 +292,10 @@ static const char * const lookupTableFailsafeSwitchMode[] = { }; static const char * const lookupTableBusType[] = { - "NONE", "I2C", "SPI", "SLAVE" + "NONE", "I2C", "SPI", "SLAVE", +#if defined(USE_SPI_GYRO) && defined(USE_I2C_GYRO) + "GYROAUTO" +#endif }; #ifdef USE_MAX7456 @@ -339,7 +352,7 @@ static const char * const lookupTableVideoSystem[] = { #if defined(USE_ITERM_RELAX) static const char * const lookupTableItermRelax[] = { - "OFF", "RP", "RPY" + "OFF", "RP", "RPY", "RP_INC", "RPY_INC" }; static const char * const lookupTableItermRelaxType[] = { "GYRO", "SETPOINT" @@ -367,6 +380,27 @@ static const char * const lookupTableRcSmoothingDerivativeType[] = { }; #endif // USE_RC_SMOOTHING_FILTER +#ifdef USE_GYRO_DATA_ANALYSE +static const char * const lookupTableDynamicFftLocation[] = { + "BEFORE_STATIC_FILTERS", "AFTER_STATIC_FILTERS" +}; +static const char * const lookupTableDynamicFilterRange[] = { + "HIGH", "MEDIUM", "LOW" +}; +#endif // USE_GYRO_DATA_ANALYSE + +#ifdef USE_VTX_COMMON +static const char * const lookupTableVtxLowPowerDisarm[] = { + "OFF", "ON", "UNTIL_FIRST_ARM" +}; +#endif + +#ifdef USE_SDCARD +static const char * const lookupTableSdcardMode[] = { + "OFF", "SPI", "SDIO" +}; +#endif + #define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) } const lookupTableEntry_t lookupTables[] = { @@ -396,7 +430,7 @@ const lookupTableEntry_t lookupTables[] = { LOOKUP_TABLE_ENTRY(lookupTableRxSpi), #endif LOOKUP_TABLE_ENTRY(lookupTableGyroHardwareLpf), -#ifdef USE_32K_CAPABLE_GYRO +#if defined(USE_32K_CAPABLE_GYRO) && defined(USE_GYRO_DLPF_EXPERIMENTAL) LOOKUP_TABLE_ENTRY(lookupTableGyro32khzHardwareLpf), #endif LOOKUP_TABLE_ENTRY(lookupTableAccHardware), @@ -412,6 +446,7 @@ const lookupTableEntry_t lookupTables[] = { LOOKUP_TABLE_ENTRY(lookupTableRcInterpolationChannels), LOOKUP_TABLE_ENTRY(lookupTableLowpassType), LOOKUP_TABLE_ENTRY(lookupTableDtermLowpassType), + LOOKUP_TABLE_ENTRY(lookupTableAntiGravityMode), LOOKUP_TABLE_ENTRY(lookupTableFailsafe), LOOKUP_TABLE_ENTRY(lookupTableFailsafeSwitchMode), LOOKUP_TABLE_ENTRY(lookupTableCrashRecovery), @@ -435,7 +470,7 @@ const lookupTableEntry_t lookupTables[] = { #ifdef USE_LED_STRIP LOOKUP_TABLE_ENTRY(lookupLedStripFormatRGB), #endif -#ifdef USE_DUAL_GYRO +#ifdef USE_MULTI_GYRO LOOKUP_TABLE_ENTRY(lookupTableGyro), #endif LOOKUP_TABLE_ENTRY(lookupTableThrottleLimitType), @@ -455,6 +490,17 @@ const lookupTableEntry_t lookupTables[] = { LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingInputType), LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingDerivativeType), #endif // USE_RC_SMOOTHING_FILTER +#ifdef USE_GYRO_DATA_ANALYSE + LOOKUP_TABLE_ENTRY(lookupTableDynamicFftLocation), + LOOKUP_TABLE_ENTRY(lookupTableDynamicFilterRange), +#endif // USE_GYRO_DATA_ANALYSE +#ifdef USE_VTX_COMMON + LOOKUP_TABLE_ENTRY(lookupTableVtxLowPowerDisarm), +#endif + LOOKUP_TABLE_ENTRY(lookupTableGyroHardware), +#ifdef USE_SDCARD + LOOKUP_TABLE_ENTRY(lookupTableSdcardMode), +#endif }; #undef LOOKUP_TABLE_ENTRY @@ -463,7 +509,7 @@ const clivalue_t valueTable[] = { // PG_GYRO_CONFIG { "align_gyro", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_align) }, { "gyro_hardware_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO_HARDWARE_LPF }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_hardware_lpf) }, -#ifdef USE_32K_CAPABLE_GYRO +#if defined(USE_32K_CAPABLE_GYRO) && defined(USE_GYRO_DLPF_EXPERIMENTAL) { "gyro_32khz_hardware_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO_32KHZ_HARDWARE_LPF }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_32khz_hardware_lpf) }, #endif #if defined(USE_GYRO_SPI_ICM20649) @@ -493,14 +539,23 @@ const clivalue_t valueTable[] = { { "yaw_spin_threshold", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 500, 1950 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, yaw_spin_threshold) }, #endif -#if defined(GYRO_USES_SPI) -#ifdef USE_32K_CAPABLE_GYRO +#if defined(GYRO_USES_SPI) && defined(USE_32K_CAPABLE_GYRO) { "gyro_use_32khz", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_use_32khz) }, #endif -#endif -#ifdef USE_DUAL_GYRO +#ifdef USE_MULTI_GYRO { "gyro_to_use", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_to_use) }, #endif +#if defined(USE_GYRO_DATA_ANALYSE) + { "dyn_fft_location", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DYNAMIC_FFT_LOCATION }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_fft_location) }, + { "dyn_filter_width_percent", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 99 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_width_percent) }, + { "dyn_filter_range", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DYNAMIC_FILTER_RANGE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_range) }, +#endif +#ifdef USE_DYN_LPF + { "dyn_lpf_gyro_max_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_lpf_gyro_max_hz) }, + { "dyn_lpf_gyro_idle", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_lpf_gyro_idle) }, + { "dyn_lpf_dterm_max_hz", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dyn_lpf_dterm_max_hz) }, + { "dyn_lpf_dterm_idle", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, dyn_lpf_dterm_idle) }, +#endif // PG_ACCELEROMETER_CONFIG { "align_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_align) }, @@ -560,7 +615,7 @@ const clivalue_t valueTable[] = { { "rc_smoothing_derivative_type",VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RC_SMOOTHING_DERIVATIVE_TYPE }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_derivative_type) }, #endif // USE_RC_SMOOTHING_FILTER - { "fpv_mix_degrees", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 50 }, PG_RX_CONFIG, offsetof(rxConfig_t, fpvCamAngleDegrees) }, + { "fpv_mix_degrees", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 90 }, PG_RX_CONFIG, offsetof(rxConfig_t, fpvCamAngleDegrees) }, { "max_aux_channels", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, MAX_AUX_CHANNEL_COUNT }, PG_RX_CONFIG, offsetof(rxConfig_t, max_aux_channel) }, #ifdef USE_SERIAL_RX { "serialrx_provider", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_SERIAL_RX }, PG_RX_CONFIG, offsetof(rxConfig_t, serialrx_provider) }, @@ -580,7 +635,7 @@ const clivalue_t valueTable[] = { #endif // PG_ADC_CONFIG -#if defined(ADC) +#if defined(USE_ADC) { "adc_device", VAR_INT8 | MASTER_VALUE, .config.minmax = { 0, ADCDEV_COUNT }, PG_ADC_CONFIG, offsetof(adcConfig_t, device) }, #endif @@ -649,6 +704,7 @@ const clivalue_t valueTable[] = { { "use_cbat_alerts", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, useConsumptionAlerts) }, { "cbat_alert_percent", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, consumptionWarningPercentage) }, { "vbat_cutoff_percent", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, lvcPercentage) }, + { "force_battery_cell_count", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 24 }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, forceBatteryCellCount) }, // PG_VOLTAGE_SENSOR_ADC_CONFIG { "vbat_scale", VAR_UINT8 | MASTER_VALUE, .config.minmax = { VBAT_SCALE_MIN, VBAT_SCALE_MAX }, PG_VOLTAGE_SENSOR_ADC_CONFIG, offsetof(voltageSensorADCConfig_t, vbatscale) }, @@ -684,7 +740,7 @@ const clivalue_t valueTable[] = { { "3d_deadband_low", VAR_UINT16 | MASTER_VALUE, .config.minmax = { PWM_PULSE_MIN, PWM_RANGE_MIDDLE }, PG_MOTOR_3D_CONFIG, offsetof(flight3DConfig_t, deadband3d_low) }, { "3d_deadband_high", VAR_UINT16 | MASTER_VALUE, .config.minmax = { PWM_RANGE_MIDDLE, PWM_PULSE_MAX }, PG_MOTOR_3D_CONFIG, offsetof(flight3DConfig_t, deadband3d_high) }, { "3d_neutral", VAR_UINT16 | MASTER_VALUE, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_MOTOR_3D_CONFIG, offsetof(flight3DConfig_t, neutral3d) }, - { "3d_deadband_throttle", VAR_UINT16 | MASTER_VALUE, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_MOTOR_3D_CONFIG, offsetof(flight3DConfig_t, deadband3d_throttle) }, + { "3d_deadband_throttle", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1, 100 }, PG_MOTOR_3D_CONFIG, offsetof(flight3DConfig_t, deadband3d_throttle) }, { "3d_limit_low", VAR_UINT16 | MASTER_VALUE, .config.minmax = { PWM_PULSE_MIN, PWM_RANGE_MIDDLE }, PG_MOTOR_3D_CONFIG, offsetof(flight3DConfig_t, limit3d_low) }, { "3d_limit_high", VAR_UINT16 | MASTER_VALUE, .config.minmax = { PWM_RANGE_MIDDLE, PWM_PULSE_MAX }, PG_MOTOR_3D_CONFIG, offsetof(flight3DConfig_t, limit3d_high) }, { "3d_switched_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MOTOR_3D_CONFIG, offsetof(flight3DConfig_t, switched_mode3d) }, @@ -715,15 +771,15 @@ const clivalue_t valueTable[] = { { "tpa_breakpoint", VAR_UINT16 | PROFILE_RATE_VALUE, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, tpa_breakpoint) }, { "throttle_limit_type", VAR_UINT8 | PROFILE_RATE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_THROTTLE_LIMIT_TYPE }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, throttle_limit_type) }, { "throttle_limit_percent", VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmax = { 25, 100 }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, throttle_limit_percent) }, + { "roll_rate_limit", VAR_UINT16 | PROFILE_RATE_VALUE, .config.minmax = { CONTROL_RATE_CONFIG_RATE_LIMIT_MIN, CONTROL_RATE_CONFIG_RATE_LIMIT_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rate_limit[FD_ROLL]) }, + { "pitch_rate_limit", VAR_UINT16 | PROFILE_RATE_VALUE, .config.minmax = { CONTROL_RATE_CONFIG_RATE_LIMIT_MIN, CONTROL_RATE_CONFIG_RATE_LIMIT_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rate_limit[FD_PITCH]) }, + { "yaw_rate_limit", VAR_UINT16 | PROFILE_RATE_VALUE, .config.minmax = { CONTROL_RATE_CONFIG_RATE_LIMIT_MIN, CONTROL_RATE_CONFIG_RATE_LIMIT_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rate_limit[FD_YAW]) }, // PG_SERIAL_CONFIG { "reboot_character", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 48, 126 }, PG_SERIAL_CONFIG, offsetof(serialConfig_t, reboot_character) }, { "serial_update_rate_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 100, 2000 }, PG_SERIAL_CONFIG, offsetof(serialConfig_t, serial_update_rate_hz) }, // PG_IMU_CONFIG - { "accxy_deadband", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_IMU_CONFIG, offsetof(imuConfig_t, accDeadband.xy) }, - { "accz_deadband", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_IMU_CONFIG, offsetof(imuConfig_t, accDeadband.z) }, - { "acc_unarmedcal", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_IMU_CONFIG, offsetof(imuConfig_t, acc_unarmedcal) }, { "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 32000 }, PG_IMU_CONFIG, offsetof(imuConfig_t, dcm_kp) }, { "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 32000 }, PG_IMU_CONFIG, offsetof(imuConfig_t, dcm_ki) }, { "small_angle", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 180 }, PG_IMU_CONFIG, offsetof(imuConfig_t, small_angle) }, @@ -744,8 +800,8 @@ const clivalue_t valueTable[] = { #ifdef USE_GPS_RESCUE // PG_GPS_RESCUE { "gps_rescue_angle", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 200 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, angle) }, - { "gps_rescue_initial_alt", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 20, 100 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, initialAltitude) }, - { "gps_rescue_descent_dist", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 30, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, descentDistance) }, + { "gps_rescue_initial_alt", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 20, 100 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, initialAltitudeM) }, + { "gps_rescue_descent_dist", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 30, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, descentDistanceM) }, { "gps_rescue_ground_speed", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 30, 3000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, rescueGroundspeed) }, { "gps_rescue_throttle_p", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleP) }, { "gps_rescue_throttle_i", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleI) }, @@ -760,6 +816,7 @@ const clivalue_t valueTable[] = { { "gps_rescue_throttle_hover", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleHover) }, { "gps_rescue_sanity_checks", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_RESCUE }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, sanityChecks) }, { "gps_rescue_min_sats", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 50 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, minSats) }, + { "gps_rescue_min_dth", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 50, 1000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, minRescueDth) }, #endif #endif @@ -778,15 +835,16 @@ const clivalue_t valueTable[] = { // PG_PID_PROFILE { "dterm_lowpass_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DTERM_LOWPASS_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_filter_type) }, { "dterm_lowpass_hz", VAR_INT16 | PROFILE_VALUE, .config.minmax = { 0, 16000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_lowpass_hz) }, + { "dterm_lowpass2_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DTERM_LOWPASS_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_filter2_type) }, { "dterm_lowpass2_hz", VAR_INT16 | PROFILE_VALUE, .config.minmax = { 0, 16000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_lowpass2_hz) }, { "dterm_notch_hz", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 16000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_notch_hz) }, { "dterm_notch_cutoff", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 16000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_notch_cutoff) }, { "vbat_pid_gain", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, vbatPidCompensation) }, { "pid_at_min_throttle", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, pidAtMinThrottle) }, + { "anti_gravity_mode", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ANTI_GRAVITY_MODE }, PG_PID_PROFILE, offsetof(pidProfile_t, antiGravityMode) }, { "anti_gravity_threshold", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 20, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermThrottleThreshold) }, { "anti_gravity_gain", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 1000, 30000 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermAcceleratorGain) }, - { "setpoint_relax_ratio", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, setpointRelaxRatio) }, - { "dterm_setpoint_weight", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dtermSetpointWeight) }, + { "feedforward_transition", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, feedForwardTransition) }, { "acc_limit_yaw", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, yawRateAccelLimit) }, { "acc_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, rateAccelLimit) }, { "crash_dthreshold", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_dthreshold) }, @@ -806,7 +864,7 @@ const clivalue_t valueTable[] = { #if defined(USE_ITERM_RELAX) { "iterm_relax", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ITERM_RELAX }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax) }, { "iterm_relax_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ITERM_RELAX_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax_type) }, - { "iterm_relax_cutoff", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax_cutoff) }, + { "iterm_relax_cutoff", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax_cutoff) }, #endif { "iterm_windup", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 30, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermWindupPointPercent) }, { "iterm_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermLimit) }, @@ -829,16 +887,19 @@ const clivalue_t valueTable[] = { { "p_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].P) }, { "i_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].I) }, { "d_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].D) }, + { "f_pitch", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 2000 },PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].F) }, { "p_roll", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_ROLL].P) }, { "i_roll", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_ROLL].I) }, { "d_roll", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_ROLL].D) }, + { "f_roll", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 2000 },PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_ROLL].F) }, { "p_yaw", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_YAW].P) }, { "i_yaw", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_YAW].I) }, { "d_yaw", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_YAW].D) }, + { "f_yaw", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 2000 },PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_YAW].F) }, - { "p_level", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].P) }, - { "i_level", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].I) }, - { "d_level", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].D) }, + { "angle_level_strength", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].P) }, + { "horizon_level_strength", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].I) }, + { "horizon_transition", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].D) }, { "level_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 10, 90 }, PG_PID_PROFILE, offsetof(pidProfile_t, levelAngleLimit) }, @@ -871,6 +932,15 @@ const clivalue_t valueTable[] = { #if defined(USE_TELEMETRY_IBUS) { "ibus_sensor", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = IBUS_SENSOR_COUNT, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, flysky_sensors)}, #endif +#if defined(USE_TELEMETRY_SMARTPORT) + { "smartport_use_extra_sensors", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, smartport_use_extra_sensors)}, +#endif +#ifdef USE_TELEMETRY_MAVLINK + // Support for misusing the heading field in MAVlink to indicate mAh drawn for Connex Prosight OSD + // Set to 10 to show a tenth of your capacity drawn. + // Set to $size_of_battery to get a percentage of battery used. + { "mavlink_mah_as_heading_divisor", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 30000 }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, mavlink_mah_as_heading_divisor) }, +#endif #endif // USE_TELEMETRY // PG_LED_STRIP_CONFIG @@ -881,7 +951,12 @@ const clivalue_t valueTable[] = { // PG_SDCARD_CONFIG #ifdef USE_SDCARD + { "sdcard_detect_inverted", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_SDCARD_CONFIG, offsetof(sdcardConfig_t, cardDetectInverted) }, + { "sdcard_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_SDCARD_MODE }, PG_SDCARD_CONFIG, offsetof(sdcardConfig_t, mode) }, +#endif +#ifdef USE_SDCARD_SPI { "sdcard_dma", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_SDCARD_CONFIG, offsetof(sdcardConfig_t, useDma) }, + { "sdcard_spi_bus", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, SPIDEV_COUNT }, PG_SDCARD_CONFIG, offsetof(sdcardConfig_t, device) }, #endif #ifdef USE_SDCARD_SDIO { "sdio_clk_bypass", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_SDIO_CONFIG, offsetof(sdioConfig_t, clockBypass) }, @@ -902,6 +977,7 @@ const clivalue_t valueTable[] = { #ifdef USE_ADC_INTERNAL { "osd_warn_core_temp", VAR_UINT16 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_WARNING_CORE_TEMPERATURE, PG_OSD_CONFIG, offsetof(osdConfig_t, enabledWarnings)}, #endif + { "osd_warn_fail_safe", VAR_UINT16 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_WARNING_FAIL_SAFE, PG_OSD_CONFIG, offsetof(osdConfig_t, enabledWarnings)}, { "osd_rssi_alarm", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_OSD_CONFIG, offsetof(osdConfig_t, rssi_alarm) }, { "osd_cap_alarm", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 20000 }, PG_OSD_CONFIG, offsetof(osdConfig_t, cap_alarm) }, @@ -926,6 +1002,7 @@ const clivalue_t valueTable[] = { { "osd_remaining_time_estimate_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_REMAINING_TIME_ESTIMATE]) }, { "osd_flymode_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_FLYMODE]) }, { "osd_anti_gravity_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_ANTI_GRAVITY]) }, + { "osd_g_force_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_G_FORCE]) }, { "osd_throttle_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_THROTTLE_POS]) }, { "osd_vtx_channel_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_VTX_CHANNEL]) }, { "osd_crosshairs_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_CROSSHAIRS]) }, @@ -956,14 +1033,20 @@ const clivalue_t valueTable[] = { { "osd_battery_usage_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_MAIN_BATT_USAGE]) }, { "osd_disarmed_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_DISARMED]) }, { "osd_nheading_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_NUMERICAL_HEADING]) }, +#ifdef USE_VARIO { "osd_nvario_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_NUMERICAL_VARIO]) }, +#endif { "osd_esc_tmp_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_ESC_TMP]) }, { "osd_esc_rpm_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_ESC_RPM]) }, { "osd_rtc_date_time_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_RTC_DATETIME]) }, { "osd_adjustment_range_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_ADJUSTMENT_RANGE]) }, + { "osd_flip_arrow_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_FLIP_ARROW]) }, #ifdef USE_ADC_INTERNAL { "osd_core_temp_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_CORE_TEMPERATURE]) }, #endif +#ifdef USE_BLACKBOX + { "osd_log_status_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_LOG_STATUS]) }, +#endif // OSD stats enabled flags are stored as bitmapped values inside a 32bit parameter // It is recommended to keep the settings order the same as the enumeration. This way the settings are displayed in the cli in the same order making it easier on the users @@ -981,11 +1064,14 @@ const clivalue_t valueTable[] = { { "osd_stat_max_alt", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_MAX_ALTITUDE, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)}, { "osd_stat_bbox", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_BLACKBOX, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)}, { "osd_stat_bb_no", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_BLACKBOX_NUMBER, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)}, + { "osd_stat_max_g_force", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_MAX_G_FORCE, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)}, + { "osd_stat_max_esc_temp", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_MAX_ESC_TEMP, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)}, + { "osd_stat_max_esc_rpm", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_MAX_ESC_RPM, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)}, #endif // PG_SYSTEM_CONFIG -#ifndef SKIP_TASK_STATISTICS +#if defined(USE_TASK_STATISTICS) { "task_statistics", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_SYSTEM_CONFIG, offsetof(systemConfig_t, task_statistics) }, #endif { "debug_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DEBUG }, PG_SYSTEM_CONFIG, offsetof(systemConfig_t, debug_mode) }, @@ -1000,7 +1086,7 @@ const clivalue_t valueTable[] = { { "vtx_band", VAR_UINT8 | MASTER_VALUE, .config.minmax = { VTX_SETTINGS_NO_BAND, VTX_SETTINGS_MAX_BAND }, PG_VTX_SETTINGS_CONFIG, offsetof(vtxSettingsConfig_t, band) }, { "vtx_channel", VAR_UINT8 | MASTER_VALUE, .config.minmax = { VTX_SETTINGS_MIN_CHANNEL, VTX_SETTINGS_MAX_CHANNEL }, PG_VTX_SETTINGS_CONFIG, offsetof(vtxSettingsConfig_t, channel) }, { "vtx_power", VAR_UINT8 | MASTER_VALUE, .config.minmax = { VTX_SETTINGS_MIN_POWER, VTX_SETTINGS_POWER_COUNT-1 }, PG_VTX_SETTINGS_CONFIG, offsetof(vtxSettingsConfig_t, power) }, - { "vtx_low_power_disarm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_VTX_SETTINGS_CONFIG, offsetof(vtxSettingsConfig_t, lowPowerDisarm) }, + { "vtx_low_power_disarm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_VTX_LOW_POWER_DISARM }, PG_VTX_SETTINGS_CONFIG, offsetof(vtxSettingsConfig_t, lowPowerDisarm) }, #ifdef VTX_SETTINGS_FREQCMD { "vtx_freq", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, VTX_SETTINGS_MAX_FREQUENCY_MHZ }, PG_VTX_SETTINGS_CONFIG, offsetof(vtxSettingsConfig_t, freq) }, { "vtx_pit_mode_freq", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, VTX_SETTINGS_MAX_FREQUENCY_MHZ }, PG_VTX_SETTINGS_CONFIG, offsetof(vtxSettingsConfig_t, pitModeFreq) }, @@ -1092,10 +1178,29 @@ const clivalue_t valueTable[] = { #ifdef USE_FLASH { "flash_spi_bus", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, SPIDEV_COUNT }, PG_FLASH_CONFIG, offsetof(flashConfig_t, spiDevice) }, #endif +// RCDEVICE +#ifdef USE_RCDEVICE + { "rcdevice_init_dev_attempts", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 10 }, PG_RCDEVICE_CONFIG, offsetof(rcdeviceConfig_t, initDeviceAttempts) }, + { "rcdevice_init_dev_attempt_interval", VAR_UINT32 | MASTER_VALUE, .config.minmax = { 500, 5000 }, PG_RCDEVICE_CONFIG, offsetof(rcdeviceConfig_t, initDeviceAttemptInterval) }, +#endif + +// PG_GYRO_DEVICE_CONFIG + { "gyro_1_bustype", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BUS_TYPE }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, bustype) }, + { "gyro_1_spibus", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, SPIDEV_COUNT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, spiBus) }, + { "gyro_1_i2cBus", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, I2CDEV_COUNT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, i2cBus) }, + { "gyro_1_i2c_address", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, I2C_ADDR7_MAX }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, i2cAddress) }, + { "gyro_1_sensor_align", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, align) }, +#ifdef USE_MULTI_GYRO + { "gyro_2_bustype", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BUS_TYPE }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 1, bustype) }, + { "gyro_2_spibus", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, SPIDEV_COUNT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 1, spiBus) }, + { "gyro_2_i2cBus", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, I2CDEV_COUNT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 1, i2cBus) }, + { "gyro_2_i2c_address", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, I2C_ADDR7_MAX }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 1, i2cAddress) }, + { "gyro_2_sensor_align", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 1, align) }, +#endif }; const uint16_t valueTableEntryCount = ARRAYLEN(valueTable); void settingsBuildCheck() { - BUILD_BUG_ON(LOOKUP_TABLE_COUNT != ARRAYLEN(lookupTables)); + STATIC_ASSERT(LOOKUP_TABLE_COUNT == ARRAYLEN(lookupTables), LOOKUP_TABLE_COUNT_incorrect); } diff --git a/src/main/interface/settings.h b/src/main/interface/settings.h index cbcc60266f..a42ed5bc76 100644 --- a/src/main/interface/settings.h +++ b/src/main/interface/settings.h @@ -52,7 +52,7 @@ typedef enum { TABLE_RX_SPI, #endif TABLE_GYRO_HARDWARE_LPF, -#ifdef USE_32K_CAPABLE_GYRO +#if defined(USE_32K_CAPABLE_GYRO) && defined(USE_GYRO_DLPF_EXPERIMENTAL) TABLE_GYRO_32KHZ_HARDWARE_LPF, #endif TABLE_ACC_HARDWARE, @@ -68,6 +68,7 @@ typedef enum { TABLE_RC_INTERPOLATION_CHANNELS, TABLE_LOWPASS_TYPE, TABLE_DTERM_LOWPASS_TYPE, + TABLE_ANTI_GRAVITY_MODE, TABLE_FAILSAFE, TABLE_FAILSAFE_SWITCH_MODE, TABLE_CRASH_RECOVERY, @@ -91,7 +92,7 @@ typedef enum { #ifdef USE_LED_STRIP TABLE_RGB_GRB, #endif -#ifdef USE_DUAL_GYRO +#ifdef USE_MULTI_GYRO TABLE_GYRO, #endif TABLE_THROTTLE_LIMIT_TYPE, @@ -111,6 +112,17 @@ typedef enum { TABLE_RC_SMOOTHING_INPUT_TYPE, TABLE_RC_SMOOTHING_DERIVATIVE_TYPE, #endif // USE_RC_SMOOTHING_FILTER +#ifdef USE_GYRO_DATA_ANALYSE + TABLE_DYNAMIC_FFT_LOCATION, + TABLE_DYNAMIC_FILTER_RANGE, +#endif // USE_GYRO_DATA_ANALYSE +#ifdef USE_VTX_COMMON + TABLE_VTX_LOW_POWER_DISARM, +#endif + TABLE_GYRO_HARDWARE, +#ifdef USE_SDCARD + TABLE_SDCARD_MODE, +#endif LOOKUP_TABLE_COUNT } lookupTableIndex_e; diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index 8819ebec98..3007c093a6 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -36,7 +36,7 @@ #include "flight/mixer.h" #include "fc/config.h" -#include "fc/fc_core.h" +#include "fc/core.h" #include "fc/runtime_config.h" #include "io/statusindicator.h" @@ -160,6 +160,11 @@ static const uint8_t beep_camCloseBeep[] = { 10, 8, 5, BEEPER_COMMAND_STOP }; +// RC Smoothing filter not initialized - 3 short + 1 long +static const uint8_t beep_rcSmoothingInitFail[] = { + 10, 10, 10, 10, 10, 10, 50, 25, BEEPER_COMMAND_STOP +}; + // array used for variable # of beeps (reporting GPS sat count, etc) static uint8_t beep_multiBeeps[MAX_MULTI_BEEPS + 1]; @@ -172,8 +177,7 @@ static uint8_t beep_multiBeeps[MAX_MULTI_BEEPS + 1]; #define BEEPER_WARNING_BEEP_2_DURATION 5 #define BEEPER_WARNING_BEEP_GAP_DURATION 10 -// Beeper off = 0 Beeper on = 1 -static uint8_t beeperIsOn = 0; +static bool beeperIsOn = false; // Place in current sequence static uint16_t beeperPos = 0; @@ -219,10 +223,11 @@ static const beeperTableEntry_t beeperTable[] = { { BEEPER_ENTRY(BEEPER_SYSTEM_INIT, 16, NULL, "SYSTEM_INIT") }, { BEEPER_ENTRY(BEEPER_USB, 17, NULL, "ON_USB") }, { BEEPER_ENTRY(BEEPER_BLACKBOX_ERASE, 18, beep_2shortBeeps, "BLACKBOX_ERASE") }, - { BEEPER_ENTRY(BEEPER_CRASH_FLIP_MODE, 19, beep_2longerBeeps, "CRASH FLIP") }, + { BEEPER_ENTRY(BEEPER_CRASH_FLIP_MODE, 19, beep_2longerBeeps, "CRASH_FLIP") }, { BEEPER_ENTRY(BEEPER_CAM_CONNECTION_OPEN, 20, beep_camOpenBeep, "CAM_CONNECTION_OPEN") }, - { BEEPER_ENTRY(BEEPER_CAM_CONNECTION_CLOSE, 21, beep_camCloseBeep, "CAM_CONNECTION_CLOSED") }, - { BEEPER_ENTRY(BEEPER_ALL, 22, NULL, "ALL") }, + { BEEPER_ENTRY(BEEPER_CAM_CONNECTION_CLOSE, 21, beep_camCloseBeep, "CAM_CONNECTION_CLOSE") }, + { BEEPER_ENTRY(BEEPER_RC_SMOOTHING_INIT_FAIL,22, beep_rcSmoothingInitFail, "RC_SMOOTHING_INIT_FAIL") }, + { BEEPER_ENTRY(BEEPER_ALL, 23, NULL, "ALL") }, }; static const beeperTableEntry_t *currentBeeperEntry = NULL; @@ -277,12 +282,11 @@ void beeper(beeperMode_e mode) void beeperSilence(void) { BEEP_OFF; + beeperIsOn = false; + warningLedDisable(); warningLedRefresh(); - - beeperIsOn = 0; - beeperNextToggleTime = 0; beeperPos = 0; @@ -378,7 +382,7 @@ void beeperUpdate(timeUs_t currentTimeUs) if (IS_RC_MODE_ACTIVE(BOXBEEPERON)) { beeper(BEEPER_RX_SET); #ifdef USE_GPS - } else if (feature(FEATURE_GPS) && IS_RC_MODE_ACTIVE(BOXBEEPGPSCOUNT)) { + } else if (featureIsEnabled(FEATURE_GPS) && IS_RC_MODE_ACTIVE(BOXBEEPGPSCOUNT)) { beeperGpsStatus(); #endif } @@ -393,8 +397,6 @@ void beeperUpdate(timeUs_t currentTimeUs) } if (!beeperIsOn) { - beeperIsOn = 1; - #ifdef USE_DSHOT if (!areMotorsRunning() && ((currentBeeperEntry->mode == BEEPER_RX_SET && !(beeperConfig()->dshotBeaconOffFlags & BEEPER_GET_FLAG(BEEPER_RX_SET))) @@ -408,8 +410,11 @@ void beeperUpdate(timeUs_t currentTimeUs) #endif if (currentBeeperEntry->sequence[beeperPos] != 0) { - if (!(beeperConfigMutable()->beeper_off_flags & BEEPER_GET_FLAG(currentBeeperEntry->mode))) + if (!(beeperConfigMutable()->beeper_off_flags & BEEPER_GET_FLAG(currentBeeperEntry->mode))) { BEEP_ON; + beeperIsOn = true; + } + warningLedEnable(); warningLedRefresh(); // if this was arming beep then mark time (for blackbox) @@ -421,9 +426,10 @@ void beeperUpdate(timeUs_t currentTimeUs) } } } else { - beeperIsOn = 0; if (currentBeeperEntry->sequence[beeperPos] != 0) { BEEP_OFF; + beeperIsOn = false; + warningLedDisable(); warningLedRefresh(); } diff --git a/src/main/io/beeper.h b/src/main/io/beeper.h index effc003e51..c6bca70555 100644 --- a/src/main/io/beeper.h +++ b/src/main/io/beeper.h @@ -25,7 +25,7 @@ #define BEEPER_GET_FLAG(mode) (1 << (mode - 1)) #ifdef USE_DSHOT -#define DSHOT_BEACON_GUARD_DELAY_US 2000000 // Time to separate dshot beacon and armining/disarming events +#define DSHOT_BEACON_GUARD_DELAY_US 1200000 // Time to separate dshot beacon and armining/disarming events // to prevent interference with motor direction commands #endif @@ -41,20 +41,21 @@ typedef enum { BEEPER_ARMING_GPS_FIX, // Beep a special tone when arming the board and GPS has fix BEEPER_BAT_CRIT_LOW, // Longer warning beeps when battery is critically low (repeats) BEEPER_BAT_LOW, // Warning beeps when battery is getting low (repeats) - BEEPER_GPS_STATUS, // FIXME **** Disable beeper when connected to USB **** - BEEPER_RX_SET, // Beeps when aux channel is set for beep or beep sequence how many satellites has found if GPS enabled + BEEPER_GPS_STATUS, // Use the number of beeps to indicate how many GPS satellites were found + BEEPER_RX_SET, // Beeps when aux channel is set for beep BEEPER_ACC_CALIBRATION, // ACC inflight calibration completed confirmation BEEPER_ACC_CALIBRATION_FAIL, // ACC inflight calibration failed BEEPER_READY_BEEP, // Ring a tone when GPS is locked and ready BEEPER_MULTI_BEEPS, // Internal value used by 'beeperConfirmationBeeps()'. BEEPER_DISARM_REPEAT, // Beeps sounded while stick held in disarm position - BEEPER_ARMED, // Warning beeps when board is armed (repeats until board is disarmed or throttle is increased) + BEEPER_ARMED, // Warning beeps when board is armed with motors off when idle (repeats until board is disarmed or throttle is increased) BEEPER_SYSTEM_INIT, // Initialisation beeps when board is powered on BEEPER_USB, // Some boards have beeper powered USB connected BEEPER_BLACKBOX_ERASE, // Beep when blackbox erase completes BEEPER_CRASH_FLIP_MODE, // Crash flip mode is active BEEPER_CAM_CONNECTION_OPEN, // When the 5 key simulation stated BEEPER_CAM_CONNECTION_CLOSE, // When the 5 key simulation stop + BEEPER_RC_SMOOTHING_INIT_FAIL, // Warning beep pattern when armed and rc smoothing has not initialized filters BEEPER_ALL, // Turn ON or OFF all beeper conditions // BEEPER_ALL must remain at the bottom of this enum } beeperMode_e; @@ -82,7 +83,9 @@ typedef enum { | BEEPER_GET_FLAG(BEEPER_BLACKBOX_ERASE) \ | BEEPER_GET_FLAG(BEEPER_CRASH_FLIP_MODE) \ | BEEPER_GET_FLAG(BEEPER_CAM_CONNECTION_OPEN) \ - | BEEPER_GET_FLAG(BEEPER_CAM_CONNECTION_CLOSE) ) + | BEEPER_GET_FLAG(BEEPER_CAM_CONNECTION_CLOSE) \ + | BEEPER_GET_FLAG(BEEPER_RC_SMOOTHING_INIT_FAIL) \ + ) #define DSHOT_BEACON_ALLOWED_MODES ( \ BEEPER_GET_FLAG(BEEPER_RX_LOST) \ diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c index 20abe2a8e3..18ae74665d 100644 --- a/src/main/io/dashboard.c +++ b/src/main/io/dashboard.c @@ -357,7 +357,7 @@ static void showProfilePage(void) #ifdef USE_GPS static void showGpsPage(void) { - if (!feature(FEATURE_GPS)) { + if (!featureIsEnabled(FEATURE_GPS)) { pageState.pageFlags |= PAGE_STATE_FLAG_FORCE_PAGE_CHANGE; return; } @@ -526,7 +526,7 @@ static void showSensorsPage(void) } -#ifndef SKIP_TASK_STATISTICS +#if defined(USE_TASK_STATISTICS) static void showTasksPage(void) { uint8_t rowIndex = PAGE_TITLE_LINE_COUNT; @@ -576,7 +576,7 @@ static const pageEntry_t pages[PAGE_COUNT] = { { PAGE_RX, "RX", showRxPage, PAGE_FLAGS_NONE }, { PAGE_BATTERY, "BATTERY", showBatteryPage, PAGE_FLAGS_NONE }, { PAGE_SENSORS, "SENSORS", showSensorsPage, PAGE_FLAGS_NONE }, -#ifndef SKIP_TASK_STATISTICS +#if defined(USE_TASK_STATISTICS) { PAGE_TASKS, "TASKS", showTasksPage, PAGE_FLAGS_NONE }, #endif #ifdef ENABLE_DEBUG_DASHBOARD_PAGE diff --git a/src/main/io/dashboard.h b/src/main/io/dashboard.h index 2212b84d22..7d5d4dd18d 100644 --- a/src/main/io/dashboard.h +++ b/src/main/io/dashboard.h @@ -43,7 +43,7 @@ typedef enum { PAGE_SENSORS, PAGE_RX, PAGE_PROFILE, -#ifndef SKIP_TASK_STATISTICS +#if defined(USE_TASK_STATISTICS) PAGE_TASKS, #endif #ifdef USE_GPS diff --git a/src/main/io/displayport_crsf.c b/src/main/io/displayport_crsf.c index 11c3a716a6..8276bfc77e 100644 --- a/src/main/io/displayport_crsf.c +++ b/src/main/io/displayport_crsf.c @@ -200,6 +200,7 @@ int crsfDisplayPortNextRow(void) displayPort_t *displayPortCrsfInit() { + crsfDisplayPortSetDimensions(CRSF_DISPLAY_PORT_ROWS_MAX, CRSF_DISPLAY_PORT_COLS_MAX); displayInit(&crsfDisplayPort, &crsfDisplayPortVTable); return &crsfDisplayPort; } diff --git a/src/main/io/displayport_hott.c b/src/main/io/displayport_hott.c new file mode 100644 index 0000000000..21078ad0cb --- /dev/null +++ b/src/main/io/displayport_hott.c @@ -0,0 +1,187 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include + +#include "platform.h" +#if defined (USE_HOTT_TEXTMODE) && defined (USE_CMS) + +// Fixme: +// Let the CMS believe we have a bigger display to avoid empty rows and columns +// Better make this configurable in CMS +#define ROW_CORRECTION 1 +#define COL_CORRECTION 2 + +#include "common/utils.h" +#include "cms/cms.h" +#include "telemetry/hott.h" + +displayPort_t hottDisplayPort; + +static int hottDrawScreen(displayPort_t *displayPort) +{ + UNUSED(displayPort); + return 0; +} + +static int hottScreenSize(const displayPort_t *displayPort) +{ + return displayPort->rows * displayPort->cols; +} + +static int hottWriteChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint8_t c) +{ + UNUSED(displayPort); + + if (row < ROW_CORRECTION || row >= HOTT_TEXTMODE_DISPLAY_ROWS + ROW_CORRECTION + || col < COL_CORRECTION || col >= HOTT_TEXTMODE_DISPLAY_COLUMNS + COL_CORRECTION) { + return 0; + } + row -= ROW_CORRECTION; + col -= COL_CORRECTION; + + hottTextmodeWriteChar(col, row, c); + return 0; +} + +static int hottWriteString(displayPort_t *displayPort, uint8_t col, uint8_t row, const char *s) +{ + while (*s) { + hottWriteChar(displayPort, col++, row, *(s++)); + } + return 0; +} + +static int hottClearScreen(displayPort_t *displayPort) +{ + for (int row = 0; row < displayPort->rows; row++) { + for (int col= 0; col < displayPort->cols; col++) { + hottWriteChar(displayPort, col, row, ' '); + } + } + return 0; +} + +static bool hottIsTransferInProgress(const displayPort_t *displayPort) +{ + UNUSED(displayPort); + return false; +} + +static int hottHeartbeat(displayPort_t *displayPort) +{ + if (!hottTextmodeIsAlive()) { + cmsMenuExit(displayPort, (void*)CMS_EXIT_SAVE); + } + + return 0; +} + +static void hottResync(displayPort_t *displayPort) +{ + UNUSED(displayPort); +} + +static uint32_t hottTxBytesFree(const displayPort_t *displayPort) +{ + UNUSED(displayPort); + return UINT32_MAX; +} + +static int hottGrab(displayPort_t *displayPort) +{ + hottTextmodeGrab(); + return displayPort->grabCount = 1; +} + +static int hottRelease(displayPort_t *displayPort) +{ + int cnt = displayPort->grabCount = 0; + hottClearScreen(displayPort); + hottTextmodeExit(); + return cnt; +} + +static const displayPortVTable_t hottVTable = { + .grab = hottGrab, + .release = hottRelease, + .clearScreen = hottClearScreen, + .drawScreen = hottDrawScreen, + .screenSize = hottScreenSize, + .writeString = hottWriteString, + .writeChar = hottWriteChar, + .isTransferInProgress = hottIsTransferInProgress, + .heartbeat = hottHeartbeat, + .resync = hottResync, + .txBytesFree = hottTxBytesFree +}; + +displayPort_t *displayPortHottInit() +{ + hottDisplayPort.device = NULL; + displayInit(&hottDisplayPort, &hottVTable); + hottDisplayPort.rows = HOTT_TEXTMODE_DISPLAY_ROWS + ROW_CORRECTION * 2; + hottDisplayPort.cols = HOTT_TEXTMODE_DISPLAY_COLUMNS + COL_CORRECTION * 2; + return &hottDisplayPort; +} + +void hottDisplayportRegister() +{ + cmsDisplayPortRegister(displayPortHottInit()); +} + +void hottCmsOpen() +{ + if (!cmsInMenu) { + cmsDisplayPortSelect(&hottDisplayPort); + cmsMenuOpen(); + } +} + +void hottSetCmsKey(uint8_t hottKey, bool keepCmsOpen) +{ + switch (hottKey) { + case HOTTV4_BUTTON_DEC: + cmsSetExternKey(CMS_KEY_UP); + break; + case HOTTV4_BUTTON_INC: + cmsSetExternKey(CMS_KEY_DOWN); + break; + case HOTTV4_BUTTON_SET: + if (cmsInMenu) { + cmsMenuExit(pCurrentDisplay, (void*)CMS_EXIT_SAVE); + } + cmsSetExternKey(CMS_KEY_NONE); + break; + case HOTTV4_BUTTON_NEXT: + cmsSetExternKey(CMS_KEY_RIGHT); + break; + case HOTTV4_BUTTON_PREV: + cmsSetExternKey(CMS_KEY_LEFT); + if (keepCmsOpen) { // Make sure CMS is open until textmode is closed. + cmsMenuOpen(); + } + break; + default: + cmsSetExternKey(CMS_KEY_NONE); + break; + } +} + +#endif diff --git a/src/main/io/displayport_hott.h b/src/main/io/displayport_hott.h new file mode 100644 index 0000000000..c1e5026905 --- /dev/null +++ b/src/main/io/displayport_hott.h @@ -0,0 +1,27 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once +#include "drivers/display.h" + +displayPort_t *displayPortHottInit(); + +extern displayPort_t hottDisplayPort; + +void hottDisplayportRegister(); +void hottCmsOpen(); +void hottSetCmsKey(uint8_t hottKey, bool esc); diff --git a/src/main/io/displayport_max7456.c b/src/main/io/displayport_max7456.c index f865c2b997..e076a7c82d 100644 --- a/src/main/io/displayport_max7456.c +++ b/src/main/io/displayport_max7456.c @@ -34,7 +34,6 @@ #include "io/displayport_max7456.h" #include "io/osd.h" -#include "io/osd_slave.h" #include "pg/max7456.h" #include "pg/pg.h" @@ -166,12 +165,14 @@ static const displayPortVTable_t max7456VTable = { displayPort_t *max7456DisplayPortInit(const vcdProfile_t *vcdProfile) { + if ( + !max7456Init(max7456Config(), vcdProfile, systemConfig()->cpu_overclock) + ) { + return NULL; + } + displayInit(&max7456DisplayPort, &max7456VTable); -#ifdef USE_OSD_SLAVE - max7456Init(max7456Config(), vcdProfile, false); -#else - max7456Init(max7456Config(), vcdProfile, systemConfig()->cpu_overclock); -#endif + resync(&max7456DisplayPort); return &max7456DisplayPort; } diff --git a/src/main/io/displayport_rcdevice.c b/src/main/io/displayport_rcdevice.c deleted file mode 100644 index 8b3185f7d0..0000000000 --- a/src/main/io/displayport_rcdevice.c +++ /dev/null @@ -1,65 +0,0 @@ -/* - * 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 . - */ - -#include -#include - -#include "platform.h" - -#ifdef USE_RCDEVICE - -#include "drivers/display.h" - -#include "io/rcdevice.h" -#include "io/rcdevice_osd.h" - -#include "pg/vcd.h" - -#include "displayport_rcdevice.h" - -displayPort_t rcdeviceOSDDisplayPort; - -static const displayPortVTable_t rcdeviceOSDVTable = { - .grab = rcdeviceOSDGrab, - .release = rcdeviceOSDRelease, - .clearScreen = rcdeviceOSDClearScreen, - .drawScreen = rcdeviceOSDDrawScreen, - .writeString = rcdeviceOSDWriteString, - .writeChar = rcdeviceOSDWriteChar, - .isTransferInProgress = rcdeviceOSDIsTransferInProgress, - .heartbeat = rcdeviceOSDHeartbeat, - .resync = rcdeviceOSDResync, - .isSynced = rcdeviceOSDIsSynced, - .txBytesFree = rcdeviceOSDTxBytesFree, - .screenSize = rcdeviceScreenSize, -}; - -displayPort_t *rcdeviceDisplayPortInit(const vcdProfile_t *vcdProfile) -{ - if (rcdeviceOSDInit(vcdProfile)) { - displayInit(&rcdeviceOSDDisplayPort, &rcdeviceOSDVTable); - rcdeviceOSDResync(&rcdeviceOSDDisplayPort); - return &rcdeviceOSDDisplayPort; - } else { - return NULL; - } -} - -#endif // defined(USE_RCDEVICE) diff --git a/src/main/io/displayport_srxl.h b/src/main/io/displayport_srxl.h index 5547205208..8702cc1b2f 100644 --- a/src/main/io/displayport_srxl.h +++ b/src/main/io/displayport_srxl.h @@ -21,4 +21,5 @@ #pragma once displayPort_t *displayPortSrxlInit(); -displayPort_t srxlDisplayPort; + +extern displayPort_t srxlDisplayPort; diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 90bedf39a1..0f9c99b018 100644 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -98,7 +98,7 @@ static uint16_t fraction3[2]; gpsSolutionData_t gpsSol; uint32_t GPS_packetCount = 0; uint32_t GPS_svInfoReceivedCount = 0; // SV = Space Vehicle, counter increments each time SV info is received. -uint8_t GPS_update = 0; // it's a binary toggle to distinct a GPS position update +uint8_t GPS_update = 0; // toogle to distinct a GPS position update (directly or via MSP) uint8_t GPS_numCh; // Number of channels uint8_t GPS_svinfo_chn[GPS_SV_MAXSATS]; // Channel number @@ -224,6 +224,7 @@ static const uint8_t ubloxGalileoInit[] = { typedef enum { GPS_UNKNOWN, GPS_INITIALIZING, + GPS_INITIALIZED, GPS_CHANGE_BAUD, GPS_CONFIGURE, GPS_RECEIVING_DATA, @@ -280,10 +281,14 @@ void gpsInit(void) gpsSetState(GPS_UNKNOWN); gpsData.lastMessage = millis(); + + if (gpsConfig()->provider == GPS_MSP) { // no serial ports used when GPS_MSP is configured + gpsSetState(GPS_INITIALIZED); + return; + } serialPortConfig_t *gpsPortConfig = findSerialPortConfig(FUNCTION_GPS); if (!gpsPortConfig) { - featureClear(FEATURE_GPS); return; } @@ -305,7 +310,6 @@ void gpsInit(void) // no callback - buffer will be consumed in gpsUpdate() gpsPort = openSerialPort(gpsPortConfig->identifier, FUNCTION_GPS, NULL, NULL, baudRates[gpsInitData[gpsData.baudrateIndex].baudrateIndex], mode, SERIAL_NOT_INVERTED); if (!gpsPort) { - featureClear(FEATURE_GPS); return; } @@ -476,6 +480,8 @@ void gpsInitHardware(void) gpsInitUblox(); #endif break; + default: + break; } } @@ -494,10 +500,17 @@ void gpsUpdate(timeUs_t currentTimeUs) if (gpsPort) { while (serialRxBytesWaiting(gpsPort)) gpsNewData(serialRead(gpsPort)); + } else if (GPS_update & GPS_MSP_UPDATE) { // GPS data received via MSP + gpsSetState(GPS_RECEIVING_DATA); + gpsData.lastMessage = millis(); + sensorsSet(SENSOR_GPS); + onGpsNewData(); + GPS_update &= ~GPS_MSP_UPDATE; } switch (gpsData.state) { case GPS_UNKNOWN: + case GPS_INITIALIZED: break; case GPS_INITIALIZING: @@ -531,6 +544,11 @@ void gpsUpdate(timeUs_t currentTimeUs) if (sensors(SENSOR_GPS)) { updateGpsIndicator(currentTimeUs); } +#if defined(USE_GPS_RESCUE) + if (gpsRescueIsConfigured()) { + updateGPSRescueState(); + } +#endif } static void gpsNewData(uint16_t c) @@ -544,10 +562,7 @@ static void gpsNewData(uint16_t c) gpsData.lastMessage = millis(); sensorsSet(SENSOR_GPS); - if (GPS_update == 1) - GPS_update = 0; - else - GPS_update = 1; + GPS_update ^= GPS_DIRECT_TICK; #if 0 debug[3] = GPS_update; @@ -569,6 +584,8 @@ bool gpsNewFrame(uint8_t c) return gpsNewFrameUBLOX(c); #endif break; + default: + break; } return false; } @@ -641,28 +658,36 @@ static uint32_t grab_fields(char *src, uint8_t mult) { // convert string to uint32 uint32_t i; uint32_t tmp = 0; + int isneg = 0; for (i = 0; src[i] != 0; i++) { + if ((i == 0) && (src[0] == '-')) { // detect negative sign + isneg = 1; + continue; // jump to next character if the first one was a negative sign + } if (src[i] == '.') { i++; - if (mult == 0) + if (mult == 0) { break; - else + } else { src[i + mult] = 0; + } } tmp *= 10; - if (src[i] >= '0' && src[i] <= '9') + if (src[i] >= '0' && src[i] <= '9') { tmp += src[i] - '0'; - if (i >= 15) + } + if (i >= 15) { return 0; // out of bounds + } } - return tmp; + return isneg ? -tmp : tmp; // handle negative altitudes } typedef struct gpsDataNmea_s { int32_t latitude; int32_t longitude; uint8_t numSat; - uint16_t altitude; + int32_t altitudeCm; uint16_t speed; uint16_t hdop; uint16_t ground_course; @@ -692,12 +717,13 @@ static bool gpsNewFrameNMEA(char c) string[offset] = 0; if (param == 0) { //frame identification gps_frame = NO_FRAME; - if (string[0] == 'G' && string[1] == 'P' && string[2] == 'G' && string[3] == 'G' && string[4] == 'A') + if (0 == strcmp(string, "GPGGA") || 0 == strcmp(string, "GNGGA")) { gps_frame = FRAME_GGA; - if (string[0] == 'G' && string[1] == 'P' && string[2] == 'R' && string[3] == 'M' && string[4] == 'C') + } else if (0 == strcmp(string, "GPRMC") || 0 == strcmp(string, "GNRMC")) { gps_frame = FRAME_RMC; - if (string[0] == 'G' && string[1] == 'P' && string[2] == 'G' && string[3] == 'S' && string[4] == 'V') + } else if (0 == strcmp(string, "GPGSV")) { gps_frame = FRAME_GSV; + } } switch (gps_frame) { @@ -733,7 +759,7 @@ static bool gpsNewFrameNMEA(char c) gps_Msg.hdop = grab_fields(string, 1) * 100; // hdop break; case 9: - gps_Msg.altitude = grab_fields(string, 0); // altitude in meters added by Mis + gps_Msg.altitudeCm = grab_fields(string, 1) * 10; // altitude in centimeters. Note: NMEA delivers altitude with 1 or 3 decimals. It's safer to cut at 0.1m and multiply by 10 break; } break; @@ -824,7 +850,7 @@ static bool gpsNewFrameNMEA(char c) gpsSol.llh.lat = gps_Msg.latitude; gpsSol.llh.lon = gps_Msg.longitude; gpsSol.numSat = gps_Msg.numSat; - gpsSol.llh.alt = gps_Msg.altitude; + gpsSol.llh.altCm = gps_Msg.altitudeCm; gpsSol.hdop = gps_Msg.hdop; } break; @@ -879,7 +905,7 @@ typedef struct { int32_t longitude; int32_t latitude; int32_t altitude_ellipsoid; - int32_t altitude_msl; + int32_t altitudeMslMm; uint32_t horizontal_accuracy; uint32_t vertical_accuracy; } ubx_nav_posllh; @@ -1048,7 +1074,7 @@ static bool UBLOX_parse_gps(void) //i2c_dataset.time = _buffer.posllh.time; gpsSol.llh.lon = _buffer.posllh.longitude; gpsSol.llh.lat = _buffer.posllh.latitude; - gpsSol.llh.alt = _buffer.posllh.altitude_msl / 10; //alt in cm + gpsSol.llh.altCm = _buffer.posllh.altitudeMslMm / 10; //alt in cm if (next_fix) { ENABLE_STATE(GPS_FIX); } else { @@ -1202,7 +1228,7 @@ static void gpsHandlePassthrough(uint8_t data) { gpsNewData(data); #ifdef USE_DASHBOARD - if (feature(FEATURE_DASHBOARD)) { + if (featureIsEnabled(FEATURE_DASHBOARD)) { dashboardUpdate(micros()); } #endif @@ -1218,7 +1244,7 @@ void gpsEnablePassthrough(serialPort_t *gpsPassthroughPort) serialSetMode(gpsPort, gpsPort->mode | MODE_TX); #ifdef USE_DASHBOARD - if (feature(FEATURE_DASHBOARD)) { + if (featureIsEnabled(FEATURE_DASHBOARD)) { dashboardShowFixedPage(PAGE_GPS); } #endif diff --git a/src/main/io/gps.h b/src/main/io/gps.h index ed54cb99ab..f5e257be6e 100644 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -34,7 +34,8 @@ typedef enum { GPS_NMEA = 0, - GPS_UBLOX + GPS_UBLOX, + GPS_MSP } gpsProvider_e; typedef enum { @@ -86,12 +87,11 @@ typedef struct gpsCoordinateDDDMMmmmm_s { typedef struct gpsLocation_s { int32_t lat; // latitude * 1e+7 int32_t lon; // longitude * 1e+7 - int32_t alt; // altitude in 0.1m + int32_t altCm; // altitude in 0.01m } gpsLocation_t; typedef struct gpsSolutionData_s { gpsLocation_t llh; - uint16_t GPS_altitude; // altitude in 0.1m uint16_t groundSpeed; // speed in 0.1m/s uint16_t groundCourse; // degrees * 10 uint16_t hdop; // generic HDOP value (*100) @@ -138,10 +138,15 @@ typedef enum { } navigationMode_e; extern navigationMode_e nav_mode; // Navigation mode +typedef enum { + GPS_DIRECT_TICK = 1 << 0, + GPS_MSP_UPDATE = 1 << 1 +} gpsUpdateToggle_e; + extern gpsData_t gpsData; extern gpsSolutionData_t gpsSol; -extern uint8_t GPS_update; // it's a binary toogle to distinct a GPS position update +extern uint8_t GPS_update; // toogle to distinct a GPS position update (directly or via MSP) extern uint32_t GPS_packetCount; extern uint32_t GPS_svInfoReceivedCount; extern uint8_t GPS_numCh; // Number of channels diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 6b62a5c4bf..3cb9725b8b 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -77,6 +77,10 @@ PG_REGISTER_WITH_RESET_FN(ledStripConfig_t, ledStripConfig, PG_LED_STRIP_CONFIG, 0); +hsvColor_t *colors; +const modeColorIndexes_t *modeColors; +specialColorIndexes_t specialColors; + static bool ledStripInitialised = false; static bool ledStripEnabled = true; @@ -145,7 +149,6 @@ static const modeColorIndexes_t defaultModeColors[] = { [LED_MODE_HORIZON] = {{ COLOR_BLUE, COLOR_DARK_VIOLET, COLOR_YELLOW, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }}, [LED_MODE_ANGLE] = {{ COLOR_CYAN, COLOR_DARK_VIOLET, COLOR_YELLOW, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }}, [LED_MODE_MAG] = {{ COLOR_MINT_GREEN, COLOR_DARK_VIOLET, COLOR_ORANGE, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }}, - [LED_MODE_BARO] = {{ COLOR_LIGHT_BLUE, COLOR_DARK_VIOLET, COLOR_RED, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }}, }; static const specialColorIndexes_t defaultSpecialColors[] = { @@ -165,7 +168,7 @@ void pgResetFn_ledStripConfig(ledStripConfig_t *ledStripConfig) memset(ledStripConfig->ledConfigs, 0, LED_MAX_STRIP_LENGTH * sizeof(ledConfig_t)); // copy hsv colors as default memset(ledStripConfig->colors, 0, ARRAYLEN(hsv) * sizeof(hsvColor_t)); - BUILD_BUG_ON(LED_CONFIGURABLE_COLOR_COUNT < ARRAYLEN(hsv)); + STATIC_ASSERT(LED_CONFIGURABLE_COLOR_COUNT >= ARRAYLEN(hsv), LED_CONFIGURABLE_COLOR_COUNT_invalid); for (unsigned colorIndex = 0; colorIndex < ARRAYLEN(hsv); colorIndex++) { ledStripConfig->colors[colorIndex] = hsv[colorIndex]; } @@ -432,9 +435,6 @@ static const struct { {HEADFREE_MODE, LED_MODE_HEADFREE}, #ifdef USE_MAG {MAG_MODE, LED_MODE_MAG}, -#endif -#ifdef USE_BARO - {BARO_MODE, LED_MODE_BARO}, #endif {HORIZON_MODE, LED_MODE_HORIZON}, {ANGLE_MODE, LED_MODE_ANGLE}, diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h index a3d82ec717..07641492df 100644 --- a/src/main/io/ledstrip.h +++ b/src/main/io/ledstrip.h @@ -22,8 +22,11 @@ #include "common/color.h" #include "common/time.h" -#include "pg/pg.h" + #include "drivers/io_types.h" +#include "drivers/light_ws2811strip.h" + +#include "pg/pg.h" #define LED_MAX_STRIP_LENGTH 32 #define LED_CONFIGURABLE_COLOR_COUNT 16 @@ -124,12 +127,6 @@ typedef enum { LED_OVERLAY_WARNING } ledOverlayId_e; -// Enumeration to match the string options defined in lookupLedStripFormatRGB in settings.c -typedef enum { - LED_GRB, - LED_RGB -} ledStripFormatRGB_e; - typedef struct modeColorIndexes_s { uint8_t color[LED_DIRECTION_COUNT]; } modeColorIndexes_t; @@ -160,9 +157,9 @@ typedef struct ledStripConfig_s { PG_DECLARE(ledStripConfig_t, ledStripConfig); -hsvColor_t *colors; -const modeColorIndexes_t *modeColors; -specialColorIndexes_t specialColors; +extern hsvColor_t *colors; +extern const modeColorIndexes_t *modeColors; +extern specialColorIndexes_t specialColors; #define LF(name) LED_FUNCTION_ ## name #define LO(name) LED_FLAG_OVERLAY(LED_OVERLAY_ ## name) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 0367a2f25c..33849d29ce 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -46,6 +46,7 @@ #include "cms/cms.h" #include "cms/cms_types.h" +#include "common/axis.h" #include "common/maths.h" #include "common/printf.h" #include "common/typeconversion.h" @@ -60,11 +61,14 @@ #include "drivers/time.h" #include "fc/config.h" -#include "fc/fc_core.h" +#include "fc/core.h" #include "fc/rc_adjustments.h" #include "fc/rc_controls.h" +#include "fc/rc_modes.h" +#include "fc/rc.h" #include "fc/runtime_config.h" +#include "flight/failsafe.h" #include "flight/position.h" #include "flight/imu.h" #include "flight/mixer.h" @@ -84,6 +88,7 @@ #include "rx/rx.h" +#include "sensors/acceleration.h" #include "sensors/adcinternal.h" #include "sensors/barometer.h" #include "sensors/battery.h" @@ -121,6 +126,7 @@ static uint32_t blinkBits[(OSD_ITEM_COUNT + 31)/32]; #define IS_MID(X) (rcData[X] > 1250 && rcData[X] < 1750) static timeUs_t flyTime = 0; +static float osdGForce = 0; typedef struct statistic_s { timeUs_t armed_time; @@ -130,6 +136,9 @@ typedef struct statistic_s { int16_t min_rssi; int32_t max_altitude; int16_t max_distance; + float max_g_force; + int16_t max_esc_temp; + int32_t max_esc_rpm; } statistic_t; static statistic_t stats; @@ -196,7 +205,17 @@ static const uint8_t osdElementDisplayOrder[] = { OSD_NUMERICAL_VARIO, OSD_COMPASS_BAR, OSD_ANTI_GRAVITY, - OSD_MOTOR_DIAG + OSD_MOTOR_DIAG, + OSD_FLIP_ARROW, +#ifdef USE_RTC_TIME + OSD_RTC_DATETIME, +#endif +#ifdef USE_OSD_ADJUSTMENTS + OSD_ADJUSTMENT_RANGE, +#endif +#ifdef USE_ADC_INTERNAL + OSD_CORE_TEMPERATURE, +#endif }; PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 3); @@ -248,13 +267,13 @@ static int32_t osdGetMetersToSelectedUnit(int32_t meters) } #if defined(USE_ADC_INTERNAL) || defined(USE_ESC_SENSOR) -STATIC_UNIT_TESTED int osdConvertTemperatureToSelectedUnit(int tempInDeciDegrees) +STATIC_UNIT_TESTED int osdConvertTemperatureToSelectedUnit(int tempInDegreesCelcius) { switch (osdConfig()->units) { case OSD_UNIT_IMPERIAL: - return ((tempInDeciDegrees * 9) / 5) + 320; + return lrintf(((tempInDegreesCelcius * 9.0f) / 5) + 32); default: - return tempInDeciDegrees; + return tempInDegreesCelcius; } } @@ -269,16 +288,16 @@ static char osdGetTemperatureSymbolForSelectedUnit(void) } #endif -static void osdFormatAltitudeString(char * buff, int altitude) +static void osdFormatAltitudeString(char * buff, int32_t altitudeCm) { - const int alt = osdGetMetersToSelectedUnit(altitude) / 10; + const int alt = osdGetMetersToSelectedUnit(altitudeCm) / 10; tfp_sprintf(buff, "%5d %c", alt, osdGetMetersToSelectedUnitSymbol()); buff[5] = buff[4]; buff[4] = '.'; } -static void osdFormatPID(char * buff, const char * label, const pid8_t * pid) +static void osdFormatPID(char * buff, const char * label, const pidf_t * pid) { tfp_sprintf(buff, "%s %3d %3d %3d", label, pid->P, pid->I, pid->D); } @@ -456,6 +475,30 @@ static bool osdDrawSingleElement(uint8_t item) char buff[OSD_ELEMENT_BUFFER_LENGTH] = ""; switch (item) { + case OSD_FLIP_ARROW: + { + const int angleR = attitude.values.roll / 10; + const int angleP = attitude.values.pitch / 10; // still gotta update all angleR and angleP pointers. + if (IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) { + if (angleP > 0 && ((angleR > 175 && angleR < 180) || (angleR > -180 && angleR < -175))) { + buff[0] = SYM_ARROW_SOUTH; + } else if (angleP > 0 && angleR > 0 && angleR < 175) { + buff[0] = (SYM_ARROW_EAST + 2); + } else if (angleP > 0 && angleR < 0 && angleR > -175) { + buff[0] = (SYM_ARROW_WEST + 2); + } else if (angleP <= 0 && ((angleR > 175 && angleR < 180) || (angleR > -180 && angleR < -175))) { + buff[0] = SYM_ARROW_NORTH; + } else if (angleP <= 0 && angleR > 0 && angleR < 175) { + buff[0] = (SYM_ARROW_NORTH + 2); + } else if (angleP <= 0 && angleR < 0 && angleR > -175) { + buff[0] = (SYM_ARROW_SOUTH + 2); + } + } else { + buff[0] = ' '; + } + buff[1] = '\0'; + break; + } case OSD_RSSI_VALUE: { uint16_t osdRssi = getRssi() * 100 / 1024; // change range @@ -549,7 +592,7 @@ static bool osdDrawSingleElement(uint8_t item) break; case OSD_ALTITUDE: - osdFormatAltitudeString(buff, getEstimatedAltitude()); + osdFormatAltitudeString(buff, getEstimatedAltitudeCm()); break; case OSD_ITEM_TIMER_1: @@ -560,13 +603,13 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_REMAINING_TIME_ESTIMATE: { const int mAhDrawn = getMAhDrawn(); - const int remaining_time = (int)((osdConfig()->cap_alarm - mAhDrawn) * ((float)flyTime) / mAhDrawn); - if (mAhDrawn < 0.1 * osdConfig()->cap_alarm) { + if (mAhDrawn <= 0.1 * osdConfig()->cap_alarm) { // also handles the mAhDrawn == 0 condition tfp_sprintf(buff, "--:--"); } else if (mAhDrawn > osdConfig()->cap_alarm) { tfp_sprintf(buff, "00:00"); } else { + const int remaining_time = (int)((osdConfig()->cap_alarm - mAhDrawn) * ((float)flyTime) / mAhDrawn); osdFormatTime(buff, OSD_TIMER_PREC_SECOND, remaining_time); } break; @@ -574,15 +617,26 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_FLYMODE: { + // Note that flight mode display has precedence in what to display. + // 1. FS + // 2. GPS RESCUE + // 3. ANGLE, HORIZON, ACRO TRAINER + // 4. AIR + // 5. ACRO + if (FLIGHT_MODE(FAILSAFE_MODE)) { strcpy(buff, "!FS!"); + } else if (FLIGHT_MODE(GPS_RESCUE_MODE)) { + strcpy(buff, "RESC"); + } else if (FLIGHT_MODE(HEADFREE_MODE)) { + strcpy(buff, "HEAD"); } else if (FLIGHT_MODE(ANGLE_MODE)) { strcpy(buff, "STAB"); } else if (FLIGHT_MODE(HORIZON_MODE)) { strcpy(buff, "HOR "); - } else if (FLIGHT_MODE(GPS_RESCUE_MODE)) { - strcpy(buff, "RESC"); - } else if (isAirmodeActive()) { + } else if (IS_RC_MODE_ACTIVE(BOXACROTRAINER)) { + strcpy(buff, "ATRN"); + } else if (airmodeIsEnabled()) { strcpy(buff, "AIR "); } else { strcpy(buff, "ACRO"); @@ -593,7 +647,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_ANTI_GRAVITY: { - if (pidItermAccelerator() > 1.0f) { + if (pidOsdAntiGravityActive()) { strcpy(buff, "AG"); } @@ -671,7 +725,10 @@ static bool osdDrawSingleElement(uint8_t item) int pitchAngle = constrain(attitude.values.pitch, -maxPitch, maxPitch); // Convert pitchAngle to y compensation value // (maxPitch / 25) divisor matches previous settings of fixed divisor of 8 and fixed max AHI pitch angle of 20.0 degrees - pitchAngle = ((pitchAngle * 25) / maxPitch) - 41; // 41 = 4 * AH_SYMBOL_COUNT + 5 + if (maxPitch > 0) { + pitchAngle = ((pitchAngle * 25) / maxPitch); + } + pitchAngle -= 41; // 41 = 4 * AH_SYMBOL_COUNT + 5 for (int x = -4; x <= 4; x++) { const int y = ((-rollAngle * x) / 64) - pitchAngle; @@ -700,6 +757,13 @@ static bool osdDrawSingleElement(uint8_t item) return true; } + case OSD_G_FORCE: + { + const int gForce = lrintf(osdGForce * 10); + tfp_sprintf(buff, "%01d.%01dG", gForce / 10, gForce % 10); + break; + } + case OSD_ROLL_PIDS: osdFormatPID(buff, "ROL", ¤tPidProfile->pid[PID_ROLL]); break; @@ -729,17 +793,81 @@ static bool osdDrawSingleElement(uint8_t item) STATIC_ASSERT(OSD_FORMAT_MESSAGE_BUFFER_SIZE <= sizeof(buff), osd_warnings_size_exceeds_buffer_size); const batteryState_e batteryState = getBatteryState(); + const timeUs_t currentTimeUs = micros(); + + static timeUs_t armingDisabledUpdateTimeUs; + static unsigned armingDisabledDisplayIndex; + + // Cycle through the arming disabled reasons + if (osdWarnGetState(OSD_WARNING_ARMING_DISABLE)) { + if (IS_RC_MODE_ACTIVE(BOXARM) && isArmingDisabled()) { + const armingDisableFlags_e armSwitchOnlyFlag = 1 << (ARMING_DISABLE_FLAGS_COUNT - 1); + armingDisableFlags_e flags = getArmingDisableFlags(); + + // Remove the ARMSWITCH flag unless it's the only one + if ((flags & armSwitchOnlyFlag) && (flags != armSwitchOnlyFlag)) { + flags -= armSwitchOnlyFlag; + } + + // Rotate to the next arming disabled reason after a 0.5 second time delay + // or if the current flag is no longer set + if ((currentTimeUs - armingDisabledUpdateTimeUs > 5e5) || !(flags & (1 << armingDisabledDisplayIndex))) { + if (armingDisabledUpdateTimeUs == 0) { + armingDisabledDisplayIndex = ARMING_DISABLE_FLAGS_COUNT - 1; + } + armingDisabledUpdateTimeUs = currentTimeUs; + + do { + if (++armingDisabledDisplayIndex >= ARMING_DISABLE_FLAGS_COUNT) { + armingDisabledDisplayIndex = 0; + } + } while (!(flags & (1 << armingDisabledDisplayIndex))); + } + + osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, armingDisableFlagNames[armingDisabledDisplayIndex]); + break; + } else { + armingDisabledUpdateTimeUs = 0; + } + } + +#ifdef USE_DSHOT + if (isTryingToArm() && !ARMING_FLAG(ARMED)) { + int armingDelayTime = (getLastDshotBeaconCommandTimeUs() + DSHOT_BEACON_GUARD_DELAY_US - currentTimeUs) / 1e5; + if (armingDelayTime < 0) { + armingDelayTime = 0; + } + if (armingDelayTime >= (DSHOT_BEACON_GUARD_DELAY_US / 1e5 - 5)) { + osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, " BEACON ON"); // Display this message for the first 0.5 seconds + } else { + char armingDelayMessage[OSD_FORMAT_MESSAGE_BUFFER_SIZE]; + tfp_sprintf(armingDelayMessage, "ARM IN %d.%d", armingDelayTime / 10, armingDelayTime % 10); + osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, armingDelayMessage); + } + break; + } +#endif + if (osdWarnGetState(OSD_WARNING_FAIL_SAFE) && failsafeIsActive()) { + osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "FAIL SAFE"); + break; + } if (osdWarnGetState(OSD_WARNING_BATTERY_CRITICAL) && batteryState == BATTERY_CRITICAL) { osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, " LAND NOW"); break; } + // Show warning if in HEADFREE flight mode + if (FLIGHT_MODE(HEADFREE_MODE)) { + osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "HEADFREE"); + break; + } + #ifdef USE_ADC_INTERNAL - uint8_t coreTemperature = getCoreTemperatureCelsius(); + const int16_t coreTemperature = getCoreTemperatureCelsius(); if (osdWarnGetState(OSD_WARNING_CORE_TEMPERATURE) && coreTemperature >= osdConfig()->core_temp_alarm) { char coreTemperatureWarningMsg[OSD_FORMAT_MESSAGE_BUFFER_SIZE]; - tfp_sprintf(coreTemperatureWarningMsg, "CORE: %3d%c", osdConvertTemperatureToSelectedUnit(getCoreTemperatureCelsius() * 10) / 10, osdGetTemperatureSymbolForSelectedUnit()); + tfp_sprintf(coreTemperatureWarningMsg, "CORE: %3d%c", osdConvertTemperatureToSelectedUnit(coreTemperature), osdGetTemperatureSymbolForSelectedUnit()); osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, coreTemperatureWarningMsg); @@ -749,7 +877,7 @@ static bool osdDrawSingleElement(uint8_t item) #ifdef USE_ESC_SENSOR // Show warning if we lose motor output, the ESC is overheating or excessive current draw - if (feature(FEATURE_ESC_SENSOR) && osdWarnGetState(OSD_WARNING_ESC_FAIL)) { + if (featureIsEnabled(FEATURE_ESC_SENSOR) && osdWarnGetState(OSD_WARNING_ESC_FAIL)) { char escWarningMsg[OSD_FORMAT_MESSAGE_BUFFER_SIZE]; unsigned pos = 0; @@ -793,8 +921,8 @@ static bool osdDrawSingleElement(uint8_t item) if (escWarningCount > 0) { osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, escWarningMsg); + break; } - break; } #endif @@ -804,23 +932,19 @@ static bool osdDrawSingleElement(uint8_t item) break; } - // Show most severe reason for arming being disabled - if (osdWarnGetState(OSD_WARNING_ARMING_DISABLE) && IS_RC_MODE_ACTIVE(BOXARM) && isArmingDisabled()) { - const armingDisableFlags_e flags = getArmingDisableFlags(); - for (int i = 0; i < ARMING_DISABLE_FLAGS_COUNT; i++) { - if (flags & (1 << i)) { - osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, armingDisableFlagNames[i]); - break; - } - } - break; - } - if (osdWarnGetState(OSD_WARNING_BATTERY_WARNING) && batteryState == BATTERY_WARNING) { osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "LOW BATTERY"); break; } +#ifdef USE_RC_SMOOTHING_FILTER + // Show warning if rc smoothing hasn't initialized the filters + if (osdWarnGetState(OSD_WARNING_RC_SMOOTHING) && ARMING_FLAG(ARMED) && !rcSmoothingInitializationComplete()) { + osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "RCSMOOTHING"); + break; + } +#endif + // Show warning if battery is not fresh if (osdWarnGetState(OSD_WARNING_BATTERY_NOT_FULL) && !ARMING_FLAG(WAS_EVER_ARMED) && (getBatteryState() == BATTERY_OK) && getBatteryAverageCellVoltage() < batteryConfig()->vbatfullcellvoltage) { @@ -898,7 +1022,7 @@ static bool osdDrawSingleElement(uint8_t item) tfp_sprintf(buff, "%c%03d", osdGetDirectionSymbolFromHeading(heading), heading); break; } - +#ifdef USE_VARIO case OSD_NUMERICAL_VARIO: { const int verticalSpeed = osdGetMetersToSelectedUnit(getEstimatedVario()); @@ -906,16 +1030,17 @@ static bool osdDrawSingleElement(uint8_t item) tfp_sprintf(buff, "%c%01d.%01d", directionSymbol, abs(verticalSpeed / 100), abs((verticalSpeed % 100) / 10)); break; } +#endif #ifdef USE_ESC_SENSOR case OSD_ESC_TMP: - if (feature(FEATURE_ESC_SENSOR)) { - tfp_sprintf(buff, "%3d%c", osdConvertTemperatureToSelectedUnit(escDataCombined->temperature * 10) / 10, osdGetTemperatureSymbolForSelectedUnit()); + if (featureIsEnabled(FEATURE_ESC_SENSOR)) { + tfp_sprintf(buff, "%3d%c", osdConvertTemperatureToSelectedUnit(escDataCombined->temperature), osdGetTemperatureSymbolForSelectedUnit()); } break; case OSD_ESC_RPM: - if (feature(FEATURE_ESC_SENSOR)) { + if (featureIsEnabled(FEATURE_ESC_SENSOR)) { tfp_sprintf(buff, "%5d", escDataCombined == NULL ? 0 : calcEscRpm(escDataCombined->rpm)); } break; @@ -929,13 +1054,27 @@ static bool osdDrawSingleElement(uint8_t item) #ifdef USE_OSD_ADJUSTMENTS case OSD_ADJUSTMENT_RANGE: - tfp_sprintf(buff, "%s: %3d", adjustmentRangeName, adjustmentRangeValue); + if (getAdjustmentsRangeName()) { + tfp_sprintf(buff, "%s: %3d", getAdjustmentsRangeName(), getAdjustmentsRangeValue()); + } break; #endif #ifdef USE_ADC_INTERNAL case OSD_CORE_TEMPERATURE: - tfp_sprintf(buff, "%3d%c", osdConvertTemperatureToSelectedUnit(getCoreTemperatureCelsius() * 10) / 10, osdGetTemperatureSymbolForSelectedUnit()); + tfp_sprintf(buff, "%3d%c", osdConvertTemperatureToSelectedUnit(getCoreTemperatureCelsius()), osdGetTemperatureSymbolForSelectedUnit()); + break; +#endif + +#ifdef USE_BLACKBOX + case OSD_LOG_STATUS: + if (!isBlackboxDeviceWorking()) { + tfp_sprintf(buff, "L-"); + } else if (isBlackboxDeviceFull()) { + tfp_sprintf(buff, "L>"); + } else { + tfp_sprintf(buff, "L%d", blackboxGetLogNumber()); + } break; #endif @@ -957,8 +1096,18 @@ static void osdDrawElements(void) return; } + osdGForce = 0.0f; if (sensors(SENSOR_ACC)) { + // only calculate the G force if the element is visible or the stat is enabled + if (VISIBLE(osdConfig()->item_pos[OSD_G_FORCE]) || osdStatGetState(OSD_STAT_MAX_G_FORCE)) { + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + const float a = accAverage[axis]; + osdGForce += a * a; + } + osdGForce = sqrtf(osdGForce) * acc.dev.acc_1G_rec; + } osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON); + osdDrawSingleElement(OSD_G_FORCE); } @@ -978,22 +1127,16 @@ static void osdDrawElements(void) #endif // GPS #ifdef USE_ESC_SENSOR - if (feature(FEATURE_ESC_SENSOR)) { + if (featureIsEnabled(FEATURE_ESC_SENSOR)) { osdDrawSingleElement(OSD_ESC_TMP); osdDrawSingleElement(OSD_ESC_RPM); } #endif -#ifdef USE_RTC_TIME - osdDrawSingleElement(OSD_RTC_DATETIME); -#endif - -#ifdef USE_OSD_ADJUSTMENTS - osdDrawSingleElement(OSD_ADJUSTMENT_RANGE); -#endif - -#ifdef USE_ADC_INTERNAL - osdDrawSingleElement(OSD_CORE_TEMPERATURE); +#ifdef USE_BLACKBOX + if (IS_RC_MODE_ACTIVE(BOXBLACKBOX)) { + osdDrawSingleElement(OSD_LOG_STATUS); + } #endif } @@ -1063,7 +1206,7 @@ void osdInit(displayPort_t *osdDisplayPortToUse) return; } - BUILD_BUG_ON(OSD_POS_MAX != OSD_POS(31,31)); + STATIC_ASSERT(OSD_POS_MAX == OSD_POS(31,31), OSD_POS_MAX_incorrect); osdDisplayPort = osdDisplayPortToUse; #ifdef USE_CMS @@ -1099,11 +1242,16 @@ void osdInit(displayPort_t *osdDisplayPortToUse) resumeRefreshAt = micros() + (4 * REFRESH_1S); } +bool osdInitialized(void) +{ + return osdDisplayPort; +} + void osdUpdateAlarms(void) { // This is overdone? - int32_t alt = osdGetMetersToSelectedUnit(getEstimatedAltitude()) / 100; + int32_t alt = osdGetMetersToSelectedUnit(getEstimatedAltitudeCm()) / 100; if (getRssiPercent() < osdConfig()->rssi_alarm) { SET_BLINK(OSD_RSSI_VALUE); @@ -1111,12 +1259,22 @@ void osdUpdateAlarms(void) CLR_BLINK(OSD_RSSI_VALUE); } - if (getBatteryState() == BATTERY_OK) { + // Determine if the OSD_WARNINGS should blink + if (getBatteryState() != BATTERY_OK + && (osdWarnGetState(OSD_WARNING_BATTERY_CRITICAL) || osdWarnGetState(OSD_WARNING_BATTERY_WARNING)) +#ifdef USE_DSHOT + && (!isTryingToArm()) +#endif + ) { + SET_BLINK(OSD_WARNINGS); + } else { CLR_BLINK(OSD_WARNINGS); + } + + if (getBatteryState() == BATTERY_OK) { CLR_BLINK(OSD_MAIN_BATT_VOLTAGE); CLR_BLINK(OSD_AVG_CELL_VOLTAGE); } else { - SET_BLINK(OSD_WARNINGS); SET_BLINK(OSD_MAIN_BATT_VOLTAGE); SET_BLINK(OSD_AVG_CELL_VOLTAGE); } @@ -1155,7 +1313,7 @@ void osdUpdateAlarms(void) } #ifdef USE_ESC_SENSOR - if (feature(FEATURE_ESC_SENSOR)) { + if (featureIsEnabled(FEATURE_ESC_SENSOR)) { // This works because the combined ESC data contains the maximum temperature seen amongst all ESCs if (osdConfig()->esc_temp_alarm != ESC_TEMP_ALARM_OFF && escDataCombined->temperature >= osdConfig()->esc_temp_alarm) { SET_BLINK(OSD_ESC_TMP); @@ -1187,11 +1345,13 @@ static void osdResetStats(void) stats.max_current = 0; stats.max_speed = 0; stats.min_voltage = 500; - stats.max_current = 0; stats.min_rssi = 99; stats.max_altitude = 0; stats.max_distance = 0; stats.armed_time = 0; + stats.max_g_force = 0; + stats.max_esc_temp = 0; + stats.max_esc_rpm = 0; } static void osdUpdateStats(void) @@ -1226,9 +1386,13 @@ static void osdUpdateStats(void) stats.min_rssi = value; } - int altitude = getEstimatedAltitude(); - if (stats.max_altitude < altitude) { - stats.max_altitude = altitude; + int32_t altitudeCm = getEstimatedAltitudeCm(); + if (stats.max_altitude < altitudeCm) { + stats.max_altitude = altitudeCm; + } + + if (stats.max_g_force < osdGForce) { + stats.max_g_force = osdGForce; } #ifdef USE_GPS @@ -1240,19 +1404,31 @@ static void osdUpdateStats(void) } } #endif +#ifdef USE_ESC_SENSOR + if (featureIsEnabled(FEATURE_ESC_SENSOR)) { + value = escDataCombined->temperature; + if (stats.max_esc_temp < value) { + stats.max_esc_temp = value; + } + value = calcEscRpm(escDataCombined->rpm); + if (stats.max_esc_rpm < value) { + stats.max_esc_rpm = value; + } + } +#endif } #ifdef USE_BLACKBOX + static void osdGetBlackboxStatusString(char * buff) { - bool storageDeviceIsWorking = false; + bool storageDeviceIsWorking = isBlackboxDeviceWorking(); uint32_t storageUsed = 0; uint32_t storageTotal = 0; switch (blackboxConfig()->device) { #ifdef USE_SDCARD case BLACKBOX_DEVICE_SDCARD: - storageDeviceIsWorking = sdcard_isInserted() && sdcard_isFunctional() && (afatfs_getFilesystemState() == AFATFS_FILESYSTEM_STATE_READY); if (storageDeviceIsWorking) { storageTotal = sdcard_getMetadata()->numBlocks / 2000; storageUsed = storageTotal - (afatfs_getContiguousFreeSpace() / 1024000); @@ -1262,7 +1438,6 @@ static void osdGetBlackboxStatusString(char * buff) #ifdef USE_FLASHFS case BLACKBOX_DEVICE_FLASH: - storageDeviceIsWorking = flashfsIsReady(); if (storageDeviceIsWorking) { const flashGeometry_t *geometry = flashfsGetGeometry(); storageTotal = geometry->totalSize / 1024; @@ -1398,6 +1573,24 @@ static void osdShowStats(uint16_t endBatteryVoltage) } #endif + if (osdStatGetState(OSD_STAT_MAX_G_FORCE) && sensors(SENSOR_ACC)) { + const int gForce = lrintf(stats.max_g_force * 10); + tfp_sprintf(buff, "%01d.%01dG", gForce / 10, gForce % 10); + osdDisplayStatisticLabel(top++, "MAX G-FORCE", buff); + } + +#ifdef USE_ESC_SENSOR + if (osdStatGetState(OSD_STAT_MAX_ESC_TEMP)) { + tfp_sprintf(buff, "%3d%c", osdConvertTemperatureToSelectedUnit(stats.max_esc_temp), osdGetTemperatureSymbolForSelectedUnit()); + osdDisplayStatisticLabel(top++, "MAX ESC TEMP", buff); + } + + if (osdStatGetState(OSD_STAT_MAX_ESC_RPM)) { + itoa(stats.max_esc_rpm, buff, 10); + osdDisplayStatisticLabel(top++, "MAX ESC RPM", buff); + } +#endif + } static void osdShowArmed(void) @@ -1481,7 +1674,7 @@ STATIC_UNIT_TESTED void osdRefresh(timeUs_t currentTimeUs) blinkState = (currentTimeUs / 200000) % 2; #ifdef USE_ESC_SENSOR - if (feature(FEATURE_ESC_SENSOR)) { + if (featureIsEnabled(FEATURE_ESC_SENSOR)) { escDataCombined = getEscSensorData(ESC_SENSOR_COMBINED); } #endif diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 4045a281b6..3566d36f33 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -96,6 +96,9 @@ typedef enum { OSD_CORE_TEMPERATURE, OSD_ANTI_GRAVITY, OSD_MOTOR_DIAG, + OSD_G_FORCE, + OSD_LOG_STATUS, + OSD_FLIP_ARROW, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; @@ -123,6 +126,9 @@ typedef enum { OSD_STAT_MAX_ALTITUDE, OSD_STAT_BLACKBOX, OSD_STAT_BLACKBOX_NUMBER, + OSD_STAT_MAX_G_FORCE, + OSD_STAT_MAX_ESC_TEMP, + OSD_STAT_MAX_ESC_RPM, OSD_STAT_COUNT // MUST BE LAST } osd_stats_e; @@ -162,6 +168,8 @@ typedef enum { OSD_WARNING_CRASH_FLIP, OSD_WARNING_ESC_FAIL, OSD_WARNING_CORE_TEMPERATURE, + OSD_WARNING_RC_SMOOTHING, + OSD_WARNING_FAIL_SAFE, OSD_WARNING_COUNT // MUST BE LAST } osdWarningsFlags_e; @@ -200,6 +208,7 @@ extern timeUs_t resumeRefreshAt; struct displayPort_s; void osdInit(struct displayPort_s *osdDisplayPort); +bool osdInitialized(void); void osdResetAlarms(void); void osdUpdate(timeUs_t currentTimeUs); void osdStatSetState(uint8_t statIndex, bool enabled); diff --git a/src/main/io/osd_slave.c b/src/main/io/osd_slave.c deleted file mode 100644 index 6f6a7bd534..0000000000 --- a/src/main/io/osd_slave.c +++ /dev/null @@ -1,171 +0,0 @@ -/* - * 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 . - */ - -/* - Created by Dominic Clifton - */ - -#include -#include - -#include "platform.h" - -#ifdef USE_OSD_SLAVE - -#include "build/debug.h" -#include "build/version.h" - -#include "common/printf.h" -#include "common/utils.h" - -#include "drivers/display.h" -#include "drivers/max7456_symbols.h" -#include "drivers/time.h" - -#include "io/osd_slave.h" - -//#define OSD_SLAVE_DEBUG - -// when locked the system ignores requests to enter cli or bootloader mode via serial connection. -bool osdSlaveIsLocked = false; - -static displayPort_t *osdDisplayPort; - -static void osdDrawLogo(int x, int y) -{ - char fontOffset = 160; - for (int row = 0; row < 4; row++) { - for (int column = 0; column < 24; column++) { - if (fontOffset != 255) // FIXME magic number - displayWriteChar(osdDisplayPort, x + column, y + row, fontOffset++); - } - } -} - -bool displayDrawScreenQueued = false; -bool receivingScreen = false; -bool stalled = false; - -void osdSlaveDrawScreen(void) -{ - displayDrawScreenQueued = true; -} - -static uint32_t timeoutAt = 0; - -void osdSlaveClearScreen(void) -{ - displayClearScreen(osdDisplayPort); - receivingScreen = true; -} - -void osdSlaveWriteChar(const uint8_t x, const uint8_t y, const uint8_t c) -{ - displayWriteChar(osdDisplayPort, x, y, c); -} - -void osdSlaveWrite(const uint8_t x, const uint8_t y, const char *s) -{ - displayWrite(osdDisplayPort, x, y, s); -} - -void osdSlaveHeartbeat(void) -{ - timeoutAt = micros() + (1000 * 1000); - if (stalled) { - stalled = false; - - displayResync(osdDisplayPort); - } -} - -void osdSlaveInit(displayPort_t *osdDisplayPortToUse) -{ - if (!osdDisplayPortToUse) - return; - - osdDisplayPort = osdDisplayPortToUse; - - delay(100); // need max7456 to be ready before using the displayPort API further. - - displayClearScreen(osdDisplayPort); - displayResync(osdDisplayPort); - - delay(100); // wait a little for video to stabilise - - osdDrawLogo(3, 1); - - char string_buffer[30]; - tfp_sprintf(string_buffer, "V%s", FC_VERSION_STRING); - displayWrite(osdDisplayPort, 20, 6, string_buffer); - displayWrite(osdDisplayPort, 13, 6, "OSD"); - - displayResync(osdDisplayPort); -} - -bool osdSlaveCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs) -{ - UNUSED(currentTimeUs); - UNUSED(currentDeltaTimeUs); - - if (!stalled && (cmp32(currentTimeUs, timeoutAt) > 0)) { - stalled = true; - - displayWrite(osdDisplayPort, 8, 12, "WAITING FOR FC"); - displayResync(osdDisplayPort); - } - - if (!receivingScreen && !displayIsSynced(osdDisplayPort)) { - // queue a screen draw to ensure any remaining characters not written to the screen yet - // remember that displayDrawScreen() may return WITHOUT having fully updated the screen. - displayDrawScreenQueued = true; - } - return receivingScreen || displayDrawScreenQueued; -} - -/* - * Called periodically by the scheduler - */ -void osdSlaveUpdate(timeUs_t currentTimeUs) -{ - UNUSED(currentTimeUs); - -#ifdef MAX7456_DMA_CHANNEL_TX - // don't touch buffers if DMA transaction is in progress - if (displayIsTransferInProgress(osdDisplayPort)) { - return; - } -#endif // MAX7456_DMA_CHANNEL_TX - -#ifdef OSD_SLAVE_DEBUG - char buff[32]; - for (int i = 0; i < 4; i ++) { - tfp_sprintf(buff, "%5d", debug[i]); - displayWrite(osdDisplayPort, i * 8, 0, buff); - } -#endif - - if (displayDrawScreenQueued) { - displayDrawScreen(osdDisplayPort); - displayDrawScreenQueued = false; - receivingScreen = false; - } -} -#endif // OSD_SLAVE diff --git a/src/main/io/rcdevice.c b/src/main/io/rcdevice.c index 4196c66b5f..84d634edb3 100644 --- a/src/main/io/rcdevice.c +++ b/src/main/io/rcdevice.c @@ -30,15 +30,12 @@ #include "io/serial.h" +#include "pg/rcdevice.h" + #include "rcdevice.h" #ifdef USE_RCDEVICE -typedef enum { - RCDP_SETTING_PARSE_WAITING_ID, - RCDP_SETTING_PARSE_WAITING_NAME, - RCDP_SETTING_PARSE_WAITING_VALUE, -} runcamDeviceSettingParseStep_e; typedef struct runcamDeviceExpectedResponseLength_s { uint8_t command; @@ -46,16 +43,16 @@ typedef struct runcamDeviceExpectedResponseLength_s { } runcamDeviceExpectedResponseLength_t; static runcamDeviceExpectedResponseLength_t expectedResponsesLength[] = { - { RCDEVICE_PROTOCOL_COMMAND_GET_SETTINGS, 0xFF}, - { RCDEVICE_PROTOCOL_COMMAND_READ_SETTING_DETAIL, 0xFF}, { RCDEVICE_PROTOCOL_COMMAND_GET_DEVICE_INFO, 5}, { RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_PRESS, 2}, { RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_RELEASE, 2}, { RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION, 3}, - { RCDEVICE_PROTOCOL_COMMAND_WRITE_SETTING, 4}, }; -static uint8_t runcamDeviceGetResponseLength(uint8_t command) +rcdeviceWaitingResponseQueue watingResponseQueue; +static uint8_t recvBuf[RCDEVICE_PROTOCOL_MAX_PACKET_SIZE]; // all the response contexts using same recv buffer + +static uint8_t runcamDeviceGetRespLen(uint8_t command) { for (unsigned int i = 0; i < ARRAYLEN(expectedResponsesLength); i++) { if (expectedResponsesLength[i].command == command) { @@ -66,76 +63,49 @@ static uint8_t runcamDeviceGetResponseLength(uint8_t command) return 0; } -// Verify the response data has received done, return true if the data is still receiving or it was received done. -// return false if the packet has incorrect -static uint8_t runcamDeviceIsResponseReceiveDone(uint8_t command, uint8_t *data, uint8_t dataLen, bool *isDone) +static bool rcdeviceRespCtxQueuePush(rcdeviceWaitingResponseQueue *queue, rcdeviceResponseParseContext_t *respCtx) { - uint8_t expectedResponseDataLength = runcamDeviceGetResponseLength(command); - if (expectedResponseDataLength == 0xFF) { - uint8_t settingDataLength = 0x00; - // get setting datalen first - if (dataLen >= 3) { - settingDataLength = data[2]; - if (dataLen >= (settingDataLength + 4)) { - *isDone = true; - return true; - } - } - - if (settingDataLength > 60) { - return false; - } - } else if (dataLen >= expectedResponseDataLength) { - *isDone = true; - return true; + if (queue == NULL || (queue->itemCount + 1) > MAX_WAITING_RESPONSES) { + return false; } + + queue->buffer[queue->tailPos] = *respCtx; + + int newTailPos = queue->tailPos + 1; + if (newTailPos >= MAX_WAITING_RESPONSES) { + newTailPos = 0; + } + queue->itemCount += 1; + queue->tailPos = newTailPos; + return true; } -// a common way to receive packet and verify it -static uint8_t runcamDeviceReceivePacket(runcamDevice_t *device, uint8_t command, uint8_t *data, int timeoutms) +static rcdeviceResponseParseContext_t* rcdeviceRespCtxQueuePeekFront(rcdeviceWaitingResponseQueue *queue) { - uint8_t dataPos = 0; - uint8_t crc = 0; - uint8_t responseDataLen = 0; - - // wait for reply until timeout(specialy by timeoutms) - timeMs_t timeout = millis() + timeoutms; - bool isWaitingHeader = true; - while (millis() < timeout) { - if (serialRxBytesWaiting(device->serialPort) > 0) { - uint8_t c = serialRead(device->serialPort); - crc = crc8_dvb_s2(crc, c); - - if (data) { - data[dataPos] = c; - } - dataPos++; - - if (isWaitingHeader) { - if (c == RCDEVICE_PROTOCOL_HEADER) { - isWaitingHeader = false; - } - } else { - bool isDone = false; - if (!runcamDeviceIsResponseReceiveDone(command, data, dataPos, &isDone)) { - return 0; - } - - if (isDone) { - responseDataLen = dataPos; - break; - } - } - } + if (queue == NULL || queue->itemCount == 0) { + return NULL; } - // check crc - if (crc != 0) { - return 0; + rcdeviceResponseParseContext_t *ctx = &queue->buffer[queue->headPos]; + return ctx; +} + +STATIC_UNIT_TESTED rcdeviceResponseParseContext_t* rcdeviceRespCtxQueueShift(rcdeviceWaitingResponseQueue *queue) +{ + if (queue == NULL || queue->itemCount == 0) { + return NULL; } - return responseDataLen; + rcdeviceResponseParseContext_t *ctx = &queue->buffer[queue->headPos]; + int newHeadPos = queue->headPos + 1; + if (newHeadPos >= MAX_WAITING_RESPONSES) { + newHeadPos = 0; + } + queue->itemCount -= 1; + queue->headPos = newHeadPos; + + return ctx; } // every time send packet to device, and want to get something from device, @@ -180,62 +150,64 @@ static void runcamDeviceSendPacket(runcamDevice_t *device, uint8_t command, uint } // a common way to send a packet to device, and get response from the device. -static bool runcamDeviceSendRequestAndWaitingResp(runcamDevice_t *device, uint8_t commandID, uint8_t *paramData, uint8_t paramDataLen, uint8_t *outputBuffer, uint8_t *outputBufferLen) +static void runcamDeviceSendRequestAndWaitingResp(runcamDevice_t *device, uint8_t commandID, uint8_t *paramData, uint8_t paramDataLen, timeMs_t tiemout, int maxRetryTimes, void *userInfo, rcdeviceRespParseFunc parseFunc) { - int max_retries = 1; - // here using 1000ms as timeout, because the response from 5 key simulation command need a long time about >= 600ms, - // so set a max value to ensure we can receive the response - int timeoutMs = 1000; + runcamDeviceFlushRxBuffer(device); - // only the command sending on initializing step need retry logic, - // otherwise, the timeout of 1000 ms is enough for the response from device - if (commandID == RCDEVICE_PROTOCOL_COMMAND_GET_DEVICE_INFO) { - max_retries = 5; - timeoutMs = 60; // we have test some device, 60ms as timeout, and retry times be 5, it's stable for most case + rcdeviceResponseParseContext_t responseCtx; + memset(&responseCtx, 0, sizeof(rcdeviceResponseParseContext_t)); + responseCtx.recvBuf = recvBuf; + responseCtx.command = commandID; + responseCtx.maxRetryTimes = maxRetryTimes; + responseCtx.expectedRespLen = runcamDeviceGetRespLen(commandID); + responseCtx.timeout = tiemout; + responseCtx.timeoutTimestamp = millis() + tiemout; + responseCtx.parserFunc = parseFunc; + responseCtx.device = device; + responseCtx.protocolVer = RCDEVICE_PROTOCOL_VERSION_1_0; + if (paramData != NULL) { + memcpy(responseCtx.paramData, paramData, paramDataLen); + responseCtx.paramDataLen = paramDataLen; } + responseCtx.userInfo = userInfo; + rcdeviceRespCtxQueuePush(&watingResponseQueue, &responseCtx); - for (int i = 0; i < max_retries; i++) { - // flush rx buffer - runcamDeviceFlushRxBuffer(device); - - // send packet - runcamDeviceSendPacket(device, commandID, paramData, paramDataLen); - - // waiting response - uint8_t responseLength = runcamDeviceReceivePacket(device, commandID, outputBuffer, timeoutMs); - if (responseLength) { - if (outputBufferLen) { - *outputBufferLen = responseLength; - } - - return true; - } - } - - return false; + // send packet + runcamDeviceSendPacket(device, commandID, paramData, paramDataLen); } -static uint8_t calcCRCFromData(uint8_t *ptr, uint8_t len) +static void runcamDeviceParseV1DeviceInfo(rcdeviceResponseParseContext_t *ctx) +{ + if (ctx->result != RCDEVICE_RESP_SUCCESS) { + return; + } + + runcamDevice_t *device = ctx->device; + device->info.protocolVer = RCDEVICE_PROTOCOL_RCSPLIT_VERSION; + device->info.features = RCDEVICE_PROTOCOL_FEATURE_SIMULATE_POWER_BUTTON | RCDEVICE_PROTOCOL_FEATURE_SIMULATE_WIFI_BUTTON | RCDEVICE_PROTOCOL_FEATURE_CHANGE_MODE; + device->isReady = true; +} + +static uint8_t crc8HighFirst(uint8_t *ptr, uint8_t len) { - uint8_t i; uint8_t crc = 0x00; while (len--) { crc ^= *ptr++; - for (i = 8; i > 0; --i) { - if (crc & 0x80) { + for (unsigned i = 8; i > 0; --i) { + if (crc & 0x80) crc = (crc << 1) ^ 0x31; - } else { + else crc = (crc << 1); - } } } - return crc; + return (crc); } -static void sendCtrlCommand(runcamDevice_t *device, rcsplit_ctrl_argument_e argument) +// for the rcsplits that firmware <= 1.1.0 +static void runcamSplitSendCommand(runcamDevice_t *device, uint8_t argument) { if (!device->serialPort) { - return ; + return; } uint8_t uart_buffer[5] = {0}; @@ -245,7 +217,7 @@ static void sendCtrlCommand(runcamDevice_t *device, rcsplit_ctrl_argument_e argu uart_buffer[1] = RCSPLIT_PACKET_CMD_CTRL; uart_buffer[2] = argument; uart_buffer[3] = RCSPLIT_PACKET_TAIL; - crc = calcCRCFromData(uart_buffer, 4); + crc = crc8HighFirst(uart_buffer, 4); // build up a full request [header]+[command]+[argument]+[crc]+[tail] uart_buffer[3] = crc; @@ -255,87 +227,50 @@ static void sendCtrlCommand(runcamDevice_t *device, rcsplit_ctrl_argument_e argu serialWriteBuf(device->serialPort, uart_buffer, 5); } -// get the device info(firmware version, protocol version and features, see the -// definition of runcamDeviceInfo_t to know more) -static bool runcamDeviceGetDeviceInfo(runcamDevice_t *device, uint8_t *outputBuffer) +static void runcamDeviceParseV2DeviceInfo(rcdeviceResponseParseContext_t *ctx) { - // Send "who are you" command to device to detect the device whether is running RCSplit FW1.0 or RCSplit FW1.1 - int max_retries = 2; - for (int i = 0; i < max_retries; i++) { - runcamDeviceFlushRxBuffer(device); - sendCtrlCommand(device, RCSPLIT_CTRL_ARGU_WHO_ARE_YOU); + if (ctx->result != RCDEVICE_RESP_SUCCESS) { + runcamDeviceFlushRxBuffer(ctx->device); - timeMs_t timeout = millis() + 500; - uint8_t response[5] = { 0 }; - while (millis() < timeout) { - if (serialRxBytesWaiting(device->serialPort) >= 5) { - response[0] = serialRead(device->serialPort); - response[1] = serialRead(device->serialPort); - response[2] = serialRead(device->serialPort); - response[3] = serialRead(device->serialPort); - response[4] = serialRead(device->serialPort); - if (response[0] != RCSPLIT_PACKET_HEADER || response[1] != RCSPLIT_PACKET_CMD_CTRL || response[2] != RCSPLIT_CTRL_ARGU_WHO_ARE_YOU || response[4] != RCSPLIT_PACKET_TAIL) { - break; - } + rcdeviceResponseParseContext_t responseCtx; + memset(&responseCtx, 0, sizeof(rcdeviceResponseParseContext_t)); + responseCtx.recvBuf = recvBuf; + responseCtx.command = 0xFF; + responseCtx.maxRetryTimes = rcdeviceConfig()->initDeviceAttempts; + responseCtx.expectedRespLen = 5; + responseCtx.timeout = rcdeviceConfig()->initDeviceAttemptInterval; + responseCtx.timeoutTimestamp = millis() + rcdeviceConfig()->initDeviceAttemptInterval; + responseCtx.parserFunc = runcamDeviceParseV1DeviceInfo; + responseCtx.device = ctx->device; + responseCtx.protocolVer = RCDEVICE_PROTOCOL_RCSPLIT_VERSION; + rcdeviceRespCtxQueuePush(&watingResponseQueue, &responseCtx); - uint8_t crcFromPacket = response[3]; - response[3] = response[4]; // move packet tail field to crc field, and calc crc with first 4 bytes - uint8_t crc = calcCRCFromData(response, 4); - if (crc != crcFromPacket) { - break; - } - - // generate response for RCSplit FW 1.0 and FW 1.1 - outputBuffer[0] = RCDEVICE_PROTOCOL_HEADER; - // protocol version - outputBuffer[1] = RCDEVICE_PROTOCOL_RCSPLIT_VERSION; - // features - outputBuffer[2] = RCDEVICE_PROTOCOL_FEATURE_SIMULATE_POWER_BUTTON | RCDEVICE_PROTOCOL_FEATURE_SIMULATE_WIFI_BUTTON | RCDEVICE_PROTOCOL_FEATURE_CHANGE_MODE; - outputBuffer[3] = 0; - - crc = 0; - const uint8_t * const end = outputBuffer + 4; - for (const uint8_t *ptr = outputBuffer; ptr < end; ++ptr) { - crc = crc8_dvb_s2(crc, *ptr); - } - outputBuffer[4] = crc; - return true; - } - } + runcamSplitSendCommand(ctx->device, 0xFF); + return; } + runcamDevice_t *device = ctx->device; + device->info.protocolVer = ctx->recvBuf[1]; - return runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_GET_DEVICE_INFO, NULL, 0, outputBuffer, NULL); + uint8_t featureLowBits = ctx->recvBuf[2]; + uint8_t featureHighBits = ctx->recvBuf[3]; + device->info.features = (featureHighBits << 8) | featureLowBits; + device->isReady = true; } -static bool runcamDeviceSend5KeyOSDCableConnectionEvent(runcamDevice_t *device, uint8_t operation, uint8_t *outActionID, uint8_t *outErrorCode) +// get the device info(firmware version, protocol version and features, see the +// definition of runcamDeviceInfo_t to know more) +static void runcamDeviceGetDeviceInfo(runcamDevice_t *device) { - uint8_t outputDataLen = RCDEVICE_PROTOCOL_MAX_PACKET_SIZE; - uint8_t respBuf[RCDEVICE_PROTOCOL_MAX_PACKET_SIZE]; - if (!runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION, &operation, sizeof(uint8_t), respBuf, &outputDataLen)) { - return false; - } - - // the high 4 bits is the operationID that we sent - // the low 4 bits is the result code - uint8_t operationID = (respBuf[1] & 0xF0) >> 4; - bool errorCode = (respBuf[1] & 0x0F); - if (outActionID) { - *outActionID = operationID; - } - - if (outErrorCode) { - *outErrorCode = errorCode; - } - - return true; + runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_GET_DEVICE_INFO, NULL, 0, rcdeviceConfig()->initDeviceAttemptInterval, rcdeviceConfig()->initDeviceAttempts, NULL, runcamDeviceParseV2DeviceInfo); } // init the runcam device, it'll search the UART port with FUNCTION_RCDEVICE id // this function will delay 400ms in the first loop to wait the device prepared, // as we know, there are has some camera need about 200~400ms to initialization, // and then we can send/receive from it. -bool runcamDeviceInit(runcamDevice_t *device) +void runcamDeviceInit(runcamDevice_t *device) { + device->isReady = false; serialPortFunction_e portID = FUNCTION_RCDEVICE; serialPortConfig_t *portConfig = findSerialPortConfig(portID); if (portConfig != NULL) { @@ -344,31 +279,18 @@ bool runcamDeviceInit(runcamDevice_t *device) if (device->serialPort != NULL) { // send RCDEVICE_PROTOCOL_COMMAND_GET_DEVICE_INFO to device to retrive // device info, e.g protocol version, supported features - uint8_t respBuf[RCDEVICE_PROTOCOL_MAX_PACKET_SIZE]; - if (runcamDeviceGetDeviceInfo(device, respBuf)) { - device->info.protocolVersion = respBuf[1]; - - uint8_t featureLowBits = respBuf[2]; - uint8_t featureHighBits = respBuf[3]; - device->info.features = (featureHighBits << 8) | featureLowBits; - - return true; - } - - closeSerialPort(device->serialPort); + runcamDeviceGetDeviceInfo(device); } } - - device->serialPort = NULL; - return false; } + bool runcamDeviceSimulateCameraButton(runcamDevice_t *device, uint8_t operation) { - if (device->info.protocolVersion == RCDEVICE_PROTOCOL_RCSPLIT_VERSION) { - sendCtrlCommand(device, operation + 1); - } else if (device->info.protocolVersion == RCDEVICE_PROTOCOL_VERSION_1_0) { + if (device->info.protocolVer == RCDEVICE_PROTOCOL_VERSION_1_0) { runcamDeviceSendPacket(device, RCDEVICE_PROTOCOL_COMMAND_CAMERA_CONTROL, &operation, sizeof(operation)); + } else if (device->info.protocolVer == RCDEVICE_PROTOCOL_RCSPLIT_VERSION) { + runcamSplitSendCommand(device, operation + 1); } else { return false; } @@ -378,392 +300,107 @@ bool runcamDeviceSimulateCameraButton(runcamDevice_t *device, uint8_t operation) // every time start to control the OSD menu of camera, must call this method to // camera -bool runcamDeviceOpen5KeyOSDCableConnection(runcamDevice_t *device) +void runcamDeviceOpen5KeyOSDCableConnection(runcamDevice_t *device, rcdeviceRespParseFunc parseFunc) { - uint8_t actionID = 0xFF; - uint8_t code = 0xFF; - bool r = runcamDeviceSend5KeyOSDCableConnectionEvent(device, RCDEVICE_PROTOCOL_5KEY_CONNECTION_OPEN, &actionID, &code); - return r && (code == 1) && (actionID == RCDEVICE_PROTOCOL_5KEY_CONNECTION_OPEN); + uint8_t operation = RCDEVICE_PROTOCOL_5KEY_CONNECTION_OPEN; + runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION, &operation, sizeof(uint8_t), 400, 2, NULL, parseFunc); } // when the control was stop, must call this method to the camera to disconnect // with camera. -bool runcamDeviceClose5KeyOSDCableConnection(runcamDevice_t *device, uint8_t *resultCode) +void runcamDeviceClose5KeyOSDCableConnection(runcamDevice_t *device, rcdeviceRespParseFunc parseFunc) { - uint8_t actionID = 0xFF; - uint8_t code = 0xFF; - bool r = runcamDeviceSend5KeyOSDCableConnectionEvent(device, RCDEVICE_PROTOCOL_5KEY_CONNECTION_CLOSE, &actionID, &code); - if (resultCode) { - *resultCode = code; - } - return r; + uint8_t operation = RCDEVICE_PROTOCOL_5KEY_CONNECTION_CLOSE; + runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION, &operation, sizeof(uint8_t), 400, 2, NULL, parseFunc); } // simulate button press event of 5 key osd cable with special button -bool runcamDeviceSimulate5KeyOSDCableButtonPress(runcamDevice_t *device, uint8_t operation) +void runcamDeviceSimulate5KeyOSDCableButtonPress(runcamDevice_t *device, uint8_t operation, rcdeviceRespParseFunc parseFunc) { if (operation == RCDEVICE_PROTOCOL_5KEY_SIMULATION_NONE) { - return false; + return; } - if (runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_PRESS, &operation, sizeof(uint8_t), NULL, NULL)) { - return true; - } - - return false; + runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_PRESS, &operation, sizeof(uint8_t), 400, 2, NULL, parseFunc); } // simulate button release event of 5 key osd cable -bool runcamDeviceSimulate5KeyOSDCableButtonRelease(runcamDevice_t *device) +void runcamDeviceSimulate5KeyOSDCableButtonRelease(runcamDevice_t *device, rcdeviceRespParseFunc parseFunc) { - return runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_RELEASE, NULL, 0, NULL, NULL); + runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_RELEASE, NULL, 0, 400, 2, NULL, parseFunc); } -// fill a region with same char on screen, this is used to DisplayPort feature -// support -void runcamDeviceDispFillRegion(runcamDevice_t *device, uint8_t x, uint8_t y, uint8_t width, uint8_t height, uint8_t c) +static rcdeviceResponseParseContext_t* getWaitingResponse(timeMs_t currentTimeMs) { - uint8_t paramsBuf[5]; - - // fill parameters buf - paramsBuf[0] = x; - paramsBuf[1] = y; - paramsBuf[2] = width; - paramsBuf[3] = height; - paramsBuf[4] = c; - - runcamDeviceSendPacket(device, RCDEVICE_PROTOCOL_COMMAND_DISP_FILL_REGION, paramsBuf, sizeof(paramsBuf)); -} - -// draw a single char on special position on screen, this is used to DisplayPort -// feature support -void runcamDeviceDispWriteChar(runcamDevice_t *device, uint8_t x, uint8_t y, uint8_t c) -{ - uint8_t paramsBuf[3]; - - // fill parameters buf - paramsBuf[0] = x; - paramsBuf[1] = y; - paramsBuf[2] = c; - - runcamDeviceSendPacket(device, RCDEVICE_PROTOCOL_COMMAND_DISP_WRITE_CHAR, paramsBuf, sizeof(paramsBuf)); -} - -static void runcamDeviceDispWriteString(runcamDevice_t *device, uint8_t x, uint8_t y, const char *text, bool isHorizontal) -{ - uint8_t textLen = strlen(text); - if (textLen > 60) { // if text len more then 60 chars, cut it to 60 - textLen = 60; - } - - uint8_t paramsBufLen = 3 + textLen; - uint8_t paramsBuf[RCDEVICE_PROTOCOL_MAX_DATA_SIZE]; - - paramsBuf[0] = paramsBufLen - 1; - paramsBuf[1] = x; - paramsBuf[2] = y; - memcpy(paramsBuf + 3, text, textLen); - - uint8_t command = isHorizontal ? RCDEVICE_PROTOCOL_COMMAND_DISP_WRITE_HORIZONTAL_STRING : RCDEVICE_PROTOCOL_COMMAND_DISP_WRITE_VERTICAL_STRING; - runcamDeviceSendPacket(device, command, paramsBuf, paramsBufLen); -} - -// draw a string on special position on screen, this is used to DisplayPort -// feature support -void runcamDeviceDispWriteHorizontalString(runcamDevice_t *device, uint8_t x, uint8_t y, const char *text) -{ - runcamDeviceDispWriteString(device, x, y, text, true); -} - -void runcamDeviceDispWriteVerticalString(runcamDevice_t *device, uint8_t x, uint8_t y, const char *text) -{ - runcamDeviceDispWriteString(device, x, y, text, false); -} - -void runcamDeviceDispWriteChars(runcamDevice_t *device, uint8_t *data, uint8_t datalen) -{ - uint8_t adjustedDataLen = datalen; - if (adjustedDataLen > 60) { // if data len more then 60 chars, cut it to 60 - adjustedDataLen = 60; - } - - uint8_t paramsBufLen = adjustedDataLen + 1; - uint8_t paramsBuf[RCDEVICE_PROTOCOL_MAX_DATA_SIZE]; - - paramsBuf[0] = adjustedDataLen; - memcpy(paramsBuf + 1, data, adjustedDataLen); - - runcamDeviceSendPacket(device, RCDEVICE_PROTOCOL_COMMAND_DISP_WRITE_CHARS, paramsBuf, paramsBufLen); -} - -static bool runcamDeviceDecodeSettings(sbuf_t *buf, runcamDeviceSetting_t *outSettingList, int maxSettingItemCount) -{ - if (outSettingList == NULL) { - return false; - } - - if (maxSettingItemCount > RCDEVICE_PROTOCOL_MAX_MENUITEM_PER_PAGE) - maxSettingItemCount = RCDEVICE_PROTOCOL_MAX_MENUITEM_PER_PAGE; - - runcamDeviceSettingParseStep_e parseStep = RCDP_SETTING_PARSE_WAITING_ID; - memset(outSettingList, 0, maxSettingItemCount * sizeof(runcamDeviceSetting_t)); - runcamDeviceSetting_t *settingIterator = outSettingList; - while (sbufBytesRemaining(buf) > 0) { - if (settingIterator >= (outSettingList + maxSettingItemCount)) { - break; - } - - switch (parseStep) { - case RCDP_SETTING_PARSE_WAITING_ID: { - settingIterator->id = sbufReadU8(buf); - parseStep = RCDP_SETTING_PARSE_WAITING_NAME; - } - break; - case RCDP_SETTING_PARSE_WAITING_NAME: { - const char *str = (const char *)sbufConstPtr(buf); - uint8_t nameLen = strlen(str) + 1; - memset(settingIterator->name, 0, RCDEVICE_PROTOCOL_MAX_SETTING_NAME_LENGTH); - strncpy(settingIterator->name, str, RCDEVICE_PROTOCOL_MAX_SETTING_NAME_LENGTH); - sbufAdvance(buf, nameLen); - - parseStep = RCDP_SETTING_PARSE_WAITING_VALUE; - } - break; - case RCDP_SETTING_PARSE_WAITING_VALUE: { - const char *str = (const char *)sbufConstPtr(buf); - uint8_t valueLen = strlen(str) + 1; - memset(settingIterator->value, 0, RCDEVICE_PROTOCOL_MAX_SETTING_VALUE_LENGTH); - strcpy(settingIterator->value, str); - sbufAdvance(buf, valueLen); - parseStep = RCDP_SETTING_PARSE_WAITING_ID; - - settingIterator++; - } - break; - } - } - - if (RCDP_SETTING_PARSE_WAITING_ID != parseStep) { - return false; - } - - return true; -} - -static bool runcamDeviceGetResponseWithMultipleChunk(runcamDevice_t *device, uint8_t command, uint8_t settingID, uint8_t *responseData, uint16_t *responseDatalen) -{ - if (responseData == NULL || responseDatalen == NULL) { - return false; - } - - // fill parameters buf - uint8_t paramsBuf[2]; - uint8_t chunkIndex = 0; - paramsBuf[0] = settingID; // parent setting id - paramsBuf[1] = chunkIndex; // chunk index - - uint8_t outputBufLen = RCDEVICE_PROTOCOL_MAX_PACKET_SIZE; - uint8_t outputBuf[RCDEVICE_PROTOCOL_MAX_PACKET_SIZE]; - bool result = runcamDeviceSendRequestAndWaitingResp(device, command, paramsBuf, sizeof(paramsBuf), outputBuf, &outputBufLen); - if (!result) { - return false; - } - - uint8_t remainingChunk = outputBuf[1]; - // Every response chunk count must less than or equal to RCDEVICE_PROTOCOL_MAX_CHUNK_PER_RESPONSE - if (remainingChunk >= RCDEVICE_PROTOCOL_MAX_CHUNK_PER_RESPONSE) { - return false; - } - - // save setting data to sbuf_t object - const uint16_t maxDataLen = RCDEVICE_PROTOCOL_MAX_CHUNK_PER_RESPONSE * RCDEVICE_PROTOCOL_MAX_DATA_SIZE; - // uint8_t data[maxDataLen]; - sbuf_t dataBuf; - dataBuf.ptr = responseData; - dataBuf.end = responseData + maxDataLen; - sbufWriteData(&dataBuf, outputBuf + 3, outputBufLen - 4); - - // get the remaining chunks - while (remainingChunk > 0) { - paramsBuf[1] = ++chunkIndex; // chunk index - - outputBufLen = RCDEVICE_PROTOCOL_MAX_PACKET_SIZE; - result = runcamDeviceSendRequestAndWaitingResp(device, command, paramsBuf, sizeof(paramsBuf), outputBuf, &outputBufLen); - - if (!result) { - return false; - } - - // append the trailing chunk to the sbuf_t object, - // but only append the actually setting data - sbufWriteData(&dataBuf, outputBuf + 3, outputBufLen - 4); - - remainingChunk--; - } - - sbufSwitchToReader(&dataBuf, responseData); - *responseDatalen = sbufBytesRemaining(&dataBuf); - - return true; -} - -// get settings with parent setting id, the type of parent setting must be a -// FOLDER after this function called, the settings will fill into outSettingList -// argument -bool runcamDeviceGetSettings(runcamDevice_t *device, uint8_t parentSettingID, runcamDeviceSetting_t *outSettingList, int maxSettingItemCount) -{ - if (outSettingList == NULL) { - return false; - } - - uint16_t responseDataLength = 0; - uint8_t data[RCDEVICE_PROTOCOL_MAX_CHUNK_PER_RESPONSE * RCDEVICE_PROTOCOL_MAX_DATA_SIZE]; - if (!runcamDeviceGetResponseWithMultipleChunk(device, RCDEVICE_PROTOCOL_COMMAND_GET_SETTINGS, parentSettingID, data, &responseDataLength)) { - return false; - } - - sbuf_t dataBuf; - dataBuf.ptr = data; - dataBuf.end = data + responseDataLength; - - // parse the settings data and convert them into a runcamDeviceSetting_t list - if (!runcamDeviceDecodeSettings(&dataBuf, outSettingList, maxSettingItemCount)) { - return false; - } - - return true; -} - -static bool runcamDeviceDecodeSettingDetail(sbuf_t *buf, runcamDeviceSettingDetail_t *outSettingDetail) -{ - if (outSettingDetail == NULL || sbufBytesRemaining(buf) == 0) { - return false; - } - - rcdeviceSettingType_e settingType = sbufReadU8(buf); - outSettingDetail->type = settingType; - switch (settingType) { - case RCDEVICE_PROTOCOL_SETTINGTYPE_UINT8: - case RCDEVICE_PROTOCOL_SETTINGTYPE_INT8: - outSettingDetail->value = sbufReadU8(buf); - outSettingDetail->minValue = sbufReadU8(buf); - outSettingDetail->maxValue = sbufReadU8(buf); - outSettingDetail->stepSize = sbufReadU8(buf); - break; - case RCDEVICE_PROTOCOL_SETTINGTYPE_UINT16: - case RCDEVICE_PROTOCOL_SETTINGTYPE_INT16: - outSettingDetail->value = sbufReadU16(buf); - outSettingDetail->minValue = sbufReadU16(buf); - outSettingDetail->maxValue = sbufReadU16(buf); - outSettingDetail->stepSize = sbufReadU8(buf); - break; - case RCDEVICE_PROTOCOL_SETTINGTYPE_FLOAT: - outSettingDetail->value = sbufReadU32(buf); - outSettingDetail->minValue = sbufReadU32(buf); - outSettingDetail->maxValue = sbufReadU32(buf); - outSettingDetail->decimalPoint = sbufReadU8(buf); - outSettingDetail->stepSize = sbufReadU32(buf); - break; - case RCDEVICE_PROTOCOL_SETTINGTYPE_TEXT_SELECTION: { - outSettingDetail->value = sbufReadU8(buf); - - const char *tmp = (const char *)sbufConstPtr(buf); - const uint16_t maxLen = RCDEVICE_PROTOCOL_MAX_DATA_SIZE * RCDEVICE_PROTOCOL_MAX_TEXT_SELECTIONS; - char textSels[maxLen]; - memset(textSels, 0, maxLen); - strncpy(textSels, tmp, maxLen); - char delims[] = ";"; - char *result = strtok(textSels, delims); - int i = 0; - runcamDeviceSettingTextSelection_t *iterator = outSettingDetail->textSelections; - while (result != NULL) { - if (i >= RCDEVICE_PROTOCOL_MAX_TEXT_SELECTIONS) { - break; + rcdeviceResponseParseContext_t *respCtx = rcdeviceRespCtxQueuePeekFront(&watingResponseQueue); + while (respCtx != NULL && respCtx->timeoutTimestamp != 0 && currentTimeMs > respCtx->timeoutTimestamp) { + if (respCtx->maxRetryTimes > 0) { + if (respCtx->protocolVer == RCDEVICE_PROTOCOL_VERSION_1_0) { + runcamDeviceSendPacket(respCtx->device, respCtx->command, respCtx->paramData, respCtx->paramDataLen); + } else if (respCtx->protocolVer == RCDEVICE_PROTOCOL_RCSPLIT_VERSION) { + runcamSplitSendCommand(respCtx->device, respCtx->command); } - memset(iterator->text, 0, RCDEVICE_PROTOCOL_MAX_SETTING_VALUE_LENGTH); - strncpy(iterator->text, result, RCDEVICE_PROTOCOL_MAX_SETTING_VALUE_LENGTH); - iterator++; - result = strtok(NULL, delims); - i++; + respCtx->recvRespLen = 0; + respCtx->timeoutTimestamp = currentTimeMs + respCtx->timeout; + respCtx->maxRetryTimes -= 1; + respCtx = NULL; + break; + } else { + respCtx->result = RCDEVICE_RESP_TIMEOUT; + if (respCtx->parserFunc != NULL) { + respCtx->parserFunc(respCtx); + } + + // dequeue and get next waiting response context + rcdeviceRespCtxQueueShift(&watingResponseQueue); + respCtx = rcdeviceRespCtxQueuePeekFront(&watingResponseQueue); } } - break; - case RCDEVICE_PROTOCOL_SETTINGTYPE_STRING: { - const char *tmp = (const char *)sbufConstPtr(buf); - strncpy(outSettingDetail->stringValue, tmp, RCDEVICE_PROTOCOL_MAX_STRING_LENGTH); - sbufAdvance(buf, strlen(tmp) + 1); - outSettingDetail->maxStringSize = sbufReadU8(buf); - } - break; - case RCDEVICE_PROTOCOL_SETTINGTYPE_FOLDER: - break; - case RCDEVICE_PROTOCOL_SETTINGTYPE_INFO: { - const char *tmp = (const char *)sbufConstPtr(buf); - strncpy(outSettingDetail->stringValue, tmp, RCDEVICE_PROTOCOL_MAX_STRING_LENGTH); - sbufAdvance(buf, strlen(outSettingDetail->stringValue) + 1); - } - break; - case RCDEVICE_PROTOCOL_SETTINGTYPE_UNKNOWN: - break; - } - - return true; + return respCtx; } -// get the setting details with setting id -// after this function called, the setting detail will fill into -// outSettingDetail argument -bool runcamDeviceGetSettingDetail(runcamDevice_t *device, uint8_t settingID, runcamDeviceSettingDetail_t *outSettingDetail) +void rcdeviceReceive(timeUs_t currentTimeUs) { - if (outSettingDetail == NULL) - return false; + UNUSED(currentTimeUs); + rcdeviceResponseParseContext_t *respCtx = NULL; + while ((respCtx = getWaitingResponse(millis())) != NULL && serialRxBytesWaiting(respCtx->device->serialPort)) { + const uint8_t c = serialRead(respCtx->device->serialPort); - uint16_t responseDataLength = 0; - uint8_t data[RCDEVICE_PROTOCOL_MAX_CHUNK_PER_RESPONSE * RCDEVICE_PROTOCOL_MAX_DATA_SIZE]; - if (!runcamDeviceGetResponseWithMultipleChunk(device, RCDEVICE_PROTOCOL_COMMAND_READ_SETTING_DETAIL, settingID, data, &responseDataLength)) { - return false; + if (respCtx->recvRespLen == 0) { + // Only start receiving packet when we found a header + if ((respCtx->protocolVer == RCDEVICE_PROTOCOL_VERSION_1_0 && c != RCDEVICE_PROTOCOL_HEADER) || (respCtx->protocolVer == RCDEVICE_PROTOCOL_RCSPLIT_VERSION && c != RCSPLIT_PACKET_HEADER)) { + continue; + } + } + + respCtx->recvBuf[respCtx->recvRespLen] = c; + respCtx->recvRespLen += 1; + + // if data received done, trigger callback to parse response data, and update rcdevice state + if (respCtx->recvRespLen == respCtx->expectedRespLen) { + // verify the crc value + if (respCtx->protocolVer == RCDEVICE_PROTOCOL_VERSION_1_0) { + uint8_t crc = 0; + for (int i = 0; i < respCtx->recvRespLen; i++) { + crc = crc8_dvb_s2(crc, respCtx->recvBuf[i]); + } + + respCtx->result = (crc == 0) ? RCDEVICE_RESP_SUCCESS : RCDEVICE_RESP_INCORRECT_CRC; + } else if (respCtx->protocolVer == RCDEVICE_PROTOCOL_RCSPLIT_VERSION) { + // do nothing, just call parserFunc + respCtx->result = RCDEVICE_RESP_SUCCESS; + } + + if (respCtx->parserFunc != NULL) { + respCtx->parserFunc(respCtx); + } + + // dequeue current response context + rcdeviceRespCtxQueueShift(&watingResponseQueue); + } } - - sbuf_t dataBuf; - dataBuf.ptr = data; - dataBuf.end = data + responseDataLength; - - // parse the settings data and convert them into a runcamDeviceSettingDetail_t - if (!runcamDeviceDecodeSettingDetail(&dataBuf, outSettingDetail)) { - return false; - } - - return true; -} - -// write new value with to the setting -bool runcamDeviceWriteSetting(runcamDevice_t *device, uint8_t settingID, uint8_t *paramData, uint8_t paramDataLen, runcamDeviceWriteSettingResponse_t *response) -{ - if (response == NULL || paramDataLen > (RCDEVICE_PROTOCOL_MAX_DATA_SIZE - 1)) { - return false; - } - - memset(response, 0, sizeof(runcamDeviceWriteSettingResponse_t)); - response->resultCode = 1; // initialize the result code to failed - - uint8_t paramsBufLen = sizeof(uint8_t) + paramDataLen; - uint8_t paramsBuf[RCDEVICE_PROTOCOL_MAX_DATA_SIZE]; - paramsBuf[0] = settingID; - memcpy(paramsBuf + 1, paramData, paramDataLen); - - uint8_t outputBufLen = RCDEVICE_PROTOCOL_MAX_PACKET_SIZE; - uint8_t outputBuf[RCDEVICE_PROTOCOL_MAX_PACKET_SIZE]; - bool result = runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_WRITE_SETTING, paramsBuf, paramsBufLen, outputBuf, &outputBufLen); - if (!result) { - return false; - } - - response->resultCode = outputBuf[1]; - response->needUpdateMenuItems = outputBuf[2]; - - return true; } #endif diff --git a/src/main/io/rcdevice.h b/src/main/io/rcdevice.h index 52a9030249..175077c036 100644 --- a/src/main/io/rcdevice.h +++ b/src/main/io/rcdevice.h @@ -29,13 +29,6 @@ #define RCDEVICE_PROTOCOL_MAX_PACKET_SIZE 64 #define RCDEVICE_PROTOCOL_MAX_DATA_SIZE 62 -#define RCDEVICE_PROTOCOL_MAX_DATA_SIZE_WITH_CRC_FIELD 63 -#define RCDEVICE_PROTOCOL_MAX_MENUITEM_PER_PAGE 32 -#define RCDEVICE_PROTOCOL_MAX_SETTING_NAME_LENGTH 20 -#define RCDEVICE_PROTOCOL_MAX_SETTING_VALUE_LENGTH 20 -#define RCDEVICE_PROTOCOL_MAX_CHUNK_PER_RESPONSE 12 -#define RCDEVICE_PROTOCOL_MAX_TEXT_SELECTIONS 30 -#define RCDEVICE_PROTOCOL_MAX_STRING_LENGTH 58 // Commands #define RCDEVICE_PROTOCOL_COMMAND_GET_DEVICE_INFO 0x00 @@ -45,17 +38,11 @@ #define RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_PRESS 0x02 #define RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_RELEASE 0x03 #define RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION 0x04 -// device setting access -#define RCDEVICE_PROTOCOL_COMMAND_GET_SETTINGS 0x10 -#define RCDEVICE_PROTOCOL_COMMAND_READ_SETTING_DETAIL 0x11 -#define RCDEVICE_PROTOCOL_COMMAND_READ_SETTING 0x12 -#define RCDEVICE_PROTOCOL_COMMAND_WRITE_SETTING 0x13 -// display port support -#define RCDEVICE_PROTOCOL_COMMAND_DISP_FILL_REGION 0x20 -#define RCDEVICE_PROTOCOL_COMMAND_DISP_WRITE_CHAR 0x21 -#define RCDEVICE_PROTOCOL_COMMAND_DISP_WRITE_HORIZONTAL_STRING 0x22 -#define RCDEVICE_PROTOCOL_COMMAND_DISP_WRITE_VERTICAL_STRING 0x23 -#define RCDEVICE_PROTOCOL_COMMAND_DISP_WRITE_CHARS 0x24 + +// Old protocol defines +#define RCSPLIT_PACKET_HEADER 0x55 +#define RCSPLIT_PACKET_CMD_CTRL 0x01 +#define RCSPLIT_PACKET_TAIL 0xaa // Feature Flag sets, it's a uint16_t flag typedef enum { @@ -63,8 +50,6 @@ typedef enum { RCDEVICE_PROTOCOL_FEATURE_SIMULATE_WIFI_BUTTON = (1 << 1), RCDEVICE_PROTOCOL_FEATURE_CHANGE_MODE = (1 << 2), RCDEVICE_PROTOCOL_FEATURE_SIMULATE_5_KEY_OSD_CABLE = (1 << 3), - RCDEVICE_PROTOCOL_FEATURE_DEVICE_SETTINGS_ACCESS = (1 << 4), - RCDEVICE_PROTOCOL_FEATURE_DISPLAYP_PORT = (1 << 5), RCDEVICE_PROTOCOL_FEATURE_START_RECORDING = (1 << 6), RCDEVICE_PROTOCOL_FEATURE_STOP_RECORDING = (1 << 7), RCDEVICE_PROTOCOL_FEATURE_CMS_MENU = (1 << 8), @@ -116,127 +101,63 @@ typedef enum { RCDEVICE_PROTOCOL_UNKNOWN } rcdevice_protocol_version_e; -// Reserved setting ids -typedef enum { - RCDEVICE_PROTOCOL_SETTINGID_DISP_CHARSET = 0, // type: text_selection, read&write, 0: use charset with betaflight logo, 1 use - // charset with cleanflight logo, other id are not used - RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS = 1, // type: uint8_t, read only, the column count of the OSD layer - RCDEVICE_PROTOCOL_SETTINGID_DISP_TV_MODE = 2, // type: text_selection, read&write, 0:NTSC, 1:PAL - RCDEVICE_PROTOCOL_SETTINGID_SDCARD_CAPACITY = 3, // type: info, read only, return sd card capacity - RCDEVICE_PROTOCOL_SETTINGID_REMAINING_RECORDING_TIME = 4, // type: info, read only, return remaining recording time - RCDEVICE_PROTOCOL_SETTINGID_RESOLUTION = 5, // type: text selection, read&write, return the current resolution and all available resolutions - RCDEVICE_PROTOCOL_SETTINGID_CAMERA_TIME = 6, // type: string, read&write, update the camera time, the time attribute of medias file in camera will use this time. - RCDEVICE_PROTOCOL_SETTINGID_RESERVED7 = 7, - RCDEVICE_PROTOCOL_SETTINGID_RESERVED8 = 8, - RCDEVICE_PROTOCOL_SETTINGID_RESERVED9 = 9, - RCDEVICE_PROTOCOL_SETTINGID_RESERVED10 = 10, - RCDEVICE_PROTOCOL_SETTINGID_RESERVED11 = 11, - RCDEVICE_PROTOCOL_SETTINGID_RESERVED12 = 12, - RCDEVICE_PROTOCOL_SETTINGID_RESERVED13 = 13, - RCDEVICE_PROTOCOL_SETTINGID_RESERVED14 = 14, - RCDEVICE_PROTOCOL_SETTINGID_RESERVED15 = 15, - RCDEVICE_PROTOCOL_SETTINGID_RESERVED16 = 16, - RCDEVICE_PROTOCOL_SETTINGID_RESERVED17 = 17, - RCDEVICE_PROTOCOL_SETTINGID_RESERVED18 = 18, - RCDEVICE_PROTOCOL_SETTINGID_RESERVED19 = 19, -} rcdeviceReservedSettingID_e; - -typedef enum { - RCDEVICE_PROTOCOL_SETTINGTYPE_UINT8 = 0, - RCDEVICE_PROTOCOL_SETTINGTYPE_INT8 = 1, - RCDEVICE_PROTOCOL_SETTINGTYPE_UINT16 = 2, - RCDEVICE_PROTOCOL_SETTINGTYPE_INT16 = 3, - RCDEVICE_PROTOCOL_SETTINGTYPE_FLOAT = 8, - RCDEVICE_PROTOCOL_SETTINGTYPE_TEXT_SELECTION = 9, - RCDEVICE_PROTOCOL_SETTINGTYPE_STRING = 10, - RCDEVICE_PROTOCOL_SETTINGTYPE_FOLDER = 11, - RCDEVICE_PROTOCOL_SETTINGTYPE_INFO = 12, - RCDEVICE_PROTOCOL_SETTINGTYPE_UNKNOWN -} rcdeviceSettingType_e; - -typedef enum { - RCDEVICE_SUCCEED = 0, - RCDEVICE_INVALID = 1, - RCDEVICE_NODEV = 2, - RCDEVICE_DEVBUSY = 3, -} rcdeviceErrorCode_e; - // end of Runcam Device definition -// Old version defination(RCSplit firmware v1.0.0 and v1.1.0) -// packet header and tail -#define RCSPLIT_PACKET_HEADER 0x55 -#define RCSPLIT_PACKET_CMD_CTRL 0x01 -#define RCSPLIT_PACKET_TAIL 0xaa - - - -typedef enum { - RCSPLIT_CTRL_ARGU_INVALID = 0x0, - RCSPLIT_CTRL_ARGU_WIFI_BTN = 0x1, - RCSPLIT_CTRL_ARGU_POWER_BTN = 0x2, - RCSPLIT_CTRL_ARGU_CHANGE_MODE = 0x3, - RCSPLIT_CTRL_ARGU_WHO_ARE_YOU = 0xFF, -} rcsplit_ctrl_argument_e; -// end of old version protocol definition - typedef struct runcamDeviceInfo_s { - rcdevice_protocol_version_e protocolVersion; + rcdevice_protocol_version_e protocolVer; uint16_t features; } runcamDeviceInfo_t; -typedef struct runcamDeviceSetting_s { - uint8_t id; - char name[RCDEVICE_PROTOCOL_MAX_SETTING_NAME_LENGTH]; - char value[RCDEVICE_PROTOCOL_MAX_SETTING_VALUE_LENGTH]; -} runcamDeviceSetting_t; - -typedef struct runcamDeviceSettingTextSelection_s { - char text[RCDEVICE_PROTOCOL_MAX_SETTING_VALUE_LENGTH]; -} runcamDeviceSettingTextSelection_t; - -typedef struct runcamDeviceSettingDetail_s { - uint8_t type; - uint32_t value; - uint32_t minValue; - uint32_t maxValue; - uint8_t decimalPoint; - uint32_t stepSize; - uint8_t maxStringSize; - char stringValue[RCDEVICE_PROTOCOL_MAX_STRING_LENGTH]; // when settingType is RCDEVICE_PROTOCOL_SETTINGTYPE_INFO or RCDEVICE_PROTOCOL_SETTINGTYPE_STRING, this field store the string/info value; - runcamDeviceSettingTextSelection_t textSelections[RCDEVICE_PROTOCOL_MAX_TEXT_SELECTIONS]; -} runcamDeviceSettingDetail_t; - -typedef struct runcamDeviceWriteSettingResponse_s { - uint8_t resultCode; - uint8_t needUpdateMenuItems; -} runcamDeviceWriteSettingResponse_t; - typedef struct runcamDevice_s { serialPort_t *serialPort; uint8_t buffer[RCDEVICE_PROTOCOL_MAX_PACKET_SIZE]; runcamDeviceInfo_t info; + bool isReady; } runcamDevice_t; -bool runcamDeviceInit(runcamDevice_t *device); +#define MAX_WAITING_RESPONSES 20 + +typedef enum { + RCDEVICE_RESP_SUCCESS = 0, + RCDEVICE_RESP_INCORRECT_CRC = 1, + RCDEVICE_RESP_TIMEOUT = 2 +} rcdeviceResponseStatus_e; + +typedef struct rcdeviceResponseParseContext_s rcdeviceResponseParseContext_t; +typedef void(*rcdeviceRespParseFunc)(rcdeviceResponseParseContext_t*); +struct rcdeviceResponseParseContext_s { + uint8_t command; + uint8_t expectedRespLen; // total length of response data + uint8_t recvRespLen; // length of the data received + uint8_t *recvBuf; // response data buffer + timeMs_t timeout; + timeMs_t timeoutTimestamp; // if zero, it's means keep waiting for the response + rcdeviceRespParseFunc parserFunc; + runcamDevice_t *device; + uint8_t paramData[RCDEVICE_PROTOCOL_MAX_DATA_SIZE]; + uint8_t paramDataLen; + uint8_t protocolVer; + int maxRetryTimes; + void *userInfo; + rcdeviceResponseStatus_e result; +}; + +typedef struct { + uint8_t headPos; // current head position of the queue + uint8_t tailPos; + uint8_t itemCount; // the item count in the queue + rcdeviceResponseParseContext_t buffer[MAX_WAITING_RESPONSES]; + rcdeviceRespParseFunc parseFunc; +} rcdeviceWaitingResponseQueue; + +void runcamDeviceInit(runcamDevice_t *device); +void rcdeviceReceive(timeUs_t currentTimeUs); // camera button simulation bool runcamDeviceSimulateCameraButton(runcamDevice_t *device, uint8_t operation); // 5 key osd cable simulation -bool runcamDeviceOpen5KeyOSDCableConnection(runcamDevice_t *device); -bool runcamDeviceClose5KeyOSDCableConnection(runcamDevice_t *device, uint8_t *resultCode); -bool runcamDeviceSimulate5KeyOSDCableButtonPress(runcamDevice_t *device, uint8_t operation); -bool runcamDeviceSimulate5KeyOSDCableButtonRelease(runcamDevice_t *device); - -// DisplayPort feature support -void runcamDeviceDispFillRegion(runcamDevice_t *device, uint8_t x, uint8_t y, uint8_t width, uint8_t height, uint8_t c); -void runcamDeviceDispWriteChar(runcamDevice_t *device, uint8_t x, uint8_t y, uint8_t c); -void runcamDeviceDispWriteHorizontalString(runcamDevice_t *device, uint8_t x, uint8_t y, const char *text); -void runcamDeviceDispWriteVerticalString(runcamDevice_t *device, uint8_t x, uint8_t y, const char *text); -void runcamDeviceDispWriteChars(runcamDevice_t *device, uint8_t *data, uint8_t datalen); - -// Device Setting Access -bool runcamDeviceGetSettings(runcamDevice_t *device, uint8_t parentSettingID, runcamDeviceSetting_t *outSettingList, int maxSettingItemCount); -bool runcamDeviceGetSettingDetail(runcamDevice_t *device, uint8_t settingID, runcamDeviceSettingDetail_t *outSettingDetail); -bool runcamDeviceWriteSetting(runcamDevice_t *device, uint8_t settingID, uint8_t *data, uint8_t dataLen, runcamDeviceWriteSettingResponse_t *response); +void runcamDeviceOpen5KeyOSDCableConnection(runcamDevice_t *device, rcdeviceRespParseFunc parseFunc); +void runcamDeviceClose5KeyOSDCableConnection(runcamDevice_t *device, rcdeviceRespParseFunc parseFunc); +void runcamDeviceSimulate5KeyOSDCableButtonPress(runcamDevice_t *device, uint8_t operation, rcdeviceRespParseFunc parseFunc); +void runcamDeviceSimulate5KeyOSDCableButtonRelease(runcamDevice_t *device, rcdeviceRespParseFunc parseFunc); \ No newline at end of file diff --git a/src/main/io/rcdevice_cam.c b/src/main/io/rcdevice_cam.c index faf1978733..fd3d956850 100644 --- a/src/main/io/rcdevice_cam.c +++ b/src/main/io/rcdevice_cam.c @@ -25,7 +25,7 @@ #include "pg/rx.h" -#include "drivers/time.h" +#include "common/time.h" #include "cms/cms.h" @@ -44,11 +44,14 @@ #define IS_LO(X) (rcData[X] < FIVE_KEY_CABLE_JOYSTICK_MIN) #define IS_MID(X) (rcData[X] > FIVE_KEY_CABLE_JOYSTICK_MID_START && rcData[X] < FIVE_KEY_CABLE_JOYSTICK_MID_END) + static runcamDevice_t runcamDevice; runcamDevice_t *camDevice = &runcamDevice; rcdeviceSwitchState_t switchStates[BOXCAMERA3 - BOXCAMERA1 + 1]; -bool rcdeviceInMenu; -bool needRelease = false; +bool rcdeviceInMenu = false; +bool isButtonPressed = false; +bool waitingDeviceResponse = false; + static bool isFeatureSupported(uint8_t feature) { @@ -59,21 +62,6 @@ static bool isFeatureSupported(uint8_t feature) return false; } -static bool rcdeviceIsCameraControlEnabled(void) -{ - bool isPowerSimulationSupported = isFeatureSupported(RCDEVICE_PROTOCOL_FEATURE_SIMULATE_POWER_BUTTON); - bool isWiFiSimulationSupported = isFeatureSupported(RCDEVICE_PROTOCOL_FEATURE_SIMULATE_WIFI_BUTTON); - bool isChangeModeSupported = isFeatureSupported(RCDEVICE_PROTOCOL_FEATURE_CHANGE_MODE); - bool isStartRecordingSupported = isFeatureSupported(RCDEVICE_PROTOCOL_FEATURE_START_RECORDING); - bool isStopRecordingSupported = isFeatureSupported(RCDEVICE_PROTOCOL_FEATURE_STOP_RECORDING); - - if (camDevice->serialPort != NULL && (isPowerSimulationSupported || isWiFiSimulationSupported || isChangeModeSupported || isStartRecordingSupported || isStopRecordingSupported)) { - return true; - } - - return false; -} - bool rcdeviceIsEnabled(void) { return camDevice->serialPort != NULL; @@ -88,39 +76,13 @@ static bool rcdeviceIs5KeyEnabled(void) return false; } -static bool reInitializeDevice() { -#define MAX_RETRY_COUNT 4 -#define RETRY_INTERVAL_MS 500 - static timeMs_t lastInitializeTime = 0; - static int tryInitCount = 0; - bool result = false; - - if (ARMING_FLAG(ARMED)) { - return false; - } - - if (tryInitCount < MAX_RETRY_COUNT) { - timeMs_t nextRetryTime = lastInitializeTime + RETRY_INTERVAL_MS; - if (millis() >= nextRetryTime) { - result = rcdeviceInit(); - tryInitCount++; - lastInitializeTime = millis(); - } - } - - return result; -} - static void rcdeviceCameraControlProcess(void) { for (boxId_e i = BOXCAMERA1; i <= BOXCAMERA3; i++) { uint8_t switchIndex = i - BOXCAMERA1; if (IS_RC_MODE_ACTIVE(i)) { - if (!rcdeviceIsCameraControlEnabled()) { - reInitializeDevice(); - } - + // check last state of this mode, if it's true, then ignore it. // Here is a logic to make a toggle control for this mode if (switchStates[switchIndex].isActivated) { @@ -131,17 +93,25 @@ static void rcdeviceCameraControlProcess(void) switch (i) { case BOXCAMERA1: if (isFeatureSupported(RCDEVICE_PROTOCOL_FEATURE_SIMULATE_WIFI_BUTTON)) { - behavior = RCDEVICE_PROTOCOL_CAM_CTRL_SIMULATE_WIFI_BTN; + // avoid display wifi page when arming, in the next firmware(>2.0) of rcsplit we have change the wifi page logic: + // when the wifi was turn on it won't turn off the analog video output, + // and just put a wifi indicator on the right top of the video output. here is for the old split firmware + if (!ARMING_FLAG(ARMED) && ((getArmingDisableFlags() & ARMING_DISABLED_RUNAWAY_TAKEOFF) == 0)) { + behavior = RCDEVICE_PROTOCOL_CAM_CTRL_SIMULATE_WIFI_BTN; + } } break; case BOXCAMERA2: if (isFeatureSupported(RCDEVICE_PROTOCOL_FEATURE_SIMULATE_POWER_BUTTON)) { - behavior = RCDEVICE_PROTOCOL_CAM_CTRL_SIMULATE_POWER_BTN; + behavior = RCDEVICE_PROTOCOL_CAM_CTRL_SIMULATE_POWER_BTN; } break; case BOXCAMERA3: if (isFeatureSupported(RCDEVICE_PROTOCOL_FEATURE_CHANGE_MODE)) { - behavior = RCDEVICE_PROTOCOL_CAM_CTRL_CHANGE_MODE; + // avoid change camera mode when arming + if (!ARMING_FLAG(ARMED) && ((getArmingDisableFlags() & ARMING_DISABLED_RUNAWAY_TAKEOFF) == 0)) { + behavior = RCDEVICE_PROTOCOL_CAM_CTRL_CHANGE_MODE; + } } break; default: @@ -157,7 +127,60 @@ static void rcdeviceCameraControlProcess(void) } } -static bool rcdeviceCamSimulate5KeyCablePress(rcdeviceCamSimulationKeyEvent_e key) +static void rcdeviceSimulationOSDCableFailed(rcdeviceResponseParseContext_t *ctx) +{ + waitingDeviceResponse = false; + if (ctx->command == RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION) { + uint8_t operationID = ctx->paramData[0]; + if (operationID == RCDEVICE_PROTOCOL_5KEY_CONNECTION_CLOSE) { + return; + } + } +} + +static void rcdeviceSimulationRespHandle(rcdeviceResponseParseContext_t *ctx) +{ + if (ctx->result != RCDEVICE_RESP_SUCCESS) { + rcdeviceSimulationOSDCableFailed(ctx); + waitingDeviceResponse = false; + return; + } + + switch (ctx->command) { + case RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_RELEASE: + isButtonPressed = false; + break; + case RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION: + { + // the high 4 bits is the operationID that we sent + // the low 4 bits is the result code + isButtonPressed = true; + uint8_t operationID = ctx->paramData[0]; + bool errorCode = (ctx->recvBuf[1] & 0x0F); + if (operationID == RCDEVICE_PROTOCOL_5KEY_CONNECTION_OPEN) { + if (errorCode == 1) { + rcdeviceInMenu = true; + beeper(BEEPER_CAM_CONNECTION_OPEN); + } else { + beeper(BEEPER_CAM_CONNECTION_CLOSE); + } + } else if (operationID == RCDEVICE_PROTOCOL_5KEY_CONNECTION_CLOSE) { + if (errorCode == 1) { + rcdeviceInMenu = false; + beeper(BEEPER_CAM_CONNECTION_CLOSE); + } + } + } + break; + case RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_PRESS: + isButtonPressed = true; + break; + } + + waitingDeviceResponse = false; +} + +static void rcdeviceCamSimulate5KeyCablePress(rcdeviceCamSimulationKeyEvent_e key) { uint8_t operation = RCDEVICE_PROTOCOL_5KEY_SIMULATION_NONE; switch (key) { @@ -176,50 +199,38 @@ static bool rcdeviceCamSimulate5KeyCablePress(rcdeviceCamSimulationKeyEvent_e ke case RCDEVICE_CAM_KEY_ENTER: operation = RCDEVICE_PROTOCOL_5KEY_SIMULATION_SET; break; + case RCDEVICE_CAM_KEY_NONE: default: operation = RCDEVICE_PROTOCOL_5KEY_SIMULATION_NONE; break; } - return runcamDeviceSimulate5KeyOSDCableButtonPress(camDevice, operation); + runcamDeviceSimulate5KeyOSDCableButtonPress(camDevice, operation, rcdeviceSimulationRespHandle); } -static bool rcdeviceSend5KeyOSDCableSimualtionEvent(rcdeviceCamSimulationKeyEvent_e key) +void rcdeviceSend5KeyOSDCableSimualtionEvent(rcdeviceCamSimulationKeyEvent_e key) { - bool reqResult = false; switch (key) { case RCDEVICE_CAM_KEY_CONNECTION_OPEN: - reqResult = runcamDeviceOpen5KeyOSDCableConnection(camDevice); - if (reqResult) { - rcdeviceInMenu = true; - beeper(BEEPER_CAM_CONNECTION_OPEN); - } + runcamDeviceOpen5KeyOSDCableConnection(camDevice, rcdeviceSimulationRespHandle); break; - case RCDEVICE_CAM_KEY_CONNECTION_CLOSE: { - uint8_t resultCode = 0; - reqResult = runcamDeviceClose5KeyOSDCableConnection(camDevice, &resultCode); - if (resultCode == 1) { - rcdeviceInMenu = false; - beeper(BEEPER_CAM_CONNECTION_CLOSE); - } - } + case RCDEVICE_CAM_KEY_CONNECTION_CLOSE: + runcamDeviceClose5KeyOSDCableConnection(camDevice, rcdeviceSimulationRespHandle); break; case RCDEVICE_CAM_KEY_ENTER: case RCDEVICE_CAM_KEY_LEFT: case RCDEVICE_CAM_KEY_UP: case RCDEVICE_CAM_KEY_RIGHT: case RCDEVICE_CAM_KEY_DOWN: - reqResult = rcdeviceCamSimulate5KeyCablePress(key); + rcdeviceCamSimulate5KeyCablePress(key); break; case RCDEVICE_CAM_KEY_RELEASE: - reqResult = runcamDeviceSimulate5KeyOSDCableButtonRelease(camDevice); + runcamDeviceSimulate5KeyOSDCableButtonRelease(camDevice, rcdeviceSimulationRespHandle); break; + case RCDEVICE_CAM_KEY_NONE: default: - reqResult = false; break; } - - return reqResult; } static void rcdevice5KeySimulationProcess(timeUs_t currentTimeUs) @@ -236,22 +247,19 @@ static void rcdevice5KeySimulationProcess(timeUs_t currentTimeUs) return; } - rcdeviceCamSimulationKeyEvent_e key = RCDEVICE_CAM_KEY_NONE; - - if (needRelease) { + if (isButtonPressed) { if (IS_MID(YAW) && IS_MID(PITCH) && IS_MID(ROLL)) { - if ((camDevice->serialPort != NULL || reInitializeDevice()) && rcdeviceIs5KeyEnabled()) { - key = RCDEVICE_CAM_KEY_RELEASE; - if (rcdeviceSend5KeyOSDCableSimualtionEvent(key)) { - needRelease = false; - } else { - rcdeviceInMenu = false; - } - } + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_RELEASE); + waitingDeviceResponse = true; } - return; } else { - if (IS_MID(THROTTLE) && IS_MID(ROLL) && IS_MID(PITCH) && IS_LO(YAW)) { // Disconnect HI YAW + if (waitingDeviceResponse) { + return; + } + + rcdeviceCamSimulationKeyEvent_e key = RCDEVICE_CAM_KEY_NONE; + + if (IS_MID(THROTTLE) && IS_MID(ROLL) && IS_MID(PITCH) && IS_LO(YAW)) { // Disconnect Lo YAW if (rcdeviceInMenu) { key = RCDEVICE_CAM_KEY_CONNECTION_CLOSE; } @@ -274,38 +282,35 @@ static void rcdevice5KeySimulationProcess(timeUs_t currentTimeUs) } } } - } - if (key != RCDEVICE_CAM_KEY_NONE) { - if ((camDevice->serialPort != NULL || reInitializeDevice()) && rcdeviceIs5KeyEnabled()) { - if (rcdeviceSend5KeyOSDCableSimualtionEvent(key)) { - needRelease = true; - } else { - rcdeviceInMenu = false; - } + if (key != RCDEVICE_CAM_KEY_NONE) { + rcdeviceSend5KeyOSDCableSimualtionEvent(key); + isButtonPressed = true; + waitingDeviceResponse = true; } } } void rcdeviceUpdate(timeUs_t currentTimeUs) { + rcdeviceReceive(currentTimeUs); + rcdeviceCameraControlProcess(); - rcdevice5KeySimulationProcess(currentTimeUs); + + if (rcdeviceIs5KeyEnabled()) { + rcdevice5KeySimulationProcess(currentTimeUs); + } } -bool rcdeviceInit(void) +void rcdeviceInit(void) { // open serial port - if (!runcamDeviceInit(camDevice)) { - return false; - } + runcamDeviceInit(camDevice); for (boxId_e i = BOXCAMERA1; i <= BOXCAMERA3; i++) { uint8_t switchIndex = i - BOXCAMERA1; switchStates[switchIndex].isActivated = true; } - - return true; } #endif diff --git a/src/main/io/rcdevice_cam.h b/src/main/io/rcdevice_cam.h index 6d36c77642..f8fc924705 100644 --- a/src/main/io/rcdevice_cam.h +++ b/src/main/io/rcdevice_cam.h @@ -39,10 +39,7 @@ typedef struct rcdeviceSwitchState_s { extern runcamDevice_t *camDevice; extern bool rcdeviceInMenu; -bool rcdeviceInit(void); +void rcdeviceInit(void); void rcdeviceUpdate(timeUs_t currentTimeUs); bool rcdeviceIsEnabled(void); - -// used for unit test -rcdeviceSwitchState_t switchStates[BOXCAMERA3 - BOXCAMERA1 + 1]; diff --git a/src/main/io/rcdevice_osd.c b/src/main/io/rcdevice_osd.c deleted file mode 100644 index 252b551ef2..0000000000 --- a/src/main/io/rcdevice_osd.c +++ /dev/null @@ -1,247 +0,0 @@ -/* - * 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 . - */ - -#include -#include - -#include "platform.h" - -#ifdef USE_RCDEVICE - -#include "io/rcdevice.h" - -#include "pg/vcd.h" - -#include "rcdevice_osd.h" - -#define VIDEO_BUFFER_CHARS_PAL 480 - -static uint8_t columnCount = 30; - -static runcamDevice_t runcamOSDDevice; -runcamDevice_t *osdDevice = &runcamOSDDevice; - -static uint8_t video_system; -static uint16_t maxScreenSize = VIDEO_BUFFER_CHARS_PAL; - -#ifdef USE_PARTICLE_DRAW -#define MAX_CHARS2UPDATE 20 -static uint8_t screenBuffer[VIDEO_BUFFER_CHARS_PAL + 40]; // For faster writes - // we use memcpy so we - // need some space to - // don't overwrite - // buffer -static uint8_t shadowBuffer[VIDEO_BUFFER_CHARS_PAL]; -static bool rcdeviceOSDLock = false; -#endif - -bool rcdeviceOSDInit(const vcdProfile_t *vcdProfile) -{ - if (!runcamDeviceInit(osdDevice)) { - return false; - } - - if ((osdDevice->info.features & RCDEVICE_PROTOCOL_FEATURE_DISPLAYP_PORT) == 0) { - return false; - } - - // get screen column count - runcamDeviceSettingDetail_t settingDetail; - if (!runcamDeviceGetSettingDetail(osdDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &settingDetail)) { - return false; - } - - columnCount = settingDetail.value; - - video_system = vcdProfile->video_system; - if (video_system == VIDEO_SYSTEM_AUTO) { - // fetch current video mode from device - runcamDeviceSettingDetail_t settingDetail; - if (!runcamDeviceGetSettingDetail(osdDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_TV_MODE, &settingDetail)) { - return false; - } - video_system = settingDetail.value; - } else { - // set video system - runcamDeviceWriteSettingResponse_t response; - uint8_t tvMode = 0; - if (video_system == VIDEO_SYSTEM_NTSC) { - tvMode = 0; - } else if (video_system == VIDEO_SYSTEM_PAL) { - tvMode = 1; - } - - if (!runcamDeviceWriteSetting(osdDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_TV_MODE, &tvMode, sizeof(uint8_t), &response)) { - return false; - } - - if (response.resultCode) { - return false; - } - } - - // user bf charset - uint8_t charsetID = 0; - runcamDeviceWriteSettingResponse_t updateCharsetResp; - if (!runcamDeviceWriteSetting(osdDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_CHARSET, &charsetID, sizeof(uint8_t), &updateCharsetResp) || updateCharsetResp.resultCode != 0) { - return false; - } - -#ifdef USE_PARTICLE_DRAW - memset(shadowBuffer, 2, VIDEO_BUFFER_CHARS_PAL); -#endif - - // fill screen with ' ' - rcdeviceOSDClearScreen(NULL); - return true; -} - -int rcdeviceOSDGrab(displayPort_t *displayPort) -{ - UNUSED(displayPort); - // osdResetAlarms(); - // resumeRefreshAt = 0; - return 0; -} - -int rcdeviceOSDRelease(displayPort_t *displayPort) -{ - UNUSED(displayPort); - return 0; -} - -int rcdeviceOSDDrawScreen(displayPort_t *displayPort) -{ - UNUSED(displayPort); - -#ifdef USE_PARTICLE_DRAW - static uint16_t pos = 0; - int k = 0; - - if (!rcdeviceOSDLock) { - rcdeviceOSDLock = true; - - uint8_t data[60]; - uint8_t dataLen = 0; - for (k = 0; k < MAX_CHARS2UPDATE; k++) { - if (screenBuffer[pos] != shadowBuffer[pos]) { - shadowBuffer[pos] = screenBuffer[pos]; - uint8_t x = pos % columnCount; - uint8_t y = pos / columnCount; - data[dataLen++] = x; - data[dataLen++] = y; - data[dataLen++] = screenBuffer[pos]; - } - - if (++pos >= maxScreenSize) { - pos = 0; - break; - } - } - runcamDeviceDispWriteChars(osdDevice, data, dataLen); - - rcdeviceOSDLock = false; - } -#endif - return 0; -} - -int rcdeviceOSDWriteString(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *buff) -{ - UNUSED(displayPort); -#if !defined(USE_PARTICLE_DRAW) - runcamDeviceDispWriteHorizontalString(osdDevice, x, y, buff); -#else - for (int xpos = x; *buff && xpos < columnCount; xpos++) { - screenBuffer[y * columnCount + xpos] = *buff++; - } -#endif - return 0; -} - -int rcdeviceOSDWriteChar(displayPort_t *displayPort, uint8_t x, uint8_t y, uint8_t c) -{ - UNUSED(displayPort); -#if !defined(USE_PARTICLE_DRAW) - runcamDeviceDispWriteChar(osdDevice, x, y, c); -#else - screenBuffer[y * columnCount + x] = c; -#endif - - return 0; -} - -int rcdeviceOSDClearScreen(displayPort_t *displayPort) -{ - UNUSED(displayPort); - -#if defined(USE_PARTICLE_DRAW) - memset(screenBuffer, 0x20, sizeof(screenBuffer)); -#else - runcamDeviceDispFillRegion(osdDevice, 0, 0, 255, 255, ' '); -#endif - - return 0; -} - -bool rcdeviceOSDIsTransferInProgress(const displayPort_t *displayPort) -{ - UNUSED(displayPort); - return false; -} - -bool rcdeviceOSDIsSynced(const displayPort_t *displayPort) -{ - UNUSED(displayPort); - return true; -} - -int rcdeviceOSDHeartbeat(displayPort_t *displayPort) -{ - UNUSED(displayPort); - return 0; -} - -void rcdeviceOSDResync(displayPort_t *displayPort) -{ - UNUSED(displayPort); - - if (video_system == VIDEO_SYSTEM_PAL) { - displayPort->rows = RCDEVICE_PROTOCOL_OSD_VIDEO_LINES_PAL; - } else { - displayPort->rows = RCDEVICE_PROTOCOL_OSD_VIDEO_LINES_NTSC; - } - - displayPort->cols = columnCount; - maxScreenSize = displayPort->rows * displayPort->cols; -} - -uint32_t rcdeviceOSDTxBytesFree(const displayPort_t *displayPort) -{ - UNUSED(displayPort); - return INT32_MAX; -} - -int rcdeviceScreenSize(const displayPort_t *displayPort) -{ - return displayPort->rows * displayPort->cols; -} - -#endif diff --git a/src/main/io/rcdevice_osd.h b/src/main/io/rcdevice_osd.h deleted file mode 100644 index 3199581c0d..0000000000 --- a/src/main/io/rcdevice_osd.h +++ /dev/null @@ -1,43 +0,0 @@ -/* - * 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 . - */ - -#include "drivers/display.h" - -#define RCDEVICE_PROTOCOL_OSD_VIDEO_LINES_NTSC 13 -#define RCDEVICE_PROTOCOL_OSD_VIDEO_LINES_PAL 16 - -struct vcdProfile_s; - -bool rcdeviceOSDInit(const struct vcdProfile_s *vcdProfile); -int rcdeviceOSDGrab(displayPort_t *displayPort); -int rcdeviceOSDRelease(displayPort_t *displayPort); - -int rcdeviceOSDDrawScreen(displayPort_t *); - -int rcdeviceOSDWriteString(displayPort_t *, uint8_t x, uint8_t y, const char *buff); -int rcdeviceOSDWriteChar(displayPort_t *, uint8_t x, uint8_t y, uint8_t c); -int rcdeviceOSDReloadProfile(displayPort_t *); -int rcdeviceOSDClearScreen(displayPort_t *); -bool rcdeviceOSDIsTransferInProgress(const displayPort_t *); -int rcdeviceOSDHeartbeat(displayPort_t *displayPort); -void rcdeviceOSDResync(displayPort_t *displayPort); -bool rcdeviceOSDIsSynced(const displayPort_t *displayPort); -uint32_t rcdeviceOSDTxBytesFree(const displayPort_t *displayPort); -int rcdeviceScreenSize(const displayPort_t *displayPort); diff --git a/src/main/io/serial.c b/src/main/io/serial.c index f0612f783c..c5379e6c72 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -505,7 +505,7 @@ void waitForSerialPortToFinishTransmitting(serialPort_t *serialPort) }; } -#if defined(USE_GPS) || ! defined(SKIP_SERIAL_PASSTHROUGH) +#if defined(USE_GPS) || defined(USE_SERIAL_PASSTHROUGH) // Default data consumer for serialPassThrough. static void nopConsumer(uint8_t data) { diff --git a/src/main/io/usb_cdc_hid.c b/src/main/io/usb_cdc_hid.c new file mode 100644 index 0000000000..48b6edf156 --- /dev/null +++ b/src/main/io/usb_cdc_hid.c @@ -0,0 +1,81 @@ +/* + * 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 . + */ + +#include "platform.h" + +#ifdef USE_USB_CDC_HID + +#include "common/maths.h" + +#include "fc/rc_controls.h" + +#include "rx/rx.h" + +//TODO: Make it platform independent in the future +#if defined(STM32F4) +#include "vcpf4/usbd_cdc_vcp.h" + +#include "usbd_hid_core.h" +#elif defined(STM32F7) +#include "drivers/serial_usb_vcp.h" + +#include "vcp_hal/usbd_cdc_interface.h" + +#include "usbd_hid.h" +#endif + +#define USB_CDC_HID_NUM_AXES 8 + +#define USB_CDC_HID_RANGE_MIN -127 +#define USB_CDC_HID_RANGE_MAX 127 + +// In the windows joystick driver, the axes are defined as shown in the second column. + +const uint8_t hidChannelMapping[] = { + ROLL, // X + PITCH, // Y + AUX3, + YAW, // X Rotation + AUX1, // Z Rotation + THROTTLE, // Y Rotation + AUX4, + AUX2, // Wheel +}; + +void sendRcDataToHid(void) +{ + int8_t report[8]; + for (unsigned i = 0; i < USB_CDC_HID_NUM_AXES; i++) { + const uint8_t channel = hidChannelMapping[i]; + report[i] = scaleRange(constrain(rcData[channel], PWM_RANGE_MIN, PWM_RANGE_MAX), PWM_RANGE_MIN, PWM_RANGE_MAX, USB_CDC_HID_RANGE_MIN, USB_CDC_HID_RANGE_MAX); + if (i == 1) { + // For some reason ROLL is inverted in Windows + report[i] = -report[i]; + } + } +#if defined(STM32F4) + USBD_HID_SendReport(&USB_OTG_dev, (uint8_t*)report, sizeof(report)); +#elif defined(STM32F7) + USBD_HID_SendReport(&USBD_Device, (uint8_t*)report, sizeof(report)); +#else +# error "MCU does not support USB HID." +#endif +} +#endif diff --git a/src/main/io/usb_cdc_hid.h b/src/main/io/usb_cdc_hid.h new file mode 100644 index 0000000000..ff0b538862 --- /dev/null +++ b/src/main/io/usb_cdc_hid.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 sendRcDataToHid(void); diff --git a/src/main/io/usb_msc.c b/src/main/io/usb_msc.c new file mode 100644 index 0000000000..adb67e1744 --- /dev/null +++ b/src/main/io/usb_msc.c @@ -0,0 +1,42 @@ +/* + * 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 . + */ + +#include + +#include "platform.h" + +#include "drivers/sdcard.h" + +#include "io/flashfs.h" + +#if defined(USE_USB_MSC) + +bool mscCheckFilesystemReady(void) +{ + return false +#if defined(USE_SDCARD) + || sdcard_isFunctional() +#endif +#if defined(USE_FLASHFS) + || flashfsGetSize() > 0 +#endif + ; +} +#endif diff --git a/src/main/io/usb_msc.h b/src/main/io/usb_msc.h new file mode 100644 index 0000000000..35164c7de5 --- /dev/null +++ b/src/main/io/usb_msc.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 + +bool mscCheckFilesystemReady(void); diff --git a/src/main/io/vtx.c b/src/main/io/vtx.c index 2e9568881c..36ffc2aa82 100644 --- a/src/main/io/vtx.c +++ b/src/main/io/vtx.c @@ -54,7 +54,7 @@ PG_RESET_TEMPLATE(vtxSettingsConfig_t, vtxSettingsConfig, .power = VTX_SETTINGS_DEFAULT_POWER, .freq = VTX_SETTINGS_DEFAULT_FREQ, .pitModeFreq = VTX_SETTINGS_DEFAULT_PITMODE_FREQ, - .lowPowerDisarm = VTX_SETTINGS_DEFAULT_LOW_POWER_DISARM, + .lowPowerDisarm = VTX_LOW_POWER_DISARM_OFF, ); typedef enum { @@ -92,7 +92,7 @@ void vtxInit(void) } } -static vtxSettingsConfig_t vtxGetSettings(void) +STATIC_UNIT_TESTED vtxSettingsConfig_t vtxGetSettings(void) { vtxSettingsConfig_t settings = { .band = vtxSettingsConfig()->band, @@ -104,14 +104,16 @@ static vtxSettingsConfig_t vtxGetSettings(void) }; #if defined(VTX_SETTINGS_FREQCMD) - if (IS_RC_MODE_ACTIVE(BOXVTXPITMODE) && isModeActivationConditionPresent(BOXVTXPITMODE) && settings.pitModeFreq) { + if (IS_RC_MODE_ACTIVE(BOXVTXPITMODE) && settings.pitModeFreq) { settings.band = 0; settings.freq = settings.pitModeFreq; settings.power = VTX_SETTINGS_DEFAULT_POWER; } #endif - if (!ARMING_FLAG(ARMED) && settings.lowPowerDisarm && !failsafeIsActive()) { + if (!ARMING_FLAG(ARMED) && !failsafeIsActive() && + (settings.lowPowerDisarm == VTX_LOW_POWER_DISARM_ALWAYS || + (settings.lowPowerDisarm == VTX_LOW_POWER_DISARM_UNTIL_FIRST_ARM && !ARMING_FLAG(WAS_EVER_ARMED)))) { settings.power = VTX_SETTINGS_DEFAULT_POWER; } @@ -166,13 +168,11 @@ static bool vtxProcessPower(vtxDevice_t *vtxDevice) static bool vtxProcessPitMode(vtxDevice_t *vtxDevice) { - uint8_t pitOnOff; - - bool currPmSwitchState; static bool prevPmSwitchState = false; + uint8_t pitOnOff; if (!ARMING_FLAG(ARMED) && vtxCommonGetPitMode(vtxDevice, &pitOnOff)) { - currPmSwitchState = IS_RC_MODE_ACTIVE(BOXVTXPITMODE); + bool currPmSwitchState = IS_RC_MODE_ACTIVE(BOXVTXPITMODE); if (currPmSwitchState != prevPmSwitchState) { prevPmSwitchState = currPmSwitchState; @@ -183,20 +183,21 @@ static bool vtxProcessPitMode(vtxDevice_t *vtxDevice) return false; } #endif - if (isModeActivationConditionPresent(BOXVTXPITMODE)) { - if (!pitOnOff) { - vtxCommonSetPitMode(vtxDevice, true); - return true; - } + if (!pitOnOff) { + vtxCommonSetPitMode(vtxDevice, true); + + return true; } } else { if (pitOnOff) { vtxCommonSetPitMode(vtxDevice, false); + return true; } } } } + return false; } diff --git a/src/main/io/vtx.h b/src/main/io/vtx.h index 1de44faf3d..47cbd8b0f2 100644 --- a/src/main/io/vtx.h +++ b/src/main/io/vtx.h @@ -26,13 +26,19 @@ #include "common/time.h" #include "pg/pg.h" +typedef enum { + VTX_LOW_POWER_DISARM_OFF = 0, + VTX_LOW_POWER_DISARM_ALWAYS, + VTX_LOW_POWER_DISARM_UNTIL_FIRST_ARM, // Set low power until arming for the first time +} vtxLowerPowerDisarm_e; + typedef struct vtxSettingsConfig_s { uint8_t band; // 1=A, 2=B, 3=E, 4=F(Airwaves/Fatshark), 5=Raceband uint8_t channel; // 1-8 uint8_t power; // 0 = lowest uint16_t freq; // sets freq in MHz if band=0 uint16_t pitModeFreq; // sets out-of-range pitmode frequency - uint8_t lowPowerDisarm; // min power while disarmed + uint8_t lowPowerDisarm; // min power while disarmed, from vtxLowerPowerDisarm_e } vtxSettingsConfig_t; PG_DECLARE(vtxSettingsConfig_t, vtxSettingsConfig); diff --git a/src/main/io/vtx_rtc6705.c b/src/main/io/vtx_rtc6705.c index 5ea31f169f..a2c4c4387b 100644 --- a/src/main/io/vtx_rtc6705.c +++ b/src/main/io/vtx_rtc6705.c @@ -72,7 +72,7 @@ bool vtxRTC6705Init(void) bool vtxRTC6705CanUpdate(void) { #if defined(MAX7456_SPI_INSTANCE) && defined(RTC6705_SPI_INSTANCE) && defined(SPI_SHARED_MAX7456_AND_RTC6705) - if (feature(FEATURE_OSD)) { + if (featureIsEnabled(FEATURE_OSD)) { return !max7456DmaInProgress(); } #endif diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index df424e45b2..07a59e1a5a 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -268,6 +268,12 @@ static void saProcessResponse(uint8_t *buf, int len) { uint8_t resp = buf[0]; + if (IS_RC_MODE_ACTIVE(BOXVTXCONTROLDISABLE)) { + sa_outstanding = SA_CMD_NONE; + + return; + } + if (resp == sa_outstanding) { sa_outstanding = SA_CMD_NONE; } else if ((resp == SA_CMD_GET_SETTINGS_V2) && (sa_outstanding == SA_CMD_GET_SETTINGS)) { @@ -355,7 +361,7 @@ static void saProcessResponse(uint8_t *buf, int len) // Datalink // -static void saReceiveFramer(uint8_t c) +static void saReceiveFrame(uint8_t c) { static enum saFramerState_e { @@ -435,21 +441,26 @@ static void saReceiveFramer(uint8_t c) static void saSendFrame(uint8_t *buf, int len) { - switch (smartAudioSerialPort->identifier) { - case SERIAL_PORT_SOFTSERIAL1: - case SERIAL_PORT_SOFTSERIAL2: - break; - default: - serialWrite(smartAudioSerialPort, 0x00); // Generate 1st start bit - break; - } + if (!IS_RC_MODE_ACTIVE(BOXVTXCONTROLDISABLE)) { + switch (smartAudioSerialPort->identifier) { + case SERIAL_PORT_SOFTSERIAL1: + case SERIAL_PORT_SOFTSERIAL2: + break; + default: + serialWrite(smartAudioSerialPort, 0x00); // Generate 1st start bit + break; + } - for (int i = 0 ; i < len ; i++) { - serialWrite(smartAudioSerialPort, buf[i]); + for (int i = 0 ; i < len ; i++) { + serialWrite(smartAudioSerialPort, buf[i]); + } + + saStat.pktsent++; + } else { + sa_outstanding = SA_CMD_NONE; } sa_lastTransmissionMs = millis(); - saStat.pktsent++; } /* @@ -525,7 +536,7 @@ static void saQueueCmd(uint8_t *buf, int len) static void saSendQueue(void) { if (saQueueEmpty()) { - return; + return; } saSendCmd(sa_queue[sa_qtail].buf, sa_queue[sa_qtail].len); @@ -703,7 +714,7 @@ static void vtxSAProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs) while (serialRxBytesWaiting(smartAudioSerialPort) > 0) { uint8_t c = serialRead(smartAudioSerialPort); - saReceiveFramer((uint16_t)c); + saReceiveFrame((uint16_t)c); } // Re-evaluate baudrate after each frame reception diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index 2c6086b063..591bdd4b89 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -120,9 +120,9 @@ static uint8_t trampChecksum(uint8_t *trampBuf) return cksum; } -void trampCmdU16(uint8_t cmd, uint16_t param) +static void trampCmdU16(uint8_t cmd, uint16_t param) { - if (!trampSerialPort) { + if (!trampSerialPort || IS_RC_MODE_ACTIVE(BOXVTXCONTROLDISABLE)) { return; } @@ -217,7 +217,7 @@ void trampSetPitMode(uint8_t onoff) } // returns completed response code -char trampHandleResponse(void) +static char trampHandleResponse(void) { const uint8_t respCode = trampRespBuffer[1]; @@ -333,7 +333,7 @@ static char trampReceive(uint32_t currentTimeUs) trampResetReceiver(); - if ((trampRespBuffer[14] == cksum) && (trampRespBuffer[15] == 0)) { + if ((trampRespBuffer[14] == cksum) && (trampRespBuffer[15] == 0) && !IS_RC_MODE_ACTIVE(BOXVTXCONTROLDISABLE)) { return trampHandleResponse(); } } diff --git a/src/main/main.c b/src/main/main.c index 9c24fe197c..2d93e83afd 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -23,7 +23,7 @@ #include "platform.h" -#include "fc/fc_init.h" +#include "fc/init.h" #include "scheduler/scheduler.h" diff --git a/src/main/osd_slave/osd_slave_init.c b/src/main/osd_slave/osd_slave_init.c deleted file mode 100644 index 3c061a99d2..0000000000 --- a/src/main/osd_slave/osd_slave_init.c +++ /dev/null @@ -1,308 +0,0 @@ -/* - * 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 . - */ - -#include -#include -#include - -#include "platform.h" - -#include "blackbox/blackbox.h" - -#include "common/axis.h" -#include "common/color.h" -#include "common/maths.h" -#include "common/printf.h" - -#include "config/config_eeprom.h" -#include "config/feature.h" - -#include "drivers/adc.h" -#include "drivers/bus.h" -#include "drivers/bus_i2c.h" -#include "drivers/bus_spi.h" -#include "drivers/dma.h" -#include "drivers/exti.h" -#include "drivers/inverter.h" -#include "drivers/io.h" -#include "drivers/light_led.h" -#include "drivers/nvic.h" -#include "drivers/sensor.h" -#include "drivers/serial.h" -#include "drivers/serial_softserial.h" -#include "drivers/serial_uart.h" -#include "drivers/sound_beeper.h" -#include "drivers/system.h" -#include "drivers/time.h" -#include "drivers/timer.h" -#include "drivers/transponder_ir.h" -#include "drivers/usb_io.h" - -#include "fc/config.h" -#include "fc/rc_controls.h" -#include "fc/fc_tasks.h" -#include "fc/runtime_config.h" - -#include "interface/cli.h" -#include "interface/msp.h" - -#include "msp/msp_serial.h" - -#include "io/beeper.h" -#include "io/displayport_max7456.h" -#include "io/flashfs.h" -#include "io/ledstrip.h" -#include "io/osd_slave.h" -#include "io/serial.h" -#include "io/transponder_ir.h" - -#include "osd_slave/osd_slave_init.h" - -#include "pg/adc.h" -#include "pg/bus_i2c.h" -#include "pg/bus_spi.h" -#include "pg/pg.h" -#include "pg/pg_ids.h" -#include "pg/vcd.h" - -#include "scheduler/scheduler.h" - -#include "sensors/acceleration.h" -#include "sensors/barometer.h" -#include "sensors/battery.h" - -#ifdef USE_HARDWARE_REVISION_DETECTION -#include "hardware_revision.h" -#endif - -#include "build/build_config.h" -#include "build/debug.h" - -#ifdef TARGET_PREINIT -void targetPreInit(void); -#endif - -uint8_t systemState = SYSTEM_STATE_INITIALISING; - -void processLoopback(void) -{ -} - - -#ifdef BUS_SWITCH_PIN -void busSwitchInit(void) -{ -static IO_t busSwitchResetPin = IO_NONE; - - busSwitchResetPin = IOGetByTag(IO_TAG(BUS_SWITCH_PIN)); - IOInit(busSwitchResetPin, OWNER_SYSTEM, 0); - IOConfigGPIO(busSwitchResetPin, IOCFG_OUT_PP); - - // ENABLE - IOLo(busSwitchResetPin); -} -#endif - -void init(void) -{ -#ifdef USE_HAL_DRIVER - HAL_Init(); -#endif - - printfSupportInit(); - - systemInit(); - - // initialize IO (needed for all IO operations) - IOInitGlobal(); - -#ifdef USE_HARDWARE_REVISION_DETECTION - detectHardwareRevision(); -#endif - - initEEPROM(); - - ensureEEPROMStructureIsValid(); - readEEPROM(); - - systemState |= SYSTEM_STATE_CONFIG_LOADED; - - debugMode = systemConfig()->debug_mode; - - // Latch active features to be used for feature() in the remainder of init(). - latchActiveFeatures(); - -#ifdef TARGET_PREINIT - targetPreInit(); -#endif - - ledInit(statusLedConfig()); - LED2_ON; - -#ifdef USE_EXTI - EXTIInit(); -#endif - - delay(100); - - timerInit(); // timer must be initialized before any channel is allocated - -#ifdef BUS_SWITCH_PIN - busSwitchInit(); -#endif - -#if defined(USE_UART) && !defined(SIMULATOR_BUILD) - uartPinConfigure(serialPinConfig()); -#endif - - serialInit(false, SERIAL_PORT_NONE); - -#ifdef USE_BEEPER - beeperInit(beeperDevConfig()); -#endif -/* temp until PGs are implemented. */ -#ifdef USE_INVERTER - initInverters(); -#endif - -#ifdef TARGET_BUS_INIT - targetBusInit(); -#else - -#ifdef USE_SPI - spiPinConfigure(spiPinConfig(0)); - - // Initialize CS lines and keep them high - spiPreInit(); - -#ifdef USE_SPI_DEVICE_1 - spiInit(SPIDEV_1); -#endif -#ifdef USE_SPI_DEVICE_2 - spiInit(SPIDEV_2); -#endif -#ifdef USE_SPI_DEVICE_3 - spiInit(SPIDEV_3); -#endif -#ifdef USE_SPI_DEVICE_4 - spiInit(SPIDEV_4); -#endif -#endif /* USE_SPI */ - -#ifdef USE_I2C - i2cHardwareConfigure(i2cConfig(0)); - - // Note: Unlike UARTs which are configured when client is present, - // I2C buses are initialized unconditionally if they are configured. - -#ifdef USE_I2C_DEVICE_1 - i2cInit(I2CDEV_1); -#endif -#ifdef USE_I2C_DEVICE_2 - i2cInit(I2CDEV_2); -#endif -#ifdef USE_I2C_DEVICE_3 - i2cInit(I2CDEV_3); -#endif -#ifdef USE_I2C_DEVICE_4 - i2cInit(I2CDEV_4); -#endif -#endif /* USE_I2C */ - -#endif /* TARGET_BUS_INIT */ - -#ifdef USE_HARDWARE_REVISION_DETECTION - updateHardwareRevision(); -#endif - -#ifdef USE_ADC - adcConfigMutable()->vbat.enabled = (batteryConfig()->voltageMeterSource == VOLTAGE_METER_ADC); - adcConfigMutable()->current.enabled = (batteryConfig()->currentMeterSource == CURRENT_METER_ADC); - - adcConfigMutable()->rssi.enabled = feature(FEATURE_RSSI_ADC); - adcInit(adcConfig()); -#endif - - LED1_ON; - LED0_OFF; - LED2_OFF; - - for (int i = 0; i < 10; i++) { - LED1_TOGGLE; - LED0_TOGGLE; -#if defined(USE_BEEPER) - delay(25); - if (!(beeperConfig()->beeper_off_flags & BEEPER_GET_FLAG(BEEPER_SYSTEM_INIT))) BEEP_ON; - delay(25); - BEEP_OFF; -#else - delay(50); -#endif - } - LED0_OFF; - LED1_OFF; - - mspInit(); - mspSerialInit(); - -#ifdef USE_CLI - cliInit(serialConfig()); -#endif - - displayPort_t *osdDisplayPort = NULL; - -#if defined(USE_MAX7456) - // If there is a max7456 chip for the OSD then use it - osdDisplayPort = max7456DisplayPortInit(vcdProfile()); - // osdInit will register with CMS by itself. - osdSlaveInit(osdDisplayPort); -#endif - -#ifdef USE_LED_STRIP - ledStripInit(); - - if (feature(FEATURE_LED_STRIP)) { - ledStripEnable(); - } -#endif - -#ifdef USE_USB_DETECT - usbCableDetectInit(); -#endif - -#ifdef USE_TRANSPONDER - if (feature(FEATURE_TRANSPONDER)) { - transponderInit(); - transponderStartRepeating(); - systemState |= SYSTEM_STATE_TRANSPONDER_ENABLED; - } -#endif - - timerStart(); - - batteryInit(); - - // Latch active features AGAIN since some may be modified by init(). - latchActiveFeatures(); - - fcTasksInit(); - - systemState |= SYSTEM_STATE_READY; -} diff --git a/src/main/pg/bus_spi.c b/src/main/pg/bus_spi.c index 65360274d0..be31aa8106 100644 --- a/src/main/pg/bus_spi.c +++ b/src/main/pg/bus_spi.c @@ -77,24 +77,6 @@ ioTag_t preinitIPUList[SPI_PREINIT_IPU_COUNT] = { #ifdef GYRO_2_CS_PIN IO_TAG(GYRO_2_CS_PIN), #endif -#ifdef MPU6000_CS_PIN - IO_TAG(MPU6000_CS_PIN), -#endif -#ifdef MPU6500_CS_PIN - IO_TAG(MPU6500_CS_PIN), -#endif -#ifdef MPU9250_CS_PIN - IO_TAG(MPU9250_CS_PIN), -#endif -#ifdef ICM20649_CS_PIN - IO_TAG(ICM20649_CS_PIN), -#endif -#ifdef ICM20689_CS_PIN - IO_TAG(ICM20689_CS_PIN), -#endif -#ifdef BMI160_CS_PIN - IO_TAG(BMI160_CS_PIN), -#endif #ifdef L3GD20_CS_PIN IO_TAG(L3GD20_CS_PIN), #endif diff --git a/src/main/pg/flash.c b/src/main/pg/flash.c index 7c2632d8b1..1ca604386f 100644 --- a/src/main/pg/flash.c +++ b/src/main/pg/flash.c @@ -37,11 +37,7 @@ PG_REGISTER_WITH_RESET_FN(flashConfig_t, flashConfig, PG_FLASH_CONFIG, 0); void pgResetFn_flashConfig(flashConfig_t *flashConfig) { -#ifdef FLASH_CS_PIN flashConfig->csTag = IO_TAG(FLASH_CS_PIN); -#else - flashConfig->csTag = IO_TAG_NONE; -#endif flashConfig->spiDevice = SPI_DEV_TO_CFG(spiDeviceByInstance(FLASH_SPI_INSTANCE)); } #endif diff --git a/src/main/pg/gyrodev.c b/src/main/pg/gyrodev.c new file mode 100644 index 0000000000..d9a346f159 --- /dev/null +++ b/src/main/pg/gyrodev.c @@ -0,0 +1,91 @@ +/* + * 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 . + */ + +#include +#include + +#include "platform.h" + +#include "pg/pg.h" +#include "pg/pg_ids.h" +#include "pg/gyrodev.h" + +#include "drivers/io.h" +#include "drivers/bus_spi.h" +#include "drivers/sensor.h" +#include "sensors/gyro.h" + + +ioTag_t selectMPUIntExtiConfigByHardwareRevision(void); // XXX Should be gone + +#if defined(USE_SPI_GYRO) || defined(USE_I2C_GYRO) +static void gyroResetCommonDeviceConfig(gyroDeviceConfig_t *devconf, ioTag_t extiTag, uint8_t align) +{ + devconf->extiTag = extiTag; + devconf->align = align; +} +#endif + +#ifdef USE_SPI_GYRO +static void gyroResetSpiDeviceConfig(gyroDeviceConfig_t *devconf, SPI_TypeDef *instance, ioTag_t csnTag, ioTag_t extiTag, uint8_t align) +{ + devconf->bustype = BUSTYPE_SPI; + devconf->spiBus = SPI_DEV_TO_CFG(spiDeviceByInstance(instance)); + devconf->csnTag = csnTag; + gyroResetCommonDeviceConfig(devconf, extiTag, align); +} +#endif + +#ifdef USE_I2C_GYRO +static void gyroResetI2cDeviceConfig(gyroDeviceConfig_t *devconf, I2CDevice i2cbus, ioTag_t extiTag, uint8_t align) +{ + devconf->bustype = BUSTYPE_I2C; + devconf->i2cBus = I2C_DEV_TO_CFG(i2cbus); + devconf->i2cAddress = GYRO_I2C_ADDRESS; + gyroResetCommonDeviceConfig(devconf, extiTag, align); +} +#endif + +PG_REGISTER_ARRAY_WITH_RESET_FN(gyroDeviceConfig_t, MAX_GYRODEV_COUNT, gyroDeviceConfig, PG_GYRO_DEVICE_CONFIG, 0); + +void pgResetFn_gyroDeviceConfig(gyroDeviceConfig_t *devconf) +{ + devconf[0].index = 0; + + // All multi-gyro boards use SPI based gyros. +#ifdef USE_SPI_GYRO + gyroResetSpiDeviceConfig(&devconf[0], GYRO_1_SPI_INSTANCE, IO_TAG(GYRO_1_CS_PIN), IO_TAG(GYRO_1_EXTI_PIN), GYRO_1_ALIGN); +#ifdef USE_MULTI_GYRO + gyroResetSpiDeviceConfig(&devconf[1], GYRO_2_SPI_INSTANCE, IO_TAG(GYRO_2_CS_PIN), IO_TAG(GYRO_2_EXTI_PIN), GYRO_2_ALIGN); + devconf[1].index = 1; +#endif +#endif + + // I2C gyros appear as a sole gyro in single gyro boards. +#if defined(USE_I2C_GYRO) && !defined(USE_MULTI_GYRO) + devconf[0].i2cBus = I2C_DEV_TO_CFG(I2CINVALID); // XXX Not required? + gyroResetI2cDeviceConfig(&devconf[0], I2C_DEVICE, IO_TAG(GYRO_1_EXTI_PIN), GYRO_1_ALIGN); +#endif + +// Special treatment for very rare F3 targets with variants having either I2C or SPI acc/gyro chip; mark it for run time detection. +#if defined(USE_SPI_GYRO) && defined(USE_I2C_GYRO) + devconf[0].bustype = BUSTYPE_GYRO_AUTO; +#endif +} diff --git a/src/main/pg/gyrodev.h b/src/main/pg/gyrodev.h new file mode 100644 index 0000000000..fe3d2dbf1c --- /dev/null +++ b/src/main/pg/gyrodev.h @@ -0,0 +1,40 @@ +/* + * 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 + + +#include + +#include "pg/pg.h" +#include "drivers/io_types.h" + +typedef struct gyroDeviceConfig_s { + int8_t index; + uint8_t bustype; + uint8_t spiBus; + ioTag_t csnTag; + uint8_t i2cBus; + uint8_t i2cAddress; + ioTag_t extiTag; + uint8_t align; // sensor_align_e +} gyroDeviceConfig_t; + +PG_DECLARE_ARRAY(gyroDeviceConfig_t, MAX_GYRODEV_COUNT, gyroDeviceConfig); diff --git a/src/main/pg/pg.h b/src/main/pg/pg.h index a0eb315c1c..cab5724bd2 100644 --- a/src/main/pg/pg.h +++ b/src/main/pg/pg.h @@ -174,6 +174,8 @@ extern const uint8_t __pg_resetdata_end[]; /**/ #endif +#define PG_ARRAY_ELEMENT_OFFSET(type, index, member) (index * sizeof(type) + offsetof(type, member)) + // Emit reset defaults for config. // Config must be registered with PG_REGISTER__WITH_RESET_TEMPLATE macro #define PG_RESET_TEMPLATE(_type, _name, ...) \ diff --git a/src/main/pg/pg_ids.h b/src/main/pg/pg_ids.h index 275b0a8e69..d174482294 100644 --- a/src/main/pg/pg_ids.h +++ b/src/main/pg/pg_ids.h @@ -133,7 +133,9 @@ #define PG_SPI_PREINIT_OPU_CONFIG 536 #define PG_RX_SPI_CONFIG 537 #define PG_BOARD_CONFIG 538 -#define PG_BETAFLIGHT_END 538 +#define PG_RCDEVICE_CONFIG 539 +#define PG_GYRO_DEVICE_CONFIG 540 +#define PG_BETAFLIGHT_END 540 // OSD configuration (subject to change) diff --git a/src/main/pg/rcdevice.c b/src/main/pg/rcdevice.c new file mode 100644 index 0000000000..4dd60507e7 --- /dev/null +++ b/src/main/pg/rcdevice.c @@ -0,0 +1,30 @@ +/* + * 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 . + */ + +#include "pg/pg_ids.h" +#include "pg/rcdevice.h" + +PG_REGISTER_WITH_RESET_FN(rcdeviceConfig_t, rcdeviceConfig, PG_RCDEVICE_CONFIG, 0); + +void pgResetFn_rcdeviceConfig(rcdeviceConfig_t *rcdeviceConfig) +{ + rcdeviceConfig->initDeviceAttempts = 6; + rcdeviceConfig->initDeviceAttemptInterval = 1000; +} \ No newline at end of file diff --git a/src/main/osd_slave/osd_slave_init.h b/src/main/pg/rcdevice.h similarity index 75% rename from src/main/osd_slave/osd_slave_init.h rename to src/main/pg/rcdevice.h index 444bc89a4d..5b7a67d971 100644 --- a/src/main/osd_slave/osd_slave_init.h +++ b/src/main/pg/rcdevice.h @@ -20,13 +20,12 @@ #pragma once -typedef enum { - SYSTEM_STATE_INITIALISING = 0, - SYSTEM_STATE_CONFIG_LOADED = (1 << 0), - SYSTEM_STATE_TRANSPONDER_ENABLED = (1 << 3), - SYSTEM_STATE_READY = (1 << 7) -} systemState_e; +#include "pg/pg.h" +#include "common/time.h" -extern uint8_t systemState; +typedef struct rcdeviceConfig_s { + uint8_t initDeviceAttempts; + timeMs_t initDeviceAttemptInterval; +} rcdeviceConfig_t; -void init(void); +PG_DECLARE(rcdeviceConfig_t, rcdeviceConfig); \ No newline at end of file diff --git a/src/main/pg/rx.c b/src/main/pg/rx.c index 2bef6d3d15..e106da5f09 100644 --- a/src/main/pg/rx.c +++ b/src/main/pg/rx.c @@ -29,7 +29,7 @@ #include "config/config_reset.h" #include "drivers/io.h" -#include "fc/fc_rc.h" +#include "fc/rc.h" #include "fc/rc_controls.h" #include "rx/rx.h" #include "rx/rx_spi.h" @@ -56,12 +56,12 @@ void pgResetFn_rxConfig(rxConfig_t *rxConfig) .rssi_offset = 0, .rssi_invert = 0, .rcInterpolation = RC_SMOOTHING_AUTO, - .rcInterpolationChannels = INTERPOLATION_CHANNELS_RPT, + .rcInterpolationChannels = INTERPOLATION_CHANNELS_RPYT, .rcInterpolationInterval = 19, .fpvCamAngleDegrees = 0, .airModeActivateThreshold = 32, .max_aux_channel = DEFAULT_AUX_CHANNEL_COUNT, - .rc_smoothing_type = RC_SMOOTHING_TYPE_INTERPOLATION, + .rc_smoothing_type = RC_SMOOTHING_TYPE_FILTER, .rc_smoothing_input_cutoff = 0, // automatically calculate the cutoff by default .rc_smoothing_derivative_cutoff = 0, // automatically calculate the cutoff by default .rc_smoothing_debug_axis = ROLL, // default to debug logging for the roll axis diff --git a/src/main/pg/sdcard.c b/src/main/pg/sdcard.c index 07d5d7d291..e46c56bf61 100644 --- a/src/main/pg/sdcard.c +++ b/src/main/pg/sdcard.c @@ -33,37 +33,34 @@ #include "drivers/io.h" #include "drivers/dma.h" -PG_REGISTER_WITH_RESET_FN(sdcardConfig_t, sdcardConfig, PG_SDCARD_CONFIG, 0); +PG_REGISTER_WITH_RESET_FN(sdcardConfig_t, sdcardConfig, PG_SDCARD_CONFIG, 1); void pgResetFn_sdcardConfig(sdcardConfig_t *config) { config->useDma = false; -#ifdef SDCARD_SPI_INSTANCE - config->enabled = 1; - config->device = spiDeviceByInstance(SDCARD_SPI_INSTANCE); -#elif defined(USE_SDCARD_SDIO) - config->enabled = 1; -#else - config->enabled = 0; - config->device = 0; -#endif -#ifdef SDCARD_DETECT_PIN - config->cardDetectTag = IO_TAG(SDCARD_DETECT_PIN); -#else - config->cardDetectTag = IO_TAG_NONE; -#endif -#ifdef SDCARD_SPI_CS_PIN - config->chipSelectTag = IO_TAG(SDCARD_SPI_CS_PIN); -#else - config->chipSelectTag = IO_TAG_NONE; + config->device = SPI_DEV_TO_CFG(SPIINVALID); + config->mode = SDCARD_MODE_NONE; + + // We can safely handle SPI and SDIO cases separately on custom targets, as these are exclusive per target. + // On generic targets, SPI has precedence over SDIO; SDIO must be post-flash configured. + +#ifdef USE_SDCARD_SDIO + config->mode = SDCARD_MODE_SDIO; #endif -#ifdef SDCARD_DETECT_INVERTED - config->cardDetectInverted = 1; -#else - config->cardDetectInverted = 0; +#ifdef USE_SDCARD_SPI + SPIDevice spidevice = spiDeviceByInstance(SDCARD_SPI_INSTANCE); + config->device = SPI_DEV_TO_CFG(spidevice); + config->chipSelectTag = IO_TAG(SDCARD_SPI_CS_PIN); + + if (spidevice != SPIINVALID && config->chipSelectTag) { + config->mode = SDCARD_MODE_SPI; + } #endif + config->cardDetectTag = IO_TAG(SDCARD_DETECT_PIN); + config->cardDetectInverted = SDCARD_DETECT_IS_INVERTED; + #if defined(SDCARD_DMA_STREAM_TX_FULL) config->dmaIdentifier = (uint8_t)dmaGetIdentifier(SDCARD_DMA_STREAM_TX_FULL); #elif defined(SDCARD_DMA_CHANNEL_TX) diff --git a/src/main/pg/sdcard.h b/src/main/pg/sdcard.h index 338d03c41d..bf3b43a5df 100644 --- a/src/main/pg/sdcard.h +++ b/src/main/pg/sdcard.h @@ -23,15 +23,21 @@ #include "pg/pg.h" #include "drivers/io.h" +typedef enum { + SDCARD_MODE_NONE = 0, + SDCARD_MODE_SPI, + SDCARD_MODE_SDIO +} sdcardMode_e; + typedef struct sdcardConfig_s { uint8_t useDma; - uint8_t enabled; - uint8_t device; + int8_t device; ioTag_t cardDetectTag; ioTag_t chipSelectTag; uint8_t cardDetectInverted; uint8_t dmaIdentifier; uint8_t dmaChannel; + sdcardMode_e mode; } sdcardConfig_t; PG_DECLARE(sdcardConfig_t, sdcardConfig); diff --git a/src/main/pg/timerio.h b/src/main/pg/timerio.h index 859714de4a..bf18f13065 100644 --- a/src/main/pg/timerio.h +++ b/src/main/pg/timerio.h @@ -27,8 +27,6 @@ #ifdef USE_TIMER_MGMT -#define MAX_TIMER_PINMAP_COUNT 10 - typedef struct timerIOConfig_s { ioTag_t ioTag; uint8_t index; @@ -36,4 +34,4 @@ typedef struct timerIOConfig_s { PG_DECLARE_ARRAY(timerIOConfig_t, MAX_TIMER_PINMAP_COUNT, timerIOConfig); -#endif \ No newline at end of file +#endif diff --git a/src/main/pg/usb.c b/src/main/pg/usb.c index 3eddf59e76..c13853a7ab 100644 --- a/src/main/pg/usb.c +++ b/src/main/pg/usb.c @@ -20,28 +20,20 @@ #include "platform.h" -#if defined(USE_USB_ADVANCED_PROFILES) +#ifdef USE_VCP + #include "drivers/io.h" #include "pg/pg_ids.h" #include "usb.h" -#if !defined(USB_MSC_BUTTON_PIN) -#define USB_MSC_BUTTON_PIN NONE -#endif - -#if defined(USE_USB_MSC_BUTTON_IPU) -#define MSC_BUTTON_IPU true -#else -#define MSC_BUTTON_IPU false -#endif - PG_REGISTER_WITH_RESET_TEMPLATE(usbDev_t, usbDevConfig, PG_USB_CONFIG, 0); PG_RESET_TEMPLATE(usbDev_t, usbDevConfig, .type = DEFAULT, .mscButtonPin = IO_TAG(USB_MSC_BUTTON_PIN), .mscButtonUsePullup = MSC_BUTTON_IPU, + .detectPin = IO_TAG(USB_DETECT_PIN), ); #endif diff --git a/src/main/pg/usb.h b/src/main/pg/usb.h index 538881dc14..8acffd24b5 100644 --- a/src/main/pg/usb.h +++ b/src/main/pg/usb.h @@ -33,6 +33,7 @@ typedef struct usbDev_s { uint8_t type; ioTag_t mscButtonPin; uint8_t mscButtonUsePullup; + ioTag_t detectPin; } usbDev_t; PG_DECLARE(usbDev_t, usbDevConfig); diff --git a/src/main/platform.h b/src/main/platform.h index 000d0b9bcf..d297a8a159 100644 --- a/src/main/platform.h +++ b/src/main/platform.h @@ -26,7 +26,7 @@ #pragma GCC poison sprintf snprintf #endif -#if defined(STM32F745xx) || defined(STM32F746xx) || defined(STM32F722xx) +#if defined(STM32F722xx) || defined(STM32F745xx) || defined(STM32F746xx) || defined(STM32F765xx) #include "stm32f7xx.h" #include "stm32f7xx_hal.h" #include "system_stm32f7xx.h" @@ -106,12 +106,7 @@ #error "Invalid chipset specified. Update platform.h" #endif -#ifdef USE_OSD_SLAVE -#include "target/common_osd_slave.h" +#include "target/common_pre.h" #include "target.h" -#else -#include "target/common_fc_pre.h" -#include "target.h" -#include "target/common_fc_post.h" -#endif +#include "target/common_post.h" #include "target/common_defaults_post.h" diff --git a/src/main/rx/cc2500_common.c b/src/main/rx/cc2500_common.c new file mode 100755 index 0000000000..a0bf3e9cad --- /dev/null +++ b/src/main/rx/cc2500_common.c @@ -0,0 +1,229 @@ +/* + * 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 . + */ + +#include + +#include "platform.h" + +#if defined(USE_RX_FRSKY_SPI) || defined(USE_RX_SFHSS_SPI) + +#include "common/maths.h" + +#include "pg/pg.h" +#include "pg/pg_ids.h" +#include "pg/rx.h" +#include "pg/rx_spi.h" + +#include "drivers/rx/rx_cc2500.h" +#include "drivers/io.h" +#include "drivers/time.h" + +#include "fc/config.h" + +#include "rx/rx.h" +#include "rx/rx_spi.h" + +#include "rx/cc2500_common.h" + +static IO_t gdoPin; +static IO_t bindPin = DEFIO_IO(NONE); +static IO_t cc2500LedPin; +static bool bindRequested; + +static bool lastBindPinStatus; +#if defined(USE_RX_CC2500_SPI_PA_LNA) +static IO_t txEnPin; +static IO_t rxLnaEnPin; +static IO_t antSelPin; +#endif +static int16_t rssiDbm; + +uint16_t cc2500getRssiDbm(void) +{ + return rssiDbm; +} + +void cc2500setRssiDbm(uint8_t value) +{ + if (value >= 128) { + rssiDbm = ((((uint16_t)value) * 18) >> 5) - 82; + } else { + rssiDbm = ((((uint16_t)value) * 18) >> 5) + 65; + } + + setRssi(rssiDbm << 3, RSSI_SOURCE_RX_PROTOCOL); +} + +void cc2500SpiBind(void) +{ + bindRequested = true; +} + +bool cc2500checkBindRequested(bool reset) +{ + if (bindPin) { + bool bindPinStatus = IORead(bindPin); + if (lastBindPinStatus && !bindPinStatus) { + bindRequested = true; + } + lastBindPinStatus = bindPinStatus; + } + + if (!bindRequested) { + return false; + } else { + if (reset) { + bindRequested = false; + } + + return true; + } +} + +bool cc2500getGdo(void) +{ + return IORead(gdoPin); +} + +#if defined(USE_RX_CC2500_SPI_PA_LNA) && defined(USE_RX_CC2500_SPI_DIVERSITY) +void cc2500switchAntennae(void) +{ + static bool alternativeAntennaSelected = true; + + if (alternativeAntennaSelected) { + IOLo(antSelPin); + } else { + IOHi(antSelPin); + } + alternativeAntennaSelected = !alternativeAntennaSelected; +} +#endif +#if defined(USE_RX_CC2500_SPI_PA_LNA) +void cc2500TxEnable(void) +{ + IOHi(txEnPin); +} + +void cc2500TxDisable(void) +{ + IOLo(txEnPin); +} +#endif + +void cc2500LedOn(void) +{ +#if defined(RX_CC2500_SPI_LED_PIN_INVERTED) + IOLo(cc2500LedPin); +#else + IOHi(cc2500LedPin); +#endif +} + +void cc2500LedOff(void) +{ +#if defined(RX_CC2500_SPI_LED_PIN_INVERTED) + IOHi(cc2500LedPin); +#else + IOLo(cc2500LedPin); +#endif +} + +void cc2500LedBlink(timeMs_t blinkms) +{ + static bool ledIsOn = true; + static timeMs_t ledBlinkMs = 0; + + if ( (ledBlinkMs + blinkms) > millis() ) { + return; + } + ledBlinkMs = millis(); + + if (ledIsOn) { + cc2500LedOff(); + } else { + cc2500LedOn(); + } + ledIsOn = !ledIsOn; +} + +static bool cc2500SpiDetect(void) +{ + const uint8_t chipPartNum = cc2500ReadReg(CC2500_30_PARTNUM | CC2500_READ_BURST); //CC2500 read registers chip part num + const uint8_t chipVersion = cc2500ReadReg(CC2500_31_VERSION | CC2500_READ_BURST); //CC2500 read registers chip version + if (chipPartNum == 0x80 && chipVersion == 0x03) { + return true; + } + + return false; +} + +bool cc2500SpiInit(void) +{ +#if !defined(RX_CC2500_SPI_DISABLE_CHIP_DETECTION) + if (!cc2500SpiDetect()) { + return false; + } +#else + UNUSED(cc2500SpiDetect); +#endif + + if (rssiSource == RSSI_SOURCE_NONE) { + rssiSource = RSSI_SOURCE_RX_PROTOCOL; + } + + // gpio init here + gdoPin = IOGetByTag(IO_TAG(RX_CC2500_SPI_GDO_0_PIN)); + IOInit(gdoPin, OWNER_RX_SPI, 0); + IOConfigGPIO(gdoPin, IOCFG_IN_FLOATING); + cc2500LedPin = IOGetByTag(IO_TAG(RX_CC2500_SPI_LED_PIN)); + IOInit(cc2500LedPin, OWNER_LED, 0); + IOConfigGPIO(cc2500LedPin, IOCFG_OUT_PP); +#if defined(USE_RX_CC2500_SPI_PA_LNA) + rxLnaEnPin = IOGetByTag(IO_TAG(RX_CC2500_SPI_LNA_EN_PIN)); + IOInit(rxLnaEnPin, OWNER_RX_SPI, 0); + IOConfigGPIO(rxLnaEnPin, IOCFG_OUT_PP); + IOHi(rxLnaEnPin); // always on at the moment + txEnPin = IOGetByTag(IO_TAG(RX_CC2500_SPI_TX_EN_PIN)); + IOInit(txEnPin, OWNER_RX_SPI, 0); + IOConfigGPIO(txEnPin, IOCFG_OUT_PP); +#if defined(USE_RX_CC2500_SPI_DIVERSITY) + antSelPin = IOGetByTag(IO_TAG(RX_CC2500_SPI_ANT_SEL_PIN)); + IOInit(antSelPin, OWNER_RX_SPI, 0); + IOConfigGPIO(antSelPin, IOCFG_OUT_PP); +#endif +#endif // USE_RX_CC2500_SPI_PA_LNA +#if defined(BINDPLUG_PIN) + bindPin = IOGetByTag(IO_TAG(BINDPLUG_PIN)); + IOInit(bindPin, OWNER_RX_BIND, 0); + IOConfigGPIO(bindPin, IOCFG_IPU); + + lastBindPinStatus = IORead(bindPin); +#endif + +#if defined(USE_RX_CC2500_SPI_PA_LNA) +#if defined(USE_RX_CC2500_SPI_DIVERSITY) + IOHi(antSelPin); +#endif + cc2500TxDisable(); +#endif // USE_RX_CC2500_SPI_PA_LNA + + return true; +} +#endif diff --git a/src/main/io/osd_slave.h b/src/main/rx/cc2500_common.h old mode 100644 new mode 100755 similarity index 59% rename from src/main/io/osd_slave.h rename to src/main/rx/cc2500_common.h index 8c5287b516..f54e41eab1 --- a/src/main/io/osd_slave.h +++ b/src/main/rx/cc2500_common.h @@ -20,26 +20,23 @@ #pragma once -#ifdef USE_OSD_SLAVE -#include "common/time.h" +#include "pg/pg.h" -struct displayPort_s; - -extern bool osdSlaveIsLocked; - -// init -void osdSlaveInit(struct displayPort_s *osdDisplayPort); - -// task api -bool osdSlaveCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs); -void osdSlaveUpdate(timeUs_t currentTimeUs); - -// msp api -void osdSlaveHeartbeat(void); -void osdSlaveClearScreen(void); -void osdSlaveWriteChar(const uint8_t x, const uint8_t y, const uint8_t c); -void osdSlaveWrite(const uint8_t x, const uint8_t y, const char *s); - -void osdSlaveDrawScreen(void); +#include "rx/rx_spi.h" +uint16_t cc2500getRssiDbm(void); +void cc2500setRssiDbm(uint8_t value); +void cc2500SpiBind(void); +bool cc2500checkBindRequested(bool reset); +bool cc2500getGdo(void); +#if defined(USE_RX_CC2500_SPI_PA_LNA) && defined(USE_RX_CC2500_SPI_DIVERSITY) +void cc2500switchAntennae(void); #endif +#if defined(USE_RX_CC2500_SPI_PA_LNA) +void cc2500TxEnable(void); +void cc2500TxDisable(void); +#endif +void cc2500LedOn(void); +void cc2500LedOff(void); +void cc2500LedBlink(timeMs_t blinkms); +bool cc2500SpiInit(void); diff --git a/src/main/rx/cc2500_frsky_common.h b/src/main/rx/cc2500_frsky_common.h index 26d9b98df8..3843c954f0 100644 --- a/src/main/rx/cc2500_frsky_common.h +++ b/src/main/rx/cc2500_frsky_common.h @@ -38,5 +38,3 @@ PG_DECLARE(rxFrSkySpiConfig_t, rxFrSkySpiConfig); bool frSkySpiInit(const rxSpiConfig_t *rxSpiConfig, rxRuntimeConfig_t *rxRuntimeConfig); rx_spi_received_e frSkySpiDataReceived(uint8_t *packet); void frSkySpiSetRcData(uint16_t *rcData, const uint8_t *payload); - -void frSkySpiBind(void); diff --git a/src/main/rx/cc2500_frsky_d.c b/src/main/rx/cc2500_frsky_d.c index 52aaa95e63..e7706ce6f6 100644 --- a/src/main/rx/cc2500_frsky_d.c +++ b/src/main/rx/cc2500_frsky_d.c @@ -46,6 +46,7 @@ #include "fc/config.h" +#include "rx/cc2500_common.h" #include "rx/cc2500_frsky_common.h" #include "rx/cc2500_frsky_shared.h" @@ -123,7 +124,7 @@ static void buildTelemetryFrame(uint8_t *packet) frame[2] = rxFrSkySpiConfig()->bindTxId[1]; frame[3] = a1Value; frame[4] = a2Value; - frame[5] = (uint8_t)rssiDbm; + frame[5] = (uint8_t)cc2500getRssiDbm(); uint8_t bytesUsed = 0; #if defined(USE_TELEMETRY_FRSKY_HUB) if (telemetryEnabled) { @@ -195,7 +196,7 @@ rx_spi_received_e frSkyDHandlePacket(uint8_t * const packet, uint8_t * const pro lastPacketReceivedTime = currentPacketReceivedTime; *protocolState = STATE_DATA; - if (checkBindRequested(false)) { + if (cc2500checkBindRequested(false)) { lastPacketReceivedTime = 0; timeoutUs = 50; missingPackets = 0; @@ -207,7 +208,7 @@ rx_spi_received_e frSkyDHandlePacket(uint8_t * const packet, uint8_t * const pro FALLTHROUGH; //!!TODO -check this fall through is correct // here FS code could be case STATE_DATA: - if (IORead(gdoPin)) { + if (cc2500getGdo()) { uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; if (ccLen >= 20) { cc2500ReadFifo(packet, 20); @@ -217,12 +218,12 @@ rx_spi_received_e frSkyDHandlePacket(uint8_t * const packet, uint8_t * const pro if (packet[0] == 0x11) { if ((packet[1] == rxFrSkySpiConfig()->bindTxId[0]) && (packet[2] == rxFrSkySpiConfig()->bindTxId[1])) { - LedOn(); + cc2500LedOn(); nextChannel(1); + cc2500setRssiDbm(packet[18]); #if defined(USE_RX_FRSKY_SPI_TELEMETRY) if ((packet[3] % 4) == 2) { telemetryTimeUs = micros(); - setRssiDbm(packet[18]); buildTelemetryFrame(packet); *protocolState = STATE_TELEMETRY; } else @@ -240,37 +241,33 @@ rx_spi_received_e frSkyDHandlePacket(uint8_t * const packet, uint8_t * const pro } if (cmpTimeUs(currentPacketReceivedTime, lastPacketReceivedTime) > (timeoutUs * SYNC_DELAY_MAX)) { -#if defined(USE_RX_FRSKY_SPI_PA_LNA) - TxDisable(); +#if defined(USE_RX_CC2500_SPI_PA_LNA) + cc2500TxDisable(); #endif if (timeoutUs == 1) { -#if defined(USE_RX_FRSKY_SPI_PA_LNA) && defined(USE_RX_FRSKY_SPI_DIVERSITY) // SE4311 chip +#if defined(USE_RX_CC2500_SPI_PA_LNA) && defined(USE_RX_CC2500_SPI_DIVERSITY) // SE4311 chip if (missingPackets >= 2) { - switchAntennae(); + cc2500switchAntennae(); } #endif if (missingPackets > MAX_MISSING_PKT) { timeoutUs = 50; -#if defined(USE_RX_FRSKY_SPI_TELEMETRY) setRssiDirect(0, RSSI_SOURCE_RX_PROTOCOL); -#endif } missingPackets++; nextChannel(1); } else { if (ledIsOn) { - LedOff(); + cc2500LedOff(); } else { - LedOn(); + cc2500LedOn(); } ledIsOn = !ledIsOn; -#if defined(USE_RX_FRSKY_SPI_TELEMETRY) setRssi(0, RSSI_SOURCE_RX_PROTOCOL); -#endif nextChannel(13); } @@ -284,8 +281,8 @@ rx_spi_received_e frSkyDHandlePacket(uint8_t * const packet, uint8_t * const pro cc2500Strobe(CC2500_SIDLE); cc2500SetPower(6); cc2500Strobe(CC2500_SFRX); -#if defined(USE_RX_FRSKY_SPI_PA_LNA) - TxEnable(); +#if defined(USE_RX_CC2500_SPI_PA_LNA) + cc2500TxEnable(); #endif cc2500Strobe(CC2500_SIDLE); cc2500WriteFifo(frame, frame[0] + 1); @@ -305,7 +302,7 @@ rx_spi_received_e frSkyDHandlePacket(uint8_t * const packet, uint8_t * const pro void frSkyDInit(void) { #if defined(USE_RX_FRSKY_SPI_TELEMETRY) && defined(USE_TELEMETRY_FRSKY_HUB) - if (feature(FEATURE_TELEMETRY)) { + if (featureIsEnabled(FEATURE_TELEMETRY)) { telemetryEnabled = initFrSkyHubTelemetryExternal(frSkyDTelemetryWriteByte); } #endif diff --git a/src/main/rx/cc2500_frsky_shared.c b/src/main/rx/cc2500_frsky_shared.c index cb59417522..6ba52305c0 100644 --- a/src/main/rx/cc2500_frsky_shared.c +++ b/src/main/rx/cc2500_frsky_shared.c @@ -40,6 +40,7 @@ #include "rx/rx.h" #include "rx/rx_spi.h" +#include "rx/cc2500_common.h" #include "rx/cc2500_frsky_common.h" #include "rx/cc2500_frsky_d.h" #include "rx/cc2500_frsky_x.h" @@ -59,9 +60,6 @@ static timeMs_t timeTunedMs; uint8_t listLength; static uint8_t bindIdx; static int8_t bindOffset; -static bool lastBindPinStatus; - -static bool bindRequested; typedef uint8_t handlePacketFn(uint8_t * const packet, uint8_t * const protocolState); typedef void setRcDataFn(uint16_t *rcData, const uint8_t *payload); @@ -69,20 +67,6 @@ typedef void setRcDataFn(uint16_t *rcData, const uint8_t *payload); static handlePacketFn *handlePacket; static setRcDataFn *setRcData; -IO_t gdoPin; -static IO_t bindPin = DEFIO_IO(NONE); -static IO_t frSkyLedPin; - -#if defined(USE_RX_FRSKY_SPI_PA_LNA) -static IO_t txEnPin; -static IO_t rxLnaEnPin; -static IO_t antSelPin; -#endif - -#ifdef USE_RX_FRSKY_SPI_TELEMETRY -int16_t rssiDbm; -#endif - PG_REGISTER_WITH_RESET_TEMPLATE(rxFrSkySpiConfig_t, rxFrSkySpiConfig, PG_RX_FRSKY_SPI_CONFIG, 1); PG_RESET_TEMPLATE(rxFrSkySpiConfig_t, rxFrSkySpiConfig, @@ -97,52 +81,9 @@ PG_RESET_TEMPLATE(rxFrSkySpiConfig_t, rxFrSkySpiConfig, .useExternalAdc = false, ); -#if defined(USE_RX_FRSKY_SPI_TELEMETRY) -void setRssiDbm(uint8_t value) -{ - if (value >= 128) { - rssiDbm = ((((uint16_t)value) * 18) >> 5) - 82; - } else { - rssiDbm = ((((uint16_t)value) * 18) >> 5) + 65; - } - - setRssi(rssiDbm << 3, RSSI_SOURCE_RX_PROTOCOL); -} -#endif // USE_RX_FRSKY_SPI_TELEMETRY - -#if defined(USE_RX_FRSKY_SPI_PA_LNA) -void TxEnable(void) -{ - IOHi(txEnPin); -} - -void TxDisable(void) -{ - IOLo(txEnPin); -} -#endif - -void LedOn(void) -{ -#if defined(RX_FRSKY_SPI_LED_PIN_INVERTED) - IOLo(frSkyLedPin); -#else - IOHi(frSkyLedPin); -#endif -} - -void LedOff(void) -{ -#if defined(RX_FRSKY_SPI_LED_PIN_INVERTED) - IOHi(frSkyLedPin); -#else - IOLo(frSkyLedPin); -#endif -} - void frSkySpiBind(void) { - bindRequested = true; + cc2500SpiBind(); } static void initialise() { @@ -252,7 +193,7 @@ static bool tuneRx(uint8_t *packet) bindOffset += 5; cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)bindOffset); } - if (IORead(gdoPin)) { + if (cc2500getGdo()) { uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; if (ccLen) { cc2500ReadFifo(packet, ccLen); @@ -292,7 +233,7 @@ static bool getBind1(uint8_t *packet) // len|bind |tx // id|03|01|idx|h0|h1|h2|h3|h4|00|00|00|00|00|00|00|00|00|00|00|00|00|00|00|CHK1|CHK2|RSSI|LQI/CRC| // Start by getting bind packet 0 and the txid - if (IORead(gdoPin)) { + if (cc2500getGdo()) { uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; if (ccLen) { cc2500ReadFifo(packet, ccLen); @@ -321,7 +262,7 @@ static bool getBind1(uint8_t *packet) static bool getBind2(uint8_t *packet) { if (bindIdx <= 120) { - if (IORead(gdoPin)) { + if (cc2500getGdo()) { uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; if (ccLen) { cc2500ReadFifo(packet, ccLen); @@ -369,27 +310,6 @@ static bool getBind2(uint8_t *packet) } } -bool checkBindRequested(bool reset) -{ - if (bindPin) { - bool bindPinStatus = IORead(bindPin); - if (lastBindPinStatus && !bindPinStatus) { - bindRequested = true; - } - lastBindPinStatus = bindPinStatus; - } - - if (!bindRequested) { - return false; - } else { - if (reset) { - bindRequested = false; - } - - return true; - } -} - rx_spi_received_e frSkySpiDataReceived(uint8_t *packet) { rx_spi_received_e ret = RX_SPI_RECEIVED_NONE; @@ -404,8 +324,8 @@ rx_spi_received_e frSkySpiDataReceived(uint8_t *packet) break; case STATE_BIND: - if (checkBindRequested(true) || rxFrSkySpiConfig()->autoBind) { - LedOn(); + if (cc2500checkBindRequested(true) || rxFrSkySpiConfig()->autoBind) { + cc2500LedOn(); initTuneRx(); protocolState = STATE_BIND_TUNING; @@ -443,9 +363,9 @@ rx_spi_received_e frSkySpiDataReceived(uint8_t *packet) } else { uint8_t ctr = 40; while (ctr--) { - LedOn(); + cc2500LedOn(); delay(50); - LedOff(); + cc2500LedOff(); delay(50); } } @@ -489,40 +409,9 @@ void nextChannel(uint8_t skip) } } -#if defined(USE_RX_FRSKY_SPI_PA_LNA) && defined(USE_RX_FRSKY_SPI_DIVERSITY) -void switchAntennae(void) -{ - static bool alternativeAntennaSelected = true; - - if (alternativeAntennaSelected) { - IOLo(antSelPin); - } else { - IOHi(antSelPin); - } - alternativeAntennaSelected = !alternativeAntennaSelected; -} -#endif - -static bool frSkySpiDetect(void) -{ - const uint8_t chipPartNum = cc2500ReadReg(CC2500_30_PARTNUM | CC2500_READ_BURST); //CC2500 read registers chip part num - const uint8_t chipVersion = cc2500ReadReg(CC2500_31_VERSION | CC2500_READ_BURST); //CC2500 read registers chip version - if (chipPartNum == 0x80 && chipVersion == 0x03) { - return true; - } - - return false; -} - bool frSkySpiInit(const rxSpiConfig_t *rxSpiConfig, rxRuntimeConfig_t *rxRuntimeConfig) { -#if !defined(RX_FRSKY_SPI_DISABLE_CHIP_DETECTION) - if (!frSkySpiDetect()) { - return false; - } -#else - UNUSED(frSkySpiDetect); -#endif + cc2500SpiInit(); spiProtocol = rxSpiConfig->rx_spi_protocol; @@ -554,42 +443,6 @@ bool frSkySpiInit(const rxSpiConfig_t *rxSpiConfig, rxRuntimeConfig_t *rxRuntime } #endif - // gpio init here - gdoPin = IOGetByTag(IO_TAG(RX_FRSKY_SPI_GDO_0_PIN)); - IOInit(gdoPin, OWNER_RX_SPI, 0); - IOConfigGPIO(gdoPin, IOCFG_IN_FLOATING); - frSkyLedPin = IOGetByTag(IO_TAG(RX_FRSKY_SPI_LED_PIN)); - IOInit(frSkyLedPin, OWNER_LED, 0); - IOConfigGPIO(frSkyLedPin, IOCFG_OUT_PP); -#if defined(USE_RX_FRSKY_SPI_PA_LNA) - rxLnaEnPin = IOGetByTag(IO_TAG(RX_FRSKY_SPI_LNA_EN_PIN)); - IOInit(rxLnaEnPin, OWNER_RX_SPI, 0); - IOConfigGPIO(rxLnaEnPin, IOCFG_OUT_PP); - IOHi(rxLnaEnPin); // always on at the moment - txEnPin = IOGetByTag(IO_TAG(RX_FRSKY_SPI_TX_EN_PIN)); - IOInit(txEnPin, OWNER_RX_SPI, 0); - IOConfigGPIO(txEnPin, IOCFG_OUT_PP); -#if defined(USE_RX_FRSKY_SPI_DIVERSITY) - antSelPin = IOGetByTag(IO_TAG(RX_FRSKY_SPI_ANT_SEL_PIN)); - IOInit(antSelPin, OWNER_RX_SPI, 0); - IOConfigGPIO(antSelPin, IOCFG_OUT_PP); -#endif -#endif // USE_RX_FRSKY_SPI_PA_LNA -#if defined(BINDPLUG_PIN) - bindPin = IOGetByTag(IO_TAG(BINDPLUG_PIN)); - IOInit(bindPin, OWNER_RX_BIND, 0); - IOConfigGPIO(bindPin, IOCFG_IPU); - - lastBindPinStatus = IORead(bindPin); -#endif - -#if defined(USE_RX_FRSKY_SPI_PA_LNA) -#if defined(USE_RX_FRSKY_SPI_DIVERSITY) - IOHi(antSelPin); -#endif - TxDisable(); -#endif // USE_RX_FRSKY_SPI_PA_LNA - missingPackets = 0; timeoutUs = 50; diff --git a/src/main/rx/cc2500_frsky_shared.h b/src/main/rx/cc2500_frsky_shared.h index da6700da43..e8ed074592 100644 --- a/src/main/rx/cc2500_frsky_shared.h +++ b/src/main/rx/cc2500_frsky_shared.h @@ -50,22 +50,7 @@ enum { extern uint8_t listLength; extern uint32_t missingPackets; extern timeDelta_t timeoutUs; -extern int16_t rssiDbm; - -extern IO_t gdoPin; - -void setRssiDbm(uint8_t value); - -void TxEnable(void); -void TxDisable(void); - -void LedOn(void); -void LedOff(void); - -void switchAntennae(void); void initialiseData(uint8_t adr); -bool checkBindRequested(bool reset); - void nextChannel(uint8_t skip); diff --git a/src/main/rx/cc2500_frsky_x.c b/src/main/rx/cc2500_frsky_x.c index f37da6967c..96e3840142 100644 --- a/src/main/rx/cc2500_frsky_x.c +++ b/src/main/rx/cc2500_frsky_x.c @@ -47,6 +47,7 @@ #include "fc/config.h" +#include "rx/cc2500_common.h" #include "rx/cc2500_frsky_common.h" #include "rx/cc2500_frsky_shared.h" @@ -190,7 +191,7 @@ static void buildTelemetryFrame(uint8_t *packet) frame[3] = packet[3]; if (evenRun) { - frame[4] = (uint8_t)rssiDbm | 0x80; + frame[4] = (uint8_t)cc2500getRssiDbm() | 0x80; } else { uint8_t a1Value; if (rxFrSkySpiConfig()->useExternalAdc) { @@ -282,12 +283,9 @@ void frSkyXSetRcData(uint16_t *rcData, const uint8_t *packet) c[7] = (uint16_t)((packet[20] << 4) & 0xFF0) | (packet[19] >> 4); for (unsigned i = 0; i < 8; i++) { - bool channelIsShifted = false; - if (c[i] & 0x800) { - channelIsShifted = true; - } + const bool channelIsShifted = c[i] & 0x800; const uint16_t channelValue = c[i] & 0x7FF; - rcData[channelIsShifted ? i + 8 : i] = channelValue * 2.0f / 3 + 860 - 64 * 2 / 3; + rcData[channelIsShifted ? i + 8 : i] = ((channelValue - 64) * 2 + 860 * 3) / 3; } } @@ -331,7 +329,7 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro *protocolState = STATE_DATA; frameReceived = false; // again set for receive receiveDelayUs = 5300; - if (checkBindRequested(false)) { + if (cc2500checkBindRequested(false)) { packetTimerUs = 0; timeoutUs = 50; missingPackets = 0; @@ -343,7 +341,7 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro FALLTHROUGH; // here FS code could be case STATE_DATA: - if (IORead(gdoPin) && (frameReceived == false)){ + if (cc2500getGdo() && (frameReceived == false)){ uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; // read 2 times to avoid reading errors if (ccLen > 32) { @@ -360,7 +358,7 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro missingPackets = 0; timeoutUs = 1; receiveDelayUs = 0; - LedOn(); + cc2500LedOn(); if (skipChannels) { channelsToSkip = packet[5] << 2; if (packet[4] >= listLength) { @@ -375,9 +373,7 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro telemetryReceived = true; // now telemetry can be sent skipChannels = false; } -#ifdef USE_RX_FRSKY_SPI_TELEMETRY - setRssiDbm(packet[ccLen - 2]); -#endif + cc2500setRssiDbm(packet[ccLen - 2]); telemetrySequenceMarker_t *inFrameMarker = (telemetrySequenceMarker_t *)&packet[21]; @@ -444,15 +440,13 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro } if (cmpTimeUs(micros(), packetTimerUs) > timeoutUs * SYNC_DELAY_MAX) { if (ledIsOn) { - LedOff(); + cc2500LedOff(); } else { - LedOn(); + cc2500LedOn(); } ledIsOn = !ledIsOn; -#if defined(USE_RX_FRSKY_SPI_TELEMETRY) setRssiDirect(0, RSSI_SOURCE_RX_PROTOCOL); -#endif nextChannel(1); cc2500Strobe(CC2500_SRX); *protocolState = STATE_UPDATE; @@ -465,8 +459,8 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro cc2500SetPower(6); cc2500Strobe(CC2500_SFRX); delayMicroseconds(30); -#if defined(USE_RX_FRSKY_SPI_PA_LNA) - TxEnable(); +#if defined(USE_RX_CC2500_SPI_PA_LNA) + cc2500TxEnable(); #endif cc2500Strobe(CC2500_SIDLE); cc2500WriteFifo(frame, frame[0] + 1); @@ -514,14 +508,14 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro frameReceived = false; // again set for receive nextChannel(channelsToSkip); cc2500Strobe(CC2500_SRX); -#ifdef USE_RX_FRSKY_SPI_PA_LNA - TxDisable(); -#if defined(USE_RX_FRSKY_SPI_DIVERSITY) +#ifdef USE_RX_CC2500_SPI_PA_LNA + cc2500TxDisable(); +#if defined(USE_RX_CC2500_SPI_DIVERSITY) if (missingPackets >= 2) { - switchAntennae(); + cc2500switchAntennae(); } #endif -#endif // USE_RX_FRSKY_SPI_PA_LNA +#endif // USE_RX_CC2500_SPI_PA_LNA if (missingPackets > MAX_MISSING_PKT) { timeoutUs = 50; skipChannels = true; @@ -543,7 +537,7 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro void frSkyXInit(void) { #if defined(USE_TELEMETRY_SMARTPORT) - if (feature(FEATURE_TELEMETRY)) { + if (featureIsEnabled(FEATURE_TELEMETRY)) { telemetryEnabled = initSmartPortTelemetryExternal(frSkyXTelemetryWriteFrame); } #endif diff --git a/src/main/rx/cc2500_sfhss.c b/src/main/rx/cc2500_sfhss.c new file mode 100755 index 0000000000..740a36f047 --- /dev/null +++ b/src/main/rx/cc2500_sfhss.c @@ -0,0 +1,443 @@ +/* + * 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 . + */ + +#include + +#include "platform.h" + +#ifdef USE_RX_SFHSS_SPI + +#include "build/build_config.h" +#include "build/debug.h" + +#include "common/maths.h" + +#include "pg/pg.h" +#include "pg/pg_ids.h" +#include "pg/rx.h" +#include "pg/rx_spi.h" + +#include "drivers/rx/rx_cc2500.h" +#include "drivers/io.h" +#include "drivers/time.h" + +#include "fc/config.h" + +#include "rx/rx.h" +#include "rx/rx_spi.h" + +#include "rx/cc2500_common.h" +#include "rx/cc2500_frsky_common.h" +#include "rx/cc2500_sfhss.h" + +//void cliPrintLinef(const char *format, ...); + +#define BIND_CH 15 +#define SFHSS_PACKET_LEN 15 +#define BIND_TUNE_STEP 4 + +#define SFHSSCH2CHANNR(ch) (ch * 6 + 16) +#define GET_CHAN(x) ((int)((x[5]>>3) & 0x1f)) +#define GET_CODE(x) (((x[11] & 0x7)<<2 ) | ((x[12]>>6) & 0x3)) +#define GET_COMMAND(x) (x[12] & 0xf) +#define GET_CH1(x) ((uint16_t)(((x[5] & 0x07)<<9 | x[6]<<1) | (x[7] & 0x80)>>7)) +#define GET_CH2(x) (uint16_t)(((x[7] & 0x7f)<<5 | (x[8] & 0xf8)>>3)) +#define GET_CH3(x) (uint16_t)(((x[8] & 0x07)<<9 | x[9]<<1) | (x[10] & 0x80)>>7) +#define GET_CH4(x) (uint16_t)(((x[10] & 0x7f)<<5 | (x[11] & 0xf8)>>3)) +#define GET_TXID1(x) (uint8_t)(x[1]) +#define GET_TXID2(x) (uint8_t)(x[2]) +#define SET_STATE(x) {protocolState = x; DEBUG_SET(DEBUG_RX_SFHSS_SPI, DEBUG_DATA_STATE, x);} + +#define NEXT_CH_TIME_HUNT 500000 /* hunt */ +#define NEXT_CH_TIME_SYNC0 6800 /* sync no recv */ +#define NEXT_CH_TIME_SYNC1 3500 /* sync ch1-4 recv */ +#define NEXT_CH_TIME_SYNC2 500 /* sync ch5-8 recv */ + +static int8_t sfhss_channel = 0; +static int8_t sfhss_code = 0; + +static timeMs_t start_time; +static uint8_t protocolState; + +static uint32_t missingPackets; + +static uint8_t calData[32][3]; +static timeMs_t timeTunedMs; +static int8_t bindOffset_max = 0; +static int8_t bindOffset_min = 0; + +static void initialise() +{ + cc2500Reset(); + + cc2500WriteReg(CC2500_02_IOCFG0, 0x01); + cc2500WriteReg(CC2500_03_FIFOTHR, 0x07); + cc2500WriteReg(CC2500_04_SYNC1, 0xD3); + cc2500WriteReg(CC2500_05_SYNC0, 0x91); + cc2500WriteReg(CC2500_06_PKTLEN, 0x0D); + cc2500WriteReg(CC2500_07_PKTCTRL1, 0x04); + cc2500WriteReg(CC2500_08_PKTCTRL0, 0x0C); + cc2500WriteReg(CC2500_09_ADDR, 0x29); + cc2500WriteReg(CC2500_0B_FSCTRL1, 0x06); + cc2500WriteReg(CC2500_0C_FSCTRL0, (rxFrSkySpiConfigMutable()->bindOffset)); + cc2500WriteReg(CC2500_0D_FREQ2, 0x5C); + cc2500WriteReg(CC2500_0E_FREQ1, 0x4E); + cc2500WriteReg(CC2500_0F_FREQ0, 0xC4); + cc2500WriteReg(CC2500_10_MDMCFG4, 0x7C); + cc2500WriteReg(CC2500_11_MDMCFG3, 0x43); + cc2500WriteReg(CC2500_12_MDMCFG2, 0x03); + cc2500WriteReg(CC2500_13_MDMCFG1, 0x23); + cc2500WriteReg(CC2500_14_MDMCFG0, 0x3B); + cc2500WriteReg(CC2500_15_DEVIATN, 0x44); + cc2500WriteReg(CC2500_17_MCSM1, 0x0F); + cc2500WriteReg(CC2500_18_MCSM0, 0x08); + cc2500WriteReg(CC2500_19_FOCCFG, 0x1D); + cc2500WriteReg(CC2500_1A_BSCFG, 0x6C); + cc2500WriteReg(CC2500_1B_AGCCTRL2, 0x03); + cc2500WriteReg(CC2500_1C_AGCCTRL1, 0x40); + cc2500WriteReg(CC2500_1D_AGCCTRL0, 0x91); + cc2500WriteReg(CC2500_21_FREND1, 0x56); + cc2500WriteReg(CC2500_22_FREND0, 0x10); + cc2500WriteReg(CC2500_23_FSCAL3, 0xA9); + cc2500WriteReg(CC2500_24_FSCAL2, 0x0A); + cc2500WriteReg(CC2500_25_FSCAL1, 0x00); + cc2500WriteReg(CC2500_26_FSCAL0, 0x11); + cc2500WriteReg(CC2500_29_FSTEST, 0x59); + cc2500WriteReg(CC2500_2C_TEST2, 0x88); + cc2500WriteReg(CC2500_2D_TEST1, 0x31); + cc2500WriteReg(CC2500_2E_TEST0, 0x0B); + cc2500WriteReg(CC2500_3E_PATABLE, 0xFF); + + for (unsigned c = 0; c < 30; c++) { + //calibrate all channels + cc2500Strobe(CC2500_SIDLE); + cc2500WriteReg(CC2500_0A_CHANNR, SFHSSCH2CHANNR(c)); + cc2500Strobe(CC2500_SCAL); + delayMicroseconds(900); + calData[c][0] = cc2500ReadReg(CC2500_23_FSCAL3); + calData[c][1] = cc2500ReadReg(CC2500_24_FSCAL2); + calData[c][2] = cc2500ReadReg(CC2500_25_FSCAL1); + } +} + +static bool sfhssRecv(uint8_t *packet) +{ + uint8_t ccLen; + + if (!(cc2500getGdo())) { + return false; + } + ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; + if (ccLen < SFHSS_PACKET_LEN) { + return false; + } + + cc2500ReadFifo(packet, SFHSS_PACKET_LEN); + return true; +} + +static bool sfhssPacketParse(uint8_t *packet, bool check_txid) +{ + if (!(packet[SFHSS_PACKET_LEN - 1] & 0x80)) { + return false; /* crc fail */ + } + if (packet[0] != 0x81) { + return false; /* sfhss header fail */ + } + if (GET_CHAN(packet) != sfhss_channel) { + return false; /* channel fail */ + } + + if (check_txid) { + if ((rxFrSkySpiConfigMutable()->bindTxId[0] != GET_TXID1(packet)) || + (rxFrSkySpiConfigMutable()->bindTxId[1] != GET_TXID2(packet))) { + return false; /* txid fail */ + } + } + + cc2500setRssiDbm(packet[SFHSS_PACKET_LEN - 2]); + sfhss_code = GET_CODE(packet); + + return true; +} + +void sfhssRx(void) +{ + cc2500Strobe(CC2500_SIDLE); + cc2500WriteReg(CC2500_23_FSCAL3, calData[sfhss_channel][0]); + cc2500WriteReg(CC2500_24_FSCAL2, calData[sfhss_channel][1]); + cc2500WriteReg(CC2500_25_FSCAL1, calData[sfhss_channel][2]); + cc2500WriteReg(CC2500_0A_CHANNR, SFHSSCH2CHANNR(sfhss_channel)); + cc2500Strobe(CC2500_SFRX); + cc2500Strobe(CC2500_SRX); +} + +static void initTuneRx(void) +{ + timeTunedMs = millis(); + bindOffset_min = -64; + DEBUG_SET(DEBUG_RX_SFHSS_SPI, DEBUG_DATA_OFFSET_MIN, bindOffset_min); + cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)bindOffset_min); + sfhss_channel = BIND_CH; + sfhssRx(); +} + +static bool tune1Rx(uint8_t *packet) +{ + if (bindOffset_min >= 126) { + bindOffset_min = -126; + DEBUG_SET(DEBUG_RX_SFHSS_SPI, DEBUG_DATA_OFFSET_MIN, bindOffset_min); + } + if ((millis() - timeTunedMs) > 220) { // 220ms + timeTunedMs = millis(); + bindOffset_min += BIND_TUNE_STEP << 2; + DEBUG_SET(DEBUG_RX_SFHSS_SPI, DEBUG_DATA_OFFSET_MIN, bindOffset_min); + cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)bindOffset_min); + cc2500Strobe(CC2500_SRX); + } + if (sfhssRecv(packet)) { + if (sfhssPacketParse(packet, false)) { + if ((packet[SFHSS_PACKET_LEN - 1] & 0x7F) > 40 ) { /* lqi */ + rxFrSkySpiConfigMutable()->bindTxId[0] = GET_TXID1(packet); + rxFrSkySpiConfigMutable()->bindTxId[1] = GET_TXID2(packet); + bindOffset_max = bindOffset_min; + DEBUG_SET(DEBUG_RX_SFHSS_SPI, DEBUG_DATA_OFFSET_MAX, bindOffset_max); + cc2500Strobe(CC2500_SRX); + timeTunedMs = millis(); + return true; + } + } + cc2500Strobe(CC2500_SRX); + } + return false; +} + +static bool tune2Rx(uint8_t *packet) +{ + cc2500LedBlink(100); + if (((millis() - timeTunedMs) > 880) || bindOffset_max > (126 - BIND_TUNE_STEP)) { // 220ms *4 + timeTunedMs = millis(); + cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)bindOffset_min); + cc2500Strobe(CC2500_SRX); + return true; + } + if (sfhssRecv(packet)) { + if (sfhssPacketParse(packet, true)) { + timeTunedMs = millis(); + bindOffset_max += BIND_TUNE_STEP; + DEBUG_SET(DEBUG_RX_SFHSS_SPI, DEBUG_DATA_OFFSET_MAX, bindOffset_max); + cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)bindOffset_max); + } + cc2500Strobe(CC2500_SRX); + } + return false; +} + +static bool tune3Rx(uint8_t *packet) +{ + cc2500LedBlink(100); + if (((millis() - timeTunedMs) > 880) || bindOffset_min < (-126 + BIND_TUNE_STEP)) { // 220ms *4 + return true; + } + if (sfhssRecv(packet)) { + if (sfhssPacketParse(packet, true)) { + timeTunedMs = millis(); + bindOffset_min -= BIND_TUNE_STEP; + DEBUG_SET(DEBUG_RX_SFHSS_SPI, DEBUG_DATA_OFFSET_MIN, bindOffset_min); + cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)bindOffset_min); + } + cc2500Strobe(CC2500_SRX); + } + return false; +} + +void sfhssnextChannel(void) +{ + do { + sfhss_channel += sfhss_code + 2; + if (sfhss_channel > 29) { + sfhss_channel -= 31; + } + } while ( sfhss_channel < 0); + + sfhssRx(); +} + +void sfhssSpiSetRcData(uint16_t *rcData, const uint8_t *payload) +{ + if ( GET_COMMAND(payload) & 0x8 ) { + rcData[4] = GET_CH1(payload); + rcData[5] = GET_CH2(payload); + rcData[6] = GET_CH3(payload); + rcData[7] = GET_CH4(payload); + } else { + rcData[0] = GET_CH1(payload); + rcData[1] = GET_CH2(payload); + rcData[2] = GET_CH3(payload); + rcData[3] = GET_CH4(payload); + } +} + +rx_spi_received_e sfhssSpiDataReceived(uint8_t *packet) +{ + static uint16_t dataMissingFrame = 0; + static timeUs_t nextFrameReceiveStartTime = 0; + static uint8_t frame_recvd = 0; + timeUs_t currentPacketReceivedTime; + rx_spi_received_e ret = RX_SPI_RECEIVED_NONE; + + currentPacketReceivedTime = micros(); + switch (protocolState) { + case STATE_INIT: + if ((millis() - start_time) > 10) { + cc2500LedOff(); + dataMissingFrame = 0; + initialise(); + SET_STATE(STATE_BIND); + DEBUG_SET(DEBUG_RX_SFHSS_SPI, DEBUG_DATA_MISSING_FRAME, dataMissingFrame); + } + break; + case STATE_BIND: + if (cc2500checkBindRequested(true)) { + cc2500LedOn(); + initTuneRx(); + SET_STATE(STATE_BIND_TUNING1); + } else { + SET_STATE(STATE_HUNT); + sfhssnextChannel(); + setRssiDirect(0, RSSI_SOURCE_RX_PROTOCOL); + nextFrameReceiveStartTime = currentPacketReceivedTime + NEXT_CH_TIME_HUNT; + } + break; + case STATE_BIND_TUNING1: + if (tune1Rx(packet)) { + SET_STATE(STATE_BIND_TUNING2); + } + break; + case STATE_BIND_TUNING2: + if (tune2Rx(packet)) { + SET_STATE(STATE_BIND_TUNING3); + } + break; + case STATE_BIND_TUNING3: + if (tune3Rx(packet)) { + if (((int16_t)bindOffset_max - (int16_t)bindOffset_min) <= 2) { + initTuneRx(); + SET_STATE(STATE_BIND_TUNING1); // retry + } else { + rxFrSkySpiConfigMutable()->bindOffset = ((int16_t)bindOffset_max + (int16_t)bindOffset_min) / 2 ; + SET_STATE(STATE_BIND_COMPLETE); + } + } + break; + case STATE_BIND_COMPLETE: + writeEEPROM(); + ret = RX_SPI_RECEIVED_BIND; + SET_STATE(STATE_INIT); + break; + case STATE_HUNT: + if (sfhssRecv(packet)) { + if (sfhssPacketParse(packet, true)) { + if (GET_COMMAND(packet) & 0x8) { /* ch=5-8 */ + missingPackets = 0; + cc2500LedOn(); + frame_recvd = 0x3; + SET_STATE(STATE_SYNC); + nextFrameReceiveStartTime = currentPacketReceivedTime + NEXT_CH_TIME_SYNC2; + return RX_SPI_RECEIVED_NONE; + } + } + cc2500Strobe(CC2500_SRX); + } else if (cmpTimeUs(currentPacketReceivedTime, nextFrameReceiveStartTime) > 0) { + cc2500LedBlink(500); +#if defined(USE_RX_CC2500_SPI_PA_LNA) && defined(USE_RX_CC2500_SPI_DIVERSITY) // SE4311 chip + cc2500switchAntennae(); +#endif + sfhssnextChannel(); + nextFrameReceiveStartTime += NEXT_CH_TIME_HUNT; + } else if (cc2500checkBindRequested(false)) { + SET_STATE(STATE_INIT); + break; + } + break; + case STATE_SYNC: + if (sfhssRecv(packet)) { + if (sfhssPacketParse(packet, true)) { + missingPackets = 0; + if ( GET_COMMAND(packet) & 0x8 ) { + nextFrameReceiveStartTime = currentPacketReceivedTime + NEXT_CH_TIME_SYNC2; + frame_recvd |= 0x2; /* ch5-8 */ + } else { + nextFrameReceiveStartTime = currentPacketReceivedTime + NEXT_CH_TIME_SYNC1; + cc2500Strobe(CC2500_SRX); + frame_recvd |= 0x1; /* ch1-4 */ + } + if (GET_COMMAND(packet) & 0x4) { + return RX_SPI_RECEIVED_NONE; /* failsafe data */ + } + return RX_SPI_RECEIVED_DATA; + } + cc2500Strobe(CC2500_SRX); + } else if (cmpTimeUs(currentPacketReceivedTime, nextFrameReceiveStartTime) > 0) { + nextFrameReceiveStartTime += NEXT_CH_TIME_SYNC0; + if (frame_recvd != 0x3) { + DEBUG_SET(DEBUG_RX_SFHSS_SPI, DEBUG_DATA_MISSING_FRAME, ++dataMissingFrame); + } + if (frame_recvd == 0) { + if (++missingPackets > MAX_MISSING_PKT) { + SET_STATE(STATE_HUNT); + sfhssnextChannel(); + setRssiDirect(0, RSSI_SOURCE_RX_PROTOCOL); + nextFrameReceiveStartTime = currentPacketReceivedTime + NEXT_CH_TIME_HUNT; + break; + } +#if defined(USE_RX_CC2500_SPI_PA_LNA) && defined(USE_RX_CC2500_SPI_DIVERSITY) // SE4311 chip + if (missingPackets >= 2) { + cc2500switchAntennae(); + } +#endif + } + frame_recvd = 0; + sfhssnextChannel(); + } else if (cc2500checkBindRequested(false)) { + SET_STATE(STATE_INIT); + break; + } + break; + } + + return ret; +} + +bool sfhssSpiInit(const rxSpiConfig_t *rxSpiConfig, rxRuntimeConfig_t *rxRuntimeConfig) +{ + UNUSED(rxSpiConfig); + + cc2500SpiInit(); + + rxRuntimeConfig->channelCount = RC_CHANNEL_COUNT_SFHSS; + + start_time = millis(); + SET_STATE(STATE_INIT); + + return true; +} +#endif + diff --git a/src/main/rx/cc2500_sfhss.h b/src/main/rx/cc2500_sfhss.h new file mode 100755 index 0000000000..c1490e2ca1 --- /dev/null +++ b/src/main/rx/cc2500_sfhss.h @@ -0,0 +1,55 @@ +/* + * 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 + +#include "pg/pg.h" + +#include "rx/rx_spi.h" + +#define MAX_MISSING_PKT 100 +#define RC_CHANNEL_COUNT_SFHSS 8 + +#define DEBUG_DATA_STATE 0 +#define DEBUG_DATA_MISSING_FRAME 1 +#define DEBUG_DATA_OFFSET_MAX 2 +#define DEBUG_DATA_OFFSET_MIN 3 + +#define STATE_INIT 0 +#define STATE_HUNT 1 +#define STATE_SYNC 2 +#define STATE_BIND 10 +#define STATE_BIND_TUNING1 11 +#define STATE_BIND_TUNING2 12 +#define STATE_BIND_TUNING3 13 +#define STATE_BIND_COMPLETE 14 + +typedef struct rxSfhssSpiConfig_s { + uint8_t bindTxId[2]; + int8_t bindOffset; +} rxSfhssSpiConfig_t; + +PG_DECLARE(rxSfhssSpiConfig_t, rxSfhssSpiConfig); + +bool sfhssSpiInit(const rxSpiConfig_t *rxSpiConfig, rxRuntimeConfig_t *rxRuntimeConfig); +rx_spi_received_e sfhssSpiDataReceived(uint8_t *packet); +void sfhssSpiSetRcData(uint16_t *rcData, const uint8_t *payload); + +void sfhssSpiBind(void); diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c index 2f3753cb17..87961d690c 100644 --- a/src/main/rx/crsf.c +++ b/src/main/rx/crsf.c @@ -25,7 +25,7 @@ #include "platform.h" -#ifdef USE_SERIAL_RX +#ifdef USE_SERIALRX_CRSF #include "build/build_config.h" #include "build/debug.h" @@ -159,7 +159,7 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *data) if (crc == crsfFrame.bytes[fullFrameLength - 1]) { switch (crsfFrame.frame.type) { -#if defined(USE_MSP_OVER_TELEMETRY) +#if defined(USE_TELEMETRY_CRSF) && defined(USE_MSP_OVER_TELEMETRY) case CRSF_FRAMETYPE_MSP_REQ: case CRSF_FRAMETYPE_MSP_WRITE: { uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE; @@ -169,10 +169,10 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *data) break; } #endif +#if defined(USE_CRSF_CMS_TELEMETRY) case CRSF_FRAMETYPE_DEVICE_PING: crsfScheduleDeviceInfoResponse(); break; -#if defined(USE_CRSF_CMS_TELEMETRY) case CRSF_FRAMETYPE_DISPLAYPORT_CMD: { uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE; crsfProcessDisplayPortCmd(frameStart); diff --git a/src/main/rx/fport.c b/src/main/rx/fport.c index f31624ee68..a6d455d0c9 100644 --- a/src/main/rx/fport.c +++ b/src/main/rx/fport.c @@ -25,7 +25,7 @@ #include "platform.h" -#if defined(USE_SERIAL_RX) +#ifdef USE_SERIALRX_FPORT #include "build/debug.h" @@ -53,7 +53,7 @@ #define FPORT_MIN_TELEMETRY_RESPONSE_DELAY_US 500 #define FPORT_MAX_TELEMETRY_AGE_MS 500 -#define FPORT_TELEMETRY_MAX_CONSECUTIVE_SENSORS 2 +#define FPORT_TELEMETRY_MAX_CONSECUTIVE_TELEMETRY_FRAMES 2 #define FPORT_FRAME_MARKER 0x7E @@ -71,7 +71,7 @@ enum { DEBUG_FPORT_FRAME_INTERVAL = 0, DEBUG_FPORT_FRAME_ERRORS, DEBUG_FPORT_FRAME_LAST_ERROR, - DEBUG_FPORT_TELEMETRY_DELAY, + DEBUG_FPORT_TELEMETRY_INTERVAL, }; enum { @@ -114,7 +114,9 @@ typedef struct fportFrame_s { fportData_t data; } fportFrame_t; +#ifdef USE_TELEMETRY_SMARTPORT static const smartPortPayload_t emptySmartPortFrame = { .frameId = 0, .valueId = 0, .data = 0 }; +#endif #define FPORT_REQUEST_FRAME_LENGTH sizeof(fportFrame_t) #define FPORT_RESPONSE_FRAME_LENGTH (sizeof(uint8_t) + sizeof(smartPortPayload_t)) @@ -144,12 +146,16 @@ static smartPortPayload_t *mspPayload = NULL; static timeUs_t lastRcFrameReceivedMs = 0; static serialPort_t *fportPort; +#ifdef USE_TELEMETRY_SMARTPORT static bool telemetryEnabled = false; +#endif static void reportFrameError(uint8_t errorReason) { static volatile uint16_t frameErrors = 0; - DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_FRAME_ERRORS, ++frameErrors); + frameErrors++; + + DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_FRAME_ERRORS, frameErrors); DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_FRAME_LAST_ERROR, errorReason); } @@ -248,11 +254,17 @@ static bool checkChecksum(uint8_t *data, uint8_t length) static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) { - static smartPortPayload_t payloadBuffer; static bool hasTelemetryRequest = false; +#ifdef USE_TELEMETRY_SMARTPORT + static smartPortPayload_t payloadBuffer; + static bool rxDrivenFrameRate = false; + static uint8_t consecutiveTelemetryFrameCount = 0; +#endif + uint8_t result = RX_FRAME_PENDING; + if (rxBufferReadIndex != rxBufferWriteIndex) { uint8_t bufferLength = rxBuffer[rxBufferReadIndex].length; uint8_t frameLength = rxBuffer[rxBufferReadIndex].data[0]; @@ -287,10 +299,25 @@ static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) } switch(frame->data.telemetryData.frameId) { - case FPORT_FRAME_ID_NULL: - case FPORT_FRAME_ID_DATA: // never used + case FPORT_FRAME_ID_DATA: + if (!rxDrivenFrameRate) { + rxDrivenFrameRate = true; + } + hasTelemetryRequest = true; + break; + case FPORT_FRAME_ID_NULL: + if (!rxDrivenFrameRate) { + if (consecutiveTelemetryFrameCount >= FPORT_TELEMETRY_MAX_CONSECUTIVE_TELEMETRY_FRAMES && !(mspPayload && smartPortPayloadContainsMSP(mspPayload))) { + consecutiveTelemetryFrameCount = 0; + } else { + hasTelemetryRequest = true; + + consecutiveTelemetryFrameCount++; + } + } + break; case FPORT_FRAME_ID_READ: case FPORT_FRAME_ID_WRITE: // never used @@ -337,7 +364,7 @@ static bool fportProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig) UNUSED(rxRuntimeConfig); #if defined(USE_TELEMETRY_SMARTPORT) - static uint8_t consecutiveSensorCount = 0; + static timeUs_t lastTelemetryFrameSentUs; timeUs_t currentTimeUs = micros(); if (cmpTimeUs(currentTimeUs, lastTelemetryFrameReceivedUs) > FPORT_MAX_TELEMETRY_RESPONSE_DELAY_US) { @@ -345,24 +372,16 @@ static bool fportProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig) } if (clearToSend) { - DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_TELEMETRY_DELAY, currentTimeUs - lastTelemetryFrameReceivedUs); - - if (consecutiveSensorCount >= FPORT_TELEMETRY_MAX_CONSECUTIVE_SENSORS && !(mspPayload && smartPortPayloadContainsMSP(mspPayload))) { -// Stop one cycle to avoid saturating the buffer in the RX, since the -// downstream bandwidth doesn't allow sensor sensors on every cycle. -// We allow MSP frames to run over the resting period, expecting that -// the caller won't flood us with requests. - consecutiveSensorCount = 0; - } else { - consecutiveSensorCount++; - processSmartPortTelemetry(mspPayload, &clearToSend, NULL); - } + processSmartPortTelemetry(mspPayload, &clearToSend, NULL); if (clearToSend) { smartPortWriteFrameFport(&emptySmartPortFrame); clearToSend = false; } + + DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_TELEMETRY_INTERVAL, currentTimeUs - lastTelemetryFrameSentUs); + lastTelemetryFrameSentUs = currentTimeUs; } mspPayload = NULL; diff --git a/src/main/rx/ibus.c b/src/main/rx/ibus.c index dd9bca21fc..727f071e3d 100644 --- a/src/main/rx/ibus.c +++ b/src/main/rx/ibus.c @@ -31,7 +31,7 @@ #include "platform.h" -#ifdef USE_SERIAL_RX +#ifdef USE_SERIALRX_IBUS #include "pg/rx.h" diff --git a/src/main/rx/jetiexbus.c b/src/main/rx/jetiexbus.c index 07667c715d..c2a64e8a76 100644 --- a/src/main/rx/jetiexbus.c +++ b/src/main/rx/jetiexbus.c @@ -33,24 +33,20 @@ * Number of channels: 16 * * Connect as follows: - * Jeti EX Bus -> Serial RX (connect directly) - * Serial TX -> Resistor(2k4) ->Serial RX - * In jeti pdf it is different, but if the resistor breaks, the receiver continues to operate. - * + * Jeti EX Bus -> Serial TX (connect directly) */ + #include #include #include "platform.h" -#ifdef USE_SERIAL_RX +#ifdef USE_SERIALRX_JETIEXBUS #include "build/build_config.h" #include "build/debug.h" -#include "pg/rx.h" - #include "common/utils.h" #include "drivers/time.h" @@ -278,9 +274,8 @@ bool jetiExBusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfi NULL, JETIEXBUS_BAUDRATE, MODE_RXTX, - JETIEXBUS_OPTIONS | (rxConfig->serialrx_inverted ? SERIAL_INVERTED : 0) | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0) + JETIEXBUS_OPTIONS | (rxConfig->serialrx_inverted ? SERIAL_INVERTED : 0) | SERIAL_BIDIR ); - serialSetMode(jetiExBusPort, MODE_RX); return jetiExBusPort != NULL; } #endif // SERIAL_RX diff --git a/src/main/rx/nrf24_h8_3d.c b/src/main/rx/nrf24_h8_3d.c index 74b73c81db..963a0e9d4e 100644 --- a/src/main/rx/nrf24_h8_3d.c +++ b/src/main/rx/nrf24_h8_3d.c @@ -93,7 +93,7 @@ STATIC_UNIT_TESTED uint8_t payloadSize; STATIC_UNIT_TESTED uint8_t rxTxAddrXN297[RX_TX_ADDR_LEN] = {0x41, 0xbd, 0x42, 0xd4, 0xc2}; // converted XN297 address #define TX_ID_LEN 4 STATIC_UNIT_TESTED uint8_t txId[TX_ID_LEN]; -uint32_t *rxSpiIdPtr; +static uint32_t *rxSpiIdPtr; // radio channels for frequency hopping #define H8_3D_RF_CHANNEL_COUNT 4 diff --git a/src/main/rx/nrf24_inav.c b/src/main/rx/nrf24_inav.c index 5082d64683..0fc8df2dda 100644 --- a/src/main/rx/nrf24_inav.c +++ b/src/main/rx/nrf24_inav.c @@ -128,7 +128,7 @@ uint8_t receivedPowerSnapshot; #define RX_TX_ADDR_LEN 5 // set rxTxAddr to the bind address STATIC_UNIT_TESTED uint8_t rxTxAddr[RX_TX_ADDR_LEN] = {0x4b,0x5c,0x6d,0x7e,0x8f}; -uint32_t *rxSpiIdPtr; +static uint32_t *rxSpiIdPtr; #define RX_TX_ADDR_4 0xD2 // rxTxAddr[4] always set to this value // radio channels for frequency hopping diff --git a/src/main/rx/pwm.c b/src/main/rx/pwm.c index 356752797f..1ddf46542e 100644 --- a/src/main/rx/pwm.c +++ b/src/main/rx/pwm.c @@ -60,10 +60,10 @@ void rxPwmInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) rxRuntimeConfig->rxRefreshRate = 20000; // configure PWM/CPPM read function and max number of channels. serial rx below will override both of these, if enabled - if (feature(FEATURE_RX_PARALLEL_PWM)) { + if (featureIsEnabled(FEATURE_RX_PARALLEL_PWM)) { rxRuntimeConfig->channelCount = MAX_SUPPORTED_RC_PARALLEL_PWM_CHANNEL_COUNT; rxRuntimeConfig->rcReadRawFn = pwmReadRawRC; - } else if (feature(FEATURE_RX_PPM)) { + } else if (featureIsEnabled(FEATURE_RX_PPM)) { rxRuntimeConfig->channelCount = MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT; rxRuntimeConfig->rcReadRawFn = ppmReadRawRC; } diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index f565249d84..d4c8e29ea0 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -102,6 +102,7 @@ uint32_t rcInvalidPulsPeriod[MAX_SUPPORTED_RC_CHANNEL_COUNT]; #define PPM_AND_PWM_SAMPLE_COUNT 3 #define DELAY_50_HZ (1000000 / 50) +#define DELAY_33_HZ (1000000 / 33) #define DELAY_10_HZ (1000000 / 10) #define DELAY_5_HZ (1000000 / 5) #define SKIP_RC_ON_SUSPEND_PERIOD 1500000 // 1.5 second period in usec (call frequency independent) @@ -247,7 +248,7 @@ void rxInit(void) rcInvalidPulsPeriod[i] = millis() + MAX_INVALID_PULS_TIME; } - rcData[THROTTLE] = (feature(FEATURE_3D)) ? rxConfig()->midrc : rxConfig()->rx_min_usec; + rcData[THROTTLE] = (featureIsEnabled(FEATURE_3D)) ? rxConfig()->midrc : rxConfig()->rx_min_usec; // Initialize ARM switch to OFF position when arming via switch is defined // TODO - move to rc_mode.c @@ -267,10 +268,9 @@ void rxInit(void) } #ifdef USE_SERIAL_RX - if (feature(FEATURE_RX_SERIAL)) { + if (featureIsEnabled(FEATURE_RX_SERIAL)) { const bool enabled = serialRxInit(rxConfig(), &rxRuntimeConfig); if (!enabled) { - featureClear(FEATURE_RX_SERIAL); rxRuntimeConfig.rcReadRawFn = nullReadRawRC; rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus; } @@ -278,17 +278,16 @@ void rxInit(void) #endif #ifdef USE_RX_MSP - if (feature(FEATURE_RX_MSP)) { + if (featureIsEnabled(FEATURE_RX_MSP)) { rxMspInit(rxConfig(), &rxRuntimeConfig); needRxSignalMaxDelayUs = DELAY_5_HZ; } #endif #ifdef USE_RX_SPI - if (feature(FEATURE_RX_SPI)) { + if (featureIsEnabled(FEATURE_RX_SPI)) { const bool enabled = rxSpiInit(rxSpiConfig(), &rxRuntimeConfig); if (!enabled) { - featureClear(FEATURE_RX_SPI); rxRuntimeConfig.rcReadRawFn = nullReadRawRC; rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus; } @@ -296,13 +295,13 @@ void rxInit(void) #endif #if defined(USE_PWM) || defined(USE_PPM) - if (feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM)) { + if (featureIsEnabled(FEATURE_RX_PPM | FEATURE_RX_PARALLEL_PWM)) { rxPwmInit(rxConfig(), &rxRuntimeConfig); } #endif #if defined(USE_ADC) - if (feature(FEATURE_RSSI_ADC)) { + if (featureIsEnabled(FEATURE_RSSI_ADC)) { rssiSource = RSSI_SOURCE_ADC; } else #endif @@ -323,18 +322,26 @@ bool rxAreFlightChannelsValid(void) return rxFlightChannelsValid; } -void suspendRxSignal(void) +void suspendRxPwmPpmSignal(void) { - suspendRxSignalUntil = micros() + SKIP_RC_ON_SUSPEND_PERIOD; - skipRxSamples = SKIP_RC_SAMPLES_ON_RESUME; - failsafeOnRxSuspend(SKIP_RC_ON_SUSPEND_PERIOD); +#if defined(USE_PWM) || defined(USE_PPM) + if (featureIsEnabled(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM)) { + suspendRxSignalUntil = micros() + SKIP_RC_ON_SUSPEND_PERIOD; + skipRxSamples = SKIP_RC_SAMPLES_ON_RESUME; + failsafeOnRxSuspend(SKIP_RC_ON_SUSPEND_PERIOD); + } +#endif } -void resumeRxSignal(void) +void resumeRxPwmPpmSignal(void) { - suspendRxSignalUntil = micros(); - skipRxSamples = SKIP_RC_SAMPLES_ON_RESUME; - failsafeOnRxResume(); +#if defined(USE_PWM) || defined(USE_PPM) + if (featureIsEnabled(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM)) { + suspendRxSignalUntil = micros(); + skipRxSamples = SKIP_RC_SAMPLES_ON_RESUME; + failsafeOnRxResume(); + } +#endif } bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime) @@ -345,14 +352,14 @@ bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime) bool useDataDrivenProcessing = true; #if defined(USE_PWM) || defined(USE_PPM) - if (feature(FEATURE_RX_PPM)) { + if (featureIsEnabled(FEATURE_RX_PPM)) { if (isPPMDataBeingReceived()) { signalReceived = true; rxIsInFailsafeMode = false; needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs; resetPPMDataReceivedState(); } - } else if (feature(FEATURE_RX_PARALLEL_PWM)) { + } else if (featureIsEnabled(FEATURE_RX_PARALLEL_PWM)) { if (isPWMDataBeingReceived()) { signalReceived = true; rxIsInFailsafeMode = false; @@ -398,6 +405,7 @@ bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime) return rxDataProcessingRequired || auxiliaryProcessingRequired; // data driven or 50Hz } +#if defined(USE_PWM) || defined(USE_PPM) static uint16_t calculateChannelMovingAverage(uint8_t chan, uint16_t sample) { static int16_t rcSamples[MAX_SUPPORTED_RX_PARALLEL_PWM_OR_PPM_CHANNEL_COUNT][PPM_AND_PWM_SAMPLE_COUNT]; @@ -423,6 +431,7 @@ static uint16_t calculateChannelMovingAverage(uint8_t chan, uint16_t sample) } return rcDataMean[chan] / PPM_AND_PWM_SAMPLE_COUNT; } +#endif static uint16_t getRxfailValue(uint8_t channel) { @@ -436,7 +445,7 @@ static uint16_t getRxfailValue(uint8_t channel) case YAW: return rxConfig()->midrc; case THROTTLE: - if (feature(FEATURE_3D)) + if (featureIsEnabled(FEATURE_3D)) return rxConfig()->midrc; else return rxConfig()->rx_min_usec; @@ -511,10 +520,13 @@ static void detectAndApplySignalLossBehaviour(void) } } } - if (feature(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM)) { +#if defined(USE_PWM) || defined(USE_PPM) + if (featureIsEnabled(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM)) { // smooth output for PWM and PPM rcData[channel] = calculateChannelMovingAverage(channel, sample); - } else { + } else +#endif + { rcData[channel] = sample; } } @@ -542,10 +554,10 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs) } rxDataProcessingRequired = false; - rxNextUpdateAtUs = currentTimeUs + DELAY_50_HZ; + rxNextUpdateAtUs = currentTimeUs + DELAY_33_HZ; // only proceed when no more samples to skip and suspend period is over - if (skipRxSamples) { + if (skipRxSamples || currentTimeUs <= suspendRxSignalUntil) { if (currentTimeUs > suspendRxSignalUntil) { skipRxSamples--; } @@ -685,3 +697,8 @@ uint16_t rxGetRefreshRate(void) { return rxRuntimeConfig.rxRefreshRate; } + +bool isRssiConfigured(void) +{ + return rssiSource != RSSI_SOURCE_NONE; +} diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index cabe221433..92b4a6108b 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -166,11 +166,11 @@ void setRssiMsp(uint8_t newMspRssi); void updateRSSI(timeUs_t currentTimeUs); uint16_t getRssi(void); uint8_t getRssiPercent(void); +bool isRssiConfigured(void); void resetAllRxChannelRangeConfigurations(rxChannelRangeConfig_t *rxChannelRangeConfig); -void suspendRxSignal(void); -void resumeRxSignal(void); +void suspendRxPwmPpmSignal(void); +void resumeRxPwmPpmSignal(void); uint16_t rxGetRefreshRate(void); - diff --git a/src/main/rx/rx_spi.c b/src/main/rx/rx_spi.c index c8cb39d40c..e69e6a2046 100644 --- a/src/main/rx/rx_spi.c +++ b/src/main/rx/rx_spi.c @@ -47,6 +47,7 @@ #include "rx/nrf24_inav.h" #include "rx/nrf24_kn.h" #include "rx/flysky.h" +#include "rx/cc2500_sfhss.h" uint16_t rxSpiRcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; @@ -63,7 +64,7 @@ static protocolSetRcDataFromPayloadFnPtr protocolSetRcDataFromPayload; STATIC_UNIT_TESTED uint16_t rxSpiReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channel) { - BUILD_BUG_ON(NRF24L01_MAX_PAYLOAD_SIZE > RX_SPI_MAX_PAYLOAD_SIZE); + STATIC_ASSERT(NRF24L01_MAX_PAYLOAD_SIZE <= RX_SPI_MAX_PAYLOAD_SIZE, NRF24L01_MAX_PAYLOAD_SIZE_larger_than_RX_SPI_MAX_PAYLOAD_SIZE); if (channel >= rxRuntimeConfig->channelCount) { return 0; @@ -144,6 +145,13 @@ STATIC_UNIT_TESTED bool rxSpiSetProtocol(rx_spi_protocol_e protocol) protocolDataReceived = flySkyDataReceived; protocolSetRcDataFromPayload = flySkySetRcDataFromPayload; break; +#endif +#ifdef USE_RX_SFHSS_SPI + case RX_SPI_SFHSS: + protocolInit = sfhssSpiInit; + protocolDataReceived = sfhssSpiDataReceived; + protocolSetRcDataFromPayload = sfhssSpiSetRcData; + break; #endif } return true; diff --git a/src/main/rx/rx_spi.h b/src/main/rx/rx_spi.h index 8f095a7844..d94554749e 100644 --- a/src/main/rx/rx_spi.h +++ b/src/main/rx/rx_spi.h @@ -39,6 +39,7 @@ typedef enum { RX_SPI_A7105_FLYSKY, RX_SPI_A7105_FLYSKY_2A, RX_SPI_NRF24_KN, + RX_SPI_SFHSS, RX_SPI_PROTOCOL_COUNT } rx_spi_protocol_e; diff --git a/src/main/rx/sbus.c b/src/main/rx/sbus.c index c5fb0787d5..14a544c723 100644 --- a/src/main/rx/sbus.c +++ b/src/main/rx/sbus.c @@ -24,7 +24,7 @@ #include "platform.h" -#ifdef USE_SERIAL_RX +#ifdef USE_SERIALRX_SBUS #include "build/debug.h" diff --git a/src/main/rx/sbus_channels.c b/src/main/rx/sbus_channels.c index a709427e20..531c9b2b5d 100644 --- a/src/main/rx/sbus_channels.c +++ b/src/main/rx/sbus_channels.c @@ -24,7 +24,7 @@ #include "platform.h" -#ifdef USE_SERIAL_RX +#ifdef USE_SBUS_CHANNELS #include "common/utils.h" diff --git a/src/main/rx/spektrum.c b/src/main/rx/spektrum.c index 9045b68631..7b7a7d41a9 100644 --- a/src/main/rx/spektrum.c +++ b/src/main/rx/spektrum.c @@ -19,7 +19,8 @@ */ #include "platform.h" -#ifdef USE_SERIAL_RX + +#ifdef USE_SERIALRX_SPEKTRUM #include @@ -36,7 +37,6 @@ #include "io/serial.h" #include "fc/config.h" -#include "fc/fc_dispatch.h" #include "io/spektrum_rssi.h" #include "io/spektrum_vtx_control.h" @@ -53,6 +53,8 @@ // driver for spektrum satellite receiver / sbus +#define SPEKTRUM_TELEMETRY_FRAME_DELAY_US 1000 // Gap between received Rc frame and transmited TM frame + bool srxlEnabled = false; int32_t resolution; uint8_t rssi_channel; @@ -67,12 +69,9 @@ static volatile uint8_t spekFrame[SPEK_FRAME_SIZE]; static rxRuntimeConfig_t *rxRuntimeConfigPtr; static serialPort_t *serialPort; -#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SRXL) +#if defined(USE_TELEMETRY_SRXL) static uint8_t telemetryBuf[SRXL_FRAME_SIZE_MAX]; static uint8_t telemetryBufLen = 0; - -void srxlRxSendTelemetryDataDispatch(dispatchEntry_t *self); -static dispatchEntry_t srxlTelemetryDispatch = { .dispatch = srxlRxSendTelemetryDataDispatch}; #endif // Receive ISR callback @@ -109,60 +108,66 @@ static uint8_t spektrumFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) { UNUSED(rxRuntimeConfig); - if (!rcFrameComplete) { - return RX_FRAME_PENDING; - } +#if defined(USE_TELEMETRY_SRXL) + static timeUs_t telemetryFrameRequestedUs = 0; - rcFrameComplete = false; + timeUs_t currentTimeUs = micros(); +#endif + + uint8_t result = RX_FRAME_PENDING; + + if (rcFrameComplete) { + rcFrameComplete = false; #if defined(USE_SPEKTRUM_REAL_RSSI) || defined(USE_SPEKTRUM_FAKE_RSSI) - spektrumHandleRSSI(spekFrame); + spektrumHandleRSSI(spekFrame); #endif - // Get the VTX control bytes in a frame - uint32_t vtxControl = ((spekFrame[SPEKTRUM_VTX_CONTROL_1] << 24) | - (spekFrame[SPEKTRUM_VTX_CONTROL_2] << 16) | - (spekFrame[SPEKTRUM_VTX_CONTROL_3] << 8) | - (spekFrame[SPEKTRUM_VTX_CONTROL_4] << 0) ); + // Get the VTX control bytes in a frame + uint32_t vtxControl = ((spekFrame[SPEKTRUM_VTX_CONTROL_1] << 24) | + (spekFrame[SPEKTRUM_VTX_CONTROL_2] << 16) | + (spekFrame[SPEKTRUM_VTX_CONTROL_3] << 8) | + (spekFrame[SPEKTRUM_VTX_CONTROL_4] << 0) ); - int8_t spektrumRcDataSize; - // Handle VTX control frame. - if ((vtxControl & SPEKTRUM_VTX_CONTROL_FRAME_MASK) == SPEKTRUM_VTX_CONTROL_FRAME && - (spekFrame[2] & 0x80) == 0 ) { + int8_t spektrumRcDataSize; + // Handle VTX control frame. + if ((vtxControl & SPEKTRUM_VTX_CONTROL_FRAME_MASK) == SPEKTRUM_VTX_CONTROL_FRAME && + (spekFrame[2] & 0x80) == 0 ) { #if defined(USE_SPEKTRUM_VTX_CONTROL) && defined(USE_VTX_COMMON) - spektrumHandleVtxControl(vtxControl); + spektrumHandleVtxControl(vtxControl); #endif - spektrumRcDataSize = SPEK_FRAME_SIZE - SPEKTRUM_VTX_CONTROL_SIZE; - } else { - spektrumRcDataSize = SPEK_FRAME_SIZE; - } + spektrumRcDataSize = SPEK_FRAME_SIZE - SPEKTRUM_VTX_CONTROL_SIZE; + } else { + spektrumRcDataSize = SPEK_FRAME_SIZE; + } - // Get the RC control channel inputs - for (int b = 3; b < spektrumRcDataSize; b += 2) { - const uint8_t spekChannel = 0x0F & (spekFrame[b - 1] >> spek_chan_shift); - if (spekChannel < rxRuntimeConfigPtr->channelCount && spekChannel < SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT) { - if (rssi_channel == 0 || spekChannel != rssi_channel) { - spekChannelData[spekChannel] = ((uint32_t)(spekFrame[b - 1] & spek_chan_mask) << 8) + spekFrame[b]; + // Get the RC control channel inputs + for (int b = 3; b < spektrumRcDataSize; b += 2) { + const uint8_t spekChannel = 0x0F & (spekFrame[b - 1] >> spek_chan_shift); + if (spekChannel < rxRuntimeConfigPtr->channelCount && spekChannel < SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT) { + if (rssi_channel == 0 || spekChannel != rssi_channel) { + spekChannelData[spekChannel] = ((uint32_t)(spekFrame[b - 1] & spek_chan_mask) << 8) + spekFrame[b]; + } } } + +#if defined(USE_TELEMETRY_SRXL) + if (srxlEnabled && (spekFrame[2] & 0x80) == 0) { + telemetryFrameRequestedUs = currentTimeUs; + } +#endif + result = RX_FRAME_COMPLETE; } -#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SRXL) - if (srxlEnabled) { - /* Only dispatch for transmission if there are some data in buffer AND servos in phase 0 */ - if (telemetryBufLen && (spekFrame[2] & 0x80) == 0) { - dispatchAdd(&srxlTelemetryDispatch, SPEKTRUM_TELEMETRY_FRAME_DELAY); - } +#if defined(USE_TELEMETRY_SRXL) + if (telemetryBufLen && telemetryFrameRequestedUs && cmpTimeUs(currentTimeUs, telemetryFrameRequestedUs) >= SPEKTRUM_TELEMETRY_FRAME_DELAY_US) { + telemetryFrameRequestedUs = 0; - /* Trigger tm data collection if buffer has been sent and is empty, - so data will be ready to transmit in the next phase 0 */ - if (telemetryBufLen == 0) { - srxlCollectTelemetryNow(); - } + result = (result & ~RX_FRAME_PENDING) | RX_FRAME_PROCESSING_REQUIRED; } #endif - return RX_FRAME_COMPLETE; + return result; } static uint16_t spektrumReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) @@ -237,12 +242,12 @@ void spektrumBind(rxConfig_t *rxConfig) // Take care half-duplex case switch (rxConfig->serialrx_provider) { case SERIALRX_SRXL: -#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SRXL) - if (feature(FEATURE_TELEMETRY) && !telemetryCheckRxPortShared(portConfig)) { +#if defined(USE_TELEMETRY_SRXL) + if (featureIsEnabled(FEATURE_TELEMETRY) && !telemetryCheckRxPortShared(portConfig)) { bindPin = txPin; } break; -#endif // USE_TELEMETRY && USE_TELEMETRY_SRXL +#endif // USE_TELEMETRY_SRXL default: bindPin = rxConfig->halfDuplex ? txPin : rxPin; @@ -303,6 +308,37 @@ void spektrumBind(rxConfig_t *rxConfig) } #endif // USE_SPEKTRUM_BIND +#if defined(USE_TELEMETRY_SRXL) +static bool spektrumProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig) +{ + UNUSED(rxRuntimeConfig); + + // if there is telemetry data to write + if (telemetryBufLen > 0) { + serialWriteBuf(serialPort, telemetryBuf, telemetryBufLen); + telemetryBufLen = 0; // reset telemetry buffer + } + + return true; +} + +bool srxlTelemetryBufferEmpty() +{ + if (telemetryBufLen == 0) { + return true; + } else { + return false; + } +} + +void srxlRxWriteTelemetryData(const void *data, int len) +{ + len = MIN(len, (int)sizeof(telemetryBuf)); + memcpy(telemetryBuf, data, len); + telemetryBufLen = len; +} +#endif + bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) { rxRuntimeConfigPtr = rxRuntimeConfig; @@ -313,7 +349,7 @@ bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig } srxlEnabled = false; -#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SRXL) +#if defined(USE_TELEMETRY_SRXL) bool portShared = telemetryCheckRxPortShared(portConfig); #else bool portShared = false; @@ -321,8 +357,8 @@ bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig switch (rxConfig->serialrx_provider) { case SERIALRX_SRXL: -#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SRXL) - srxlEnabled = (feature(FEATURE_TELEMETRY) && !portShared); +#if defined(USE_TELEMETRY_SRXL) + srxlEnabled = (featureIsEnabled(FEATURE_TELEMETRY) && !portShared); FALLTHROUGH; #endif case SERIALRX_SPEKTRUM2048: @@ -347,6 +383,9 @@ bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig rxRuntimeConfig->rcReadRawFn = spektrumReadRawRC; rxRuntimeConfig->rcFrameStatusFn = spektrumFrameStatus; +#if defined(USE_TELEMETRY_SRXL) + rxRuntimeConfig->rcProcessFrameFn = spektrumProcessFrame; +#endif serialPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, @@ -356,7 +395,7 @@ bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig portShared || srxlEnabled ? MODE_RXTX : MODE_RX, (rxConfig->serialrx_inverted ? SERIAL_INVERTED : 0) | ((srxlEnabled || rxConfig->halfDuplex) ? SERIAL_BIDIR : 0) ); -#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SRXL) +#if defined(USE_TELEMETRY_SRXL) if (portShared) { telemetrySharedPort = serialPort; } @@ -367,31 +406,9 @@ bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig rssi_channel = 0; } - if (serialPort && srxlEnabled) { - dispatchEnable(); - } return serialPort != NULL; } -#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SRXL) -void srxlRxWriteTelemetryData(const void *data, int len) -{ - len = MIN(len, (int)sizeof(telemetryBuf)); - memcpy(telemetryBuf, data, len); - telemetryBufLen = len; -} - -void srxlRxSendTelemetryDataDispatch(dispatchEntry_t* self) -{ - UNUSED(self); - // if there is telemetry data to write - if (telemetryBufLen > 0) { - serialWriteBuf(serialPort, telemetryBuf, telemetryBufLen); - telemetryBufLen = 0; // reset telemetry buffer - } -} -#endif - bool srxlRxIsActive(void) { return serialPort != NULL; diff --git a/src/main/rx/spektrum.h b/src/main/rx/spektrum.h index 3825be93bc..ed41c8b147 100644 --- a/src/main/rx/spektrum.h +++ b/src/main/rx/spektrum.h @@ -32,7 +32,6 @@ #define SRXL_FRAME_SIZE_MAX (SPEK_FRAME_SIZE + SRXL_FRAME_OVERHEAD) #define SPEKTRUM_NEEDED_FRAME_INTERVAL 5000 -#define SPEKTRUM_TELEMETRY_FRAME_DELAY 1000 // Gap between received Rc frame and transmited TM frame, uS #define SPEKTRUM_BAUDRATE 115200 @@ -52,5 +51,6 @@ extern uint8_t rssi_channel; // Stores the RX RSSI channel. void spektrumBind(rxConfig_t *rxConfig); bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); +bool srxlTelemetryBufferEmpty(); void srxlRxWriteTelemetryData(const void *data, int len); bool srxlRxIsActive(void); diff --git a/src/main/rx/sumd.c b/src/main/rx/sumd.c index 7d4140df84..717d126ba8 100644 --- a/src/main/rx/sumd.c +++ b/src/main/rx/sumd.c @@ -24,10 +24,11 @@ #include "platform.h" -#ifdef USE_SERIAL_RX +#ifdef USE_SERIALRX_SUMD #include "common/crc.h" #include "common/utils.h" +#include "common/maths.h" #include "drivers/time.h" @@ -44,16 +45,18 @@ // driver for SUMD receiver using UART2 -// FIXME test support for more than 8 channels, should probably work up to 12 channels +// Support for SUMD and SUMD V3 +// Tested with 16 channels, SUMD supports up to 16(*), SUMD V3 up to 32 (MZ-32) channels, but limit to MAX_SUPPORTED_RC_CHANNEL_COUNT (currently 8, BF 3.4) +// * According to the original SUMD V1 documentation, SUMD V1 already supports up to 32 Channels?!? #define SUMD_SYNCBYTE 0xA8 -#define SUMD_MAX_CHANNEL 16 +#define SUMD_MAX_CHANNEL 32 #define SUMD_BUFFSIZE (SUMD_MAX_CHANNEL * 2 + 5) // 6 channels + 5 = 17 bytes for 6 channels #define SUMD_BAUDRATE 115200 static bool sumdFrameDone = false; -static uint16_t sumdChannels[SUMD_MAX_CHANNEL]; +static uint16_t sumdChannels[MAX_SUPPORTED_RC_CHANNEL_COUNT]; static uint16_t crc; static uint8_t sumd[SUMD_BUFFSIZE] = { 0, }; @@ -101,7 +104,8 @@ static void sumdDataReceive(uint16_t c, void *data) #define SUMD_BYTES_PER_CHANNEL 2 -#define SUMD_FRAME_STATE_OK 0x01 +#define SUMDV1_FRAME_STATE_OK 0x01 +#define SUMDV3_FRAME_STATE_OK 0x03 #define SUMD_FRAME_STATE_FAILSAFE 0x81 static uint8_t sumdFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) @@ -127,15 +131,16 @@ static uint8_t sumdFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) case SUMD_FRAME_STATE_FAILSAFE: frameStatus = RX_FRAME_COMPLETE | RX_FRAME_FAILSAFE; break; - case SUMD_FRAME_STATE_OK: + case SUMDV1_FRAME_STATE_OK: + case SUMDV3_FRAME_STATE_OK: frameStatus = RX_FRAME_COMPLETE; break; default: return frameStatus; } - if (sumdChannelCount > SUMD_MAX_CHANNEL) - sumdChannelCount = SUMD_MAX_CHANNEL; + if (sumdChannelCount > MAX_SUPPORTED_RC_CHANNEL_COUNT) + sumdChannelCount = MAX_SUPPORTED_RC_CHANNEL_COUNT; for (channelIndex = 0; channelIndex < sumdChannelCount; channelIndex++) { sumdChannels[channelIndex] = ( @@ -156,7 +161,7 @@ bool sumdInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) { UNUSED(rxConfig); - rxRuntimeConfig->channelCount = SUMD_MAX_CHANNEL; + rxRuntimeConfig->channelCount = MIN(SUMD_MAX_CHANNEL, MAX_SUPPORTED_RC_CHANNEL_COUNT); rxRuntimeConfig->rxRefreshRate = 11000; rxRuntimeConfig->rcReadRawFn = sumdReadRawRC; diff --git a/src/main/rx/sumh.c b/src/main/rx/sumh.c index c23f30e6a4..07af12aa20 100644 --- a/src/main/rx/sumh.c +++ b/src/main/rx/sumh.c @@ -30,7 +30,7 @@ #include "platform.h" -#ifdef USE_SERIAL_RX +#ifdef USE_SERIALRX_SUMH #include "common/utils.h" @@ -47,8 +47,6 @@ #include "rx/rx.h" #include "rx/sumh.h" -// driver for SUMH receiver using UART2 - #define SUMH_BAUDRATE 115200 #define SUMH_MAX_CHANNEL_COUNT 8 diff --git a/src/main/rx/xbus.c b/src/main/rx/xbus.c index 748f70b0fe..8ac66055b4 100644 --- a/src/main/rx/xbus.c +++ b/src/main/rx/xbus.c @@ -24,7 +24,7 @@ #include "platform.h" -#if defined(USE_SERIAL_RX) && defined(USE_SERIALRX_XBUS) +#ifdef USE_SERIALRX_XBUS #include "common/crc.h" diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c index d5ea17b74e..a3caf4a392 100644 --- a/src/main/scheduler/scheduler.c +++ b/src/main/scheduler/scheduler.c @@ -123,7 +123,7 @@ FAST_CODE cfTask_t *queueNext(void) return taskQueueArray[++taskQueuePos]; // guaranteed to be NULL at end of queue } -void taskSystem(timeUs_t currentTimeUs) +void taskSystemLoad(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); @@ -138,7 +138,7 @@ void taskSystem(timeUs_t currentTimeUs) #endif } -#ifndef SKIP_TASK_STATISTICS +#if defined(USE_TASK_STATISTICS) #define MOVING_SUM_COUNT 32 timeUs_t checkFuncMaxExecutionTime; timeUs_t checkFuncTotalExecutionTime; @@ -206,9 +206,7 @@ void schedulerSetCalulateTaskStatistics(bool calculateTaskStatisticsToUse) void schedulerResetTaskStatistics(cfTaskId_e taskId) { -#ifdef SKIP_TASK_STATISTICS - UNUSED(taskId); -#else +#if defined(USE_TASK_STATISTICS) if (taskId == TASK_SELF) { currentTask->movingSumExecutionTime = 0; currentTask->totalExecutionTime = 0; @@ -218,6 +216,21 @@ void schedulerResetTaskStatistics(cfTaskId_e taskId) cfTasks[taskId].totalExecutionTime = 0; cfTasks[taskId].maxExecutionTime = 0; } +#else + UNUSED(taskId); +#endif +} + +void schedulerResetTaskMaxExecutionTime(cfTaskId_e taskId) +{ +#if defined(USE_TASK_STATISTICS) + if (taskId == TASK_SELF) { + currentTask->maxExecutionTime = 0; + } else if (taskId < TASK_COUNT) { + cfTasks[taskId].maxExecutionTime = 0; + } +#else + UNUSED(taskId); #endif } @@ -266,7 +279,7 @@ FAST_CODE void scheduler(void) #if defined(SCHEDULER_DEBUG) DEBUG_SET(DEBUG_SCHEDULER, 3, micros() - currentTimeBeforeCheckFuncCall); #endif -#ifndef SKIP_TASK_STATISTICS +#if defined(USE_TASK_STATISTICS) if (calculateTaskStatistics) { const uint32_t checkFuncExecutionTime = micros() - currentTimeBeforeCheckFuncCall; checkFuncMovingSumExecutionTime += checkFuncExecutionTime - checkFuncMovingSumExecutionTime / MOVING_SUM_COUNT; @@ -315,9 +328,7 @@ FAST_CODE void scheduler(void) selectedTask->dynamicPriority = 0; // Execute task -#ifdef SKIP_TASK_STATISTICS - selectedTask->taskFunc(currentTimeUs); -#else +#if defined(USE_TASK_STATISTICS) if (calculateTaskStatistics) { const timeUs_t currentTimeBeforeTaskCall = micros(); selectedTask->taskFunc(currentTimeBeforeTaskCall); @@ -325,11 +336,12 @@ FAST_CODE void scheduler(void) selectedTask->movingSumExecutionTime += taskExecutionTime - selectedTask->movingSumExecutionTime / MOVING_SUM_COUNT; selectedTask->totalExecutionTime += taskExecutionTime; // time consumed by scheduler + task selectedTask->maxExecutionTime = MAX(selectedTask->maxExecutionTime, taskExecutionTime); - } else { + } else +#endif + { selectedTask->taskFunc(currentTimeUs); } -#endif #if defined(SCHEDULER_DEBUG) DEBUG_SET(DEBUG_SCHEDULER, 2, micros() - currentTimeUs - taskExecutionTime); // time spent in scheduler } else { diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index e55cfae51a..ff6509a6bf 100644 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -58,6 +58,7 @@ typedef struct { typedef enum { /* Actual tasks */ TASK_SYSTEM = 0, + TASK_MAIN, TASK_GYROPID, TASK_ACCEL, TASK_ATTITUDE, @@ -103,9 +104,6 @@ typedef enum { #ifdef USE_OSD TASK_OSD, #endif -#ifdef USE_OSD_SLAVE - TASK_OSD_SLAVE, -#endif #ifdef USE_BST TASK_BST_MASTER_PROCESS, #endif @@ -158,7 +156,7 @@ typedef struct { timeUs_t lastExecutedAt; // last time of invocation timeUs_t lastSignaledAt; // time of invocation event for event-driven tasks -#ifndef SKIP_TASK_STATISTICS +#if defined(USE_TASK_STATISTICS) // Statistics timeUs_t movingSumExecutionTime; // moving sum over 32 samples timeUs_t maxExecutionTime; @@ -176,10 +174,11 @@ void setTaskEnabled(cfTaskId_e taskId, bool newEnabledState); timeDelta_t getTaskDeltaTime(cfTaskId_e taskId); void schedulerSetCalulateTaskStatistics(bool calculateTaskStatistics); void schedulerResetTaskStatistics(cfTaskId_e taskId); +void schedulerResetTaskMaxExecutionTime(cfTaskId_e taskId); void schedulerInit(void); void scheduler(void); -void taskSystem(timeUs_t currentTime); +void taskSystemLoad(timeUs_t currentTime); #define LOAD_PERCENTAGE_ONE 100 diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index c1616fca0c..8970d66cec 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -157,9 +157,6 @@ retry: acc_params.useFifo = false; acc_params.dataRate = 800; // unused currently if (adxl345Detect(&acc_params, dev)) { -#ifdef ACC_ADXL345_ALIGN - dev->accAlign = ACC_ADXL345_ALIGN; -#endif accHardware = ACC_ADXL345; break; } @@ -169,9 +166,6 @@ retry: #ifdef USE_ACC_LSM303DLHC case ACC_LSM303DLHC: if (lsm303dlhcAccDetect(dev)) { -#ifdef ACC_LSM303DLHC_ALIGN - dev->accAlign = ACC_LSM303DLHC_ALIGN; -#endif accHardware = ACC_LSM303DLHC; break; } @@ -181,9 +175,6 @@ retry: #ifdef USE_ACC_MPU6050 case ACC_MPU6050: // MPU6050 if (mpu6050AccDetect(dev)) { -#ifdef ACC_MPU6050_ALIGN - dev->accAlign = ACC_MPU6050_ALIGN; -#endif accHardware = ACC_MPU6050; break; } @@ -193,9 +184,6 @@ retry: #ifdef USE_ACC_MMA8452 case ACC_MMA8452: // MMA8452 if (mma8452Detect(dev)) { -#ifdef ACC_MMA8452_ALIGN - dev->accAlign = ACC_MMA8452_ALIGN; -#endif accHardware = ACC_MMA8452; break; } @@ -205,9 +193,6 @@ retry: #ifdef USE_ACC_BMA280 case ACC_BMA280: // BMA280 if (bma280Detect(dev)) { -#ifdef ACC_BMA280_ALIGN - dev->accAlign = ACC_BMA280_ALIGN; -#endif accHardware = ACC_BMA280; break; } @@ -217,9 +202,6 @@ retry: #ifdef USE_ACC_SPI_MPU6000 case ACC_MPU6000: if (mpu6000SpiAccDetect(dev)) { -#ifdef ACC_MPU6000_ALIGN - dev->accAlign = ACC_MPU6000_ALIGN; -#endif accHardware = ACC_MPU6000; break; } @@ -229,9 +211,6 @@ retry: #ifdef USE_ACC_SPI_MPU9250 case ACC_MPU9250: if (mpu9250SpiAccDetect(dev)) { -#ifdef ACC_MPU9250_ALIGN - dev->accAlign = ACC_MPU9250_ALIGN; -#endif accHardware = ACC_MPU9250; break; } @@ -247,9 +226,6 @@ retry: if (mpu6500AccDetect(dev) || mpu6500SpiAccDetect(dev)) { #else if (mpu6500AccDetect(dev)) { -#endif -#ifdef ACC_MPU6500_ALIGN - dev->accAlign = ACC_MPU6500_ALIGN; #endif switch (dev->mpuDetectionResult.sensor) { case MPU_9250_SPI: @@ -276,9 +252,6 @@ retry: case ACC_ICM20649: if (icm20649SpiAccDetect(dev)) { accHardware = ACC_ICM20649; -#ifdef ACC_ICM20649_ALIGN - dev->accAlign = ACC_ICM20649_ALIGN; -#endif break; } FALLTHROUGH; @@ -288,9 +261,6 @@ retry: case ACC_ICM20689: if (icm20689SpiAccDetect(dev)) { accHardware = ACC_ICM20689; -#ifdef ACC_ICM20689_ALIGN - dev->accAlign = ACC_ICM20689_ALIGN; -#endif break; } FALLTHROUGH; @@ -300,9 +270,6 @@ retry: case ACC_BMI160: if (bmi160SpiAccDetect(dev)) { accHardware = ACC_BMI160; -#ifdef ACC_BMI160_ALIGN - dev->accAlign = ACC_BMI160_ALIGN; -#endif break; } FALLTHROUGH; @@ -347,14 +314,11 @@ bool accInit(uint32_t gyroSamplingInverval) acc.dev.mpuDetectionResult = *gyroMpuDetectionResult(); acc.dev.acc_high_fsr = accelerometerConfig()->acc_high_fsr; -#ifdef USE_DUAL_GYRO + acc.dev.accAlign = ACC_1_ALIGN; +#ifdef ACC_2_ALIGN if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_2) { acc.dev.accAlign = ACC_2_ALIGN; - } else { - acc.dev.accAlign = ACC_1_ALIGN; } -#else - acc.dev.accAlign = ALIGN_DEFAULT; #endif if (!accDetect(&acc.dev, accelerometerConfig()->acc_hardware)) { @@ -362,6 +326,7 @@ bool accInit(uint32_t gyroSamplingInverval) } acc.dev.acc_1G = 256; // set default acc.dev.initFn(&acc.dev); // driver initialisation + acc.dev.acc_1G_rec = 1.0f / acc.dev.acc_1G; // set the acc sampling interval according to the gyro sampling interval switch (gyroSamplingInverval) { // Switch statement kept in place to change acc sampling interval in the future case 500: @@ -527,7 +492,7 @@ void accUpdate(timeUs_t currentTimeUs, rollAndPitchTrims_t *rollAndPitchTrims) performAcclerationCalibration(rollAndPitchTrims); } - if (feature(FEATURE_INFLIGHT_ACC_CAL)) { + if (featureIsEnabled(FEATURE_INFLIGHT_ACC_CAL)) { performInflightAccelerationCalibration(rollAndPitchTrims); } diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index bde24f4f28..296dbbc652 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -105,6 +105,9 @@ PG_RESET_TEMPLATE(batteryConfig_t, batteryConfig, .batteryCapacity = 0, .currentMeterSource = DEFAULT_CURRENT_METER_SOURCE, + // cells + .forceBatteryCellCount = 0, //0 will be ignored + // warnings / alerts .useVBatAlerts = true, .useConsumptionAlerts = false, @@ -121,7 +124,7 @@ void batteryUpdateVoltage(timeUs_t currentTimeUs) switch (batteryConfig()->voltageMeterSource) { #ifdef USE_ESC_SENSOR case VOLTAGE_METER_ESC: - if (feature(FEATURE_ESC_SENSOR)) { + if (featureIsEnabled(FEATURE_ESC_SENSOR)) { voltageMeterESCRefresh(); voltageMeterESCReadCombined(&voltageMeter); } @@ -180,14 +183,18 @@ void batteryUpdatePresence(void) /* battery has just been connected - calculate cells, warning voltages and reset state */ - unsigned cells = (voltageMeter.filtered / batteryConfig()->vbatmaxcellvoltage) + 1; - if (cells > 8) { - // something is wrong, we expect 8 cells maximum (and autodetection will be problematic at 6+ cells) - cells = 8; - } consumptionState = voltageState = BATTERY_OK; - batteryCellCount = cells; + if (batteryConfig()->forceBatteryCellCount != 0) { + batteryCellCount = batteryConfig()->forceBatteryCellCount; + } else { + unsigned cells = (voltageMeter.filtered / batteryConfig()->vbatmaxcellvoltage) + 1; + if (cells > 8) { + // something is wrong, we expect 8 cells maximum (and autodetection will be problematic at 6+ cells) + cells = 8; + } + batteryCellCount = cells; + } batteryWarningVoltage = batteryCellCount * batteryConfig()->vbatwarningcellvoltage; batteryCriticalVoltage = batteryCellCount * batteryConfig()->vbatmincellvoltage; lowVoltageCutoff.percentage = 100; @@ -279,6 +286,16 @@ batteryState_e getBatteryState(void) return batteryState; } +batteryState_e getVoltageState(void) +{ + return voltageState; +} + +batteryState_e getConsumptionState(void) +{ + return consumptionState; +} + const char * const batteryStateStrings[] = {"OK", "WARNING", "CRITICAL", "NOT PRESENT", "INIT"}; const char * getBatteryStateString(void) @@ -389,7 +406,7 @@ void batteryUpdateCurrentMeter(timeUs_t currentTimeUs) case CURRENT_METER_VIRTUAL: { #ifdef USE_VIRTUAL_CURRENT_METER throttleStatus_e throttleStatus = calculateThrottleStatus(); - bool throttleLowAndMotorStop = (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)); + bool throttleLowAndMotorStop = (throttleStatus == THROTTLE_LOW && featureIsEnabled(FEATURE_MOTOR_STOP)); int32_t throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000; currentMeterVirtualRefresh(lastUpdateAt, ARMING_FLAG(ARMED), throttleLowAndMotorStop, throttleOffset); @@ -400,7 +417,7 @@ void batteryUpdateCurrentMeter(timeUs_t currentTimeUs) case CURRENT_METER_ESC: #ifdef USE_ESC_SENSOR - if (feature(FEATURE_ESC_SENSOR)) { + if (featureIsEnabled(FEATURE_ESC_SENSOR)) { currentMeterESCRefresh(lastUpdateAt); currentMeterESCReadCombined(¤tMeter); } diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index f65d5289f4..5f311ba42b 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -47,6 +47,8 @@ typedef struct batteryConfig_s { uint8_t vbathysteresis; // hysteresis for alarm, default 1 = 0.1V uint8_t vbatfullcellvoltage; // Cell voltage at which the battery is deemed to be "full" 0.1V units, default is 41 (4.1V) + + uint8_t forceBatteryCellCount; // number of cells in battery, used for overwriting auto-detected cell count if someone has issues with it. } batteryConfig_t; @@ -71,6 +73,8 @@ void batteryUpdateVoltage(timeUs_t currentTimeUs); void batteryUpdatePresence(void); batteryState_e getBatteryState(void); +batteryState_e getVoltageState(void); +batteryState_e getConsumptionState(void); const char * getBatteryStateString(void); void batteryUpdateStates(timeUs_t currentTimeUs); diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 2aaa4ff8af..e52ae497fe 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -38,6 +38,7 @@ #include "drivers/compass/compass_fake.h" #include "drivers/compass/compass_hmc5883l.h" #include "drivers/compass/compass_qmc5883l.h" +#include "drivers/compass/compass_lis3mdl.h" #include "drivers/io.h" #include "drivers/light_led.h" @@ -183,6 +184,22 @@ bool compassDetect(magDev_t *dev) #endif FALLTHROUGH; + case MAG_LIS3MDL: +#if defined(USE_MAG_LIS3MDL) + if (busdev->bustype == BUSTYPE_I2C) { + busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address; + } + + if (lis3mdlDetect(dev)) { +#ifdef MAG_LIS3MDL_ALIGN + dev->magAlign = MAG_LIS3MDL_ALIGN; +#endif + magHardware = MAG_LIS3MDL; + break; + } +#endif + FALLTHROUGH; + case MAG_QMC5883: #ifdef USE_MAG_QMC5883 if (busdev->bustype == BUSTYPE_I2C) { diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index 27946211d4..cd8480dc1b 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -34,7 +34,8 @@ typedef enum { MAG_HMC5883 = 2, MAG_AK8975 = 3, MAG_AK8963 = 4, - MAG_QMC5883 = 5 + MAG_QMC5883 = 5, + MAG_LIS3MDL = 6 } magSensor_e; typedef struct mag_s { diff --git a/src/main/sensors/current.c b/src/main/sensors/current.c index 4a8ec008b4..9261266cfc 100644 --- a/src/main/sensors/current.c +++ b/src/main/sensors/current.c @@ -140,7 +140,7 @@ currentMeterADCState_t currentMeterADCState; void currentMeterADCInit(void) { memset(¤tMeterADCState, 0, sizeof(currentMeterADCState_t)); - biquadFilterInitLPF(&adciBatFilter, IBAT_LPF_FREQ, 50000); //50HZ Update + biquadFilterInitLPF(&adciBatFilter, IBAT_LPF_FREQ, HZ_TO_INTERVAL_US(50)); } void currentMeterADCRefresh(int32_t lastUpdateAt) diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index 958fc43f53..8d6226223a 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -148,7 +148,7 @@ bool isEscSensorActive(void) escSensorData_t *getEscSensorData(uint8_t motorNumber) { - if (!feature(FEATURE_ESC_SENSOR)) { + if (!featureIsEnabled(FEATURE_ESC_SENSOR)) { return NULL; } diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 5d62ff66a5..84e658b57b 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -36,6 +36,7 @@ #include "pg/pg.h" #include "pg/pg_ids.h" +#include "pg/gyrodev.h" #include "drivers/accgyro/accgyro.h" #include "drivers/accgyro/accgyro_fake.h" @@ -72,12 +73,10 @@ #include "sensors/boardalignment.h" #include "sensors/gyro.h" +#ifdef USE_GYRO_DATA_ANALYSE #include "sensors/gyroanalyse.h" -#include "sensors/sensors.h" - -#ifdef USE_HARDWARE_REVISION_DETECTION -#include "hardware_revision.h" #endif +#include "sensors/sensors.h" #if ((FLASH_SIZE > 128) && (defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_MPU6500))) #define USE_GYRO_SLEW_LIMITER @@ -87,15 +86,23 @@ FAST_RAM_ZERO_INIT gyro_t gyro; static FAST_RAM_ZERO_INIT uint8_t gyroDebugMode; static uint8_t gyroToUse = 0; +static FAST_RAM_ZERO_INIT bool overflowDetected; #ifdef USE_GYRO_OVERFLOW_CHECK static FAST_RAM_ZERO_INIT uint8_t overflowAxisMask; #endif + +#ifdef USE_YAW_SPIN_RECOVERY +static FAST_RAM_ZERO_INIT bool yawSpinDetected; +#endif + static FAST_RAM_ZERO_INIT float accumulatedMeasurements[XYZ_AXIS_COUNT]; static FAST_RAM_ZERO_INIT float gyroPrevious[XYZ_AXIS_COUNT]; static FAST_RAM_ZERO_INIT timeUs_t accumulatedMeasurementTimeUs; static FAST_RAM_ZERO_INIT timeUs_t accumulationLastTimeSampledUs; +static FAST_RAM_ZERO_INIT int16_t gyroSensorTemperature; + static bool gyroHasOverflowProtection = true; typedef struct gyroCalibration_s { @@ -140,10 +147,17 @@ typedef struct gyroSensor_s { timeUs_t yawSpinTimeUs; bool yawSpinDetected; #endif // USE_YAW_SPIN_RECOVERY + +#ifdef USE_GYRO_DATA_ANALYSE +#define DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 350 +#define DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ 300 + gyroAnalyseState_t gyroAnalyseState; +#endif + } gyroSensor_t; STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT gyroSensor_t gyroSensor1; -#ifdef USE_DUAL_GYRO +#ifdef USE_MULTI_GYRO STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT gyroSensor_t gyroSensor2; #endif @@ -163,101 +177,84 @@ static void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int typ || defined(USE_GYRO_SPI_ICM20689) #define GYRO_SYNC_DENOM_DEFAULT 1 #else -#define GYRO_SYNC_DENOM_DEFAULT 4 +#define GYRO_SYNC_DENOM_DEFAULT 3 #endif #define GYRO_OVERFLOW_TRIGGER_THRESHOLD 31980 // 97.5% full scale (1950dps for 2000dps gyro) #define GYRO_OVERFLOW_RESET_THRESHOLD 30340 // 92.5% full scale (1850dps for 2000dps gyro) -PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 4); +PG_REGISTER_WITH_RESET_FN(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 5); #ifndef GYRO_CONFIG_USE_GYRO_DEFAULT #define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1 #endif -PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, - .gyro_align = ALIGN_DEFAULT, - .gyroCalibrationDuration = 125, // 1.25 seconds - .gyroMovementCalibrationThreshold = 48, - .gyro_sync_denom = GYRO_SYNC_DENOM_DEFAULT, - .gyro_hardware_lpf = GYRO_HARDWARE_LPF_NORMAL, - .gyro_32khz_hardware_lpf = GYRO_32KHZ_HARDWARE_LPF_NORMAL, - .gyro_lowpass_type = FILTER_PT1, - .gyro_lowpass_hz = 100, - .gyro_lowpass2_type = FILTER_PT1, - .gyro_lowpass2_hz = 300, - .gyro_high_fsr = false, - .gyro_use_32khz = false, - .gyro_to_use = GYRO_CONFIG_USE_GYRO_DEFAULT, - .gyro_soft_notch_hz_1 = 0, - .gyro_soft_notch_cutoff_1 = 0, - .gyro_soft_notch_hz_2 = 0, - .gyro_soft_notch_cutoff_2 = 0, - .checkOverflow = GYRO_OVERFLOW_CHECK_ALL_AXES, - .gyro_offset_yaw = 0, - .yaw_spin_recovery = true, - .yaw_spin_threshold = 1950, -); +void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig) +{ + gyroConfig->gyro_align = ALIGN_DEFAULT; + gyroConfig->gyroCalibrationDuration = 125; // 1.25 seconds + gyroConfig->gyroMovementCalibrationThreshold = 48; + gyroConfig->gyro_sync_denom = GYRO_SYNC_DENOM_DEFAULT; + gyroConfig->gyro_hardware_lpf = GYRO_HARDWARE_LPF_NORMAL; + gyroConfig->gyro_32khz_hardware_lpf = GYRO_32KHZ_HARDWARE_LPF_NORMAL; + gyroConfig->gyro_lowpass_type = FILTER_PT1; + gyroConfig->gyro_lowpass_hz = 100; + gyroConfig->gyro_lowpass2_type = FILTER_PT1; + gyroConfig->gyro_lowpass2_hz = 300; + gyroConfig->gyro_high_fsr = false; + gyroConfig->gyro_use_32khz = false; + gyroConfig->gyro_to_use = GYRO_CONFIG_USE_GYRO_DEFAULT; + gyroConfig->gyro_soft_notch_hz_1 = 0; + gyroConfig->gyro_soft_notch_cutoff_1 = 0; + gyroConfig->gyro_soft_notch_hz_2 = 0; + gyroConfig->gyro_soft_notch_cutoff_2 = 0; + gyroConfig->checkOverflow = GYRO_OVERFLOW_CHECK_ALL_AXES; + gyroConfig->gyro_offset_yaw = 0; + gyroConfig->yaw_spin_recovery = true; + gyroConfig->yaw_spin_threshold = 1950; + gyroConfig->dyn_filter_width_percent = 40; + gyroConfig->dyn_fft_location = DYN_FFT_AFTER_STATIC_FILTERS; + gyroConfig->dyn_filter_range = DYN_FILTER_RANGE_MEDIUM; + gyroConfig->dyn_lpf_gyro_max_hz = 400; + gyroConfig->dyn_lpf_gyro_idle = 20; +#ifdef USE_DYN_LPF + gyroConfig->gyro_lowpass_hz = 120; +#endif +} +#ifdef USE_MULTI_GYRO +#define ACTIVE_GYRO ((gyroToUse == GYRO_CONFIG_USE_GYRO_2) ? &gyroSensor2 : &gyroSensor1) +#else +#define ACTIVE_GYRO (&gyroSensor1) +#endif const busDevice_t *gyroSensorBus(void) { -#ifdef USE_DUAL_GYRO - if (gyroToUse == GYRO_CONFIG_USE_GYRO_2) { - return &gyroSensor2.gyroDev.bus; - } else { - return &gyroSensor1.gyroDev.bus; - } -#else - return &gyroSensor1.gyroDev.bus; -#endif + return &ACTIVE_GYRO->gyroDev.bus; } #ifdef USE_GYRO_REGISTER_DUMP const busDevice_t *gyroSensorBusByDevice(uint8_t whichSensor) { -#ifdef USE_DUAL_GYRO +#ifdef USE_MULTI_GYRO if (whichSensor == GYRO_CONFIG_USE_GYRO_2) { return &gyroSensor2.gyroDev.bus; - } else { - return &gyroSensor1.gyroDev.bus; } #else UNUSED(whichSensor); - return &gyroSensor1.gyroDev.bus; #endif + return &gyroSensor1.gyroDev.bus; } #endif // USE_GYRO_REGISTER_DUMP -const mpuConfiguration_t *gyroMpuConfiguration(void) -{ -#ifdef USE_DUAL_GYRO - if (gyroToUse == GYRO_CONFIG_USE_GYRO_2) { - return &gyroSensor2.gyroDev.mpuConfiguration; - } else { - return &gyroSensor1.gyroDev.mpuConfiguration; - } -#else - return &gyroSensor1.gyroDev.mpuConfiguration; -#endif -} - const mpuDetectionResult_t *gyroMpuDetectionResult(void) { -#ifdef USE_DUAL_GYRO - if (gyroToUse == GYRO_CONFIG_USE_GYRO_2) { - return &gyroSensor2.gyroDev.mpuDetectionResult; - } else { - return &gyroSensor1.gyroDev.mpuDetectionResult; - } -#else - return &gyroSensor1.gyroDev.mpuDetectionResult; -#endif + return &ACTIVE_GYRO->gyroDev.mpuDetectionResult; } -STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev) +STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev) { - gyroSensor_e gyroHardware = GYRO_DEFAULT; + gyroHardware_e gyroHardware = GYRO_DEFAULT; switch (gyroHardware) { case GYRO_DEFAULT: @@ -267,9 +264,6 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev) case GYRO_MPU6050: if (mpu6050GyroDetect(dev)) { gyroHardware = GYRO_MPU6050; -#ifdef GYRO_MPU6050_ALIGN - dev->gyroAlign = GYRO_MPU6050_ALIGN; -#endif break; } FALLTHROUGH; @@ -279,9 +273,6 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev) case GYRO_L3G4200D: if (l3g4200dDetect(dev)) { gyroHardware = GYRO_L3G4200D; -#ifdef GYRO_L3G4200D_ALIGN - dev->gyroAlign = GYRO_L3G4200D_ALIGN; -#endif break; } FALLTHROUGH; @@ -291,9 +282,6 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev) case GYRO_MPU3050: if (mpu3050Detect(dev)) { gyroHardware = GYRO_MPU3050; -#ifdef GYRO_MPU3050_ALIGN - dev->gyroAlign = GYRO_MPU3050_ALIGN; -#endif break; } FALLTHROUGH; @@ -303,9 +291,6 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev) case GYRO_L3GD20: if (l3gd20Detect(dev)) { gyroHardware = GYRO_L3GD20; -#ifdef GYRO_L3GD20_ALIGN - dev->gyroAlign = GYRO_L3GD20_ALIGN; -#endif break; } FALLTHROUGH; @@ -315,9 +300,6 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev) case GYRO_MPU6000: if (mpu6000SpiGyroDetect(dev)) { gyroHardware = GYRO_MPU6000; -#ifdef GYRO_MPU6000_ALIGN - dev->gyroAlign = GYRO_MPU6000_ALIGN; -#endif break; } FALLTHROUGH; @@ -349,9 +331,6 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev) default: gyroHardware = GYRO_MPU6500; } -#ifdef GYRO_MPU6500_ALIGN - dev->gyroAlign = GYRO_MPU6500_ALIGN; -#endif break; } FALLTHROUGH; @@ -361,9 +340,6 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev) case GYRO_MPU9250: if (mpu9250SpiGyroDetect(dev)) { gyroHardware = GYRO_MPU9250; -#ifdef GYRO_MPU9250_ALIGN - dev->gyroAlign = GYRO_MPU9250_ALIGN; -#endif break; } FALLTHROUGH; @@ -373,9 +349,6 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev) case GYRO_ICM20649: if (icm20649SpiGyroDetect(dev)) { gyroHardware = GYRO_ICM20649; -#ifdef GYRO_ICM20649_ALIGN - dev->gyroAlign = GYRO_ICM20649_ALIGN; -#endif break; } FALLTHROUGH; @@ -385,9 +358,6 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev) case GYRO_ICM20689: if (icm20689SpiGyroDetect(dev)) { gyroHardware = GYRO_ICM20689; -#ifdef GYRO_ICM20689_ALIGN - dev->gyroAlign = GYRO_ICM20689_ALIGN; -#endif break; } FALLTHROUGH; @@ -397,9 +367,6 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev) case GYRO_BMI160: if (bmi160SpiGyroDetect(dev)) { gyroHardware = GYRO_BMI160; -#ifdef GYRO_BMI160_ALIGN - dev->gyroAlign = GYRO_BMI160_ALIGN; -#endif break; } FALLTHROUGH; @@ -427,18 +394,19 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev) return gyroHardware; } -static bool gyroInitSensor(gyroSensor_t *gyroSensor) +static bool gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *config) { gyroSensor->gyroDev.gyro_high_fsr = gyroConfig()->gyro_high_fsr; + gyroSensor->gyroDev.gyroAlign = config->align; + gyroSensor->gyroDev.mpuIntExtiTag = config->extiTag; #if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) \ || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) || defined(USE_GYRO_SPI_ICM20689) - mpuDetect(&gyroSensor->gyroDev); - mpuResetFn = gyroSensor->gyroDev.mpuConfiguration.resetFn; // must be set after mpuDetect + mpuDetect(&gyroSensor->gyroDev, config); #endif - const gyroSensor_e gyroHardware = gyroDetect(&gyroSensor->gyroDev); + const gyroHardware_e gyroHardware = gyroDetect(&gyroSensor->gyroDev); gyroSensor->gyroDev.gyroHardware = gyroHardware; if (gyroHardware == GYRO_NONE) { return false; @@ -501,8 +469,10 @@ static bool gyroInitSensor(gyroSensor_t *gyroSensor) gyroInitSensorFilters(gyroSensor); #ifdef USE_GYRO_DATA_ANALYSE - gyroDataAnalyseInit(gyro.targetLooptime); + gyroDataAnalyseStateInit(&gyroSensor->gyroAnalyseState, gyro.targetLooptime); + #endif + return true; } @@ -520,6 +490,7 @@ bool gyroInit(void) switch (debugMode) { case DEBUG_FFT: + case DEBUG_FFT_FREQ: case DEBUG_GYRO_RAW: case DEBUG_GYRO_SCALED: case DEBUG_GYRO_FILTERED: @@ -533,82 +504,25 @@ bool gyroInit(void) firstArmingCalibrationWasStarted = false; bool ret = false; - memset(&gyro, 0, sizeof(gyro)); gyroToUse = gyroConfig()->gyro_to_use; -#if defined(USE_DUAL_GYRO) && defined(GYRO_1_CS_PIN) if (gyroToUse == GYRO_CONFIG_USE_GYRO_1 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) { - gyroSensor1.gyroDev.bus.busdev_u.spi.csnPin = IOGetByTag(IO_TAG(GYRO_1_CS_PIN)); - IOInit(gyroSensor1.gyroDev.bus.busdev_u.spi.csnPin, OWNER_MPU_CS, RESOURCE_INDEX(0)); - IOHi(gyroSensor1.gyroDev.bus.busdev_u.spi.csnPin); // Ensure device is disabled, important when two devices are on the same bus. - IOConfigGPIO(gyroSensor1.gyroDev.bus.busdev_u.spi.csnPin, SPI_IO_CS_CFG); - } -#endif - -#if defined(USE_DUAL_GYRO) && defined(GYRO_2_CS_PIN) - if (gyroToUse == GYRO_CONFIG_USE_GYRO_2 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) { - gyroSensor2.gyroDev.bus.busdev_u.spi.csnPin = IOGetByTag(IO_TAG(GYRO_2_CS_PIN)); - IOInit(gyroSensor2.gyroDev.bus.busdev_u.spi.csnPin, OWNER_MPU_CS, RESOURCE_INDEX(1)); - IOHi(gyroSensor2.gyroDev.bus.busdev_u.spi.csnPin); // Ensure device is disabled, important when two devices are on the same bus. - IOConfigGPIO(gyroSensor2.gyroDev.bus.busdev_u.spi.csnPin, SPI_IO_CS_CFG); - } -#endif - - gyroSensor1.gyroDev.gyroAlign = ALIGN_DEFAULT; - -#if defined(GYRO_1_EXTI_PIN) - gyroSensor1.gyroDev.mpuIntExtiTag = IO_TAG(GYRO_1_EXTI_PIN); -#elif defined(MPU_INT_EXTI) - gyroSensor1.gyroDev.mpuIntExtiTag = IO_TAG(MPU_INT_EXTI); -#elif defined(USE_HARDWARE_REVISION_DETECTION) - gyroSensor1.gyroDev.mpuIntExtiTag = selectMPUIntExtiConfigByHardwareRevision(); -#else - gyroSensor1.gyroDev.mpuIntExtiTag = IO_TAG_NONE; -#endif // GYRO_1_EXTI_PIN -#ifdef USE_DUAL_GYRO -#ifdef GYRO_1_ALIGN - gyroSensor1.gyroDev.gyroAlign = GYRO_1_ALIGN; -#endif - gyroSensor1.gyroDev.bus.bustype = BUSTYPE_SPI; - spiBusSetInstance(&gyroSensor1.gyroDev.bus, GYRO_1_SPI_INSTANCE); - if (gyroToUse == GYRO_CONFIG_USE_GYRO_1 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) { - ret = gyroInitSensor(&gyroSensor1); + ret = gyroInitSensor(&gyroSensor1, gyroDeviceConfig(0)); if (!ret) { return false; // TODO handle failure of first gyro detection better. - Perhaps update the config to use second gyro then indicate a new failure mode and reboot. } gyroHasOverflowProtection = gyroHasOverflowProtection && gyroSensor1.gyroDev.gyroHasOverflowProtection; } -#else // USE_DUAL_GYRO - ret = gyroInitSensor(&gyroSensor1); - gyroHasOverflowProtection = gyroHasOverflowProtection && gyroSensor1.gyroDev.gyroHasOverflowProtection; -#endif // USE_DUAL_GYRO -#ifdef USE_DUAL_GYRO - - gyroSensor2.gyroDev.gyroAlign = ALIGN_DEFAULT; - -#if defined(GYRO_2_EXTI_PIN) - gyroSensor2.gyroDev.mpuIntExtiTag = IO_TAG(GYRO_2_EXTI_PIN); -#elif defined(USE_HARDWARE_REVISION_DETECTION) - gyroSensor2.gyroDev.mpuIntExtiTag = selectMPUIntExtiConfigByHardwareRevision(); -#else - gyroSensor2.gyroDev.mpuIntExtiTag = IO_TAG_NONE; -#endif // GYRO_2_EXTI_PIN -#ifdef GYRO_2_ALIGN - gyroSensor2.gyroDev.gyroAlign = GYRO_2_ALIGN; -#endif - gyroSensor2.gyroDev.bus.bustype = BUSTYPE_SPI; - spiBusSetInstance(&gyroSensor2.gyroDev.bus, GYRO_2_SPI_INSTANCE); +#ifdef USE_MULTI_GYRO if (gyroToUse == GYRO_CONFIG_USE_GYRO_2 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) { - ret = gyroInitSensor(&gyroSensor2); + ret = gyroInitSensor(&gyroSensor2, gyroDeviceConfig(1)); if (!ret) { return false; // TODO handle failure of second gyro detection better. - Perhaps update the config to use first gyro then indicate a new failure mode and reboot. } gyroHasOverflowProtection = gyroHasOverflowProtection && gyroSensor2.gyroDev.gyroHasOverflowProtection; } -#endif // USE_DUAL_GYRO -#ifdef USE_DUAL_GYRO // Only allow using both gyros simultaneously if they are the same hardware type. // If the user selected "BOTH" and they are not the same type, then reset to using only the first gyro. if (gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) { @@ -620,10 +534,44 @@ bool gyroInit(void) } } -#endif // USE_DUAL_GYRO +#endif + return ret; } +#ifdef USE_DYN_LPF +static FAST_RAM int8_t dynLpfFilter = DYN_LPF_NONE; +static FAST_RAM_ZERO_INIT float dynLpfIdle; +static FAST_RAM_ZERO_INIT float dynLpfIdlePoint; +static FAST_RAM_ZERO_INIT float dynLpfInvIdlePointScaled; +static FAST_RAM_ZERO_INIT uint16_t dynLpfMin; + +static void dynLpfFilterInit() +{ + if (gyroConfig()->dyn_lpf_gyro_idle > 0 && gyroConfig()->dyn_lpf_gyro_max_hz > 0) { + if (gyroConfig()->gyro_lowpass_hz > 0 ) { + dynLpfMin = gyroConfig()->gyro_lowpass_hz; + switch (gyroConfig()->gyro_lowpass_type) { + case FILTER_PT1: + dynLpfFilter = DYN_LPF_PT1; + break; + case FILTER_BIQUAD: + dynLpfFilter = DYN_LPF_BIQUAD; + break; + default: + dynLpfFilter = DYN_LPF_NONE; + break; + } + } + } else { + dynLpfFilter = DYN_LPF_NONE; + } + dynLpfIdle = gyroConfig()->dyn_lpf_gyro_idle / 100.0f; + dynLpfIdlePoint = (dynLpfIdle - (dynLpfIdle * dynLpfIdle * dynLpfIdle) / 3.0f) * 1.5f; + dynLpfInvIdlePointScaled = 1 / (1 - dynLpfIdlePoint) * (gyroConfig()->dyn_lpf_gyro_max_hz - dynLpfMin); +} +#endif + void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint16_t lpfHz) { filterApplyFnPtr *lowpassFilterApplyFn; @@ -653,7 +601,7 @@ void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint // Dereference the pointer to null before checking valid cutoff and filter // type. It will be overridden for positive cases. - *lowpassFilterApplyFn = &nullFilterApply; + *lowpassFilterApplyFn = nullFilterApply; // If lowpass cutoff has been specified and is less than the Nyquist frequency if (lpfHz && lpfHz <= gyroFrequencyNyquist) { @@ -665,7 +613,11 @@ void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint } break; case FILTER_BIQUAD: +#ifdef USE_DYN_LPF + *lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApplyDF1; +#else *lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApply; +#endif for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { biquadFilterInitLPF(&lowpassFilter[axis].biquadFilterState, lpfHz, gyro.targetLooptime); } @@ -730,7 +682,7 @@ static void gyroInitFilterNotch2(gyroSensor_t *gyroSensor, uint16_t notchHz, uin #ifdef USE_GYRO_DATA_ANALYSE static bool isDynamicFilterActive(void) { - return feature(FEATURE_DYNAMIC_FILTER); + return featureIsEnabled(FEATURE_DYNAMIC_FILTER); } static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor) @@ -739,9 +691,9 @@ static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor) if (isDynamicFilterActive()) { gyroSensor->notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2 - const float notchQ = filterGetNotchQ(400, 390); //just any init value + const float notchQ = filterGetNotchQ(DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ); // any defaults OK here for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - biquadFilterInit(&gyroSensor->notchFilterDyn[axis], 400, gyro.targetLooptime, notchQ, FILTER_NOTCH); + biquadFilterInit(&gyroSensor->notchFilterDyn[axis], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, gyro.targetLooptime, notchQ, FILTER_NOTCH); } } } @@ -773,12 +725,15 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor) #ifdef USE_GYRO_DATA_ANALYSE gyroInitFilterDynamicNotch(gyroSensor); #endif +#ifdef USE_DYN_LPF + dynLpfFilterInit(); +#endif } void gyroInitFilters(void) { gyroInitSensorFilters(&gyroSensor1); -#ifdef USE_DUAL_GYRO +#ifdef USE_MULTI_GYRO gyroInitSensorFilters(&gyroSensor2); #endif } @@ -790,22 +745,20 @@ FAST_CODE bool isGyroSensorCalibrationComplete(const gyroSensor_t *gyroSensor) FAST_CODE bool isGyroCalibrationComplete(void) { -#ifdef USE_DUAL_GYRO switch (gyroToUse) { default: case GYRO_CONFIG_USE_GYRO_1: { return isGyroSensorCalibrationComplete(&gyroSensor1); } +#ifdef USE_MULTI_GYRO case GYRO_CONFIG_USE_GYRO_2: { return isGyroSensorCalibrationComplete(&gyroSensor2); } case GYRO_CONFIG_USE_GYRO_BOTH: { return isGyroSensorCalibrationComplete(&gyroSensor1) && isGyroSensorCalibrationComplete(&gyroSensor2); } - } -#else - return isGyroSensorCalibrationComplete(&gyroSensor1); #endif + } } static bool isOnFinalGyroCalibrationCycle(const gyroCalibration_t *gyroCalibration) @@ -832,7 +785,7 @@ void gyroStartCalibration(bool isFirstArmingCalibration) { if (!(isFirstArmingCalibration && firstArmingCalibrationWasStarted)) { gyroSetCalibrationCycles(&gyroSensor1); -#ifdef USE_DUAL_GYRO +#ifdef USE_MULTI_GYRO gyroSetCalibrationCycles(&gyroSensor2); #endif @@ -916,9 +869,9 @@ FAST_CODE int32_t gyroSlewLimiter(gyroSensor_t *gyroSensor, int axis) static FAST_CODE_NOINLINE void handleOverflow(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs) { const float gyroOverflowResetRate = GYRO_OVERFLOW_RESET_THRESHOLD * gyroSensor->gyroDev.scale; - if ((abs(gyro.gyroADCf[X]) < gyroOverflowResetRate) - && (abs(gyro.gyroADCf[Y]) < gyroOverflowResetRate) - && (abs(gyro.gyroADCf[Z]) < gyroOverflowResetRate)) { + if ((abs(gyroSensor->gyroDev.gyroADCf[X]) < gyroOverflowResetRate) + && (abs(gyroSensor->gyroDev.gyroADCf[Y]) < gyroOverflowResetRate) + && (abs(gyroSensor->gyroDev.gyroADCf[Z]) < gyroOverflowResetRate)) { // if we have 50ms of consecutive OK gyro vales, then assume yaw readings are OK again and reset overflowDetected // reset requires good OK values on all axes if (cmpTimeUs(currentTimeUs, gyroSensor->overflowTimeUs) > 50000) { @@ -943,13 +896,13 @@ static FAST_CODE void checkForOverflow(gyroSensor_t *gyroSensor, timeUs_t curren // check for overflow in the axes set in overflowAxisMask gyroOverflow_e overflowCheck = GYRO_OVERFLOW_NONE; const float gyroOverflowTriggerRate = GYRO_OVERFLOW_TRIGGER_THRESHOLD * gyroSensor->gyroDev.scale; - if (abs(gyro.gyroADCf[X]) > gyroOverflowTriggerRate) { + if (abs(gyroSensor->gyroDev.gyroADCf[X]) > gyroOverflowTriggerRate) { overflowCheck |= GYRO_OVERFLOW_X; } - if (abs(gyro.gyroADCf[Y]) > gyroOverflowTriggerRate) { + if (abs(gyroSensor->gyroDev.gyroADCf[Y]) > gyroOverflowTriggerRate) { overflowCheck |= GYRO_OVERFLOW_Y; } - if (abs(gyro.gyroADCf[Z]) > gyroOverflowTriggerRate) { + if (abs(gyroSensor->gyroDev.gyroADCf[Z]) > gyroOverflowTriggerRate) { overflowCheck |= GYRO_OVERFLOW_Z; } if (overflowCheck & overflowAxisMask) { @@ -968,7 +921,7 @@ static FAST_CODE void checkForOverflow(gyroSensor_t *gyroSensor, timeUs_t curren static FAST_CODE_NOINLINE void handleYawSpin(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs) { const float yawSpinResetRate = gyroConfig()->yaw_spin_threshold - 100.0f; - if (abs(gyro.gyroADCf[Z]) < yawSpinResetRate) { + if (abs(gyroSensor->gyroDev.gyroADCf[Z]) < yawSpinResetRate) { // testing whether 20ms of consecutive OK gyro yaw values is enough if (cmpTimeUs(currentTimeUs, gyroSensor->yawSpinTimeUs) > 20000) { gyroSensor->yawSpinDetected = false; @@ -994,7 +947,7 @@ static FAST_CODE void checkForYawSpin(gyroSensor_t *gyroSensor, timeUs_t current } else { #ifndef SIMULATOR_BUILD // check for spin on yaw axis only - if (abs(gyro.gyroADCf[Z]) > gyroConfig()->yaw_spin_threshold) { + if (abs(gyroSensor->gyroDev.gyroADCf[Z]) > gyroConfig()->yaw_spin_threshold) { gyroSensor->yawSpinDetected = true; gyroSensor->yawSpinTimeUs = currentTimeUs; } @@ -1003,6 +956,18 @@ static FAST_CODE void checkForYawSpin(gyroSensor_t *gyroSensor, timeUs_t current } #endif // USE_YAW_SPIN_RECOVERY +#define GYRO_FILTER_FUNCTION_NAME filterGyro +#define GYRO_FILTER_DEBUG_SET(mode, index, value) { UNUSED(mode); UNUSED(index); UNUSED(value); } +#include "gyro_filter_impl.h" +#undef GYRO_FILTER_FUNCTION_NAME +#undef GYRO_FILTER_DEBUG_SET + +#define GYRO_FILTER_FUNCTION_NAME filterGyroDebug +#define GYRO_FILTER_DEBUG_SET DEBUG_SET +#include "gyro_filter_impl.h" +#undef GYRO_FILTER_FUNCTION_NAME +#undef GYRO_FILTER_DEBUG_SET + static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs) { if (!gyroSensor->gyroDev.readFn(&gyroSensor->gyroDev)) { @@ -1030,15 +995,11 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens return; } -#ifdef USE_GYRO_DATA_ANALYSE - if (isDynamicFilterActive()) { - gyroDataAnalyse(&gyroSensor->gyroDev, gyroSensor->notchFilterDyn); + if (gyroDebugMode == DEBUG_NONE) { + filterGyro(gyroSensor); + } else { + filterGyroDebug(gyroSensor); } -#endif - - const timeDelta_t sampleDeltaUs = currentTimeUs - accumulationLastTimeSampledUs; - accumulationLastTimeSampledUs = currentTimeUs; - accumulatedMeasurementTimeUs += sampleDeltaUs; #ifdef USE_GYRO_OVERFLOW_CHECK if (gyroConfig()->checkOverflow && !gyroHasOverflowProtection) { @@ -1052,67 +1013,23 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens } #endif - if (gyroDebugMode == DEBUG_NONE) { - for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - // NOTE: this branch optimized for when there is no gyro debugging, ensure it is kept in step with non-optimized branch - float gyroADCf = gyroSensor->gyroDev.gyroADC[axis] * gyroSensor->gyroDev.scale; - #ifdef USE_GYRO_DATA_ANALYSE - gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis], gyroADCf); -#endif - gyroADCf = gyroSensor->notchFilter1ApplyFn((filter_t *)&gyroSensor->notchFilter1[axis], gyroADCf); - gyroADCf = gyroSensor->notchFilter2ApplyFn((filter_t *)&gyroSensor->notchFilter2[axis], gyroADCf); - gyroADCf = gyroSensor->lowpassFilterApplyFn((filter_t *)&gyroSensor->lowpassFilter[axis], gyroADCf); - gyroADCf = gyroSensor->lowpass2FilterApplyFn((filter_t *)&gyroSensor->lowpass2Filter[axis], gyroADCf); - gyroSensor->gyroDev.gyroADCf[axis] = gyroADCf; - - if (!gyroSensor->overflowDetected) { - // integrate using trapezium rule to avoid bias - accumulatedMeasurements[axis] += 0.5f * (gyroPrevious[axis] + gyroADCf) * sampleDeltaUs; - gyroPrevious[axis] = gyroADCf; - } - } - } else { - for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - DEBUG_SET(DEBUG_GYRO_RAW, axis, gyroSensor->gyroDev.gyroADCRaw[axis]); - // scale gyro output to degrees per second - float gyroADCf = gyroSensor->gyroDev.gyroADC[axis] * gyroSensor->gyroDev.scale; - // DEBUG_GYRO_SCALED records the unfiltered, scaled gyro output - DEBUG_SET(DEBUG_GYRO_SCALED, axis, lrintf(gyroADCf)); - -#ifdef USE_GYRO_DATA_ANALYSE - // apply dynamic notch filter - if (isDynamicFilterActive()) { - if (axis == X) { - DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf)); // store raw data - } - gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis], gyroADCf); - if (axis == X) { - DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf)); // store data after dynamic notch - } - } -#endif - // apply static notch filters and software lowpass filters - gyroADCf = gyroSensor->notchFilter1ApplyFn((filter_t *)&gyroSensor->notchFilter1[axis], gyroADCf); - gyroADCf = gyroSensor->notchFilter2ApplyFn((filter_t *)&gyroSensor->notchFilter2[axis], gyroADCf); - gyroADCf = gyroSensor->lowpassFilterApplyFn((filter_t *)&gyroSensor->lowpassFilter[axis], gyroADCf); - gyroADCf = gyroSensor->lowpass2FilterApplyFn((filter_t *)&gyroSensor->lowpass2Filter[axis], gyroADCf); - // DEBUG_GYRO_FILTERED records the scaled, filtered, after all software filtering has been applied. - DEBUG_SET(DEBUG_GYRO_FILTERED, axis, lrintf(gyroADCf)); - - gyroSensor->gyroDev.gyroADCf[axis] = gyroADCf; - if (!gyroSensor->overflowDetected) { - // integrate using trapezium rule to avoid bias - accumulatedMeasurements[axis] += 0.5f * (gyroPrevious[axis] + gyroADCf) * sampleDeltaUs; - gyroPrevious[axis] = gyroADCf; - } - } + if (isDynamicFilterActive()) { + gyroDataAnalyse(&gyroSensor->gyroAnalyseState, gyroSensor->notchFilterDyn); } +#endif + +#if (!defined(USE_GYRO_OVERFLOW_CHECK) && !defined(USE_YAW_SPIN_RECOVERY)) + UNUSED(currentTimeUs); +#endif } FAST_CODE void gyroUpdate(timeUs_t currentTimeUs) { -#ifdef USE_DUAL_GYRO + const timeDelta_t sampleDeltaUs = currentTimeUs - accumulationLastTimeSampledUs; + accumulationLastTimeSampledUs = currentTimeUs; + accumulatedMeasurementTimeUs += sampleDeltaUs; + switch (gyroToUse) { case GYRO_CONFIG_USE_GYRO_1: gyroUpdateSensor(&gyroSensor1, currentTimeUs); @@ -1120,6 +1037,12 @@ FAST_CODE void gyroUpdate(timeUs_t currentTimeUs) gyro.gyroADCf[X] = gyroSensor1.gyroDev.gyroADCf[X]; gyro.gyroADCf[Y] = gyroSensor1.gyroDev.gyroADCf[Y]; gyro.gyroADCf[Z] = gyroSensor1.gyroDev.gyroADCf[Z]; +#ifdef USE_GYRO_OVERFLOW_CHECK + overflowDetected = gyroSensor1.overflowDetected; +#endif +#ifdef USE_YAW_SPIN_RECOVERY + yawSpinDetected = gyroSensor1.yawSpinDetected; +#endif } DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 0, gyroSensor1.gyroDev.gyroADCRaw[X]); DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 1, gyroSensor1.gyroDev.gyroADCRaw[Y]); @@ -1128,12 +1051,19 @@ FAST_CODE void gyroUpdate(timeUs_t currentTimeUs) DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 0, lrintf(gyro.gyroADCf[X])); DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 1, lrintf(gyro.gyroADCf[Y])); break; +#ifdef USE_MULTI_GYRO case GYRO_CONFIG_USE_GYRO_2: gyroUpdateSensor(&gyroSensor2, currentTimeUs); if (isGyroSensorCalibrationComplete(&gyroSensor2)) { gyro.gyroADCf[X] = gyroSensor2.gyroDev.gyroADCf[X]; gyro.gyroADCf[Y] = gyroSensor2.gyroDev.gyroADCf[Y]; gyro.gyroADCf[Z] = gyroSensor2.gyroDev.gyroADCf[Z]; +#ifdef USE_GYRO_OVERFLOW_CHECK + overflowDetected = gyroSensor2.overflowDetected; +#endif +#ifdef USE_YAW_SPIN_RECOVERY + yawSpinDetected = gyroSensor2.yawSpinDetected; +#endif } DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 2, gyroSensor2.gyroDev.gyroADCRaw[X]); DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 3, gyroSensor2.gyroDev.gyroADCRaw[Y]); @@ -1149,7 +1079,14 @@ FAST_CODE void gyroUpdate(timeUs_t currentTimeUs) gyro.gyroADCf[X] = (gyroSensor1.gyroDev.gyroADCf[X] + gyroSensor2.gyroDev.gyroADCf[X]) / 2.0f; gyro.gyroADCf[Y] = (gyroSensor1.gyroDev.gyroADCf[Y] + gyroSensor2.gyroDev.gyroADCf[Y]) / 2.0f; gyro.gyroADCf[Z] = (gyroSensor1.gyroDev.gyroADCf[Z] + gyroSensor2.gyroDev.gyroADCf[Z]) / 2.0f; +#ifdef USE_GYRO_OVERFLOW_CHECK + overflowDetected = gyroSensor1.overflowDetected || gyroSensor2.overflowDetected; +#endif +#ifdef USE_YAW_SPIN_RECOVERY + yawSpinDetected = gyroSensor1.yawSpinDetected || gyroSensor2.yawSpinDetected; +#endif } + DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 0, gyroSensor1.gyroDev.gyroADCRaw[X]); DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 1, gyroSensor1.gyroDev.gyroADCRaw[Y]); DEBUG_SET(DEBUG_DUAL_GYRO, 0, lrintf(gyroSensor1.gyroDev.gyroADCf[X])); @@ -1164,13 +1101,17 @@ FAST_CODE void gyroUpdate(timeUs_t currentTimeUs) DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 1, lrintf(gyroSensor1.gyroDev.gyroADCf[Y] - gyroSensor2.gyroDev.gyroADCf[Y])); DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 2, lrintf(gyroSensor1.gyroDev.gyroADCf[Z] - gyroSensor2.gyroDev.gyroADCf[Z])); break; - } -#else - gyroUpdateSensor(&gyroSensor1, currentTimeUs); - gyro.gyroADCf[X] = gyroSensor1.gyroDev.gyroADCf[X]; - gyro.gyroADCf[Y] = gyroSensor1.gyroDev.gyroADCf[Y]; - gyro.gyroADCf[Z] = gyroSensor1.gyroDev.gyroADCf[Z]; #endif + } + + if (!overflowDetected) { + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + // integrate using trapezium rule to avoid bias + accumulatedMeasurements[axis] += 0.5f * (gyroPrevious[axis] + gyro.gyroADCf[axis]) * sampleDeltaUs; + gyroPrevious[axis] = gyro.gyroADCf[axis]; + } + } + } bool gyroGetAccumulationAverage(float *accumulationAverage) @@ -1191,40 +1132,56 @@ bool gyroGetAccumulationAverage(float *accumulationAverage) } } +int16_t gyroReadSensorTemperature(gyroSensor_t gyroSensor) +{ + if (gyroSensor.gyroDev.temperatureFn) { + gyroSensor.gyroDev.temperatureFn(&gyroSensor.gyroDev, &gyroSensor.gyroDev.temperature); + } + return gyroSensor.gyroDev.temperature; +} + void gyroReadTemperature(void) { - if (gyroSensor1.gyroDev.temperatureFn) { - gyroSensor1.gyroDev.temperatureFn(&gyroSensor1.gyroDev, &gyroSensor1.gyroDev.temperature); + switch (gyroToUse) { + case GYRO_CONFIG_USE_GYRO_1: + gyroSensorTemperature = gyroReadSensorTemperature(gyroSensor1); + break; + +#ifdef USE_MULTI_GYRO + case GYRO_CONFIG_USE_GYRO_2: + gyroSensorTemperature = gyroReadSensorTemperature(gyroSensor2); + break; + + case GYRO_CONFIG_USE_GYRO_BOTH: + gyroSensorTemperature = MAX(gyroReadSensorTemperature(gyroSensor1), gyroReadSensorTemperature(gyroSensor2)); + break; +#endif // USE_MULTI_GYRO } } int16_t gyroGetTemperature(void) { - return gyroSensor1.gyroDev.temperature; + return gyroSensorTemperature; } int16_t gyroRateDps(int axis) { -#ifdef USE_DUAL_GYRO - if (gyroToUse == GYRO_CONFIG_USE_GYRO_2) { - return lrintf(gyro.gyroADCf[axis] / gyroSensor2.gyroDev.scale); - } else { - return lrintf(gyro.gyroADCf[axis] / gyroSensor1.gyroDev.scale); - } -#else - return lrintf(gyro.gyroADCf[axis] / gyroSensor1.gyroDev.scale); -#endif + return lrintf(gyro.gyroADCf[axis] / ACTIVE_GYRO->gyroDev.scale); } bool gyroOverflowDetected(void) { - return gyroSensor1.overflowDetected; +#ifdef USE_GYRO_OVERFLOW_CHECK + return overflowDetected; +#else + return false; +#endif // USE_GYRO_OVERFLOW_CHECK } #ifdef USE_YAW_SPIN_RECOVERY bool gyroYawSpinDetected(void) { - return gyroSensor1.yawSpinDetected; + return yawSpinDetected; } #endif // USE_YAW_SPIN_RECOVERY @@ -1239,3 +1196,44 @@ uint8_t gyroReadRegister(uint8_t whichSensor, uint8_t reg) return mpuGyroReadRegister(gyroSensorBusByDevice(whichSensor), reg); } #endif // USE_GYRO_REGISTER_DUMP + +#ifdef USE_DYN_LPF +void dynLpfGyroUpdate(float throttle) +{ + if (dynLpfFilter != DYN_LPF_NONE) { + uint16_t cutoffFreq = dynLpfMin; + if (throttle > dynLpfIdle) { + const float dynThrottle = (throttle - (throttle * throttle * throttle) / 3.0f) * 1.5f; + cutoffFreq += (dynThrottle - dynLpfIdlePoint) * dynLpfInvIdlePointScaled; + } + if (dynLpfFilter == DYN_LPF_PT1) { + const float gyroDt = gyro.targetLooptime * 1e-6f; + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { +#ifdef USE_MULTI_GYRO + if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_1 || gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH) { + pt1FilterUpdateCutoff(&gyroSensor1.lowpassFilter[axis].pt1FilterState, pt1FilterGain(cutoffFreq, gyroDt)); + } + if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_2 || gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH) { + pt1FilterUpdateCutoff(&gyroSensor2.lowpassFilter[axis].pt1FilterState, pt1FilterGain(cutoffFreq, gyroDt)); + } +#else + pt1FilterUpdateCutoff(&gyroSensor1.lowpassFilter[axis].pt1FilterState, pt1FilterGain(cutoffFreq, gyroDt)); +#endif + } + } else { + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { +#ifdef USE_MULTI_GYRO + if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_1 || gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH) { + biquadFilterUpdateLPF(&gyroSensor1.lowpassFilter[axis].biquadFilterState, cutoffFreq, gyro.targetLooptime); + } + if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_2 || gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH) { + biquadFilterUpdateLPF(&gyroSensor2.lowpassFilter[axis].biquadFilterState, cutoffFreq, gyro.targetLooptime); + } +#else + biquadFilterUpdateLPF(&gyroSensor1.lowpassFilter[axis].biquadFilterState, cutoffFreq, gyro.targetLooptime); +#endif + } + } + } +} +#endif diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 415412aac2..7d35e8681c 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -21,29 +21,13 @@ #pragma once #include "common/axis.h" +#include "common/filter.h" #include "common/time.h" -#include "pg/pg.h" + #include "drivers/bus.h" #include "drivers/sensor.h" -typedef enum { - GYRO_NONE = 0, - GYRO_DEFAULT, - GYRO_MPU6050, - GYRO_L3G4200D, - GYRO_MPU3050, - GYRO_L3GD20, - GYRO_MPU6000, - GYRO_MPU6500, - GYRO_MPU9250, - GYRO_ICM20601, - GYRO_ICM20602, - GYRO_ICM20608G, - GYRO_ICM20649, - GYRO_ICM20689, - GYRO_BMI160, - GYRO_FAKE -} gyroSensor_e; +#include "pg/pg.h" typedef struct gyro_s { uint32_t targetLooptime; @@ -58,6 +42,23 @@ typedef enum { GYRO_OVERFLOW_CHECK_ALL_AXES } gyroOverflowCheck_e; +enum { + DYN_FFT_BEFORE_STATIC_FILTERS = 0, + DYN_FFT_AFTER_STATIC_FILTERS +} ; + +enum { + DYN_FILTER_RANGE_HIGH = 0, + DYN_FILTER_RANGE_MEDIUM, + DYN_FILTER_RANGE_LOW +} ; + +enum { + DYN_LPF_NONE = 0, + DYN_LPF_PT1, + DYN_LPF_BIQUAD +}; + #define GYRO_CONFIG_USE_GYRO_1 0 #define GYRO_CONFIG_USE_GYRO_2 1 #define GYRO_CONFIG_USE_GYRO_BOTH 2 @@ -96,6 +97,12 @@ typedef struct gyroConfig_s { int16_t yaw_spin_threshold; uint16_t gyroCalibrationDuration; // Gyro calibration duration in 1/100 second + + uint8_t dyn_filter_width_percent; + uint8_t dyn_fft_location; // before or after static filters + uint8_t dyn_filter_range; // ignore any FFT bin below this threshold + uint16_t dyn_lpf_gyro_max_hz; + uint8_t dyn_lpf_gyro_idle; } gyroConfig_t; PG_DECLARE(gyroConfig_t, gyroConfig); @@ -106,8 +113,6 @@ void gyroInitFilters(void); void gyroUpdate(timeUs_t currentTimeUs); bool gyroGetAccumulationAverage(float *accumulation); const busDevice_t *gyroSensorBus(void); -struct mpuConfiguration_s; -const struct mpuConfiguration_s *gyroMpuConfiguration(void); struct mpuDetectionResult_s; const struct mpuDetectionResult_s *gyroMpuDetectionResult(void); void gyroStartCalibration(bool isFirstArmingCalibration); @@ -120,3 +125,6 @@ bool gyroOverflowDetected(void); bool gyroYawSpinDetected(void); uint16_t gyroAbsRateDps(int axis); uint8_t gyroReadRegister(uint8_t whichSensor, uint8_t reg); +#ifdef USE_DYN_LPF +void dynLpfGyroUpdate(float throttle); +#endif diff --git a/src/main/sensors/gyro_filter_impl.h b/src/main/sensors/gyro_filter_impl.h new file mode 100644 index 0000000000..faf2564693 --- /dev/null +++ b/src/main/sensors/gyro_filter_impl.h @@ -0,0 +1,48 @@ +static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor) +{ + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_RAW, axis, gyroSensor->gyroDev.gyroADCRaw[axis]); + // scale gyro output to degrees per second + float gyroADCf = gyroSensor->gyroDev.gyroADC[axis] * gyroSensor->gyroDev.scale; + // DEBUG_GYRO_SCALED records the unfiltered, scaled gyro output + GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_SCALED, axis, lrintf(gyroADCf)); + +#ifdef USE_GYRO_DATA_ANALYSE + if (isDynamicFilterActive()) { + if (axis == X) { + GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf)); + GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf)); + } + } + + float gyroDataForAnalysis = gyroADCf; +#endif + + // apply static notch filters and software lowpass filters + gyroADCf = gyroSensor->notchFilter1ApplyFn((filter_t *)&gyroSensor->notchFilter1[axis], gyroADCf); + gyroADCf = gyroSensor->notchFilter2ApplyFn((filter_t *)&gyroSensor->notchFilter2[axis], gyroADCf); + gyroADCf = gyroSensor->lowpassFilterApplyFn((filter_t *)&gyroSensor->lowpassFilter[axis], gyroADCf); + gyroADCf = gyroSensor->lowpass2FilterApplyFn((filter_t *)&gyroSensor->lowpass2Filter[axis], gyroADCf); + +#ifdef USE_GYRO_DATA_ANALYSE + if (isDynamicFilterActive()) { + if (gyroConfig()->dyn_fft_location == DYN_FFT_AFTER_STATIC_FILTERS) { + gyroDataForAnalysis = gyroADCf; + } + + if (axis == X) { + GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroDataForAnalysis)); + GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 2, lrintf(gyroDataForAnalysis)); + } + + gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, axis, gyroDataForAnalysis); + gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis], gyroADCf); + } +#endif + + // DEBUG_GYRO_FILTERED records the scaled, filtered, after all software filtering has been applied. + GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_FILTERED, axis, lrintf(gyroADCf)); + + gyroSensor->gyroDev.gyroADCf[axis] = gyroADCf; + } +} diff --git a/src/main/sensors/gyroanalyse.c b/src/main/sensors/gyroanalyse.c index a5958894b3..845cafbd47 100644 --- a/src/main/sensors/gyroanalyse.c +++ b/src/main/sensors/gyroanalyse.c @@ -18,13 +18,16 @@ * If not, see . */ +/* original work by Rav + * 2018_07 updated by ctzsnooze to post filter, wider Q, different peak detection + * coding assistance and advice from DieHertz, Rav, eTracer + * test pilots icr4sh, UAV Tech, Flint723 + */ #include #include "platform.h" #ifdef USE_GYRO_DATA_ANALYSE -#include "arm_math.h" - #include "build/debug.h" #include "common/filter.h" @@ -39,179 +42,188 @@ #include "sensors/gyroanalyse.h" // The FFT splits the frequency domain into an number of bins -// A sampling frequency of 1000 and max frequency of 500 at a window size of 32 gives 16 frequency bins each with a width 31.25Hz +// A sampling frequency of 1000 and max frequency of 500 at a window size of 32 gives 16 frequency bins each 31.25Hz wide // Eg [0,31), [31,62), [62, 93) etc +// for gyro loop >= 4KHz, sample rate 2000 defines FFT range to 1000Hz, 16 bins each 62.5 Hz wide +// NB FFT_WINDOW_SIZE is set to 32 in gyroanalyse.h +#define FFT_BIN_COUNT (FFT_WINDOW_SIZE / 2) +// start to compare 3rd bin to 2nd bin, ie start comparing from 77Hz, 100Hz, or 150Hz centres +#define FFT_BIN_OFFSET 2 +// smoothing frequency for FFT centre frequency +#define DYN_NOTCH_SMOOTH_FREQ_HZ 50 +// notch centre point will not go below sample rate divided by these dividers, resulting in range limits: +// HIGH : 133/166-1000Hz, MEDIUM -> 89/111-666Hz, LOW -> 67/83-500Hz +#define DYN_NOTCH_MIN_CENTRE_DIV 12 +// divider to get lowest allowed notch cutoff frequency +// otherwise cutoff is user configured percentage below centre frequency +#define DYN_NOTCH_MIN_CUTOFF_DIV 15 +// we need 4 steps for each axis +#define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * 4) -#define FFT_WINDOW_SIZE 32 // max for f3 targets -#define FFT_BIN_COUNT (FFT_WINDOW_SIZE / 2) -#define FFT_MIN_FREQ 100 // not interested in filtering frequencies below 100Hz -#define FFT_SAMPLING_RATE 1000 // allows analysis up to 500Hz which is more than motors create -#define FFT_MAX_FREQUENCY (FFT_SAMPLING_RATE / 2) // nyquist rate -#define FFT_BPF_HZ 200 // use a bandpass on gyro data to ignore extreme low and extreme high frequencies -#define FFT_RESOLUTION ((float)FFT_SAMPLING_RATE / FFT_WINDOW_SIZE) // hz per bin -#define DYN_NOTCH_WIDTH 100 // just an orientation and start value -#define DYN_NOTCH_CHANGERATE 60 // lower cut does not improve the performance much, higher cut makes it worse... -#define DYN_NOTCH_MIN_CUTOFF 120 // don't cut too deep into low frequencies -#define DYN_NOTCH_MAX_CUTOFF 200 // don't go above this cutoff (better filtering with "constant" delay at higher center frequencies) -#define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * 4) // we need 4 steps for each axis - -#define BIQUAD_Q 1.0f / sqrtf(2.0f) // quality factor - butterworth - -static FAST_RAM_ZERO_INIT uint16_t fftSamplingScale; - -// gyro data used for frequency analysis -static float FAST_RAM_ZERO_INIT gyroData[XYZ_AXIS_COUNT][FFT_WINDOW_SIZE]; - -static FAST_RAM_ZERO_INIT arm_rfft_fast_instance_f32 fftInstance; -static FAST_RAM_ZERO_INIT float fftData[FFT_WINDOW_SIZE]; -static FAST_RAM_ZERO_INIT float rfftData[FFT_WINDOW_SIZE]; -static FAST_RAM_ZERO_INIT gyroFftData_t fftResult[XYZ_AXIS_COUNT]; - -// use a circular buffer for the last FFT_WINDOW_SIZE samples -static FAST_RAM_ZERO_INIT uint16_t fftIdx; - -// bandpass filter gyro data -static FAST_RAM_ZERO_INIT biquadFilter_t fftGyroFilter[XYZ_AXIS_COUNT]; - -// filter for smoothing frequency estimation -static FAST_RAM_ZERO_INIT biquadFilter_t fftFreqFilter[XYZ_AXIS_COUNT]; +static uint16_t FAST_RAM_ZERO_INIT fftSamplingRateHz; +static float FAST_RAM_ZERO_INIT fftResolution; +static uint8_t FAST_RAM_ZERO_INIT fftBinOffset; +static uint16_t FAST_RAM_ZERO_INIT dynamicNotchMinCenterHz; +static uint16_t FAST_RAM_ZERO_INIT dynamicNotchMaxCenterHz; +static uint16_t FAST_RAM_ZERO_INIT dynamicNotchMinCutoffHz; +static float FAST_RAM_ZERO_INIT dynamicFilterWidthFactor; +static uint8_t dynamicFilterRange; // Hanning window, see https://en.wikipedia.org/wiki/Window_function#Hann_.28Hanning.29_window static FAST_RAM_ZERO_INIT float hanningWindow[FFT_WINDOW_SIZE]; -void initHanning(void) -{ - for (int i = 0; i < FFT_WINDOW_SIZE; i++) { - hanningWindow[i] = (0.5 - 0.5 * cos_approx(2 * M_PIf * i / (FFT_WINDOW_SIZE - 1))); - } -} - -void initGyroData(void) -{ - for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - for (int i = 0; i < FFT_WINDOW_SIZE; i++) { - gyroData[axis][i] = 0; - } - } -} - void gyroDataAnalyseInit(uint32_t targetLooptimeUs) { - // initialise even if FEATURE_DYNAMIC_FILTER not set, since it may be set later - const uint16_t samplingFrequency = 1000000 / targetLooptimeUs; - fftSamplingScale = samplingFrequency / FFT_SAMPLING_RATE; - arm_rfft_fast_init_f32(&fftInstance, FFT_WINDOW_SIZE); +#ifdef USE_DUAL_GYRO + static bool gyroAnalyseInitialized; + if (gyroAnalyseInitialized) { + return; + } + gyroAnalyseInitialized = true; +#endif - initGyroData(); - initHanning(); + dynamicFilterRange = gyroConfig()->dyn_filter_range; + + fftSamplingRateHz = 1000; + if (dynamicFilterRange == DYN_FILTER_RANGE_HIGH) { + fftSamplingRateHz = 2000; + } + else if (dynamicFilterRange == DYN_FILTER_RANGE_MEDIUM) { + fftSamplingRateHz = 1333; + } + // If we get at least 3 samples then use the default FFT sample frequency + // otherwise we need to calculate a FFT sample frequency to ensure we get 3 samples (gyro loops < 4K) + const int gyroLoopRateHz = lrintf((1.0f / targetLooptimeUs) * 1e6f); + + fftSamplingRateHz = MIN((gyroLoopRateHz / 3), fftSamplingRateHz); - // recalculation of filters takes 4 calls per axis => each filter gets updated every DYN_NOTCH_CALC_TICKS calls - // at 4khz gyro loop rate this means 4khz / 4 / 3 = 333Hz => update every 3ms - // for gyro rate > 16kHz, we have update frequency of 1kHz => 1ms - const float looptime = MAX(1000000u / FFT_SAMPLING_RATE, targetLooptimeUs * DYN_NOTCH_CALC_TICKS); - for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - fftResult[axis].centerFreq = 200; // any init value - biquadFilterInitLPF(&fftFreqFilter[axis], DYN_NOTCH_CHANGERATE, looptime); - biquadFilterInit(&fftGyroFilter[axis], FFT_BPF_HZ, 1000000 / FFT_SAMPLING_RATE, BIQUAD_Q, FILTER_BPF); + fftResolution = (float)fftSamplingRateHz / FFT_WINDOW_SIZE; + fftBinOffset = FFT_BIN_OFFSET; + + dynamicNotchMaxCenterHz = fftSamplingRateHz / 2; //Nyquist + dynamicNotchMinCenterHz = fftSamplingRateHz / DYN_NOTCH_MIN_CENTRE_DIV; + dynamicNotchMinCutoffHz = fftSamplingRateHz / DYN_NOTCH_MIN_CUTOFF_DIV; + dynamicFilterWidthFactor = (100.0f - gyroConfig()->dyn_filter_width_percent) / 100; + + + for (int i = 0; i < FFT_WINDOW_SIZE; i++) { + hanningWindow[i] = (0.5f - 0.5f * cos_approx(2 * M_PIf * i / (FFT_WINDOW_SIZE - 1))); } } -// used in OSD -const gyroFftData_t *gyroFftData(int axis) +void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs) { - return &fftResult[axis]; + // initialise even if FEATURE_DYNAMIC_FILTER not set, since it may be set later + // *** can this next line be removed ??? *** + gyroDataAnalyseInit(targetLooptimeUs); + + const uint16_t samplingFrequency = 1000000 / targetLooptimeUs; + state->maxSampleCount = samplingFrequency / fftSamplingRateHz; + state->maxSampleCountRcp = 1.f / state->maxSampleCount; + + arm_rfft_fast_init_f32(&state->fftInstance, FFT_WINDOW_SIZE); + +// recalculation of filters takes 4 calls per axis => each filter gets updated every DYN_NOTCH_CALC_TICKS calls +// at 4khz gyro loop rate this means 4khz / 4 / 3 = 333Hz => update every 3ms +// for gyro rate > 16kHz, we have update frequency of 1kHz => 1ms + const float looptime = MAX(1000000u / fftSamplingRateHz, targetLooptimeUs * DYN_NOTCH_CALC_TICKS); + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + // any init value + state->centerFreq[axis] = dynamicNotchMaxCenterHz; + state->prevCenterFreq[axis] = dynamicNotchMaxCenterHz; + biquadFilterInitLPF(&state->detectedFrequencyFilter[axis], DYN_NOTCH_SMOOTH_FREQ_HZ, looptime); + } } +void gyroDataAnalysePush(gyroAnalyseState_t *state, const int axis, const float sample) +{ + state->oversampledGyroAccumulator[axis] += sample; +} + +static void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn); + /* * Collect gyro data, to be analysed in gyroDataAnalyseUpdate function */ -void gyroDataAnalyse(const gyroDev_t *gyroDev, biquadFilter_t *notchFilterDyn) +void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn) { - // accumulator for oversampled data => no aliasing and less noise - static FAST_RAM_ZERO_INIT float fftAcc[XYZ_AXIS_COUNT]; - static FAST_RAM_ZERO_INIT uint32_t fftAccCount; - - static FAST_RAM_ZERO_INIT uint32_t gyroDataAnalyseUpdateTicks; - + // samples should have been pushed by `gyroDataAnalysePush` // if gyro sampling is > 1kHz, accumulate multiple samples - for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - fftAcc[axis] += gyroDev->gyroADC[axis]; - } - fftAccCount++; + state->sampleCount++; // this runs at 1kHz - if (fftAccCount == fftSamplingScale) { - fftAccCount = 0; + if (state->sampleCount == state->maxSampleCount) { + state->sampleCount = 0; - //calculate mean value of accumulated samples + // calculate mean value of accumulated samples for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - float sample = fftAcc[axis] / fftSamplingScale; - sample = biquadFilterApply(&fftGyroFilter[axis], sample); - gyroData[axis][fftIdx] = sample; - if (axis == 0) - DEBUG_SET(DEBUG_FFT, 2, lrintf(sample * gyroDev->scale)); - fftAcc[axis] = 0; + float sample = state->oversampledGyroAccumulator[axis] * state->maxSampleCountRcp; + state->downsampledGyroData[axis][state->circularBufferIdx] = sample; + if (axis == 0) { + DEBUG_SET(DEBUG_FFT, 2, lrintf(sample)); + } + + state->oversampledGyroAccumulator[axis] = 0; } - fftIdx = (fftIdx + 1) % FFT_WINDOW_SIZE; + state->circularBufferIdx = (state->circularBufferIdx + 1) % FFT_WINDOW_SIZE; // We need DYN_NOTCH_CALC_TICKS tick to update all axis with newly sampled value - gyroDataAnalyseUpdateTicks = DYN_NOTCH_CALC_TICKS; + state->updateTicks = DYN_NOTCH_CALC_TICKS; } // calculate FFT and update filters - if (gyroDataAnalyseUpdateTicks > 0) { - gyroDataAnalyseUpdate(notchFilterDyn); - --gyroDataAnalyseUpdateTicks; + if (state->updateTicks > 0) { + gyroDataAnalyseUpdate(state, notchFilterDyn); + --state->updateTicks; } } -void stage_rfft_f32(arm_rfft_fast_instance_f32 * S, float32_t * p, float32_t * pOut); -void arm_cfft_radix8by2_f32( arm_cfft_instance_f32 * S, float32_t * p1); -void arm_cfft_radix8by4_f32( arm_cfft_instance_f32 * S, float32_t * p1); -void arm_radix8_butterfly_f32(float32_t * pSrc, uint16_t fftLen, const float32_t * pCoef, uint16_t twidCoefModifier); -void arm_bitreversal_32(uint32_t * pSrc, const uint16_t bitRevLen, const uint16_t * pBitRevTable); - -typedef enum { - STEP_ARM_CFFT_F32, - STEP_BITREVERSAL, - STEP_STAGE_RFFT_F32, - STEP_ARM_CMPLX_MAG_F32, - STEP_CALC_FREQUENCIES, - STEP_UPDATE_FILTERS, - STEP_HANNING, - STEP_COUNT -} UpdateStep_e; +void stage_rfft_f32(arm_rfft_fast_instance_f32 *S, float32_t *p, float32_t *pOut); +void arm_cfft_radix8by2_f32(arm_cfft_instance_f32 *S, float32_t *p1); +void arm_cfft_radix8by4_f32(arm_cfft_instance_f32 *S, float32_t *p1); +void arm_radix8_butterfly_f32(float32_t *pSrc, uint16_t fftLen, const float32_t *pCoef, uint16_t twidCoefModifier); +void arm_bitreversal_32(uint32_t *pSrc, const uint16_t bitRevLen, const uint16_t *pBitRevTable); /* * Analyse last gyro data from the last FFT_WINDOW_SIZE milliseconds */ -void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn) +static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn) { - static int axis; - static int step; - arm_cfft_instance_f32 * Sint = &(fftInstance.Sint); + enum { + STEP_ARM_CFFT_F32, + STEP_BITREVERSAL, + STEP_STAGE_RFFT_F32, + STEP_ARM_CMPLX_MAG_F32, + STEP_CALC_FREQUENCIES, + STEP_UPDATE_FILTERS, + STEP_HANNING, + STEP_COUNT + }; + + arm_cfft_instance_f32 *Sint = &(state->fftInstance.Sint); uint32_t startTime = 0; - if (debugMode == (DEBUG_FFT_TIME)) + if (debugMode == (DEBUG_FFT_TIME)) { startTime = micros(); + } - DEBUG_SET(DEBUG_FFT_TIME, 0, step); - switch (step) { + DEBUG_SET(DEBUG_FFT_TIME, 0, state->updateStep); + switch (state->updateStep) { case STEP_ARM_CFFT_F32: { switch (FFT_BIN_COUNT) { case 16: // 16us - arm_cfft_radix8by2_f32(Sint, fftData); + arm_cfft_radix8by2_f32(Sint, state->fftData); break; case 32: // 35us - arm_cfft_radix8by4_f32(Sint, fftData); + arm_cfft_radix8by4_f32(Sint, state->fftData); break; case 64: // 70us - arm_radix8_butterfly_f32(fftData, FFT_BIN_COUNT, Sint->pTwiddle, 1); + arm_radix8_butterfly_f32(state->fftData, FFT_BIN_COUNT, Sint->pTwiddle, 1); break; } DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); @@ -220,76 +232,110 @@ void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn) case STEP_BITREVERSAL: { // 6us - arm_bitreversal_32((uint32_t*) fftData, Sint->bitRevLength, Sint->pBitRevTable); + arm_bitreversal_32((uint32_t*) state->fftData, Sint->bitRevLength, Sint->pBitRevTable); DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); - step++; + state->updateStep++; FALLTHROUGH; } case STEP_STAGE_RFFT_F32: { // 14us // this does not work in place => fftData AND rfftData needed - stage_rfft_f32(&fftInstance, fftData, rfftData); + stage_rfft_f32(&state->fftInstance, state->fftData, state->rfftData); DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); break; } case STEP_ARM_CMPLX_MAG_F32: { // 8us - arm_cmplx_mag_f32(rfftData, fftData, FFT_BIN_COUNT); + arm_cmplx_mag_f32(state->rfftData, state->fftData, FFT_BIN_COUNT); DEBUG_SET(DEBUG_FFT_TIME, 2, micros() - startTime); - step++; + state->updateStep++; FALLTHROUGH; } case STEP_CALC_FREQUENCIES: { - // 13us - float fftSum = 0; - float fftWeightedSum = 0; - - fftResult[axis].maxVal = 0; - // iterate over fft data and calculate weighted indexes - float squaredData; - for (int i = 0; i < FFT_BIN_COUNT; i++) { - squaredData = fftData[i] * fftData[i]; //more weight on higher peaks - fftResult[axis].maxVal = MAX(fftResult[axis].maxVal, squaredData); - fftSum += squaredData; - fftWeightedSum += squaredData * (i + 1); // calculate weighted index starting at 1, not 0 - } - - // get weighted center of relevant frequency range (this way we have a better resolution than 31.25Hz) - if (fftSum > 0) { - // idx was shifted by 1 to start at 1, not 0 - float fftMeanIndex = (fftWeightedSum / fftSum) - 1; - // the index points at the center frequency of each bin so index 0 is actually 16.125Hz - // fftMeanIndex += 0.5; - - // don't go below the minimal cutoff frequency + 10 and don't jump around too much - float centerFreq; - centerFreq = constrain(fftMeanIndex * FFT_RESOLUTION, DYN_NOTCH_MIN_CUTOFF + 10, FFT_MAX_FREQUENCY); - centerFreq = biquadFilterApply(&fftFreqFilter[axis], centerFreq); - centerFreq = constrain(centerFreq, DYN_NOTCH_MIN_CUTOFF + 10, FFT_MAX_FREQUENCY); - fftResult[axis].centerFreq = centerFreq; - if (axis == 0) { - DEBUG_SET(DEBUG_FFT, 3, lrintf(fftMeanIndex * 100)); + bool fftIncreased = false; + float dataMax = 0; + uint8_t binStart = 0; + uint8_t binMax = 0; + //for bins after initial decline, identify start bin and max bin + for (int i = 1 + fftBinOffset; i < FFT_BIN_COUNT; i++) { + if (fftIncreased || (state->fftData[i] > state->fftData[i - 1])) { + if (!fftIncreased) { + binStart = i; // first up-step bin + fftIncreased = true; + } + if (state->fftData[i] > dataMax) { + dataMax = state->fftData[i]; + binMax = i; // tallest bin + } } } + // accumulate fftSum and fftWeightedSum from peak bin, and shoulder bins either side of peak + float cubedData = state->fftData[binMax] * state->fftData[binMax] * state->fftData[binMax]; + float fftSum = cubedData; + float fftWeightedSum = cubedData * (binMax + 1); + // accumulate upper shoulder + for (int i = binMax; i < FFT_BIN_COUNT - 1; i++) { + if (state->fftData[i] > state->fftData[i + 1]) { + cubedData = state->fftData[i] * state->fftData[i] * state->fftData[i]; + fftSum += cubedData; + fftWeightedSum += cubedData * (i + 1); + } else { + break; + } + } + // accumulate lower shoulder + for (int i = binMax; i > binStart + 1; i--) { + if (state->fftData[i] > state->fftData[i - 1]) { + cubedData = state->fftData[i] * state->fftData[i] * state->fftData[i]; + fftSum += cubedData; + fftWeightedSum += cubedData * (i + 1); + } else { + break; + } + } + // get weighted center of relevant frequency range (this way we have a better resolution than 31.25Hz) + float centerFreq = dynamicNotchMaxCenterHz; + float fftMeanIndex = 0; + // idx was shifted by 1 to start at 1, not 0 + if (fftSum > 0) { + fftMeanIndex = (fftWeightedSum / fftSum) - 1; + // the index points at the center frequency of each bin so index 0 is actually 16.125Hz + centerFreq = fftMeanIndex * fftResolution; + } else { + centerFreq = state->prevCenterFreq[state->updateAxis]; + } + // constrain and low-pass smooth centre frequency + centerFreq = constrain(centerFreq, dynamicNotchMinCenterHz, dynamicNotchMaxCenterHz); + centerFreq = biquadFilterApply(&state->detectedFrequencyFilter[state->updateAxis], centerFreq); + centerFreq = constrain(centerFreq, dynamicNotchMinCenterHz, dynamicNotchMaxCenterHz); + state->centerFreq[state->updateAxis] = centerFreq; - DEBUG_SET(DEBUG_FFT_FREQ, axis, fftResult[axis].centerFreq); + if (state->updateAxis == 0) { + DEBUG_SET(DEBUG_FFT, 3, lrintf(fftMeanIndex * 100)); + DEBUG_SET(DEBUG_FFT_FREQ, 0, state->centerFreq[state->updateAxis]); + } + if (state->updateAxis == 1) { + DEBUG_SET(DEBUG_FFT_FREQ, 1, state->centerFreq[state->updateAxis]); + } + // Debug FFT_Freq carries raw gyro, gyro after first filter set, FFT centre for roll and for pitch DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); break; } case STEP_UPDATE_FILTERS: { // 7us - // calculate new filter coefficients - float cutoffFreq = constrain(fftResult[axis].centerFreq - DYN_NOTCH_WIDTH, DYN_NOTCH_MIN_CUTOFF, DYN_NOTCH_MAX_CUTOFF); - float notchQ = filterGetNotchQ(fftResult[axis].centerFreq, cutoffFreq); - biquadFilterUpdate(¬chFilterDyn[axis], fftResult[axis].centerFreq, gyro.targetLooptime, notchQ, FILTER_NOTCH); + // calculate cutoffFreq and notch Q, update notch filter + const float cutoffFreq = fmax(state->centerFreq[state->updateAxis] * dynamicFilterWidthFactor, dynamicNotchMinCutoffHz); + const float notchQ = filterGetNotchQ(state->centerFreq[state->updateAxis], cutoffFreq); + biquadFilterUpdate(¬chFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis], gyro.targetLooptime, notchQ, FILTER_NOTCH); + DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); - axis = (axis + 1) % 3; - step++; + state->updateAxis = (state->updateAxis + 1) % XYZ_AXIS_COUNT; + state->updateStep++; FALLTHROUGH; } case STEP_HANNING: @@ -297,16 +343,16 @@ void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn) // 5us // apply hanning window to gyro samples and store result in fftData // hanning starts and ends with 0, could be skipped for minor speed improvement - uint8_t ringBufIdx = FFT_WINDOW_SIZE - fftIdx; - arm_mult_f32(&gyroData[axis][fftIdx], &hanningWindow[0], &fftData[0], ringBufIdx); - if (fftIdx > 0) - arm_mult_f32(&gyroData[axis][0], &hanningWindow[ringBufIdx], &fftData[ringBufIdx], fftIdx); + const uint8_t ringBufIdx = FFT_WINDOW_SIZE - state->circularBufferIdx; + arm_mult_f32(&state->downsampledGyroData[state->updateAxis][state->circularBufferIdx], &hanningWindow[0], &state->fftData[0], ringBufIdx); + if (state->circularBufferIdx > 0) { + arm_mult_f32(&state->downsampledGyroData[state->updateAxis][0], &hanningWindow[ringBufIdx], &state->fftData[ringBufIdx], state->circularBufferIdx); + } DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); } } - step = (step + 1) % STEP_COUNT; + state->updateStep = (state->updateStep + 1) % STEP_COUNT; } - #endif // USE_GYRO_DATA_ANALYSE diff --git a/src/main/sensors/gyroanalyse.h b/src/main/sensors/gyroanalyse.h index b0565ec3f8..a1b8e7a175 100644 --- a/src/main/sensors/gyroanalyse.h +++ b/src/main/sensors/gyroanalyse.h @@ -20,16 +20,42 @@ #pragma once -#include "common/time.h" +#include "arm_math.h" + #include "common/filter.h" -typedef struct gyroFftData_s { - float maxVal; - uint16_t centerFreq; -} gyroFftData_t; +#include "sensors/gyro.h" -void gyroDataAnalyseInit(uint32_t targetLooptime); -const gyroFftData_t *gyroFftData(int axis); -struct gyroDev_s; -void gyroDataAnalyse(const struct gyroDev_s *gyroDev, biquadFilter_t *notchFilterDyn); -void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn); +// max for F3 targets +#define FFT_WINDOW_SIZE 32 + +typedef struct gyroAnalyseState_s { + // accumulator for oversampled data => no aliasing and less noise + uint8_t sampleCount; + uint8_t maxSampleCount; + float maxSampleCountRcp; + float oversampledGyroAccumulator[XYZ_AXIS_COUNT]; + + // downsampled gyro data circular buffer for frequency analysis + uint8_t circularBufferIdx; + float downsampledGyroData[XYZ_AXIS_COUNT][FFT_WINDOW_SIZE]; + + // update state machine step information + uint8_t updateTicks; + uint8_t updateStep; + uint8_t updateAxis; + + arm_rfft_fast_instance_f32 fftInstance; + float fftData[FFT_WINDOW_SIZE]; + float rfftData[FFT_WINDOW_SIZE]; + + biquadFilter_t detectedFrequencyFilter[XYZ_AXIS_COUNT]; + uint16_t centerFreq[XYZ_AXIS_COUNT]; + uint16_t prevCenterFreq[XYZ_AXIS_COUNT]; +} gyroAnalyseState_t; + +STATIC_ASSERT(FFT_WINDOW_SIZE <= (uint8_t) -1, window_size_greater_than_underlying_type); + +void gyroDataAnalyseStateInit(gyroAnalyseState_t *gyroAnalyse, uint32_t targetLooptime); +void gyroDataAnalysePush(gyroAnalyseState_t *gyroAnalyse, int axis, float sample); +void gyroDataAnalyse(gyroAnalyseState_t *gyroAnalyse, biquadFilter_t *notchFilterDyn); diff --git a/src/main/sensors/rangefinder.c b/src/main/sensors/rangefinder.c index 0878718288..3bf386450f 100644 --- a/src/main/sensors/rangefinder.c +++ b/src/main/sensors/rangefinder.c @@ -46,7 +46,7 @@ #include "fc/config.h" #include "fc/runtime_config.h" -#include "fc/fc_tasks.h" +#include "fc/tasks.h" #include "sensors/sensors.h" #include "sensors/rangefinder.h" diff --git a/src/main/sensors/voltage.c b/src/main/sensors/voltage.c index 996c229b14..b12c2fb1d5 100644 --- a/src/main/sensors/voltage.c +++ b/src/main/sensors/voltage.c @@ -191,7 +191,7 @@ void voltageMeterADCInit(void) voltageMeterADCState_t *state = &voltageMeterADCStates[i]; memset(state, 0, sizeof(voltageMeterADCState_t)); - biquadFilterInitLPF(&state->filter, VBAT_LPF_FREQ, 50000); + biquadFilterInitLPF(&state->filter, VBAT_LPF_FREQ, HZ_TO_INTERVAL_US(50)); } } @@ -215,7 +215,7 @@ void voltageMeterESCInit(void) { #ifdef USE_ESC_SENSOR memset(&voltageMeterESCState, 0, sizeof(voltageMeterESCState_t)); - biquadFilterInitLPF(&voltageMeterESCState.filter, VBAT_LPF_FREQ, 50000); //50HZ Update + biquadFilterInitLPF(&voltageMeterESCState.filter, VBAT_LPF_FREQ, HZ_TO_INTERVAL_US(50)); #endif } diff --git a/src/main/startup/startup_stm32f411xe.s b/src/main/startup/startup_stm32f411xe.s index 6c217c4193..591fc888a9 100644 --- a/src/main/startup/startup_stm32f411xe.s +++ b/src/main/startup/startup_stm32f411xe.s @@ -71,13 +71,8 @@ defined in linker script */ .weak Reset_Handler .type Reset_Handler, %function Reset_Handler: - // Check for bootloader reboot - ldr r0, =0x2001FFFC // mj666 - ldr r1, =0xDEADBEEF // mj666 - ldr r2, [r0, #0] // mj666 - str r0, [r0, #0] // mj666 - cmp r2, r1 // mj666 - beq Reboot_Loader // mj666 + // Defined in C code + bl checkForBootLoaderRequest /* Copy the data segment initializers from flash to SRAM */ movs r1, #0 @@ -136,13 +131,6 @@ LoopMarkHeapStack: LoopForever: b LoopForever -Reboot_Loader: // mj666 - - // Reboot to ROM // mj666 - ldr r0, =0x1FFF0000 // mj666 - ldr sp,[r0, #0] // mj666 - ldr r0,[r0, #4] // mj666 - bx r0 // mj666 .size Reset_Handler, .-Reset_Handler /** diff --git a/src/main/startup/startup_stm32f765xx.s b/src/main/startup/startup_stm32f765xx.s new file mode 100644 index 0000000000..07f79cc0b6 --- /dev/null +++ b/src/main/startup/startup_stm32f765xx.s @@ -0,0 +1,654 @@ +/** + ****************************************************************************** + * @file startup_stm32f765xx.s + * @author MCD Application Team + * @version V1.2.0 + * @date 30-December-2016 + * @brief STM32F765xx Devices vector table for GCC based toolchain. + * This module performs: + * - Set the initial SP + * - Set the initial PC == Reset_Handler, + * - Set the vector table entries with the exceptions ISR address + * - Branches to main in the C library (which eventually + * calls main()). + * After Reset the Cortex-M7 processor is in Thread mode, + * priority is Privileged, and the Stack is set to Main. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2016 STMicroelectronics

+ * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * 3. Neither the name of STMicroelectronics nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************** + */ + + .syntax unified + .cpu cortex-m7 + .fpu softvfp + .thumb + +.global g_pfnVectors +.global Default_Handler + +/* start address for the initialization values of the .data section. +defined in linker script */ +.word _sidata +/* start address for the .data section. defined in linker script */ +.word _sdata +/* end address for the .data section. defined in linker script */ +.word _edata +/* start address for the .bss section. defined in linker script */ +.word _sbss +/* end address for the .bss section. defined in linker script */ +.word _ebss +/* start address for the .fastram_bss section. defined in linker script */ +.word _sfastram_bss +/* end address for the .fastram_bss section. defined in linker script */ +.word _efastram_bss +/* stack used for SystemInit_ExtMemCtl; always internal RAM used */ + +/** + * @brief This is the code that gets called when the processor first + * starts execution following a reset event. Only the absolutely + * necessary set is performed, after which the application + * supplied main() routine is called. + * @param None + * @retval : None +*/ + + .section .text.Reset_Handler + .weak Reset_Handler + .type Reset_Handler, %function +Reset_Handler: + ldr sp, =_estack /* set stack pointer */ + +/* Copy the data segment initializers from flash to SRAM */ + movs r1, #0 + b LoopCopyDataInit + +CopyDataInit: + ldr r3, =_sidata + ldr r3, [r3, r1] + str r3, [r0, r1] + adds r1, r1, #4 + +LoopCopyDataInit: + ldr r0, =_sdata + ldr r3, =_edata + adds r2, r0, r1 + cmp r2, r3 + bcc CopyDataInit + + ldr r2, =_sbss + b LoopFillZerobss +/* Zero fill the bss segment. */ +FillZerobss: + movs r3, #0 + str r3, [r2], #4 + +LoopFillZerobss: + ldr r3, = _ebss + cmp r2, r3 + bcc FillZerobss + + ldr r2, =_ssram2 + b LoopFillZerosram2 +/* Zero fill the sram2 segment. */ +FillZerosram2: + movs r3, #0 + str r3, [r2], #4 + +LoopFillZerosram2: + ldr r3, = _esram2 + cmp r2, r3 + bcc FillZerosram2 + + ldr r2, =_sfastram_bss + b LoopFillZerofastram_bss +/* Zero fill the fastram_bss segment. */ +FillZerofastram_bss: + movs r3, #0 + str r3, [r2], #4 + +LoopFillZerofastram_bss: + ldr r3, = _efastram_bss + cmp r2, r3 + bcc FillZerofastram_bss + +/* Call the clock system initialization function.*/ + bl SystemInit +/* Call the application's entry point.*/ + bl main + bx lr +.size Reset_Handler, .-Reset_Handler + +/** + * @brief This is the code that gets called when the processor receives an + * unexpected interrupt. This simply enters an infinite loop, preserving + * the system state for examination by a debugger. + * @param None + * @retval None +*/ + .section .text.Default_Handler,"ax",%progbits +Default_Handler: +Infinite_Loop: + b Infinite_Loop + .size Default_Handler, .-Default_Handler +/****************************************************************************** +* +* The minimal vector table for a Cortex M7. Note that the proper constructs +* must be placed on this to ensure that it ends up at physical address +* 0x0000.0000. +* +*******************************************************************************/ + .section .isr_vector,"a",%progbits + .type g_pfnVectors, %object + .size g_pfnVectors, .-g_pfnVectors + + +g_pfnVectors: + .word _estack + .word Reset_Handler + + .word NMI_Handler + .word HardFault_Handler + .word MemManage_Handler + .word BusFault_Handler + .word UsageFault_Handler + .word 0 + .word 0 + .word 0 + .word 0 + .word SVC_Handler + .word DebugMon_Handler + .word 0 + .word PendSV_Handler + .word SysTick_Handler + + /* External Interrupts */ + .word WWDG_IRQHandler /* Window WatchDog */ + .word PVD_IRQHandler /* PVD through EXTI Line detection */ + .word TAMP_STAMP_IRQHandler /* Tamper and TimeStamps through the EXTI line */ + .word RTC_WKUP_IRQHandler /* RTC Wakeup through the EXTI line */ + .word FLASH_IRQHandler /* FLASH */ + .word RCC_IRQHandler /* RCC */ + .word EXTI0_IRQHandler /* EXTI Line0 */ + .word EXTI1_IRQHandler /* EXTI Line1 */ + .word EXTI2_IRQHandler /* EXTI Line2 */ + .word EXTI3_IRQHandler /* EXTI Line3 */ + .word EXTI4_IRQHandler /* EXTI Line4 */ + .word DMA1_Stream0_IRQHandler /* DMA1 Stream 0 */ + .word DMA1_Stream1_IRQHandler /* DMA1 Stream 1 */ + .word DMA1_Stream2_IRQHandler /* DMA1 Stream 2 */ + .word DMA1_Stream3_IRQHandler /* DMA1 Stream 3 */ + .word DMA1_Stream4_IRQHandler /* DMA1 Stream 4 */ + .word DMA1_Stream5_IRQHandler /* DMA1 Stream 5 */ + .word DMA1_Stream6_IRQHandler /* DMA1 Stream 6 */ + .word ADC_IRQHandler /* ADC1, ADC2 and ADC3s */ + .word CAN1_TX_IRQHandler /* CAN1 TX */ + .word CAN1_RX0_IRQHandler /* CAN1 RX0 */ + .word CAN1_RX1_IRQHandler /* CAN1 RX1 */ + .word CAN1_SCE_IRQHandler /* CAN1 SCE */ + .word EXTI9_5_IRQHandler /* External Line[9:5]s */ + .word TIM1_BRK_TIM9_IRQHandler /* TIM1 Break and TIM9 */ + .word TIM1_UP_TIM10_IRQHandler /* TIM1 Update and TIM10 */ + .word TIM1_TRG_COM_TIM11_IRQHandler /* TIM1 Trigger and Commutation and TIM11 */ + .word TIM1_CC_IRQHandler /* TIM1 Capture Compare */ + .word TIM2_IRQHandler /* TIM2 */ + .word TIM3_IRQHandler /* TIM3 */ + .word TIM4_IRQHandler /* TIM4 */ + .word I2C1_EV_IRQHandler /* I2C1 Event */ + .word I2C1_ER_IRQHandler /* I2C1 Error */ + .word I2C2_EV_IRQHandler /* I2C2 Event */ + .word I2C2_ER_IRQHandler /* I2C2 Error */ + .word SPI1_IRQHandler /* SPI1 */ + .word SPI2_IRQHandler /* SPI2 */ + .word USART1_IRQHandler /* USART1 */ + .word USART2_IRQHandler /* USART2 */ + .word USART3_IRQHandler /* USART3 */ + .word EXTI15_10_IRQHandler /* External Line[15:10]s */ + .word RTC_Alarm_IRQHandler /* RTC Alarm (A and B) through EXTI Line */ + .word OTG_FS_WKUP_IRQHandler /* USB OTG FS Wakeup through EXTI line */ + .word TIM8_BRK_TIM12_IRQHandler /* TIM8 Break and TIM12 */ + .word TIM8_UP_TIM13_IRQHandler /* TIM8 Update and TIM13 */ + .word TIM8_TRG_COM_TIM14_IRQHandler /* TIM8 Trigger and Commutation and TIM14 */ + .word TIM8_CC_IRQHandler /* TIM8 Capture Compare */ + .word DMA1_Stream7_IRQHandler /* DMA1 Stream7 */ + .word FMC_IRQHandler /* FMC */ + .word SDMMC1_IRQHandler /* SDMMC1 */ + .word TIM5_IRQHandler /* TIM5 */ + .word SPI3_IRQHandler /* SPI3 */ + .word UART4_IRQHandler /* UART4 */ + .word UART5_IRQHandler /* UART5 */ + .word TIM6_DAC_IRQHandler /* TIM6 and DAC1&2 underrun errors */ + .word TIM7_IRQHandler /* TIM7 */ + .word DMA2_Stream0_IRQHandler /* DMA2 Stream 0 */ + .word DMA2_Stream1_IRQHandler /* DMA2 Stream 1 */ + .word DMA2_Stream2_IRQHandler /* DMA2 Stream 2 */ + .word DMA2_Stream3_IRQHandler /* DMA2 Stream 3 */ + .word DMA2_Stream4_IRQHandler /* DMA2 Stream 4 */ + .word ETH_IRQHandler /* Ethernet */ + .word ETH_WKUP_IRQHandler /* Ethernet Wakeup through EXTI line */ + .word CAN2_TX_IRQHandler /* CAN2 TX */ + .word CAN2_RX0_IRQHandler /* CAN2 RX0 */ + .word CAN2_RX1_IRQHandler /* CAN2 RX1 */ + .word CAN2_SCE_IRQHandler /* CAN2 SCE */ + .word OTG_FS_IRQHandler /* USB OTG FS */ + .word DMA2_Stream5_IRQHandler /* DMA2 Stream 5 */ + .word DMA2_Stream6_IRQHandler /* DMA2 Stream 6 */ + .word DMA2_Stream7_IRQHandler /* DMA2 Stream 7 */ + .word USART6_IRQHandler /* USART6 */ + .word I2C3_EV_IRQHandler /* I2C3 event */ + .word I2C3_ER_IRQHandler /* I2C3 error */ + .word OTG_HS_EP1_OUT_IRQHandler /* USB OTG HS End Point 1 Out */ + .word OTG_HS_EP1_IN_IRQHandler /* USB OTG HS End Point 1 In */ + .word OTG_HS_WKUP_IRQHandler /* USB OTG HS Wakeup through EXTI */ + .word OTG_HS_IRQHandler /* USB OTG HS */ + .word DCMI_IRQHandler /* DCMI */ + .word 0 /* Reserved */ + .word RNG_IRQHandler /* RNG */ + .word FPU_IRQHandler /* FPU */ + .word UART7_IRQHandler /* UART7 */ + .word UART8_IRQHandler /* UART8 */ + .word SPI4_IRQHandler /* SPI4 */ + .word SPI5_IRQHandler /* SPI5 */ + .word SPI6_IRQHandler /* SPI6 */ + .word SAI1_IRQHandler /* SAI1 */ + .word 0 /* Reserved */ + .word 0 /* Reserved */ + .word DMA2D_IRQHandler /* DMA2D */ + .word SAI2_IRQHandler /* SAI2 */ + .word QUADSPI_IRQHandler /* QUADSPI */ + .word LPTIM1_IRQHandler /* LPTIM1 */ + .word CEC_IRQHandler /* HDMI_CEC */ + .word I2C4_EV_IRQHandler /* I2C4 Event */ + .word I2C4_ER_IRQHandler /* I2C4 Error */ + .word SPDIF_RX_IRQHandler /* SPDIF_RX */ + .word 0 /* Reserved */ + .word DFSDM1_FLT0_IRQHandler /* DFSDM1 Filter 0 global Interrupt */ + .word DFSDM1_FLT1_IRQHandler /* DFSDM1 Filter 1 global Interrupt */ + .word DFSDM1_FLT2_IRQHandler /* DFSDM1 Filter 2 global Interrupt */ + .word DFSDM1_FLT3_IRQHandler /* DFSDM1 Filter 3 global Interrupt */ + .word SDMMC2_IRQHandler /* SDMMC2 */ + .word CAN3_TX_IRQHandler /* CAN3 TX */ + .word CAN3_RX0_IRQHandler /* CAN3 RX0 */ + .word CAN3_RX1_IRQHandler /* CAN3 RX1 */ + .word CAN3_SCE_IRQHandler /* CAN3 SCE */ + .word 0 /* Reserved */ + .word MDIOS_IRQHandler /* MDIOS */ + +/******************************************************************************* +* +* Provide weak aliases for each Exception handler to the Default_Handler. +* As they are weak aliases, any function with the same name will override +* this definition. +* +*******************************************************************************/ + .weak NMI_Handler + .thumb_set NMI_Handler,Default_Handler + + .weak HardFault_Handler + .thumb_set HardFault_Handler,Default_Handler + + .weak MemManage_Handler + .thumb_set MemManage_Handler,Default_Handler + + .weak BusFault_Handler + .thumb_set BusFault_Handler,Default_Handler + + .weak UsageFault_Handler + .thumb_set UsageFault_Handler,Default_Handler + + .weak SVC_Handler + .thumb_set SVC_Handler,Default_Handler + + .weak DebugMon_Handler + .thumb_set DebugMon_Handler,Default_Handler + + .weak PendSV_Handler + .thumb_set PendSV_Handler,Default_Handler + + .weak SysTick_Handler + .thumb_set SysTick_Handler,Default_Handler + + .weak WWDG_IRQHandler + .thumb_set WWDG_IRQHandler,Default_Handler + + .weak PVD_IRQHandler + .thumb_set PVD_IRQHandler,Default_Handler + + .weak TAMP_STAMP_IRQHandler + .thumb_set TAMP_STAMP_IRQHandler,Default_Handler + + .weak RTC_WKUP_IRQHandler + .thumb_set RTC_WKUP_IRQHandler,Default_Handler + + .weak FLASH_IRQHandler + .thumb_set FLASH_IRQHandler,Default_Handler + + .weak RCC_IRQHandler + .thumb_set RCC_IRQHandler,Default_Handler + + .weak EXTI0_IRQHandler + .thumb_set EXTI0_IRQHandler,Default_Handler + + .weak EXTI1_IRQHandler + .thumb_set EXTI1_IRQHandler,Default_Handler + + .weak EXTI2_IRQHandler + .thumb_set EXTI2_IRQHandler,Default_Handler + + .weak EXTI3_IRQHandler + .thumb_set EXTI3_IRQHandler,Default_Handler + + .weak EXTI4_IRQHandler + .thumb_set EXTI4_IRQHandler,Default_Handler + + .weak DMA1_Stream0_IRQHandler + .thumb_set DMA1_Stream0_IRQHandler,Default_Handler + + .weak DMA1_Stream1_IRQHandler + .thumb_set DMA1_Stream1_IRQHandler,Default_Handler + + .weak DMA1_Stream2_IRQHandler + .thumb_set DMA1_Stream2_IRQHandler,Default_Handler + + .weak DMA1_Stream3_IRQHandler + .thumb_set DMA1_Stream3_IRQHandler,Default_Handler + + .weak DMA1_Stream4_IRQHandler + .thumb_set DMA1_Stream4_IRQHandler,Default_Handler + + .weak DMA1_Stream5_IRQHandler + .thumb_set DMA1_Stream5_IRQHandler,Default_Handler + + .weak DMA1_Stream6_IRQHandler + .thumb_set DMA1_Stream6_IRQHandler,Default_Handler + + .weak ADC_IRQHandler + .thumb_set ADC_IRQHandler,Default_Handler + + .weak CAN1_TX_IRQHandler + .thumb_set CAN1_TX_IRQHandler,Default_Handler + + .weak CAN1_RX0_IRQHandler + .thumb_set CAN1_RX0_IRQHandler,Default_Handler + + .weak CAN1_RX1_IRQHandler + .thumb_set CAN1_RX1_IRQHandler,Default_Handler + + .weak CAN1_SCE_IRQHandler + .thumb_set CAN1_SCE_IRQHandler,Default_Handler + + .weak EXTI9_5_IRQHandler + .thumb_set EXTI9_5_IRQHandler,Default_Handler + + .weak TIM1_BRK_TIM9_IRQHandler + .thumb_set TIM1_BRK_TIM9_IRQHandler,Default_Handler + + .weak TIM1_UP_TIM10_IRQHandler + .thumb_set TIM1_UP_TIM10_IRQHandler,Default_Handler + + .weak TIM1_TRG_COM_TIM11_IRQHandler + .thumb_set TIM1_TRG_COM_TIM11_IRQHandler,Default_Handler + + .weak TIM1_CC_IRQHandler + .thumb_set TIM1_CC_IRQHandler,Default_Handler + + .weak TIM2_IRQHandler + .thumb_set TIM2_IRQHandler,Default_Handler + + .weak TIM3_IRQHandler + .thumb_set TIM3_IRQHandler,Default_Handler + + .weak TIM4_IRQHandler + .thumb_set TIM4_IRQHandler,Default_Handler + + .weak I2C1_EV_IRQHandler + .thumb_set I2C1_EV_IRQHandler,Default_Handler + + .weak I2C1_ER_IRQHandler + .thumb_set I2C1_ER_IRQHandler,Default_Handler + + .weak I2C2_EV_IRQHandler + .thumb_set I2C2_EV_IRQHandler,Default_Handler + + .weak I2C2_ER_IRQHandler + .thumb_set I2C2_ER_IRQHandler,Default_Handler + + .weak SPI1_IRQHandler + .thumb_set SPI1_IRQHandler,Default_Handler + + .weak SPI2_IRQHandler + .thumb_set SPI2_IRQHandler,Default_Handler + + .weak USART1_IRQHandler + .thumb_set USART1_IRQHandler,Default_Handler + + .weak USART2_IRQHandler + .thumb_set USART2_IRQHandler,Default_Handler + + .weak USART3_IRQHandler + .thumb_set USART3_IRQHandler,Default_Handler + + .weak EXTI15_10_IRQHandler + .thumb_set EXTI15_10_IRQHandler,Default_Handler + + .weak RTC_Alarm_IRQHandler + .thumb_set RTC_Alarm_IRQHandler,Default_Handler + + .weak OTG_FS_WKUP_IRQHandler + .thumb_set OTG_FS_WKUP_IRQHandler,Default_Handler + + .weak TIM8_BRK_TIM12_IRQHandler + .thumb_set TIM8_BRK_TIM12_IRQHandler,Default_Handler + + .weak TIM8_UP_TIM13_IRQHandler + .thumb_set TIM8_UP_TIM13_IRQHandler,Default_Handler + + .weak TIM8_TRG_COM_TIM14_IRQHandler + .thumb_set TIM8_TRG_COM_TIM14_IRQHandler,Default_Handler + + .weak TIM8_CC_IRQHandler + .thumb_set TIM8_CC_IRQHandler,Default_Handler + + .weak DMA1_Stream7_IRQHandler + .thumb_set DMA1_Stream7_IRQHandler,Default_Handler + + .weak FMC_IRQHandler + .thumb_set FMC_IRQHandler,Default_Handler + + .weak SDMMC1_IRQHandler + .thumb_set SDMMC1_IRQHandler,Default_Handler + + .weak TIM5_IRQHandler + .thumb_set TIM5_IRQHandler,Default_Handler + + .weak SPI3_IRQHandler + .thumb_set SPI3_IRQHandler,Default_Handler + + .weak UART4_IRQHandler + .thumb_set UART4_IRQHandler,Default_Handler + + .weak UART5_IRQHandler + .thumb_set UART5_IRQHandler,Default_Handler + + .weak TIM6_DAC_IRQHandler + .thumb_set TIM6_DAC_IRQHandler,Default_Handler + + .weak TIM7_IRQHandler + .thumb_set TIM7_IRQHandler,Default_Handler + + .weak DMA2_Stream0_IRQHandler + .thumb_set DMA2_Stream0_IRQHandler,Default_Handler + + .weak DMA2_Stream1_IRQHandler + .thumb_set DMA2_Stream1_IRQHandler,Default_Handler + + .weak DMA2_Stream2_IRQHandler + .thumb_set DMA2_Stream2_IRQHandler,Default_Handler + + .weak DMA2_Stream3_IRQHandler + .thumb_set DMA2_Stream3_IRQHandler,Default_Handler + + .weak DMA2_Stream4_IRQHandler + .thumb_set DMA2_Stream4_IRQHandler,Default_Handler + + .weak DMA2_Stream4_IRQHandler + .thumb_set DMA2_Stream4_IRQHandler,Default_Handler + + .weak ETH_IRQHandler + .thumb_set ETH_IRQHandler,Default_Handler + + .weak ETH_WKUP_IRQHandler + .thumb_set ETH_WKUP_IRQHandler,Default_Handler + + .weak CAN2_TX_IRQHandler + .thumb_set CAN2_TX_IRQHandler,Default_Handler + + .weak CAN2_RX0_IRQHandler + .thumb_set CAN2_RX0_IRQHandler,Default_Handler + + .weak CAN2_RX1_IRQHandler + .thumb_set CAN2_RX1_IRQHandler,Default_Handler + + .weak CAN2_SCE_IRQHandler + .thumb_set CAN2_SCE_IRQHandler,Default_Handler + + .weak OTG_FS_IRQHandler + .thumb_set OTG_FS_IRQHandler,Default_Handler + + .weak DMA2_Stream5_IRQHandler + .thumb_set DMA2_Stream5_IRQHandler,Default_Handler + + .weak DMA2_Stream6_IRQHandler + .thumb_set DMA2_Stream6_IRQHandler,Default_Handler + + .weak DMA2_Stream7_IRQHandler + .thumb_set DMA2_Stream7_IRQHandler,Default_Handler + + .weak USART6_IRQHandler + .thumb_set USART6_IRQHandler,Default_Handler + + .weak I2C3_EV_IRQHandler + .thumb_set I2C3_EV_IRQHandler,Default_Handler + + .weak I2C3_ER_IRQHandler + .thumb_set I2C3_ER_IRQHandler,Default_Handler + + .weak OTG_HS_EP1_OUT_IRQHandler + .thumb_set OTG_HS_EP1_OUT_IRQHandler,Default_Handler + + .weak OTG_HS_EP1_IN_IRQHandler + .thumb_set OTG_HS_EP1_IN_IRQHandler,Default_Handler + + .weak OTG_HS_WKUP_IRQHandler + .thumb_set OTG_HS_WKUP_IRQHandler,Default_Handler + + .weak OTG_HS_IRQHandler + .thumb_set OTG_HS_IRQHandler,Default_Handler + + .weak DCMI_IRQHandler + .thumb_set DCMI_IRQHandler,Default_Handler + + .weak RNG_IRQHandler + .thumb_set RNG_IRQHandler,Default_Handler + + .weak FPU_IRQHandler + .thumb_set FPU_IRQHandler,Default_Handler + + .weak UART7_IRQHandler + .thumb_set UART7_IRQHandler,Default_Handler + + .weak UART8_IRQHandler + .thumb_set UART8_IRQHandler,Default_Handler + + .weak SPI4_IRQHandler + .thumb_set SPI4_IRQHandler,Default_Handler + + .weak SPI5_IRQHandler + .thumb_set SPI5_IRQHandler,Default_Handler + + .weak SPI6_IRQHandler + .thumb_set SPI6_IRQHandler,Default_Handler + + .weak SAI1_IRQHandler + .thumb_set SAI1_IRQHandler,Default_Handler + + .weak DMA2D_IRQHandler + .thumb_set DMA2D_IRQHandler,Default_Handler + + .weak SAI2_IRQHandler + .thumb_set SAI2_IRQHandler,Default_Handler + + .weak QUADSPI_IRQHandler + .thumb_set QUADSPI_IRQHandler,Default_Handler + + .weak LPTIM1_IRQHandler + .thumb_set LPTIM1_IRQHandler,Default_Handler + + .weak CEC_IRQHandler + .thumb_set CEC_IRQHandler,Default_Handler + + .weak I2C4_EV_IRQHandler + .thumb_set I2C4_EV_IRQHandler,Default_Handler + + .weak I2C4_ER_IRQHandler + .thumb_set I2C4_ER_IRQHandler,Default_Handler + + .weak SPDIF_RX_IRQHandler + .thumb_set SPDIF_RX_IRQHandler,Default_Handler + + .weak DFSDM1_FLT0_IRQHandler + .thumb_set DFSDM1_FLT0_IRQHandler,Default_Handler + + .weak DFSDM1_FLT1_IRQHandler + .thumb_set DFSDM1_FLT1_IRQHandler,Default_Handler + + .weak DFSDM1_FLT2_IRQHandler + .thumb_set DFSDM1_FLT2_IRQHandler,Default_Handler + + .weak DFSDM1_FLT3_IRQHandler + .thumb_set DFSDM1_FLT3_IRQHandler,Default_Handler + + .weak SDMMC2_IRQHandler + .thumb_set SDMMC2_IRQHandler,Default_Handler + + .weak CAN3_TX_IRQHandler + .thumb_set CAN3_TX_IRQHandler,Default_Handler + + .weak CAN3_RX0_IRQHandler + .thumb_set CAN3_RX0_IRQHandler,Default_Handler + + .weak CAN3_RX1_IRQHandler + .thumb_set CAN3_RX1_IRQHandler,Default_Handler + + .weak CAN3_SCE_IRQHandler + .thumb_set CAN3_SCE_IRQHandler,Default_Handler + + .weak MDIOS_IRQHandler + .thumb_set MDIOS_IRQHandler,Default_Handler + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ + diff --git a/src/main/target/AG3X/target.c b/src/main/target/AG3X/target.c index 962a8442ac..5458ee3aa0 100644 --- a/src/main/target/AG3X/target.c +++ b/src/main/target/AG3X/target.c @@ -28,34 +28,34 @@ #include "drivers/timer_def.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - // ADC1 D(2,4) D(2,0) + // ADC1 *D(2,4) D(2,0) // ADC2 D(2,2) D(2,3) // ADC3 D(2,0) D(2,1) - DEF_TIM(TIM1, CH1, PA8, TIM_USE_PPM, 0, 0), // PPM D(1,0) + DEF_TIM(TIM1, CH1, PA8, TIM_USE_PPM, 0, 0), // PPM D(1,0) // Motors - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, 0, 0), // MOTOR1 U(2,1) D(2,2) D(2,4) - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0), // MOTOR2 U(1,2) D(1,7) - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0), // MOTOR3 U(1,2) D(1,2) - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MOTOR, 0, 0), // MOTOR4 U(1,6) D(1,3) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, 0, 0), // MOTOR1 U(2,1) *D(2,2) D(2,4) + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0), // MOTOR2 U(1,2) D(1,7) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0), // MOTOR3 U(1,2) D(1,2) + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MOTOR, 0, 0), // MOTOR4 U(1,6) D(1,3) // More outputs - DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, 0, 0), // MOTOR5 U(1,6) D(1,7) - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MOTOR, 0, 0), // MOTOR6 U(2,1) D(2,7) - DEF_TIM(TIM12, CH1, PB14, TIM_USE_MOTOR, 0, 0), // MOTOR7 ; SS2 - DEF_TIM(TIM12, CH2, PB15, TIM_USE_MOTOR, 0, 0), // MOTOR8 + DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, 0, 0), // MOTOR5 U(1,6) D(1,7) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MOTOR, 0, 0), // MOTOR6 U(2,1) D(2,7) + DEF_TIM(TIM12, CH1, PB14, TIM_USE_MOTOR, 0, 0), // MOTOR7 ; SS2 + DEF_TIM(TIM12, CH2, PB15, TIM_USE_MOTOR, 0, 0), // MOTOR8 // DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), // LED strip XXX Collision with M4 // Backdoor timers // UART1 (Collision with PPM) - DEF_TIM(TIM1, CH2, PA9, TIM_USE_NONE, 0, 1), // TX1 D(2,6) D(2,2) ; SS1 - DEF_TIM(TIM1, CH3, PA10, TIM_USE_NONE, 0, 0), // RX1 D(2,6) D(2,6) + DEF_TIM(TIM1, CH2, PA9, TIM_USE_NONE, 0, 1), // TX1 D(2,6) D(2,2) ; SS1 + DEF_TIM(TIM1, CH3, PA10, TIM_USE_NONE, 0, 0), // RX1 D(2,6) D(2,6) // UART2 - DEF_TIM(TIM5, CH3, PA2, TIM_USE_LED, 0, 0), // TX2 D(1,0) ; SA port ---> LED - DEF_TIM(TIM9, CH2, PA3, TIM_USE_NONE, 0, 0), // RX2 ; CAMC port + DEF_TIM(TIM5, CH3, PA2, TIM_USE_LED, 0, 0), // TX2 D(1,0) ; SA port ---> LED + DEF_TIM(TIM9, CH2, PA3, TIM_USE_CAMERA_CONTROL, 0, 0), // RX2 ; CAMC port // I2C2 DEF_TIM(TIM2, CH3, PB10, TIM_USE_NONE, 0, 0), // SCL2 D(1,1) diff --git a/src/main/target/AG3X/target.h b/src/main/target/AG3X/target.h index f4a74a9718..91b219d7de 100644 --- a/src/main/target/AG3X/target.h +++ b/src/main/target/AG3X/target.h @@ -37,28 +37,37 @@ // Note, beeper is on the LED pin #define LED0_PIN PC13 -#define BEEPER NONE +#define USE_BEEPER +#define BEEPER_PIN NONE #define BEEPER_INVERTED #define USE_ACC -#define USE_GYRO - #define USE_ACC_SPI_MPU6000 -#define USE_GYRO_SPI_MPU6000 -#define MPU6000_CS_PIN PA4 -#define MPU6000_SPI_INSTANCE SPI1 -#define ACC_MPU6000_ALIGN CW0_DEG_FLIP -#define GYRO_MPU6000_ALIGN CW0_DEG_FLIP - #define USE_ACC_SPI_MPU6500 + +#define USE_GYRO +#define USE_GYRO_SPI_MPU6000 #define USE_GYRO_SPI_MPU6500 -#define MPU6500_CS_PIN MPU6000_CS_PIN -#define MPU6500_SPI_INSTANCE MPU6000_SPI_INSTANCE -#define ACC_MPU6500_ALIGN CW0_DEG_FLIP -#define GYRO_MPU6500_ALIGN CW0_DEG_FLIP + +#define USE_MULTI_GYRO + +#define GYRO_1_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PA4 +#define GYRO_1_EXTI_PIN NONE + +#define ACC_1_ALIGN CW0_DEG_FLIP +#define GYRO_1_ALIGN CW0_DEG_FLIP + +#define GYRO_2_SPI_INSTANCE SPI1 +#define GYRO_2_CS_PIN PC15 +#define GYRO_2_EXTI_PIN NONE + +#define ACC_2_ALIGN CW0_DEG_FLIP +#define GYRO_2_ALIGN CW0_DEG_FLIP #define USE_MAG #define USE_MAG_HMC5883 +#define USE_MAG_LIS3MDL #define USE_BARO #define USE_BARO_SPI_BMP280 @@ -66,7 +75,6 @@ #define BMP280_SPI_INSTANCE SPI2 #define BMP280_CS_PIN PB9 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_INSTANCE SPI3 #define MAX7456_SPI_CS_PIN PA15 @@ -155,8 +163,6 @@ #define USE_RANGEFINDER_HCSR04 //#define USE_RANGEFINDER_TF -#define CAMERA_CONTROL_PIN PA3 - #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS diff --git a/src/main/target/AG3X/target.mk b/src/main/target/AG3X/target.mk index 7fd871549e..21aa1e7db4 100644 --- a/src/main/target/AG3X/target.mk +++ b/src/main/target/AG3X/target.mk @@ -17,4 +17,5 @@ TARGET_SRC = \ drivers/barometer/barometer_bmp280.c \ drivers/barometer/barometer_ms5611.c \ drivers/compass/compass_hmc5883l.c \ + drivers/compass/compass_lis3mdl.c \ drivers/max7456.c diff --git a/src/main/target/AIKONF4/target.c b/src/main/target/AIKONF4/target.c index 00dd655e5c..3a4dd74381 100644 --- a/src/main/target/AIKONF4/target.c +++ b/src/main/target/AIKONF4/target.c @@ -28,7 +28,7 @@ #include "drivers/timer_def.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - DEF_TIM(TIM2, CH2, PB3, TIM_USE_ANY, 0, 0), // CAMERA_CONTROL_PIN + DEF_TIM(TIM2, CH2, PB3, TIM_USE_CAMERA_CONTROL, 0, 0), // CAMERA_CONTROL_PIN // TIM8_UP (DMA2_ST1_CH7) DEF_TIM(TIM8, CH1, PC6, TIM_USE_MOTOR, 0, 0), // M1 diff --git a/src/main/target/AIKONF4/target.h b/src/main/target/AIKONF4/target.h index 1656e08644..99df8ad06b 100644 --- a/src/main/target/AIKONF4/target.h +++ b/src/main/target/AIKONF4/target.h @@ -32,32 +32,27 @@ #define INVERTER_PIN_UART1 PC0 -#define CAMERA_CONTROL_PIN PB3 - #define USE_EXTI -#define MPU_INT_EXTI PC4 -#define USE_MPU_DATA_READY_SIGNAL -#define MPU6000_CS_PIN SPI1_NSS_PIN -#define MPU6000_SPI_INSTANCE SPI1 #define USE_GYRO #define USE_GYRO_SPI_MPU6000 +#define USE_GYRO_SPI_MPU6500 +#define GYRO_1_CS_PIN SPI1_NSS_PIN +#define GYRO_1_SPI_INSTANCE SPI1 +#define GYRO_1_ALIGN CW0_DEG + #define USE_ACC #define USE_ACC_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW0_DEG -#define ACC_MPU6000_ALIGN CW0_DEG - -#define USE_GYRO_SPI_MPU6500 #define USE_ACC_SPI_MPU6500 -#define MPU6500_CS_PIN MPU6000_CS_PIN -#define MPU6500_SPI_INSTANCE MPU6000_SPI_INSTANCE -#define GYRO_MPU6500_ALIGN GYRO_MPU6000_ALIGN -#define ACC_MPU6500_ALIGN ACC_MPU6000_ALIGN +#define ACC_1_ALIGN CW0_DEG + +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC4 +#define USE_MPU_DATA_READY_SIGNAL #define USE_BARO #define USE_BARO_BMP280 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_INSTANCE SPI3 #define MAX7456_SPI_CS_PIN SPI3_NSS_PIN @@ -76,7 +71,6 @@ #define USE_UART1 #define UART1_RX_PIN PA10 -#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 #define USE_UART2 #define UART2_RX_PIN PA3 @@ -144,5 +138,5 @@ #define TARGET_IO_PORTC ( BIT(12) | BIT(11) | BIT(10) | BIT(9) | BIT(8) | BIT(7) | BIT(6) | BIT(4) | BIT(3) | BIT(2) | BIT(1) | BIT(0) ) #define TARGET_IO_PORTD ( BIT(2) ) -#define USABLE_TIMER_CHANNEL_COUNT 16 +#define USABLE_TIMER_CHANNEL_COUNT 13 #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(8) | TIM_N(9) ) diff --git a/src/main/target/AIR32/target.h b/src/main/target/AIR32/target.h index 12fe579567..b7e59b0540 100644 --- a/src/main/target/AIR32/target.h +++ b/src/main/target/AIR32/target.h @@ -22,7 +22,6 @@ #define TARGET_BOARD_IDENTIFIER "AR32" // AiR32 -#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT #define LED0_PIN PB5 // Blue LED - PB5 @@ -31,26 +30,27 @@ // MPU6050 interrupts #define USE_EXTI -#define MPU_INT_EXTI PA15 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PA15 #define USE_MPU_DATA_READY_SIGNAL //#define ENSURE_MPU_DATA_READY_IS_LOW #define USE_GYRO #define USE_ACC -#define USE_GYRO_MPU6050 -#define GYRO_MPU6050_ALIGN CW180_DEG - -#define USE_ACC_MPU6050 -#define ACC_MPU6050_ALIGN CW180_DEG +// MPU6050 support has been dropped according to board wiki +// https://github.com/betaflight/betaflight/wiki/Board---AIR32 +//#define USE_GYRO_MPU6050 +//#define USE_ACC_MPU6050 #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW180_DEG #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW180_DEG -#define MPU6000_CS_PIN PB12 -#define MPU6000_SPI_INSTANCE SPI2 +#define GYRO_1_CS_PIN PB12 +#define GYRO_1_SPI_INSTANCE SPI2 + +#define GYRO_1_ALIGN CW180_DEG +#define ACC_1_ALIGN CW180_DEG //#define USE_BARO //#define USE_BARO_MS5611 @@ -111,4 +111,4 @@ #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) #define USABLE_TIMER_CHANNEL_COUNT 10 -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(16) | TIM_N(17)) diff --git a/src/main/target/AIRHEROF3/target.h b/src/main/target/AIRHEROF3/target.h index dcefca6aff..8e22b6c2a1 100644 --- a/src/main/target/AIRHEROF3/target.h +++ b/src/main/target/AIRHEROF3/target.h @@ -22,7 +22,6 @@ #define TARGET_BOARD_IDENTIFIER "AIR3" -#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT #define CONFIG_PREFER_ACC_ON #define LED0_PIN PB3 @@ -33,23 +32,24 @@ #define BEEPER_INVERTED #define USE_EXTI -#define MPU_INT_EXTI PC13 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC13 #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW #define USE_SPI #define USE_SPI_DEVICE_2 -#define MPU6500_CS_PIN PB12 -#define MPU6500_SPI_INSTANCE SPI2 +#define GYRO_1_CS_PIN PB12 +#define GYRO_1_SPI_INSTANCE SPI2 #define USE_GYRO #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW0_DEG +#define GYRO_1_ALIGN CW0_DEG #define USE_ACC #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW0_DEG +#define ACC_1_ALIGN CW0_DEG #define USE_BARO #define USE_BARO_SPI_BMP280 @@ -101,4 +101,4 @@ #define TARGET_IO_PORTF (BIT(4)) #define USABLE_TIMER_CHANNEL_COUNT 14 -#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) ) +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) ) diff --git a/src/main/target/ALIENFLIGHTF1/target.h b/src/main/target/ALIENFLIGHTF1/target.h index 5c630d116e..0615867136 100644 --- a/src/main/target/ALIENFLIGHTF1/target.h +++ b/src/main/target/ALIENFLIGHTF1/target.h @@ -37,12 +37,12 @@ #define USE_GYRO #define USE_GYRO_MPU6050 -#define GYRO_MPU6050_ALIGN CW0_DEG +#define GYRO_1_ALIGN CW0_DEG #define USE_ACC #define USE_ACC_MPU6050 -#define ACC_MPU6050_ALIGN CW0_DEG +#define ACC_1_ALIGN CW0_DEG #define USE_UART1 #define USE_UART2 diff --git a/src/main/target/ALIENFLIGHTF3/config.c b/src/main/target/ALIENFLIGHTF3/config.c index 0e63f95570..ba444b8098 100644 --- a/src/main/target/ALIENFLIGHTF3/config.c +++ b/src/main/target/ALIENFLIGHTF3/config.c @@ -37,6 +37,7 @@ #include "flight/pid.h" #include "pg/beeper_dev.h" +#include "pg/gyrodev.h" #include "pg/rx.h" #include "rx/rx.h" @@ -60,6 +61,8 @@ // alternative defaults settings for AlienFlight targets void targetConfiguration(void) { + gyroDeviceConfigMutable(0)->extiTag = selectMPUIntExtiConfigByHardwareRevision(); + /* depending on revision ... depends on the LEDs to be utilised. */ if (hardwareRevision == AFF3_REV_2) { statusLedConfigMutable()->inversion = 0 @@ -101,7 +104,7 @@ void targetConfiguration(void) rxConfigMutable()->serialrx_inverted = true; serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIALRX_UART)].functionMask = FUNCTION_TELEMETRY_FRSKY_HUB | FUNCTION_RX_SERIAL; telemetryConfigMutable()->telemetry_inverted = false; - featureSet(FEATURE_TELEMETRY); + featureEnable(FEATURE_TELEMETRY); beeperDevConfigMutable()->isOpenDrain = false; beeperDevConfigMutable()->isInverted = true; parseRcChannels("AETR1234", rxConfigMutable()); diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h index b28e3a4f7f..20ea903e4d 100644 --- a/src/main/target/ALIENFLIGHTF3/target.h +++ b/src/main/target/ALIENFLIGHTF3/target.h @@ -25,7 +25,6 @@ #define TARGET_BUS_INIT #define REMAP_TIM17_DMA -#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT #define USE_HARDWARE_REVISION_DETECTION #define HW_PIN PB2 @@ -50,15 +49,13 @@ #define USE_GYRO_MPU6050 #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6050_ALIGN CW270_DEG -#define GYRO_MPU6500_ALIGN CW270_DEG +#define GYRO_1_ALIGN CW270_DEG #define USE_ACC #define USE_ACC_MPU6050 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6050_ALIGN CW270_DEG -#define ACC_MPU6500_ALIGN CW270_DEG +#define ACC_1_ALIGN CW270_DEG // No baro support. //#define USE_BARO @@ -107,8 +104,8 @@ #define SPI3_MISO_PIN PB4 #define SPI3_MOSI_PIN PB5 -#define MPU6500_CS_PIN SPI3_NSS_PIN -#define MPU6500_SPI_INSTANCE SPI3 +#define GYRO_1_CS_PIN SPI3_NSS_PIN +#define GYRO_1_SPI_INSTANCE SPI3 #define USE_ADC @@ -131,4 +128,4 @@ #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) #define USABLE_TIMER_CHANNEL_COUNT 11 -#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17) ) +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) diff --git a/src/main/target/ALIENFLIGHTF4/config.c b/src/main/target/ALIENFLIGHTF4/config.c index 94ac50e095..3c67839ba3 100644 --- a/src/main/target/ALIENFLIGHTF4/config.c +++ b/src/main/target/ALIENFLIGHTF4/config.c @@ -73,7 +73,7 @@ void targetConfiguration(void) telemetryConfigMutable()->telemetry_inverted = false; batteryConfigMutable()->voltageMeterSource = VOLTAGE_METER_ADC; batteryConfigMutable()->currentMeterSource = CURRENT_METER_ADC; - featureSet(FEATURE_TELEMETRY); + featureEnable(FEATURE_TELEMETRY); } for (uint8_t pidProfileIndex = 0; pidProfileIndex < MAX_PROFILE_COUNT; pidProfileIndex++) { diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index 9c472a681b..19a3845370 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -38,26 +38,28 @@ // MPU interrupt #define USE_EXTI -#define MPU_INT_EXTI PC14 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC14 //#define DEBUG_MPU_DATA_READY_INTERRUPT #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW -#define MPU6500_CS_PIN SPI1_NSS_PIN -#define MPU6500_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN SPI1_NSS_PIN +#define GYRO_1_SPI_INSTANCE SPI1 #define USE_ACC #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW270_DEG +#define ACC_1_ALIGN CW270_DEG #define USE_GYRO #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW270_DEG +#define GYRO_1_ALIGN CW270_DEG #define USE_MAG #define USE_MAG_HMC5883 #define USE_MAG_QMC5883 #define USE_MAG_AK8963 +#define USE_MAG_LIS3MDL #define MAG_AK8963_ALIGN CW180_DEG_FLIP @@ -66,18 +68,11 @@ #define USE_BARO_BMP280 #define USE_SDCARD - +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED #define SDCARD_DETECT_PIN PB11 - #define SDCARD_SPI_INSTANCE SPI2 #define SDCARD_SPI_CS_PIN PB10 - -// SPI2 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init: -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz -// Divide to under 25MHz for normal operation: -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 // 21MHz - #define SDCARD_DMA_CHANNEL_TX DMA1_Stream4 #define SDCARD_DMA_CHANNEL 0 diff --git a/src/main/target/ALIENFLIGHTF4/target.mk b/src/main/target/ALIENFLIGHTF4/target.mk index e733398ee9..9aa170bb0a 100644 --- a/src/main/target/ALIENFLIGHTF4/target.mk +++ b/src/main/target/ALIENFLIGHTF4/target.mk @@ -1,5 +1,5 @@ F405_TARGETS += $(TARGET) -FEATURES += SDCARD VCP ONBOARDFLASH +FEATURES += SDCARD_SPI VCP ONBOARDFLASH TARGET_SRC = \ drivers/accgyro/accgyro_mpu6500.c \ @@ -8,5 +8,6 @@ TARGET_SRC = \ drivers/barometer/barometer_ms5611.c \ drivers/compass/compass_ak8963.c \ drivers/compass/compass_hmc5883l.c \ - drivers/compass/compass_qmc5883l.c + drivers/compass/compass_qmc5883l.c \ + drivers/compass/compass_lis3mdl.c diff --git a/src/main/target/ALIENFLIGHTNGF7/config.c b/src/main/target/ALIENFLIGHTNGF7/config.c index acd7a9ad9b..5dc19e1567 100644 --- a/src/main/target/ALIENFLIGHTNGF7/config.c +++ b/src/main/target/ALIENFLIGHTNGF7/config.c @@ -72,7 +72,7 @@ void targetConfiguration(void) telemetryConfigMutable()->telemetry_inverted = false; batteryConfigMutable()->voltageMeterSource = VOLTAGE_METER_ADC; batteryConfigMutable()->currentMeterSource = CURRENT_METER_ADC; - featureSet(FEATURE_TELEMETRY); + featureEnable(FEATURE_TELEMETRY); } for (uint8_t pidProfileIndex = 0; pidProfileIndex < MAX_PROFILE_COUNT; pidProfileIndex++) { diff --git a/src/main/target/ALIENFLIGHTNGF7/target.c b/src/main/target/ALIENFLIGHTNGF7/target.c index a93edbd1e0..ce5f5ef71a 100644 --- a/src/main/target/ALIENFLIGHTNGF7/target.c +++ b/src/main/target/ALIENFLIGHTNGF7/target.c @@ -35,10 +35,10 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { DEF_TIM(TIM1, CH1, PA8, TIM_USE_PPM | TIM_USE_LED, 0, 1), // PPM - DMA2_ST6, *DMA2_ST1, DMA2_ST3 DEF_TIM(TIM8, CH1, PC6, TIM_USE_MOTOR, 0, 0), // PWM1 - DMA2_ST2, DMA2_ST2 DEF_TIM(TIM3, CH2, PC7, TIM_USE_MOTOR, 0, 0), // PWM2 - DMA1_ST5 - DEF_TIM(TIM8, CH2N, PB14, TIM_USE_MOTOR, 0, 0), // PWM3 - DMA2_ST3, DMA2_ST2 + DEF_TIM(TIM8, CH2N, PB14, TIM_USE_MOTOR, 0, 1), // PWM3 - DMA2_ST3, DMA2_ST2 DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0), // PWM4 - DMA1_ST7 DEF_TIM(TIM5, CH1, PA0, TIM_USE_MOTOR, 0, 0), // PWM5 - DMA1_ST2 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, 0, 0), // PWM6 - (DMA2_ST4) DMA2_ST2 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, 0, 1), // PWM6 - (DMA2_ST4) DMA2_ST2 DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR, 0, 0), // PWM7 - (DMA1_ST4) - DMA SDCard, DMA Serial_TX4 DEF_TIM(TIM8, CH4, PC9, TIM_USE_MOTOR, 0, 0), // PWM8 - (DMA2_ST7) - DMA Serial_TX1 DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0), // PWM9 - (DMA1_ST2) - Collision diff --git a/src/main/target/ALIENFLIGHTNGF7/target.h b/src/main/target/ALIENFLIGHTNGF7/target.h index 66a4cd7b61..eb0fa8090b 100644 --- a/src/main/target/ALIENFLIGHTNGF7/target.h +++ b/src/main/target/ALIENFLIGHTNGF7/target.h @@ -36,21 +36,22 @@ // MPU interrupt #define USE_EXTI -#define MPU_INT_EXTI PC14 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC14 //#define DEBUG_MPU_DATA_READY_INTERRUPT #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW -#define MPU6500_CS_PIN SPI1_NSS_PIN -#define MPU6500_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN SPI1_NSS_PIN +#define GYRO_1_SPI_INSTANCE SPI1 #define USE_ACC #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW270_DEG +#define ACC_1_ALIGN CW270_DEG #define USE_GYRO #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW270_DEG +#define GYRO_1_ALIGN CW270_DEG #define USE_MAG #define USE_MAG_HMC5883 @@ -58,6 +59,7 @@ #define USE_MAG_QMC5883 #define USE_MAG_AK8963 #define USE_MAG_SPI_AK8963 +#define USE_MAG_LIS3MDL #define HMC5883_CS_PIN PC15 #define HMC5883_SPI_INSTANCE SPI3 @@ -80,18 +82,11 @@ #define BMP280_SPI_INSTANCE SPI3 #define USE_SDCARD - +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED #define SDCARD_DETECT_PIN PB11 - #define SDCARD_SPI_INSTANCE SPI2 #define SDCARD_SPI_CS_PIN PB10 - -// SPI2 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init: -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 422kHz -// Divide to under 25MHz for normal operation: -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 8 // 27MHz - #define SDCARD_DMA_STREAM_TX_FULL DMA1_Stream4 #define SDCARD_DMA_CHANNEL 0 @@ -157,7 +152,6 @@ #define I2C1_SCL PB6 #define I2C1_SDA PB7 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_INSTANCE SPI3 #define MAX7456_SPI_CS_PIN PB12 diff --git a/src/main/target/ALIENFLIGHTNGF7/target.mk b/src/main/target/ALIENFLIGHTNGF7/target.mk index 2d9d8709bd..fa6f397b66 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_SPI VCP ONBOARDFLASH TARGET_SRC = \ drivers/accgyro/accgyro_mpu6500.c \ @@ -9,4 +9,5 @@ TARGET_SRC = \ drivers/compass/compass_ak8963.c \ drivers/compass/compass_hmc5883l.c \ drivers/compass/compass_qmc5883l.c \ + drivers/compass/compass_lis3mdl.c \ drivers/max7456.c diff --git a/src/main/target/ALIENWHOOP/config.c b/src/main/target/ALIENWHOOP/config.c index 2983443e8f..191dec4679 100644 --- a/src/main/target/ALIENWHOOP/config.c +++ b/src/main/target/ALIENWHOOP/config.c @@ -113,7 +113,7 @@ void targetConfiguration(void) pidConfigMutable()->runaway_takeoff_prevention = false; - featureSet((FEATURE_DYNAMIC_FILTER | FEATURE_AIRMODE | FEATURE_ANTI_GRAVITY) ^ FEATURE_RX_PARALLEL_PWM); + featureEnable((FEATURE_DYNAMIC_FILTER | FEATURE_AIRMODE | FEATURE_ANTI_GRAVITY) ^ FEATURE_RX_PARALLEL_PWM); /* AlienWhoop PIDs tested with 6mm and 7mm motors on most frames */ for (uint8_t pidProfileIndex = 0; pidProfileIndex < MAX_PROFILE_COUNT; pidProfileIndex++) { @@ -139,8 +139,9 @@ void targetConfiguration(void) /* Setpoints */ pidProfile->dterm_filter_type = FILTER_BIQUAD; pidProfile->dterm_notch_hz = 0; - pidProfile->dtermSetpointWeight = 100; - pidProfile->setpointRelaxRatio = 0; + pidProfile->pid[PID_PITCH].F = 100; + pidProfile->pid[PID_ROLL].F = 100; + pidProfile->feedForwardTransition = 0; /* Anti-Gravity */ pidProfile->itermThrottleThreshold = 500; diff --git a/src/main/target/ALIENWHOOP/target.h b/src/main/target/ALIENWHOOP/target.h index 42b470e335..6087eb4f4f 100644 --- a/src/main/target/ALIENWHOOP/target.h +++ b/src/main/target/ALIENWHOOP/target.h @@ -48,7 +48,6 @@ #define USE_TARGET_CONFIG // see config.c for target specific customizations -#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT #define BRUSHED_MOTORS /* Visual Alerts - SMD LEDs @@ -122,26 +121,28 @@ */ // Interrupt #define USE_EXTI -#define MPU_INT_EXTI PC14 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC14 // MPU -#define MPU6500_CS_PIN SPI1_NSS_PIN -#define MPU6500_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN SPI1_NSS_PIN +#define GYRO_1_SPI_INSTANCE SPI1 #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW // MAG #define USE_MAG #define USE_MAG_AK8963 +#define USE_MAG_LIS3MDL #define MAG_AK8963_ALIGN CW0_DEG #define USE_MAG_DATA_READY_SIGNAL #define ENSURE_MAG_DATA_READY_IS_HIGH // GYRO #define USE_GYRO #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW0_DEG +#define GYRO_1_ALIGN CW0_DEG // ACC #define USE_ACC #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW0_DEG +#define ACC_1_ALIGN CW0_DEG /* Optional Digital Pressure Sensor (barometer) - Bosch BMP280 * TODO: not implemented on V1 or V2 pcb diff --git a/src/main/target/ALIENWHOOP/target.mk b/src/main/target/ALIENWHOOP/target.mk index 4d72357939..f23a6d92bf 100644 --- a/src/main/target/ALIENWHOOP/target.mk +++ b/src/main/target/ALIENWHOOP/target.mk @@ -22,6 +22,7 @@ TARGET_SRC = \ drivers/accgyro/accgyro_spi_mpu6500.c \ drivers/barometer/barometer_bmp280.c \ drivers/compass/compass_ak8963.c \ + drivers/compass/compass_lis3mdl.c \ drivers/flash_m25p16.c \ drivers/max7456.c \ io/osd.c diff --git a/src/main/target/ANYFCF7/target.c b/src/main/target/ANYFCF7/target.c index 17dab51594..c1269441dd 100644 --- a/src/main/target/ANYFCF7/target.c +++ b/src/main/target/ANYFCF7/target.c @@ -31,8 +31,8 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, 0, 0 ), // S1_IN DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0 ), // S2_IN DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM, 0, 0 ), // S3_IN DMA2_ST2 DMA2_ST2 - DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM, 0, 0 ), // S4_IN DMA2_ST3 DMA2_ST2 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0, 0 ), // S5_IN DMA2_ST4 DMA2_ST2 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM, 0, 1 ), // S4_IN DMA2_ST3 DMA2_ST2 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0, 1 ), // S5_IN DMA2_ST4 DMA2_ST2 DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM, 0, 0 ), // S6_IN DMA2_ST7 DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, 0, 0 ), // S10_OUT 1 DMA1_ST7 diff --git a/src/main/target/ANYFCF7/target.h b/src/main/target/ANYFCF7/target.h index ef9fc4e30c..06b9020934 100644 --- a/src/main/target/ANYFCF7/target.h +++ b/src/main/target/ANYFCF7/target.h @@ -31,20 +31,22 @@ #define BEEPER_PIN PB2 // Unused pin, can be mapped to elsewhere #define BEEPER_INVERTED -#define MPU6000_CS_PIN PA4 -#define MPU6000_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 #define USE_ACC #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW270_DEG +#define ACC_1_ALIGN CW270_DEG #define USE_GYRO #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW270_DEG +#define GYRO_1_ALIGN CW270_DEG // MPU6000 interrupts #define USE_MPU_DATA_READY_SIGNAL -#define MPU_INT_EXTI PC4 +#define USE_EXTI +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC4 #define USE_EXTI #define USE_MAG @@ -133,16 +135,11 @@ #define MAX7456_RESTORE_CLK (SPI_CLOCK_FAST) #define USE_SDCARD +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED #define SDCARD_DETECT_PIN PD3 - #define SDCARD_SPI_INSTANCE SPI4 #define SDCARD_SPI_CS_PIN SPI4_NSS_PIN - -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 422kHz -// Divide to under 25MHz for normal operation: -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 8 // 27MHz - #define SDCARD_DMA_STREAM_TX_FULL DMA2_Stream1 #define SDCARD_DMA_CHANNEL 4 @@ -154,7 +151,7 @@ #define USE_ADC #define VBAT_ADC_PIN PC0 #define CURRENT_METER_ADC_PIN PC1 -#define RSSI_ADC_GPIO_PIN PC2 +#define RSSI_ADC_PIN PC2 #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT @@ -169,4 +166,4 @@ #define TARGET_IO_PORTD 0xffff #define TARGET_IO_PORTE 0xffff -#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9) | TIM_N(10) | TIM_N(11)) +#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(8) | TIM_N(9) | TIM_N(12) ) diff --git a/src/main/target/ANYFCF7/target.mk b/src/main/target/ANYFCF7/target.mk index 1145bf5015..25b9d7a30d 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_SPI VCP TARGET_SRC = \ drivers/accgyro/accgyro_spi_mpu6000.c \ diff --git a/src/main/target/ANYFCM7/target.c b/src/main/target/ANYFCM7/target.c index a88b85c0aa..06de11de18 100644 --- a/src/main/target/ANYFCM7/target.c +++ b/src/main/target/ANYFCM7/target.c @@ -31,8 +31,8 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, 0, 0 ), // S1_IN DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0 ), // S2_IN DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM, 0, 0 ), // S3_IN DMA2_ST2 DMA2_ST2 - DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM, 0, 0 ), // S4_IN DMA2_ST3 DMA2_ST2 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0, 0 ), // S5_IN DMA2_ST4 DMA2_ST2 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM, 0, 1 ), // S4_IN DMA2_ST3 DMA2_ST2 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0, 1 ), // S5_IN DMA2_ST4 DMA2_ST2 DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM, 0, 0 ), // S6_IN DMA2_ST7 DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, 0, 0 ), // S10_OUT 1 DMA1_ST7 diff --git a/src/main/target/ANYFCM7/target.h b/src/main/target/ANYFCM7/target.h index d9e3cf20d7..7e25321360 100644 --- a/src/main/target/ANYFCM7/target.h +++ b/src/main/target/ANYFCM7/target.h @@ -31,20 +31,22 @@ #define BEEPER_PIN PB2 // Unused pin, can be mapped to elsewhere #define BEEPER_INVERTED -#define MPU6000_CS_PIN PA4 -#define MPU6000_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 #define USE_ACC #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW270_DEG +#define ACC_1_ALIGN CW270_DEG #define USE_GYRO #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW270_DEG +#define GYRO_1_ALIGN CW270_DEG // MPU6000 interrupts #define USE_MPU_DATA_READY_SIGNAL -#define MPU_INT_EXTI PC4 +#define USE_EXTI +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC4 #define USE_EXTI #define USE_MAG diff --git a/src/main/target/BEEBRAIN_V2F/config.c b/src/main/target/BEEBRAIN_V2F/config.c index 70def80078..ef7573702e 100644 --- a/src/main/target/BEEBRAIN_V2F/config.c +++ b/src/main/target/BEEBRAIN_V2F/config.c @@ -150,6 +150,8 @@ void targetConfiguration(void) osdConfigMutable()->item_pos[OSD_NUMERICAL_VARIO] &= ~VISIBLE_FLAG; osdConfigMutable()->item_pos[OSD_ESC_TMP] &= ~VISIBLE_FLAG; osdConfigMutable()->item_pos[OSD_ESC_RPM] &= ~VISIBLE_FLAG; + osdConfigMutable()->item_pos[OSD_G_FORCE] &= ~VISIBLE_FLAG; + osdConfigMutable()->item_pos[OSD_FLIP_ARROW] &= ~VISIBLE_FLAG; modeActivationConditionsMutable(0)->modeId = BOXANGLE; modeActivationConditionsMutable(0)->auxChannelIndex = AUX2 - NON_AUX_CHANNEL_COUNT; diff --git a/src/main/target/BEEBRAIN_V2F/target.h b/src/main/target/BEEBRAIN_V2F/target.h index c5319c03c6..4bd12eb93c 100644 --- a/src/main/target/BEEBRAIN_V2F/target.h +++ b/src/main/target/BEEBRAIN_V2F/target.h @@ -27,29 +27,29 @@ #endif #define USE_TARGET_CONFIG -#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT #define LED0_PIN PB1 #define LED1_PIN PB2 #define USE_EXTI -#define MPU_INT_EXTI PB6 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PB6 #define USE_MPU_DATA_READY_SIGNAL #define USE_GYRO #define USE_GYRO_SPI_MPU6500 #if defined(BEESTORM) -#define GYRO_MPU6500_ALIGN CW180_DEG +#define GYRO_1_ALIGN CW180_DEG #else -#define GYRO_MPU6500_ALIGN CW270_DEG +#define GYRO_1_ALIGN CW270_DEG #endif #define USE_ACC #define USE_ACC_SPI_MPU6500 #if defined(BEESTORM) -#define ACC_MPU6500_ALIGN CW180_DEG +#define ACC_1_ALIGN CW180_DEG #else -#define ACC_MPU6500_ALIGN CW270_DEG +#define ACC_1_ALIGN CW270_DEG #endif #define SERIAL_PORT_COUNT 4 @@ -86,8 +86,8 @@ #define SPI3_MISO_PIN PB4 #define SPI3_MOSI_PIN PB5 -#define MPU6500_CS_PIN PA15 -#define MPU6500_SPI_INSTANCE SPI3 +#define GYRO_1_CS_PIN PA15 +#define GYRO_1_SPI_INSTANCE SPI3 #define USE_MAX7456 #define MAX7456_SPI_INSTANCE SPI1 diff --git a/src/main/target/BEEROTORF4/target.h b/src/main/target/BEEROTORF4/target.h index bb5592055f..297db18a2e 100644 --- a/src/main/target/BEEROTORF4/target.h +++ b/src/main/target/BEEROTORF4/target.h @@ -33,21 +33,22 @@ // ICM20689 interrupt #define USE_EXTI -#define MPU_INT_EXTI PA8 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PA8 //#define DEBUG_MPU_DATA_READY_INTERRUPT #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW -#define ICM20689_CS_PIN SPI1_NSS_PIN -#define ICM20689_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN SPI1_NSS_PIN +#define GYRO_1_SPI_INSTANCE SPI1 #define USE_ACC #define USE_ACC_SPI_ICM20689 -#define ACC_ICM20689_ALIGN CW270_DEG +#define ACC_1_ALIGN CW270_DEG #define USE_GYRO #define USE_GYRO_SPI_ICM20689 -#define GYRO_ICM20689_ALIGN CW270_DEG +#define GYRO_1_ALIGN CW270_DEG #define USE_BARO #define USE_BARO_BMP280 @@ -61,17 +62,11 @@ #define MAX7456_DMA_IRQ_HANDLER_ID DMA1_ST0_HANDLER #define USE_SDCARD - +#define USE_SDCARD_SPI #define SDCARD_DETECT_PIN PC3 #define SDCARD_DETECT_INVERTED #define SDCARD_SPI_INSTANCE SPI2 #define SDCARD_SPI_CS_PIN SPI2_NSS_PIN - -// SPI2 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init: -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz -// Divide to under 25MHz for normal operation: -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 // 21MHz - #define SDCARD_DMA_CHANNEL_TX DMA1_Stream4 #define SDCARD_DMA_CHANNEL 0 @@ -82,7 +77,6 @@ #define USE_UART1 #define UART1_RX_PIN PA10 #define UART1_TX_PIN PA9 -#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 //SerialRX #define USE_UART2 @@ -162,4 +156,4 @@ #define TARGET_IO_PORTD (BIT(2)) #define USABLE_TIMER_CHANNEL_COUNT 10 -#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(8) | TIM_N(9)) +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(9) ) diff --git a/src/main/target/BEEROTORF4/target.mk b/src/main/target/BEEROTORF4/target.mk index a54056c083..6b6cef764e 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 +FEATURES += VCP SDCARD_SPI TARGET_SRC = \ drivers/accgyro/accgyro_spi_icm20689.c \ diff --git a/src/main/target/BETAFLIGHTF3/target.h b/src/main/target/BETAFLIGHTF3/target.h index ff833c8bc2..956b93d182 100644 --- a/src/main/target/BETAFLIGHTF3/target.h +++ b/src/main/target/BETAFLIGHTF3/target.h @@ -25,13 +25,23 @@ #define TARGET_BOARD_IDENTIFIER "BFF3" // Removed to make the firmware fit into flash (in descending order of priority): +//#undef USE_GYRO_OVERFLOW_CHECK +//#undef USE_GYRO_LPF2 +//#undef USE_ITERM_RELAX +//#undef USE_RC_SMOOTHING_FILTER + +#undef USE_TELEMETRY_HOTT +#undef USE_TELEMETRY_MAVLINK +#undef USE_TELEMETRY_LTM +#undef USE_SERIALRX_XBUS + +#undef USE_BOARD_INFO #undef USE_EXTENDED_CMS_MENUS #undef USE_RTC_TIME #undef USE_RX_MSP #undef USE_ESC_SENSOR_INFO -#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE #define USE_BEEPER #define BEEPER_PIN PC15 @@ -41,20 +51,22 @@ #define USABLE_TIMER_CHANNEL_COUNT 10 -#define MPU6000_CS_PIN PA15 -#define MPU6000_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PA15 +#define GYRO_1_SPI_INSTANCE SPI1 #define USE_GYRO #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW180_DEG +#define GYRO_1_ALIGN CW180_DEG #define USE_ACC #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW180_DEG +#define ACC_1_ALIGN CW180_DEG // MPU6000 interrupts #define USE_MPU_DATA_READY_SIGNAL -#define MPU_INT_EXTI PC13 +#define USE_EXTI +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC13 #define USE_EXTI #define REMAP_TIM16_DMA @@ -108,16 +120,11 @@ #define MAX7456_RESTORE_CLK (SPI_CLOCK_FAST) #define USE_SDCARD - +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED #define SDCARD_DETECT_PIN PC14 - #define SDCARD_SPI_INSTANCE SPI2 #define SDCARD_SPI_CS_PIN SPI2_NSS_PIN - -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128 -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 2 - #define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 #define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC @@ -147,4 +154,4 @@ #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) -#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(16) | TIM_N(17) ) diff --git a/src/main/target/BETAFLIGHTF3/target.mk b/src/main/target/BETAFLIGHTF3/target.mk index 297f9be3f1..d470b1b5e3 100644 --- a/src/main/target/BETAFLIGHTF3/target.mk +++ b/src/main/target/BETAFLIGHTF3/target.mk @@ -1,6 +1,6 @@ F3_TARGETS += $(TARGET) -FEATURES = VCP SDCARD +FEATURES = VCP SDCARD_SPI TARGET_FLAGS = -DSPRACINGF3 TARGET_SRC = \ diff --git a/src/main/target/BETAFLIGHTF4/target.h b/src/main/target/BETAFLIGHTF4/target.h index 04b0b7a4ed..533cd15997 100644 --- a/src/main/target/BETAFLIGHTF4/target.h +++ b/src/main/target/BETAFLIGHTF4/target.h @@ -32,22 +32,23 @@ // PC13 used as inverter select GPIO for UART2 #define INVERTER_PIN_UART2 PC13 -#define MPU6000_CS_PIN PA4 -#define MPU6000_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 #define USE_ACC #define USE_ACC_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW180_DEG +#define GYRO_1_ALIGN CW180_DEG #define USE_GYRO #define USE_GYRO_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW180_DEG +#define ACC_1_ALIGN CW180_DEG // MPU6000 interrupts #define USE_EXTI -#define MPU_INT_EXTI PC4 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC4 #define USE_MPU_DATA_READY_SIGNAL #define USE_BARO @@ -57,6 +58,7 @@ #define USE_MAG #define USE_MAG_HMC5883 #define USE_MAG_QMC5883 +#define USE_MAG_LIS3MDL #define USE_MAX7456 #define MAX7456_SPI_INSTANCE SPI2 @@ -78,7 +80,6 @@ #define USE_UART1 #define UART1_RX_PIN PA10 #define UART1_TX_PIN PA9 -#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 #define USE_UART2 #define UART2_RX_PIN PA3 diff --git a/src/main/target/BETAFLIGHTF4/target.mk b/src/main/target/BETAFLIGHTF4/target.mk index ca33021603..c44fb08acb 100644 --- a/src/main/target/BETAFLIGHTF4/target.mk +++ b/src/main/target/BETAFLIGHTF4/target.mk @@ -9,4 +9,5 @@ TARGET_SRC = \ drivers/barometer/barometer_ms5611.c \ drivers/compass/compass_hmc5883l.c \ drivers/compass/compass_qmc5883l.c \ + drivers/compass/compass_lis3mdl.c \ drivers/max7456.c diff --git a/src/main/target/BLUEJAYF4/initialisation.c b/src/main/target/BLUEJAYF4/initialisation.c index 2ee012a2ce..ff17d47307 100644 --- a/src/main/target/BLUEJAYF4/initialisation.c +++ b/src/main/target/BLUEJAYF4/initialisation.c @@ -37,6 +37,8 @@ #include "hardware_revision.h" +#define UART1_INVERTER PC9 + void targetPreInit(void) { switch (hardwareRevision) { @@ -56,7 +58,7 @@ void targetPreInit(void) serialPortConfig_t *portConfig = serialFindPortConfiguration(SERIAL_PORT_USART1); if (portConfig) { bool smartportEnabled = (portConfig->functionMask & FUNCTION_TELEMETRY_SMARTPORT); - if (smartportEnabled && (!telemetryConfig()->telemetry_inverted) && (feature(FEATURE_TELEMETRY))) { + if (smartportEnabled && (!telemetryConfig()->telemetry_inverted) && (featureIsEnabled(FEATURE_TELEMETRY))) { high = true; } } diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index b346aefdac..e35384a72c 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -41,33 +41,31 @@ #define BEEPER_INVERTED #define INVERTER_PIN_UART6 PB15 -//#define INVERTER_PIN_UART1 PC9 - -#define UART1_INVERTER PC9 +//#define INVERTER_PIN_UART1 PC9 // Polarity depends on revision; handled in config.c // MPU6500 interrupt #define USE_EXTI -#define MPU_INT_EXTI PC5 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC5 #define USE_MPU_DATA_READY_SIGNAL //#define DEBUG_MPU_DATA_READY_INTERRUPT -#define MPU6500_CS_PIN PC4 -#define MPU6500_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PC4 +#define GYRO_1_SPI_INSTANCE SPI1 #define USE_ACC -#define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW0_DEG +#define ACC_1_ALIGN CW0_DEG #define USE_GYRO -#define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW0_DEG +#define GYRO_1_ALIGN CW0_DEG #define USE_MAG #define USE_MAG_HMC5883 #define USE_MAG_QMC5883 //#define USE_MAG_AK8963 +#define USE_MAG_LIS3MDL #define HMC5883_I2C_INSTANCE I2CDEV_1 #define USE_BARO @@ -75,18 +73,11 @@ #define MS5611_I2C_INSTANCE I2CDEV_1 #define USE_SDCARD - +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED - #define SDCARD_DETECT_PIN PD2 #define SDCARD_SPI_INSTANCE SPI3 #define SDCARD_SPI_CS_PIN PA15 - -// SPI2 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init: -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz -// Divide to under 25MHz for normal operation: -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 // 21MHz - #define SDCARD_DMA_CHANNEL_TX DMA1_Stream5 #define SDCARD_DMA_CHANNEL 0 @@ -175,4 +166,4 @@ #define TARGET_IO_PORTD (BIT(2)) #define USABLE_TIMER_CHANNEL_COUNT 8 -#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(8) | TIM_N(9) ) +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(8) ) diff --git a/src/main/target/BLUEJAYF4/target.mk b/src/main/target/BLUEJAYF4/target.mk index 6b7d592ae2..bbde2b8087 100644 --- a/src/main/target/BLUEJAYF4/target.mk +++ b/src/main/target/BLUEJAYF4/target.mk @@ -1,10 +1,10 @@ F405_TARGETS += $(TARGET) -FEATURES += SDCARD VCP ONBOARDFLASH +FEATURES += SDCARD_SPI VCP ONBOARDFLASH TARGET_SRC = \ drivers/accgyro/accgyro_spi_mpu6500.c \ drivers/accgyro/accgyro_mpu6500.c \ drivers/barometer/barometer_ms5611.c \ drivers/compass/compass_hmc5883l.c \ - drivers/compass/compass_qmc5883l.c - + drivers/compass/compass_qmc5883l.c \ + drivers/compass/compass_lis3mdl.c diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index e47eeee045..7ea23e995f 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -31,7 +31,8 @@ #define BEEPER_OPT PA2 #define USE_EXTI -#define MPU_INT_EXTI PA3 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PA3 #define USE_MPU_DATA_READY_SIGNAL //#define DEBUG_MPU_DATA_READY_INTERRUPT @@ -39,8 +40,8 @@ #define USE_SPI_DEVICE_1 #define USE_SPI_DEVICE_2 -#define MPU6000_CS_PIN PA4 -#define MPU6000_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 #define FLASH_CS_PIN PB12 #define FLASH_SPI_INSTANCE SPI2 @@ -50,11 +51,11 @@ #define USE_GYRO #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW270_DEG +#define GYRO_1_ALIGN CW270_DEG #define USE_ACC #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW270_DEG +#define ACC_1_ALIGN CW270_DEG // MPU6000 interrupts #define USE_MPU_DATA_READY_SIGNAL @@ -107,7 +108,6 @@ #undef USE_MAG #ifdef CC3D_OPBL -#define SKIP_CLI_COMMAND_HELP //#undef USE_SERVOS #undef USE_BARO #undef USE_RANGEFINDER @@ -120,7 +120,6 @@ #undef USE_SERIALRX_SUMD // Graupner Hott protocol #undef USE_SERIALRX_SUMH // Graupner legacy protocol #undef USE_SERIALRX_XBUS // JR -#undef USE_LED_STRIP #endif //#undef USE_LED_STRIP diff --git a/src/main/target/CHEBUZZF3/target.h b/src/main/target/CHEBUZZF3/target.h index c54d7d1d96..fc0b003195 100644 --- a/src/main/target/CHEBUZZF3/target.h +++ b/src/main/target/CHEBUZZF3/target.h @@ -22,7 +22,6 @@ #define TARGET_BOARD_IDENTIFIER "CHF3" // Chebuzz F3 -#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE #ifndef STM32F3DISCOVERY #define STM32F3DISCOVERY @@ -47,16 +46,11 @@ #define SPI2_MOSI_PIN PB15 #define USE_SDCARD - +#define USE_SDCARD_SPI #define SDCARD_DETECT_PIN PC14 #define SDCARD_SPI_INSTANCE SPI2 #define SDCARD_SPI_CS_PIN SPI2_NSS_PIN -// SPI2 is on the APB1 bus whose clock runs at 36MHz. Divide to under 400kHz for init: -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128 -// Divide to under 25MHz for normal operation: -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 2 - // Note, this is the same DMA channel as UART1_RX. Luckily we don't use DMA for USART Rx. #define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 @@ -65,12 +59,11 @@ #define USE_GYRO_MPU6050 //#define L3GD20_SPI SPI1 -//#define L3GD20_CS_GPIO_CLK_PERIPHERAL RCC_AHBPeriph_GPIOE //#define L3GD20_CS_GPIO GPIOE //#define L3GD20_CS_PIN PE3 //#define GYRO_L3GD20_ALIGN CW270_DEG -#define GYRO_MPU6050_ALIGN CW0_DEG +#define GYRO_1_ALIGN CW0_DEG #define USE_ACC #define USE_ACC_MPU6050 @@ -82,7 +75,7 @@ //#define LSM303DLHC_I2C_INT1_PIN PE4 //#define LSM303DLHC_I2C_INT2_PIN PE5 -#define ACC_MPU6050_ALIGN CW0_DEG +#define ACC_1_ALIGN CW0_DEG #define USE_BARO #define USE_BARO_MS5611 diff --git a/src/main/target/CHEBUZZF3/target.mk b/src/main/target/CHEBUZZF3/target.mk index cdf3c6d26e..11eca6ef24 100644 --- a/src/main/target/CHEBUZZF3/target.mk +++ b/src/main/target/CHEBUZZF3/target.mk @@ -1,17 +1,11 @@ F3_TARGETS += $(TARGET) -FEATURES = VCP SDCARD +FEATURES = VCP SDCARD_SPI TARGET_SRC = \ drivers/compass/compass_hmc5883l.c \ drivers/accgyro/accgyro_mpu.c \ drivers/accgyro/accgyro_mpu3050.c \ drivers/accgyro/accgyro_mpu6050.c \ - drivers/accgyro_legacy/accgyro_l3gd20.c \ - drivers/accgyro_legacy/accgyro_lsm303dlhc.c \ - drivers/accgyro_legacy/accgyro_adxl345.c \ - drivers/accgyro_legacy/accgyro_bma280.c \ - drivers/accgyro_legacy/accgyro_mma845x.c \ - drivers/accgyro_legacy/accgyro_l3g4200d.c \ drivers/barometer/barometer_ms5611.c \ drivers/barometer/barometer_bmp280.c \ drivers/compass/compass_ak8975.c diff --git a/src/main/target/CJMCU/config.c b/src/main/target/CJMCU/config.c new file mode 100644 index 0000000000..13668a4b17 --- /dev/null +++ b/src/main/target/CJMCU/config.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 . + */ + +#include +#include + +#include "platform.h" + +#ifdef USE_TARGET_CONFIG + +#include "drivers/io_types.h" +#include "hardware_revision.h" + +#include "pg/pg.h" +#include "pg/gyrodev.h" + +void targetConfiguration(void) +{ + gyroDeviceConfigMutable(0)->extiTag = selectMPUIntExtiConfigByHardwareRevision(); +} +#endif diff --git a/src/main/target/CJMCU/initialisation.c b/src/main/target/CJMCU/initialisation.c index 46b2ce5d13..21074ee54e 100644 --- a/src/main/target/CJMCU/initialisation.c +++ b/src/main/target/CJMCU/initialisation.c @@ -30,7 +30,7 @@ #include "pg/bus_spi.h" -extern void spiPreInit(void); // XXX In fc/fc_init.c +extern void spiPreInit(void); // XXX In fc/init.c void targetBusInit(void) { diff --git a/src/main/target/CJMCU/target.h b/src/main/target/CJMCU/target.h index 8e25aa985f..af0e65110f 100644 --- a/src/main/target/CJMCU/target.h +++ b/src/main/target/CJMCU/target.h @@ -22,6 +22,7 @@ #define TARGET_BOARD_IDENTIFIER "CJM1" // CJMCU #define USE_HARDWARE_REVISION_DETECTION +#define USE_TARGET_CONFIG #define TARGET_BUS_INIT #define LED0_PIN PC14 @@ -66,15 +67,13 @@ // Nordic Semiconductor uses 'CSN', STM uses 'NSS' #define RX_CE_PIN PA4 #define RX_NSS_PIN PA11 -#define RX_SCK_PIN PA5 -#define RX_MISO_PIN PA6 -#define RX_MOSI_PIN PA7 #define RX_IRQ_PIN PA8 // CJMCU has NSS on PA11, rather than the standard PA4 #define SPI1_NSS_PIN RX_NSS_PIN -#define SPI1_SCK_PIN RX_SCK_PIN -#define SPI1_MISO_PIN RX_MISO_PIN -#define SPI1_MOSI_PIN RX_MOSI_PIN + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 #define USE_RX_NRF24 #define USE_RX_CX10 @@ -104,8 +103,6 @@ #ifdef USE_SERIAL_RX #undef USE_SERIAL_RX #endif -//#undef SKIP_TASK_STATISTICS - #else #define DEFAULT_RX_FEATURE FEATURE_RX_PPM @@ -115,7 +112,7 @@ #define BRUSHED_MOTORS #define DEFAULT_FEATURES FEATURE_MOTOR_STOP -#define SKIP_SERIAL_PASSTHROUGH +#undef USE_SERIAL_PASSTHROUGH #undef USE_CLI // Since the CJMCU PCB has holes for 4 motors in each corner we can save same flash space by disabling support for other mixers. diff --git a/src/main/target/CLRACINGF4/target.c b/src/main/target/CLRACINGF4/target.c index fe531ec15e..292ffdbd5a 100644 --- a/src/main/target/CLRACINGF4/target.c +++ b/src/main/target/CLRACINGF4/target.c @@ -30,7 +30,7 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - DEF_TIM(TIM11, CH1, PB9, TIM_USE_ANY, 0, 0), // CAMERA_CONTROL_PIN + DEF_TIM(TIM11, CH1, PB9, TIM_USE_CAMERA_CONTROL, 0, 0), // CAMERA_CONTROL_PIN DEF_TIM(TIM1, CH2N, PB0, TIM_USE_MOTOR, 0, 0), // M1 - D2_ST6 DEF_TIM(TIM8, CH3N, PB1, TIM_USE_MOTOR, 0, 0), // M2_OUT D2_ST2 diff --git a/src/main/target/CLRACINGF4/target.h b/src/main/target/CLRACINGF4/target.h index e3dc0898d2..d101162a12 100644 --- a/src/main/target/CLRACINGF4/target.h +++ b/src/main/target/CLRACINGF4/target.h @@ -38,34 +38,29 @@ #define INVERTER_PIN_UART1 PC0 // PC0 used as inverter select GPIO -#define CAMERA_CONTROL_PIN PB9 // define dedicated camera_osd_control pin +#define CAMERA_CONTROL_PIN PB9 // define dedicated camera_osd_control pin #define USE_EXTI -#define MPU_INT_EXTI PC4 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC4 #define USE_MPU_DATA_READY_SIGNAL -// MPU 6000 -#define MPU6000_CS_PIN PA4 -#define MPU6000_SPI_INSTANCE SPI1 #define USE_ACC -#define USE_ACC_SPI_MPU6000 #define USE_GYRO + +// MPU 6000 +#define USE_ACC_SPI_MPU6000 #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW0_DEG -#define ACC_MPU6000_ALIGN CW0_DEG // ICM-20602 -#define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW0_DEG -#define GYRO_MPU6500_ALIGN CW0_DEG -#define MPU6500_CS_PIN PA4 -#define MPU6500_SPI_INSTANCE SPI1 - +#define GYRO_1_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 +#define GYRO_1_ALIGN CW0_DEG +#define ACC_1_ALIGN CW0_DEG #define USE_MAX7456 #define MAX7456_SPI_INSTANCE SPI3 @@ -75,13 +70,10 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define USE_SDCARD +#define USE_SDCARD_SPI #define SDCARD_DETECT_PIN PB7 #define SDCARD_SPI_INSTANCE SPI2 #define SDCARD_SPI_CS_PIN SPI2_NSS_PIN -// SPI2 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init: -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz -// Divide to under 25MHz for normal operation: -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 // 21MHz #define SDCARD_DMA_CHANNEL_TX DMA1_Stream4 #define SDCARD_DMA_CHANNEL 0 @@ -98,7 +90,6 @@ #define USE_UART1 #define UART1_RX_PIN PA10 #define UART1_TX_PIN PA9 -#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 #define USE_UART3 #define UART3_RX_PIN PB11 diff --git a/src/main/target/CLRACINGF4/target.mk b/src/main/target/CLRACINGF4/target.mk index a2703931df..c3c53e1ab5 100644 --- a/src/main/target/CLRACINGF4/target.mk +++ b/src/main/target/CLRACINGF4/target.mk @@ -1,5 +1,5 @@ F405_TARGETS += $(TARGET) -FEATURES = VCP SDCARD ONBOARDFLASH +FEATURES = VCP SDCARD_SPI ONBOARDFLASH TARGET_SRC = \ drivers/accgyro/accgyro_mpu.c \ drivers/accgyro/accgyro_mpu6500.c \ diff --git a/src/main/target/CLRACINGF7/target.c b/src/main/target/CLRACINGF7/target.c index 8b0dc6018d..68e297817f 100644 --- a/src/main/target/CLRACINGF7/target.c +++ b/src/main/target/CLRACINGF7/target.c @@ -28,7 +28,7 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - DEF_TIM(TIM2, CH2, PB3, TIM_USE_ANY, 0, 0), // USE FOR CAMERA CONTROL + DEF_TIM(TIM2, CH2, PB3, TIM_USE_CAMERA_CONTROL, 0, 0), // USE FOR CAMERA CONTROL DEF_TIM(TIM4, CH1, PB6, TIM_USE_MOTOR, 0, 0), // D1-ST6 DEF_TIM(TIM4, CH2, PB7, TIM_USE_MOTOR, 0, 0), // D1-ST6 diff --git a/src/main/target/CLRACINGF7/target.h b/src/main/target/CLRACINGF7/target.h index bae20e3641..cf2e3517bb 100644 --- a/src/main/target/CLRACINGF7/target.h +++ b/src/main/target/CLRACINGF7/target.h @@ -29,33 +29,28 @@ #define BEEPER_PIN PB4 #define BEEPER_INVERTED -//define camera control -#define CAMERA_CONTROL_PIN PB3 - #define USE_EXTI -#define MPU_INT_EXTI PC4 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC4 #define USE_MPU_DATA_READY_SIGNAL -//MPU-6000 + #define USE_ACC -#define USE_ACC_SPI_MPU6000 #define USE_GYRO + +//MPU-6000 +#define USE_ACC_SPI_MPU6000 #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW0_DEG -#define ACC_MPU6000_ALIGN CW0_DEG -#define MPU6000_CS_PIN PA4 -#define MPU6000_SPI_INSTANCE SPI1 - // ICM-20602 -#define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW0_DEG -#define GYRO_MPU6500_ALIGN CW0_DEG -#define MPU6500_CS_PIN SPI1_NSS_PIN -#define MPU6500_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 + +#define GYRO_1_ALIGN CW0_DEG +#define ACC_1_ALIGN CW0_DEG + #define USE_MAX7456 #define MAX7456_SPI_INSTANCE SPI3 @@ -70,12 +65,10 @@ //define use SD card #define USE_SDCARD +#define USE_SDCARD_SPI #define SDCARD_DETECT_PIN PA8 #define SDCARD_SPI_INSTANCE SPI2 #define SDCARD_SPI_CS_PIN SPI2_NSS_PIN -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 422kHz -// Divide to under 25MHz for normal operation: -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 8 // 27MHz #define SDCARD_DMA_STREAM_TX_FULL DMA1_Stream4 #define SDCARD_DMA_CHANNEL 0 diff --git a/src/main/target/CLRACINGF7/target.mk b/src/main/target/CLRACINGF7/target.mk index 2f2f023cd6..7073804f28 100644 --- a/src/main/target/CLRACINGF7/target.mk +++ b/src/main/target/CLRACINGF7/target.mk @@ -1,5 +1,5 @@ F7X2RE_TARGETS += $(TARGET) -FEATURES += VCP ONBOARDFLASH SDCARD +FEATURES += VCP ONBOARDFLASH SDCARD_SPI TARGET_SRC = \ drivers/accgyro/accgyro_mpu.c \ drivers/accgyro/accgyro_mpu6500.c \ diff --git a/src/main/target/COLIBRI/target.h b/src/main/target/COLIBRI/target.h index ae40d63a81..865cd1a4f9 100644 --- a/src/main/target/COLIBRI/target.h +++ b/src/main/target/COLIBRI/target.h @@ -37,20 +37,21 @@ #define INVERTER_PIN_UART2 PB2 // PB2 used as inverter select GPIO -#define MPU6000_CS_PIN PC4 -#define MPU6000_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PC4 +#define GYRO_1_SPI_INSTANCE SPI1 #define USE_ACC #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW270_DEG_FLIP +#define ACC_1_ALIGN CW270_DEG_FLIP #define USE_GYRO #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW270_DEG_FLIP +#define GYRO_1_ALIGN CW270_DEG_FLIP // MPU6000 interrupts #define USE_EXTI -#define MPU_INT_EXTI PC0 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC0 #define USE_MPU_DATA_READY_SIGNAL #define USE_MAG @@ -79,7 +80,6 @@ #define USE_UART1 #define UART1_RX_PIN PB7 #define UART1_TX_PIN PB6 -#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 #define USE_UART2 #define UART2_RX_PIN PA3 diff --git a/src/main/target/COLIBRI_RACE/bst.h b/src/main/target/COLIBRI_RACE/bst.h new file mode 100644 index 0000000000..c6b23af1db --- /dev/null +++ b/src/main/target/COLIBRI_RACE/bst.h @@ -0,0 +1,28 @@ +/* + * 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 + +#define BST_DEVICE_NAME "COLIBRI RACE" +#define BST_DEVICE_NAME_LENGTH 12 +#define BST_DEVICE (BSTDEV_1) +/* Configure the CRC peripheral to use the polynomial x8 + x7 + x6 + x4 + x2 + 1 */ +#define BST_CRC_POLYNOM 0xD5 + diff --git a/src/main/target/COLIBRI_RACE/bus_bst_stm32f30x.c b/src/main/target/COLIBRI_RACE/bus_bst_stm32f30x.c index 17b99f7b1c..0a6e818d2f 100644 --- a/src/main/target/COLIBRI_RACE/bus_bst_stm32f30x.c +++ b/src/main/target/COLIBRI_RACE/bus_bst_stm32f30x.c @@ -10,6 +10,8 @@ #include "platform.h" +#ifdef USE_BST + #include "build/build_config.h" #include "drivers/nvic.h" @@ -17,10 +19,9 @@ #include "drivers/io_impl.h" #include "drivers/rcc.h" +#include "bst.h" #include "bus_bst.h" - -#ifdef USE_BST #define NVIC_PRIO_BST_READ_DATA NVIC_BUILD_PRIORITY(1, 1) #define BST_SHORT_TIMEOUT ((uint32_t)0x1000) diff --git a/src/main/target/COLIBRI_RACE/config.c b/src/main/target/COLIBRI_RACE/config.c new file mode 100644 index 0000000000..02f1203136 --- /dev/null +++ b/src/main/target/COLIBRI_RACE/config.c @@ -0,0 +1,49 @@ +/* + * 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 . + */ + +#include +#include + +#include "platform.h" + +#ifdef USE_BST + +#include "bst.h" +#include "bus_bst.h" +#include "pg/bus_i2c.h" +#include "pg/bus_spi.h" + +// XXX Requires some additional work here. +// XXX Can't do this now without proper semantics about I2C on this target. +void targetBusInit(void) +{ +#ifdef USE_SPI + spiPinConfigure(spiPinConfig(0)); +#ifdef USE_SPI_DEVICE_1 + spiInit(SPIDEV_1); +#endif +#endif + + i2cHardwareConfigure(i2cConfig(0)); + i2cInit(I2CDEV_2); + + bstInit(BST_DEVICE); +} +#endif diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index f74f84d624..b2321cf4e7 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -27,7 +27,7 @@ #include "fc/config.h" #include "fc/controlrate_profile.h" -#include "fc/fc_core.h" +#include "fc/core.h" #include "fc/rc_adjustments.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" @@ -153,26 +153,17 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { BOXARM, "ARM;", 0 }, { BOXANGLE, "ANGLE;", 1 }, { BOXHORIZON, "HORIZON;", 2 }, - { BOXBARO, "BARO;", 3 }, //{ BOXVARIO, "VARIO;", 4 }, { BOXMAG, "MAG;", 5 }, { BOXHEADFREE, "HEADFREE;", 6 }, { BOXHEADADJ, "HEADADJ;", 7 }, { BOXCAMSTAB, "CAMSTAB;", 8 }, - { BOXCAMTRIG, "CAMTRIG;", 9 }, - { BOXGPSHOME, "GPS HOME;", 10 }, - { BOXGPSHOLD, "GPS HOLD;", 11 }, { BOXPASSTHRU, "PASSTHRU;", 12 }, { BOXBEEPERON, "BEEPER;", 13 }, - { BOXLEDMAX, "LEDMAX;", 14 }, { BOXLEDLOW, "LEDLOW;", 15 }, - { BOXLLIGHTS, "LLIGHTS;", 16 }, { BOXCALIB, "CALIB;", 17 }, - { BOXGOV, "GOVERNOR;", 18 }, { BOXOSD, "OSD DISABLE SW;", 19 }, { BOXTELEMETRY, "TELEMETRY;", 20 }, - { BOXGTUNE, "GTUNE;", 21 }, - { BOXRANGEFINDER, "RANGEFINDER;", 22 }, { BOXSERVO1, "SERVO1;", 23 }, { BOXSERVO2, "SERVO2;", 24 }, { BOXSERVO3, "SERVO3;", 25 }, @@ -300,25 +291,16 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) junk = 0; tmp = IS_ENABLED(FLIGHT_MODE(ANGLE_MODE)) << BOXANGLE | IS_ENABLED(FLIGHT_MODE(HORIZON_MODE)) << BOXHORIZON | - IS_ENABLED(FLIGHT_MODE(BARO_MODE)) << BOXBARO | IS_ENABLED(FLIGHT_MODE(MAG_MODE)) << BOXMAG | IS_ENABLED(FLIGHT_MODE(HEADFREE_MODE)) << BOXHEADFREE | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHEADADJ)) << BOXHEADADJ | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMSTAB)) << BOXCAMSTAB | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMTRIG)) << BOXCAMTRIG | - IS_ENABLED(FLIGHT_MODE(GPS_HOME_MODE)) << BOXGPSHOME | - IS_ENABLED(FLIGHT_MODE(GPS_HOLD_MODE)) << BOXGPSHOLD | IS_ENABLED(FLIGHT_MODE(PASSTHRU_MODE)) << BOXPASSTHRU | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBEEPERON)) << BOXBEEPERON | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDMAX)) << BOXLEDMAX | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDLOW)) << BOXLEDLOW | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLLIGHTS)) << BOXLLIGHTS | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCALIB)) << BOXCALIB | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGOV)) << BOXGOV | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)) << BOXOSD | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)) << BOXTELEMETRY | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGTUNE)) << BOXGTUNE | - IS_ENABLED(FLIGHT_MODE(RANGEFINDER_MODE)) << BOXRANGEFINDER | IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX | IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE; @@ -570,10 +552,10 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) readEEPROM(); break; case BST_SET_FEATURE: - featureClearAll(); - featureSet(bstRead32()); // features bitmap + featureDisableAll(); + featureEnable(bstRead32()); // features bitmap #ifdef SERIALRX_UART - if (featureConfigured(FEATURE_RX_SERIAL)) { + if (featureIsEnabled(FEATURE_RX_SERIAL)) { serialConfigMutable()->portConfigs[SERIALRX_UART].functionMask = FUNCTION_RX_SERIAL; } else { serialConfigMutable()->portConfigs[SERIALRX_UART].functionMask = FUNCTION_NONE; @@ -808,23 +790,23 @@ static void bstMasterWrite32(uint32_t data) static int32_t lat = 0; static int32_t lon = 0; -static uint16_t alt = 0; +static uint16_t altM = 0; static uint8_t numOfSat = 0; #endif #ifdef USE_GPS bool writeGpsPositionPrameToBST(void) { - if ((lat != gpsSol.llh.lat) || (lon != gpsSol.llh.lon) || (alt != gpsSol.llh.alt) || (numOfSat != gpsSol.numSat)) { + if ((lat != gpsSol.llh.lat) || (lon != gpsSol.llh.lon) || (alt != (gpsSol.llh.altCm / 100)) || (numOfSat != gpsSol.numSat)) { lat = gpsSol.llh.lat; lon = gpsSol.llh.lon; - alt = gpsSol.llh.alt; + altM = gpsSol.llh.altCm / 100; numOfSat = gpsSol.numSat; uint16_t speed = (gpsSol.groundSpeed * 9 / 25); uint16_t gpsHeading = 0; uint16_t altitude = 0; gpsHeading = gpsSol.groundCourse * 10; - altitude = alt * 10 + 1000; + altitude = altM + 1000; // To be verified: in m +1000m offset for neg. altitudes? bstMasterStartBuffer(PUBLIC_ADDRESS); bstMasterWrite8(GPS_POSITION_FRAME_ID); @@ -875,10 +857,8 @@ bool writeFCModeToBST(void) tmp = IS_ENABLED(ARMING_FLAG(ARMED)) | IS_ENABLED(FLIGHT_MODE(ANGLE_MODE)) << 1 | IS_ENABLED(FLIGHT_MODE(HORIZON_MODE)) << 2 | - IS_ENABLED(FLIGHT_MODE(BARO_MODE)) << 3 | IS_ENABLED(FLIGHT_MODE(MAG_MODE)) << 4 | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << 5 | - IS_ENABLED(FLIGHT_MODE(RANGEFINDER_MODE)) << 6 | IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << 7; bstMasterStartBuffer(PUBLIC_ADDRESS); diff --git a/src/main/target/COLIBRI_RACE/target.c b/src/main/target/COLIBRI_RACE/target.c index 814b315b0a..139f1619e4 100644 --- a/src/main/target/COLIBRI_RACE/target.c +++ b/src/main/target/COLIBRI_RACE/target.c @@ -52,22 +52,3 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { DEF_TIM(TIM16, CH1, PA6, TIM_USE_LED, 0), // PWM11 - PB15 }; - -// XXX Requires some additional work here. -// XXX Can't do this now without proper semantics about I2C on this target. -#ifdef USE_BST -void targetBusInit(void) -{ -#ifdef USE_SPI - spiPinConfigure(spiPinConfig(0)); -#ifdef USE_SPI_DEVICE_1 - spiInit(SPIDEV_1); -#endif -#endif - - i2cHardwareConfigure(i2cConfig(0)); - i2cInit(I2CDEV_2); - - bstInit(BST_DEVICE); -} -#endif diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index e5d001cf4f..f028185611 100644 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -22,14 +22,11 @@ // Removed to make the firmware fit into flash (in descending order of priority): #undef USE_GYRO_OVERFLOW_CHECK // target does not use affected gyros -#undef USE_RTC_TIME +//#undef USE_RTC_TIME #define TARGET_BOARD_IDENTIFIER "CLBR" -#define BST_DEVICE_NAME "COLIBRI RACE" -#define BST_DEVICE_NAME_LENGTH 12 -#define TARGET_BUS_INIT -#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT +#define TARGET_BUS_INIT #define LED0_PIN PC15 #define LED1_PIN PC14 @@ -41,7 +38,8 @@ // MPU6500 interrupt #define USE_EXTI -#define MPU_INT_EXTI PA5 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PA5 #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW @@ -53,25 +51,18 @@ #define SPI1_MOSI_PIN PB5 #define SPI1_NSS_PIN PA4 -#define MPU6500_CS_PIN SPI1_NSS_PIN -#define MPU6500_SPI_INSTANCE SPI1 - -#define MPU6000_CS_PIN SPI1_NSS_PIN -#define MPU6000_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN SPI1_NSS_PIN +#define GYRO_1_SPI_INSTANCE SPI1 #define USE_GYRO #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW270_DEG -#define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW270_DEG +#define GYRO_1_ALIGN CW270_DEG #define USE_ACC #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW270_DEG -#define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW270_DEG +#define ACC_1_ALIGN CW270_DEG #define USE_BARO #define USE_BARO_MS5611 @@ -113,9 +104,6 @@ #define I2C2_SDA_PIN PA10 #define USE_BST -#define BST_DEVICE (BSTDEV_1) -/* Configure the CRC peripheral to use the polynomial x8 + x7 + x6 + x4 + x2 + 1 */ -#define BST_CRC_POLYNOM 0xD5 #define USE_ADC #define ADC_INSTANCE ADC1 @@ -139,4 +127,4 @@ #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) #define USABLE_TIMER_CHANNEL_COUNT 12 -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(15) | TIM_N(16)) diff --git a/src/main/target/CRAZYBEEF3FR/CRAZYBEEF3DX.mk b/src/main/target/CRAZYBEEF3FR/CRAZYBEEF3DX.mk new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/main/target/CRAZYBEEF3FR/target.c b/src/main/target/CRAZYBEEF3FR/target.c index 0361492b8e..62adec9f19 100644 --- a/src/main/target/CRAZYBEEF3FR/target.c +++ b/src/main/target/CRAZYBEEF3FR/target.c @@ -34,4 +34,6 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // TIM2_UP, DMA1_CH2 DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, 0), DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, 0), + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MOTOR, 0), //PB6 for servo + DEF_TIM(TIM3, CH1, PB4, TIM_USE_LED, 0), //LED_STRIP }; diff --git a/src/main/target/CRAZYBEEF3FR/target.h b/src/main/target/CRAZYBEEF3FR/target.h index 668c451228..54d2bd52f7 100644 --- a/src/main/target/CRAZYBEEF3FR/target.h +++ b/src/main/target/CRAZYBEEF3FR/target.h @@ -23,11 +23,64 @@ #if defined(CRAZYBEEF3FS) #define TARGET_BOARD_IDENTIFIER "CBFS" #define USBD_PRODUCT_STRING "CrazyBee F3 FS" -#else //CRAZYBEEF3FS +#elif defined(CRAZYBEEF3DX) +#define TARGET_BOARD_IDENTIFIER "CBDX" +#define USBD_PRODUCT_STRING "CrazyBee F3 DX" +#else #define TARGET_BOARD_IDENTIFIER "CBFR" #define USBD_PRODUCT_STRING "CrazyBee F3 FR" #endif +// Removed to make the firmware fit into flash (in descending order of priority): +//#undef USE_GYRO_OVERFLOW_CHECK +//#undef USE_GYRO_LPF2 + +//#undef USE_ITERM_RELAX +//#undef USE_RC_SMOOTHING_FILTER + +//#undef USE_MSP_DISPLAYPORT +//#undef USE_MSP_OVER_TELEMETRY + +//#undef USE_HUFFMAN +//#undef USE_PINIO +//#undef USE_PINIOBOX + +#undef USE_SERIALRX_CRSF +#undef USE_SERIALRX_SUMD +#undef USE_TELEMETRY_CRSF + +#if defined(CRAZYBEEF3FS) +#undef USE_SERIALRX_SBUS +#undef USE_SERIALRX_FPORT +#undef USE_SERIALRX_SPEKTRUM +#undef USE_TELEMETRY_FRSKY_HUB +#undef USE_TELEMETRY_SMARTPORT +#undef USE_TELEMETRY_SRXL +#elif defined(CRAZYBEEF3DX) +#undef USE_SERIALRX_SBUS +#undef USE_SERIALRX_FPORT +#undef USE_SERIALRX_IBUS +#undef USE_TELEMETRY_FRSKY_HUB +#undef USE_TELEMETRY_SMARTPORT +#else +#undef USE_SERIALRX_SPEKTRUM +#undef USE_SERIALRX_IBUS +#undef USE_TELEMETRY_SRXL +#endif + +#undef USE_TELEMETRY_HOTT +#undef USE_TELEMETRY_MAVLINK +#undef USE_TELEMETRY_LTM +#undef USE_SERIALRX_XBUS +#undef USE_SERIALRX_SUMH +#undef USE_PWM + +//#undef USE_BOARD_INFO +//#undef USE_EXTENDED_CMS_MENUS +//#undef USE_RTC_TIME +#undef USE_RX_MSP +//#undef USE_ESC_SENSOR_INFO + #define ENABLE_DSHOT_DMAR true #define LED0_PIN PB3 @@ -36,22 +89,33 @@ #define BEEPER_INVERTED #define USE_EXTI -#define MPU_INT_EXTI PC13 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC13 #define USE_MPU_DATA_READY_SIGNAL -#define MPU6000_SPI_INSTANCE SPI1 -#define MPU6000_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PA4 #define USE_GYRO #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW90_DEG +#define GYRO_1_ALIGN CW90_DEG #define USE_ACC #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW90_DEG +#define ACC_1_ALIGN CW90_DEG #define USE_VCP +#if defined(CRAZYBEEF3DX) +#define USE_UART2 +#define USE_UART3 +#define SERIAL_PORT_COUNT 3 +#define UART2_TX_PIN PA14 +#define UART2_RX_PIN PA15 +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 +#else #define USE_UART3 #define SERIAL_PORT_COUNT 2 #define UART3_TX_PIN PB10 #define UART3_RX_PIN PB11 +#endif #define USE_SPI #define USE_SPI_DEVICE_1 @@ -69,6 +133,8 @@ #if defined(CRAZYBEEF3FS) #define USE_RX_SPI #define USE_RX_FLYSKY +#define RX_CHANNELS_AETR +#define DEFAULT_RX_FEATURE FEATURE_RX_SPI #define RX_SPI_DEFAULT_PROTOCOL RX_SPI_A7105_FLYSKY_2A #define FLYSKY_2A_CHANNEL_COUNT 14 #define RX_SPI_INSTANCE SPI2 @@ -77,24 +143,29 @@ #define BINDPLUG_PIN PA9 #define USE_RX_FLYSKY_SPI_LED #define RX_FLYSKY_SPI_LED_PIN PA10 +#define DEFAULT_FEATURES (FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_MOTOR_STOP) +#elif defined(CRAZYBEEF3DX) +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SPEKTRUM2048 +#define SERIALRX_UART SERIAL_PORT_USART3 +#define RX_CHANNELS_TAER +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_MOTOR_STOP) #else #define USE_RX_SPI #define USE_RX_FRSKY_SPI_D #define USE_RX_FRSKY_SPI_X #define USE_RX_FRSKY_SPI_TELEMETRY +#define USE_RX_SFHSS_SPI #define DEFAULT_RX_FEATURE FEATURE_RX_SPI #define RX_SPI_DEFAULT_PROTOCOL RX_SPI_FRSKY_X #define RX_SPI_INSTANCE SPI2 #define RX_NSS_PIN SPI2_NSS_PIN -#define RX_SCK_PIN SPI2_SCK_PIN -#define RX_MISO_PIN SPI2_MISO_PIN -#define RX_MOSI_PIN SPI2_MOSI_PIN -#define RX_FRSKY_SPI_GDO_0_PIN PA8 -#define RX_FRSKY_SPI_LED_PIN PA10 +#define RX_CC2500_SPI_GDO_0_PIN PA8 +#define RX_CC2500_SPI_LED_PIN PA10 #define BINDPLUG_PIN PA9 +#define DEFAULT_FEATURES (FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_MOTOR_STOP) #endif -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_INSTANCE SPI1 #define MAX7456_SPI_CS_PIN PB1 @@ -109,11 +180,10 @@ #define ADC_INSTANCE ADC1 #define CURRENT_METER_SCALE_DEFAULT 2350 -#define DEFAULT_FEATURES (FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_RX_SPI | FEATURE_MOTOR_STOP) #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) -#define USABLE_TIMER_CHANNEL_COUNT 4 -#define USED_TIMERS (TIM_N(2) | TIM_N(8) | TIM_N(15)) +#define USABLE_TIMER_CHANNEL_COUNT 6 +#define USED_TIMERS (TIM_N(2) |TIM_N(3) |TIM_N(4) | TIM_N(8)) diff --git a/src/main/target/CRAZYBEEF3FR/target.mk b/src/main/target/CRAZYBEEF3FR/target.mk index eca394fa08..a3dad5d7de 100644 --- a/src/main/target/CRAZYBEEF3FR/target.mk +++ b/src/main/target/CRAZYBEEF3FR/target.mk @@ -15,8 +15,10 @@ else ifeq ($(TARGET), CRAZYBEEF3FR) TARGET_SRC += \ drivers/rx/rx_cc2500.c \ + rx/cc2500_common.c \ rx/cc2500_frsky_shared.c \ rx/cc2500_frsky_d.c \ - rx/cc2500_frsky_x.c + rx/cc2500_frsky_x.c \ + rx/cc2500_sfhss.c endif endif diff --git a/src/main/target/CRAZYFLIE2/target.h b/src/main/target/CRAZYFLIE2/target.h index c98588c1e7..9672759c48 100644 --- a/src/main/target/CRAZYFLIE2/target.h +++ b/src/main/target/CRAZYFLIE2/target.h @@ -37,11 +37,11 @@ #define USBD_PRODUCT_STRING "Crazyflie 2.0" #endif -#define USABLE_TIMER_CHANNEL_COUNT 14 - #if defined(CRAZYFLIE2BQ) +#define USABLE_TIMER_CHANNEL_COUNT 5 #define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(14) ) #else +#define USABLE_TIMER_CHANNEL_COUNT 4 #define USED_TIMERS ( TIM_N(2) | TIM_N(4) ) #endif @@ -82,17 +82,20 @@ #define USE_I2C_DEVICE_3 #define I2C_DEVICE (I2CDEV_3) +// This board only uses I2C acc/gyro +#undef USE_MULTI_GYRO + // MPU9250 has the AD0 pin held high so the // address is 0x69 instead of the default 0x68 #define MPU_ADDRESS 0x69 #define USE_GYRO #define USE_GYRO_MPU6500 -#define GYRO_MPU6500_ALIGN CW270_DEG +#define GYRO_1_ALIGN CW270_DEG #define USE_ACC #define USE_ACC_MPU6500 -#define ACC_MPU6500_ALIGN CW270_DEG +#define ACC_1_ALIGN CW270_DEG #define USE_MAG #define USE_MPU9250_MAG // Enables bypass configuration on the MPU9250 I2C bus @@ -100,7 +103,8 @@ #define MAG_AK8963_ALIGN CW270_DEG #define USE_EXTI -#define MPU_INT_EXTI PC13 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC13 #define USE_SERIALRX_TARGET_CUSTOM #define SERIALRX_UART SERIAL_PORT_USART6 diff --git a/src/main/target/DALRCF405/target.c b/src/main/target/DALRCF405/target.c index 59f0a571ab..d969a1c304 100644 --- a/src/main/target/DALRCF405/target.c +++ b/src/main/target/DALRCF405/target.c @@ -42,6 +42,6 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), // LED STRIP(1,0) - DEF_TIM(TIM2, CH1, PA5, TIM_USE_PWM, 0, 0), // FC CAM + DEF_TIM(TIM2, CH1, PA5, TIM_USE_CAMERA_CONTROL, 0, 0), // FC CAM }; diff --git a/src/main/target/DALRCF405/target.h b/src/main/target/DALRCF405/target.h index 636524a3b2..65f97ee591 100644 --- a/src/main/target/DALRCF405/target.h +++ b/src/main/target/DALRCF405/target.h @@ -31,9 +31,6 @@ #define BEEPER_PIN PC13 #define BEEPER_INVERTED -//define camera control -#define CAMERA_CONTROL_PIN PA5 - //Gyro & ACC------------------------------- #define USE_SPI #define USE_SPI_DEVICE_1 @@ -43,29 +40,24 @@ #define SPI1_MOSI_PIN PA7 #define USE_EXTI -#define MPU_INT_EXTI PC4 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC4 #define USE_MPU_DATA_READY_SIGNAL #define USE_GYRO #define USE_ACC +#define GYRO_1_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 +#define GYRO_1_ALIGN CW90_DEG +#define ACC_1_ALIGN CW90_DEG + //------ICM20689 -#define ICM20689_CS_PIN PA4 -#define ICM20689_SPI_INSTANCE SPI1 - #define USE_GYRO_SPI_ICM20689 -#define GYRO_ICM20689_ALIGN CW90_DEG - #define USE_ACC_SPI_ICM20689 -#define ACC_ICM20689_ALIGN CW90_DEG + //------MPU6000 -#define MPU6000_CS_PIN PA4 -#define MPU6000_SPI_INSTANCE SPI1 - #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW90_DEG - #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW90_DEG //Baro & MAG------------------------------- #define USE_I2C @@ -78,6 +70,7 @@ #define USE_MAG_AK8975 #define USE_MAG_HMC5883 #define USE_MAG_QMC5883 +#define USE_MAG_LIS3MDL //ON BOARD FLASH ----------------------------------- #define USE_SPI_DEVICE_2 @@ -101,7 +94,6 @@ #define SPI3_MISO_PIN PC11 #define SPI3_MOSI_PIN PB5 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_INSTANCE SPI3 #define MAX7456_SPI_CS_PIN PA15 diff --git a/src/main/target/DALRCF405/target.mk b/src/main/target/DALRCF405/target.mk index 0018c55e87..34b772ffc3 100644 --- a/src/main/target/DALRCF405/target.mk +++ b/src/main/target/DALRCF405/target.mk @@ -9,4 +9,5 @@ TARGET_SRC = \ drivers/compass/compass_ak8975.c \ drivers/compass/compass_hmc5883l.c \ drivers/compass/compass_qmc5883l.c \ + drivers/compass/compass_lis3mdl.c \ drivers/max7456.c diff --git a/src/main/target/DALRCF722DUAL/config.c b/src/main/target/DALRCF722DUAL/config.c new file mode 100644 index 0000000000..f07d7206a9 --- /dev/null +++ b/src/main/target/DALRCF722DUAL/config.c @@ -0,0 +1,39 @@ +/* + * 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 . + */ + +#include + +#include "platform.h" + + +#ifdef TARGET_VALIDATECONFIG + +#include "fc/config.h" + +#include "sensors/gyro.h" + +void targetValidateConfiguration(void) +{ + if (gyroConfig()->gyro_use_32khz && gyroConfig()->gyroMovementCalibrationThreshold < 148) { + gyroConfigMutable()->gyroMovementCalibrationThreshold = 148; + } +} + +#endif diff --git a/src/main/target/DALRCF722DUAL/target.c b/src/main/target/DALRCF722DUAL/target.c new file mode 100644 index 0000000000..4688464161 --- /dev/null +++ b/src/main/target/DALRCF722DUAL/target.c @@ -0,0 +1,45 @@ +/* + * 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 . + */ + +#include + +#include "platform.h" +#include "drivers/io.h" + +#include "drivers/dma.h" +#include "drivers/timer.h" +#include "drivers/timer_def.h" + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + DEF_TIM(TIM4, CH2, PB7, TIM_USE_PPM, 0, 0), // PPM&SBUS + + DEF_TIM(TIM8, CH1, PC6, TIM_USE_MOTOR, 0, 0), // S1 (2,5) + DEF_TIM(TIM8, CH2, PC7, TIM_USE_MOTOR, 0, 0), // S2 (2,5) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, 0, 0), // S3 (2,5) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MOTOR, 0, 0), // S4 (2,5) + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, 0, 0), // S5 (2,1) + DEF_TIM(TIM1, CH2, PA9, TIM_USE_MOTOR, 0, 0), // S6 (2,1) + + + DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), // LED STRIP(1,5) + + DEF_TIM(TIM3, CH4, PB1, TIM_USE_CAMERA_CONTROL, 0, 0), // FC CAM + +}; diff --git a/src/main/target/DALRCF722DUAL/target.h b/src/main/target/DALRCF722DUAL/target.h new file mode 100644 index 0000000000..8128de608a --- /dev/null +++ b/src/main/target/DALRCF722DUAL/target.h @@ -0,0 +1,161 @@ +/* + * 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 +#define TARGET_VALIDATECONFIG + +#define TARGET_BOARD_IDENTIFIER "DLF7" +#define USBD_PRODUCT_STRING "DALRCF722DUAL" + +#define ENABLE_DSHOT_DMAR true +#define LED0_PIN PC14 + +#define USE_BEEPER +#define BEEPER_PIN PC13 +#define BEEPER_INVERTED + +#define USE_DUAL_GYRO +#define USE_EXTI +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PB10 +#define GYRO_2_EXTI_PIN PC4 + +#define GYRO_1_CS_PIN PB0 +#define GYRO_1_SPI_INSTANCE SPI1 +#define GYRO_2_CS_PIN PA4 +#define GYRO_2_SPI_INSTANCE SPI1 + +#define USE_MPU_DATA_READY_SIGNAL +#define ENSURE_MPU_DATA_READY_IS_LOW + +#define USE_GYRO +#define USE_GYRO_SPI_MPU6000 +#define USE_GYRO_SPI_MPU6500 + +#define USE_ACC +#define USE_ACC_SPI_MPU6000 +#define USE_ACC_SPI_MPU6500 + +#define GYRO_1_ALIGN CW180_DEG +#define ACC_1_ALIGN CW180_DEG + +#define GYRO_2_ALIGN CW0_DEG +#define ACC_2_ALIGN CW0_DEG + +#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1 + +#define USE_BARO +#define USE_BARO_MS5611 + +#define USE_MAG +#define USE_MAG_HMC5883 +#define USE_MAG_LIS3MDL + +#define USE_VCP +#define USE_UART1 +#define USE_UART2 +#define USE_UART3 +#define USE_UART4 +#define USE_UART5 + +#define UART1_TX_PIN PB6 +#define UART1_RX_PIN PB7 + +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define UART3_TX_PIN PC10 +#define UART3_RX_PIN PC11 + +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#define UART5_TX_PIN PC12 +#define UART5_RX_PIN PD2 +#define SERIAL_PORT_COUNT 6 + +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C_DEVICE I2CDEV_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define USE_SPI + +#define USE_SPI_DEVICE_1 // 2xGYRO/ACC +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_SPI_DEVICE_2 // MAX7456 +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_MAX7456 +#define MAX7456_SPI_INSTANCE SPI2 +#define MAX7456_SPI_CS_PIN SPI2_NSS_PIN + +#define USE_SPI_DEVICE_3 // FLASH +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define FLASH_CS_PIN PB2 +#define FLASH_SPI_INSTANCE SPI3 + +#define USE_ADC +#define ADC_INSTANCE ADC3 +#define ADC3_DMA_STREAM DMA2_Stream0 + +#define VBAT_ADC_PIN PC1 +#define CURRENT_METER_ADC_PIN PC0 +#define RSSI_ADC_PIN PA0 + +#define CURRENT_METER_SCALE_DEFAULT 166 +#define VBAT_SCALE_DEFAULT 160 + +#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC +#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC + +#define USE_OSD + +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define DEFAULT_FEATURES FEATURE_OSD +#define SERIALRX_UART SERIAL_PORT_USART1 +#define SERIALRX_PROVIDER SERIALRX_SBUS + + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) + +#define USABLE_TIMER_CHANNEL_COUNT 9 +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) |TIM_N(8) ) + + diff --git a/src/main/target/DALRCF722DUAL/target.mk b/src/main/target/DALRCF722DUAL/target.mk new file mode 100644 index 0000000000..6603b9ea3e --- /dev/null +++ b/src/main/target/DALRCF722DUAL/target.mk @@ -0,0 +1,14 @@ +F7X2RE_TARGETS += $(TARGET) +FEATURES += VCP ONBOARDFLASH + +TARGET_SRC = \ + drivers/accgyro/accgyro_mpu6500.c \ + drivers/accgyro/accgyro_spi_mpu6000.c \ + drivers/accgyro/accgyro_spi_mpu6500.c \ + drivers/barometer/barometer_bmp280.c \ + drivers/barometer/barometer_ms5611.c \ + drivers/compass/compass_hmc5883l.c \ + drivers/compass/compass_lis3mdl.c \ + drivers/max7456.c + + diff --git a/src/main/target/DOGE/target.h b/src/main/target/DOGE/target.h index 30e5449725..de7071fe09 100644 --- a/src/main/target/DOGE/target.h +++ b/src/main/target/DOGE/target.h @@ -22,7 +22,6 @@ #define TARGET_BOARD_IDENTIFIER "DOGE" -#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT // tqfp48 pin 34 #define LED0_PIN PA13 @@ -58,10 +57,8 @@ #define SPI2_NSS_PIN PB12 // tqfp48 pin 3 -#define MPU6500_CS_PIN SPI1_NSS_PIN -#define MPU6500_SPI_INSTANCE SPI1 -#define MPU6000_CS_PIN SPI1_NSS_PIN -#define MPU6000_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN SPI1_NSS_PIN +#define GYRO_1_SPI_INSTANCE SPI1 // tqfp48 pin 25 #define BMP280_CS_PIN SPI2_NSS_PIN @@ -74,19 +71,14 @@ #define FLASH_SPI_INSTANCE SPI2 #define USE_GYRO -#define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW270_DEG - #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW270_DEG +#define GYRO_1_ALIGN CW270_DEG #define USE_ACC -#define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW270_DEG #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW270_DEG +#define ACC_1_ALIGN CW270_DEG #define USE_BARO #define USE_BARO_BMP280 @@ -122,7 +114,8 @@ // mpu_int definition in sensors/initialisation.c #define USE_EXTI -#define MPU_INT_EXTI PC13 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC13 //#define DEBUG_MPU_DATA_READY_INTERRUPT #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW @@ -141,4 +134,4 @@ // channel mapping in drivers/pwm_mapping.c // only 6 outputs available on hardware #define USABLE_TIMER_CHANNEL_COUNT 10 -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(16)) diff --git a/src/main/target/EACHIF3/target.h b/src/main/target/EACHIF3/target.h index 4dbb690fca..a4a91c63b1 100644 --- a/src/main/target/EACHIF3/target.h +++ b/src/main/target/EACHIF3/target.h @@ -26,7 +26,8 @@ #define USE_EXTI -#define MPU_INT_EXTI PA15 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PA15 #define USE_MPU_DATA_READY_SIGNAL @@ -35,20 +36,20 @@ #define USE_SPI_DEVICE_2 -#define MPU6500_SPI_INSTANCE SPI1 +#define GYRO_1_SPI_INSTANCE SPI1 #define SPI1_SCK_PIN PB3 #define SPI1_MISO_PIN PB4 #define SPI1_MOSI_PIN PB5 -#define MPU6500_CS_PIN PA5 +#define GYRO_1_CS_PIN PA5 #define USE_GYRO #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW90_DEG +#define GYRO_1_ALIGN CW90_DEG #define USE_ACC #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW90_DEG +#define ACC_1_ALIGN CW90_DEG #define USE_RX_SPI diff --git a/src/main/target/ELLE0/target.h b/src/main/target/ELLE0/target.h index a1a6f909fe..df079d24ef 100644 --- a/src/main/target/ELLE0/target.h +++ b/src/main/target/ELLE0/target.h @@ -31,22 +31,23 @@ // MPU9250 interrupt #define USE_EXTI -#define MPU_INT_EXTI PB5 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PB5 //#define DEBUG_MPU_DATA_READY_INTERRUPT #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW -#define MPU6500_CS_PIN PB12 -#define MPU6500_SPI_INSTANCE SPI2 +#define GYRO_1_CS_PIN PB12 +#define GYRO_1_SPI_INSTANCE SPI2 // Using MPU6050 for the moment. #define USE_GYRO #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW270_DEG +#define GYRO_1_ALIGN CW270_DEG #define USE_ACC #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW270_DEG +#define ACC_1_ALIGN CW270_DEG //#define USE_BARO //#define USE_BARO_MS5611 @@ -61,7 +62,6 @@ //#define USE_UART1 //#define UART1_RX_PIN PA10 //#define UART1_TX_PIN PA9 -//#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 /* RX1 */ #define USE_UART2 diff --git a/src/main/target/F4BY/target.h b/src/main/target/F4BY/target.h index d6c9764875..c2e45fcf4b 100644 --- a/src/main/target/F4BY/target.h +++ b/src/main/target/F4BY/target.h @@ -36,43 +36,37 @@ // MPU6000 interrupts #define USE_EXTI -#define MPU_INT_EXTI PB0 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PB0 #define USE_MPU_DATA_READY_SIGNAL -#define MPU6000_CS_PIN PA4 -#define MPU6000_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 #define USE_ACC #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW90_DEG +#define ACC_1_ALIGN CW90_DEG #define USE_GYRO #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW90_DEG +#define GYRO_1_ALIGN CW90_DEG #define USE_MAG #define USE_MAG_HMC5883 #define USE_MAG_QMC5883 +#define USE_MAG_LIS3MDL #define MAG_HMC5883_ALIGN CW90_DEG #define USE_BARO #define USE_BARO_MS5611 #define USE_SDCARD - +#define USE_SDCARD_SPI #define SDCARD_SPI_INSTANCE SPI2 #define SDCARD_SPI_CS_PIN PE15 - -// SPI2 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init: -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz -// Divide to under 25MHz for normal operation: -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 // 21MHz - - -#define SDCARD_DMA_CHANNEL_TX DMA1_Stream3 +#define SDCARD_DMA_CHANNEL_TX DMA1_Stream4 #define SDCARD_DMA_CHANNEL 0 - #define USE_VCP #define USE_USB_DETECT #define USB_DETECT_PIN PA9 @@ -80,7 +74,6 @@ #define USE_UART1 #define UART1_RX_PIN PB7 #define UART1_TX_PIN PB6 -#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 #define USE_UART2 #define UART2_RX_PIN PD6 @@ -159,4 +152,4 @@ #define TARGET_IO_PORTE 0xffff #define USABLE_TIMER_CHANNEL_COUNT 17 -#define USED_TIMERS ( TIM_N(1) |TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(8) | TIM_N(9)) +#define USED_TIMERS ( TIM_N(1) |TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(9) ) diff --git a/src/main/target/F4BY/target.mk b/src/main/target/F4BY/target.mk index e5c4a67fa4..50e30b86ed 100644 --- a/src/main/target/F4BY/target.mk +++ b/src/main/target/F4BY/target.mk @@ -1,8 +1,9 @@ F405_TARGETS += $(TARGET) -FEATURES += SDCARD VCP +FEATURES += SDCARD_SPI VCP TARGET_SRC = \ drivers/accgyro/accgyro_spi_mpu6000.c \ drivers/barometer/barometer_ms5611.c \ drivers/compass/compass_hmc5883l.c \ - drivers/compass/compass_qmc5883l.c + drivers/compass/compass_qmc5883l.c \ + drivers/compass/compass_lis3mdl.c diff --git a/src/main/target/FF_FORTINIF4/config.c b/src/main/target/FF_FORTINIF4/config.c index 775619ba46..6252aee960 100644 --- a/src/main/target/FF_FORTINIF4/config.c +++ b/src/main/target/FF_FORTINIF4/config.c @@ -35,7 +35,7 @@ void targetConfiguration(void) { if (hardwareRevision >= FORTINIF4_REV_2) { - featureSet(FEATURE_OSD); + featureEnable(FEATURE_OSD); } telemetryConfigMutable()->halfDuplex = false; diff --git a/src/main/target/FF_FORTINIF4/target.c b/src/main/target/FF_FORTINIF4/target.c index 14ff7cb079..d433e53b43 100644 --- a/src/main/target/FF_FORTINIF4/target.c +++ b/src/main/target/FF_FORTINIF4/target.c @@ -28,19 +28,20 @@ #include "drivers/timer_def.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0 ), // S1_OUT - DMA1_ST7 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0 ), // S2_OUT - DMA1_ST2 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0 ), // S1_OUT - DMA1_ST7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0 ), // S2_OUT - DMA1_ST2 #if defined(FF_FORTINIF4_REV03) - DEF_TIM(TIM2, CH4, PB11, TIM_USE_MOTOR, 0, 1 ), // S3_OUT - DMA1_ST6 - DEF_TIM(TIM2, CH3, PB10, TIM_USE_MOTOR, 0, 0 ), // S4_OUT - DMA1_ST1 + DEF_TIM(TIM2, CH4, PB11, TIM_USE_MOTOR, 0, 1 ), // S3_OUT - DMA1_ST6 + DEF_TIM(TIM2, CH3, PB10, TIM_USE_MOTOR, 0, 0 ), // S4_OUT - DMA1_ST1 #else - DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, 0, 1 ), // S3_OUT - DMA1_ST6 - DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, 0, 0 ), // S4_OUT - DMA1_ST1 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, 0, 1 ), // S3_OUT - DMA1_ST6 + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, 0, 0 ), // S4_OUT - DMA1_ST1 #endif - DEF_TIM(TIM4, CH4, PB9, TIM_USE_PPM, 0, 0 ), // PPM IN + DEF_TIM(TIM4, CH4, PB9, TIM_USE_PPM, 0, 0 ), // PPM IN #if defined(FF_FORTINIF4_REV03) - DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0 ), // LED - DMA1_ST0 + DEF_TIM(TIM1, CH3, PA10, TIM_USE_CAMERA_CONTROL, 0, 0 ), // FC CAM - DMA1_ST3 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0 ), // LED - DMA1_ST0 #else - DEF_TIM(TIM4, CH2, PB7, TIM_USE_LED, 0, 0 ), // LED - DMA1_ST3 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_LED | TIM_USE_CAMERA_CONTROL, 0, 0 ), // LED - DMA1_ST3 #endif }; diff --git a/src/main/target/FF_FORTINIF4/target.h b/src/main/target/FF_FORTINIF4/target.h index 545eb5ac70..8c04884f14 100644 --- a/src/main/target/FF_FORTINIF4/target.h +++ b/src/main/target/FF_FORTINIF4/target.h @@ -49,51 +49,35 @@ #define BEEPER_INVERTED /*---------------------------------*/ -/*----------CAMERA CONTROL---------*/ -#define CAMERA_CONTROL_PIN PB7 -/*---------------------------------*/ - /*------------SENSORS--------------*/ // MPU interrupt #define USE_EXTI -#define MPU_INT_EXTI PC4 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC4 //#define DEBUG_MPU_DATA_READY_INTERRUPT #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW #define USE_GYRO -#define USE_ACC - -#if !defined(FF_FORTINIF4_REV03) -#define MPU6000_CS_PIN SPI1_NSS_PIN -#define MPU6000_SPI_INSTANCE SPI1 - #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW180_DEG - -#define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW180_DEG - -#define ICM20689_CS_PIN SPI1_NSS_PIN -#define ICM20689_SPI_INSTANCE SPI1 - +#define USE_GYRO_SPI_MPU6500 #define USE_GYRO_SPI_ICM20689 -#define GYRO_ICM20689_ALIGN CW180_DEG +#define GYRO_1_ALIGN CW180_DEG -#define USE_ACC_SPI_ICM20689 -#define ACC_ICM20689_ALIGN CW180_DEG +#define GYRO_1_SPI_INSTANCE SPI1 + +#if defined(FF_FORTINIF4_REV03) +#define GYRO_1_CS_PIN PA4 +#else +#define GYRO_1_CS_PIN PA8 #endif -#define MPU6500_CS_PIN SPI1_NSS_PIN -#define MPU6500_SPI_INSTANCE SPI1 - -#define USE_GYRO_MPU6500 -#define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW180_DEG - -#define USE_ACC_MPU6500 +#define USE_ACC +#define USE_ACC_SPI_MPU6000 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW180_DEG +#define USE_ACC_SPI_ICM20689 +#define ACC_1_ALIGN CW180_DEG + /*---------------------------------*/ #if !defined(FF_FORTINIF4_REV03) @@ -125,7 +109,11 @@ //#define USE_USB_DETECT #define USE_UART1 +#if defined(FF_FORTINIF4_REV03) +#define UART1_RX_PIN PB7 +#else #define UART1_RX_PIN PA10 +#endif #define UART1_TX_PIN PA9 #define USE_UART4 @@ -226,6 +214,11 @@ /*---------------------------------*/ /*--------------TIMERS-------------*/ +#if defined(FF_FORTINIF4_REV03) +#define USABLE_TIMER_CHANNEL_COUNT 7 +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) ) +#else #define USABLE_TIMER_CHANNEL_COUNT 6 #define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(4) ) +#endif /*---------------------------------*/ diff --git a/src/main/target/FF_PIKOBLX/config.c b/src/main/target/FF_PIKOBLX/config.c index 073fdbbda9..e3c8f590fd 100644 --- a/src/main/target/FF_PIKOBLX/config.c +++ b/src/main/target/FF_PIKOBLX/config.c @@ -65,7 +65,7 @@ void targetConfiguration(void) #else serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART2)].functionMask = FUNCTION_TELEMETRY_FRSKY_HUB; rxConfigMutable()->serialrx_inverted = true; - featureSet(FEATURE_TELEMETRY); + featureEnable(FEATURE_TELEMETRY); #endif parseRcChannels("TAER1234", rxConfigMutable()); diff --git a/src/main/target/FF_PIKOBLX/target.h b/src/main/target/FF_PIKOBLX/target.h index 96d3f7010a..ebdc496f9c 100644 --- a/src/main/target/FF_PIKOBLX/target.h +++ b/src/main/target/FF_PIKOBLX/target.h @@ -33,7 +33,6 @@ #define ENABLE_DSHOT_DMAR true #define REMAP_TIM16_DMA -#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT #define USE_TARGET_CONFIG @@ -46,20 +45,20 @@ // MPU6000 interrupts #define USE_EXTI -#define MPU_INT_EXTI PA15 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PA15 #define USE_MPU_DATA_READY_SIGNAL #define USE_GYRO #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW180_DEG +#define GYRO_1_ALIGN CW180_DEG #define USE_ACC #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW180_DEG +#define ACC_1_ALIGN CW180_DEG -#define MPU6000_CS_GPIO GPIOB -#define MPU6000_CS_PIN PB12 -#define MPU6000_SPI_INSTANCE SPI2 +#define GYRO_1_CS_PIN PB12 +#define GYRO_1_SPI_INSTANCE SPI2 #define USE_VCP #define USE_UART1 diff --git a/src/main/target/FF_PIKOF4/target.h b/src/main/target/FF_PIKOF4/target.h index 7496b9efbb..cc6492ba0c 100644 --- a/src/main/target/FF_PIKOF4/target.h +++ b/src/main/target/FF_PIKOF4/target.h @@ -43,45 +43,32 @@ #define BEEPER_INVERTED /*---------------------------------*/ -/*----------CAMERA CONTROL---------*/ -// #define CAMERA_CONTROL_PIN PB7 -/*---------------------------------*/ - /*------------SENSORS--------------*/ // MPU interrupt #define USE_EXTI -#define MPU_INT_EXTI PC4 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC4 //#define DEBUG_MPU_DATA_READY_INTERRUPT #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW #if defined(FF_PIKOF4OSD) -#define MPU6000_CS_PIN PA15 -#define MPU6000_SPI_INSTANCE SPI3 - -#define MPU6500_CS_PIN PA15 -#define MPU6500_SPI_INSTANCE SPI3 +#define GYRO_1_CS_PIN PA15 +#define GYRO_1_SPI_INSTANCE SPI3 #else -#define MPU6000_CS_PIN PA4 -#define MPU6000_SPI_INSTANCE SPI1 - -#define MPU6500_CS_PIN PA4 -#define MPU6500_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 #endif #define USE_GYRO #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW180_DEG - #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW180_DEG +#define GYRO_1_ALIGN CW180_DEG #define USE_ACC #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW180_DEG - #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW180_DEG +#define ACC_1_ALIGN CW180_DEG /*---------------------------------*/ #if defined(FF_PIKOF4OSD) diff --git a/src/main/target/FISHDRONEF4/target.h b/src/main/target/FISHDRONEF4/target.h index 6135985c8e..13a94fe5cf 100644 --- a/src/main/target/FISHDRONEF4/target.h +++ b/src/main/target/FISHDRONEF4/target.h @@ -38,23 +38,24 @@ #define INVERTER_PIN_UART6 PC8 #define USE_EXTI -#define MPU_INT_EXTI PC4 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC4 #define USE_MPU_DATA_READY_SIGNAL // *************** Gyro & ACC ********************** #define USE_SPI #define USE_SPI_DEVICE_1 -#define MPU6500_CS_PIN PA4 -#define MPU6500_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 #define USE_GYRO #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW90_DEG +#define GYRO_1_ALIGN CW90_DEG #define USE_ACC #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW90_DEG +#define ACC_1_ALIGN CW90_DEG // *************** UART ***************************** #define USE_VCP @@ -105,18 +106,11 @@ // *************** SDCARD ***************************** #define USE_SDCARD - +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED - #define SDCARD_DETECT_PIN PB7 #define SDCARD_SPI_INSTANCE SPI3 #define SDCARD_SPI_CS_PIN PB9 - -// SPI3 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init: -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz -// Divide to under 25MHz for normal operation: -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 // 21MHz - #define SDCARD_DMA_CHANNEL_TX DMA1_Stream5 #define SDCARD_DMA_CHANNEL 0 diff --git a/src/main/target/FISHDRONEF4/target.mk b/src/main/target/FISHDRONEF4/target.mk index 48b834c6e4..a40318066f 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_SPI VCP ONBOARDFLASH TARGET_SRC = \ drivers/accgyro/accgyro_spi_mpu6500.c \ diff --git a/src/main/target/FOXEERF405/target.c b/src/main/target/FOXEERF405/target.c index 6667dec4c6..b526aabbd4 100644 --- a/src/main/target/FOXEERF405/target.c +++ b/src/main/target/FOXEERF405/target.c @@ -28,18 +28,18 @@ #include "drivers/timer_def.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - DEF_TIM(TIM12, CH2, PB15, TIM_USE_PPM, 0, 0), // PPM + DEF_TIM(TIM12, CH2, PB15, TIM_USE_PPM, 0, 0), // PPM - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0), // S1 (1,7) - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MOTOR, 0, 0), // S2 (1,4) - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, 0, 0), // S3 (2,4) - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MOTOR, 0, 0), // S4 (2,7) - DEF_TIM(TIM8, CH1, PC6, TIM_USE_MOTOR, 0, 0), // S5 (2,2) - DEF_TIM(TIM8, CH2, PC7, TIM_USE_MOTOR, 0, 0), // S6 (2,3) + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0), // S1 (1,7) + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MOTOR, 0, 0), // S2 (1,4) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, 0, 0), // S3 (2,4) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MOTOR, 0, 0), // S4 (2,7) + DEF_TIM(TIM8, CH1, PC6, TIM_USE_MOTOR, 0, 0), // S5 (2,2) + DEF_TIM(TIM8, CH2, PC7, TIM_USE_MOTOR, 0, 0), // S6 (2,3) - DEF_TIM(TIM1, CH3, PA10, TIM_USE_LED, 0, 0), // LED STRIP(2,6) + DEF_TIM(TIM1, CH3, PA10, TIM_USE_LED, 0, 0), // LED STRIP(2,6) - DEF_TIM(TIM2, CH2, PB3, TIM_USE_PWM, 0, 0), // FC CAM + DEF_TIM(TIM2, CH2, PB3, TIM_USE_CAMERA_CONTROL, 0, 0), // FC CAM }; diff --git a/src/main/target/FOXEERF405/target.h b/src/main/target/FOXEERF405/target.h index f627c18782..a4687c7c0d 100644 --- a/src/main/target/FOXEERF405/target.h +++ b/src/main/target/FOXEERF405/target.h @@ -31,9 +31,6 @@ #define BEEPER_PIN PA4 #define BEEPER_INVERTED -//define camera control -#define CAMERA_CONTROL_PIN PB3 - //Gyro & ACC------------------------------- #define USE_SPI #define USE_SPI_DEVICE_1 @@ -43,29 +40,24 @@ #define SPI1_MOSI_PIN PA7 #define USE_EXTI -#define MPU_INT_EXTI PC4 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC4 #define USE_MPU_DATA_READY_SIGNAL #define USE_GYRO #define USE_ACC +#define GYRO_1_CS_PIN PB2 +#define GYRO_1_SPI_INSTANCE SPI1 +#define GYRO_1_ALIGN CW90_DEG +#define ACC_1_ALIGN CW90_DEG + //------ICM20689 -#define ICM20689_CS_PIN PB2 -#define ICM20689_SPI_INSTANCE SPI1 - #define USE_GYRO_SPI_ICM20689 -#define GYRO_ICM20689_ALIGN CW90_DEG - #define USE_ACC_SPI_ICM20689 -#define ACC_ICM20689_ALIGN CW90_DEG + //------MPU6000 -#define MPU6000_CS_PIN PB2 -#define MPU6000_SPI_INSTANCE SPI1 - #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW90_DEG - #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW90_DEG //Baro & MAG------------------------------- #define USE_I2C @@ -96,7 +88,6 @@ #define SPI3_MISO_PIN PC11 #define SPI3_MOSI_PIN PB5 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_INSTANCE SPI3 #define MAX7456_SPI_CS_PIN PA15 diff --git a/src/main/target/YUPIF7/config.c b/src/main/target/FOXEERF722DUAL/target.c similarity index 52% rename from src/main/target/YUPIF7/config.c rename to src/main/target/FOXEERF722DUAL/target.c index 1951848c2d..fb2465d378 100644 --- a/src/main/target/YUPIF7/config.c +++ b/src/main/target/FOXEERF722DUAL/target.c @@ -18,32 +18,28 @@ * If not, see . */ -#include #include #include "platform.h" +#include "drivers/io.h" -#ifdef USE_TARGET_CONFIG -#include "fc/config.h" +#include "drivers/dma.h" +#include "drivers/timer.h" +#include "drivers/timer_def.h" -#include "flight/pid.h" +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + DEF_TIM(TIM4, CH2, PB7, TIM_USE_PPM, 0, 0), // PPM&SBUS + + DEF_TIM(TIM1, CH2, PA9, TIM_USE_MOTOR, 0, 0), // S1 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, 0, 0), // S2 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MOTOR, 0, 0), // S3 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, 0, 0), // S4 + DEF_TIM(TIM8, CH1, PC6, TIM_USE_MOTOR, 0, 0), // S5 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_MOTOR, 0, 0), // S6 -// alternative defaults settings for YuPiF4 targets -void targetConfiguration(void) -{ - /* Specific PID values for YupiF4 */ - for (uint8_t pidProfileIndex = 0; pidProfileIndex < MAX_PROFILE_COUNT; pidProfileIndex++) { - pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex); + DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), // LED STRIP(1,5) - pidProfile->pid[PID_ROLL].P = 30; - pidProfile->pid[PID_ROLL].I = 45; - pidProfile->pid[PID_ROLL].D = 20; - pidProfile->pid[PID_PITCH].P = 30; - pidProfile->pid[PID_PITCH].I = 50; - pidProfile->pid[PID_PITCH].D = 20; - pidProfile->pid[PID_YAW].P = 40; - pidProfile->pid[PID_YAW].I = 50; - } -} -#endif + DEF_TIM(TIM2, CH2, PB3, TIM_USE_CAMERA_CONTROL, 0, 0), // FC CAM + +}; diff --git a/src/main/target/FOXEERF722DUAL/target.h b/src/main/target/FOXEERF722DUAL/target.h new file mode 100644 index 0000000000..3a8225b9a4 --- /dev/null +++ b/src/main/target/FOXEERF722DUAL/target.h @@ -0,0 +1,157 @@ +/* + * 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 + +#define TARGET_BOARD_IDENTIFIER "FXF7" +#define USBD_PRODUCT_STRING "FOXEER722DUAL" + +#define ENABLE_DSHOT_DMAR true +#define LED0_PIN PC15 + +#define USE_BEEPER +#define BEEPER_PIN PA4 +#define BEEPER_INVERTED + +#define USE_DUAL_GYRO +#define USE_EXTI +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC4 +#define GYRO_2_EXTI_PIN PB0 + +#define GYRO_1_CS_PIN PB2 +#define GYRO_1_SPI_INSTANCE SPI1 +#define GYRO_2_CS_PIN PB1 +#define GYRO_2_SPI_INSTANCE SPI1 + +#define USE_MPU_DATA_READY_SIGNAL +#define ENSURE_MPU_DATA_READY_IS_LOW + +#define USE_GYRO +#define USE_GYRO_SPI_MPU6000 +#define USE_GYRO_SPI_MPU6500 + +#define USE_ACC +#define USE_ACC_SPI_MPU6000 +#define USE_ACC_SPI_MPU6500 + +#define GYRO_1_ALIGN CW270_DEG +#define ACC_1_ALIGN CW270_DEG + +#define GYRO_2_ALIGN CW180_DEG +#define ACC_2_ALIGN CW180_DEG + +#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1 + +#define USE_BARO +#define USE_BARO_MS5611 + +#define USE_MAG +#define USE_MAG_HMC5883 +#define USE_MAG_LIS3MDL + +#define USE_VCP +#define USE_UART1 +#define USE_UART2 +#define USE_UART3 +#define USE_UART4 +#define USE_UART5 + +#define UART1_TX_PIN PB6 +#define UART1_RX_PIN PB7 + +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 + +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#define UART5_TX_PIN PC12 +#define UART5_RX_PIN PD2 +#define SERIAL_PORT_COUNT 6 + +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C_DEVICE I2CDEV_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define USE_SPI + +#define USE_SPI_DEVICE_1 // 2xGYRO/ACC +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_SPI_DEVICE_2 // FLASH +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_MAX7456 +#define MAX7456_SPI_INSTANCE SPI3 +#define MAX7456_SPI_CS_PIN PC3 + +#define USE_SPI_DEVICE_3 // MAX7456 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PB5 + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define FLASH_CS_PIN PB12 +#define FLASH_SPI_INSTANCE SPI2 + +#define USE_ADC +#define ADC_INSTANCE ADC3 +#define ADC3_DMA_STREAM DMA2_Stream0 + +#define VBAT_ADC_PIN PC0 +#define CURRENT_METER_ADC_PIN PC2 +#define RSSI_ADC_PIN PA0 + +#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC +#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC + +#define USE_OSD +#define USE_LED_STRIP + +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define DEFAULT_FEATURES FEATURE_OSD +#define SERIALRX_UART SERIAL_PORT_USART1 +#define SERIALRX_PROVIDER SERIALRX_SBUS + + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) + +#define USABLE_TIMER_CHANNEL_COUNT 9 +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(4) |TIM_N(8) ) + + diff --git a/src/main/target/FOXEERF722DUAL/target.mk b/src/main/target/FOXEERF722DUAL/target.mk new file mode 100644 index 0000000000..6603b9ea3e --- /dev/null +++ b/src/main/target/FOXEERF722DUAL/target.mk @@ -0,0 +1,14 @@ +F7X2RE_TARGETS += $(TARGET) +FEATURES += VCP ONBOARDFLASH + +TARGET_SRC = \ + drivers/accgyro/accgyro_mpu6500.c \ + drivers/accgyro/accgyro_spi_mpu6000.c \ + drivers/accgyro/accgyro_spi_mpu6500.c \ + drivers/barometer/barometer_bmp280.c \ + drivers/barometer/barometer_ms5611.c \ + drivers/compass/compass_hmc5883l.c \ + drivers/compass/compass_lis3mdl.c \ + drivers/max7456.c + + diff --git a/src/main/target/FRSKYF3/target.h b/src/main/target/FRSKYF3/target.h index 112251914f..6f7bf7aa7d 100644 --- a/src/main/target/FRSKYF3/target.h +++ b/src/main/target/FRSKYF3/target.h @@ -20,13 +20,32 @@ #pragma once -// Removed to make the firmware fit into flash (in descending order of priority): -#undef USE_GYRO_OVERFLOW_CHECK // target does not use affected gyros -#undef USE_RTC_TIME - #define TARGET_BOARD_IDENTIFIER "FRF3" #define USE_TARGET_CONFIG -#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE + +// Removed to make the firmware fit into flash (in descending order of priority): +//#undef USE_GYRO_OVERFLOW_CHECK +//#undef USE_GYRO_LPF2 + +//#undef USE_ITERM_RELAX +//#undef USE_RC_SMOOTHING_FILTER + +//#undef USE_HUFFMAN +//#undef USE_PINIO +//#undef USE_PINIOBOX + +#undef USE_TELEMETRY_HOTT +#undef USE_TELEMETRY_MAVLINK +#undef USE_TELEMETRY_LTM +#undef USE_SERIALRX_XBUS +#undef USE_SERIALRX_SUMH +#undef USE_PWM + +#undef USE_BOARD_INFO +#undef USE_EXTENDED_CMS_MENUS +#undef USE_RTC_TIME +#undef USE_RX_MSP +#undef USE_ESC_SENSOR_INFO #define LED0_PIN PB3 #define USE_BEEPER @@ -34,29 +53,30 @@ #define BEEPER_INVERTED #define USE_EXTI -#define MPU_INT_EXTI PC13 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC13 #define USE_MPU_DATA_READY_SIGNAL #define EXTI15_10_CALLBACK_HANDLER_COUNT 1 // MPU_INT, SDCardDetect #define MPU_ADDRESS 0x69 #ifdef MYMPU6000 -#define MPU6000_SPI_INSTANCE SPI2 -#define MPU6000_CS_PIN PB12 +#define GYRO_1_SPI_INSTANCE SPI2 +#define GYRO_1_CS_PIN PB12 #define USE_GYRO #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW270_DEG +#define GYRO_1_ALIGN CW270_DEG #define USE_ACC #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW270_DEG +#define ACC_1_ALIGN CW270_DEG #else #define USE_GYRO #define USE_GYRO_MPU6050 -#define GYRO_MPU6050_ALIGN CW270_DEG +#define GYRO_1_ALIGN CW270_DEG #define USE_ACC #define USE_ACC_MPU6050 -#define ACC_MPU6050_ALIGN CW270_DEG +#define ACC_1_ALIGN CW270_DEG #endif #define USE_VCP @@ -107,15 +127,12 @@ #define SPI1_MOSI_PIN PA7 #define USE_SDCARD - +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED #define SDCARD_DETECT_PIN PB5 #define SDCARD_SPI_INSTANCE SPI1 #define SDCARD_SPI_CS_PIN SPI1_NSS_PIN -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 - #define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC #define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_NONE @@ -146,4 +163,4 @@ #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) #define USABLE_TIMER_CHANNEL_COUNT 9 -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(17)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(8)) diff --git a/src/main/target/FRSKYF3/target.mk b/src/main/target/FRSKYF3/target.mk index 1a9b96b664..395c6cf1c9 100644 --- a/src/main/target/FRSKYF3/target.mk +++ b/src/main/target/FRSKYF3/target.mk @@ -1,5 +1,5 @@ F3_TARGETS += $(TARGET) -FEATURES = VCP SDCARD +FEATURES = VCP SDCARD_SPI TARGET_SRC = \ drivers/accgyro/accgyro_mpu.c \ diff --git a/src/main/target/FRSKYF4/target.h b/src/main/target/FRSKYF4/target.h index 708f0732e8..4cbd0529f7 100644 --- a/src/main/target/FRSKYF4/target.h +++ b/src/main/target/FRSKYF4/target.h @@ -31,8 +31,8 @@ #define INVERTER_PIN_UART6 PC8 -#define MPU6000_CS_PIN PA4 -#define MPU6000_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 #define USE_ACC #define USE_ACC_SPI_MPU6000 @@ -40,11 +40,12 @@ #define USE_GYRO #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW270_DEG -#define ACC_MPU6000_ALIGN CW270_DEG +#define GYRO_1_ALIGN CW270_DEG +#define ACC_1_ALIGN CW270_DEG #define USE_EXTI -#define MPU_INT_EXTI PC4 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC4 #define USE_MPU_DATA_READY_SIGNAL #define USE_BARO @@ -62,13 +63,11 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define USE_SDCARD +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED #define SDCARD_DETECT_PIN PB7 #define SDCARD_SPI_INSTANCE SPI2 #define SDCARD_SPI_CS_PIN SPI2_NSS_PIN -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 // 21MHz - #define SDCARD_DMA_CHANNEL_TX DMA1_Stream4 #define SDCARD_DMA_CHANNEL 0 @@ -79,7 +78,6 @@ #define USE_UART1 #define UART1_RX_PIN PA10 #define UART1_TX_PIN PA9 -#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 #define USE_UART3 #define UART3_RX_PIN PB11 @@ -130,4 +128,4 @@ #define TARGET_IO_PORTD BIT(2) #define USABLE_TIMER_CHANNEL_COUNT 13 -#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9)) +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(8) ) diff --git a/src/main/target/FRSKYF4/target.mk b/src/main/target/FRSKYF4/target.mk index 5a9f7aea5b..85bd85add3 100644 --- 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_SPI TARGET_SRC = \ drivers/accgyro/accgyro_spi_mpu6000.c \ diff --git a/src/main/target/FURYF3/target.h b/src/main/target/FURYF3/target.h index 47c6f9e0fb..d3539d8b91 100644 --- a/src/main/target/FURYF3/target.h +++ b/src/main/target/FURYF3/target.h @@ -26,10 +26,35 @@ #else #define TARGET_BOARD_IDENTIFIER "FYF3" // #define USBD_PRODUCT_STRING "FuryF3" - #undef USE_OSD #endif -#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT +// Removed to make the firmware fit into flash (in descending order of priority): +//#undef USE_GYRO_OVERFLOW_CHECK +//#undef USE_GYRO_LPF2 + +//#undef USE_ITERM_RELAX +//#undef USE_RC_SMOOTHING_FILTER + +//#undef USE_MSP_DISPLAYPORT +//#undef USE_MSP_OVER_TELEMETRY + +//#undef USE_HUFFMAN +//#undef USE_PINIO +//#undef USE_PINIOBOX + +//#undef USE_TELEMETRY_HOTT +//#undef USE_TELEMETRY_MAVLINK +#undef USE_TELEMETRY_LTM +#undef USE_SERIALRX_XBUS +#undef USE_SERIALRX_SUMH +#undef USE_PWM + +#undef USE_BOARD_INFO +#undef USE_EXTENDED_CMS_MENUS +#undef USE_RTC_TIME +#undef USE_RX_MSP +#undef USE_ESC_SENSOR_INFO + #define CONFIG_PREFER_ACC_ON #define LED0_PIN PC14 @@ -39,40 +64,28 @@ #define BEEPER_INVERTED #define USE_EXTI -#define MPU_INT_EXTI PA3 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PA3 #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW -#define ICM20689_CS_PIN PA4 -#define ICM20689_SPI_INSTANCE SPI1 - -#define MPU6000_CS_PIN PA4 -#define MPU6000_SPI_INSTANCE SPI1 - -#define MPU6500_CS_PIN PA4 -#define MPU6500_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 #define USE_GYRO -#define USE_GYRO_SPI_ICM20689 -#define GYRO_ICM20689_ALIGN CW180_DEG - #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW180_DEG - -#define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW90_DEG +#define USE_GYRO_SPI_ICM20689 +// MPU6000 and ICM20689 are CW180 aligned. +// MPU6500 is CW90 aligned, but can't handle this. +// Must be configured after flashing. +#define GYRO_1_ALIGN CW180_DEG #define USE_ACC -#define USE_ACC_SPI_ICM20689 -#define ACC_ICM20689_ALIGN CW180_DEG - #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW180_DEG - -#define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW90_DEG +#define USE_ACC_SPI_ICM20689 +#define ACC_1_ALIGN CW180_DEG #define USE_SPI #define USE_SPI_DEVICE_1 @@ -84,12 +97,6 @@ #define SPI2_MOSI_PIN PB15 #ifdef FURYF3OSD - -// Removed to make the firmware fit into flash (in descending order of priority): -#undef USE_RTC_TIME -#undef USE_RX_MSP -#undef USE_ESC_SENSOR_INFO - // include the max7456 driver #define USE_MAX7456 #define MAX7456_SPI_INSTANCE SPI1 @@ -109,18 +116,12 @@ #else #define USE_SDCARD - + #define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED - #define SDCARD_DETECT_PIN PB2 #define SDCARD_SPI_INSTANCE SPI2 #define SDCARD_SPI_CS_PIN SPI2_NSS_PIN - // SPI2 is on the APB1 bus whose clock runs at 36MHz. Divide to under 400kHz for init: - #define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128 - // Divide to under 25MHz for normal operation: - #define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 2 - // Note, this is the same DMA channel as UART1_RX. Luckily we don't use DMA for USART Rx. #define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 diff --git a/src/main/target/FURYF3/target.mk b/src/main/target/FURYF3/target.mk index 37ef421bb0..be5e24d82f 100644 --- a/src/main/target/FURYF3/target.mk +++ b/src/main/target/FURYF3/target.mk @@ -2,7 +2,7 @@ F3_TARGETS += $(TARGET) ifeq ($(TARGET), FURYF3OSD) FEATURES += VCP ONBOARDFLASH else -FEATURES += VCP SDCARD +FEATURES += VCP SDCARD_SPI endif TARGET_SRC = \ diff --git a/src/main/target/FURYF4/target.h b/src/main/target/FURYF4/target.h index 348872608c..a534185ef0 100644 --- a/src/main/target/FURYF4/target.h +++ b/src/main/target/FURYF4/target.h @@ -39,44 +39,26 @@ // MPU6000 interrupts #define USE_EXTI -#define MPU_INT_EXTI PC4 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC4 #define USE_MPU_DATA_READY_SIGNAL -#define ICM20689_CS_PIN PA4 -#define ICM20689_SPI_INSTANCE SPI1 - -#define MPU6000_CS_PIN PA4 -#define MPU6000_SPI_INSTANCE SPI1 - -#define MPU6500_CS_PIN PA4 -#define MPU6500_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 #define USE_GYRO -#define USE_GYRO_SPI_ICM20689 -#define GYRO_ICM20689_ALIGN CW180_DEG - #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW180_DEG -#define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW180_DEG - -#define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW180_DEG +#define USE_GYRO_SPI_ICM20689 +#define GYRO_1_ALIGN CW180_DEG #define USE_ACC -#define USE_ACC_SPI_ICM20689 -#define ACC_ICM20689_ALIGN CW180_DEG - #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW180_DEG - -#define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW180_DEG +#define USE_ACC_SPI_ICM20689 +#define ACC_1_ALIGN CW180_DEG #ifdef FURYF4OSD - #define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_INSTANCE SPI2 #define MAX7456_SPI_CS_PIN PB12 @@ -94,24 +76,15 @@ #define MS5611_I2C_INSTANCE I2CDEV_1 #define USE_SDCARD - + #define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED - #define SDCARD_DETECT_PIN PD2 #define SDCARD_SPI_INSTANCE SPI2 #define SDCARD_SPI_CS_PIN PB12 - - // SPI2 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init: - #define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz - // Divide to under 25MHz for normal operation: - #define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 // 21MHz - //#define SDCARD_DMA_CHANNEL_TX DMA1_Stream5 //#define SDCARD_DMA_CHANNEL 0 - #define SDCARD_DMA_CHANNEL_TX DMA1_Stream4 #define SDCARD_DMA_CHANNEL 0 - #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #endif @@ -128,7 +101,6 @@ #define USE_UART1 #define UART1_RX_PIN PA10 #define UART1_TX_PIN PA9 -//#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 #define USE_UART3 #define UART3_RX_PIN PB11 diff --git a/src/main/target/FURYF4/target.mk b/src/main/target/FURYF4/target.mk index 06b60c1c7f..707a725aa7 100644 --- a/src/main/target/FURYF4/target.mk +++ b/src/main/target/FURYF4/target.mk @@ -2,7 +2,7 @@ F405_TARGETS += $(TARGET) ifeq ($(TARGET), FURYF4OSD) FEATURES += VCP ONBOARDFLASH else -FEATURES += VCP ONBOARDFLASH SDCARD +FEATURES += VCP ONBOARDFLASH SDCARD_SPI endif TARGET_SRC = \ diff --git a/src/main/target/FURYF7/target.h b/src/main/target/FURYF7/target.h index 572d489170..896f1f1fa5 100644 --- a/src/main/target/FURYF7/target.h +++ b/src/main/target/FURYF7/target.h @@ -32,19 +32,20 @@ #define BEEPER_INVERTED #define USE_EXTI -#define MPU_INT_EXTI PC4 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC4 #define USE_MPU_DATA_READY_SIGNAL -#define ICM20689_CS_PIN PA4 -#define ICM20689_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 #define USE_GYRO #define USE_GYRO_SPI_ICM20689 -#define GYRO_ICM20689_ALIGN CW180_DEG +#define GYRO_1_ALIGN CW180_DEG #define USE_ACC #define USE_ACC_SPI_ICM20689 -#define ACC_ICM20689_ALIGN CW180_DEG +#define ACC_1_ALIGN CW180_DEG //#define USE_BARO //#define USE_BARO_MS5611 @@ -77,16 +78,11 @@ #define SPI4_MOSI_PIN PE14 #define USE_SDCARD +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED #define SDCARD_DETECT_PIN PD2 - #define SDCARD_SPI_INSTANCE SPI4 #define SDCARD_SPI_CS_PIN SPI4_NSS_PIN - -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 422kHz -// Divide to under 25MHz for normal operation: -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 8 // 13.5MHz - #define SDCARD_DMA_STREAM_TX_FULL DMA2_Stream1 #define SDCARD_DMA_CHANNEL 4 diff --git a/src/main/target/FURYF7/target.mk b/src/main/target/FURYF7/target.mk index 003875026a..bc543450ca 100644 --- a/src/main/target/FURYF7/target.mk +++ b/src/main/target/FURYF7/target.mk @@ -1,5 +1,5 @@ F7X5XG_TARGETS += $(TARGET) -FEATURES += SDCARD VCP ONBOARDFLASH +FEATURES += SDCARD_SPI VCP ONBOARDFLASH TARGET_SRC = \ drivers/accgyro/accgyro_spi_icm20689.c diff --git a/src/main/target/SPRACINGF3OSD/target.c b/src/main/target/HAKRCF722/target.c old mode 100644 new mode 100755 similarity index 65% rename from src/main/target/SPRACINGF3OSD/target.c rename to src/main/target/HAKRCF722/target.c index c1b9cb7bd7..bdafadf0bc --- a/src/main/target/SPRACINGF3OSD/target.c +++ b/src/main/target/HAKRCF722/target.c @@ -23,16 +23,21 @@ #include "platform.h" #include "drivers/io.h" +#include "drivers/dma.h" #include "drivers/timer.h" #include "drivers/timer_def.h" -#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + DEF_TIM(TIM12, CH2, PB15, TIM_USE_PPM, 0, 0), // PPM + + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0), // S1 + DEF_TIM(TIM8, CH1, PC6, TIM_USE_MOTOR, 0, 0), // S2 + DEF_TIM(TIM1, CH3, PA10, TIM_USE_MOTOR, 0, 0), // S3 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, 0, 0), // S4 + DEF_TIM(TIM3, CH2, PC7, TIM_USE_MOTOR, 0, 0), //S5 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MOTOR, 0, 0), //S6 + + DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), + DEF_TIM(TIM2, CH1, PA5, TIM_USE_CAMERA_CONTROL, 0, 0), - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0), // DMA1_Channel2 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0), // DMA1_Channel4 - DEF_TIM(TIM16, CH1, PB4, TIM_USE_MOTOR, 0), // DMA1_Channel3 or DMA1_Channel6 with Remap (need remap to free SPI1_TX for Flash) - DEF_TIM(TIM17, CH1, PB5, TIM_USE_MOTOR, 0), // DMA1_Channel1 or DMA1_Channel7 with Remap (need remap, ADC1 is on DMA1_Channel1) - - DEF_TIM(TIM1, CH1, PA8, TIM_USE_TRANSPONDER, 0), }; diff --git a/src/main/target/HAKRCF722/target.h b/src/main/target/HAKRCF722/target.h new file mode 100644 index 0000000000..64b6cc20db --- /dev/null +++ b/src/main/target/HAKRCF722/target.h @@ -0,0 +1,146 @@ +/* + * 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 + +#define TARGET_BOARD_IDENTIFIER "HRF7" + +#define USBD_PRODUCT_STRING "HAKRCF722" + + +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C_DEVICE (I2CDEV_1) +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 +#define USE_I2C_PULLUP + +#define USE_BARO +#define USE_BARO_BMP085 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_QMP6988 +#define BARO_I2C_INSTANCE (I2CDEV_1) +#define DEFAULT_BARO_QMP6988 + +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define SPI1_SCK_PIN PB3 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define GYRO_1_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 + +#define USE_EXTI +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC4 +#define USE_MPU_DATA_READY_SIGNAL + +#define USE_GYRO +#define USE_GYRO_SPI_MPU6000 +#define GYRO_1_ALIGN CW270_DEG + +#define USE_ACC +#define USE_ACC_SPI_MPU6000 +#define ACC_1_ALIGN CW270_DEG + +#define LED0_PIN PC13 + +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PB5 + +#define USE_OSD +#define USE_MAX7456 +#define MAX7456_SPI_INSTANCE SPI3 +#define MAX7456_SPI_CS_PIN PA15 + +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PC3 + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define FLASH_CS_PIN PB12 +#define FLASH_SPI_INSTANCE SPI2 + +#define ENABLE_DSHOT_DMAR true + +#define USE_VCP + +#define USE_UART1 +#define UART1_RX_PIN PB7 +#define UART1_TX_PIN PA9 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART5 +#define UART5_RX_PIN PD2 +#define UART5_TX_PIN PC12 + +#define USE_SOFTSERIAL1 +#define USE_SOFTSERIAL2 + +#define SERIAL_PORT_COUNT 8 + +#define USE_BEEPER +#define BEEPER_PIN PC14 +#define BEEPER_INVERTED + +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define SERIALRX_UART SERIAL_PORT_USART1 +#define SERIALRX_PROVIDER SERIALRX_SBUS + +#define USE_ADC +#define ADC1_DMA_STREAM DMA2_Stream0 +#define VBAT_ADC_PIN PC1 +#define CURRENT_METER_ADC_PIN PC2 +#define RSSI_ADC_PIN PC0 + +#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC +#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC + +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_PIN PA3 +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff + +#define USABLE_TIMER_CHANNEL_COUNT 9 +#define USED_TIMERS (TIM_N(1)|TIM_N(2)|TIM_N(3)|TIM_N(4)|TIM_N(8)|TIM_N(12)) diff --git a/src/main/target/HAKRCF722/target.mk b/src/main/target/HAKRCF722/target.mk new file mode 100644 index 0000000000..74d50f918e --- /dev/null +++ b/src/main/target/HAKRCF722/target.mk @@ -0,0 +1,11 @@ +F7X2RE_TARGETS += $(TARGET) +FEATURES += VCP ONBOARDFLASH +TARGET_SRC = \ + drivers/accgyro/accgyro_spi_mpu6000.c \ + drivers/barometer/barometer_bmp085.c \ + drivers/barometer/barometer_bmp280.c \ + drivers/barometer/barometer_ms5611.c \ + drivers/barometer/barometer_qmp6988.c \ + drivers/compass/compass_hmc5883l.c \ + drivers/compass/compass_qmc5883l.c \ + drivers/max7456.c diff --git a/src/main/target/IMPULSERCF3/target.h b/src/main/target/IMPULSERCF3/target.h index 3de5a4f41b..8972875aa4 100644 --- a/src/main/target/IMPULSERCF3/target.h +++ b/src/main/target/IMPULSERCF3/target.h @@ -22,7 +22,25 @@ #define TARGET_BOARD_IDENTIFIER "IMF3" -#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE + +// Removed to make the firmware fit into flash (in descending order of priority): +//#undef USE_GYRO_OVERFLOW_CHECK +//#undef USE_GYRO_LPF2 + +//#undef USE_ITERM_RELAX +//#undef USE_RC_SMOOTHING_FILTER + +//#undef USE_TELEMETRY_HOTT +//#undef USE_TELEMETRY_MAVLINK +//#undef USE_TELEMETRY_LTM +//#undef USE_SERIALRX_XBUS + +//#undef USE_BOARD_INFO +#undef USE_EXTENDED_CMS_MENUS +//#undef USE_RTC_TIME +#undef USE_RX_MSP +#undef USE_ESC_SENSOR_INFO + #define ENABLE_DSHOT_DMAR true @@ -32,17 +50,18 @@ #define BEEPER_PIN PC15 #define USE_EXTI -#define MPU_INT_EXTI PC13 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC13 #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW #define USE_GYRO #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW0_DEG +#define GYRO_1_ALIGN CW0_DEG #define USE_ACC #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW0_DEG +#define ACC_1_ALIGN CW0_DEG #define USE_FLASHFS #define USE_FLASH_M25P16 @@ -75,8 +94,8 @@ #define FLASH_CS_PIN PB12 #define FLASH_SPI_INSTANCE SPI2 -#define MPU6000_CS_PIN PA4 -#define MPU6000_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 #define USE_MAX7456 #define MAX7456_SPI_INSTANCE SPI1 diff --git a/src/main/target/IRCFUSIONF3/target.h b/src/main/target/IRCFUSIONF3/target.h index 3194e0ff6f..1820f13a96 100644 --- a/src/main/target/IRCFUSIONF3/target.h +++ b/src/main/target/IRCFUSIONF3/target.h @@ -22,22 +22,22 @@ #define TARGET_BOARD_IDENTIFIER "IFF3" -#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE #define LED0_PIN PB3 #define USE_EXTI -#define MPU_INT_EXTI PC13 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC13 #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW #define USE_GYRO #define USE_GYRO_MPU6050 -#define GYRO_MPU6050_ALIGN CW270_DEG +#define GYRO_1_ALIGN CW270_DEG #define USE_ACC #define USE_ACC_MPU6050 -#define ACC_MPU6050_ALIGN CW270_DEG +#define ACC_1_ALIGN CW270_DEG #define USE_BARO #define USE_BARO_BMP085 @@ -113,4 +113,4 @@ #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) #define USABLE_TIMER_CHANNEL_COUNT 17 -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) |TIM_N(17)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(17)) diff --git a/src/main/target/ISHAPEDF3/target.h b/src/main/target/ISHAPEDF3/target.h index c742284e87..7d10adbd56 100644 --- a/src/main/target/ISHAPEDF3/target.h +++ b/src/main/target/ISHAPEDF3/target.h @@ -22,7 +22,6 @@ #define TARGET_BOARD_IDENTIFIER "ISF3" -#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE #define LED0_PIN PB3 @@ -32,7 +31,8 @@ #define USE_EXTI -#define MPU_INT_EXTI PC13 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC13 #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW @@ -41,11 +41,9 @@ #define USE_GYRO #define USE_GYRO_MPU6500 -//#define USE_GYRO_SPI_MPU6500 #define USE_ACC #define USE_ACC_MPU6500 -//#define USE_ACC_SPI_MPU6500 #define USE_BARO #define USE_BARO_BMP280 @@ -100,14 +98,6 @@ #define CURRENT_METER_ADC_PIN PA5 #define RSSI_ADC_PIN PB2 -#define WS2811_PIN PA8 -#define WS2811_TIMER TIM1 -#define WS2811_DMA_CHANNEL DMA1_Channel2 -#define WS2811_IRQ DMA1_Channel2_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_6 - #define DEFAULT_RX_FEATURE FEATURE_RX_PPM #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/KAKUTEF4/FLYWOOF405.mk b/src/main/target/KAKUTEF4/FLYWOOF405.mk new file mode 100644 index 0000000000..3f0326b281 --- /dev/null +++ b/src/main/target/KAKUTEF4/FLYWOOF405.mk @@ -0,0 +1 @@ +#FLYWOOF405.mk file diff --git a/src/main/target/KAKUTEF4/target.c b/src/main/target/KAKUTEF4/target.c index 6398017150..97f41b06fe 100644 --- a/src/main/target/KAKUTEF4/target.c +++ b/src/main/target/KAKUTEF4/target.c @@ -27,13 +27,28 @@ #include "drivers/timer_def.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + #if defined(FLYWOOF405) + DEF_TIM(TIM10, CH1, PB8, TIM_USE_PPM, 0, 0), // PPM IN + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0), // S1_OUT - DMA1_ST7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0), // S2_OUT - DMA1_ST2 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, 0, 1), // S3_OUT - DMA1_ST6 + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, 0, 0), // S4_OUT - DMA1_ST1 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_MOTOR, 0, 0), // S5_OUT - DMA1_ST5 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MOTOR, 0, 0), // S6_OUT - DMA1_ST3 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MOTOR, 0, 0), // S7_OUT - DMA2_ST7 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MOTOR, 0, 0), // S8_OUT - DMA1_ST4 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_LED, 0, 0), // LED_STRIP - DMA2_ST2 + #else DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM, 0, 0), // PPM IN DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0), // S1_OUT - DMA1_ST7 DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0), // S2_OUT - DMA1_ST2 DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, 0, 1), // S3_OUT - DMA1_ST6 DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, 0, 0), // S4_OUT - DMA1_ST1 - #if defined(KAKUTEF4V2) + #endif + #if defined(KAKUTEF4V2) DEF_TIM(TIM8, CH3, PC8, TIM_USE_LED, 0, 0), // LED_STRIP - DMA2_ST2 + #elif defined(FLYWOOF405) + DEF_TIM(TIM1, CH2, PA9, TIM_USE_PWM, 0, 0), // FC CAM #else DEF_TIM(TIM5, CH1, PA0, TIM_USE_MOTOR, 0, 0), // S5_OUT - DMA1_ST2 DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, 0, 1), // S6_OUT - DMA2_ST4 diff --git a/src/main/target/KAKUTEF4/target.h b/src/main/target/KAKUTEF4/target.h index 78e13e9fae..6f1006fda7 100644 --- a/src/main/target/KAKUTEF4/target.h +++ b/src/main/target/KAKUTEF4/target.h @@ -22,6 +22,9 @@ #if defined(KAKUTEF4V2) #define TARGET_BOARD_IDENTIFIER "KTV2" #define USBD_PRODUCT_STRING "KakuteF4-V2" +#elif defined(FLYWOOF405) +#define TARGET_BOARD_IDENTIFIER "FWF4" +#define USBD_PRODUCT_STRING "FLYWOOF405" #else #define TARGET_BOARD_IDENTIFIER "KTV1" #define USBD_PRODUCT_STRING "KakuteF4-V1" @@ -29,37 +32,58 @@ #define USE_TARGET_CONFIG +#if defined(FLYWOOF405) +#define LED0_PIN PC14 +#else #define LED0_PIN PB5 #define LED1_PIN PB4 #define LED2_PIN PB6 +#endif + #define USE_BEEPER + +#if defined(FLYWOOF405) +//define camera control +#define CAMERA_CONTROL_PIN PA9 +#define BEEPER_PIN PC13 +#else #define BEEPER_PIN PC9 +#endif + #define BEEPER_INVERTED #define INVERTER_PIN_UART3 PB15 // ICM20689 interrupt #define USE_EXTI -#define MPU_INT_EXTI PC5 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC5 //#define DEBUG_MPU_DATA_READY_INTERRUPT #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW -#define ICM20689_CS_PIN PC4 -#define ICM20689_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PC4 +#define GYRO_1_SPI_INSTANCE SPI1 +#define ACC_1_ALIGN CW270_DEG +#define GYRO_1_ALIGN CW270_DEG #define USE_ACC #define USE_ACC_SPI_ICM20689 -#define ACC_ICM20689_ALIGN CW270_DEG #define USE_GYRO #define USE_GYRO_SPI_ICM20689 -#define GYRO_ICM20689_ALIGN CW270_DEG -#ifdef KAKUTEF4V2 // There is invertor on RXD3(PB11), so PB10/PB11 can't be used as I2C2. +#if defined(FLYWOOF405) +//------MPU6000 +#define USE_GYRO_SPI_MPU6000 +#define USE_ACC_SPI_MPU6000 +#endif + +#if defined(KAKUTEF4V2) || defined(FLYWOOF405) // There is invertor on RXD3(PB11), so PB10/PB11 can't be used as I2C2. #define USE_I2C //No other I2C pins are fanned out, So V1 don't support I2C peripherals. #define USE_I2C_DEVICE_1 #define I2C_DEVICE (I2CDEV_1) + #define I2C1_SCL PB8 // SCL pad #define I2C1_SDA PB9 // SDA pad #define BARO_I2C_INSTANCE I2C_DEVICE @@ -68,6 +92,7 @@ #define USE_MAG #define USE_MAG_HMC5883 //External, connect to I2C1 #define USE_MAG_QMC5883 +#define USE_MAG_LIS3MDL #define MAG_HMC5883_ALIGN CW180_DEG #define USE_BARO @@ -80,7 +105,6 @@ #define MAX7456_SPI_CS_PIN PB14 #define MAX7456_SPI_CLK (SPI_CLOCK_STANDARD) #define MAX7456_RESTORE_CLK (SPI_CLOCK_FAST) - #define FLASH_CS_PIN PB3 #define FLASH_SPI_INSTANCE SPI3 @@ -93,8 +117,12 @@ #define USE_UART1 #define UART1_RX_PIN PA10 + +#if defined (FLYWOOF405) +#define UART1_TX_PIN PB6 //SCL/UART1_TX/TIM4_CH1 +#else #define UART1_TX_PIN PA9 -#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 +#endif #define USE_UART3 #define UART3_RX_PIN PB11 @@ -104,7 +132,7 @@ #define UART6_RX_PIN PC7 #define UART6_TX_PIN PC6 -#ifdef KAKUTEF4V2 // Uart4 and Uart5 are fanned out on v2 +#if defined (KAKUTEF4V2) || defined(FLYWOOF405) // Uart4 and Uart5 are fanned out on v2 #define USE_UART4 // Uart4 can be used for GPS or RunCam Split #define UART4_RX_PIN PA1 #define UART4_TX_PIN PA0 @@ -122,10 +150,14 @@ #endif #define USE_ESCSERIAL + +#if defined(FLYWOOF405) +#define ESCSERIAL_TIMER_TX_PIN PB8 +#else #define ESCSERIAL_TIMER_TX_PIN PC7 // (HARDARE=0,PPM) - +#endif + #define USE_SPI - #define USE_SPI_DEVICE_1 //ICM20689 #define SPI1_NSS_PIN PC4 #define SPI1_SCK_PIN PA5 @@ -157,9 +189,12 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(2)) -#ifdef KAKUTEF4V2 +#if defined (KAKUTEF4V2) #define USABLE_TIMER_CHANNEL_COUNT 6 #define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(8)) +#elif defined(FLYWOOF405) +#define USABLE_TIMER_CHANNEL_COUNT 11 +#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(4)| TIM_N(8)) #else #define USABLE_TIMER_CHANNEL_COUNT 8 #define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(8)) diff --git a/src/main/target/KAKUTEF4/target.mk b/src/main/target/KAKUTEF4/target.mk index 8f2f2e7348..407f901ab7 100644 --- a/src/main/target/KAKUTEF4/target.mk +++ b/src/main/target/KAKUTEF4/target.mk @@ -8,4 +8,9 @@ TARGET_SRC = \ drivers/compass/compass_ak8963.c \ drivers/compass/compass_hmc5883l.c \ drivers/compass/compass_qmc5883l.c \ + drivers/compass/compass_lis3mdl.c \ drivers/max7456.c + +ifeq ($(TARGET), FLYWOOF405) +TARGET_SRC += drivers/accgyro/accgyro_spi_mpu6000.c +endif diff --git a/src/main/target/KAKUTEF7/KAKUTEF7V2.mk b/src/main/target/KAKUTEF7/KAKUTEF7V2.mk new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/main/target/KAKUTEF7/config.c b/src/main/target/KAKUTEF7/config.c new file mode 100644 index 0000000000..7c00897e01 --- /dev/null +++ b/src/main/target/KAKUTEF7/config.c @@ -0,0 +1,42 @@ +/* + * 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 . + */ + +#include +#include + +#include "platform.h" + +#ifdef USE_TARGET_CONFIG + +#include "config_helper.h" + +#include "io/serial.h" + +#define ESC_SENSOR_UART SERIAL_PORT_USART7 + +static targetSerialPortFunction_t targetSerialPortFunction[] = { + { ESC_SENSOR_UART, FUNCTION_ESC_SENSOR }, +}; + +void targetConfiguration(void) +{ + targetSerialPortFunctionConfig(targetSerialPortFunction, ARRAYLEN(targetSerialPortFunction)); +} +#endif diff --git a/src/main/target/KAKUTEF7/target.c b/src/main/target/KAKUTEF7/target.c index 615c7d723b..f62c4c20a3 100644 --- a/src/main/target/KAKUTEF7/target.c +++ b/src/main/target/KAKUTEF7/target.c @@ -28,7 +28,7 @@ #include "drivers/timer_def.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - DEF_TIM(TIM1, CH3, PE13, TIM_USE_PPM, 0, 1), // PPM / CAM_CONTROL, DMA2_ST6 + DEF_TIM(TIM1, CH3, PE13, TIM_USE_PPM, 0, 1), // PPM, DMA2_ST6 DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0), // M1 , DMA1_ST7 DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0), // M2 , DMA1_ST2 @@ -37,6 +37,6 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { DEF_TIM(TIM8, CH4, PC9, TIM_USE_MOTOR, 0, 0), // M5 , DMA2_ST7 DEF_TIM(TIM5, CH4, PA3, TIM_USE_MOTOR, 0, 0), // M6 , DMA1_ST1 - DEF_TIM(TIM4, CH1, PD12, TIM_USE_LED, 0, 0), // LED_TRIP , DMA1_ST0 + DEF_TIM(TIM4, CH1, PD12, TIM_USE_LED, 0, 0), // LED_TRIP, DMA1_ST0 }; diff --git a/src/main/target/KAKUTEF7/target.h b/src/main/target/KAKUTEF7/target.h index 2ead6064fe..93819a5c64 100644 --- a/src/main/target/KAKUTEF7/target.h +++ b/src/main/target/KAKUTEF7/target.h @@ -20,10 +20,19 @@ #pragma once -//#define USE_TARGET_CONFIG +#define USE_TARGET_CONFIG +#ifdef KAKUTEF7V2 +#define TARGET_BOARD_IDENTIFIER "KT76" +#else #define TARGET_BOARD_IDENTIFIER "KTF7" +#endif + +#ifdef KAKUTEF7V2 +#define USBD_PRODUCT_STRING "KakuteF7-V2" +#else #define USBD_PRODUCT_STRING "KakuteF7" +#endif #define LED0_PIN PA2 @@ -31,29 +40,24 @@ #define BEEPER_PIN PD15 #define BEEPER_INVERTED +//define camera control +#define CAMERA_CONTROL_PIN PE13 + #define USE_ACC #define USE_GYRO - -//define camera control -#define CAMERA_CONTROL_PIN PE13 +#define USE_EXTI // ICM-20689 #define USE_ACC_SPI_ICM20689 #define USE_GYRO_SPI_ICM20689 -#define GYRO_ICM20689_ALIGN CW270_DEG -#define ACC_ICM20689_ALIGN CW270_DEG -#define MPU_INT_EXTI PE1 - -#define ICM20689_CS_PIN SPI4_NSS_PIN -#define ICM20689_SPI_INSTANCE SPI4 -#define GYRO_1_CS_PIN ICM20689_CS_PIN -#define GYRO_1_SPI_INSTANCE ICM20689_SPI_INSTANCE - -#define ACC_1_ALIGN ACC_ICM20689_ALIGN -#define GYRO_1_ALIGN GYRO_ICM20689_ALIGN +#define GYRO_1_CS_PIN SPI4_NSS_PIN +#define GYRO_1_SPI_INSTANCE SPI4 +#define GYRO_1_ALIGN CW270_DEG +#define ACC_1_ALIGN CW270_DEG +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PE1 #define USE_MPU_DATA_READY_SIGNAL -#define USE_EXTI #define USE_VCP #define USE_USB_DETECT @@ -119,16 +123,11 @@ #define MAX7456_RESTORE_CLK (SPI_CLOCK_FAST) #define USE_SDCARD +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED #define SDCARD_DETECT_PIN PD8 - #define SDCARD_SPI_INSTANCE SPI1 #define SDCARD_SPI_CS_PIN SPI1_NSS_PIN - -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 422kHz - -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 8 // 27MHz - #define SDCARD_DMA_STREAM_TX_FULL DMA2_Stream5 #define SDCARD_DMA_CHANNEL 3 @@ -145,10 +144,9 @@ #define USE_MAG #define USE_MAG_HMC5883 #define USE_MAG_QMC5883 +#define USE_MAG_LIS3MDL #define MAG_I2C_INSTANCE I2C_DEVICE -#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO) - #define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC #define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC @@ -163,7 +161,6 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_UART SERIAL_PORT_USART6 #define SERIALRX_PROVIDER SERIALRX_SBUS -#define ESC_SENSOR_UART SERIAL_PORT_USART7 #define USE_SERIAL_4WAY_BLHELI_INTERFACE @@ -176,4 +173,3 @@ #define USABLE_TIMER_CHANNEL_COUNT 8 #define USED_TIMERS ( TIM_N(1) | TIM_N(5) | TIM_N(3) | TIM_N(4) | TIM_N(8) ) - diff --git a/src/main/target/KAKUTEF7/target.mk b/src/main/target/KAKUTEF7/target.mk index d1d833316f..1c53e1e72e 100644 --- a/src/main/target/KAKUTEF7/target.mk +++ b/src/main/target/KAKUTEF7/target.mk @@ -1,5 +1,10 @@ +ifeq ($(TARGET), KAKUTEF7V2) +F7X5XI_TARGETS += $(TARGET) +else F7X5XG_TARGETS += $(TARGET) -FEATURES += SDCARD VCP +endif + +FEATURES += SDCARD_SPI VCP TARGET_SRC = \ drivers/accgyro/accgyro_mpu.c \ @@ -7,6 +12,7 @@ TARGET_SRC = \ drivers/barometer/barometer_bmp280.c \ drivers/compass/compass_hmc5883l.c \ drivers/compass/compass_qmc5883l.c \ + drivers/compass/compass_lis3mdl.c \ drivers/light_ws2811strip.c \ drivers/light_ws2811strip_hal.c \ drivers/max7456.c diff --git a/src/main/target/KISSFC/target.h b/src/main/target/KISSFC/target.h index 98873865b5..3856acf78d 100644 --- a/src/main/target/KISSFC/target.h +++ b/src/main/target/KISSFC/target.h @@ -22,7 +22,6 @@ #define TARGET_BOARD_IDENTIFIER "KISS" -#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE #define SBUS_PORT_OPTIONS (SERIAL_STOPBITS_2 | SERIAL_PARITY_EVEN | SERIAL_INVERTED | SERIAL_BIDIR) @@ -37,19 +36,21 @@ #define BEEPER_INVERTED #define USE_EXTI -#define MPU_INT_EXTI PB2 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PB2 #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW + #ifdef KISSCC #define USE_TARGET_CONFIG #define USE_GYRO #define USE_GYRO_MPU6050 -#define GYRO_MPU6050_ALIGN CW90_DEG +#define GYRO_1_ALIGN CW90_DEG #define USE_ACC #define USE_ACC_MPU6050 -#define ACC_MPU6050_ALIGN CW90_DEG +#define ACC_1_ALIGN CW90_DEG #define TARGET_DEFAULT_MIXER MIXER_QUADX_1234 @@ -57,11 +58,11 @@ #else #define USE_GYRO #define USE_GYRO_MPU6050 -#define GYRO_MPU6050_ALIGN CW180_DEG +#define GYRO_1_ALIGN CW180_DEG #define USE_ACC #define USE_ACC_MPU6050 -#define ACC_MPU6050_ALIGN CW180_DEG +#define ACC_1_ALIGN CW180_DEG #endif #define USE_VCP @@ -113,4 +114,4 @@ #define TARGET_IO_PORTF (BIT(4)) #define USABLE_TIMER_CHANNEL_COUNT 11 -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(15) | TIM_N(16) | TIM_N(17)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(15) | TIM_N(17)) diff --git a/src/main/target/KISSFCV2F7/stm32_flash_f722_no_split.ld b/src/main/target/KISSFCV2F7/stm32_flash_f722_no_split.ld index 14fefb0fd5..ac09a4c48a 100644 --- a/src/main/target/KISSFCV2F7/stm32_flash_f722_no_split.ld +++ b/src/main/target/KISSFCV2F7/stm32_flash_f722_no_split.ld @@ -12,236 +12,40 @@ /* Entry Point */ ENTRY(Reset_Handler) +/* +0x08000000 to 0x0807FFFF 512K full flash +0x08000000 to 0x08003FFF 16K KISS bootloader +0x08004000 to 0x08007FFF 16K config +0x08008000 to 0x0807FFFF 480K ISR vector, firmware +*/ /* Specify the memory areas */ MEMORY { - ITCM_ISR (rx) : ORIGIN = 0x00000000, LENGTH = 1K - ITCM_RAM (rx) : ORIGIN = 0x00000400, LENGTH = 15K + ITCM_RAM (rx) : ORIGIN = 0x00000000, LENGTH = 16K - /* Bootloader stays in the first sector (16K) */ - FLASH_CONFIG (r) : ORIGIN = 0x08004000, LENGTH = 16K /*second (16K) sector for eeprom emulation*/ - FLASH (rx) : ORIGIN = 0x08008000, LENGTH = 480K /*main fw*/ + /* config occupies the entire flash sector 1 for the ease of erasure, 16K on F72x */ + ITCM_FLASH_CONFIG (r) : ORIGIN = 0x00204000, LENGTH = 16K + ITCM_FLASH (rx) : ORIGIN = 0x00208000, LENGTH = 480K - TCM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K - SRAM1 (rwx) : ORIGIN = 0x20010000, LENGTH = 176K - SRAM2 (rwx) : ORIGIN = 0x2003C000, LENGTH = 16K - MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K + AXIM_FLASH_CONFIG (r) : ORIGIN = 0x08004000, LENGTH = 16K + AXIM_FLASH (rx) : ORIGIN = 0x08008000, LENGTH = 480K + + DTCM_RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + SRAM1 (rwx) : ORIGIN = 0x20010000, LENGTH = 176K + SRAM2 (rwx) : ORIGIN = 0x2003C000, LENGTH = 16K + MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K } -/* note TCM could be used for stack */ -REGION_ALIAS("STACKRAM", TCM) -REGION_ALIAS("FASTRAM", TCM) +REGION_ALIAS("ITCM_FLASH1", ITCM_FLASH) +REGION_ALIAS("AXIM_FLASH1", AXIM_FLASH) + +REGION_ALIAS("FLASH", AXIM_FLASH) +REGION_ALIAS("FLASH_CONFIG", AXIM_FLASH_CONFIG) +REGION_ALIAS("FLASH1", AXIM_FLASH1) + +REGION_ALIAS("STACKRAM", DTCM_RAM) +REGION_ALIAS("FASTRAM", DTCM_RAM) REGION_ALIAS("RAM", SRAM1) - -/* Entry Point */ -ENTRY(Reset_Handler) - -/* Highest address of the user mode stack */ -_estack = ORIGIN(STACKRAM) + LENGTH(STACKRAM); /* end of RAM */ - -/* Base address where the config is stored. */ -__config_start = ORIGIN(FLASH_CONFIG); -__config_end = ORIGIN(FLASH_CONFIG) + LENGTH(FLASH_CONFIG); - -/* Generate a link error if heap and stack don't fit into RAM */ -_Min_Heap_Size = 0; /* required amount of heap */ -_Min_Stack_Size = 0x800; /* required amount of stack */ - -/* Define output sections */ -SECTIONS -{ - /* The startup code goes first into FLASH */ - .isr_vector : - { - . = ALIGN(4); - PROVIDE (isr_vector_table_base = .); - KEEP(*(.isr_vector)) /* Startup code */ - . = ALIGN(4); - } >FLASH - - /* Critical program code goes into ITCM RAM */ - /* Copy specific fast-executing code to ITCM RAM */ - tcm_code = LOADADDR(.tcm_code); - .tcm_code : - { - . = ALIGN(4); - tcm_code_start = .; - *(.tcm_code) - *(.tcm_code*) - . = ALIGN(4); - tcm_code_end = .; - } >ITCM_RAM AT >FLASH - - /* The program code and other data goes into FLASH */ - .text : - { - . = ALIGN(4); - *(.text) /* .text sections (code) */ - *(.text*) /* .text* sections (code) */ - *(.rodata) /* .rodata sections (constants, strings, etc.) */ - *(.rodata*) /* .rodata* sections (constants, strings, etc.) */ - *(.glue_7) /* glue arm to thumb code */ - *(.glue_7t) /* glue thumb to arm code */ - *(.eh_frame) - - KEEP (*(.init)) - KEEP (*(.fini)) - - . = ALIGN(4); - _etext = .; /* define a global symbols at end of code */ - } >FLASH - - - .ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >FLASH - .ARM : { - __exidx_start = .; - *(.ARM.exidx*) - __exidx_end = .; - } >FLASH - - .preinit_array : - { - PROVIDE_HIDDEN (__preinit_array_start = .); - KEEP (*(.preinit_array*)) - PROVIDE_HIDDEN (__preinit_array_end = .); - } >FLASH - .init_array : - { - PROVIDE_HIDDEN (__init_array_start = .); - KEEP (*(SORT(.init_array.*))) - KEEP (*(.init_array*)) - PROVIDE_HIDDEN (__init_array_end = .); - } >FLASH - .fini_array : - { - PROVIDE_HIDDEN (__fini_array_start = .); - KEEP (*(.fini_array*)) - KEEP (*(SORT(.fini_array.*))) - PROVIDE_HIDDEN (__fini_array_end = .); - } >FLASH - .pg_registry : - { - PROVIDE_HIDDEN (__pg_registry_start = .); - KEEP (*(.pg_registry)) - KEEP (*(SORT(.pg_registry.*))) - PROVIDE_HIDDEN (__pg_registry_end = .); - } >FLASH - .pg_resetdata : - { - PROVIDE_HIDDEN (__pg_resetdata_start = .); - KEEP (*(.pg_resetdata)) - PROVIDE_HIDDEN (__pg_resetdata_end = .); - } >FLASH - - /* used by the startup to initialize data */ - _sidata = LOADADDR(.data); - - /* Initialized data sections goes into RAM, load LMA copy after code */ - .data : - { - . = ALIGN(4); - _sdata = .; /* create a global symbol at data start */ - *(.data) /* .data sections */ - *(.data*) /* .data* sections */ - - . = ALIGN(4); - _edata = .; /* define a global symbol at data end */ - } >RAM AT> FLASH - - /* Uninitialized data section */ - . = ALIGN(4); - .bss : - { - /* This is used by the startup in order to initialize the .bss secion */ - _sbss = .; /* define a global symbol at bss start */ - __bss_start__ = _sbss; - *(.bss) - *(SORT_BY_ALIGNMENT(.bss*)) - *(COMMON) - - . = ALIGN(4); - _ebss = .; /* define a global symbol at bss end */ - __bss_end__ = _ebss; - } >RAM - - /* Uninitialized data section */ - . = ALIGN(4); - .sram2 (NOLOAD) : - { - /* This is used by the startup in order to initialize the .sram2 secion */ - _ssram2 = .; /* define a global symbol at sram2 start */ - __sram2_start__ = _ssram2; - *(.sram2) - *(SORT_BY_ALIGNMENT(.sram2*)) - *(COMMON) - - . = ALIGN(4); - _esram2 = .; /* define a global symbol at sram2 end */ - __sram2_end__ = _esram2; - } >SRAM2 - - /* used during startup to initialized fastram_data */ - _sfastram_idata = LOADADDR(.fastram_data); - - /* Initialized FAST_RAM section for unsuspecting developers */ - .fastram_data : - { - . = ALIGN(4); - _sfastram_data = .; /* create a global symbol at data start */ - *(.fastram_data) /* .data sections */ - *(.fastram_data*) /* .data* sections */ - - . = ALIGN(4); - _efastram_data = .; /* define a global symbol at data end */ - } >FASTRAM AT> FLASH - - . = ALIGN(4); - .fastram_bss (NOLOAD) : - { - _sfastram_bss = .; - __fastram_bss_start__ = _sfastram_bss; - *(.fastram_bss) - *(SORT_BY_ALIGNMENT(.fastram_bss*)) - - . = ALIGN(4); - _efastram_bss = .; - __fastram_bss_end__ = _efastram_bss; - } >FASTRAM - - /* User_heap_stack section, used to check that there is enough RAM left */ - _heap_stack_end = ORIGIN(STACKRAM)+LENGTH(STACKRAM) - 8; /* 8 bytes to allow for alignment */ - _heap_stack_begin = _heap_stack_end - _Min_Stack_Size - _Min_Heap_Size; - . = _heap_stack_begin; - ._user_heap_stack : - { - . = ALIGN(4); - PROVIDE ( end = . ); - PROVIDE ( _end = . ); - . = . + _Min_Heap_Size; - . = . + _Min_Stack_Size; - . = ALIGN(4); - } >STACKRAM = 0xa5 - - /* MEMORY_bank1 section, code must be located here explicitly */ - /* Example: extern int foo(void) __attribute__ ((section (".mb1text"))); */ - .memory_b1_text : - { - *(.mb1text) /* .mb1text sections (code) */ - *(.mb1text*) /* .mb1text* sections (code) */ - *(.mb1rodata) /* read-only data (constants) */ - *(.mb1rodata*) - } >MEMORY_B1 - - /* Remove information from the standard libraries */ - /DISCARD/ : - { - libc.a ( * ) - libm.a ( * ) - libgcc.a ( * ) - } - - .ARM.attributes 0 : { *(.ARM.attributes) } -} - +INCLUDE "stm32_flash_f7_split.ld" diff --git a/src/main/target/KISSFCV2F7/target.c b/src/main/target/KISSFCV2F7/target.c index f6a5e2557b..82ca175d4e 100644 --- a/src/main/target/KISSFCV2F7/target.c +++ b/src/main/target/KISSFCV2F7/target.c @@ -37,7 +37,7 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { DEF_TIM(TIM4, CH1, PB6, TIM_USE_MOTOR, 0, 0), DEF_TIM(TIM4, CH2, PB7, TIM_USE_MOTOR, 0, 0), - DEF_TIM(TIM8, CH2, PC7, TIM_USE_MOTOR, 0, 0), + DEF_TIM(TIM8, CH2, PC7, TIM_USE_MOTOR, 0, 1), DEF_TIM(TIM2, CH2, PB3, TIM_USE_LED, 0, 0) diff --git a/src/main/target/KISSFCV2F7/target.h b/src/main/target/KISSFCV2F7/target.h index a9e012ff5b..2c4f971cfd 100644 --- a/src/main/target/KISSFCV2F7/target.h +++ b/src/main/target/KISSFCV2F7/target.h @@ -26,24 +26,24 @@ #define USBD_PRODUCT_STRING "KISSFCV2F7" -#define LED0 PA8 // blue -#define LED1 PC8 // blingbling +#define LED0_PIN PA8 // blue +#define LED1_PIN PC8 // blingbling #define LED1_INVERTED #define USE_BEEPER #define BEEPER_PIN PC9 #define BEEPER_INVERTED -#define MPU6000_CS_PIN PB12 -#define MPU6000_SPI_INSTANCE SPI2 +#define GYRO_1_CS_PIN PB12 +#define GYRO_1_SPI_INSTANCE SPI2 #define USE_GYRO #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW90_DEG +#define GYRO_1_ALIGN CW90_DEG #define USE_ACC #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW90_DEG +#define ACC_1_ALIGN CW90_DEG #define USE_SPI @@ -100,7 +100,7 @@ #define SPEKTRUM_BIND_PIN UART2_RX_PIN #define USE_ADC -#define BOARD_HAS_VOLTAGE_DIVIDER +#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC #define VBAT_SCALE_DEFAULT 160 #define VBAT_ADC_PIN PB1 @@ -114,4 +114,4 @@ #define USABLE_TIMER_CHANNEL_COUNT 8 -#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9) | TIM_N(10) | TIM_N(11)) +#define USED_TIMERS ( TIM_N(2) | TIM_N(4) | TIM_N(5) | TIM_N(8) | TIM_N(9) ) diff --git a/src/main/target/KISSFCV2F7/target.mk b/src/main/target/KISSFCV2F7/target.mk index 25e86b723b..7101dd1221 100644 --- a/src/main/target/KISSFCV2F7/target.mk +++ b/src/main/target/KISSFCV2F7/target.mk @@ -1,7 +1,6 @@ F7X2RE_TARGETS += $(TARGET) LD_SCRIPT = $(ROOT)/src/main/target/$(TARGET)/stm32_flash_f722_no_split.ld -CFLAGS += -D'CUSTOM_VECT_TAB_OFFSET=0x8000' \ - -DCLOCK_SOURCE_USE_HSI +CFLAGS += -DCLOCK_SOURCE_USE_HSI TARGET_SRC = \ drivers/accgyro/accgyro_spi_mpu6000.c diff --git a/src/main/target/KIWIF4/target.h b/src/main/target/KIWIF4/target.h index 4a69c7d763..b80ac659f7 100644 --- a/src/main/target/KIWIF4/target.h +++ b/src/main/target/KIWIF4/target.h @@ -49,21 +49,19 @@ // MPU6000 interrupts #define USE_EXTI -#define MPU_INT_EXTI PC4 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC4 #define USE_MPU_DATA_READY_SIGNAL -#define MPU6000_CS_PIN PA4 -#define MPU6000_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 #define USE_GYRO #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW180_DEG +#define GYRO_1_ALIGN CW180_DEG #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW180_DEG - -#define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW180_DEG +#define ACC_1_ALIGN CW180_DEG #if defined(KIWIF4) || defined(KIWIF4V2) #define USE_MAX7456 @@ -83,21 +81,12 @@ #if defined(KIWIF4V2) #define USE_SDCARD - - +#define USE_SDCARD_SPI //#define SDCARD_DETECT_PIN PB9 #define SDCARD_SPI_INSTANCE SPI2 #define SDCARD_SPI_CS_PIN PB12 - -// SPI2 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init: -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz -// Divide to under 25MHz for normal operation: -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 // 21MHz - - //#define SDCARD_DMA_CHANNEL_TX DMA1_Stream5 //#define SDCARD_DMA_CHANNEL 0 - #define SDCARD_DMA_CHANNEL_TX DMA1_Stream4 #define SDCARD_DMA_CHANNEL 0 @@ -115,7 +104,6 @@ #define USE_UART1 #define UART1_RX_PIN PA10 #define UART1_TX_PIN PA9 -//#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 #define USE_UART3 #define UART3_RX_PIN PB11 @@ -182,6 +170,6 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(2)) -#define USABLE_TIMER_CHANNEL_COUNT 12 +#define USABLE_TIMER_CHANNEL_COUNT 5 -#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(8) | TIM_N(9) ) +#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(4) ) diff --git a/src/main/target/KIWIF4/target.mk b/src/main/target/KIWIF4/target.mk index 1cb2eec9a1..0fb93c0b75 100644 --- a/src/main/target/KIWIF4/target.mk +++ b/src/main/target/KIWIF4/target.mk @@ -1,5 +1,5 @@ F405_TARGETS += $(TARGET) -FEATURES += VCP ONBOARDFLASH SDCARD +FEATURES += VCP ONBOARDFLASH SDCARD_SPI TARGET_SRC = \ drivers/accgyro/accgyro_spi_mpu6000.c \ diff --git a/src/main/target/KROOZX/target.h b/src/main/target/KROOZX/target.h index 33018e08df..6918966a70 100644 --- a/src/main/target/KROOZX/target.h +++ b/src/main/target/KROOZX/target.h @@ -39,21 +39,22 @@ #define INVERTER_PIN_UART1 PB13 #define INVERTER_PIN_UART6 PB12 -#define MPU6000_CS_PIN PB2 -#define MPU6000_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PB2 +#define GYRO_1_SPI_INSTANCE SPI1 // MPU6000 interrupts #define USE_EXTI #define USE_MPU_DATA_READY_SIGNAL -#define MPU_INT_EXTI PA4 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PA4 #define USE_GYRO #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW90_DEG +#define GYRO_1_ALIGN CW90_DEG #define USE_ACC #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW270_DEG +#define ACC_1_ALIGN CW270_DEG #define USE_MAG #define USE_MAG_HMC5883 @@ -65,13 +66,11 @@ #define USE_BARO_MS5611 #define USE_SDCARD +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED #define SDCARD_DETECT_PIN PC13 #define SDCARD_SPI_INSTANCE SPI3 #define SDCARD_SPI_CS_PIN PA15 -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 // 21MHz - #define SDCARD_DMA_CHANNEL_TX DMA1_Stream5 #define SDCARD_DMA_CHANNEL 0 diff --git a/src/main/target/KROOZX/target.mk b/src/main/target/KROOZX/target.mk index 0c294619c1..e12a6ce00f 100644 --- a/src/main/target/KROOZX/target.mk +++ b/src/main/target/KROOZX/target.mk @@ -1,5 +1,5 @@ F405_TARGETS += $(TARGET) -FEATURES += VCP SDCARD +FEATURES += VCP SDCARD_SPI HSE_VALUE = 16000000 TARGET_SRC = \ diff --git a/src/main/target/LUMBAF3/target.c b/src/main/target/LUMBAF3/target.c index 4988c9cb37..fc5deff135 100644 --- a/src/main/target/LUMBAF3/target.c +++ b/src/main/target/LUMBAF3/target.c @@ -29,16 +29,16 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - DEF_TIM(TIM2, CH2, PA1, TIM_USE_PPM, 0), // PPM - DEF_TIM(TIM3, CH3, PB0, TIM_USE_PWM, 0), // SS1Rx - DEF_TIM(TIM3, CH2, PB5, TIM_USE_PWM, 0), // SS1Tx + DEF_TIM(TIM2, CH2, PA1, TIM_USE_PPM, 0), // PPM + DEF_TIM(TIM3, CH3, PB0, TIM_USE_PWM, 0), // SS1Rx + DEF_TIM(TIM3, CH2, PB5, TIM_USE_PWM, 0), // SS1Tx - DEF_TIM(TIM8, CH3, PB9, TIM_USE_MOTOR, 0), // S1 - DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, 0), // S2 - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MOTOR, 0), // S3 - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, 0), // S4 - DEF_TIM(TIM16, CH1, PB4, TIM_USE_MOTOR, 0), // S5 + DEF_TIM(TIM8, CH3, PB9, TIM_USE_MOTOR, 0), // S1 + DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, 0), // S2 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MOTOR, 0), // S3 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, 0), // S4 + DEF_TIM(TIM16, CH1, PB4, TIM_USE_MOTOR, 0), // S5 - DEF_TIM(TIM15, CH1, PA2, TIM_USE_LED, 0), // LED_STRIP - DEF_TIM(TIM2, CH1, PA15, TIM_USE_ANY, 0), // CAMERA CONTROL + DEF_TIM(TIM15, CH1, PA2, TIM_USE_LED, 0), // LED_STRIP + DEF_TIM(TIM2, CH1, PA15, TIM_USE_CAMERA_CONTROL, 0), // CAMERA CONTROL }; diff --git a/src/main/target/LUMBAF3/target.h b/src/main/target/LUMBAF3/target.h index 52e85e9f9b..72237ff93e 100644 --- a/src/main/target/LUMBAF3/target.h +++ b/src/main/target/LUMBAF3/target.h @@ -27,15 +27,16 @@ #define BEEPER_PIN PC15 #define USE_EXTI -#define MPU_INT_EXTI PA3 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PA3 #define USE_MPU_DATA_READY_SIGNAL #define USE_SPI #define USE_SPI_DEVICE_1 #define USE_SPI_DEVICE_2 -#define MPU6000_CS_PIN PA4 -#define MPU6000_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 #define FLASH_CS_PIN PB12 #define FLASH_SPI_INSTANCE SPI2 @@ -45,11 +46,11 @@ #define USE_GYRO #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW90_DEG +#define GYRO_1_ALIGN CW90_DEG #define USE_ACC #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW90_DEG +#define ACC_1_ALIGN CW90_DEG #define USE_VCP #define USE_UART1 @@ -77,8 +78,6 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE -#define CAMERA_CONTROL_PIN PA15 - #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS #define SERIALRX_UART SERIAL_PORT_USART1 diff --git a/src/main/target/LUX_RACE/target.h b/src/main/target/LUX_RACE/target.h index d5bd3208ac..d26629a7d9 100644 --- a/src/main/target/LUX_RACE/target.h +++ b/src/main/target/LUX_RACE/target.h @@ -27,13 +27,12 @@ #endif // Removed to make the firmware fit into flash (in descending order of priority): -#undef USE_RTC_TIME +//#undef USE_RTC_TIME #undef USE_RX_MSP -#undef USE_ESC_SENSOR_INFO +//#undef USE_ESC_SENSOR_INFO #define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC -#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT #define LED0_PIN PC15 #define LED1_PIN PC14 @@ -52,7 +51,8 @@ // MPU6500 interrupt #define USE_EXTI -#define MPU_INT_EXTI PA5 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PA5 //#define DEBUG_MPU_DATA_READY_INTERRUPT #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW @@ -77,47 +77,35 @@ #define SPI2_MOSI_PIN PB15 #define USE_SDCARD - +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED #define SDCARD_DETECT_PIN PC13 - #define SDCARD_SPI_INSTANCE SPI2 #define SDCARD_SPI_CS_PIN SPI2_NSS_PIN -// SPI2 is on the APB1 bus whose clock runs at 36MHz. Divide to under 400kHz for init: -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128 -// Divide to under 25MHz for normal operation: -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 2 - // Note, this is the same DMA channel as UART1_RX. Luckily we don't use DMA for USART Rx. #define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 #endif -#define MPU6000_CS_PIN SPI1_NSS_PIN -#define MPU6000_SPI_INSTANCE SPI1 -#define MPU6500_CS_PIN SPI1_NSS_PIN -#define MPU6500_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN SPI1_NSS_PIN +#define GYRO_1_SPI_INSTANCE SPI1 #define USE_GYRO +#define GYRO_1_ALIGN CW270_DEG #ifdef LUXV2_RACE -#define USE_GYRO_MPU6000 #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW270_DEG #else -#define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW270_DEG #endif #define USE_ACC +#define ACC_1_ALIGN CW270_DEG #ifdef LUXV2_RACE #define USE_ACC_MPU6000 #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW270_DEG #else #define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW270_DEG #endif #define USE_VCP @@ -178,7 +166,8 @@ #ifdef LUXV2_RACE #define USABLE_TIMER_CHANNEL_COUNT 6 +#define USED_TIMERS (TIM_N(1) | TIM_N(3) | TIM_N(8) | TIM_N(16)) #else #define USABLE_TIMER_CHANNEL_COUNT 12 +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(15) | TIM_N(16)) #endif -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(15)) diff --git a/src/main/target/LUX_RACE/target.mk b/src/main/target/LUX_RACE/target.mk index a85188a428..f5d310faab 100644 --- a/src/main/target/LUX_RACE/target.mk +++ b/src/main/target/LUX_RACE/target.mk @@ -1,5 +1,5 @@ F3_TARGETS += $(TARGET) -FEATURES = VCP SDCARD +FEATURES = VCP SDCARD_SPI TARGET_SRC = \ drivers/accgyro/accgyro_mpu.c \ diff --git a/src/main/target/MATEKF405/target.h b/src/main/target/MATEKF405/target.h index 8043d055d1..60e2e58d81 100644 --- a/src/main/target/MATEKF405/target.h +++ b/src/main/target/MATEKF405/target.h @@ -40,33 +40,33 @@ #define SPI1_MISO_PIN PA6 #define SPI1_MOSI_PIN PA7 -#define MPU6000_CS_PIN PC2 -#define MPU6000_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PC2 +#define GYRO_1_SPI_INSTANCE SPI1 -#define MPU6500_CS_PIN PC2 -#define MPU6500_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PC2 +#define GYRO_1_SPI_INSTANCE SPI1 #define USE_EXTI -#define MPU_INT_EXTI PC3 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC3 #define USE_MPU_DATA_READY_SIGNAL #define USE_GYRO #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW270_DEG - #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW180_DEG +#define GYRO_1_ALIGN CW270_DEG +//#define GYRO_1_ALIGN CW180_DEG // XXX MPU6500 align, must be configured after flashing #define USE_ACC #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW270_DEG - #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW180_DEG +#define ACC_1_ALIGN CW270_DEG +//#define ACC_1_ALIGN CW180_DEG // XXX MPU6500 align, must be configured after flashing #define USE_MAG #define USE_MAG_HMC5883 #define USE_MAG_QMC5883 +#define USE_MAG_LIS3MDL // *************** Baro ************************** #define USE_I2C @@ -104,16 +104,10 @@ // *************** SD Card ************************** #define USE_SDCARD +#define USE_SDCARD_SPI #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT - #define SDCARD_SPI_INSTANCE SPI3 #define SDCARD_SPI_CS_PIN PC1 - -// SPI3 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init: -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz -// Divide to under 25MHz for normal operation: -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 // 21MHz - #define SDCARD_DMA_CHANNEL_TX DMA1_Stream7 #define SDCARD_DMA_CHANNEL 0 diff --git a/src/main/target/MATEKF405/target.mk b/src/main/target/MATEKF405/target.mk index aeedf2ad3e..d43148dd65 100644 --- a/src/main/target/MATEKF405/target.mk +++ b/src/main/target/MATEKF405/target.mk @@ -1,5 +1,5 @@ F405_TARGETS += $(TARGET) -FEATURES += VCP ONBOARDFLASH SDCARD +FEATURES += VCP ONBOARDFLASH SDCARD_SPI TARGET_SRC = \ drivers/accgyro/accgyro_spi_mpu6500.c \ @@ -10,4 +10,5 @@ TARGET_SRC = \ drivers/barometer/barometer_ms5611.c \ drivers/compass/compass_hmc5883l.c \ drivers/compass/compass_qmc5883l.c \ + drivers/compass/compass_lis3mdl.c \ drivers/max7456.c diff --git a/src/main/target/MATEKF411/target.h b/src/main/target/MATEKF411/target.h index 6c6581266d..bee368b113 100644 --- a/src/main/target/MATEKF411/target.h +++ b/src/main/target/MATEKF411/target.h @@ -38,27 +38,22 @@ #define SPI1_MISO_PIN PA6 #define SPI1_MOSI_PIN PA7 -#define MPU6000_CS_PIN PA4 -#define MPU6000_SPI_INSTANCE SPI1 - -#define MPU6500_CS_PIN PA4 -#define MPU6500_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 #define USE_EXTI -#define MPU_INT_EXTI PA1 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PA1 #define USE_MPU_DATA_READY_SIGNAL #define USE_GYRO #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW180_DEG +#define USE_GYRO_SPI_MPU6500 +#define GYRO_1_ALIGN CW180_DEG #define USE_ACC #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW180_DEG - -#define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW180_DEG #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW180_DEG +#define ACC_1_ALIGN CW180_DEG // *************** Baro ************************** #define USE_I2C diff --git a/src/main/target/MATEKF411/target.mk b/src/main/target/MATEKF411/target.mk index 548aef1968..cb6bf78f82 100644 --- a/src/main/target/MATEKF411/target.mk +++ b/src/main/target/MATEKF411/target.mk @@ -1,5 +1,5 @@ F411_TARGETS += $(TARGET) -FEATURES += SDCARD VCP +FEATURES += SDCARD_SPI VCP TARGET_SRC = \ drivers/accgyro/accgyro_spi_mpu6500.c \ diff --git a/src/main/target/MATEKF411RX/target.h b/src/main/target/MATEKF411RX/target.h index 7dde270664..664997cd11 100644 --- a/src/main/target/MATEKF411RX/target.h +++ b/src/main/target/MATEKF411RX/target.h @@ -37,19 +37,20 @@ #define SPI1_MISO_PIN PA6 #define SPI1_MOSI_PIN PA7 -#define MPU6000_CS_PIN PA4 -#define MPU6000_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 #define USE_EXTI -#define MPU_INT_EXTI PA1 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PA1 #define USE_MPU_DATA_READY_SIGNAL #define USE_GYRO #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW180_DEG +#define GYRO_1_ALIGN CW180_DEG #define USE_ACC #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW180_DEG +#define ACC_1_ALIGN CW180_DEG // *************** SPI2 OSD ***************************** #define USE_SPI_DEVICE_2 @@ -66,33 +67,28 @@ #define SPI3_SCK_PIN PB3 #define SPI3_MISO_PIN PB4 #define SPI3_MOSI_PIN PB5 -#define SPI3_NSS_PIN PA15 #define USE_RX_SPI #define RX_SPI_INSTANCE SPI3 -#define RX_NSS_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA +#define RX_NSS_PIN PA15 -#define RX_SCK_PIN SPI3_SCK_PIN -#define RX_MISO_PIN SPI3_MISO_PIN -#define RX_MOSI_PIN SPI3_MOSI_PIN -#define RX_NSS_PIN SPI3_NSS_PIN +#define RX_CC2500_SPI_DISABLE_CHIP_DETECTION +#define RX_CC2500_SPI_GDO_0_PIN PC14 +#define RX_CC2500_SPI_LED_PIN PB9 +#define RX_CC2500_SPI_LED_PIN_INVERTED -#define RX_FRSKY_SPI_DISABLE_CHIP_DETECTION -#define RX_FRSKY_SPI_GDO_0_PIN PC14 -#define RX_FRSKY_SPI_LED_PIN PB9 -#define RX_FRSKY_SPI_LED_PIN_INVERTED +#define USE_RX_CC2500_SPI_PA_LNA +#define RX_CC2500_SPI_TX_EN_PIN PA8 +#define RX_CC2500_SPI_LNA_EN_PIN PA13 -#define USE_RX_FRSKY_SPI_PA_LNA -#define RX_FRSKY_SPI_TX_EN_PIN PA8 -#define RX_FRSKY_SPI_LNA_EN_PIN PA13 - -#define USE_RX_FRSKY_SPI_DIVERSITY -#define RX_FRSKY_SPI_ANT_SEL_PIN PA14 +#define USE_RX_CC2500_SPI_DIVERSITY +#define RX_CC2500_SPI_ANT_SEL_PIN PA14 #define BINDPLUG_PIN PB2 #define USE_RX_FRSKY_SPI_D #define USE_RX_FRSKY_SPI_X +#define USE_RX_SFHSS_SPI #define DEFAULT_RX_FEATURE FEATURE_RX_SPI #define RX_SPI_DEFAULT_PROTOCOL RX_SPI_FRSKY_X #define USE_RX_FRSKY_SPI_TELEMETRY @@ -126,8 +122,6 @@ #define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC #define CURRENT_METER_SCALE_DEFAULT 179 -#define USE_LED_STRIP - #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff diff --git a/src/main/target/MATEKF411RX/target.mk b/src/main/target/MATEKF411RX/target.mk index 977b93bf5a..9e4bf4a968 100644 --- a/src/main/target/MATEKF411RX/target.mk +++ b/src/main/target/MATEKF411RX/target.mk @@ -1,11 +1,13 @@ F411_TARGETS += $(TARGET) -FEATURES += VCP SDCARD +FEATURES += VCP SDCARD_SPI TARGET_SRC = \ drivers/accgyro/accgyro_spi_mpu6000.c \ drivers/max7456.c \ drivers/rx/rx_cc2500.c \ + rx/cc2500_common.c \ rx/cc2500_frsky_shared.c \ rx/cc2500_frsky_d.c \ - rx/cc2500_frsky_x.c + rx/cc2500_frsky_x.c \ + rx/cc2500_sfhss.c diff --git a/src/main/target/MATEKF722/target.c b/src/main/target/MATEKF722/target.c index 473f66a3fc..829d5117c3 100644 --- a/src/main/target/MATEKF722/target.c +++ b/src/main/target/MATEKF722/target.c @@ -30,9 +30,9 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM - DEF_TIM(TIM8, CH1, PC6, TIM_USE_MOTOR, 0, 0), // S1 DMA1_ST4 - DEF_TIM(TIM8, CH2, PC7, TIM_USE_MOTOR, 0, 0), // S2 DMA2_ST3 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, 0, 0), // S3 DMA2_ST4 + DEF_TIM(TIM8, CH1, PC6, TIM_USE_MOTOR, 0, 0), // S1 DMA2_ST2 (XXX was DMA1_ST4) + DEF_TIM(TIM8, CH2, PC7, TIM_USE_MOTOR, 0, 1), // S2 DMA2_ST3 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, 0, 1), // S3 DMA2_ST4 DEF_TIM(TIM8, CH4, PC9, TIM_USE_MOTOR, 0, 0), // S4 DMA2_ST7 DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0), // S5 DMA1_ST2 diff --git a/src/main/target/MATEKF722/target.h b/src/main/target/MATEKF722/target.h index af2f59ccee..5c3a8aa057 100644 --- a/src/main/target/MATEKF722/target.h +++ b/src/main/target/MATEKF722/target.h @@ -41,29 +41,25 @@ #define SPI1_MISO_PIN PA6 #define SPI1_MOSI_PIN PA7 -#define MPU6500_CS_PIN PC2 -#define MPU6500_SPI_INSTANCE SPI1 - -#define ICM20689_CS_PIN PC2 -#define ICM20689_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PC2 +#define GYRO_1_SPI_INSTANCE SPI1 #define USE_EXTI -#define MPU_INT_EXTI PC3 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC3 #define USE_MPU_DATA_READY_SIGNAL #define USE_GYRO #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW180_DEG - #define USE_GYRO_SPI_ICM20689 -#define GYRO_ICM20689_ALIGN CW90_DEG +#define GYRO_1_ALIGN CW180_DEG +//#define GYRO_1_ALIGN CW90_DEG // XXX has to be post-flash configured #define USE_ACC #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW180_DEG - #define USE_ACC_SPI_ICM20689 -#define ACC_ICM20689_ALIGN CW90_DEG +#define ACC_1_ALIGN CW180_DEG +//#define ACC_1_ALIGN CW90_DEG // XXX has to be post-flash configured // *************** Baro ************************** #define USE_I2C @@ -83,9 +79,11 @@ #define USE_MAG #define USE_MAG_HMC5883 #define USE_MAG_QMC5883 +#define USE_MAG_LIS3MDL // *************** SD Card ************************** #define USE_SDCARD +#define USE_SDCARD_SPI #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define USE_SPI_DEVICE_3 @@ -96,11 +94,6 @@ #define SDCARD_SPI_INSTANCE SPI3 #define SDCARD_SPI_CS_PIN PC1 -// SPI3 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init: -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz -// Divide to under 25MHz for normal operation: -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 // 21MHz - #define SDCARD_DMA_STREAM_TX_FULL DMA1_Stream7 #define SDCARD_DMA_CHANNEL 0 diff --git a/src/main/target/MATEKF722/target.mk b/src/main/target/MATEKF722/target.mk index 7934b618a3..5926db3bab 100644 --- 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_SPI VCP TARGET_SRC = \ drivers/accgyro/accgyro_spi_mpu6500.c \ @@ -10,4 +10,5 @@ TARGET_SRC = \ drivers/barometer/barometer_ms5611.c \ drivers/compass/compass_hmc5883l.c \ drivers/compass/compass_qmc5883l.c \ + drivers/compass/compass_lis3mdl.c \ drivers/max7456.c diff --git a/src/main/target/MICROSCISKY/target.h b/src/main/target/MICROSCISKY/target.h index a4f573e284..70f233f8b9 100644 --- a/src/main/target/MICROSCISKY/target.h +++ b/src/main/target/MICROSCISKY/target.h @@ -51,11 +51,11 @@ #define USE_GYRO #define USE_GYRO_MPU6050 -#define GYRO_MPU6050_ALIGN CW0_DEG +#define GYRO_1_ALIGN CW0_DEG #define USE_ACC #define USE_ACC_MPU6050 -#define ACC_MPU6050_ALIGN CW0_DEG +#define ACC_1_ALIGN CW0_DEG #define USE_BARO #define USE_BARO_MS5611 diff --git a/src/main/target/MIDELICF3/target.h b/src/main/target/MIDELICF3/target.h index 094fb7524a..ad6fe13b14 100644 --- a/src/main/target/MIDELICF3/target.h +++ b/src/main/target/MIDELICF3/target.h @@ -38,14 +38,15 @@ #define USE_GYRO #define USE_GYRO_MPU6050 -#define GYRO_MPU6050_ALIGN CW270_DEG +#define GYRO_1_ALIGN CW270_DEG #define USE_ACC #define USE_ACC_MPU6050 -#define ACC_MPU6050_ALIGN CW270_DEG +#define ACC_1_ALIGN CW270_DEG #define USE_EXTI -#define MPU_INT_EXTI PA13 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PA13 #define USE_MPU_DATA_READY_SIGNAL #define USE_VCP @@ -71,28 +72,19 @@ #define USE_SPI #define USE_SPI_DEVICE_1 -#define SPI1_NSS_PIN PA4 #define SPI1_SCK_PIN PA5 #define SPI1_MISO_PIN PA6 #define SPI1_MOSI_PIN PA7 #define USE_SPI_DEVICE_2 -#define SPI2_NSS_PIN PB12 #define SPI2_SCK_PIN PB13 #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 #define USE_SDCARD -#define USE_SDCARD_SPI2 - +#define USE_SDCARD_SPI #define SDCARD_SPI_INSTANCE SPI2 -#define SDCARD_SPI_CS_PIN SPI2_NSS_PIN - -// SPI2 is on the APB1 bus whose clock runs at 36MHz. Divide to under 400kHz for init: -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128 -// // Divide to under 25MHz for normal operation: -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 2 - +#define SDCARD_SPI_CS_PIN PB12 #define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 #define USE_ADC @@ -109,41 +101,37 @@ #define USE_RX_SPI #define RX_SPI_INSTANCE SPI1 -#define RX_NSS_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA - #define USE_RX_FRSKY_SPI_D #define USE_RX_FRSKY_SPI_X +#define USE_RX_SFHSS_SPI #define DEFAULT_RX_FEATURE FEATURE_RX_SPI #define RX_SPI_DEFAULT_PROTOCOL RX_SPI_FRSKY_X #define USE_RX_FRSKY_SPI_TELEMETRY -#define RX_NSS_PIN SPI1_NSS_PIN -#define RX_SCK_PIN SPI1_SCK_PIN -#define RX_MISO_PIN SPI1_MISO_PIN -#define RX_MOSI_PIN SPI1_MOSI_PIN +#define RX_NSS_PIN PA4 -#define RX_FRSKY_SPI_GDO_0_PIN PB0 +#define RX_CC2500_SPI_GDO_0_PIN PB0 -#define RX_FRSKY_SPI_LED_PIN PB6 +#define RX_CC2500_SPI_LED_PIN PB6 -#define USE_RX_FRSKY_SPI_PA_LNA +#define USE_RX_CC2500_SPI_PA_LNA -#define RX_FRSKY_SPI_TX_EN_PIN PB1 -#define RX_FRSKY_SPI_LNA_EN_PIN PB11 +#define RX_CC2500_SPI_TX_EN_PIN PB1 +#define RX_CC2500_SPI_LNA_EN_PIN PB11 -#define USE_RX_FRSKY_SPI_DIVERSITY +#define USE_RX_CC2500_SPI_DIVERSITY -#define RX_FRSKY_SPI_ANT_SEL_PIN PB2 +#define RX_CC2500_SPI_ANT_SEL_PIN PB2 #define BINDPLUG_PIN PC13 #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define USE_ESCSERIAL -#define ESCSERIAL_TIMER_TX_HARDWARE 0 +#define ESCSERIAL_TIMER_TX_PIN PB9 // Motor 6, can't use escserial for hexa #define DEFAULT_FEATURES (FEATURE_AIRMODE | FEATURE_TELEMETRY) diff --git a/src/main/target/MIDELICF3/target.mk b/src/main/target/MIDELICF3/target.mk index 0c78eda412..ec9d644d73 100644 --- a/src/main/target/MIDELICF3/target.mk +++ b/src/main/target/MIDELICF3/target.mk @@ -1,11 +1,13 @@ F3_TARGETS += $(TARGET) -FEATURES = VCP SDCARD +FEATURES = VCP SDCARD_SPI TARGET_SRC = \ drivers/accgyro/accgyro_mpu.c \ drivers/accgyro/accgyro_mpu6050.c \ drivers/rx/rx_cc2500.c \ + rx/cc2500_common.c \ rx/cc2500_frsky_shared.c \ rx/cc2500_frsky_d.c \ - rx/cc2500_frsky_x.c + rx/cc2500_frsky_x.c \ + rx/cc2500_sfhss.c diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index 7ba0dc2d03..c5168010cb 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/src/main/target/MOTOLAB/target.h @@ -22,7 +22,6 @@ #define TARGET_BOARD_IDENTIFIER "MOTO" // MotoLab -#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT #define USE_TARGET_CONFIG #define LED0_PIN PB5 // Blue LEDs - PB5 @@ -34,7 +33,8 @@ // MPU6050 interrupts #define USE_EXTI -#define MPU_INT_EXTI PA15 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PA15 #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW @@ -42,20 +42,16 @@ #define USE_ACC #define USE_GYRO_MPU6050 -#define GYRO_MPU6050_ALIGN CW180_DEG - #define USE_ACC_MPU6050 -#define ACC_MPU6050_ALIGN CW180_DEG - #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW180_DEG #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW180_DEG -#define MPU6000_CS_GPIO GPIOB -#define MPU6000_CS_PIN PB12 -#define MPU6000_SPI_INSTANCE SPI2 +#define GYRO_1_CS_PIN PB12 +#define GYRO_1_SPI_INSTANCE SPI2 + +#define ACC_1_ALIGN CW180_DEG +#define GYRO_1_ALIGN CW180_DEG #define USE_VCP #define USE_UART1 @@ -108,4 +104,4 @@ #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) #define USABLE_TIMER_CHANNEL_COUNT 10 -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(16) | TIM_N(17)) diff --git a/src/main/target/MOTOLABF4/target.h b/src/main/target/MOTOLABF4/target.h index d2a8004140..1522e18010 100644 --- a/src/main/target/MOTOLABF4/target.h +++ b/src/main/target/MOTOLABF4/target.h @@ -38,20 +38,21 @@ #define BEEPER_PIN PB4 #define BEEPER_INVERTED -#define MPU6000_CS_PIN PA4 -#define MPU6000_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 #define USE_ACC #define USE_ACC_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW180_DEG +#define GYRO_1_ALIGN CW180_DEG #define USE_GYRO #define USE_GYRO_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW180_DEG +#define ACC_1_ALIGN CW180_DEG // MPU6000 interrupts #define USE_EXTI -#define MPU_INT_EXTI PC5 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC5 #define USE_MPU_DATA_READY_SIGNAL //#define ENSURE_MPU_DATA_READY_IS_LOW //#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready @@ -75,7 +76,6 @@ #define USE_UART1 #define UART1_RX_PIN PA10 #define UART1_TX_PIN PA9 -#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 #define USE_UART2 #define UART2_RX_PIN PA3 @@ -116,16 +116,12 @@ #define USE_SPI_DEVICE_2 // SDcard #define USE_SDCARD +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED #define SDCARD_DETECT_PIN PC13 #define SDCARD_SPI_INSTANCE SPI2 #define SDCARD_SPI_CS_PIN PB12 #define SDCARD_SPI_CS_CFG IOCFG_OUT_OD -// SPI2 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init: -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz -// Divide to under 25MHz for normal operation: -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 // 21MHz - #define SDCARD_DMA_CHANNEL_TX DMA1_Stream4 #define SDCARD_DMA_CHANNEL 0 @@ -134,11 +130,11 @@ //#define I2C_DEVICE (I2CDEV_2) #define USE_ADC -#define BOARD_HAS_VOLTAGE_DIVIDER +#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC #define VBAT_ADC_PIN PC0 #define CURRENT_METER_ADC_PIN PC1 // Reserved pins, not connected -//#define RSSI_ADC_GPIO_PIN PC2 +//#define RSSI_ADC_PIN PC2 #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/MOTOLABF4/target.mk b/src/main/target/MOTOLABF4/target.mk index 8f348d0afb..75ca759a56 100644 --- a/src/main/target/MOTOLABF4/target.mk +++ b/src/main/target/MOTOLABF4/target.mk @@ -1,5 +1,5 @@ F405_TARGETS += $(TARGET) -FEATURES += SDCARD VCP +FEATURES += SDCARD_SPI VCP ifeq ($(TARGET), MLTEMPF4) TARGET_SRC = \ diff --git a/src/main/target/MULTIFLITEPICO/target.h b/src/main/target/MULTIFLITEPICO/target.h index 733fc37f9c..4ef533a892 100644 --- a/src/main/target/MULTIFLITEPICO/target.h +++ b/src/main/target/MULTIFLITEPICO/target.h @@ -23,7 +23,6 @@ #define TARGET_BOARD_IDENTIFIER "MFPB" #define USE_TARGET_CONFIG -#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE #define LED0_PIN PB3 @@ -32,17 +31,18 @@ #define BEEPER_INVERTED #define USE_EXTI -#define MPU_INT_EXTI PC13 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC13 #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW #define USE_GYRO #define USE_GYRO_MPU6050 -#define GYRO_MPU6050_ALIGN CW90_DEG +#define GYRO_1_ALIGN CW90_DEG #define USE_ACC #define USE_ACC_MPU6050 -#define ACC_MPU6050_ALIGN CW90_DEG +#define ACC_1_ALIGN CW90_DEG #define USE_BARO #define USE_BARO_MS5611 diff --git a/src/main/target/MULTIFLITEPICO/target.mk b/src/main/target/MULTIFLITEPICO/target.mk index f642917f9e..fdc4db6500 100644 --- a/src/main/target/MULTIFLITEPICO/target.mk +++ b/src/main/target/MULTIFLITEPICO/target.mk @@ -9,4 +9,4 @@ TARGET_SRC = \ drivers/barometer/barometer_bmp280.c \ drivers/compass/compass_ak8975.c \ drivers/compass/compass_hmc5883l.c \ - drivers/compass/compass_qmc5883l.c + drivers/compass/compass_qmc5883l.c \ diff --git a/src/main/target/NAZE/config.c b/src/main/target/NAZE/config.c index 9ee4aa1793..479f4d4d7e 100644 --- a/src/main/target/NAZE/config.c +++ b/src/main/target/NAZE/config.c @@ -61,6 +61,8 @@ void targetConfiguration(void) motorConfigMutable()->minthrottle = 1049; + gyroDeviceConfigMutable()->extiTag = selectMPUIntExtiConfigByHardwareRevision(); + gyroConfigMutable()->gyro_hardware_lpf = GYRO_HARDWARE_LPF_1KHZ_SAMPLE; gyroConfigMutable()->gyro_soft_lpf_hz = 100; gyroConfigMutable()->gyro_soft_notch_hz_1 = 0; @@ -85,8 +87,9 @@ void targetConfiguration(void) pidProfile->pid[PID_LEVEL].P = 30; pidProfile->pid[PID_LEVEL].D = 30; - pidProfile->dtermSetpointWeight = 200; - pidProfile->setpointRelaxRatio = 50; + pidProfile->pid[PID_PITCH].F = 200; + pidProfile->pid[PID_ROLL].F = 200; + pidProfile->feedForwardTransition = 50; } for (uint8_t rateProfileIndex = 0; rateProfileIndex < CONTROL_RATE_PROFILE_COUNT; rateProfileIndex++) { diff --git a/src/main/target/NAZE/initialisation.c b/src/main/target/NAZE/initialisation.c index 359363dd48..feeeb3f8fc 100644 --- a/src/main/target/NAZE/initialisation.c +++ b/src/main/target/NAZE/initialisation.c @@ -31,7 +31,7 @@ #include "pg/bus_spi.h" -extern void spiPreInit(void); // XXX In fc/fc_init.c +extern void spiPreInit(void); // XXX In fc/init.c void targetBusInit(void) { diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index d583e1d16f..6de8191c17 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -58,7 +58,8 @@ #define USE_EXTI #define MAG_INT_EXTI PC14 -#define MPU_INT_EXTI PC13 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC13 #define MMA8451_INT_PIN PA5 #define USE_MPU_DATA_READY_SIGNAL @@ -74,8 +75,8 @@ #define FLASH_CS_PIN NAZE_SPI_CS_PIN #define FLASH_SPI_INSTANCE NAZE_SPI_INSTANCE -#define MPU6500_CS_PIN NAZE_SPI_CS_PIN -#define MPU6500_SPI_INSTANCE NAZE_SPI_INSTANCE +#define GYRO_1_CS_PIN NAZE_SPI_CS_PIN +#define GYRO_1_SPI_INSTANCE NAZE_SPI_INSTANCE #define USE_FLASHFS #define USE_FLASH_M25P16 @@ -86,9 +87,7 @@ #define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU3050_ALIGN CW0_DEG -#define GYRO_MPU6050_ALIGN CW0_DEG -#define GYRO_MPU6500_ALIGN CW0_DEG +#define GYRO_1_ALIGN CW0_DEG #define USE_ACC //#define USE_ACC_ADXL345 @@ -99,10 +98,9 @@ #define USE_ACC_SPI_MPU6500 //#define ACC_ADXL345_ALIGN CW270_DEG -#define ACC_MPU6050_ALIGN CW0_DEG //#define ACC_MMA8452_ALIGN CW90_DEG //#define ACC_BMA280_ALIGN CW0_DEG -#define ACC_MPU6500_ALIGN CW0_DEG +#define ACC_1_ALIGN CW0_DEG #define USE_BARO #define USE_BARO_MS5611 // needed for Flip32 board diff --git a/src/main/target/NAZE/target.mk b/src/main/target/NAZE/target.mk index bbe55d1897..96cd0814a6 100644 --- a/src/main/target/NAZE/target.mk +++ b/src/main/target/NAZE/target.mk @@ -2,10 +2,6 @@ F1_TARGETS += $(TARGET) FEATURES = ONBOARDFLASH TARGET_SRC = \ - drivers/accgyro_legacy/accgyro_adxl345.c \ - drivers/accgyro_legacy/accgyro_bma280.c \ - drivers/accgyro_legacy/accgyro_l3g4200d.c \ - drivers/accgyro_legacy/accgyro_mma845x.c \ drivers/accgyro/accgyro_mpu.c \ drivers/accgyro/accgyro_mpu3050.c \ drivers/accgyro/accgyro_mpu6050.c \ diff --git a/src/main/target/NERO/target.c b/src/main/target/NERO/target.c index 32b61c4a52..dd57b89c55 100644 --- a/src/main/target/NERO/target.c +++ b/src/main/target/NERO/target.c @@ -27,14 +27,24 @@ #include "drivers/timer.h" #include "drivers/timer_def.h" +// QUAD + LED_STRIP can be handled without DMAR (dshot_burst). +// Anything beyond should use DMAR, as TIM5_CH1 and TIM3_CH4 have +// inevitable DMA collision on D(1,2). +// +// Additional DMA resource info +// ADC1 D(2,4) +// TIM5_UP U(1,0) +// TIM3_UP U(1,2) +// TIM8_UP U(2,1) + const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { DEF_TIM(TIM3, CH2, PC7, TIM_USE_PPM, 0, 0 ), - DEF_TIM(TIM5, CH1, PA0, TIM_USE_MOTOR, 0, 0 ), - DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR, 0, 0 ), - DEF_TIM(TIM5, CH3, PA2, TIM_USE_MOTOR, 0, 0 ), - DEF_TIM(TIM5, CH4, PA3, TIM_USE_MOTOR, 0, 1 ), - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR | TIM_USE_LED, 0, 0 ), - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0 ), - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, 0, 0 ), - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MOTOR, 0, 0 ), + DEF_TIM(TIM5, CH1, PA0, TIM_USE_MOTOR, 0, 0 ), // D(1,2) + DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR, 0, 0 ), // D(1,4) + DEF_TIM(TIM5, CH3, PA2, TIM_USE_MOTOR, 0, 0 ), // D(1,0) + DEF_TIM(TIM5, CH4, PA3, TIM_USE_MOTOR, 0, 0 ), // *D(1,1) D(1,3) + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR | TIM_USE_LED, 0, 0 ), // D(1,7) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0 ), // xD(1,2) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, 0, 0 ), // *D(2,2) D(2,4) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MOTOR, 0, 0 ), // D(2,7) }; diff --git a/src/main/target/NERO/target.h b/src/main/target/NERO/target.h index b2a531de06..fa24527747 100644 --- a/src/main/target/NERO/target.h +++ b/src/main/target/NERO/target.h @@ -37,35 +37,28 @@ // MPU6500 interrupt #define USE_EXTI -#define MPU_INT_EXTI PB2 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PB2 #define USE_MPU_DATA_READY_SIGNAL //#define DEBUG_MPU_DATA_READY_INTERRUPT -#define MPU6500_CS_PIN PC4 -#define MPU6500_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PC4 +#define GYRO_1_SPI_INSTANCE SPI1 #define USE_ACC -#define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW0_DEG +#define ACC_1_ALIGN CW0_DEG #define USE_GYRO -#define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW0_DEG +#define GYRO_1_ALIGN CW0_DEG #define USE_SDCARD - +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED #define SDCARD_DETECT_PIN PD2 #define SDCARD_SPI_INSTANCE SPI3 #define SDCARD_SPI_CS_PIN PA15 - -// SPI2 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init: -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz -// Divide to under 25MHz for normal operation: -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 // 21MHz - //#define SDCARD_DMA_STREAM_TX_FULL DMA1_Stream5 //#define SDCARD_DMA_CHANNEL 0 diff --git a/src/main/target/NERO/target.mk b/src/main/target/NERO/target.mk index e2e1a265d5..3a5c6c7bf3 100644 --- a/src/main/target/NERO/target.mk +++ b/src/main/target/NERO/target.mk @@ -1,5 +1,5 @@ F7X2RE_TARGETS += $(TARGET) -FEATURES += SDCARD VCP +FEATURES += SDCARD_SPI VCP TARGET_SRC = \ drivers/accgyro/accgyro_spi_mpu6500.c \ diff --git a/src/main/target/NOX/target.h b/src/main/target/NOX/target.h index 45612a1324..a962d40b04 100644 --- a/src/main/target/NOX/target.h +++ b/src/main/target/NOX/target.h @@ -41,14 +41,12 @@ #define USE_GYRO_SPI_MPU6500 #define USE_GYRO_SPI_MPU6000 -#define MPU6500_CS_PIN PB12 -#define MPU6500_SPI_INSTANCE SPI2 - -#define MPU6000_CS_PIN PB12 -#define MPU6000_SPI_INSTANCE SPI2 +#define GYRO_1_CS_PIN PB12 +#define GYRO_1_SPI_INSTANCE SPI2 #define USE_EXTI -#define MPU_INT_EXTI PA8 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PA8 #define USE_MPU_DATA_READY_SIGNAL #define USE_BARO @@ -86,7 +84,7 @@ #define SERIAL_PORT_COUNT 5 //VCP, USART1, USART2, SOFTSERIAL1, SOFTSERIAL2 #define USE_ESCSERIAL -#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PPM +#define ESCSERIAL_TIMER_TX_PIN PB10 #define USE_SPI #define USE_SPI_DEVICE_1 diff --git a/src/main/target/NUCLEOF446RE/target.h b/src/main/target/NUCLEOF446RE/target.h index e55ee0763f..716252b903 100644 --- a/src/main/target/NUCLEOF446RE/target.h +++ b/src/main/target/NUCLEOF446RE/target.h @@ -51,15 +51,13 @@ #define USE_ACC_SPI_MPU6500 #define USE_ACC_SPI_MPU9250 -#define MPU9250_CS_PIN PB12 -#define MPU9250_SPI_INSTANCE SPI2 - -#define MPU6500_CS_PIN PB12 -#define MPU6500_SPI_INSTANCE SPI2 +#define GYRO_1_SPI_INSTANCE SPI2 +#define GYRO_1_CS_PIN PB12 #define USE_EXTI -//#define MPU_INT_EXTI PC13 +//#define USE_GYRO_EXTI +//#define GYRO_1_EXTI_PIN PC13 //#define USE_MPU_DATA_READY_SIGNAL //#define ENSURE_MPU_DATA_READY_IS_LOW @@ -76,17 +74,10 @@ #define USE_CMS //#define USE_SDCARD -// //#define SDCARD_SPI_INSTANCE SPI2 //#define SDCARD_SPI_CS_PIN PB12 -//// SPI2 is on the APB1 bus whose clock runs at 36MHz. Divide to under 400kHz for init: -//#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128 -//// Divide to under 25MHz for normal operation: -//#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 2 -// //// Note, this is the same DMA channel as UART1_RX. Luckily we don't use DMA for USART Rx. //#define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 - // Performance logging for SD card operations: // #define AFATFS_USE_INTROSPECTIVE_LOGGING @@ -101,14 +92,12 @@ // Nordic Semiconductor uses 'CSN', STM uses 'NSS' #define RX_CE_PIN PC7 // D9 #define RX_NSS_PIN PB6 // D10 -#define RX_SCK_PIN PA5 // D13 -#define RX_MISO_PIN PA6 // D12 -#define RX_MOSI_PIN PA7 // D11 // NUCLEO has NSS on PB6, rather than the standard PA4 + #define SPI1_NSS_PIN RX_NSS_PIN -#define SPI1_SCK_PIN RX_SCK_PIN -#define SPI1_MISO_PIN RX_MISO_PIN -#define SPI1_MOSI_PIN RX_MOSI_PIN +#define SPI1_SCK_PIN PA5 // D13 +#define SPI1_MISO_PIN PA6 // D12 +#define SPI1_MOSI_PIN PA7 // D11 #define USE_RX_NRF24 #define USE_RX_CX10 @@ -174,5 +163,5 @@ #define TARGET_IO_PORTC (0xffff & ~(BIT(15)|BIT(14)|BIT(13))) #define TARGET_IO_PORTD BIT(2) -#define USABLE_TIMER_CHANNEL_COUNT 12 -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) ) +#define USABLE_TIMER_CHANNEL_COUNT 8 +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(8)) diff --git a/src/main/target/NUCLEOF7/target.c b/src/main/target/NUCLEOF7/target.c index e7298a8338..4026e59c52 100644 --- a/src/main/target/NUCLEOF7/target.c +++ b/src/main/target/NUCLEOF7/target.c @@ -33,9 +33,9 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM | TIM_USE_PPM, 0, 0), // S2_IN DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM, 0, 0), // S3_IN - DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM, 0, 0), // S4_IN + DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM, 0, 1), // S4_IN DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM, 0, 0), // S5_IN - DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0, 0), // S6_IN + DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0, 1), // S6_IN DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, 0, 0), // S10_OUT 1 DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, 0, 1), // S1_OUT 4 diff --git a/src/main/target/NUCLEOF7/target.h b/src/main/target/NUCLEOF7/target.h index a11dbc444a..cf8714bd86 100644 --- a/src/main/target/NUCLEOF7/target.h +++ b/src/main/target/NUCLEOF7/target.h @@ -33,32 +33,37 @@ #define BEEPER_PIN PA0 #define BEEPER_INVERTED +#undef USE_MULTI_GYRO // XXX Drop this if target has I2C configured + #define USE_ACC #define USE_FAKE_ACC #define USE_ACC_MPU6050 -#define ACC_MPU6050_ALIGN CW270_DEG +#define ACC_1_ALIGN CW270_DEG #define USE_GYRO #define USE_FAKE_GYRO #define USE_GYRO_MPU6050 -#define GYRO_MPU6050_ALIGN CW270_DEG +#define GYRO_1_ALIGN CW270_DEG // MPU6050 interrupts #define USE_MPU_DATA_READY_SIGNAL -#define MPU_INT_EXTI PB15 +#define USE_EXTI +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PB15 #define USE_EXTI #define USE_MAG #define USE_FAKE_MAG #define USE_MAG_HMC5883 #define USE_MAG_QMC5883 +#define USE_MAG_LIS3MDL #define MAG_HMC5883_ALIGN CW270_DEG_FLIP #define USE_BARO #define USE_FAKE_BARO #define USE_BARO_MS5611 -#define USABLE_TIMER_CHANNEL_COUNT 16 +#define USABLE_TIMER_CHANNEL_COUNT 11 #define USE_VCP #define USE_USB_DETECT @@ -119,16 +124,11 @@ #define SPI4_MOSI_PIN PE14 #define USE_SDCARD +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED #define SDCARD_DETECT_PIN PF14 - #define SDCARD_SPI_INSTANCE SPI4 #define SDCARD_SPI_CS_PIN SPI4_NSS_PIN - -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 422kHz -// Divide to under 25MHz for normal operation: -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 8 // 27MHz - #define SDCARD_DMA_STREAM_TX_FULL DMA2_Stream1 #define SDCARD_DMA_CHANNEL 4 @@ -141,7 +141,7 @@ #define USE_ADC #define VBAT_ADC_PIN PA3 #define CURRENT_METER_ADC_PIN PC0 -#define RSSI_ADC_GPIO_PIN PC3 +#define RSSI_ADC_PIN PC3 #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT @@ -158,4 +158,4 @@ #define TARGET_IO_PORTF 0xffff #define TARGET_IO_PORTG 0xffff -#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9) | TIM_N(10) | TIM_N(11)) +#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(9) | TIM_N(12) ) diff --git a/src/main/target/NUCLEOF7/target.mk b/src/main/target/NUCLEOF7/target.mk index 3408bae1b0..bc21e51f75 100644 --- a/src/main/target/NUCLEOF7/target.mk +++ b/src/main/target/NUCLEOF7/target.mk @@ -1,5 +1,5 @@ F7X6XG_TARGETS += $(TARGET) -FEATURES += SDCARD VCP +FEATURES += SDCARD_SPI VCP TARGET_SRC = \ drivers/accgyro/accgyro_fake.c \ @@ -8,4 +8,5 @@ TARGET_SRC = \ drivers/barometer/barometer_ms5611.c \ drivers/compass/compass_fake.c \ drivers/compass/compass_hmc5883l.c \ - drivers/compass/compass_qmc5883l.c + drivers/compass/compass_qmc5883l.c \ + drivers/compass/compass_lis3mdl.c diff --git a/src/main/target/NUCLEOF722/target.c b/src/main/target/NUCLEOF722/target.c index 76f149db8d..bfe078fffd 100644 --- a/src/main/target/NUCLEOF722/target.c +++ b/src/main/target/NUCLEOF722/target.c @@ -31,9 +31,9 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM | TIM_USE_PPM, 0, 0), DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM, 0, 0), - DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM, 0, 0), - DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM, 0, 0), - DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0, 0), + DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM, 0, 1), +// DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM, 0, 0), // Used for SDIO +// DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0, 1), // Used for SDIO DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, 0, 0), DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, 0, 1), diff --git a/src/main/target/NUCLEOF722/target.h b/src/main/target/NUCLEOF722/target.h index b027300b2d..3476118737 100644 --- a/src/main/target/NUCLEOF722/target.h +++ b/src/main/target/NUCLEOF722/target.h @@ -29,36 +29,41 @@ #define LED0_PIN PB7 // blue #define LED1_PIN PB14 // red -#define USE_BEEPER -#define BEEPER_PIN PA0 -#define BEEPER_INVERTED +//#define USE_BEEPER +//#define BEEPER_PIN PA0 +//#define BEEPER_INVERTED + +#undef USE_MULTI_GYRO #define USE_ACC #define USE_FAKE_ACC #define USE_ACC_MPU6050 -#define ACC_MPU6050_ALIGN CW270_DEG +#define ACC_1_ALIGN CW270_DEG #define USE_GYRO #define USE_FAKE_GYRO #define USE_GYRO_MPU6050 -#define GYRO_MPU6050_ALIGN CW270_DEG +#define GYRO_1_ALIGN CW270_DEG // MPU6050 interrupts #define USE_MPU_DATA_READY_SIGNAL -#define MPU_INT_EXTI PB15 +#define USE_EXTI +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PB15 #define USE_EXTI #define USE_MAG #define USE_FAKE_MAG #define USE_MAG_HMC5883 #define USE_MAG_QMC5883 +#define USE_MAG_LIS3MDL #define MAG_HMC5883_ALIGN CW270_DEG_FLIP #define USE_BARO #define USE_FAKE_BARO #define USE_BARO_MS5611 -#define USABLE_TIMER_CHANNEL_COUNT 11 +#define USABLE_TIMER_CHANNEL_COUNT 9 #define USE_VCP #define USE_USB_DETECT @@ -77,12 +82,14 @@ #define UART3_TX_PIN PD8 #define USE_UART4 -#define UART4_RX_PIN PC11 -#define UART4_TX_PIN PC10 +//#define UART4_RX_PIN PC11 // Used for SDIO +//#define UART4_TX_PIN PC10 // Used for SDIO +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 //#define USE_UART5 -#define UART5_RX_PIN PD2 -#define UART5_TX_PIN PC12 +//#define UART5_RX_PIN PD2 // Used for SDIO +//#define UART5_TX_PIN PC12 // Used for SDIO //#define USE_UART6 #define UART6_RX_PIN PC7 @@ -99,7 +106,7 @@ #define USE_SOFTSERIAL1 #define USE_SOFTSERIAL2 -#define SERIAL_PORT_COUNT 6 //VCP, USART2, USART3, UART4,SOFTSERIAL x 2 +#define SERIAL_PORT_COUNT 6 //VCP, USART2, USART3, UART4, SOFTSERIAL x 2 #define USE_ESCSERIAL #define ESCSERIAL_TIMER_TX_PIN PB15 // (Hardware=0, PPM) @@ -118,19 +125,17 @@ #define SPI4_MISO_PIN PE13 #define SPI4_MOSI_PIN PE14 -//#define USE_SDCARD -#define SDCARD_DETECT_INVERTED -#define SDCARD_DETECT_PIN PF14 +#define USE_SDCARD +//#define SDCARD_DETECT_INVERTED +//#define SDCARD_DETECT_PIN PF14 +//#define SDCARD_SPI_INSTANCE SPI4 +//#define SDCARD_SPI_CS_PIN SPI4_NSS_PIN +//#define SDCARD_DMA_STREAM_TX_FULL DMA2_Stream1 +//#define SDCARD_DMA_CHANNEL 4 +#define USE_SDCARD_SDIO -#define SDCARD_SPI_INSTANCE SPI4 -#define SDCARD_SPI_CS_PIN SPI4_NSS_PIN - -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 422kHz -// Divide to under 25MHz for normal operation: -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 8 // 27MHz - -#define SDCARD_DMA_STREAM_TX_FULL DMA2_Stream1 -#define SDCARD_DMA_CHANNEL 4 +#define SDIO_DMA DMA2_Stream3 +#define SDCARD_SPI_CS_PIN NONE //This is not used on SDIO, has to be kept for now to keep compiler happy #define USE_I2C #define USE_I2C_DEVICE_1 @@ -141,7 +146,7 @@ #define USE_ADC #define VBAT_ADC_PIN PA3 #define CURRENT_METER_ADC_PIN PC0 -#define RSSI_ADC_GPIO_PIN PC3 +#define RSSI_ADC_PIN PC3 //#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT @@ -157,4 +162,4 @@ #define TARGET_IO_PORTE 0xffff #define TARGET_IO_PORTF 0xffff -#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9) | TIM_N(10) | TIM_N(11)) +#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(9) | TIM_N(12) ) diff --git a/src/main/target/NUCLEOF722/target.mk b/src/main/target/NUCLEOF722/target.mk index 0e8eb220bc..9beac9f7c9 100644 --- a/src/main/target/NUCLEOF722/target.mk +++ b/src/main/target/NUCLEOF722/target.mk @@ -1,5 +1,5 @@ F7X2RE_TARGETS += $(TARGET) -FEATURES += SDCARD VCP +FEATURES += VCP SDCARD_SDIO TARGET_SRC = \ drivers/accgyro/accgyro_fake.c \ @@ -9,5 +9,6 @@ TARGET_SRC = \ drivers/compass/compass_fake.c \ drivers/compass/compass_hmc5883l.c \ drivers/compass/compass_qmc5883l.c \ + drivers/compass/compass_lis3mdl.c \ drivers/light_ws2811strip.c \ drivers/light_ws2811strip_hal.c diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index f2b9aadde0..6dc2a8b89d 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -26,10 +26,19 @@ #undef USE_GYRO_OVERFLOW_CHECK #undef USE_GYRO_LPF2 +//#undef USE_ITERM_RELAX +#undef USE_RC_SMOOTHING_FILTER + +#undef USE_HUFFMAN +#undef USE_PINIO +#undef USE_PINIOBOX + #undef USE_TELEMETRY_HOTT #undef USE_TELEMETRY_MAVLINK #undef USE_TELEMETRY_LTM #undef USE_SERIALRX_XBUS +#undef USE_SERIALRX_SUMH +#undef USE_PWM #undef USE_BOARD_INFO #undef USE_EXTENDED_CMS_MENUS @@ -37,10 +46,8 @@ #undef USE_RX_MSP #undef USE_ESC_SENSOR_INFO - #define TARGET_BOARD_IDENTIFIER "OMNI" // https://en.wikipedia.org/wiki/Omnibus -#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE #define LED0_PIN PB3 @@ -49,19 +56,20 @@ #define BEEPER_INVERTED #define USE_EXTI -#define MPU_INT_EXTI PC13 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC13 #define USE_MPU_DATA_READY_SIGNAL -#define MPU6000_SPI_INSTANCE SPI1 -#define MPU6000_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PA4 #define USE_GYRO #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW90_DEG +#define GYRO_1_ALIGN CW90_DEG #define USE_ACC #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW90_DEG +#define ACC_1_ALIGN CW90_DEG #define BMP280_SPI_INSTANCE SPI1 #define BMP280_CS_PIN PA13 @@ -134,19 +142,13 @@ #define SPI2_MOSI_PIN PB15 #define USE_SDCARD - +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED #define SDCARD_DETECT_PIN PC14 - #define SDCARD_SPI_INSTANCE SPI2 #define SDCARD_SPI_CS_PIN SPI2_NSS_PIN - -// SPI2 is on the APB1 bus whose clock runs at 36MHz. Divide to under 400kHz for init: -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128 -// Divide to under 25MHz for normal operation: -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 2 - // DSHOT output 4 uses DMA1_Channel5, so don't use it for the SDCARD until we find an alternative + #ifndef USE_DSHOT #define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 #endif diff --git a/src/main/target/OMNIBUS/target.mk b/src/main/target/OMNIBUS/target.mk index 68a4060c79..109f4a4c71 100644 --- a/src/main/target/OMNIBUS/target.mk +++ b/src/main/target/OMNIBUS/target.mk @@ -1,5 +1,5 @@ F3_TARGETS += $(TARGET) -FEATURES = VCP SDCARD +FEATURES = VCP SDCARD_SPI TARGET_SRC = \ drivers/accgyro/accgyro_mpu.c \ diff --git a/src/main/target/OMNIBUSF4/EXUAVF4PRO.mk b/src/main/target/OMNIBUSF4/EXUAVF4PRO.mk index 592a073315..a168c034b9 100644 --- a/src/main/target/OMNIBUSF4/EXUAVF4PRO.mk +++ b/src/main/target/OMNIBUSF4/EXUAVF4PRO.mk @@ -1,2 +1,2 @@ #EXUAVF4PRO -FEATURES = VCP SDCARD \ No newline at end of file +FEATURES = VCP SDCARD_SPI \ No newline at end of file diff --git a/src/main/target/OMNIBUSF4/OMNIBUSF4SD.mk b/src/main/target/OMNIBUSF4/OMNIBUSF4SD.mk index 2f6492da96..58c0f73f15 100644 --- a/src/main/target/OMNIBUSF4/OMNIBUSF4SD.mk +++ b/src/main/target/OMNIBUSF4/OMNIBUSF4SD.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 +FEATURES = VCP SDCARD_SPI diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index daea8a23a7..f39a31a834 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -71,46 +71,54 @@ #define INVERTER_PIN_UART1 PC0 // DYS F4 Pro; Omnibus F4 AIO (1st gen) have a FIXED inverter on UART1 #endif +#define USE_EXTI + +#define USE_MULTI_GYRO + #define USE_ACC #define USE_ACC_SPI_MPU6000 #define USE_GYRO #define USE_GYRO_SPI_MPU6000 -#define MPU6000_CS_PIN PA4 -#define MPU6000_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 // MPU6000 interrupts -#define USE_EXTI -#define MPU_INT_EXTI PC4 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC4 #define USE_MPU_DATA_READY_SIGNAL #if defined(OMNIBUSF4SD) -#define GYRO_MPU6000_ALIGN CW270_DEG -#define ACC_MPU6000_ALIGN CW270_DEG +#define GYRO_1_ALIGN CW270_DEG +#define ACC_1_ALIGN CW270_DEG #elif defined(XRACERF4) || defined(EXUAVF4PRO) -#define GYRO_MPU6000_ALIGN CW90_DEG -#define ACC_MPU6000_ALIGN CW90_DEG +#define GYRO_1_ALIGN CW90_DEG +#define ACC_1_ALIGN CW90_DEG #else -#define GYRO_MPU6000_ALIGN CW180_DEG -#define ACC_MPU6000_ALIGN CW180_DEG +#define GYRO_1_ALIGN CW180_DEG +#define ACC_1_ALIGN CW180_DEG #endif // Support for iFlight OMNIBUS F4 V3 // Has ICM20608 instead of MPU6000 // OMNIBUSF4SD is linked with both MPU6000 and MPU6500 drivers -#if defined (OMNIBUSF4SD) +#if defined (OMNIBUSF4SD) || defined(OMNIBUSF4BASE) #define USE_ACC_SPI_MPU6500 #define USE_GYRO_SPI_MPU6500 -#define MPU6500_CS_PIN MPU6000_CS_PIN -#define MPU6500_SPI_INSTANCE MPU6000_SPI_INSTANCE -#define GYRO_MPU6500_ALIGN GYRO_MPU6000_ALIGN -#define ACC_MPU6500_ALIGN ACC_MPU6000_ALIGN #endif +// Dummy defines +#define GYRO_2_SPI_INSTANCE GYRO_1_SPI_INSTANCE +#define GYRO_2_CS_PIN NONE +#define GYRO_2_ALIGN ALIGN_DEFAULT +#define GYRO_2_EXTI_PIN NONE +#define ACC_2_ALIGN ALIGN_DEFAULT + #define USE_MAG #define USE_MAG_HMC5883 #define USE_MAG_QMC5883 +#define USE_MAG_LIS3MDL #define MAG_HMC5883_ALIGN CW90_DEG #define USE_BARO @@ -144,15 +152,11 @@ #if defined(OMNIBUSF4SD) #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define USE_SDCARD +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED #define SDCARD_DETECT_PIN PB7 #define SDCARD_SPI_INSTANCE SPI2 #define SDCARD_SPI_CS_PIN SPI2_NSS_PIN -// SPI2 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init: -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz -// Divide to under 25MHz for normal operation: -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 // 21MHz - #define SDCARD_DMA_CHANNEL_TX DMA1_Stream4 #define SDCARD_DMA_CHANNEL 0 @@ -238,7 +242,7 @@ #define USE_I2C_DEVICE_2 #define I2C2_SCL NONE // PB10, shared with UART3TX #define I2C2_SDA NONE // PB11, shared with UART3RX -#if defined(OMNIBUSF4) || defined(OMNIBUSF4SD) +#if defined(OMNIBUSF4BASE) || defined(OMNIBUSF4SD) #define USE_I2C_DEVICE_3 #define I2C3_SCL NONE // PA8, PWM6 #define I2C3_SDA NONE // PC9, CH6 @@ -282,5 +286,5 @@ #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(10) | TIM_N(12) | TIM_N(8) | TIM_N(9)) #else #define USABLE_TIMER_CHANNEL_COUNT 14 -#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9)) +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(8) | TIM_N(12) ) #endif diff --git a/src/main/target/OMNIBUSF4/target.mk b/src/main/target/OMNIBUSF4/target.mk index 89f1781ca3..b14e5964f5 100644 --- a/src/main/target/OMNIBUSF4/target.mk +++ b/src/main/target/OMNIBUSF4/target.mk @@ -9,5 +9,6 @@ TARGET_SRC = \ drivers/barometer/barometer_ms5611.c \ drivers/compass/compass_hmc5883l.c \ drivers/compass/compass_qmc5883l.c \ + drivers/compass/compass_lis3mdl.c \ drivers/max7456.c diff --git a/src/main/target/OMNIBUSF4FW/target.c b/src/main/target/OMNIBUSF4FW/target.c index 7cb995eed1..9bd335ae73 100644 --- a/src/main/target/OMNIBUSF4FW/target.c +++ b/src/main/target/OMNIBUSF4FW/target.c @@ -29,29 +29,29 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // Motors - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0), // D(1,7) U(1,2) - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0), // D(1,2) U(1,2) - DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, 0, 1), // D(1,6) U(1,7) + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0), // D(1,7) U(1,2) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0), // D(1,2) U(1,2) + DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, 0, 1), // D(1,6) U(1,7) #if defined(OMNIBUSF4FW) - DEF_TIM(TIM3, CH2, PB5, TIM_USE_MOTOR, 0, 0), // D(1,5) U(1,2) + DEF_TIM(TIM3, CH2, PB5, TIM_USE_MOTOR, 0, 0), // D(1,5) U(1,2) #elif defined(OMNIBUSF4FW1) - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, 0, 0), + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, 0, 0), #endif // Other functions - DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), // D(1,0) - DEF_TIM(TIM10, CH1, PB8, TIM_USE_PPM, 0, 0), // PPM - DEF_TIM(TIM11, CH1, PB9, TIM_USE_NONE, 0, 0), // CAM_CTL + DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), // D(1,0) + DEF_TIM(TIM10, CH1, PB8, TIM_USE_PPM, 0, 0), // PPM + DEF_TIM(TIM11, CH1, PB9, TIM_USE_CAMERA_CONTROL, 0, 0), // CAM_CTL // Spare pins and backdoor timer - DEF_TIM(TIM8, CH1, PC6, TIM_USE_NONE, 0, 0), // UART6_TX - DEF_TIM(TIM8, CH2, PC7, TIM_USE_NONE, 0, 0), // UART6_RX - DEF_TIM(TIM8, CH3, PC8, TIM_USE_NONE, 0, 0), // SJ1 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_NONE, 0, 0), // SJ2 + DEF_TIM(TIM8, CH1, PC6, TIM_USE_NONE, 0, 0), // UART6_TX + DEF_TIM(TIM8, CH2, PC7, TIM_USE_NONE, 0, 0), // UART6_RX + DEF_TIM(TIM8, CH3, PC8, TIM_USE_NONE, 0, 0), // SJ1 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_NONE, 0, 0), // SJ2 // Backdoor timers on UARTs - DEF_TIM(TIM1, CH2, PA9, TIM_USE_NONE, 0, 0), // UART1_TX - DEF_TIM(TIM1, CH3, PA10, TIM_USE_NONE, 0, 0), // UART1_RX - DEF_TIM(TIM5, CH2, PA1, TIM_USE_NONE, 0, 0), // UART4_RX - DEF_TIM(TIM9, CH1, PA2, TIM_USE_NONE, 0, 0), // UART2_TX + DEF_TIM(TIM1, CH2, PA9, TIM_USE_NONE, 0, 0), // UART1_TX + DEF_TIM(TIM1, CH3, PA10, TIM_USE_NONE, 0, 0), // UART1_RX + DEF_TIM(TIM5, CH2, PA1, TIM_USE_NONE, 0, 0), // UART4_RX + DEF_TIM(TIM9, CH1, PA2, TIM_USE_NONE, 0, 0), // UART2_TX }; diff --git a/src/main/target/OMNIBUSF4FW/target.h b/src/main/target/OMNIBUSF4FW/target.h index da7f86d683..646108c520 100644 --- a/src/main/target/OMNIBUSF4FW/target.h +++ b/src/main/target/OMNIBUSF4FW/target.h @@ -68,11 +68,14 @@ // MPU6000 interrupts #define USE_EXTI -#define MPU_INT_EXTI PC4 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC4 +#define GYRO_2_EXTI_PIN NONE #define USE_MPU_DATA_READY_SIGNAL #define USE_MAG #define USE_MAG_HMC5883 +#define USE_MAG_LIS3MDL #define MAG_HMC5883_ALIGN CW90_DEG #define USE_BARO @@ -81,7 +84,6 @@ #define BMP280_CS_PIN PB3 #define DEFAULT_BARO_SPI_BMP280 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_INSTANCE SPI3 #define MAX7456_SPI_CS_PIN PA15 @@ -151,8 +153,6 @@ #define I2C2_SDA NONE // PB11, shared with UART3RX #define I2C_DEVICE (I2CDEV_2) -#define CAMERA_CONTROL_PIN PB9 - #define USE_ADC #define ADC_INSTANCE ADC2 #define CURRENT_METER_ADC_PIN PC1 // Direct from CRNT pad (part of onboard sensor for Pro) @@ -178,8 +178,12 @@ #define TARGET_IO_PORTA (0xffff & ~(BIT(14)|BIT(13))) #define TARGET_IO_PORTB (0xffff & ~(BIT(2))) -#define TARGET_IO_PORTC (0xffff & ~(BIT(15)|BIT(14)|BIT(13))) +#define TARGET_IO_PORTC (0xffff & ~(BIT(15))) #define TARGET_IO_PORTD BIT(2) +#if defined(OMNIBUSF4FW) || defined(OMNIBUSF4FW1) #define USABLE_TIMER_CHANNEL_COUNT 15 +#else +#define USABLE_TIMER_CHANNEL_COUNT 14 +#endif #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(8) | TIM_N(9) | TIM_N(10) | TIM_N(11)) diff --git a/src/main/target/OMNIBUSF4FW/target.mk b/src/main/target/OMNIBUSF4FW/target.mk index 21caf6eb1d..111fbddee2 100644 --- a/src/main/target/OMNIBUSF4FW/target.mk +++ b/src/main/target/OMNIBUSF4FW/target.mk @@ -9,4 +9,5 @@ TARGET_SRC = \ drivers/barometer/barometer_bmp280.c \ drivers/barometer/barometer_ms5611.c \ drivers/compass/compass_hmc5883l.c \ + drivers/compass/compass_lis3mdl.c \ drivers/max7456.c diff --git a/src/main/target/OMNIBUSF7/config.c b/src/main/target/OMNIBUSF7/config.c index 2c0d96708e..aabeb9cae5 100644 --- a/src/main/target/OMNIBUSF7/config.c +++ b/src/main/target/OMNIBUSF7/config.c @@ -29,9 +29,14 @@ #include "io/serial.h" +#ifdef FPVM_BETAFLIGHTF7 +#define ESC_SENSOR_UART SERIAL_PORT_USART1 +#elif defined(OMNIBUSF7V2) +#define ESC_SENSOR_UART SERIAL_PORT_USART7 +#endif + static targetSerialPortFunction_t targetSerialPortFunction[] = { -#if defined(OMNIBUSF7V2) && defined(ESC_SENSOR_UART) - // OMNIBUS F7 V2 has an option to connect UART7_RX to ESC telemetry +#ifdef ESC_SENSOR_UART { ESC_SENSOR_UART, FUNCTION_ESC_SENSOR }, #else { SERIAL_PORT_NONE, FUNCTION_NONE }, diff --git a/src/main/target/OMNIBUSF7/target.c b/src/main/target/OMNIBUSF7/target.c index 0c1d8b6646..642eb73c11 100644 --- a/src/main/target/OMNIBUSF7/target.c +++ b/src/main/target/OMNIBUSF7/target.c @@ -30,25 +30,24 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { #ifdef FPVM_BETAFLIGHTF7 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_ANY, 0, 0), // USED FOR CAMERA CONTROL + DEF_TIM(TIM8, CH3, PC8, TIM_USE_CAMERA_CONTROL, 0, 1), // USED FOR CAMERA CONTROL #endif - DEF_TIM(TIM1, CH3, PE13, TIM_USE_NONE, 0, 1 ), // RC1 / PPM, unusable + DEF_TIM(TIM1, CH3, PE13, TIM_USE_NONE, 0, 1 ), // RC1 / PPM, unusable - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0 ), // M1 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0 ), // M2 - DEF_TIM(TIM1, CH1, PE9, TIM_USE_MOTOR, 0, 2 ), // M3 - DEF_TIM(TIM1, CH2, PE11, TIM_USE_MOTOR, 0, 1 ), // M4 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0 ), // M1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0 ), // M2 + DEF_TIM(TIM1, CH1, PE9, TIM_USE_MOTOR, 0, 2 ), // M3 + DEF_TIM(TIM1, CH2, PE11, TIM_USE_MOTOR, 0, 1 ), // M4 - DEF_TIM(TIM4, CH1, PD12, TIM_USE_LED, 0, 0 ), // LED + DEF_TIM(TIM4, CH1, PD12, TIM_USE_LED, 0, 0 ), // LED // Backdoor timers - DEF_TIM(TIM2, CH3, PB10, TIM_USE_NONE, 0, 0 ), // UART3_TX, I2C2_SCL - DEF_TIM(TIM2, CH4, PB11, TIM_USE_NONE, 0, 0 ), // UART3_RX, I2C2_SDA - DEF_TIM(TIM8, CH1, PC6, TIM_USE_NONE, 0, 0 ), // UART6_TX - DEF_TIM(TIM8, CH2, PC7, TIM_USE_NONE, 0, 0 ), // UART6_RX - DEF_TIM(TIM2, CH4, PA3, TIM_USE_PPM, 0, 0 ), // UART2_RX, joined with PE13 + DEF_TIM(TIM2, CH3, PB10, TIM_USE_NONE, 0, 0 ), // UART3_TX, I2C2_SCL + DEF_TIM(TIM2, CH4, PB11, TIM_USE_NONE, 0, 0 ), // UART3_RX, I2C2_SDA + DEF_TIM(TIM8, CH1, PC6, TIM_USE_NONE, 0, 0 ), // UART6_TX + DEF_TIM(TIM8, CH2, PC7, TIM_USE_NONE, 0, 1 ), // UART6_RX + DEF_TIM(TIM2, CH4, PA3, TIM_USE_PPM, 0, 0 ), // UART2_RX, joined with PE13 // For ESC serial - DEF_TIM(TIM9, CH1, PA2, TIM_USE_NONE, 0, 0 ), // UART2_TX (unwired) - + DEF_TIM(TIM9, CH1, PA2, TIM_USE_NONE, 0, 0 ), // UART2_TX (unwired) }; diff --git a/src/main/target/OMNIBUSF7/target.h b/src/main/target/OMNIBUSF7/target.h index 71a88e6f65..928c90c6df 100644 --- a/src/main/target/OMNIBUSF7/target.h +++ b/src/main/target/OMNIBUSF7/target.h @@ -40,83 +40,59 @@ #define BEEPER_PIN PD15 #define BEEPER_INVERTED -//CAMERA CONTROL---------------------------- -#ifdef FPVM_BETAFLIGHTF7 -//define camera control -#define CAMERA_CONTROL_PIN PC8 // Camera control. -#endif - //GYRO & ACC-------------------------------- #define USE_ACC #define USE_GYRO #define USE_DUAL_GYRO // ICM-20608-G -#define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 -//#define MPU_INT_EXTI PE8 // MPU6000 -#define USE_ACC_MPU6000 #define USE_ACC_SPI_MPU6000 -#define USE_GYRO_MPU6000 #define USE_GYRO_SPI_MPU6000 -//#define MPU_INT_EXTI PD0 -#if defined(OMNIBUSF7V2) -#define MPU6000_CS_PIN SPI1_NSS_PIN -#define MPU6000_SPI_INSTANCE SPI1 -#define MPU6500_CS_PIN SPI3_NSS_PIN -#define MPU6500_SPI_INSTANCE SPI3 -#define GYRO_1_CS_PIN MPU6500_CS_PIN -#define GYRO_2_CS_PIN MPU6000_CS_PIN -#define GYRO_MPU6500_ALIGN CW90_DEG -#define ACC_MPU6500_ALIGN CW90_DEG -#define GYRO_MPU6000_ALIGN ALIGN_DEFAULT -#define ACC_MPU6000_ALIGN ALIGN_DEFAULT -#define ACC_1_ALIGN ACC_MPU6500_ALIGN -#define ACC_2_ALIGN ACC_MPU6000_ALIGN -#define GYRO_1_ALIGN GYRO_MPU6500_ALIGN -#define GYRO_2_ALIGN GYRO_MPU6000_ALIGN -#define GYRO_1_SPI_INSTANCE MPU6500_SPI_INSTANCE -#define GYRO_2_SPI_INSTANCE MPU6000_SPI_INSTANCE -#elif defined(FPVM_BETAFLIGHTF7) -#define MPU6000_CS_PIN SPI1_NSS_PIN -#define MPU6000_SPI_INSTANCE SPI1 -#define MPU6500_CS_PIN SPI3_NSS_PIN -#define MPU6500_SPI_INSTANCE SPI3 -#define GYRO_1_CS_PIN MPU6000_CS_PIN -#define GYRO_2_CS_PIN MPU6500_CS_PIN -#define GYRO_MPU6500_ALIGN CW270_DEG -#define ACC_MPU6500_ALIGN CW270_DEG -#define GYRO_MPU6000_ALIGN CW90_DEG -#define ACC_MPU6000_ALIGN CW90_DEG -#define ACC_1_ALIGN ACC_MPU6000_ALIGN -#define ACC_2_ALIGN ACC_MPU6500_ALIGN -#define GYRO_1_ALIGN GYRO_MPU6000_ALIGN -#define GYRO_2_ALIGN GYRO_MPU6500_ALIGN -#define GYRO_1_SPI_INSTANCE MPU6000_SPI_INSTANCE -#define GYRO_2_SPI_INSTANCE MPU6500_SPI_INSTANCE -#else -#define MPU6000_CS_PIN SPI3_NSS_PIN -#define MPU6000_SPI_INSTANCE SPI3 -#define MPU6500_CS_PIN SPI1_NSS_PIN -#define MPU6500_SPI_INSTANCE SPI1 -#define GYRO_1_CS_PIN MPU6000_CS_PIN -#define GYRO_2_CS_PIN MPU6500_CS_PIN -#define ACC_1_ALIGN ALIGN_DEFAULT -#define ACC_2_ALIGN ALIGN_DEFAULT -#define GYRO_1_ALIGN ALIGN_DEFAULT -#define GYRO_2_ALIGN ALIGN_DEFAULT -#define GYRO_1_SPI_INSTANCE MPU6000_SPI_INSTANCE -#define GYRO_2_SPI_INSTANCE MPU6500_SPI_INSTANCE -#endif - -// TODO: dual gyro support //#define USE_MPU_DATA_READY_SIGNAL #define USE_EXTI +#define USE_GYRO_EXTI + +#if defined(OMNIBUSF7V2) +#define GYRO_1_SPI_INSTANCE SPI3 +#define GYRO_1_CS_PIN PA15 +#define GYRO_2_SPI_INSTANCE SPI1 +#define GYRO_2_CS_PIN PA4 +#define GYRO_1_ALIGN CW90_DEG +#define GYRO_2_ALIGN ALIGN_DEFAULT +#define ACC_1_ALIGN CW90_DEG +#define ACC_2_ALIGN ALIGN_DEFAULT +#define GYRO_1_EXTI_PIN PD0 // MPU6000 +#define GYRO_2_EXTI_PIN PE8 // ICM20608 + +#elif defined(FPVM_BETAFLIGHTF7) +#define GYRO_1_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PA4 +#define GYRO_2_SPI_INSTANCE SPI3 +#define GYRO_2_CS_PIN PA15 +#define GYRO_1_ALIGN CW90_DEG +#define ACC_1_ALIGN CW90_DEG +#define GYRO_2_ALIGN CW270_DEG +#define ACC_2_ALIGN CW270_DEG +#define GYRO_1_EXTI_PIN PD0 // Assume the same as OMNIBUSF7V2, need to verify +#define GYRO_2_EXTI_PIN PE8 // Ditto + +#else +#define GYRO_1_SPI_INSTANCE SPI3 +#define GYRO_1_CS_PIN PA15 +#define GYRO_2_SPI_INSTANCE SPI1 +#define GYRO_2_CS_PIN PA4 +#define GYRO_1_ALIGN ALIGN_DEFAULT +#define ACC_1_ALIGN ALIGN_DEFAULT +#define GYRO_2_ALIGN ALIGN_DEFAULT +#define ACC_2_ALIGN ALIGN_DEFAULT +#define GYRO_1_EXTI_PIN PE8 // ICM20608 +#define GYRO_2_EXTI_PIN PD0 // MPU6000 +#endif //UARTS------------------------------------- #define USE_VCP @@ -208,16 +184,11 @@ //SD----------------------------------------- #define USE_SDCARD +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED #define SDCARD_DETECT_PIN PE3 - #define SDCARD_SPI_INSTANCE SPI4 #define SDCARD_SPI_CS_PIN SPI4_NSS_PIN - -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 422kHz -// Divide to under 25MHz for normal operation: -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 8 // 27MHz - #define SDCARD_DMA_STREAM_TX_FULL DMA2_Stream1 #define SDCARD_DMA_CHANNEL 4 @@ -240,8 +211,8 @@ #define USE_MAG #define USE_MAG_HMC5883 #define USE_MAG_QMC5883 +#define USE_MAG_LIS3MDL -#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO) //ADC--------------------------------------- #define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC #define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC @@ -260,12 +231,10 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT #define SERIALRX_UART SERIAL_PORT_USART6 #define SERIALRX_PROVIDER SERIALRX_SBUS -#define ESC_SENSOR_UART SERIAL_PORT_USART1 #else #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define SERIALRX_UART SERIAL_PORT_USART2 #define SERIALRX_PROVIDER SERIALRX_SBUS -#define ESC_SENSOR_UART SERIAL_PORT_USART7 #endif #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/OMNIBUSF7/target.mk b/src/main/target/OMNIBUSF7/target.mk index dc42481aa1..5285a9379f 100644 --- a/src/main/target/OMNIBUSF7/target.mk +++ b/src/main/target/OMNIBUSF7/target.mk @@ -2,7 +2,7 @@ F7X5XG_TARGETS += $(TARGET) ifeq ($(TARGET), FPVM_BETAFLIGHTF7) FEATURES = VCP ONBOARDFLASH else -FEATURES = VCP SDCARD +FEATURES = VCP SDCARD_SPI endif TARGET_SRC = \ @@ -14,6 +14,7 @@ TARGET_SRC = \ drivers/barometer/barometer_ms5611.c \ drivers/compass/compass_hmc5883l.c \ drivers/compass/compass_qmc5883l.c \ + drivers/compass/compass_lis3mdl.c \ drivers/light_ws2811strip.c \ drivers/light_ws2811strip_hal.c \ drivers/max7456.c diff --git a/src/main/target/OMNINXT/hardware_revision.c b/src/main/target/OMNINXT/hardware_revision.c index ff4ff18ae1..860c98a0f4 100644 --- a/src/main/target/OMNINXT/hardware_revision.c +++ b/src/main/target/OMNINXT/hardware_revision.c @@ -296,6 +296,7 @@ void updateHardwareRevision(void) // Empty } +// XXX Can be gone as sensors/gyro.c is not calling this anymore ioTag_t selectMPUIntExtiConfigByHardwareRevision(void) { return IO_TAG_NONE; diff --git a/src/main/target/OMNINXT/target.c b/src/main/target/OMNINXT/target.c index fcb0149a7d..2888557690 100644 --- a/src/main/target/OMNINXT/target.c +++ b/src/main/target/OMNINXT/target.c @@ -36,7 +36,7 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // Additional motors/servos DEF_TIM(TIM8, CH4, PC9, TIM_USE_NONE, 0, 0), // MST5 Collision with TX/RX6 (useful for OCTO) - DEF_TIM(TIM8, CH3, PC8, TIM_USE_NONE, 0, 0), // MST6 Collision with TX/RX6 (useful for OCTO) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_NONE, 0, 1), // MST6 Collision with TX/RX6 (useful for OCTO) DEF_TIM(TIM11, CH1, PB9, TIM_USE_NONE, 0, 0), // I2C1_SDA, MST7 DEF_TIM(TIM10, CH1, PB8, TIM_USE_NONE, 0, 0), // I2C1_SCL, MST8 @@ -47,7 +47,7 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { DEF_TIM(TIM1, CH2, PA9, TIM_USE_LED, 0, 0), // CAMCTL - DEF_TIM(TIM12, CH2, PB15, TIM_USE_NONE, 0, 0), + DEF_TIM(TIM12, CH2, PB15, TIM_USE_CAMERA_CONTROL, 0, 0), // Backdoor timers on UARTs DEF_TIM(TIM4, CH1, PB6, TIM_USE_NONE, 0, 0), // UART1_TX Collision with PPM @@ -62,7 +62,7 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { DEF_TIM(TIM5, CH2, PA1, TIM_USE_NONE, 0, 0), // UART4_RX DEF_TIM(TIM8, CH1, PC6, TIM_USE_NONE, 0, 0), // UART6_TX Collision with MS1&2 (useful for OCTO) - DEF_TIM(TIM8, CH2, PC7, TIM_USE_NONE, 0, 0), // UART6_RX Collision with MS1&2 (useful for OCTO) + DEF_TIM(TIM8, CH2, PC7, TIM_USE_NONE, 0, 1), // UART6_RX Collision with MS1&2 (useful for OCTO) // Others DEF_TIM(TIM1, CH3, PA10, TIM_USE_NONE, 0, 0), // CS_ExtIMU, Collision with LED_STRIP diff --git a/src/main/target/OMNINXT/target.h b/src/main/target/OMNINXT/target.h index f5c5b52c99..55d3f016b4 100644 --- a/src/main/target/OMNINXT/target.h +++ b/src/main/target/OMNINXT/target.h @@ -46,6 +46,10 @@ #define USE_ACC #define USE_GYRO +// MPU interrupts +//#define USE_EXTI +//#define USE_GYRO_EXTI +//#define USE_MPU_DATA_READY_SIGNAL // For debugging with NUC405RG #define USE_FAKE_ACC @@ -57,28 +61,24 @@ #define USE_ACC_SPI_MPU6500 #define USE_GYRO_SPI_MPU6500 -#define USE_DUAL_GYRO - #define GYRO_1_SPI_INSTANCE SPI1 #define GYRO_1_CS_PIN PB12 // Onboard IMU #define GYRO_1_ALIGN CW0_DEG #define ACC_1_ALIGN CW0_DEG +#define GYRO_1_EXTI_PIN NONE #define GYRO_2_SPI_INSTANCE SPI1 #define GYRO_2_CS_PIN PA8 // External IMU -#define GYRO_2_ALIGN CW0_DEG -#define ACC_2_ALIGN CW0_DEG +#define GYRO_2_ALIGN CW270_DEG +#define ACC_2_ALIGN CW270_DEG +#define GYRO_2_EXTI_PIN NONE #define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1 -// MPU interrupts -//#define USE_EXTI -//#define MPU_INT_EXTI PC4 -//#define USE_MPU_DATA_READY_SIGNAL - #define USE_MAG #define MAG_I2C_INSTANCE (I2CDEV_1) #define USE_MAG_HMC5883 +#define USE_MAG_LIS3MDL #define USE_BARO #define USE_BARO_SPI_LPS @@ -91,7 +91,6 @@ #define USE_BARO_BMP280 #define USE_BARO_MS5611 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_INSTANCE SPI2 #define MAX7456_SPI_CS_PIN PA15 @@ -178,8 +177,6 @@ #define RSSI_ADC_PIN PC4 #define EXTERNAL1_ADC_PIN PA4 -#define CAMERA_CONTROL_PIN PB15 - #define USE_TRANSPONDER #define USE_RANGEFINDER @@ -200,5 +197,5 @@ #define TARGET_IO_PORTC (0xffff) #define TARGET_IO_PORTD BIT(2) -#define USABLE_TIMER_CHANNEL_COUNT 22 +#define USABLE_TIMER_CHANNEL_COUNT 21 #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(8) | TIM_N(9) | TIM_N(10) | TIM_N(11) | TIM_N(12)) diff --git a/src/main/target/OMNINXT/target.mk b/src/main/target/OMNINXT/target.mk index 4ef4d4e9e3..eb0448f4a5 100644 --- a/src/main/target/OMNINXT/target.mk +++ b/src/main/target/OMNINXT/target.mk @@ -23,4 +23,5 @@ TARGET_SRC = \ drivers/barometer/barometer_bmp280.c \ drivers/barometer/barometer_ms5611.c \ drivers/compass/compass_hmc5883l.c \ + drivers/compass/compass_lis3mdl.c \ drivers/max7456.c diff --git a/src/main/target/PYRODRONEF4/target.c b/src/main/target/PYRODRONEF4/target.c index 700f78f71a..cfeb705ec5 100644 --- a/src/main/target/PYRODRONEF4/target.c +++ b/src/main/target/PYRODRONEF4/target.c @@ -27,11 +27,11 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - DEF_TIM(TIM11, CH1, PB9, TIM_USE_ANY, 0, 0), // CAMERA_CONTROL_PIN + DEF_TIM(TIM11, CH1, PB9, TIM_USE_CAMERA_CONTROL, 0, 0), // CAMERA_CONTROL_PIN - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0), // M1_OUT - D1_ST2_CH5 - DEF_TIM(TIM8, CH2N, PB0, TIM_USE_MOTOR, 0, 0), // M2_OUT - D2_ST3_CH7 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MOTOR, 0, 0), // M3_OUT - D2_ST7_CH7 - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, 0, 1), // M4_OUT - D2_ST1_CH6 - DEF_TIM(TIM4, CH3, PB8, TIM_USE_LED , 0, 0) // LED & MOTOR5 D1_ST7_CH2 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0), // M1_OUT - D1_ST2_CH5 + DEF_TIM(TIM8, CH2N, PB0, TIM_USE_MOTOR, 0, 0), // M2_OUT - D2_ST3_CH7 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MOTOR, 0, 0), // M3_OUT - D2_ST7_CH7 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, 0, 1), // M4_OUT - D2_ST1_CH6 + DEF_TIM(TIM4, CH3, PB8, TIM_USE_LED , 0, 0) // LED & MOTOR5 D1_ST7_CH2 }; diff --git a/src/main/target/PYRODRONEF4/target.h b/src/main/target/PYRODRONEF4/target.h index 57f1ce0d20..36fe061f31 100644 --- a/src/main/target/PYRODRONEF4/target.h +++ b/src/main/target/PYRODRONEF4/target.h @@ -28,10 +28,10 @@ #define BEEPER_INVERTED #define INVERTER_PIN_UART1 PC3 // PC3 used as sBUS inverter select GPIO -#define CAMERA_CONTROL_PIN PB9 // define dedicated camera_osd_control pin #define USE_EXTI -#define MPU_INT_EXTI PC4 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC4 #define USE_MPU_DATA_READY_SIGNAL // DEFINE SPI USAGE @@ -39,14 +39,14 @@ #define USE_SPI_DEVICE_1 // MPU 6000 -#define MPU6000_CS_PIN PA4 -#define MPU6000_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 #define USE_ACC #define USE_ACC_SPI_MPU6000 #define USE_GYRO #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW0_DEG -#define ACC_MPU6000_ALIGN CW0_DEG +#define GYRO_1_ALIGN CW0_DEG +#define ACC_1_ALIGN CW0_DEG // DEFINE OSD #define USE_SPI_DEVICE_2 @@ -69,7 +69,6 @@ #define USE_UART1 #define UART1_RX_PIN PA10 #define UART1_TX_PIN PA9 -#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 #define USE_UART2 #define UART2_RX_PIN PA3 diff --git a/src/main/target/RACEBASE/target.h b/src/main/target/RACEBASE/target.h index 743e34a844..fb86e1fb42 100644 --- a/src/main/target/RACEBASE/target.h +++ b/src/main/target/RACEBASE/target.h @@ -24,6 +24,30 @@ #define USE_HARDWARE_REVISION_DETECTION #define USE_TARGET_CONFIG +// Removed to make the firmware fit into flash (in descending order of priority): +//#undef USE_GYRO_OVERFLOW_CHECK +//#undef USE_GYRO_LPF2 + +//#undef USE_ITERM_RELAX +//#undef USE_RC_SMOOTHING_FILTER + +//#undef USE_HUFFMAN +//#undef USE_PINIO +//#undef USE_PINIOBOX + +//#undef USE_TELEMETRY_HOTT +//#undef USE_TELEMETRY_MAVLINK +//#undef USE_TELEMETRY_LTM +//#undef USE_SERIALRX_XBUS +//#undef USE_SERIALRX_SUMH +//#undef USE_PWM + +//#undef USE_BOARD_INFO +#undef USE_EXTENDED_CMS_MENUS +//#undef USE_RTC_TIME +#undef USE_RX_MSP +#undef USE_ESC_SENSOR_INFO + #define LED0_PIN PB3 #define LED0_INVERTED @@ -35,13 +59,14 @@ #define BEEPER_INVERTED #define USE_EXTI -#define MPU_INT_EXTI PC5 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC5 #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW -#define MPU6000_CS_PIN PB5 -#define MPU6000_SPI_INSTANCE SPI2 +#define GYRO_1_CS_PIN PB5 +#define GYRO_1_SPI_INSTANCE SPI2 #define USE_GYRO #define USE_GYRO_SPI_MPU6000 @@ -49,8 +74,8 @@ #define USE_ACC #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW90_DEG -#define GYRO_MPU6000_ALIGN CW90_DEG +#define ACC_1_ALIGN CW90_DEG +#define GYRO_1_ALIGN CW90_DEG #define USE_UART1 #define USE_UART2 @@ -103,7 +128,6 @@ #define CURRENT_METER_ADC_PIN PA5 #define RSSI_ADC_PIN PA6 -#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_RSSI_ADC | FEATURE_OSD) @@ -121,4 +145,4 @@ #endif #define USABLE_TIMER_CHANNEL_COUNT 6 -#define USED_TIMERS (TIM_N(2) | TIM_N(3)| TIM_N(4) | TIM_N(8) | TIM_N(17)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2)| TIM_N(3) | TIM_N(8) | TIM_N(17)) diff --git a/src/main/target/RCEXPLORERF3/target.h b/src/main/target/RCEXPLORERF3/target.h index cf161e3269..3ebc2a5a51 100644 --- a/src/main/target/RCEXPLORERF3/target.h +++ b/src/main/target/RCEXPLORERF3/target.h @@ -26,8 +26,8 @@ #undef USE_GYRO_OVERFLOW_CHECK #undef USE_GYRO_LPF2 -#undef USE_TELEMETRY_MAVLINK -#undef USE_TELEMETRY_LTM +//#undef USE_TELEMETRY_MAVLINK +//#undef USE_TELEMETRY_LTM #undef USE_SERIALRX_XBUS #undef USE_EXTENDED_CMS_MENUS @@ -36,7 +36,6 @@ #undef USE_ESC_SENSOR_INFO -#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE #define LED0_PIN PB4 #define LED1_PIN PB5 @@ -47,19 +46,20 @@ #define USE_EXTI #define USE_MPU_DATA_READY_SIGNAL -#define MPU_INT_EXTI PA15 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PA15 #define USE_GYRO #define USE_GYRO_SPI_MPU6000 #define USE_ACC_SPI_MPU6000 -#define MPU6000_CS_PIN PB12 -#define MPU6000_SPI_INSTANCE SPI2 +#define GYRO_1_CS_PIN PB12 +#define GYRO_1_SPI_INSTANCE SPI2 #define USE_ACC -#define ACC_MPU6000_ALIGN CW180_DEG -#define GYRO_MPU6000_ALIGN CW180_DEG +#define ACC_1_ALIGN CW180_DEG +#define GYRO_1_ALIGN CW180_DEG #define USE_BARO #define USE_BARO_MS5611 @@ -134,4 +134,4 @@ #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) #define USABLE_TIMER_CHANNEL_COUNT 7 -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(17)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(16) | TIM_N(17)) diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index 1a4d07b958..8f63f98efd 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -76,70 +76,68 @@ #define INVERTER_PIN_UART1 PC0 #endif -#define MPU6000_CS_PIN PA4 -#define MPU6000_SPI_INSTANCE SPI1 - -#define MPU6500_CS_PIN PA4 -#define MPU6500_SPI_INSTANCE SPI1 - #define USE_GYRO #define USE_ACC #ifdef AIRBOTF4SD -#undef MPU6000_CS_PIN -#define MPU6000_CS_PIN PB13 -#define USE_GYRO_SPI_ICM20601 -#define ICM20601_CS_PIN PA4 // served through MPU6500 code -#define ICM20601_SPI_INSTANCE SPI1 -#define USE_DUAL_GYRO -#define GYRO_1_CS_PIN MPU6000_CS_PIN -#define GYRO_2_CS_PIN ICM20601_CS_PIN -#define GYRO_1_SPI_INSTANCE MPU6000_SPI_INSTANCE -#define GYRO_2_SPI_INSTANCE ICM20601_SPI_INSTANCE -#define ACC_1_ALIGN ALIGN_DEFAULT -#define ACC_2_ALIGN ALIGN_DEFAULT -#endif - -#if defined(SOULF4) #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW180_DEG +#define USE_ACC_SPI_MPU6000 +#define USE_GYRO_SPI_MPU6500 +#define USE_ACC_SPI_MPU6500 + +#define GYRO_1_CS_PIN PB13 +#define GYRO_2_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 +#define GYRO_2_SPI_INSTANCE SPI1 + +#define GYRO_1_ALIGN CW270_DEG +#define GYRO_2_ALIGN CW270_DEG +#define ACC_1_ALIGN CW270_DEG +#define ACC_2_ALIGN CW270_DEG + +#elif defined(SOULF4) + +#define USE_GYRO_SPI_MPU6000 +#define GYRO_1_ALIGN CW180_DEG #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW180_DEG +#define ACC_1_ALIGN CW180_DEG + +#define GYRO_1_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 #elif defined(PODIUMF4) -#define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW0_DEG +#define GYRO_1_ALIGN CW0_DEG -#define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW0_DEG +#define ACC_1_ALIGN CW0_DEG + +#define GYRO_1_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 #else #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW270_DEG - -#define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW270_DEG - #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW270_DEG - -#define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW270_DEG +#define GYRO_1_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 +#define GYRO_1_ALIGN CW270_DEG +#define ACC_1_ALIGN CW270_DEG #endif // MPU6000 interrupts #define USE_EXTI -#define MPU_INT_EXTI PC4 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC4 #define USE_MPU_DATA_READY_SIGNAL +#define GYRO_2_EXTI_PIN NONE + // Configure MAG and BARO unconditionally. #define USE_MAG #define USE_MAG_HMC5883 @@ -161,16 +159,11 @@ // SDCARD support for AIRBOTF4SD #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define USE_SDCARD +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED #define SDCARD_DETECT_PIN PC0 #define SDCARD_SPI_INSTANCE SPI3 #define SDCARD_SPI_CS_PIN SPI3_NSS_PIN - -// SPI2 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init: -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz -// Divide to under 25MHz for normal operation: -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 // 21MHz - #define SDCARD_DMA_CHANNEL_TX DMA1_Stream5 #define SDCARD_DMA_CHANNEL 0 @@ -196,7 +189,6 @@ #define USE_UART1 #define UART1_RX_PIN PA10 #define UART1_TX_PIN PA9 -#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 #define USE_UART3 #define UART3_RX_PIN PB11 @@ -258,7 +250,6 @@ #define VBAT_ADC_PIN PC2 #else #define VBAT_ADC_PIN PC3 -#define VBAT_ADC_CHANNEL ADC_Channel_13 #endif #if defined(AIRBOTF4SD) diff --git a/src/main/target/REVO/target.mk b/src/main/target/REVO/target.mk index b45407a446..b01f8931ab 100644 --- a/src/main/target/REVO/target.mk +++ b/src/main/target/REVO/target.mk @@ -1,6 +1,6 @@ F405_TARGETS += $(TARGET) ifeq ($(TARGET), AIRBOTF4SD) -FEATURES = VCP SDCARD +FEATURES = VCP SDCARD_SPI else FEATURES = VCP ONBOARDFLASH endif diff --git a/src/main/target/REVOLT/target.h b/src/main/target/REVOLT/target.h index 129dc15889..dcf8dee680 100644 --- a/src/main/target/REVOLT/target.h +++ b/src/main/target/REVOLT/target.h @@ -83,26 +83,21 @@ #define I2C1_SDA PB9 /*----------Gyro Config--------*/ -#define MPU6000_CS_PIN PA4 -#define MPU6000_SPI_INSTANCE SPI1 - -#define MPU6500_CS_PIN PA4 -#define MPU6500_SPI_INSTANCE SPI1 - #define USE_GYRO #define USE_ACC -#define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW0_DEG - -#define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW0_DEG +#define GYRO_1_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 + +#define GYRO_1_ALIGN CW0_DEG +#define ACC_1_ALIGN CW0_DEG #define USE_EXTI -#define MPU_INT_EXTI PC4 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC4 #define USE_MPU_DATA_READY_SIGNAL /*----------Flash Config--------*/ @@ -133,7 +128,6 @@ #define USE_UART1 #define UART1_RX_PIN PA10 #define UART1_TX_PIN PA9 -#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 #define USE_UART3 #define UART3_RX_PIN PB11 @@ -171,5 +165,5 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(2)) -#define USABLE_TIMER_CHANNEL_COUNT 11 -#define USED_TIMERS ( TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(8) | TIM_N(11) ) +#define USABLE_TIMER_CHANNEL_COUNT 6 +#define USED_TIMERS ( TIM_N(2) | TIM_N(4) | TIM_N(8) | TIM_N(11) ) diff --git a/src/main/target/REVONANO/target.h b/src/main/target/REVONANO/target.h index 9e034c91af..07030f3558 100644 --- a/src/main/target/REVONANO/target.h +++ b/src/main/target/REVONANO/target.h @@ -32,25 +32,24 @@ #define USE_BEEPER #define BEEPER_PIN PC13 -#define MPU6500_CS_PIN PB12 -#define MPU6500_SPI_INSTANCE SPI2 +#define GYRO_1_CS_PIN PB12 +#define GYRO_1_SPI_INSTANCE SPI2 #define USE_ACC -#define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW270_DEG +#define ACC_1_ALIGN CW270_DEG #define USE_GYRO -#define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW270_DEG +#define GYRO_1_ALIGN CW270_DEG #define USE_BARO #define USE_BARO_MS5611 // MPU6500 interrupts #define USE_EXTI -#define MPU_INT_EXTI PA15 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PA15 #define USE_MPU_DATA_READY_SIGNAL #define USE_VCP diff --git a/src/main/target/RG_SSD_F3/target.h b/src/main/target/RG_SSD_F3/target.h index e7b3fd6c1d..82b00cc701 100644 --- a/src/main/target/RG_SSD_F3/target.h +++ b/src/main/target/RG_SSD_F3/target.h @@ -23,9 +23,9 @@ #define TARGET_BOARD_IDENTIFIER "RGF3" // rgSSD_F3 // Removed to make the firmware fit into flash (in descending order of priority): -#undef USE_RTC_TIME +//#undef USE_RTC_TIME #undef USE_RX_MSP -#undef USE_ESC_SENSOR_INFO +//#undef USE_ESC_SENSOR_INFO #define LED0_PIN PC1 #define LED1_PIN PC0 @@ -34,8 +34,8 @@ #define BEEPER_PIN PA8 #define BEEPER_INVERTED -#define MPU6000_CS_PIN PB2 -#define MPU6000_SPI_INSTANCE SPI2 +#define GYRO_1_CS_PIN PB2 +#define GYRO_1_SPI_INSTANCE SPI2 #define USE_SPI #define USE_SPI_DEVICE_1 @@ -54,33 +54,27 @@ #define USE_GYRO #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW180_DEG +#define GYRO_1_ALIGN CW180_DEG #define USE_ACC #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW180_DEG +#define ACC_1_ALIGN CW180_DEG #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT #define USE_TARGET_CONFIG #define USE_EXTI -#define MPU_INT_EXTI PC13 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC13 #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW #define USE_SDCARD - +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED #define SDCARD_DETECT_PIN PC14 - #define SDCARD_SPI_INSTANCE SPI1 #define SDCARD_SPI_CS_GPIO GPIOB #define SDCARD_SPI_CS_PIN SPI1_NSS_PIN - -// SPI2 is on the APB1 bus whose clock runs at 36MHz. Divide to under 400kHz for init: -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128 -// Divide to under 25MHz for normal operation: -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 2 - #define SDCARD_DMA_CHANNEL_TX DMA1_Channel3 #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT @@ -114,12 +108,10 @@ #define I2C2_SCL_GPIO_AF GPIO_AF_4 #define I2C2_SCL_PIN GPIO_Pin_9 #define I2C2_SCL_PIN_SOURCE GPIO_PinSource9 -#define I2C2_SCL_CLK_SOURCE RCC_AHBPeriph_GPIOA #define I2C2_SDA_GPIO GPIOA #define I2C2_SDA_GPIO_AF GPIO_AF_4 #define I2C2_SDA_PIN GPIO_Pin_10 #define I2C2_SDA_PIN_SOURCE GPIO_PinSource10 -#define I2C2_SDA_CLK_SOURCE RCC_AHBPeriph_GPIOA #define USE_ADC #define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC @@ -147,6 +139,6 @@ #define TARGET_IO_PORTD (BIT(2)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) -#define USED_TIMERS (TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(16) | TIM_N(17)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(16)) #define USABLE_TIMER_CHANNEL_COUNT 9 diff --git a/src/main/target/RG_SSD_F3/target.mk b/src/main/target/RG_SSD_F3/target.mk index 59b6ea1842..6598b60ee8 100644 --- a/src/main/target/RG_SSD_F3/target.mk +++ b/src/main/target/RG_SSD_F3/target.mk @@ -1,5 +1,5 @@ F3_TARGETS += $(TARGET) -FEATURES = VCP SDCARD +FEATURES = VCP SDCARD_SPI TARGET_SRC = \ drivers/accgyro/accgyro_mpu.c \ diff --git a/src/main/target/SINGULARITY/target.h b/src/main/target/SINGULARITY/target.h index a0693f0c55..507ccd579c 100644 --- a/src/main/target/SINGULARITY/target.h +++ b/src/main/target/SINGULARITY/target.h @@ -22,7 +22,6 @@ #define TARGET_BOARD_IDENTIFIER "SING" -#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE #define LED0_PIN PB3 @@ -30,16 +29,17 @@ #define BEEPER_PIN PC15 #define USE_EXTI -#define MPU_INT_EXTI PC13 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC13 #define USE_MPU_DATA_READY_SIGNAL #define USE_GYRO #define USE_GYRO_MPU6050 -#define GYRO_MPU6050_ALIGN CW0_DEG_FLIP +#define GYRO_1_ALIGN CW0_DEG_FLIP #define USE_ACC #define USE_ACC_MPU6050 -#define ACC_MPU6050_ALIGN CW0_DEG_FLIP +#define ACC_1_ALIGN CW0_DEG_FLIP #define USE_FLASHFS #define USE_FLASH_M25P16 @@ -106,5 +106,5 @@ #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) #define USABLE_TIMER_CHANNEL_COUNT 10 -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(16) |TIM_N(17)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) |TIM_N(17)) diff --git a/src/main/target/SIRINFPV/target.h b/src/main/target/SIRINFPV/target.h index dc205dc088..0a62dd82b9 100644 --- a/src/main/target/SIRINFPV/target.h +++ b/src/main/target/SIRINFPV/target.h @@ -22,6 +22,30 @@ #define TARGET_BOARD_IDENTIFIER "SIRF" +// Removed to make the firmware fit into flash (in descending order of priority): +//#undef USE_GYRO_OVERFLOW_CHECK +//#undef USE_GYRO_LPF2 + +//#undef USE_ITERM_RELAX +//#undef USE_RC_SMOOTHING_FILTER + +#undef USE_HUFFMAN +#undef USE_PINIO +#undef USE_PINIOBOX + +//#undef USE_TELEMETRY_HOTT +//#undef USE_TELEMETRY_MAVLINK +#undef USE_TELEMETRY_LTM +#undef USE_SERIALRX_XBUS +#undef USE_SERIALRX_SUMH +#undef USE_PWM + +#undef USE_BOARD_INFO +#undef USE_EXTENDED_CMS_MENUS +#undef USE_RTC_TIME +#undef USE_RX_MSP +#undef USE_ESC_SENSOR_INFO + #define LED0_PIN PB2 #define USE_BEEPER #define BEEPER_PIN PA1 @@ -29,32 +53,31 @@ #define USE_EXTI #define USE_MPU_DATA_READY_SIGNAL -#define MPU_INT_EXTI PA8 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PA8 #define ENSURE_MPU_DATA_READY_IS_LOW #define USE_GYRO #define USE_GYRO_SPI_MPU6000 -#define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 #define USE_ACC #define USE_ACC_SPI_MPU6000 -#define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 // MPU6000 -#define ACC_MPU6000_ALIGN CW180_DEG -#define GYRO_MPU6000_ALIGN CW180_DEG +#define ACC_1_ALIGN CW180_DEG +#define GYRO_1_ALIGN CW180_DEG -#define MPU6000_CS_PIN PA4 -#define MPU6000_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 // MPU6500 -#define ACC_MPU6500_ALIGN CW90_DEG -#define GYRO_MPU6500_ALIGN CW90_DEG +//#define ACC_1_ALIGN CW90_DEG // XXX Must be post-flash configured +//#define GYRO_1_ALIGN CW90_DEG // XXX Must be post-flash configured -#define MPU6500_CS_PIN PA4 -#define MPU6500_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 #define USE_VCP #define USE_UART1 @@ -120,16 +143,11 @@ #define RTC6705_CS_PIN PC14 #define USE_SDCARD - +#define USE_SDCARD_SPI #define SDCARD_SPI_INSTANCE SPI2 #define SDCARD_SPI_CS_GPIO SPI2_GPIO #define SDCARD_SPI_CS_PIN SPI2_NSS_PIN -// SPI2 is on the APB1 bus whose clock runs at 36MHz. Divide to under 400kHz for init: -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128 -// Divide to under 25MHz for normal operation: -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 2 - // Note, this is the same DMA channel as UART1_RX. Luckily we don't use DMA for USART Rx. #define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 @@ -146,7 +164,6 @@ //#define USE_QUAD_MIXER_ONLY #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT -#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT #define DEFAULT_RX_FEATURE FEATURE_RX_PPM #define DEFAULT_FEATURES (FEATURE_RX_SERIAL | FEATURE_OSD) diff --git a/src/main/target/SIRINFPV/target.mk b/src/main/target/SIRINFPV/target.mk index d11e00c40d..57abe64cfa 100644 --- a/src/main/target/SIRINFPV/target.mk +++ b/src/main/target/SIRINFPV/target.mk @@ -1,5 +1,5 @@ F3_TARGETS += $(TARGET) -FEATURES = VCP SDCARD +FEATURES = VCP SDCARD_SPI TARGET_SRC = \ drivers/accgyro/accgyro_mpu.c \ diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c index 6b39529f97..dfeccecb8c 100644 --- a/src/main/target/SITL/target.c +++ b/src/main/target/SITL/target.c @@ -55,6 +55,8 @@ const timerHardware_t timerHardware[1]; // unused #include "dyad.h" #include "target/SITL/udplink.h" +uint32_t SystemCoreClock; + static fdm_packet fdmPkt; static servo_packet pwmPkt; @@ -112,7 +114,7 @@ void updateState(const fdm_packet* pkt) { fakeGyroSet(fakeGyroDev, x, y, z); // printf("[gyr]%lf,%lf,%lf\n", pkt->imu_angular_velocity_rpy[0], pkt->imu_angular_velocity_rpy[1], pkt->imu_angular_velocity_rpy[2]); -#if defined(SKIP_IMU_CALC) +#if !defined(USE_IMU_CALC) #if defined(SET_IMU_FROM_EULER) // set from Euler double qw = pkt->imu_orientation_quat[0]; @@ -436,7 +438,7 @@ void pwmCompleteMotorUpdate(uint8_t motorCount) { // for gazebo8 ArduCopterPlugin remap, normal range = [0.0, 1.0], 3D rang = [-1.0, 1.0] double outScale = 1000.0; - if (feature(FEATURE_3D)) { + if (featureIsEnabled(FEATURE_3D)) { outScale = 500.0; } diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index 20cc9b569e..ee83d961be 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -33,7 +33,7 @@ // use simulatior's attitude directly // disable this if wants to test AHRS algorithm -#define SKIP_IMU_CALC +#undef USE_IMU_CALC //#define SIMULATOR_ACC_SYNC //#define SIMULATOR_GYRO_SYNC @@ -138,13 +138,9 @@ #define DEFIO_NO_PORTS // suppress 'no pins defined' warning -#define WS2811_DMA_TC_FLAG (void *)1 -#define WS2811_DMA_HANDLER_IDENTIFER 0 - - // belows are internal stuff -uint32_t SystemCoreClock; +extern uint32_t SystemCoreClock; #ifdef EEPROM_IN_RAM extern uint8_t eepromData[EEPROM_SIZE]; diff --git a/src/main/target/SITL/target.mk b/src/main/target/SITL/target.mk index 801acaf0a2..80f025cf99 100644 --- a/src/main/target/SITL/target.mk +++ b/src/main/target/SITL/target.mk @@ -1,5 +1,5 @@ SITL_TARGETS += $(TARGET) -#FEATURES += SDCARD VCP +FEATURES += #SDCARD_SPI VCP TARGET_SRC = \ drivers/accgyro/accgyro_fake.c \ diff --git a/src/main/target/SKYZONEF405/target.c b/src/main/target/SKYZONEF405/target.c new file mode 100644 index 0000000000..a3f8ee4302 --- /dev/null +++ b/src/main/target/SKYZONEF405/target.c @@ -0,0 +1,45 @@ +/* + * 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 . + */ + +#include + +#include +#include "drivers/io.h" + +#include "drivers/dma.h" +#include "drivers/timer.h" +#include "drivers/timer_def.h" + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + DEF_TIM(TIM12, CH2, PB15, TIM_USE_PPM, 0, 0), // PPM + + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0), // S1 (1,7) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0), // S2 (2,2) + DEF_TIM(TIM3, CH2, PC7, TIM_USE_MOTOR, 0, 0), // S3 (2,4) (2.2) + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MOTOR, 0, 0), // S4 (2,7) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, 0, 0), // S5 (1,2) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MOTOR, 0, 0), // S6 (1,5) + DEF_TIM(TIM8, CH1, PC6, TIM_USE_MOTOR, 0, 0), // S7 (2,6) + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, 0, 1), // S8 (2,1) (2.3 2.6) + + DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), // LED STRIP(1,0) + + DEF_TIM(TIM2, CH2, PB3, TIM_USE_CAMERA_CONTROL, 0, 0), // FC CAM +}; diff --git a/src/main/target/SKYZONEF405/target.h b/src/main/target/SKYZONEF405/target.h new file mode 100644 index 0000000000..f543cecbb6 --- /dev/null +++ b/src/main/target/SKYZONEF405/target.h @@ -0,0 +1,150 @@ +/* + * 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 + +#define TARGET_BOARD_IDENTIFIER "SKF4" +#define TARGET_MANUFACTURER_IDENTIFIER "SKYZ" + +#define USBD_PRODUCT_STRING "skyzone F405" + +#define ENABLE_DSHOT_DMAR true + +#define LED0_PIN PC14 + +#define USE_BEEPER +#define BEEPER_PIN PC13 +#define BEEPER_INVERTED + +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_EXTI +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC4 +#define USE_MPU_DATA_READY_SIGNAL + +#define USE_GYRO +#define USE_ACC +#define USE_GYRO_SPI_ICM20689 +#define USE_ACC_SPI_ICM20689 + +#define GYRO_1_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 + +#define GYRO_1_ALIGN CW90_DEG +#define ACC_1_ALIGN CW90_DEG + +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C_DEVICE (I2CDEV_1) +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define USE_BARO +#define BARO_I2C_INSTANCE (I2CDEV_1) +#define USE_BARO_BMP085 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 + +#define USE_MAG +#define MAG_I2C_INSTANCE (I2CDEV_1) +#define USE_MAG_HMC5883 +#define USE_MAG_LIS3MDL + +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PC3 + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define FLASH_CS_PIN PB12 +#define FLASH_SPI_INSTANCE SPI2 + +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PB5 + +#define USE_OSD +#define USE_MAX7456 +#define MAX7456_SPI_INSTANCE SPI3 +#define MAX7456_SPI_CS_PIN PA15 + +#define USE_VCP + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART5 +#define UART5_RX_PIN PD2 +#define UART5_TX_PIN PC12 + +#define USE_SOFTSERIAL1 +#define USE_SOFTSERIAL2 + +#define SERIAL_PORT_COUNT 8 + +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART2 +#define SBUS_TELEMETRY_UART SERIAL_PORT_USART3 + +#define USE_ADC +#define ADC1_DMA_STREAM DMA2_Stream0 +#define VBAT_ADC_PIN PC2 +#define CURRENT_METER_ADC_PIN PC1 +#define RSSI_ADC_PIN PA0 + +#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC +#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC + +#define DEFAULT_FEATURES ( FEATURE_TELEMETRY | FEATURE_OSD ) + +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_PIN PA3 +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) + +#define USABLE_TIMER_CHANNEL_COUNT 11 +#define USED_TIMERS (TIM_N(1)|TIM_N(2)|TIM_N(3)|TIM_N(4)|TIM_N(8)|TIM_N(12)) diff --git a/src/main/target/SKYZONEF405/target.mk b/src/main/target/SKYZONEF405/target.mk new file mode 100644 index 0000000000..9f1c1b1e70 --- /dev/null +++ b/src/main/target/SKYZONEF405/target.mk @@ -0,0 +1,12 @@ +F405_TARGETS += $(TARGET) + +FEATURES += VCP ONBOARDFLASH + +TARGET_SRC = \ + drivers/accgyro/accgyro_spi_icm20689.c \ + drivers/barometer/barometer_bmp085.c \ + drivers/barometer/barometer_bmp280.c \ + drivers/barometer/barometer_ms5611.c \ + drivers/compass/compass_hmc5883l.c \ + drivers/compass/compass_lis3mdl.c \ + drivers/max7456.c diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index 70167cb8f7..55bcfc6765 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -22,7 +22,6 @@ #define TARGET_BOARD_IDENTIFIER "SPKY" // SParKY -#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE #define LED0_PIN PB4 // Blue (Rev 1 & 2) - PB4 #define LED1_PIN PB5 // Green (Rev 1) / Red (Rev 2) - PB5 @@ -33,17 +32,18 @@ // MPU6050 interrupts #define USE_EXTI -#define MPU_INT_EXTI PA15 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PA15 #define USE_MPU_DATA_READY_SIGNAL // MPU 9150 INT connected to PA15, pulled up to VCC by 10K Resistor, contains MPU6050 and AK8975 in single component. #define USE_GYRO #define USE_GYRO_MPU6050 -#define GYRO_MPU6050_ALIGN CW270_DEG +#define GYRO_1_ALIGN CW270_DEG #define USE_ACC #define USE_ACC_MPU6050 -#define ACC_MPU6050_ALIGN CW270_DEG +#define ACC_1_ALIGN CW270_DEG #define USE_BARO #define USE_BARO_MS5611 @@ -108,4 +108,4 @@ #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) #define USABLE_TIMER_CHANNEL_COUNT 11 -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(16) | TIM_N(17)) diff --git a/src/main/target/SPARKY2/target.h b/src/main/target/SPARKY2/target.h index 2c073e0dfa..a88e6ec9d1 100644 --- a/src/main/target/SPARKY2/target.h +++ b/src/main/target/SPARKY2/target.h @@ -38,21 +38,22 @@ // MPU9250 interrupt #define USE_EXTI -#define MPU_INT_EXTI PC5 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC5 //#define DEBUG_MPU_DATA_READY_INTERRUPT #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW -#define MPU9250_CS_PIN PC4 -#define MPU9250_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PC4 +#define GYRO_1_SPI_INSTANCE SPI1 #define USE_ACC #define USE_ACC_SPI_MPU9250 -#define ACC_MPU9250_ALIGN CW270_DEG +#define ACC_1_ALIGN CW270_DEG #define USE_GYRO #define USE_GYRO_SPI_MPU9250 -#define GYRO_MPU9250_ALIGN CW270_DEG +#define GYRO_1_ALIGN CW270_DEG #define USE_MAG //#define USE_MAG_HMC5883 @@ -82,7 +83,6 @@ #define USE_UART1 #define UART1_RX_PIN PA10 #define UART1_TX_PIN PA9 -#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 #define USE_UART3 #define UART3_RX_PIN PB11 @@ -132,4 +132,4 @@ #define TARGET_IO_PORTC 0xffff #define USABLE_TIMER_CHANNEL_COUNT 11 -#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9)) +#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(8) | TIM_N(12)) diff --git a/src/main/target/SPRACINGF3OSD/config.c b/src/main/target/SPEDIXF4/config.c similarity index 84% rename from src/main/target/SPRACINGF3OSD/config.c rename to src/main/target/SPEDIXF4/config.c index 0a6fbf6125..7611d69831 100644 --- a/src/main/target/SPRACINGF3OSD/config.c +++ b/src/main/target/SPEDIXF4/config.c @@ -23,17 +23,14 @@ #include "platform.h" -#include "drivers/serial.h" - -#include "io/serial.h" - -#include "config/feature.h" +#ifdef USE_TARGET_CONFIG #include "fc/config.h" -#ifdef USE_TARGET_CONFIG +#include "telemetry/telemetry.h" + void targetConfiguration(void) { - serialConfigMutable()->portConfigs[1].functionMask = FUNCTION_MSP; // To connect to FC. + telemetryConfigMutable()->halfDuplex = 0; } #endif diff --git a/src/main/target/SPEDIXF4/target.c b/src/main/target/SPEDIXF4/target.c new file mode 100644 index 0000000000..66ea662d3b --- /dev/null +++ b/src/main/target/SPEDIXF4/target.c @@ -0,0 +1,42 @@ +/* + * 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 . + */ + +#include "platform.h" +#include "drivers/io.h" + +#include "drivers/dma.h" +#include "drivers/timer.h" +#include "drivers/timer_def.h" + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MOTOR, 0, 0), // M1 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MOTOR, 0, 0), // M2 + DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, 0, 0), // M3 + DEF_TIM(TIM4, CH4, PB9, TIM_USE_MOTOR, 0, 0), // M4 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0), // M5 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0), // M6 + + DEF_TIM(TIM3, CH1, PC6, TIM_USE_ANY, 0, 0), // UART6_TX, M7 + DEF_TIM(TIM3, CH2, PC7, TIM_USE_ANY, 0, 0), // UART6_RX, M8 + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_PPM, 0, 0), + DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), + DEF_TIM(TIM8, CH4, PC9, TIM_USE_CAMERA_CONTROL, 0, 0) +}; diff --git a/src/main/target/SPEDIXF4/target.h b/src/main/target/SPEDIXF4/target.h new file mode 100644 index 0000000000..2534e5df83 --- /dev/null +++ b/src/main/target/SPEDIXF4/target.h @@ -0,0 +1,155 @@ +/* + * 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 + +#define TARGET_BOARD_IDENTIFIER "SPD4" +#define USBD_PRODUCT_STRING "SPEDIXF4" + +#define USE_TARGET_CONFIG + +#define LED0_PIN PC5 + +#define USE_BEEPER +#define BEEPER_PIN PC13 +#define BEEPER_INVERTED + +#define ENABLE_DSHOT_DMAR true + +#define USE_EXTI +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC4 +#define USE_MPU_DATA_READY_SIGNAL + +#define USE_GYRO +#define USE_GYRO_SPI_MPU6000 +#define USE_GYRO_SPI_MPU6500 +#define GYRO_1_CS_PIN SPI1_NSS_PIN +#define GYRO_1_SPI_INSTANCE SPI1 +#define GYRO_1_ALIGN CW0_DEG + +#define USE_ACC +#define USE_ACC_SPI_MPU6000 +#define USE_ACC_SPI_MPU6500 +#define ACC_1_ALIGN CW0_DEG + +#define USE_BARO +#define USE_BARO_SPI_BMP280 +#define DEFAULT_BARO_SPI_BMP280 +#define BMP280_SPI_INSTANCE SPI3 +#define BMP280_CS_PIN PC0 + +#define USE_MAG +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define MAG_I2C_INSTANCE (I2CDEV_2) + +#define USE_MAX7456 +#define MAX7456_SPI_INSTANCE SPI2 +#define MAX7456_SPI_CS_PIN SPI2_NSS_PIN +#define MAX7456_SPI_CLK ( SPI_CLOCK_STANDARD ) +#define MAX7456_RESTORE_CLK ( SPI_CLOCK_FAST ) + +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define FLASH_SPI_INSTANCE SPI3 +#define FLASH_CS_PIN SPI3_NSS_PIN +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +#define USE_VCP + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 +#define INVERTER_PIN_UART1 PC14 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PC11 +#define UART3_TX_PIN PC10 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART5 +#define UART5_RX_PIN PD2 +#define UART5_TX_PIN PC12 +#define INVERTER_PIN_UART5 PC15 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define SERIAL_PORT_COUNT 7 + +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_PIN PA8 + +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_NSS_PIN PA4 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_SPI_DEVICE_2 +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_SPI_DEVICE_3 +#define SPI3_NSS_PIN PC8 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + +#define USE_I2C +#define USE_I2C_DEVICE_2 +#define I2C2_SCL PB10 +#define I2C2_SDA PB11 +#define I2C_DEVICE (I2CDEV_2) + +#define USE_ADC +#define CURRENT_METER_ADC_PIN PC1 +#define VBAT_ADC_PIN PC2 +#define RSSI_ADC_PIN PC3 +#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC +#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC + +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_UART5 +#define SBUS_TELEMETRY_UART SERIAL_PORT_USART1 + +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define DEFAULT_FEATURES ( FEATURE_OSD | FEATURE_TELEMETRY ) +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA ( 0xffff ) +#define TARGET_IO_PORTB ( 0xffff ) +#define TARGET_IO_PORTC ( 0xffff ) +#define TARGET_IO_PORTD ( 0x0004 ) + +#define USABLE_TIMER_CHANNEL_COUNT 11 +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) ) diff --git a/src/main/target/SPEDIXF4/target.mk b/src/main/target/SPEDIXF4/target.mk new file mode 100644 index 0000000000..61f01a3e84 --- /dev/null +++ b/src/main/target/SPEDIXF4/target.mk @@ -0,0 +1,10 @@ +F405_TARGETS += $(TARGET) +FEATURES = VCP ONBOARDFLASH +TARGET_SRC = \ + drivers/accgyro/accgyro_mpu6500.c \ + drivers/accgyro/accgyro_spi_mpu6000.c \ + drivers/accgyro/accgyro_spi_mpu6500.c \ + drivers/barometer/barometer_bmp280.c \ + drivers/compass/compass_hmc5883l.c \ + drivers/compass/compass_qmc5883l.c \ + drivers/max7456.c diff --git a/src/main/target/SPEEDYBEEF4/target.h b/src/main/target/SPEEDYBEEF4/target.h index 536d59f964..258f01dce9 100644 --- a/src/main/target/SPEEDYBEEF4/target.h +++ b/src/main/target/SPEEDYBEEF4/target.h @@ -40,11 +40,12 @@ #define SPI1_MISO_PIN PA6 #define SPI1_MOSI_PIN PA7 -#define MPU6000_CS_PIN PB11 -#define MPU6000_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PB11 +#define GYRO_1_SPI_INSTANCE SPI1 #define USE_EXTI -#define MPU_INT_EXTI PC4 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC4 #define USE_MPU_DATA_READY_SIGNAL #define USE_GYRO @@ -56,6 +57,7 @@ #define USE_MAG #define USE_MAG_HMC5883 #define USE_MAG_QMC5883 +#define USE_MAG_LIS3MDL // *************** Baro ************************** #define USE_I2C @@ -88,7 +90,6 @@ #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_INSTANCE SPI2 #define MAX7456_SPI_CS_PIN PB10 diff --git a/src/main/target/SPEEDYBEEF4/target.mk b/src/main/target/SPEEDYBEEF4/target.mk index 90a40d0592..03531ccc6c 100644 --- a/src/main/target/SPEEDYBEEF4/target.mk +++ b/src/main/target/SPEEDYBEEF4/target.mk @@ -8,4 +8,5 @@ TARGET_SRC = \ drivers/barometer/barometer_ms5611.c \ drivers/compass/compass_hmc5883l.c \ drivers/compass/compass_qmc5883l.c \ + drivers/compass/compass_lis3mdl.c \ drivers/max7456.c diff --git a/src/main/target/SPEKTRUMF400/target.h b/src/main/target/SPEKTRUMF400/target.h index c983c0bccd..87b44a91a6 100644 --- a/src/main/target/SPEKTRUMF400/target.h +++ b/src/main/target/SPEKTRUMF400/target.h @@ -54,21 +54,19 @@ #define USE_GYRO #define USE_GYRO_SPI_MPU6500 #define USE_GYRO_SPI_MPU9250 -#define GYRO_MPU9250_ALIGN CW270_DEG +#define GYRO_1_ALIGN CW270_DEG #define USE_ACC #define USE_ACC_SPI_MPU6500 #define USE_ACC_SPI_MPU9250 -#define ACC_MPU9250_ALIGN CW270_DEG +#define ACC_1_ALIGN CW270_DEG -#define MPU9250_CS_PIN PB12 -#define MPU9250_SPI_INSTANCE SPI2 - -#define MPU6500_CS_PIN PB12 -#define MPU6500_SPI_INSTANCE SPI2 +#define GYRO_1_CS_PIN PB12 +#define GYRO_1_SPI_INSTANCE SPI2 #define USE_EXTI -#define MPU_INT_EXTI PC13 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC13 #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW diff --git a/src/main/target/SPRACINGF3/IRCSYNERGYF3.mk b/src/main/target/SPRACINGF3/IRCSYNERGYF3.mk new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index cb5d143b9b..949794bf49 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -26,26 +26,61 @@ #define TARGET_BOARD_IDENTIFIER "ZCF3" #elif defined(FLIP32F3OSD) #define TARGET_BOARD_IDENTIFIER "FLF3" +#elif defined(IRCSYNERGYF3) +#define TARGET_BOARD_IDENTIFIER "ISF3" #else #define TARGET_BOARD_IDENTIFIER "SRF3" #endif +// Removed to make the firmware fit into flash (in descending order of priority): +//#undef USE_GYRO_OVERFLOW_CHECK +//#undef USE_GYRO_LPF2 + +#if !(defined(ZCOREF3) || defined(FLIP32F3OSD) || defined(IRCSYNERGYF3)) +//#undef USE_ITERM_RELAX +//#undef USE_RC_SMOOTHING_FILTER + +//#undef USE_MSP_DISPLAYPORT +//#undef USE_MSP_OVER_TELEMETRY + +#undef USE_LED_STRIP +//#undef USE_HUFFMAN +//#undef USE_PINIO +//#undef USE_PINIOBOX + +//#undef USE_TELEMETRY_HOTT +//#undef USE_TELEMETRY_MAVLINK +//#undef USE_TELEMETRY_LTM +//#undef USE_SERIALRX_XBUS +//#undef USE_SERIALRX_SUMH +#undef USE_PWM + +#undef USE_EXTENDED_CMS_MENUS +#endif + +#undef USE_BOARD_INFO +#undef USE_RTC_TIME + +#undef USE_RX_MSP +#undef USE_ESC_SENSOR_INFO + #if defined(ZCOREF3) -#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT #define LED0_PIN PB8 #else -#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE #define LED0_PIN PB3 #endif +#if !defined(IRCSYNERGYF3) #define USE_BEEPER #define BEEPER_PIN PC15 #define BEEPER_INVERTED +#endif #define USE_EXTI -#define MPU_INT_EXTI PC13 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC13 #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW @@ -58,21 +93,19 @@ #if defined(FLIP32F3OSD) #define USE_GYRO_MPU6500 -#define GYRO_MPU6500_ALIGN CW270_DEG +#define GYRO_1_ALIGN CW270_DEG #define USE_ACC_MPU6500 -#define ACC_MPU6500_ALIGN CW270_DEG +#define ACC_1_ALIGN CW270_DEG #elif defined(ZCOREF3) -#define USE_GYRO #define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW180_DEG +#define GYRO_1_ALIGN CW180_DEG -#define USE_ACC #define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW180_DEG +#define ACC_1_ALIGN CW180_DEG #define USE_SPI_DEVICE_1 // PB9,3,4,5 on AF5 SPI1 (MPU) @@ -81,15 +114,24 @@ #define SPI1_MISO_PIN PB4 #define SPI1_MOSI_PIN PB5 -#define MPU6500_CS_PIN PB9 -#define MPU6500_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PB9 +#define GYRO_1_SPI_INSTANCE SPI1 +#elif defined(IRCSYNERGYF3) +#define USE_GYRO_SPI_MPU6000 +#define GYRO_1_ALIGN CW270_DEG + +#define USE_ACC_SPI_MPU6000 +#define ACC_1_ALIGN CW270_DEG + +#define GYRO_1_CS_PIN PB12 +#define GYRO_1_SPI_INSTANCE SPI2 #else #define USE_GYRO_MPU6050 -#define GYRO_MPU6050_ALIGN CW270_DEG +#define GYRO_1_ALIGN CW270_DEG #define USE_ACC_MPU6050 -#define ACC_MPU6050_ALIGN CW270_DEG +#define ACC_1_ALIGN CW270_DEG #endif #if defined(FLIP32F3OSD) @@ -123,8 +165,10 @@ #define MAG_INT_EXTI PC14 #endif +#if !defined(IRCSYNERGYF3) #define USE_FLASHFS #define USE_FLASH_M25P16 +#endif #define USE_UART1 #define USE_UART2 @@ -163,6 +207,7 @@ #define USE_SPI #define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 +#if !defined(IRCSYNERGYF3) #define FLASH_CS_PIN PB12 #define FLASH_SPI_INSTANCE SPI2 @@ -174,8 +219,10 @@ #define CURRENT_METER_ADC_PIN PA5 #define RSSI_ADC_PIN PB2 +#define USE_OSD #define USE_OSD_OVER_MSP_DISPLAYPORT #define USE_SLOW_MSP_DISPLAYPORT_RATE_WHEN_UNARMED +#endif #define USE_MSP_CURRENT_METER @@ -186,10 +233,12 @@ #undef USE_UART1_TX_DMA #endif +#if !defined(IRCSYNERGYF3) #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT #define DEFAULT_RX_FEATURE FEATURE_RX_PPM #define DEFAULT_FEATURES (FEATURE_RSSI_ADC | FEATURE_TELEMETRY) +#endif #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/SPRACINGF3/target.mk b/src/main/target/SPRACINGF3/target.mk index 9ce0e8c69b..0d96cb1702 100644 --- a/src/main/target/SPRACINGF3/target.mk +++ b/src/main/target/SPRACINGF3/target.mk @@ -19,7 +19,12 @@ TARGET_SRC += \ drivers/accgyro/accgyro_mpu6500.c \ drivers/accgyro/accgyro_spi_mpu6500.c else +ifeq ($(TARGET), IRCSYNERGYF3) +TARGET_SRC += \ + drivers/accgyro/accgyro_spi_mpu6000.c +else TARGET_SRC += \ drivers/accgyro/accgyro_mpu6050.c endif endif +endif diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index 7b0a826065..c4f66c3e04 100644 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -29,20 +29,36 @@ #endif // Removed to make the firmware fit into flash (in descending order of priority): +#if !defined(AIORACERF3) #undef USE_GYRO_OVERFLOW_CHECK #undef USE_GYRO_LPF2 +#undef USE_RC_SMOOTHING_FILTER +#undef ITERM_RELAX + +#undef USE_MSP_DISPLAYPORT +#undef USE_MSP_OVER_TELEMETRY + +#undef USE_HUFFMAN +#undef USE_PINIO +#undef USE_PINIOBOX + +#undef USE_VIRTUAL_CURRENT_METER +#endif + #undef USE_TELEMETRY_HOTT #undef USE_TELEMETRY_MAVLINK #undef USE_TELEMETRY_LTM #undef USE_SERIALRX_XBUS +#undef USE_SERIALRX_SUMH +#undef USE_PWM +#undef USE_BOARD_INFO #undef USE_EXTENDED_CMS_MENUS #undef USE_RTC_TIME #undef USE_RX_MSP #undef USE_ESC_SENSOR_INFO - #if !defined(AIORACERF3) #define USE_TARGET_CONFIG #endif @@ -57,7 +73,6 @@ #endif // SPRACINGF3MQ -#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT #define LED0_PIN PB8 @@ -66,7 +81,8 @@ #define BEEPER_INVERTED #define USE_EXTI -#define MPU_INT_EXTI PC13 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC13 #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW @@ -79,8 +95,8 @@ #define USE_ACC #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW180_DEG -#define GYRO_MPU6500_ALIGN CW180_DEG +#define ACC_1_ALIGN CW180_DEG +#define GYRO_1_ALIGN CW180_DEG #define USE_BARO #define USE_BARO_BMP280 @@ -141,23 +157,17 @@ #define SPI2_MOSI_PIN PB15 #define USE_SDCARD - +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED #define SDCARD_DETECT_PIN PC14 - #define SDCARD_SPI_INSTANCE SPI2 #define SDCARD_SPI_CS_PIN SPI2_NSS_PIN -// SPI2 is on the APB1 bus whose clock runs at 36MHz. Divide to under 400kHz for init: -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128 -// Divide to under 25MHz for normal operation: -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 2 - // Note, this is the same DMA channel as UART1_RX. Luckily we don't use DMA for USART Rx. #define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 -#define MPU6500_CS_PIN PB9 -#define MPU6500_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PB9 +#define GYRO_1_SPI_INSTANCE SPI1 #define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC @@ -173,8 +183,11 @@ #define CURRENT_METER_ADC_PIN PA5 #endif +#if !defined(AIORACERF3) +#define USE_OSD #define USE_OSD_OVER_MSP_DISPLAYPORT #define USE_MSP_CURRENT_METER +#endif #define USE_TRANSPONDER diff --git a/src/main/target/SPRACINGF3EVO/target.mk b/src/main/target/SPRACINGF3EVO/target.mk index bf60918251..89265f16e0 100644 --- a/src/main/target/SPRACINGF3EVO/target.mk +++ b/src/main/target/SPRACINGF3EVO/target.mk @@ -1,5 +1,5 @@ F3_TARGETS += $(TARGET) -FEATURES = VCP SDCARD +FEATURES = VCP SDCARD_SPI TARGET_SRC = \ drivers/accgyro/accgyro_mpu.c \ diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h index a51e3ed25d..7611c6ee78 100644 --- a/src/main/target/SPRACINGF3MINI/target.h +++ b/src/main/target/SPRACINGF3MINI/target.h @@ -23,18 +23,41 @@ #ifdef TINYBEEF3 #define TARGET_BOARD_IDENTIFIER "TBF3" -#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT #define LED0_PIN PB8 #else #define TARGET_BOARD_IDENTIFIER "SRFM" -#undef USE_OSD #ifndef SPRACINGF3MINI_REV #define SPRACINGF3MINI_REV 2 #endif -#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE +// Removed to make the firmware fit into flash (in descending order of priority): +//#undef USE_GYRO_OVERFLOW_CHECK +//#undef USE_GYRO_LPF2 + +//#undef USE_ITERM_RELAX +//#undef USE_RC_SMOOTHING_FILTER + +//#undef USE_MSP_DISPLAYPORT +//#undef USE_MSP_OVER_TELEMETRY + +#undef USE_HUFFMAN +#undef USE_PINIO +#undef USE_PINIOBOX + +#undef USE_TELEMETRY_HOTT +#undef USE_TELEMETRY_MAVLINK +#undef USE_TELEMETRY_LTM +#undef USE_SERIALRX_XBUS +#undef USE_SERIALRX_SUMH +#undef USE_PWM + +#undef USE_BOARD_INFO +#undef USE_EXTENDED_CMS_MENUS +#undef USE_RTC_TIME +#undef USE_RX_MSP +#undef USE_ESC_SENSOR_INFO #define LED0_PIN PB3 #endif @@ -44,7 +67,8 @@ #define BEEPER_INVERTED #define USE_EXTI -#define MPU_INT_EXTI PC13 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC13 #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW @@ -54,20 +78,20 @@ #ifdef TINYBEEF3 #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW270_DEG +#define GYRO_1_ALIGN CW270_DEG #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW270_DEG +#define ACC_1_ALIGN CW270_DEG #else #define USE_MAG_DATA_READY_SIGNAL #define ENSURE_MAG_DATA_READY_IS_HIGH #define USE_GYRO_MPU6500 -#define GYRO_MPU6500_ALIGN CW180_DEG +#define GYRO_1_ALIGN CW180_DEG #define USE_ACC_MPU6500 -#define ACC_MPU6500_ALIGN CW180_DEG +#define ACC_1_ALIGN CW180_DEG #define USE_BARO #define USE_BARO_BMP280 @@ -140,8 +164,8 @@ #define SPI1_MISO_PIN PB4 #define SPI1_MOSI_PIN PB5 -#define MPU6500_CS_PIN SPI1_NSS_PIN -#define MPU6500_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN SPI1_NSS_PIN +#define GYRO_1_SPI_INSTANCE SPI1 #else #define USE_I2C #define USE_I2C_DEVICE_1 @@ -155,18 +179,12 @@ #define SPI2_MOSI_PIN PB15 #define USE_SDCARD - +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED #define SDCARD_DETECT_PIN PC14 - #define SDCARD_SPI_INSTANCE SPI2 #define SDCARD_SPI_CS_PIN SPI2_NSS_PIN -// SPI2 is on the APB1 bus whose clock runs at 36MHz. Divide to under 400kHz for init: -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128 -// Divide to under 25MHz for normal operation: -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 2 - // Note, this is the same DMA channel as UART1_RX. Luckily we don't use DMA for USART Rx. #define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 diff --git a/src/main/target/SPRACINGF3MINI/target.mk b/src/main/target/SPRACINGF3MINI/target.mk index cbf1aa87f8..e20629f150 100644 --- a/src/main/target/SPRACINGF3MINI/target.mk +++ b/src/main/target/SPRACINGF3MINI/target.mk @@ -1,5 +1,5 @@ F3_TARGETS += $(TARGET) -FEATURES = VCP SDCARD +FEATURES = VCP SDCARD_SPI TARGET_SRC = \ drivers/accgyro/accgyro_mpu.c \ diff --git a/src/main/target/SPRACINGF3NEO/target.h b/src/main/target/SPRACINGF3NEO/target.h index 25e03d9126..79c0611e7c 100644 --- a/src/main/target/SPRACINGF3NEO/target.h +++ b/src/main/target/SPRACINGF3NEO/target.h @@ -25,20 +25,29 @@ // Removed to make the firmware fit into flash (in descending order of priority): // NOTE: Don't disable USE_GYRO_OVERFLOW_CHECK - board has ICM20602 gyro -#undef USE_GYRO_LPF2 +//#undef USE_GYRO_OVERFLOW_CHECK +//#undef USE_GYRO_LPF2 +#undef USE_ITERM_RELAX +#undef USE_RC_SMOOTHING_FILTER + +#undef USE_HUFFMAN +#undef USE_PINIO +#undef USE_PINIOBOX + +#undef USE_TELEMETRY_HOTT #undef USE_TELEMETRY_MAVLINK #undef USE_TELEMETRY_LTM #undef USE_SERIALRX_XBUS +#undef USE_SERIALRX_SUMH +#undef USE_PWM +#undef USE_BOARD_INFO #undef USE_EXTENDED_CMS_MENUS #undef USE_RTC_TIME #undef USE_RX_MSP #undef USE_ESC_SENSOR_INFO - -#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT - #define LED0_PIN PB9 #define LED1_PIN PB2 @@ -47,7 +56,8 @@ #define BEEPER_INVERTED //#define USE_EXTI -//#define MPU_INT_EXTI PC13 +//#define USE_GYRO_EXTI +//#define GYRO_1_EXTI_PIN PC13 //#define USE_MPU_DATA_READY_SIGNAL //#define ENSURE_MPU_DATA_READY_IS_LOW @@ -60,8 +70,8 @@ #define USE_ACC #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW0_DEG -#define GYRO_MPU6500_ALIGN CW0_DEG +#define ACC_1_ALIGN CW0_DEG +#define GYRO_1_ALIGN CW0_DEG #define USE_VCP #define USE_UART1 @@ -131,23 +141,17 @@ #define SPI_SHARED_MAX7456_AND_RTC6705 #define USE_SDCARD - +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED #define SDCARD_DETECT_PIN PC14 - #define SDCARD_SPI_INSTANCE SPI2 #define SDCARD_SPI_CS_PIN SPI2_NSS_PIN -// SPI2 is on the APB1 bus whose clock runs at 36MHz. Divide to under 400kHz for init: -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128 -// Divide to under 25MHz for normal operation: -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 2 - // Note, this is the same DMA channel as UART1_RX. Luckily we don't use DMA for USART Rx. #define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 -#define MPU6500_CS_PIN SPI1_NSS_PIN -#define MPU6500_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN SPI1_NSS_PIN +#define GYRO_1_SPI_INSTANCE SPI1 #define CURRENT_METER_SCALE_DEFAULT 300 @@ -160,15 +164,6 @@ #define CURRENT_METER_ADC_PIN PC2 #define RSSI_ADC_PIN PC0 -#define USE_LED_STRIP_ON_DMA1_CHANNEL2 -#define WS2811_PIN PA8 -#define WS2811_TIMER TIM1 -#define WS2811_DMA_CHANNEL DMA1_Channel2 -#define WS2811_IRQ DMA1_Channel2_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_6 - #define USE_TRANSPONDER #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT diff --git a/src/main/target/SPRACINGF3NEO/target.mk b/src/main/target/SPRACINGF3NEO/target.mk index 3bb8199299..32ef98f4b3 100644 --- a/src/main/target/SPRACINGF3NEO/target.mk +++ b/src/main/target/SPRACINGF3NEO/target.mk @@ -1,5 +1,5 @@ F3_TARGETS += $(TARGET) -FEATURES = VCP SDCARD +FEATURES = VCP SDCARD_SPI TARGET_SRC = \ drivers/accgyro/accgyro_mpu.c \ diff --git a/src/main/target/SPRACINGF3OSD/target.h b/src/main/target/SPRACINGF3OSD/target.h deleted file mode 100644 index 2e9d2a52d6..0000000000 --- a/src/main/target/SPRACINGF3OSD/target.h +++ /dev/null @@ -1,104 +0,0 @@ -/* - * 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 - -#define TARGET_BOARD_IDENTIFIER "SOF3" -#define USE_TARGET_CONFIG - -#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT - -#define LED0_PIN PA15 - -#define USE_EXTI - - -#define USE_GYRO -#define USE_FAKE_GYRO - -#define USE_ACC -#define USE_FAKE_ACC - -#define REMAP_TIM16_DMA -#define REMAP_TIM17_DMA - -#define USE_VCP -#define USE_UART1 -#define USE_UART3 -#define SERIAL_PORT_COUNT 3 - -#define UART1_TX_PIN PA9 -#define UART1_RX_PIN PA10 - -#define BUS_SWITCH_PIN PB3 // connects and disconnects UART1 from external devices - -#define UART3_TX_PIN PB10 -#define UART3_RX_PIN PB11 - -#define USE_I2C -#define USE_I2C_DEVICE_1 -#define I2C_DEVICE (I2CDEV_1) - -#define USE_SPI -#define USE_SPI_DEVICE_1 // Flash Chip -#define USE_SPI_DEVICE_2 // MAX7456 - -#define SPI1_NSS_PIN PA4 -#define SPI1_SCK_PIN PA5 -#define SPI1_MISO_PIN PA6 -#define SPI1_MOSI_PIN PA7 - -#define SPI2_NSS_PIN PB12 -#define SPI2_SCK_PIN PB13 -#define SPI2_MISO_PIN PB14 -#define SPI2_MOSI_PIN PB15 - -#define USE_MAX7456 -#define MAX7456_SPI_INSTANCE SPI2 -#define MAX7456_SPI_CS_PIN SPI2_NSS_PIN -#define MAX7456_NRST_PIN PB2 - -#define MAX7456_DMA_CHANNEL_TX DMA1_Channel5 -#define MAX7456_DMA_CHANNEL_RX DMA1_Channel4 -#define MAX7456_DMA_IRQ_HANDLER_ID DMA1_CH4_HANDLER - -#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC -#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC - -#define USE_ADC -#define ADC_INSTANCE ADC1 -#define VBAT_ADC_PIN PA2 -#define CURRENT_METER_ADC_PIN PA3 -#define VOLTAGE_12V_ADC_PIN PA0 -#define VOLTAGE_5V_ADC_PIN PA1 - -#define USE_TRANSPONDER - -#define DEFAULT_FEATURES (FEATURE_TRANSPONDER) - -// IO - assuming 303 in 64pin package, TODO -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(2)) -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) - -#define USABLE_TIMER_CHANNEL_COUNT 12 // 2xPPM, 6xPWM, UART3 RX/TX, LED Strip, IR. -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(15) | TIM_N(16)) diff --git a/src/main/target/SPRACINGF3OSD/target.mk b/src/main/target/SPRACINGF3OSD/target.mk deleted file mode 100644 index 288dd9340b..0000000000 --- a/src/main/target/SPRACINGF3OSD/target.mk +++ /dev/null @@ -1,6 +0,0 @@ -F3_TARGETS += $(TARGET) -FEATURES = VCP SDCARD - -TARGET_SRC = \ - drivers/accgyro/accgyro_fake.c \ - drivers/max7456.c diff --git a/src/main/target/SPRACINGF4EVO/target.h b/src/main/target/SPRACINGF4EVO/target.h index 8a4408cb58..31088e6c48 100644 --- a/src/main/target/SPRACINGF4EVO/target.h +++ b/src/main/target/SPRACINGF4EVO/target.h @@ -27,7 +27,7 @@ #define SPRACINGF4EVO_REV 2 #endif #ifdef SPRACINGF4EVODG -#define USE_DUAL_GYRO +#define USE_MULTI_GYRO #endif #define USBD_PRODUCT_STRING "SP Racing F4 EVO" @@ -41,11 +41,11 @@ #define INVERTER_PIN_UART2 PB2 #define USE_EXTI +#define USE_GYRO_EXTI #define GYRO_1_EXTI_PIN PC13 -#ifdef USE_DUAL_GYRO +#ifdef USE_MULTI_GYRO #define GYRO_2_EXTI_PIN PC5 // GYRO 2 / NC on prototype boards, but if it was it'd be here. #endif -#define MPU_INT_EXTI #ifndef SPRACINGF4EVODG #define USE_MPU_DATA_READY_SIGNAL @@ -62,20 +62,14 @@ #define USE_ACC #define USE_ACC_SPI_MPU6500 -#ifndef USE_DUAL_GYRO -#define ACC_MPU6500_ALIGN CW0_DEG -#define GYRO_MPU6500_ALIGN CW0_DEG +#ifndef USE_MULTI_GYRO +#define ACC_1_ALIGN CW0_DEG +#define GYRO_1_ALIGN CW0_DEG #else -#define ACC_MPU6500_1_ALIGN CW0_DEG -#define GYRO_MPU6500_1_ALIGN CW0_DEG - -#define ACC_MPU6500_2_ALIGN CW0_DEG -#define GYRO_MPU6500_2_ALIGN CW0_DEG - -#define GYRO_1_ALIGN GYRO_MPU6500_1_ALIGN -#define GYRO_2_ALIGN GYRO_MPU6500_2_ALIGN -#define ACC_1_ALIGN ACC_MPU6500_1_ALIGN -#define ACC_2_ALIGN ACC_MPU6500_2_ALIGN +#define GYRO_1_ALIGN CW0_DEG +#define GYRO_2_ALIGN CW0_DEG +#define ACC_1_ALIGN CW0_DEG +#define ACC_2_ALIGN CW0_DEG #endif #define USE_BARO @@ -86,6 +80,7 @@ #define USE_MAG_AK8975 #define USE_MAG_HMC5883 #define USE_MAG_QMC5883 +#define USE_MAG_LIS3MDL #define USE_VCP #define USE_UART1 @@ -153,23 +148,17 @@ #endif #define USE_SDCARD - +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED #define SDCARD_DETECT_PIN PC14 - #define SDCARD_SPI_INSTANCE SPI2 #define SDCARD_SPI_CS_PIN SPI2_NSS_PIN - -// SPI3 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init: -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz -// Divide to under 25MHz for normal operation: -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 // 21MHz #define SDCARD_DMA_CHANNEL_TX DMA1_Stream4 #define SDCARD_DMA_CHANNEL 0 -#ifndef USE_DUAL_GYRO -#define MPU6500_CS_PIN SPI1_NSS_PIN -#define MPU6500_SPI_INSTANCE SPI1 +#ifndef USE_MULTI_GYRO +#define GYRO_1_CS_PIN SPI1_NSS_PIN +#define GYRO_1_SPI_INSTANCE SPI1 #else #define GYRO_1_CS_PIN SPI1_NSS_PIN #define GYRO_1_SPI_INSTANCE SPI1 @@ -197,6 +186,7 @@ #define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC +#define USE_OSD #define USE_OSD_OVER_MSP_DISPLAYPORT #define USE_MSP_CURRENT_METER @@ -217,14 +207,10 @@ #define TARGET_IO_PORTD (BIT(2)) #define USABLE_TIMER_CHANNEL_COUNT 16 // 4xPWM, 8xESC, 2xESC via UART3 RX/TX, 1xLED Strip, 1xIR. -#if (SPRACINGF4NEO_REV >= 2) +#if (SPRACINGF4EVO_REV >= 2) #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(9)) #else #define USE_TIM10_TIM11_FOR_MOTORS -#ifdef USE_TIM10_TIM11_FOR_MOTORS #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(9) | TIM_N(10) | TIM_N(11)) -#else -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(9)) -#endif #endif diff --git a/src/main/target/SPRACINGF4EVO/target.mk b/src/main/target/SPRACINGF4EVO/target.mk index 86d4303c7b..d44e5b07f4 100644 --- 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_SPI TARGET_SRC = \ drivers/accgyro/accgyro_mpu6500.c \ @@ -9,6 +9,7 @@ TARGET_SRC = \ drivers/compass/compass_ak8975.c \ drivers/compass/compass_hmc5883l.c \ drivers/compass/compass_qmc5883l.c \ + drivers/compass/compass_lis3mdl.c \ drivers/max7456.c \ drivers/vtx_rtc6705.c diff --git a/src/main/target/SPRACINGF4NEO/target.h b/src/main/target/SPRACINGF4NEO/target.h index c97aa92d46..8d655f0ff6 100644 --- a/src/main/target/SPRACINGF4NEO/target.h +++ b/src/main/target/SPRACINGF4NEO/target.h @@ -55,7 +55,8 @@ #endif #define USE_EXTI -#define MPU_INT_EXTI PC13 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC13 #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW @@ -69,8 +70,8 @@ #define USE_ACC #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW0_DEG -#define GYRO_MPU6500_ALIGN CW0_DEG +#define ACC_1_ALIGN CW0_DEG +#define GYRO_1_ALIGN CW0_DEG #define USE_BARO #define USE_BARO_BMP280 @@ -160,23 +161,16 @@ //#define MAX7456_DMA_IRQ_HANDLER_ID DMA1_ST0_HANDLER #define USE_SDCARD - +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED #define SDCARD_DETECT_PIN PC14 - #define SDCARD_SPI_INSTANCE SPI2 #define SDCARD_SPI_CS_PIN SPI2_NSS_PIN - -// SPI3 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init: -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz -// Divide to under 25MHz for normal operation: -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 // 21MHz - #define SDCARD_DMA_CHANNEL_TX DMA1_Stream4 #define SDCARD_DMA_CHANNEL 0 -#define MPU6500_CS_PIN SPI1_NSS_PIN -#define MPU6500_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN SPI1_NSS_PIN +#define GYRO_1_SPI_INSTANCE SPI1 #define USE_ADC #define ADC_INSTANCE ADC1 @@ -198,14 +192,9 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_RSSI_ADC | FEATURE_TELEMETRY | FEATURE_LED_STRIP) -#define GPS_UART SERIAL_PORT_USART3 - #define SERIALRX_UART SERIAL_PORT_USART2 #define SERIALRX_PROVIDER SERIALRX_SBUS -#define TELEMETRY_UART SERIAL_PORT_UART5 -#define TELEMETRY_PROVIDER_DEFAULT FUNCTION_TELEMETRY_SMARTPORT - #define USE_BUTTONS // Physically located on the optional OSD/VTX board. #if (SPRACINGF4NEO_REV >= 3) #define BUTTON_A_PIN PB0 @@ -224,4 +213,8 @@ #define TARGET_IO_PORTD (BIT(2)) #define USABLE_TIMER_CHANNEL_COUNT 14 // 4xPWM, 6xESC, 2xESC via UART3 RX/TX, 1xLED Strip, 1xIR. +#if (SPRACINGF4NEO_REV >= 3) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(4) | TIM_N(8) | TIM_N(9)) +#else #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(9)) +#endif diff --git a/src/main/target/SPRACINGF4NEO/target.mk b/src/main/target/SPRACINGF4NEO/target.mk index 6568f56a5b..11b2828f79 100644 --- a/src/main/target/SPRACINGF4NEO/target.mk +++ b/src/main/target/SPRACINGF4NEO/target.mk @@ -1,5 +1,5 @@ F405_TARGETS += $(TARGET) -FEATURES = VCP SDCARD +FEATURES = VCP SDCARD_SPI TARGET_SRC = \ drivers/accgyro/accgyro_mpu6500.c \ diff --git a/src/main/target/SPRACINGF7DUAL/config.c b/src/main/target/SPRACINGF7DUAL/config.c index 61200984c9..96e96a2418 100644 --- a/src/main/target/SPRACINGF7DUAL/config.c +++ b/src/main/target/SPRACINGF7DUAL/config.c @@ -55,13 +55,20 @@ #include "config_helper.h" +#define GPS_UART SERIAL_PORT_USART3 #define TELEMETRY_UART SERIAL_PORT_UART5 -#ifdef USE_TELEMETRY static targetSerialPortFunction_t targetSerialPortFunction[] = { - { TELEMETRY_UART, FUNCTION_TELEMETRY_SMARTPORT }, -}; +#ifdef USE_GPS + { GPS_UART, FUNCTION_GPS }, #endif +#ifdef USE_TELEMETRY + { TELEMETRY_UART, FUNCTION_TELEMETRY_SMARTPORT }, +#endif +#if !defined(USE_GPS) && !defined(USE_TELEMETRY) + { SERIAL_PORT_NONE, FUNCTION_NONE }, +#endif +}; void targetConfiguration(void) { diff --git a/src/main/target/SPRACINGF7DUAL/target.c b/src/main/target/SPRACINGF7DUAL/target.c index 50d0c27fb9..0aa184f8de 100644 --- a/src/main/target/SPRACINGF7DUAL/target.c +++ b/src/main/target/SPRACINGF7DUAL/target.c @@ -29,21 +29,22 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + DEF_TIM(TIM5, CH1, PA0, TIM_USE_CAMERA_CONTROL, 0, 0), DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM | TIM_USE_PWM, 0, 0), // PPM / PWM1 / UART2 RX DEF_TIM(TIM9, CH1, PA2, TIM_USE_PWM, 0, 0), // PPM / PWM2 / UART2 TX #if (SPRACINGF7DUAL_REV <= 1) - DEF_TIM(TIM8, CH2, PC7, TIM_USE_MOTOR, 0, 0), // ESC 1 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_MOTOR, 0, 1), // ESC 1 #else - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, 0, 0), // ESC 1 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, 0, 1), // ESC 1 #endif DEF_TIM(TIM8, CH1, PC6, TIM_USE_MOTOR, 0, 0), // ESC 2 DEF_TIM(TIM8, CH4, PC9, TIM_USE_MOTOR, 0, 0), // ESC 3 #if (SPRACINGF7DUAL_REV <= 1) - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, 0, 0), // ESC 4 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, 0, 1), // ESC 4 #else - DEF_TIM(TIM8, CH2, PC7, TIM_USE_MOTOR, 0, 0), // ESC 4 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_MOTOR, 0, 1), // ESC 4 #endif DEF_TIM(TIM4, CH1, PB6, TIM_USE_MOTOR, 0, 0), // ESC 5 / Conflicts with USART5_RX / SPI3_RX - SPI3_RX can be mapped to DMA1_ST3_CH0 diff --git a/src/main/target/SPRACINGF7DUAL/target.h b/src/main/target/SPRACINGF7DUAL/target.h index 4919d32bb5..afb4cde393 100644 --- a/src/main/target/SPRACINGF7DUAL/target.h +++ b/src/main/target/SPRACINGF7DUAL/target.h @@ -43,9 +43,9 @@ #define BEEPER_INVERTED #define USE_EXTI +#define USE_GYRO_EXTI #define GYRO_1_EXTI_PIN PC13 #define GYRO_2_EXTI_PIN PC14 -#define MPU_INT_EXTI #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW @@ -57,25 +57,19 @@ #define USE_ACC_SPI_MPU6500 #if (SPRACINGF7DUAL_REV >= 2) -#define ACC_MPU6500_1_ALIGN CW0_DEG -#define GYRO_MPU6500_1_ALIGN CW0_DEG +#define GYRO_1_ALIGN CW0_DEG +#define ACC_1_ALIGN CW0_DEG -#define ACC_MPU6500_2_ALIGN CW270_DEG -#define GYRO_MPU6500_2_ALIGN CW270_DEG +#define GYRO_2_ALIGN CW270_DEG +#define ACC_2_ALIGN CW270_DEG #else -#define ACC_MPU6500_1_ALIGN CW180_DEG -#define GYRO_MPU6500_1_ALIGN CW180_DEG +#define GYRO_1_ALIGN CW180_DEG +#define ACC_1_ALIGN CW180_DEG -#define ACC_MPU6500_2_ALIGN CW270_DEG -#define GYRO_MPU6500_2_ALIGN CW270_DEG +#define GYRO_2_ALIGN CW270_DEG +#define ACC_2_ALIGN CW270_DEG #endif - -#define GYRO_1_ALIGN GYRO_MPU6500_1_ALIGN -#define GYRO_2_ALIGN GYRO_MPU6500_2_ALIGN -#define ACC_1_ALIGN ACC_MPU6500_1_ALIGN -#define ACC_2_ALIGN ACC_MPU6500_2_ALIGN - #define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_BOTH #define USE_BARO @@ -86,6 +80,7 @@ #define USE_MAG #define USE_MAG_HMC5883 +#define USE_MAG_LIS3MDL #define USE_VCP #define USE_UART1 @@ -148,22 +143,10 @@ #define SPI3_MOSI_PIN PB5 #define USE_SDCARD - +#define USE_SDCARD_SPI #define SDCARD_SPI_INSTANCE SPI3 #define SDCARD_SPI_CS_PIN PC3 - - -// SPI3 is on the APB1 bus whose clock runs at 54MHz. Divide to under 400kHz for init: -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 54Mhz/256 = 210kHz -// Divide to under 25MHz for normal operation: -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 // 54Mhz/3 = 18Mhz // FIXME currently running slower than 18mhz for testing. - #define SDCARD_DMA_STREAM_TX_FULL DMA1_Stream7 -#define SDCARD_DMA_TX DMA1 -#define SDCARD_DMA_STREAM_TX 7 -#define SDCARD_DMA_CLK LL_AHB1_GRP1_PERIPH_DMA1 - -#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF7_4 #define SDCARD_DMA_CHANNEL DMA_CHANNEL_0 #define USE_VTX_RTC6705 @@ -204,8 +187,6 @@ #define USE_PID_AUDIO -#define USE_LED_STRIP - #define USE_TRANSPONDER #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT @@ -213,8 +194,6 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_RSSI_ADC | FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_LED_STRIP) -#define GPS_UART SERIAL_PORT_USART3 - #define SERIALRX_UART SERIAL_PORT_USART2 #define SERIALRX_PROVIDER SERIALRX_SBUS @@ -231,7 +210,7 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(2)) -//#define USABLE_TIMER_CHANNEL_COUNT 16 // 4xPWM, 8xESC, 2xESC via UART3 RX/TX, 1xLED Strip, 1xIR. -#define USABLE_TIMER_CHANNEL_COUNT 16 -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(9)) +//#define USABLE_TIMER_CHANNEL_COUNT 17 // 4xPWM, 8xESC, 2xESC via UART3 RX/TX, 1xLED Strip, 1xIR, CAM_C +#define USABLE_TIMER_CHANNEL_COUNT 17 +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(8) | TIM_N(9)) diff --git a/src/main/target/SPRACINGF7DUAL/target.mk b/src/main/target/SPRACINGF7DUAL/target.mk index 27cd2b15a6..e756fd03e9 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_SPI TARGET_SRC = \ drivers/accgyro/accgyro_fake.c \ @@ -10,6 +10,7 @@ TARGET_SRC = \ drivers/barometer/barometer_bmp280.c \ drivers/barometer/barometer_ms5611.c \ drivers/compass/compass_hmc5883l.c \ + drivers/compass/compass_lis3mdl.c \ drivers/max7456.c \ drivers/vtx_rtc6705_soft_spi.c diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index e8768ff1b1..f0f38601eb 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -36,9 +36,30 @@ #define TARGET_BOARD_IDENTIFIER "SDF3" // STM Discovery F3 -#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE +// Removed to make the firmware fit into flash (in descending order of priority): +//#undef USE_GYRO_OVERFLOW_CHECK +//#undef USE_GYRO_LPF2 + +//#undef USE_ITERM_RELAX +//#undef USE_RC_SMOOTHING_FILTER + +//#undef USE_HUFFMAN +//#undef USE_PINIO +//#undef USE_PINIOBOX + +#undef USE_TELEMETRY_HOTT +#undef USE_TELEMETRY_MAVLINK +#undef USE_TELEMETRY_LTM +#undef USE_SERIALRX_XBUS +#undef USE_SERIALRX_SUMH +//#undef USE_PWM + +#undef USE_BOARD_INFO +//#undef USE_RX_MSP +//#undef USE_RTC_TIME +#undef USE_EXTENDED_CMS_MENUS +#undef USE_ESC_SENSOR_INFO -#undef USE_OSD // ROM SAVING #define CURRENT_TARGET_CPU_VOLTAGE 3.0 @@ -92,25 +113,19 @@ //#define USE_GYRO_L3G4200D #define USE_GYRO_MPU3050 #define USE_GYRO_MPU6050 -#define USE_GYRO_SPI_MPU6000 -#define MPU6000_CS_PIN SPI2_NSS_PIN -#define MPU6000_SPI_INSTANCE SPI2 -// Support the GY-91 MPU9250 dev board #define USE_GYRO_MPU6500 +#define USE_GYRO_SPI_MPU6000 #define USE_GYRO_SPI_MPU6500 -#define MPU6500_CS_PIN PC14 -#define MPU6500_SPI_INSTANCE SPI2 -#define GYRO_MPU6500_ALIGN CW270_DEG_FLIP #define USE_GYRO_SPI_MPU9250 -#define MPU9250_CS_PIN SPI2_NSS_PIN -#define MPU9250_SPI_INSTANCE SPI2 -// BMI160 gyro support +#define GYRO_1_CS_PIN SPI2_NSS_PIN +#define GYRO_1_SPI_INSTANCE SPI2 +#define GYRO_1_ALIGN CW0_DEG #define USE_ACCGYRO_BMI160 #ifdef USE_ACCGYRO_BMI160 -#define BMI160_CS_PIN SPI2_NSS_PIN -#define BMI160_SPI_INSTANCE SPI2 #define BMI160_SPI_DIVISOR 16 -#define BMI160_INT_EXTI PC13 +#define USE_EXTI +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC13 #define USE_MPU_DATA_READY_SIGNAL #define USE_EXTI #endif @@ -128,7 +143,7 @@ #define USE_ACC_SPI_MPU6500 #define USE_ACC_MPU9250 #define USE_ACC_SPI_MPU9250 -#define ACC_MPU6500_ALIGN CW270_DEG_FLIP +#define ACC_1_ALIGN CW270_DEG_FLIP #define USE_BARO #define USE_FAKE_BARO @@ -136,22 +151,15 @@ #define USE_BARO_BMP280 #define USE_BARO_MS5611 -#define USE_MAX7456 -#define MAX7456_SPI_INSTANCE SPI2 -#define MAX7456_SPI_CS_PIN SPI2_NSS_PIN +//#define USE_MAX7456 +//#define MAX7456_SPI_INSTANCE SPI2 +//#define MAX7456_SPI_CS_PIN SPI2_NSS_PIN //#define USE_SDCARD -// //#define SDCARD_SPI_INSTANCE SPI2 //#define SDCARD_SPI_CS_PIN PB12 -//// SPI2 is on the APB1 bus whose clock runs at 36MHz. Divide to under 400kHz for init: -//#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128 -//// Divide to under 25MHz for normal operation: -//#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 2 -// //// Note, this is the same DMA channel as UART1_RX. Luckily we don't use DMA for USART Rx. //#define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 - // Performance logging for SD card operations: // #define AFATFS_USE_INTROSPECTIVE_LOGGING diff --git a/src/main/target/STM32F3DISCOVERY/target.mk b/src/main/target/STM32F3DISCOVERY/target.mk index a455a47c87..3b338c0e0a 100644 --- a/src/main/target/STM32F3DISCOVERY/target.mk +++ b/src/main/target/STM32F3DISCOVERY/target.mk @@ -1,5 +1,5 @@ F3_TARGETS += $(TARGET) -FEATURES = VCP SDCARD ONBOARDFLASH +FEATURES = VCP SDCARD_SPI ONBOARDFLASH TARGET_SRC = \ drivers/accgyro/accgyro_fake.c \ @@ -11,14 +11,6 @@ TARGET_SRC = \ drivers/accgyro/accgyro_spi_mpu6000.c \ drivers/accgyro/accgyro_spi_mpu6500.c \ drivers/accgyro/accgyro_spi_mpu9250.c \ - drivers/accgyro_legacy/accgyro_adxl345.c \ - drivers/accgyro_legacy/accgyro_bma280.c \ - drivers/accgyro_legacy/accgyro_l3gd20.c \ - drivers/accgyro_legacy/accgyro_l3g4200d.c \ - drivers/accgyro_legacy/accgyro_lsm303dlhc.c \ - drivers/accgyro_legacy/accgyro_adxl345.c \ - drivers/accgyro_legacy/accgyro_bma280.c \ - drivers/accgyro_legacy/accgyro_mma845x.c \ drivers/barometer/barometer_bmp085.c \ drivers/barometer/barometer_bmp280.c \ drivers/barometer/barometer_fake.c \ diff --git a/src/main/target/STM32F4DISCOVERY/target.h b/src/main/target/STM32F4DISCOVERY/target.h index 269a7bb391..30046655c3 100644 --- a/src/main/target/STM32F4DISCOVERY/target.h +++ b/src/main/target/STM32F4DISCOVERY/target.h @@ -32,27 +32,26 @@ // MPU6500 interrupt #define USE_EXTI -#define MPU_INT_EXTI PC4 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC4 #define USE_MPU_DATA_READY_SIGNAL //#define DEBUG_MPU_DATA_READY_INTERRUPT -#define MPU6500_CS_PIN PC4 -#define MPU6500_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PC4 +#define GYRO_1_SPI_INSTANCE SPI1 // ACC section -- start #define USE_ACC #define USE_FAKE_ACC -#define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW180_DEG_FLIP +#define ACC_1_ALIGN CW180_DEG_FLIP // ACC section -- end // GYRO section -- start #define USE_GYRO #define USE_FAKE_GYRO -#define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW180_DEG_FLIP +#define GYRO_1_ALIGN CW180_DEG_FLIP // GYRO section -- end #define USE_VCP @@ -83,7 +82,7 @@ #define SERIAL_PORT_COUNT 6 //VCP, USART1, USART3, USART4, USART6 #define USE_ESCSERIAL -#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 +#define ESCSERIAL_TIMER_TX_PIN PB9 //SPI #define USE_SPI @@ -96,20 +95,15 @@ #define SPI2_MOSI_PIN PB15 #define USE_SDCARD +#define USE_SDCARD_SPI #define SDCARD_SPI_INSTANCE SPI2 #define SDCARD_SPI_CS_PIN PD8 -// SPI2 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init: -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz -// Divide to under 25MHz for normal operation: -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 // 5,5MHz - #define SDCARD_DMA_CHANNEL_TX DMA1_Stream5 #define SDCARD_DMA_CHANNEL 0 #define USE_ADC #define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC #define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC -#define BOARD_HAS_VOLTAGE_DIVIDER #define VBAT_ADC_PIN PC1 #define CURRENT_METER_ADC_PIN PC2 @@ -124,4 +118,4 @@ #define TARGET_IO_PORTD 0xffff #define USABLE_TIMER_CHANNEL_COUNT 7 -#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(8)) +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) ) diff --git a/src/main/target/STM32F4DISCOVERY/target.mk b/src/main/target/STM32F4DISCOVERY/target.mk index c3619efdb4..3a09402a71 100644 --- a/src/main/target/STM32F4DISCOVERY/target.mk +++ b/src/main/target/STM32F4DISCOVERY/target.mk @@ -1,5 +1,5 @@ F405_TARGETS += $(TARGET) -FEATURES += VCP SDCARD MSC +FEATURES += VCP SDCARD_SPI MSC TARGET_SRC = \ drivers/accgyro/accgyro_fake.c \ diff --git a/src/main/target/STM32F7X2/config/NERO.config b/src/main/target/STM32F7X2/config/NERO.config index 0284786218..0a5b084d4f 100644 --- a/src/main/target/STM32F7X2/config/NERO.config +++ b/src/main/target/STM32F7X2/config/NERO.config @@ -34,6 +34,14 @@ resource SPI_MOSI 3 C12 resource SPI_PREINIT_IPU 1 C04 // MPU6500 resource SPI_PREINIT_IPU 2 A15 // SDCARD +# Acc/gyro +resource gyro_cs 1 C4 +resource GYRO_EXTI 1 B15 +set gyro_1_bustype = SPI +set gyro_1_spibus = 1 +set gyro_1_sensor_align = CW0 +#set gyro_1_hardware = MPU6500 # Not working (yet ... will it ever?) + # Timers # First four timers # timer is zero origin @@ -85,3 +93,4 @@ resource SDCARD_DETECT 1 D02 FEATURE RX_SERIAL set serialrx_provider = SBUS serial 5 64 115200 57600 0 115200 +set battery_meter = ADC diff --git a/src/main/target/STM32F7X2/target.h b/src/main/target/STM32F7X2/target.h index 51c74e26c8..343e7ad9f2 100644 --- a/src/main/target/STM32F7X2/target.h +++ b/src/main/target/STM32F7X2/target.h @@ -30,13 +30,11 @@ #define USE_BEEPER -#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC - // MPU interrupt #define USE_EXTI #define USE_MPU_DATA_READY_SIGNAL //#define DEBUG_MPU_DATA_READY_INTERRUPT -#define MPU_INT_EXTI PB15 // XXX Should be gone +#define USE_GYRO_EXTI #define USE_ACC #define USE_GYRO @@ -45,16 +43,12 @@ #define USE_GYRO_SPI_MPU6500 // Other USE_ACCs and USE_GYROs should follow -// Should be gone -#define MPU6500_CS_PIN PC4 // XXX Should be gone -#define MPU6500_SPI_INSTANCE SPI1 // XXX Should be gone -#define ACC_MPU6500_ALIGN CW0_DEG -#define GYRO_MPU6500_ALIGN CW0_DEG - +#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1 #define USE_MAG #define USE_MAG_HMC5883 #define USE_MAG_QMC5883 +#define USE_MAG_LIS3MDL #define MAG_HMC5883_ALIGN CW270_DEG_FLIP #define USE_BARO @@ -63,9 +57,7 @@ #define USE_BARO_LPS #define USE_SDCARD - -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 // 21MHz +#define USE_SDCARD_SPI #define USE_I2C #define USE_I2C_DEVICE_1 diff --git a/src/main/target/STM32F7X2/target.mk b/src/main/target/STM32F7X2/target.mk index 4269ebdb17..4637331c0c 100644 --- a/src/main/target/STM32F7X2/target.mk +++ b/src/main/target/STM32F7X2/target.mk @@ -1,5 +1,5 @@ F7X2RE_TARGETS += $(TARGET) -FEATURES += SDCARD VCP +FEATURES += SDCARD_SPI VCP TARGET_SRC = \ $(addprefix drivers/accgyro/,$(notdir $(wildcard $(SRC_DIR)/drivers/accgyro/*.c))) \ diff --git a/src/main/target/TINYFISH/target.h b/src/main/target/TINYFISH/target.h index a28b8edd81..c4120752e2 100644 --- a/src/main/target/TINYFISH/target.h +++ b/src/main/target/TINYFISH/target.h @@ -34,19 +34,20 @@ #define USE_EXTI -#define MPU_INT_EXTI PC13 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC13 #define USE_MPU_DATA_READY_SIGNAL -#define MPU6000_SPI_INSTANCE SPI1 -#define MPU6000_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PA4 #define USE_GYRO #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW180_DEG_FLIP +#define GYRO_1_ALIGN CW180_DEG_FLIP #define USE_ACC #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW180_DEG_FLIP +#define ACC_1_ALIGN CW180_DEG_FLIP #if USB_VCP_ENABLED @@ -114,13 +115,6 @@ #define CURRENT_METER_SCALE_DEFAULT (0.005 * 0.001 * 30000) * 1000 * 10 * (CURRENT_TARGET_CPU_VOLTAGE / 3.3) #define CURRENT_METER_OFFSET_DEFAULT 0 -#define WS2811_PIN PA8 -#define WS2811_TIMER TIM1 -#define WS2811_DMA_CHANNEL DMA1_Channel2 -#define WS2811_IRQ DMA1_Channel2_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER - #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define DEFAULT_FEATURES ( FEATURE_TELEMETRY ) #define USE_TARGET_CONFIG diff --git a/src/main/target/UAVPNG030MINI/target.c b/src/main/target/UAVPNG030MINI/target.c new file mode 100644 index 0000000000..5ac94896a3 --- /dev/null +++ b/src/main/target/UAVPNG030MINI/target.c @@ -0,0 +1,39 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include + +#include +#include "drivers/io.h" + +#include "drivers/dma.h" +#include "drivers/timer.h" +#include "drivers/timer_def.h" + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + DEF_TIM(TIM3, CH1, PA2, TIM_USE_PPM, 0, 0), // PPM_IN_A + DEF_TIM(TIM3, CH1, PA3, TIM_USE_ANY, 0, 0), // Secondary PPM input PPM_IN_B (user can assign this as he wants) + + DEF_TIM(TIM3, CH3, PA8, TIM_USE_MOTOR, 0, 0), // PPM_OUT_A + DEF_TIM(TIM1, CH2, PE11, TIM_USE_MOTOR, 0, 0), // PPM_OUT_B + DEF_TIM(TIM1, CH3, PE13, TIM_USE_MOTOR, 0, 0), // PPM_OUT_C + DEF_TIM(TIM1, CH4, PE14, TIM_USE_MOTOR, 0, 0), // PPM_OUT_D + DEF_TIM(TIM3, CH3, PB5, TIM_USE_MOTOR, 0, 0), // PPM_OUT_E + DEF_TIM(TIM4, CH2, PD13, TIM_USE_MOTOR, 0, 0), // PPM_OUT_F + DEF_TIM(TIM4, CH3, PD14, TIM_USE_MOTOR, 0, 0), // PPM_OUT_G + DEF_TIM(TIM4, CH4, PD15, TIM_USE_MOTOR, 0, 0), // PPM_OUT_H +}; diff --git a/src/main/target/UAVPNG030MINI/target.h b/src/main/target/UAVPNG030MINI/target.h new file mode 100644 index 0000000000..4cd4538a9d --- /dev/null +++ b/src/main/target/UAVPNG030MINI/target.h @@ -0,0 +1,139 @@ +/* + * This 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. + * + * This software 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 this software. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "UAVP" + +#define USBD_PRODUCT_STRING "UAVP-NG HW-0.30-mini" + +#define LED0_PIN PE5 +#define LED1_PIN PE7 +#define LED2_PIN PE6 +#define LED3_PIN PE8 + +#define USE_BEEPER +#define BEEPER_PIN PB0 +#define BEEPER_INVERTED + +#define USE_ACC +#define USE_ACC_SPI_MPU6000 + +#define USE_GYRO +#define USE_GYRO_SPI_MPU6000 + +#define GYRO_1_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 + +// TODO +#define GYRO_1_ALIGN CW180_DEG +#define ACC_1_ALIGN CW180_DEG + +// MPU6000 interrupts +#define USE_EXTI +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PE0 +#define USE_MPU_DATA_READY_SIGNAL + +#define USE_MAG +#define USE_MAG_HMC5883 +#define MAG_HMC5883_ALIGN CW90_DEG +#define MAG_I2C_INSTANCE I2CDEV_2 +#define HMC5883_I2C_INSTANCE I2CDEV_2 +#define MAG_INT_EXTI PE12 +#define USE_MAG_DATA_READY_SIGNAL + +#define USE_BARO +#define USE_BARO_MS5611 +#define USE_BARO_SPI_MS5611 +#define MS5611_CS_PIN PE1 +#define MS5611_SPI_INSTANCE SPI1 + +#if 0 // TODO: Enable SDCard and blackbox logging +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +#define USE_SDCARD +#define USE_SDCARD_SPI +#define SDCARD_DETECT_INVERTED +#define SDCARD_DETECT_PIN PE2 +#define SDCARD_SPI_INSTANCE SPI2 +#define SDCARD_SPI_CS_PIN SPI2_NSS_PIN +#define SDCARD_DMA_CHANNEL_TX DMA1_Stream4 +#define SDCARD_DMA_CHANNEL DMA_Channel_0 +#endif + +#define USE_VCP +//#define VBUS_SENSING_PIN PC5 + +#define USE_UART1 +#define UART1_RX_PIN PB7 +#define UART1_TX_PIN PB6 + +#define USE_UART2 +#define UART2_RX_PIN PD6 +#define UART2_TX_PIN PD5 + +#define USE_UART3 +#define UART3_RX_PIN PD9 +#define UART3_TX_PIN PD8 + +#define USE_UART6 +#define UART6_RX_PIN PC6 +#define UART6_TX_PIN PC7 + +#define SERIAL_PORT_COUNT 5 //VCP, USART1, USART2, USART3, USART6 + +// TODO +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_PIN PA2 + +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_NSS_PIN PA4 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +/// I2C Configuration +#define USE_I2C +#define USE_I2C_PULLUP +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define USE_I2C_DEVICE_2 +#define I2C2_SCL PB10 +#define I2C2_SDA PB11 + +#define I2C_DEVICE (I2CDEV_2) + +#define USE_ADC +#define VBAT_ADC_PIN PC1 +#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC + +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA (0xffff & ~(BIT(0)|BIT(1)|BIT(10)|BIT(13)|BIT(14)|BIT(15))) +#define TARGET_IO_PORTB (0xffff & ~(BIT(2)|BIT(3)|BIT(4))) +#define TARGET_IO_PORTC (0xffff & ~(BIT(15)|BIT(14)|BIT(13))) +#define TARGET_IO_PORTD (0xffff & ~(BIT(0)|BIT(1))) +#define TARGET_IO_PORTE (0xffff & ~(BIT(15))) + +#define USABLE_TIMER_CHANNEL_COUNT 10 + +// Used Timers +//#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(8) | TIM_N(9) | TIM_N(12)) +// Defined Timers in timer.c +#define USED_TIMERS ( TIM_N(1) | TIM_N(3) | TIM_N(4)) diff --git a/src/main/target/UAVPNG030MINI/target.mk b/src/main/target/UAVPNG030MINI/target.mk new file mode 100644 index 0000000000..3ecb836a84 --- /dev/null +++ b/src/main/target/UAVPNG030MINI/target.mk @@ -0,0 +1,6 @@ +F405_TARGETS += $(TARGET) +FEATURES += VCP +TARGET_SRC = \ + drivers/accgyro/accgyro_spi_mpu6000.c \ + drivers/barometer/barometer_ms5611.c \ + drivers/compass/compass_hmc5883l.c diff --git a/src/main/target/VRRACE/target.h b/src/main/target/VRRACE/target.h index 338d7bedd7..e3a83ea6d8 100644 --- a/src/main/target/VRRACE/target.h +++ b/src/main/target/VRRACE/target.h @@ -32,20 +32,21 @@ #define INVERTER_PIN_UART6 PD7 -#define MPU6500_CS_PIN PE10 -#define MPU6500_SPI_INSTANCE SPI2 +#define GYRO_1_CS_PIN PE10 +#define GYRO_1_SPI_INSTANCE SPI2 #define USE_ACC #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW270_DEG +#define ACC_1_ALIGN CW270_DEG #define USE_GYRO #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW270_DEG +#define GYRO_1_ALIGN CW270_DEG // MPU6500 interrupts #define USE_EXTI -#define MPU_INT_EXTI PD10 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PD10 #define USE_MPU_DATA_READY_SIGNAL /* @@ -54,9 +55,8 @@ #define MS5611_I2C_INSTANCE I2CDEV_1 #define USE_SDCARD - +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED - #define SDCARD_DETECT_PIN PD2 #define SDCARD_SPI_INSTANCE SPI2 #define SDCARD_SPI_CS_PIN PB12 @@ -64,20 +64,14 @@ /* #define SDCARD_DETECT_PIN PD2 - #define SDCARD_SPI_INSTANCE SPI3 #define SDCARD_SPI_CS_PIN PB3 */ -// SPI2 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init: /* -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz */ -// Divide to under 25MHz for normal operation: /* -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 // 21MHz */ - /* #define SDCARD_DMA_CHANNEL_TX DMA1_Stream4 #define SDCARD_DMA_CHANNEL 0 @@ -172,4 +166,4 @@ #define TARGET_IO_PORTE 0xffff #define USABLE_TIMER_CHANNEL_COUNT 12 -#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9) ) +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(9) ) diff --git a/src/main/target/WORMFC/target.h b/src/main/target/WORMFC/target.h index 9646f06156..99e15871a7 100644 --- a/src/main/target/WORMFC/target.h +++ b/src/main/target/WORMFC/target.h @@ -36,25 +36,24 @@ // MPU6500 interrupt #define USE_EXTI -#define MPU_INT_EXTI PC4 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC4 #define USE_MPU_DATA_READY_SIGNAL //#define DEBUG_MPU_DATA_READY_INTERRUPT -#define MPU6500_CS_PIN PA4 -#define MPU6500_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 // ACC section -- start #define USE_ACC -#define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW180_DEG_FLIP +#define ACC_1_ALIGN CW180_DEG_FLIP // ACC section -- end // GYRO section -- start #define USE_GYRO -#define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW180_DEG_FLIP +#define GYRO_1_ALIGN CW180_DEG_FLIP // GYRO section -- end //BARO @@ -90,7 +89,7 @@ #define SERIAL_PORT_COUNT 5 //VCP, USART1, USART3, USART4, USART6 #define USE_ESCSERIAL -#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 +#define ESCSERIAL_TIMER_TX_PIN PB9 //SPI #define USE_SPI @@ -112,7 +111,6 @@ #define USE_ADC #define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC #define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC -#define BOARD_HAS_VOLTAGE_DIVIDER #define VBAT_ADC_PIN PC1 //#define RSSI_ADC_PIN PC2 #define CURRENT_METER_ADC_PIN PC2 @@ -126,6 +124,7 @@ //SD CARD #define USE_SDCARD #define USE_SDCARD_SDIO + #define SDIO_DMA DMA2_Stream3 #define SDCARD_SPI_CS_PIN NONE //This is not used on SDIO, has to be kept for now to keep compiler happy #define SDCARD_DETECT_PIN PB15 @@ -136,4 +135,4 @@ #define TARGET_IO_PORTD (BIT(2)) #define USABLE_TIMER_CHANNEL_COUNT 7 -#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(8)) +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) ) diff --git a/src/main/target/WORMFC/target.mk b/src/main/target/WORMFC/target.mk index 1a0a31d925..4fa1a426ed 100644 --- a/src/main/target/WORMFC/target.mk +++ b/src/main/target/WORMFC/target.mk @@ -1,5 +1,5 @@ F405_TARGETS += $(TARGET) -FEATURES += VCP SDIO +FEATURES += VCP SDCARD_SDIO TARGET_SRC = \ drivers/accgyro/accgyro_mpu.c \ diff --git a/src/main/target/X_RACERSPI/target.h b/src/main/target/X_RACERSPI/target.h index 4a5b0efc4a..d21a7259f6 100644 --- a/src/main/target/X_RACERSPI/target.h +++ b/src/main/target/X_RACERSPI/target.h @@ -22,7 +22,6 @@ #define TARGET_BOARD_IDENTIFIER "XRC3" -#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE #define LED0_PIN PC14 @@ -33,21 +32,22 @@ #define USE_MAG_DATA_READY_SIGNAL #define ENSURE_MAG_DATA_READY_IS_HIGH -#define MPU6000_CS_PIN PA15 -#define MPU6000_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PA15 +#define GYRO_1_SPI_INSTANCE SPI1 #define USE_GYRO #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW270_DEG +#define GYRO_1_ALIGN CW270_DEG #define USE_ACC #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW270_DEG +#define ACC_1_ALIGN CW270_DEG // MPU6000 interrupts #define USE_EXTI -#define MPU_INT_EXTI PC13 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC13 #define USE_MPU_DATA_READY_SIGNAL @@ -125,5 +125,5 @@ #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) -#define USABLE_TIMER_CHANNEL_COUNT 17 -#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) +#define USABLE_TIMER_CHANNEL_COUNT 15 +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) diff --git a/src/main/target/YUPIF4/target.c b/src/main/target/YUPIF4/target.c index 6cbe30a697..d6f94000a0 100644 --- a/src/main/target/YUPIF4/target.c +++ b/src/main/target/YUPIF4/target.c @@ -28,14 +28,14 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - DEF_TIM(TIM8, CH3, PC8, TIM_USE_PPM, 0, 0 ), // PPM IN - DEF_TIM(TIM5, CH1, PA0, TIM_USE_MOTOR, 0, 0 ), // S1_OUT - DMA1_ST2 - DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR, 0, 0 ), // S2_OUT - DMA1_ST4 - DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, 0, 0 ), // S3_OUT - DMA1_ST1 - DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, 0, 1 ), // S4_OUT - DMA1_ST6 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0 ), // S5_OUT - DMA1_ST7 - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MOTOR | TIM_USE_LED, 0, 0 ), // S6_OUT - DMA1_ST3 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_ANY, 0, 0 ), // Camera control - DEF_TIM(TIM3, CH4, PC9, TIM_USE_BEEPER, 0, 0 ), // BEEPER PWM - DEF_TIM(TIM12, CH1, PB14, TIM_USE_BEEPER, 0, 0 ), // BEEPER PWM OPT + DEF_TIM(TIM8, CH3, PC8, TIM_USE_PPM, 0, 0 ), // PPM IN + DEF_TIM(TIM5, CH1, PA0, TIM_USE_MOTOR, 0, 0 ), // S1_OUT - DMA1_ST2 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR, 0, 0 ), // S2_OUT - DMA1_ST4 + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, 0, 0 ), // S3_OUT - DMA1_ST1 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, 0, 1 ), // S4_OUT - DMA1_ST6 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0 ), // S5_OUT - DMA1_ST7 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MOTOR | TIM_USE_LED | TIM_USE_CAMERA_CONTROL, 0, 0 ), // S6_OUT - DMA1_ST3 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_ANY, 0, 0 ), // Camera control + DEF_TIM(TIM3, CH4, PC9, TIM_USE_BEEPER, 0, 0 ), // BEEPER PWM + DEF_TIM(TIM12, CH1, PB14, TIM_USE_BEEPER, 0, 0 ), // BEEPER PWM OPT }; diff --git a/src/main/target/YUPIF4/target.h b/src/main/target/YUPIF4/target.h index 90a6b00bb5..9f53f43330 100644 --- a/src/main/target/YUPIF4/target.h +++ b/src/main/target/YUPIF4/target.h @@ -30,9 +30,6 @@ #define LED1_PIN PB4 #define LED2_PIN PB5 -//define camera control -#define CAMERA_CONTROL_PIN PB7 // Camera control - #define USE_BEEPER #define BEEPER_PIN PC9 #define BEEPER_OPT PB14 @@ -41,34 +38,21 @@ // Gyro interrupt #define USE_EXTI #define USE_MPU_DATA_READY_SIGNAL -#define MPU_INT_EXTI PC4 - -//ICM 20689 -#define ICM20689_CS_PIN PA4 -#define ICM20689_SPI_INSTANCE SPI1 +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC4 #define USE_ACC #define USE_ACC_SPI_ICM20689 -#define ACC_ICM20689_ALIGN CW90_DEG +#define USE_ACC_SPI_MPU6500 +#define ACC_1_ALIGN CW90_DEG #define USE_GYRO #define USE_GYRO_SPI_ICM20689 -#define GYRO_ICM20689_ALIGN CW90_DEG - -// MPU 6500 -#define MPU6500_CS_PIN PA4 -#define MPU6500_SPI_INSTANCE SPI1 - -#define USE_ACC -#define USE_ACC_MPU6500 -#define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW90_DEG - -#define USE_GYRO -#define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW90_DEG +#define GYRO_1_ALIGN CW90_DEG +#define GYRO_1_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 #define USE_VCP @@ -100,17 +84,11 @@ // SD Card #define USE_SDCARD +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED - #define SDCARD_DETECT_PIN PD2 #define SDCARD_SPI_INSTANCE SPI3 #define SDCARD_SPI_CS_PIN PA15 - -// SPI2 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init: -#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz -// Divide to under 25MHz for normal operation: -#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 // 21MHz - #define SDCARD_DMA_CHANNEL_TX DMA1_Stream5 #define SDCARD_DMA_CHANNEL 0 @@ -140,7 +118,7 @@ #define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC #define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC #define USE_ADC -#define RSSI_ADC_GPIO_PIN PC0 +#define RSSI_ADC_PIN PC0 #define VBAT_ADC_PIN PC1 #define CURRENT_METER_ADC_PIN PC2 #define CURRENT_METER_SCALE_DEFAULT 150 diff --git a/src/main/target/YUPIF4/target.mk b/src/main/target/YUPIF4/target.mk index 8047b59f40..341ae64ee0 100644 --- a/src/main/target/YUPIF4/target.mk +++ b/src/main/target/YUPIF4/target.mk @@ -1,5 +1,5 @@ F405_TARGETS += $(TARGET) -FEATURES += SDCARD VCP +FEATURES += SDCARD_SPI VCP TARGET_SRC = \ drivers/accgyro/accgyro_mpu6500.c \ diff --git a/src/main/target/YUPIF7/target.c b/src/main/target/YUPIF7/target.c index aaa6e777ec..d989e3bb33 100644 --- a/src/main/target/YUPIF7/target.c +++ b/src/main/target/YUPIF7/target.c @@ -22,19 +22,20 @@ #include "platform.h" #include "drivers/io.h" + +#include "drivers/dma.h" #include "drivers/timer.h" #include "drivers/timer_def.h" -#include "drivers/dma.h" - const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - DEF_TIM(TIM8, CH3, PC8, TIM_USE_PPM, 0, 0 ), // PPM IN + DEF_TIM(TIM8, CH3, PC8, TIM_USE_PPM, 0, 1 ), // PPM IN DEF_TIM(TIM2, CH1, PA0, TIM_USE_MOTOR, 0, 0 ), // S1_OUT - DMA1_ST2 DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR, 0, 0 ), // S2_OUT - DMA1_ST4 - DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, 0, 0 ), // S3_OUT - DMA1_ST1 - DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, 0, 1 ), // S4_OUT - DMA1_ST6 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_MOTOR, 0, 0 ), // S3_OUT - DMA1_ST1 + DEF_TIM(TIM5, CH4, PA3, TIM_USE_MOTOR, 0, 1 ), // S4_OUT - DMA1_ST6 DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0 ), // S5_OUT - DMA1_ST7 - LED Control DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR | TIM_USE_LED, 0, 0 ), // S6_OUT - DEF_TIM(TIM4, CH2, PB7, TIM_USE_ANY, 0, 0 ), // Camera Control + DEF_TIM(TIM4, CH2, PB7, TIM_USE_CAMERA_CONTROL, 0, 0 ), // Camera Control DEF_TIM(TIM12, CH1, PB14, TIM_USE_BEEPER, 0, 0 ), // BEEPER PWM }; + diff --git a/src/main/target/YUPIF7/target.h b/src/main/target/YUPIF7/target.h index 3cc686c134..62fec2b4d8 100644 --- a/src/main/target/YUPIF7/target.h +++ b/src/main/target/YUPIF7/target.h @@ -23,31 +23,39 @@ #define USBD_PRODUCT_STRING "YUPIF7" -#define LED0_PIN PB4 +#define ENABLE_DSHOT_DMAR true -//define camera control -#define CAMERA_CONTROL_PIN PB7 +#define LED0_PIN PB4 #define USE_BEEPER #define BEEPER_PIN PB14 -#define BEEPER_PWM_HZ 3150 // Beeper PWM frequency in Hz +#define BEEPER_PWM_HZ 3150 + +// *************** Gyro & ACC ********************** +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 -// Gyro interrupt #define USE_EXTI +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PC4 + #define USE_MPU_DATA_READY_SIGNAL -#define MPU_INT_EXTI PC4 // ICM 20689 -#define ICM20689_CS_PIN PA4 -#define ICM20689_SPI_INSTANCE SPI1 +#define GYRO_1_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 #define USE_ACC #define USE_ACC_SPI_ICM20689 -#define ACC_ICM20689_ALIGN CW90_DEG +#define ACC_1_ALIGN CW90_DEG #define USE_GYRO #define USE_GYRO_SPI_ICM20689 -#define GYRO_ICM20689_ALIGN CW90_DEG +#define GYRO_1_ALIGN CW90_DEG // Serial ports #define USE_VCP @@ -76,63 +84,63 @@ #define SERIAL_PORT_COUNT 6 //VCP, USART1, USART3, USART5, USART6, SOFTSERIAL1 -#define USE_ESCSERIAL -#define ESCSERIAL_TIMER_TX_PIN PC8 // (Hardware=0, PPM) - -//SPI ports -#define USE_SPI - -#define USE_SPI_DEVICE_1 //Gyro & OSD -#define SPI1_NSS_PIN PA4 -#define SPI1_SCK_PIN PA5 -#define SPI1_MISO_PIN PA6 -#define SPI1_MOSI_PIN PA7 - -#define USE_SPI_DEVICE_3 //Dataslash +// *************** Dataflash *********************** +#define USE_SPI_DEVICE_3 #define SPI3_NSS_PIN PA15 #define SPI3_SCK_PIN PC10 #define SPI3_MISO_PIN PC11 #define SPI3_MOSI_PIN PB5 -/* I2C Port -#define USE_I2C -#define USE_I2C_PULLUP -#define USE_I2C_DEVICE_1 -#define I2C2_SCL PB8 -#define I2C2_SDA PB9 -#define I2C_DEVICE (I2CDEV_1) -*/ +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define FLASH_CS_PIN PA15 +#define FLASH_SPI_INSTANCE SPI3 -// OSD +// *************** Baro **************************** +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define USE_I2C_PULLUP +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 +#define I2C_DEVICE (I2CDEV_1) + +#define BARO_I2C_INSTANCE (I2CDEV_1) +#define USE_BARO +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 + +//*********** Magnetometer / Compass ************* +#define MAG_I2C_INSTANCE (I2CDEV_1) +#define USE_MAG +#define USE_MAG_HMC5883 +#define MAG_HMC5883_ALIGN CW270_DEG_FLIP +#define USE_MAG_QMC5883 +#define MAG_QMC5883_ALIGN CW270_DEG_FLIP +#define USE_MAG_LIS3MDL + +// *************** OSD ***************************** #define USE_MAX7456 #define MAX7456_SPI_INSTANCE SPI1 #define MAX7456_SPI_CS_PIN PA14 -#define MAX7456_SPI_CLK (SPI_CLOCK_STANDARD) // 10MHz -#define MAX7456_RESTORE_CLK (SPI_CLOCK_FAST) -// Dataflash -#define USE_FLASHFS -#define USE_FLASH_M25P16 -#define FLASH_CS_PIN SPI3_NSS_PIN -#define FLASH_SPI_INSTANCE SPI3 - -// ADC inputs +// *************** ADC ***************************** #define USE_ADC -#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC -#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC -#define RSSI_ADC_GPIO_PIN PC0 +#define ADC1_DMA_STREAM DMA2_Stream0 +#define RSSI_ADC_PIN PC0 #define VBAT_ADC_PIN PC1 #define CURRENT_METER_ADC_PIN PC2 #define CURRENT_METER_SCALE_DEFAULT 235 -// Default configuration + +// *************** Target Config ******************* +#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC +#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS #define SERIALRX_UART SERIAL_PORT_USART6 -#define TELEMETRY_UART SERIAL_PORT_USART1 -#define DEFAULT_FEATURES (FEATURE_TELEMETRY | FEATURE_OSD) +#define DEFAULT_FEATURES (FEATURE_OSD) -// Target IO and timers +#define USE_ESCSERIAL #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define TARGET_IO_PORTA 0xffff diff --git a/src/main/target/YUPIF7/target.mk b/src/main/target/YUPIF7/target.mk index 49f2102b8c..61808eabc2 100644 --- a/src/main/target/YUPIF7/target.mk +++ b/src/main/target/YUPIF7/target.mk @@ -5,6 +5,6 @@ TARGET_SRC = \ drivers/barometer/barometer_bmp280.c \ drivers/barometer/barometer_ms5611.c \ drivers/compass/compass_hmc5883l.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_hal.c \ + drivers/compass/compass_qmc5883l.c \ + drivers/compass/compass_lis3mdl.c \ drivers/max7456.c diff --git a/src/main/target/common_defaults_post.h b/src/main/target/common_defaults_post.h index ce187dc5a7..d78d755b7f 100644 --- a/src/main/target/common_defaults_post.h +++ b/src/main/target/common_defaults_post.h @@ -25,11 +25,31 @@ #define MAX7456_CLOCK_CONFIG_DEFAULT MAX7456_CLOCK_CONFIG_OC #endif +#ifndef MAX7456_SPI_CLK +#define MAX7456_SPI_CLK (SPI_CLOCK_STANDARD) +#endif + +#ifndef MAX7456_RESTORE_CLK +#define MAX7456_RESTORE_CLK (SPI_CLOCK_FAST) +#endif + #ifndef MAX7456_SPI_CS_PIN #define MAX7456_SPI_CS_PIN NONE #endif #endif +// pg/flash + +#ifdef USE_FLASH_M25P16 +#ifndef FLASH_CS_PIN +#define FLASH_CS_PIN NONE +#endif + +#ifndef FLASH_SPI_INSTANCE +#define FLASH_SPI_INSTANCE NULL +#endif +#endif + // pg/bus_i2c #ifdef I2C_FULL_RECONFIGURABILITY @@ -224,3 +244,91 @@ #ifndef BINDPLUG_PIN #define BINDPLUG_PIN NONE #endif + +// F4 and F7 single gyro boards +#if defined(USE_MULTI_GYRO) && !defined(GYRO_2_SPI_INSTANCE) +#define GYRO_2_SPI_INSTANCE GYRO_1_SPI_INSTANCE +#define GYRO_2_CS_PIN NONE +#define GYRO_2_ALIGN ALIGN_DEFAULT +#define GYRO_2_EXTI_PIN NONE +#define ACC_2_ALIGN ALIGN_DEFAULT +#endif + +#if !defined(GYRO_1_SPI_INSTANCE) +#define GYRO_1_SPI_INSTANCE NULL +#endif + +#if !defined(GYRO_1_CS_PIN) +#define GYRO_1_CS_PIN NONE +#endif + +#if !defined(GYRO_1_EXTI_PIN) +#define GYRO_1_EXTI_PIN NONE +#endif + +#if !defined(GYRO_1_ALIGN) +#define GYRO_1_ALIGN ALIGN_DEFAULT +#endif + +#if !defined(ACC_1_ALIGN) +#define ACC_1_ALIGN ALIGN_DEFAULT +#endif + +#if defined(MPU_ADDRESS) +#define GYRO_I2C_ADDRESS MPU_ADDRESS +#else +#define GYRO_I2C_ADDRESS 0 // AUTO +#endif + +#ifdef USE_MULTI_GYRO +#define MAX_GYRODEV_COUNT 2 +#else +#define MAX_GYRODEV_COUNT 1 +#endif + +#ifdef USE_VCP +#ifndef USB_DETECT_PIN +#define USB_DETECT_PIN NONE +#endif +#ifndef USB_MSC_BUTTON_PIN +#define USB_MSC_BUTTON_PIN NONE +#endif +#if !defined(MSC_BUTTON_IPU) +#define MSC_BUTTON_IPU true +#endif +#endif + +#ifdef USE_TIMER_MGMT +#ifndef MAX_TIMER_PINMAP_COUNT +#define MAX_TIMER_PINMAP_COUNT 21 // Largest known for F405RG (OMNINXT) +#endif +#endif + +#ifdef USE_SDCARD +#ifndef SDCARD_DETECT_PIN +#define SDCARD_DETECT_PIN NONE +#endif +#ifndef SDCARD_SPI_CS_PIN +#define SDCARD_SPI_CS_PIN NONE +#endif +#ifdef SDCARD_DETECT_INVERTED +#define SDCARD_DETECT_IS_INVERTED 1 +#else +#define SDCARD_DETECT_IS_INVERTED 0 +#endif +#ifdef USE_SDCARD_SPI +#ifndef SDCARD_SPI_INSTANCE +#define SDCARD_SPI_INSTANCE NULL +#endif +#endif +#endif + +#if defined(USE_UART1) || defined(USE_UART2) || defined(USE_UART3) || defined(USE_UART4) || defined(USE_UART5) || defined(USE_UART6) || defined(USE_UART7) || defined(USE_UART8) +#define USE_UART +#endif + +#ifdef USE_UART +#if defined(INVERTER_PIN_UART1) || defined(INVERTER_PIN_UART2) || defined(INVERTER_PIN_UART3) || defined(INVERTER_PIN_UART4) || defined(INVERTER_PIN_UART5) || defined(INVERTER_PIN_UART6) +#define USE_INVERTER +#endif +#endif diff --git a/src/main/target/common_osd_slave.h b/src/main/target/common_osd_slave.h deleted file mode 100644 index e85c1f523d..0000000000 --- a/src/main/target/common_osd_slave.h +++ /dev/null @@ -1,68 +0,0 @@ -/* - * 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 - -#define USE_PARAMETER_GROUPS -// type conversion warnings. -// -Wconversion can be turned on to enable the process of eliminating these warnings -//#pragma GCC diagnostic warning "-Wconversion" -#pragma GCC diagnostic ignored "-Wsign-conversion" -// -Wpadded can be turned on to check padding of structs -//#pragma GCC diagnostic warning "-Wpadded" - -//#define SCHEDULER_DEBUG // define this to use scheduler debug[] values. Undefined by default for performance reasons -#define DEBUG_MODE DEBUG_NONE // change this to change initial debug mode - -#define I2C1_OVERCLOCK true -#define I2C2_OVERCLOCK true - -#define MAX_PROFILE_COUNT 0 - -#ifdef STM32F1 -#define MINIMAL_CLI -#endif - -#ifdef STM32F3 -#define MINIMAL_CLI -#endif - -#ifdef STM32F4 -#define I2C3_OVERCLOCK true -#endif - -#ifdef STM32F7 -#define I2C3_OVERCLOCK true -#define I2C4_OVERCLOCK true -#endif - -#if defined(STM32F4) || defined(STM32F7) -#define SCHEDULER_DELAY_LIMIT 10 -#else -#define SCHEDULER_DELAY_LIMIT 100 -#endif - -#define FAST_CODE -#define FAST_CODE_NOINLINE -#define FAST_RAM_ZERO_INIT -#define FAST_RAM - -//CLI needs FC dependencies removed before we can compile it, disabling for now -//#define USE_CLI diff --git a/src/main/target/common_fc_post.h b/src/main/target/common_post.h similarity index 62% rename from src/main/target/common_fc_post.h rename to src/main/target/common_post.h index 3c550aa1f8..a9c1c6ddfe 100644 --- a/src/main/target/common_fc_post.h +++ b/src/main/target/common_post.h @@ -56,10 +56,51 @@ #endif #endif -#if defined(USE_MSP_OVER_TELEMETRY) -#if !defined(USE_TELEMETRY_SMARTPORT) && !defined(USE_TELEMETRY_CRSF) -#undef USE_MSP_OVER_TELEMETRY +#ifndef USE_BARO +#undef USE_VARIO #endif + +#if !defined(USE_SERIAL_RX) +#undef USE_SERIALRX_CRSF +#undef USE_SERIALRX_IBUS +#undef USE_SERIALRX_JETIEXBUS +#undef USE_SERIALRX_SBUS +#undef USE_SERIALRX_SPEKTRUM +#undef USE_SERIALRX_SUMD +#undef USE_SERIALRX_SUMH +#undef USE_SERIALRX_XBUS +#undef USE_SERIALRX_FPORT +#endif + +#if !defined(USE_TELEMETRY) +#undef USE_CRSF_CMS_TELEMETRY +#undef USE_TELEMETRY_CRSF +#undef USE_TELEMETRY_FRSKY_HUB +#undef USE_TELEMETRY_HOTT +#undef USE_TELEMETRY_IBUS +#undef USE_TELEMETRY_IBUS_EXTENDED +#undef USE_TELEMETRY_JETIEXBUS +#undef USE_TELEMETRY_LTM +#undef USE_TELEMETRY_MAVLINK +#undef USE_TELEMETRY_SMARTPORT +#undef USE_TELEMETRY_SRXL +#undef USE_SERIALRX_FPORT +#endif + +#if !defined(USE_SERIALRX_CRSF) +#undef USE_TELEMETRY_CRSF +#endif + +#if !defined(USE_TELEMETRY_CRSF) || !defined(USE_CMS) +#undef USE_CRSF_CMS_TELEMETRY +#endif + +#if !defined(USE_SERIALRX_JETIEXBUS) +#undef USE_TELEMETRY_JETIEXBUS +#endif + +#if !defined(USE_TELEMETRY_IBUS) +#undef USE_TELEMETRY_IBUS_EXTENDED #endif // If USE_SERIALRX_SPEKTRUM was dropped by a target, drop all related options @@ -72,6 +113,15 @@ #undef USE_SPEKTRUM_VTX_CONTROL #undef USE_SPEKTRUM_VTX_TELEMETRY #undef USE_SPEKTRUM_CMS_TELEMETRY +#undef USE_TELEMETRY_SRXL +#endif + +#if defined(USE_SERIALRX_SBUS) || defined(USE_SERIALRX_FPORT) +#define USE_SBUS_CHANNELS +#endif + +#if !defined(USE_TELEMETRY_SMARTPORT) && !defined(USE_TELEMETRY_CRSF) +#undef USE_MSP_OVER_TELEMETRY #endif /* If either VTX_CONTROL or VTX_COMMON is undefined then remove common code and device drivers */ @@ -84,9 +134,15 @@ #if defined(USE_RX_FRSKY_SPI_D) || defined(USE_RX_FRSKY_SPI_X) #define USE_RX_CC2500 +#define USE_RX_CC2500_BIND #define USE_RX_FRSKY_SPI #endif +#if defined(USE_RX_SFHSS_SPI) +#define USE_RX_CC2500 +#define USE_RX_CC2500_BIND +#endif + // Burst dshot to default off if not configured explicitly by target #ifndef ENABLE_DSHOT_DMAR #define ENABLE_DSHOT_DMAR false @@ -97,7 +153,7 @@ #undef USE_ADC_INTERNAL #endif -#if !defined(USE_SDCARD) && !defined(USE_FLASHFS) +#if (!defined(USE_SDCARD) && !defined(USE_FLASHFS)) || !(defined(STM32F4) || defined(STM32F7)) #undef USE_USB_MSC #endif @@ -122,3 +178,25 @@ #if defined(USE_FLASH_M25P16) #define USE_FLASH #endif + +#if defined(USE_MAX7456) +#define USE_OSD +#endif + +#if defined(USE_GPS_RESCUE) +#define USE_GPS +#endif + +// Generate USE_SPI_GYRO or USE_I2C_GYRO +#if defined(USE_GYRO_L3G4200D) || defined(USE_GYRO_L3GD20) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6000) || defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU6500) +#define USE_I2C_GYRO +#endif + +#if defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250) +#define USE_SPI_GYRO +#endif + +// CX10 is a special case of SPI RX which requires XN297 +#if defined(USE_RX_CX10) +#define USE_RX_XN297 +#endif diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_pre.h similarity index 96% rename from src/main/target/common_fc_pre.h rename to src/main/target/common_pre.h index 9ff5097faa..9f599536f7 100644 --- a/src/main/target/common_fc_pre.h +++ b/src/main/target/common_pre.h @@ -122,8 +122,12 @@ #endif #define USE_BRUSHED_ESC_AUTODETECT // Detect if brushed motors are connected and set defaults appropriately to avoid motors spinning on boot + #define USE_CLI +#define USE_SERIAL_PASSTHROUGH +#define USE_TASK_STATISTICS #define USE_GYRO_REGISTER_DUMP // Adds gyroregisters command to cli to dump configured register values +#define USE_IMU_CALC #define USE_PPM #define USE_PWM #define USE_SERIAL_RX @@ -146,7 +150,6 @@ #if (FLASH_SIZE > 64) #define USE_ACRO_TRAINER #define USE_BLACKBOX -#define USE_LED_STRIP #define USE_RESOURCE_MGMT #define USE_RUNAWAY_TAKEOFF // Runaway Takeoff Prevention (anti-taz) #define USE_SERVOS @@ -167,8 +170,6 @@ #define USE_HUFFMAN #define USE_MSP_DISPLAYPORT #define USE_MSP_OVER_TELEMETRY -#define USE_OSD -#define USE_OSD_OVER_MSP_DISPLAYPORT #define USE_PINIO #define USE_PINIOBOX #define USE_RCDEVICE @@ -189,6 +190,9 @@ #define USE_BOARD_INFO #define USE_SMART_FEEDFORWARD #define USE_THROTTLE_BOOST +#define USE_RC_SMOOTHING_FILTER +#define USE_ITERM_RELAX +#define USE_DYN_LPF #ifdef USE_SERIALRX_SPEKTRUM #define USE_SPEKTRUM_BIND @@ -208,6 +212,10 @@ #define USE_GPS_NMEA #define USE_GPS_UBLOX #define USE_GPS_RESCUE +#define USE_GYRO_DLPF_EXPERIMENTAL +#define USE_OSD +#define USE_OSD_OVER_MSP_DISPLAYPORT +#define USE_MULTI_GYRO #define USE_OSD_ADJUSTMENTS #define USE_SENSOR_NAMES #define USE_SERIALRX_JETIEXBUS @@ -217,7 +225,8 @@ #define USE_TELEMETRY_MAVLINK #define USE_UNCOMMON_MIXERS #define USE_SIGNATURE -#define USE_RC_SMOOTHING_FILTER -#define USE_ITERM_RELAX #define USE_ABSOLUTE_CONTROL +#define USE_HOTT_TEXTMODE +#define USE_LED_STRIP +#define USE_VARIO #endif diff --git a/src/main/target/link/stm32_flash_f722.ld b/src/main/target/link/stm32_flash_f722.ld index 47f235de21..b9e3041b99 100644 --- a/src/main/target/link/stm32_flash_f722.ld +++ b/src/main/target/link/stm32_flash_f722.ld @@ -13,10 +13,10 @@ ENTRY(Reset_Handler) /* -0x08000000 to 0x0807FFFF 512K full flash, -0x08000000 to 0x08003FFF 16K isr vector, startup code, -0x08004000 to 0x08007FFF 16K config, // FLASH_Sector_1 -0x08008000 to 0x0807FFFF 480K firmware, +0x08000000 to 0x0807FFFF 512K full flash +0x08000000 to 0x08003FFF 16K ISR vector +0x08004000 to 0x08007FFF 16K config +0x08008000 to 0x0807FFFF 480K firmware */ /* Specify the memory areas */ diff --git a/src/main/target/link/stm32_flash_f765.ld b/src/main/target/link/stm32_flash_f765.ld new file mode 100644 index 0000000000..f5faee1612 --- /dev/null +++ b/src/main/target/link/stm32_flash_f765.ld @@ -0,0 +1,51 @@ +/* +***************************************************************************** +** +** File : stm32_flash_f765.ld +** +** Abstract : Linker script for STM32F765xITx Device with +** 2048KByte FLASH, 512KByte RAM +** +***************************************************************************** +*/ + +/* Entry Point */ +ENTRY(Reset_Handler) + +/* +0x00000000 to 0x00003FFF 16K TCM RAM, + +0x08000000 to 0x081FFFFF 2048K full flash, +0x08000000 to 0x08007FFF 32K isr vector, startup code, +0x08008000 to 0x0800FFFF 32K config, +0x08010000 to 0x081FFFFF 1984K firmware, +*/ + +/* Specify the memory areas */ +MEMORY +{ + ITCM_RAM (rx) : ORIGIN = 0x00000000, LENGTH = 16K + + ITCM_FLASH (rx) : ORIGIN = 0x00200000, LENGTH = 32K + ITCM_FLASH_CONFIG (r) : ORIGIN = 0x00208000, LENGTH = 32K + ITCM_FLASH1 (rx) : ORIGIN = 0x00210000, LENGTH = 1984K + + AXIM_FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 32K + AXIM_FLASH_CONFIG (r) : ORIGIN = 0x08008000, LENGTH = 32K + AXIM_FLASH1 (rx) : ORIGIN = 0x08010000, LENGTH = 1984K + + DTCM_RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K + SRAM1 (rwx) : ORIGIN = 0x20020000, LENGTH = 368K + SRAM2 (rwx) : ORIGIN = 0x2007C000, LENGTH = 16K + MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K +} + +REGION_ALIAS("FLASH", AXIM_FLASH) +REGION_ALIAS("FLASH_CONFIG", AXIM_FLASH_CONFIG) +REGION_ALIAS("FLASH1", AXIM_FLASH1) + +REGION_ALIAS("STACKRAM", DTCM_RAM) +REGION_ALIAS("FASTRAM", DTCM_RAM) +REGION_ALIAS("RAM", DTCM_RAM) + +INCLUDE "stm32_flash_f7_split.ld" diff --git a/src/main/target/link/stm32_flash_f7_split.ld b/src/main/target/link/stm32_flash_f7_split.ld index e0bd39f5d0..f6312646d6 100644 --- a/src/main/target/link/stm32_flash_f7_split.ld +++ b/src/main/target/link/stm32_flash_f7_split.ld @@ -28,7 +28,7 @@ SECTIONS /* The startup code goes first into FLASH */ .isr_vector : { - . = ALIGN(4); + . = ALIGN(512); PROVIDE (isr_vector_table_base = .); KEEP(*(.isr_vector)) /* Startup code */ . = ALIGN(4); @@ -132,7 +132,6 @@ SECTIONS __sram2_start__ = _ssram2; *(.sram2) *(SORT_BY_ALIGNMENT(.sram2*)) - *(COMMON) . = ALIGN(4); _esram2 = .; /* define a global symbol at sram2 end */ diff --git a/src/main/target/system_stm32f4xx.c b/src/main/target/system_stm32f4xx.c index 45f14fa750..6346470f2d 100644 --- a/src/main/target/system_stm32f4xx.c +++ b/src/main/target/system_stm32f4xx.c @@ -505,7 +505,7 @@ void OverclockRebootIfNecessary(uint32_t overclockLevel) const pllConfig_t * const pll = overclockLevels + overclockLevel; // Reboot to adjust overclock frequency - if (SystemCoreClock != (pll->n / pll->p) * 1000000) { + if (SystemCoreClock != (pll->n / pll->p) * 1000000U) { currentOverclockLevel = overclockLevel; __disable_irq(); NVIC_SystemReset(); diff --git a/src/main/target/system_stm32f7xx.c b/src/main/target/system_stm32f7xx.c index bd22beadef..ef683a8550 100644 --- a/src/main/target/system_stm32f7xx.c +++ b/src/main/target/system_stm32f7xx.c @@ -100,19 +100,6 @@ * @{ */ -/************************* Miscellaneous Configuration ************************/ - -/*!< Uncomment the following line if you need to relocate your vector Table in - Internal SRAM. */ -/* #define VECT_TAB_SRAM */ -#ifdef CUSTOM_VECT_TAB_OFFSET -#define VECT_TAB_OFFSET CUSTOM_VECT_TAB_OFFSET -#else -#define VECT_TAB_OFFSET 0x00 /*!< Vector Table base offset field.*/ -#endif - /*This value must be a multiple of 0x200. */ -/******************************************************************************/ - /** * @} */ @@ -294,7 +281,7 @@ void OverclockRebootIfNecessary(uint32_t overclockLevel) const pllConfig_t * const pll = overclockLevels + overclockLevel; // Reboot to adjust overclock frequency - if (SystemCoreClock != (pll->n / pll->p) * 1000000) { + if (SystemCoreClock != (pll->n / pll->p) * 1000000U) { REQUEST_OVERCLOCK = REQUEST_OVERCLOCK_MAGIC_COOKIE; CURRENT_OVERCLOCK_LEVEL = overclockLevel; __disable_irq(); @@ -346,12 +333,15 @@ void SystemInit(void) /* Disable all interrupts */ RCC->CIR = 0x00000000; - /* Configure the Vector Table location add offset address ------------------*/ -#ifdef VECT_TAB_SRAM - SCB->VTOR = RAMDTCM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM */ -#else - SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH */ -#endif + /* Configure the Vector Table location add offset address ------------------*/ + extern uint8_t isr_vector_table_base; + const uint32_t vtorOffset = (uint32_t) &isr_vector_table_base; +#define VTOR_OFFSET_ALIGNMENT 0x200 + if (vtorOffset % VTOR_OFFSET_ALIGNMENT != 0) { + // ISR vector table base is not 512 byte aligned + while (1); + } + SCB->VTOR = vtorOffset; /* Enable I-Cache */ if (INSTRUCTION_CACHE_ENABLE) { diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index c882e924bb..e60643f87a 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -24,7 +24,7 @@ #include "platform.h" -#ifdef USE_TELEMETRY +#ifdef USE_TELEMETRY_CRSF #include "build/atomic.h" #include "build/build_config.h" @@ -40,6 +40,8 @@ #include "common/streambuf.h" #include "common/utils.h" +#include "cms/cms.h" + #include "drivers/nvic.h" #include "fc/config.h" @@ -47,6 +49,7 @@ #include "fc/runtime_config.h" #include "flight/imu.h" +#include "flight/position.h" #include "interface/crsf_protocol.h" @@ -129,7 +132,7 @@ static void crsfInitializeFrame(sbuf_t *dst) dst->ptr = crsfFrame; dst->end = ARRAYEND(crsfFrame); - sbufWriteU8(dst, CRSF_ADDRESS_BROADCAST); + sbufWriteU8(dst, CRSF_SYNC_BYTE); } static void crsfFinalize(sbuf_t *dst) @@ -177,10 +180,9 @@ void crsfFrameGps(sbuf_t *dst) sbufWriteU8(dst, CRSF_FRAMETYPE_GPS); sbufWriteU32BigEndian(dst, gpsSol.llh.lat); // CRSF and betaflight use same units for degrees sbufWriteU32BigEndian(dst, gpsSol.llh.lon); - sbufWriteU16BigEndian(dst, (gpsSol.groundSpeed * 36 + 5) / 10); // gpsSol.groundSpeed is in 0.1m/s + sbufWriteU16BigEndian(dst, (gpsSol.groundSpeed * 36 + 50) / 100); // gpsSol.groundSpeed is in cm/s sbufWriteU16BigEndian(dst, gpsSol.groundCourse * 10); // gpsSol.groundCourse is degrees * 10 - //Send real GPS altitude only if it's reliable (there's a GPS fix) - const uint16_t altitude = (STATE(GPS_FIX) ? gpsSol.llh.alt : 0) + 1000; + const uint16_t altitude = (constrain(getEstimatedAltitudeCm(), 0 * 100, 5000 * 100) / 100) + 1000; // constrain altitude from 0 to 5,000m sbufWriteU16BigEndian(dst, altitude); sbufWriteU8(dst, gpsSol.numSat); } @@ -198,7 +200,11 @@ void crsfFrameBatterySensor(sbuf_t *dst) // use sbufWrite since CRC does not include frame length sbufWriteU8(dst, CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); sbufWriteU8(dst, CRSF_FRAMETYPE_BATTERY_SENSOR); - sbufWriteU16BigEndian(dst, getBatteryVoltage()); // vbat is in units of 0.1V + if (telemetryConfig()->report_cell_voltage) { + sbufWriteU16BigEndian(dst, getBatteryAverageCellVoltage()); // vbat is in units of 0.1V + } else { + sbufWriteU16BigEndian(dst, getBatteryVoltage()); + } sbufWriteU16BigEndian(dst, getAmperage() / 10); const uint32_t mAhDrawn = getMAhDrawn(); const uint8_t batteryRemainingPercentage = calculateBatteryPercentageRemaining(); @@ -262,7 +268,7 @@ void crsfFrameFlightMode(sbuf_t *dst) // use same logic as OSD, so telemetry displays same flight text as OSD const char *flightMode = "ACRO"; - if (isAirmodeActive()) { + if (airmodeIsEnabled()) { flightMode = "AIR"; } if (FLIGHT_MODE(FAILSAFE_MODE)) { @@ -426,11 +432,19 @@ void initCrsfTelemetry(void) // and feature is enabled, if so, set CRSF telemetry enabled crsfTelemetryEnabled = crsfRxIsActive(); + if (!crsfTelemetryEnabled) { + return; + } + deviceInfoReplyPending = false; #if defined(USE_MSP_OVER_TELEMETRY) mspReplyPending = false; #endif +#if defined(USE_CMS) && defined(USE_CRSF_CMS_TELEMETRY) + cmsDisplayPortRegister(displayPortCrsfInit()); +#endif + int index = 0; if (sensors(SENSOR_ACC)) { crsfSchedule[index++] = BV(CRSF_FRAME_ATTITUDE_INDEX); @@ -439,11 +453,12 @@ void initCrsfTelemetry(void) crsfSchedule[index++] = BV(CRSF_FRAME_BATTERY_SENSOR_INDEX); } crsfSchedule[index++] = BV(CRSF_FRAME_FLIGHT_MODE_INDEX); - if (feature(FEATURE_GPS)) { +#ifdef USE_GPS + if (featureIsEnabled(FEATURE_GPS)) { crsfSchedule[index++] = BV(CRSF_FRAME_GPS_INDEX); } +#endif crsfScheduleCount = (uint8_t)index; - } bool checkCrsfTelemetryState(void) diff --git a/src/main/telemetry/frsky_hub.c b/src/main/telemetry/frsky_hub.c index 37f8ca3ef8..0825984a0f 100644 --- a/src/main/telemetry/frsky_hub.c +++ b/src/main/telemetry/frsky_hub.c @@ -29,7 +29,7 @@ #include "platform.h" -#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_FRSKY_HUB) +#if defined(USE_TELEMETRY_FRSKY_HUB) #include "common/maths.h" #include "common/axis.h" @@ -177,13 +177,13 @@ static void frSkyHubWriteByteInternal(const char data) static void sendAccel(void) { for (unsigned i = 0; i < 3; i++) { - frSkyHubWriteFrame(ID_ACC_X + i, ((int16_t)(acc.accADC[i] / acc.dev.acc_1G) * 1000)); + frSkyHubWriteFrame(ID_ACC_X + i, ((int16_t)(acc.accADC[i] * acc.dev.acc_1G_rec) * 1000)); } } static void sendThrottleOrBatterySizeAsRpm(void) { - int16_t data; + int16_t data = 0; #if defined(USE_ESC_SENSOR) escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED); if (escData) { @@ -193,7 +193,7 @@ static void sendThrottleOrBatterySizeAsRpm(void) if (ARMING_FLAG(ARMED)) { const throttleStatus_e throttleStatus = calculateThrottleStatus(); uint16_t throttleForRPM = rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER; - if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)) { + if (throttleStatus == THROTTLE_LOW && featureIsEnabled(FEATURE_MOTOR_STOP)) { throttleForRPM = 0; } data = throttleForRPM; @@ -207,7 +207,7 @@ static void sendThrottleOrBatterySizeAsRpm(void) static void sendTemperature1(void) { - int16_t data; + int16_t data = 0; #if defined(USE_ESC_SENSOR) escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED); if (escData) { @@ -269,14 +269,14 @@ static void sendLatLong(int32_t coord[2]) #if defined(USE_GPS) static void sendGpsAltitude(void) { - uint16_t altitude = gpsSol.llh.alt; + int32_t altitudeCm = gpsSol.llh.altCm; // Send real GPS altitude only if it's reliable (there's a GPS fix) if (!STATE(GPS_FIX)) { - altitude = 0; + altitudeCm = 0; } - frSkyHubWriteFrame(ID_GPS_ALTIDUTE_BP, altitude); - frSkyHubWriteFrame(ID_GPS_ALTIDUTE_AP, 0); + frSkyHubWriteFrame(ID_GPS_ALTIDUTE_BP, altitudeCm / 100); // meters: integer part, eg. 123 from 123.45m + frSkyHubWriteFrame(ID_GPS_ALTIDUTE_AP, altitudeCm % 100); // meters: fractional part, eg. 45 from 123.45m } static void sendSatalliteSignalQualityAsTemperature2(uint8_t cycleNum) @@ -340,17 +340,6 @@ static void sendGPSLatLong(void) #endif #endif -#if defined(USE_BARO) || defined(USE_RANGEFINDER) -/* - * Send vertical speed for opentx. ID_VERT_SPEED - * Unit is cm/s - */ -static void sendVario(void) -{ - frSkyHubWriteFrame(ID_VERT_SPEED, getEstimatedVario()); -} -#endif - /* * Send voltage via ID_VOLT * @@ -535,23 +524,27 @@ void processFrSkyHubTelemetry(timeUs_t currentTimeUs) sendAccel(); } -#if defined(USE_BARO) || defined(USE_RANGEFINDER) - if (sensors(SENSOR_BARO | SENSOR_RANGEFINDER)) { +#if defined(USE_BARO) || defined(USE_RANGEFINDER) || defined(USE_GPS) + if (sensors(SENSOR_BARO | SENSOR_RANGEFINDER) | sensors(SENSOR_GPS)) { // Sent every 125ms - sendVario(); + // Send vertical speed for opentx. ID_VERT_SPEED + // Unit is cm/s +#ifdef USE_VARIO + frSkyHubWriteFrame(ID_VERT_SPEED, getEstimatedVario()); +#endif // Sent every 500ms if ((cycleNum % 4) == 0) { - int16_t altitude = ABS(getEstimatedAltitude()); + int32_t altitudeCm = getEstimatedAltitudeCm(); /* Allow 5s to boot correctly othervise send zero to prevent OpenTX * sensor lost notifications after warm boot. */ if (frSkyHubLastCycleTime < DELAY_FOR_BARO_INITIALISATION_US) { - altitude = 0; + altitudeCm = 0; } - frSkyHubWriteFrame(ID_ALTITUDE_BP, altitude / 100); - frSkyHubWriteFrame(ID_ALTITUDE_AP, altitude % 100); + frSkyHubWriteFrame(ID_ALTITUDE_BP, altitudeCm / 100); // meters: integer part, eg. 123 from 123.45m + frSkyHubWriteFrame(ID_ALTITUDE_AP, altitudeCm % 100); // meters: fractional part, eg. 45 from 123.45m } } #endif diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index f756d3462b..0e9c6b77c9 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -26,6 +26,7 @@ * Carsten Giesen - cGiesen - Baseflight port * Oliver Bayer - oBayer - MultiWii-HoTT, HoTT reverse engineering * Adam Majerczyk - HoTT-for-ardupilot from which some information and ideas are borrowed. + * Scavanger & Ziege-One: CMS Textmode addon * * https://github.com/obayer/MultiWii-HoTT * https://github.com/oBayer/MultiHoTT-Module @@ -62,12 +63,13 @@ #include "platform.h" -#ifdef USE_TELEMETRY +#ifdef USE_TELEMETRY_HOTT #include "build/build_config.h" #include "build/debug.h" #include "common/axis.h" +#include "common/maths.h" #include "common/time.h" #include "drivers/serial.h" @@ -87,6 +89,15 @@ #include "telemetry/hott.h" #include "telemetry/telemetry.h" +#if defined (USE_HOTT_TEXTMODE) && defined (USE_CMS) +#include "scheduler/scheduler.h" +#include "io/displayport_hott.h" + +#define HOTT_TEXTMODE_TASK_PERIOD 1000 +#define HOTT_TEXTMODE_RX_SCHEDULE 5000 +#define HOTT_TEXTMODE_TX_DELAY_US 1000 +#endif + //#define HOTT_DEBUG #define HOTT_MESSAGE_PREPARATION_FREQUENCY_5_HZ ((1000 * 1000) / 5) @@ -94,6 +105,9 @@ #define HOTT_TX_DELAY_US 3000 #define MILLISECONDS_IN_A_SECOND 1000 +static uint32_t rxSchedule = HOTT_RX_SCHEDULE; +static uint32_t txDelayUs = HOTT_TX_DELAY_US; + static uint32_t lastHoTTRequestCheckAt = 0; static uint32_t lastMessagesPreparedAt = 0; static uint32_t lastHottAlarmSoundTime = 0; @@ -118,6 +132,20 @@ static portSharing_e hottPortSharing; static HOTT_GPS_MSG_t hottGPSMessage; static HOTT_EAM_MSG_t hottEAMMessage; +#if defined (USE_HOTT_TEXTMODE) && defined (USE_CMS) +static hottTextModeMsg_t hottTextModeMessage; +static bool textmodeIsAlive = false; +static int32_t telemetryTaskPeriod = 0; + +static void initialiseTextmodeMessage(hottTextModeMsg_t *msg) +{ + msg->start = HOTT_TEXTMODE_START; + msg->esc = HOTT_EAM_SENSOR_TEXT_ID; + msg->warning = 0; + msg->stop = HOTT_TEXTMODE_STOP; +} +#endif + static void initialiseEAMMessage(HOTT_EAM_MSG_t *msg, size_t size) { memset(msg, 0, size); @@ -151,6 +179,9 @@ static void initialiseMessages(void) #ifdef USE_GPS initialiseGPSMessage(&hottGPSMessage, sizeof(hottGPSMessage)); #endif +#if defined (USE_HOTT_TEXTMODE) && defined (USE_CMS) + initialiseTextmodeMessage(&hottTextModeMessage); +#endif } #ifdef USE_GPS @@ -206,12 +237,12 @@ void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage) hottGPSMessage->home_distance_L = GPS_distanceToHome & 0x00FF; hottGPSMessage->home_distance_H = GPS_distanceToHome >> 8; - uint16_t altitude = gpsSol.llh.alt; + int32_t altitudeM = gpsSol.llh.altCm / 100; if (!STATE(GPS_FIX)) { - altitude = getEstimatedAltitude() / 100; + altitudeM = getEstimatedAltitudeCm() / 100; } - const uint16_t hottGpsAltitude = (altitude) + HOTT_GPS_ALTITUDE_OFFSET; // gpsSol.llh.alt in m ; offset = 500 -> O m + const uint16_t hottGpsAltitude = constrain(altitudeM + HOTT_GPS_ALTITUDE_OFFSET, 0 , UINT16_MAX); // gpsSol.llh.alt in m ; offset = 500 -> O m hottGPSMessage->altitude_L = hottGpsAltitude & 0x00FF; hottGPSMessage->altitude_H = hottGpsAltitude >> 8; @@ -229,14 +260,20 @@ static inline void updateAlarmBatteryStatus(HOTT_EAM_MSG_t *hottEAMMessage) { if (shouldTriggerBatteryAlarmNow()) { lastHottAlarmSoundTime = millis(); - const batteryState_e batteryState = getBatteryState(); - if (batteryState == BATTERY_WARNING || batteryState == BATTERY_CRITICAL) { + const batteryState_e voltageState = getVoltageState(); + const batteryState_e consumptionState = getConsumptionState(); + if (voltageState == BATTERY_WARNING || voltageState == BATTERY_CRITICAL) { hottEAMMessage->warning_beeps = 0x10; hottEAMMessage->alarm_invers1 = HOTT_EAM_ALARM1_FLAG_BATTERY_1; - } else { + } + else if (consumptionState == BATTERY_WARNING || consumptionState == BATTERY_CRITICAL) { + hottEAMMessage->warning_beeps = 0x16; + hottEAMMessage->alarm_invers1 = HOTT_EAM_ALARM1_FLAG_MAH; + } + else { hottEAMMessage->warning_beeps = HOTT_EAM_ALARM1_FLAG_NONE; hottEAMMessage->alarm_invers1 = HOTT_EAM_ALARM1_FLAG_NONE; - } + } } } @@ -266,12 +303,13 @@ static inline void hottEAMUpdateBatteryDrawnCapacity(HOTT_EAM_MSG_t *hottEAMMess static inline void hottEAMUpdateAltitude(HOTT_EAM_MSG_t *hottEAMMessage) { - const uint16_t hottEamAltitude = (getEstimatedAltitude() / 100) + HOTT_EAM_OFFSET_HEIGHT; + const uint16_t hottEamAltitude = (getEstimatedAltitudeCm() / 100) + HOTT_EAM_OFFSET_HEIGHT; hottEAMMessage->altitude_L = hottEamAltitude & 0x00FF; hottEAMMessage->altitude_H = hottEamAltitude >> 8; } +#ifdef USE_VARIO static inline void hottEAMUpdateClimbrate(HOTT_EAM_MSG_t *hottEAMMessage) { const int32_t vario = getEstimatedVario(); @@ -279,6 +317,7 @@ static inline void hottEAMUpdateClimbrate(HOTT_EAM_MSG_t *hottEAMMessage) hottEAMMessage->climbrate_H = (30000 + vario) >> 8; hottEAMMessage->climbrate3s = 120 + (vario / 100); } +#endif void hottPrepareEAMResponse(HOTT_EAM_MSG_t *hottEAMMessage) { @@ -290,7 +329,9 @@ void hottPrepareEAMResponse(HOTT_EAM_MSG_t *hottEAMMessage) hottEAMUpdateCurrentMeter(hottEAMMessage); hottEAMUpdateBatteryDrawnCapacity(hottEAMMessage); hottEAMUpdateAltitude(hottEAMMessage); +#ifdef USE_VARIO hottEAMUpdateClimbrate(hottEAMMessage); +#endif } static void hottSerialWrite(uint8_t c) @@ -310,8 +351,17 @@ void freeHoTTTelemetryPort(void) void initHoTTTelemetry(void) { portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_HOTT); + + if (!portConfig) { + return; + } + hottPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_HOTT); +#if defined (USE_HOTT_TEXTMODE) && defined (USE_CMS) + hottDisplayportRegister(); +#endif + initialiseMessages(); } @@ -415,8 +465,90 @@ static void hottPrepareMessages(void) { #endif } +#if defined (USE_HOTT_TEXTMODE) && defined (USE_CMS) +static void hottTextmodeStart() +{ + // Increase menu speed + cfTaskInfo_t taskInfo; + getTaskInfo(TASK_TELEMETRY, &taskInfo); + telemetryTaskPeriod = taskInfo.desiredPeriod; + rescheduleTask(TASK_TELEMETRY, TASK_PERIOD_HZ(HOTT_TEXTMODE_TASK_PERIOD)); + + rxSchedule = HOTT_TEXTMODE_RX_SCHEDULE; + txDelayUs = HOTT_TEXTMODE_TX_DELAY_US; +} + +static void hottTextmodeStop() +{ + // Set back to avoid slow down of the FC + if (telemetryTaskPeriod > 0) { + rescheduleTask(TASK_TELEMETRY, telemetryTaskPeriod); + } + + rxSchedule = HOTT_RX_SCHEDULE; + txDelayUs = HOTT_TX_DELAY_US; +} + +bool hottTextmodeIsAlive() +{ + return textmodeIsAlive; +} + +void hottTextmodeGrab() +{ + hottTextModeMessage.esc = HOTT_EAM_SENSOR_TEXT_ID; +} + +void hottTextmodeExit() +{ + hottTextModeMessage.esc = HOTT_TEXTMODE_ESC; +} + +void hottTextmodeWriteChar(uint8_t column, uint8_t row, char c) +{ + if (column < HOTT_TEXTMODE_DISPLAY_COLUMNS && row < HOTT_TEXTMODE_DISPLAY_ROWS) { + if (hottTextModeMessage.txt[row][column] != c) + hottTextModeMessage.txt[row][column] = c; + } +} + +static void processHottTextModeRequest(const uint8_t cmd) +{ + static bool setEscBack = false; + + if (!textmodeIsAlive) { + hottTextmodeStart(); + textmodeIsAlive = true; + } + + if ((cmd & 0xF0) != HOTT_EAM_SENSOR_TEXT_ID) { + return; + } + + if (setEscBack) { + hottTextModeMessage.esc = HOTT_EAM_SENSOR_TEXT_ID; + setEscBack = false; + } + + if (hottTextModeMessage.esc != HOTT_TEXTMODE_ESC) { + hottCmsOpen(); + } else { + setEscBack = true; + } + + hottSetCmsKey(cmd & 0x0f, hottTextModeMessage.esc == HOTT_TEXTMODE_ESC); + hottSendResponse((uint8_t *)&hottTextModeMessage, sizeof(hottTextModeMessage)); +} +#endif + static void processBinaryModeRequest(uint8_t address) { +#if defined (USE_HOTT_TEXTMODE) && defined (USE_CMS) + if (textmodeIsAlive) { + hottTextmodeStop(); + textmodeIsAlive = false; + } +#endif #ifdef HOTT_DEBUG static uint8_t hottBinaryRequests = 0; @@ -474,7 +606,7 @@ static void hottCheckSerialData(uint32_t currentMicros) lookingForRequest = false; return; } else { - bool enoughTimePassed = currentMicros - lastHoTTRequestCheckAt >= HOTT_RX_SCHEDULE; + bool enoughTimePassed = currentMicros - lastHoTTRequestCheckAt >= rxSchedule; if (!enoughTimePassed) { return; @@ -495,6 +627,11 @@ static void hottCheckSerialData(uint32_t currentMicros) */ processBinaryModeRequest(address); } +#if defined (USE_HOTT_TEXTMODE) && defined (USE_CMS) + else if (requestId == HOTTV4_TEXT_MODE_REQUEST_ID) { + processHottTextModeRequest(address); + } +#endif } static void hottSendTelemetryData(void) { @@ -568,7 +705,7 @@ void handleHoTTTelemetry(timeUs_t currentTimeUs) return; if (hottIsSending) { - if (currentTimeUs - serialTimer < HOTT_TX_DELAY_US) { + if (currentTimeUs - serialTimer < txDelayUs) { return; } } diff --git a/src/main/telemetry/hott.h b/src/main/telemetry/hott.h index ba5bd39a16..e3be8aa7c2 100644 --- a/src/main/telemetry/hott.h +++ b/src/main/telemetry/hott.h @@ -23,7 +23,8 @@ * Dominic Clifton/Hydra * Carsten Giesen * Adam Majerczyk (majerczyk.adam@gmail.com) - * Texmode add-on by Michi (mamaretti32@gmail.com) + * Textmode add-on by Michi (mamaretti32@gmail.com) + * Scavanger & Ziege-One: CMS Textmode addon */ #pragma once @@ -35,12 +36,12 @@ #define HOTTV4_TEXT_MODE_REQUEST_ID 0x7f #define HOTTV4_BINARY_MODE_REQUEST_ID 0x80 -#define HOTTV4_BUTTON_DEC 0xEB -#define HOTTV4_BUTTON_INC 0xED -#define HOTTV4_BUTTON_SET 0xE9 -#define HOTTV4_BUTTON_NIL 0x0F -#define HOTTV4_BUTTON_NEXT 0xEE -#define HOTTV4_BUTTON_PREV 0xE7 +#define HOTTV4_BUTTON_DEC 0xB +#define HOTTV4_BUTTON_INC 0xD +#define HOTTV4_BUTTON_SET 0x9 +#define HOTTV4_BUTTON_NIL 0xF +#define HOTTV4_BUTTON_NEXT 0xE +#define HOTTV4_BUTTON_PREV 0x7 #define HOTT_EAM_OFFSET_HEIGHT 500 #define HOTT_EAM_OFFSET_M2S 72 @@ -105,18 +106,21 @@ typedef enum { #define HOTT_EAM_SENSOR_TEXT_ID 0xE0 // Electric Air Module ID #define HOTT_GPS_SENSOR_TEXT_ID 0xA0 // GPS Module ID - -#define HOTT_TEXTMODE_MSG_TEXT_LEN 168 +#define HOTT_TEXTMODE_DISPLAY_ROWS 8 +#define HOTT_TEXTMODE_DISPLAY_COLUMNS 21 +#define HOTT_TEXTMODE_START 0x7B +#define HOTT_TEXTMODE_STOP 0x7D +#define HOTT_TEXTMODE_ESC 0x01 //Text mode msgs type -struct HOTT_TEXTMODE_MSG { - uint8_t start_byte; //#01 constant value 0x7b - uint8_t fill1; //#02 constant value 0x00 - uint8_t warning_beeps;//#03 1=A 2=B ... - uint8_t msg_txt[HOTT_TEXTMODE_MSG_TEXT_LEN]; //#04 ASCII text to display to +typedef struct hottTextModeMsg_s { + uint8_t start; //#01 constant value 0x7b + uint8_t esc; //#02 Low byte: Sensor ID or 0x01 for escape + uint8_t warning; //#03 1=A 2=B ... + uint8_t txt[HOTT_TEXTMODE_DISPLAY_ROWS][HOTT_TEXTMODE_DISPLAY_COLUMNS]; //#04 ASCII text to display to // Bit 7 = 1 -> Inverse character display // Display 21x8 - uint8_t stop_byte; //#171 constant value 0x7d -}; + uint8_t stop; //#171 constant value 0x7d +} hottTextModeMsg_t; typedef struct HOTT_GAM_MSG_s { uint8_t start_byte; //#01 start uint8_t constant value 0x7c @@ -497,6 +501,13 @@ void initHoTTTelemetry(void); void configureHoTTTelemetryPort(void); void freeHoTTTelemetryPort(void); +#if defined (USE_HOTT_TEXTMODE) && defined (USE_CMS) +bool hottTextmodeIsAlive(); +void hottTextmodeGrab(); +void hottTextmodeExit(); +void hottTextmodeWriteChar(uint8_t column, uint8_t row, char c); +#endif + uint32_t getHoTTTelemetryProviderBaudRate(void); void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage); diff --git a/src/main/telemetry/ibus.c b/src/main/telemetry/ibus.c index 78d927c6fb..6c2bca0a96 100644 --- a/src/main/telemetry/ibus.c +++ b/src/main/telemetry/ibus.c @@ -33,7 +33,7 @@ #include "platform.h" -#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_IBUS) +#if defined(USE_TELEMETRY_IBUS) #include "common/axis.h" diff --git a/src/main/telemetry/ibus_shared.c b/src/main/telemetry/ibus_shared.c index 7f3dc9122c..70ebbc9791 100644 --- a/src/main/telemetry/ibus_shared.c +++ b/src/main/telemetry/ibus_shared.c @@ -31,16 +31,15 @@ #include #include #include -// #include +#include #include "platform.h" -//#include "common/utils.h" #include "telemetry/telemetry.h" #include "telemetry/ibus_shared.h" static uint16_t calculateChecksum(const uint8_t *ibusPacket); -#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_IBUS) +#if defined(USE_TELEMETRY_IBUS) #include "config/feature.h" #include "pg/pg.h" #include "pg/pg_ids.h" @@ -228,7 +227,7 @@ static uint16_t getRPM() if (ARMING_FLAG(ARMED)) { const throttleStatus_e throttleStatus = calculateThrottleStatus(); rpm = rcCommand[THROTTLE]; // / BLADE_NUMBER_DIVIDER; - if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)) rpm = 0; + if (throttleStatus == THROTTLE_LOW && featureIsEnabled(FEATURE_MOTOR_STOP)) rpm = 0; } else { rpm = (uint16_t)(batteryConfig()->batteryCapacity); // / BLADE_NUMBER_DIVIDER } @@ -241,27 +240,15 @@ static uint16_t getMode() if (FLIGHT_MODE(ANGLE_MODE)) { flightMode = 0; //Stab } - if (FLIGHT_MODE(BARO_MODE)) { - flightMode = 2; //AltHold - } if (FLIGHT_MODE(PASSTHRU_MODE)) { flightMode = 3; //Auto } if (FLIGHT_MODE(HEADFREE_MODE) || FLIGHT_MODE(MAG_MODE)) { flightMode = 4; //Guided! (there in no HEAD, MAG so use Guided) } - if (FLIGHT_MODE(GPS_HOLD_MODE) && FLIGHT_MODE(BARO_MODE)) { - flightMode = 5; //Loiter - } - if (FLIGHT_MODE(GPS_HOME_MODE)) { - flightMode = 6; //RTL - } if (FLIGHT_MODE(HORIZON_MODE)) { flightMode = 7; //Circle! (there in no horizon so use Circle) } - if (FLIGHT_MODE(GPS_HOLD_MODE)) { - flightMode = 8; //PosHold - } if (FLIGHT_MODE(FAILSAFE_MODE)) { flightMode = 9; //Land } @@ -270,7 +257,7 @@ static uint16_t getMode() static int16_t getACC(uint8_t index) { - return (int16_t)((acc.accADC[index] / acc.dev.acc_1G) * 1000); + return (int16_t)((acc.accADC[index] * acc.dev.acc_1G_rec) * 1000); } #if defined(USE_TELEMETRY_IBUS_EXTENDED) @@ -318,7 +305,7 @@ static bool setGPS(uint8_t sensorType, ibusTelemetry_s* value) value->int32 = gpsSol.llh.lon; break; case IBUS_SENSOR_TYPE_GPS_ALT: - value->int32 = (int32_t)gpsSol.llh.alt; + value->int32 = (int32_t)gpsSol.llh.altCm; break; case IBUS_SENSOR_TYPE_GROUND_SPEED: value->uint16 = gpsSol.groundSpeed; @@ -420,10 +407,10 @@ static void setValue(uint8_t* bufferPtr, uint8_t sensorType, uint8_t length) break; case IBUS_SENSOR_TYPE_VERTICAL_SPEED: case IBUS_SENSOR_TYPE_CLIMB_RATE: - if(sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) { - value.int16 = (int16_t)getEstimatedVario(); - } +#ifdef USE_VARIO + value.int16 = (int16_t) constrain(getEstimatedVario(), SHRT_MIN, SHRT_MAX); break; +#endif case IBUS_SENSOR_TYPE_ALT: case IBUS_SENSOR_TYPE_ALT_MAX: value.int32 = baro.BaroAlt; diff --git a/src/main/telemetry/jetiexbus.c b/src/main/telemetry/jetiexbus.c index 3f14cfe7bc..c27edc5b06 100644 --- a/src/main/telemetry/jetiexbus.c +++ b/src/main/telemetry/jetiexbus.c @@ -24,12 +24,12 @@ #include "platform.h" -#ifdef USE_SERIAL_RX -#ifdef USE_TELEMETRY +#if defined(USE_TELEMETRY_JETIEXBUS) #include "build/build_config.h" #include "build/debug.h" #include "fc/runtime_config.h" +#include "config/feature.h" #include "common/utils.h" #include "common/bitarray.h" @@ -38,17 +38,19 @@ #include "drivers/serial_uart.h" #include "drivers/time.h" + #include "flight/position.h" #include "flight/imu.h" -#include "pg/rx.h" - #include "io/serial.h" +#include "io/gps.h" + #include "rx/rx.h" #include "rx/jetiexbus.h" #include "sensors/battery.h" #include "sensors/sensors.h" +#include "sensors/acceleration.h" #include "telemetry/jetiexbus.h" #include "telemetry/telemetry.h" @@ -58,7 +60,7 @@ #define EXTEL_SYNC_LEN 1 #define EXTEL_CRC_LEN 1 #define EXTEL_HEADER_LEN 6 -#define EXTEL_MAX_LEN 28 +#define EXTEL_MAX_LEN 26 #define EXTEL_OVERHEAD (EXTEL_SYNC_LEN + EXTEL_HEADER_LEN + EXTEL_CRC_LEN) #define EXTEL_MAX_PAYLOAD (EXTEL_MAX_LEN - EXTEL_OVERHEAD) #define EXBUS_MAX_REQUEST_BUFFER_SIZE (EXBUS_OVERHEAD + EXTEL_MAX_LEN) @@ -114,16 +116,28 @@ typedef struct exBusSensor_s { // list of telemetry messages // after every 15 sensors a new header has to be inserted (e.g. "BF D2") const exBusSensor_t jetiExSensors[] = { - {"BF D1", "", EX_TYPE_DES, 0 }, // device descripton - {"Voltage", "V", EX_TYPE_14b, DECIMAL_MASK(1)}, - {"Current", "A", EX_TYPE_14b, DECIMAL_MASK(2)}, - {"Altitude", "m", EX_TYPE_14b, DECIMAL_MASK(2)}, - {"Capacity", "mAh", EX_TYPE_22b, DECIMAL_MASK(0)}, - {"Power", "W", EX_TYPE_22b, DECIMAL_MASK(1)}, - {"Roll angle", "\xB0", EX_TYPE_14b, DECIMAL_MASK(1)}, - {"Pitch angle", "\xB0", EX_TYPE_14b, DECIMAL_MASK(1)}, - {"Heading", "\xB0", EX_TYPE_14b, DECIMAL_MASK(1)}, - {"Vario", "m/s", EX_TYPE_22b, DECIMAL_MASK(2)} + {"BF D1", "", EX_TYPE_DES, 0 }, // device descripton + {"Voltage", "V", EX_TYPE_22b, DECIMAL_MASK(1)}, + {"Current", "A", EX_TYPE_22b, DECIMAL_MASK(2)}, + {"Altitude", "m", EX_TYPE_22b, DECIMAL_MASK(2)}, + {"Capacity", "mAh", EX_TYPE_22b, DECIMAL_MASK(0)}, + {"Power", "W", EX_TYPE_22b, DECIMAL_MASK(1)}, + {"Roll angle", "\xB0", EX_TYPE_22b, DECIMAL_MASK(1)}, + {"Pitch angle", "\xB0", EX_TYPE_22b, DECIMAL_MASK(1)}, + {"Heading", "\xB0", EX_TYPE_22b, DECIMAL_MASK(1)}, + {"Vario", "m/s", EX_TYPE_22b, DECIMAL_MASK(2)}, + {"GPS Sats", "", EX_TYPE_22b, DECIMAL_MASK(0)}, + {"GPS Long", "", EX_TYPE_GPS, DECIMAL_MASK(0)}, + {"GPS Lat", "", EX_TYPE_GPS, DECIMAL_MASK(0)}, + {"GPS Speed", "m/s", EX_TYPE_22b, DECIMAL_MASK(2)}, + {"GPS H-Distance", "m", EX_TYPE_22b, DECIMAL_MASK(0)}, + {"GPS H-Direction", "\xB0", EX_TYPE_22b, DECIMAL_MASK(1)}, + {"BF D2", "", EX_TYPE_DES, 0 }, // device descripton + {"GPS Heading", "\xB0", EX_TYPE_22b, DECIMAL_MASK(1)}, + {"GPS Altitude", "m", EX_TYPE_22b, DECIMAL_MASK(2)}, + {"G-Force X", "", EX_TYPE_22b, DECIMAL_MASK(3)}, + {"G-Force Y", "", EX_TYPE_22b, DECIMAL_MASK(3)}, + {"G-Force Z", "", EX_TYPE_22b, DECIMAL_MASK(3)} }; // after every 15 sensors increment the step by 2 (e.g. ...EX_VAL15, EX_VAL16 = 17) to skip the device description @@ -136,9 +150,27 @@ enum exSensors_e { EX_ROLL_ANGLE, EX_PITCH_ANGLE, EX_HEADING, - EX_VARIO + EX_VARIO, + EX_GPS_SATS, + EX_GPS_LONG, + EX_GPS_LAT, + EX_GPS_SPEED, + EX_GPS_DISTANCE_TO_HOME, + EX_GPS_DIRECTION_TO_HOME, + EX_GPS_HEADING = 17, + EX_GPS_ALTITUDE, + EX_GFORCE_X, + EX_GFORCE_Y, + EX_GFORCE_Z }; +union{ + int32_t vInt; + uint16_t vWord[2]; + char vBytes[4]; +} exGps; + + #define JETI_EX_SENSOR_COUNT (ARRAYLEN(jetiExSensors)) static uint8_t jetiExBusTelemetryFrame[40]; @@ -160,6 +192,29 @@ uint8_t calcCRC8(uint8_t *pt, uint8_t msgLen) return(crc); } +void enableGpsTelemetry(bool enable) +{ + if (enable) { + bitArraySet(&exSensorEnabled, EX_GPS_SATS); + bitArraySet(&exSensorEnabled, EX_GPS_LONG); + bitArraySet(&exSensorEnabled, EX_GPS_LAT); + bitArraySet(&exSensorEnabled, EX_GPS_SPEED); + bitArraySet(&exSensorEnabled, EX_GPS_DISTANCE_TO_HOME); + bitArraySet(&exSensorEnabled, EX_GPS_DIRECTION_TO_HOME); + bitArraySet(&exSensorEnabled, EX_GPS_HEADING); + bitArraySet(&exSensorEnabled, EX_GPS_ALTITUDE); + } else { + bitArrayClr(&exSensorEnabled, EX_GPS_SATS); + bitArrayClr(&exSensorEnabled, EX_GPS_LONG); + bitArrayClr(&exSensorEnabled, EX_GPS_LAT); + bitArrayClr(&exSensorEnabled, EX_GPS_SPEED); + bitArrayClr(&exSensorEnabled, EX_GPS_DISTANCE_TO_HOME); + bitArrayClr(&exSensorEnabled, EX_GPS_DIRECTION_TO_HOME); + bitArrayClr(&exSensorEnabled, EX_GPS_HEADING); + bitArrayClr(&exSensorEnabled, EX_GPS_ALTITUDE); + } +} + /* * ----------------------------------------------- * Jeti Ex Bus Telemetry @@ -182,7 +237,6 @@ void initJetiExBusTelemetry(void) jetiExTelemetryFrame[EXTEL_HEADER_LSN_HB] = 0x00; jetiExTelemetryFrame[EXTEL_HEADER_RES] = 0x00; // reserved, by default 0x00 - //exSensorEnabled = 0x3fe; // Check which sensors are available if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE) { bitArraySet(&exSensorEnabled, EX_VOLTAGE); @@ -196,15 +250,23 @@ void initJetiExBusTelemetry(void) } if (sensors(SENSOR_BARO)) { bitArraySet(&exSensorEnabled, EX_ALTITUDE); +#ifdef USE_VARIO bitArraySet(&exSensorEnabled, EX_VARIO); +#endif } if (sensors(SENSOR_ACC)) { bitArraySet(&exSensorEnabled, EX_ROLL_ANGLE); bitArraySet(&exSensorEnabled, EX_PITCH_ANGLE); + bitArraySet(&exSensorEnabled, EX_GFORCE_X); + bitArraySet(&exSensorEnabled, EX_GFORCE_Y); + bitArraySet(&exSensorEnabled, EX_GFORCE_Z); } if (sensors(SENSOR_MAG)) { bitArraySet(&exSensorEnabled, EX_HEADING); } + + enableGpsTelemetry(featureIsEnabled(FEATURE_GPS)); + firstActiveSensor = getNextActiveSensor(0); // find the first active sensor } @@ -224,19 +286,35 @@ void createExTelemetryTextMessage(uint8_t *exMessage, uint8_t messageID, const e exMessage[exMessage[EXTEL_HEADER_TYPE_LEN] + EXTEL_CRC_LEN] = calcCRC8(&exMessage[EXTEL_HEADER_TYPE_LEN], exMessage[EXTEL_HEADER_TYPE_LEN]); } +uint32_t calcGpsDDMMmmm(int32_t value, bool isLong) +{ + uint32_t absValue = ABS(value); + uint16_t deg16 = absValue / GPS_DEGREES_DIVIDER; + uint16_t min16 = (absValue - deg16 * GPS_DEGREES_DIVIDER) * 6 / 1000; + + exGps.vInt = 0; + exGps.vWord[0] = min16; + exGps.vWord[1] = deg16; + exGps.vWord[1] |= isLong ? 0x2000 : 0; + exGps.vWord[1] |= (value < 0) ? 0x4000 : 0; + + return exGps.vInt; +} + + int32_t getSensorValue(uint8_t sensor) { - switch(sensor) { + switch (sensor) { case EX_VOLTAGE: - return getBatteryVoltageLatest(); + return getBatteryVoltage(); break; case EX_CURRENT: - return getAmperageLatest(); + return getAmperage(); break; case EX_ALTITUDE: - return getEstimatedAltitude(); + return getEstimatedAltitudeCm(); break; case EX_CAPACITY: @@ -244,7 +322,7 @@ int32_t getSensorValue(uint8_t sensor) break; case EX_POWER: - return (getBatteryVoltageLatest() * getAmperageLatest() / 100); + return (getBatteryVoltage() * getAmperage() / 100); break; case EX_ROLL_ANGLE: @@ -259,9 +337,55 @@ int32_t getSensorValue(uint8_t sensor) return attitude.values.yaw; break; +#ifdef USE_VARIO case EX_VARIO: return getEstimatedVario(); break; +#endif + + case EX_GPS_SATS: + return gpsSol.numSat; + break; + + case EX_GPS_LONG: + return calcGpsDDMMmmm(gpsSol.llh.lon, true); + break; + + case EX_GPS_LAT: + return calcGpsDDMMmmm(gpsSol.llh.lat, false); + break; + + case EX_GPS_SPEED: + return gpsSol.groundSpeed; + break; + + case EX_GPS_DISTANCE_TO_HOME: + return GPS_distanceToHome; + break; + + case EX_GPS_DIRECTION_TO_HOME: + return GPS_directionToHome; + break; + + case EX_GPS_HEADING: + return gpsSol.groundCourse; + break; + + case EX_GPS_ALTITUDE: + return gpsSol.llh.altCm; + break; + + case EX_GFORCE_X: + return (int16_t)(((float)acc.accADC[0] / acc.dev.acc_1G) * 1000); + break; + + case EX_GFORCE_Y: + return (int16_t)(((float)acc.accADC[1] / acc.dev.acc_1G) * 1000); + break; + + case EX_GFORCE_Z: + return (int16_t)(((float)acc.accADC[2] / acc.dev.acc_1G) * 1000); + break; default: return -1; @@ -289,7 +413,7 @@ uint8_t createExTelemetryValueMessage(uint8_t *exMessage, uint8_t item) uint8_t messageSize; uint32_t sensorValue; - exMessage[EXTEL_HEADER_LSN_LB] = item & 0xF0; // Device ID + exMessage[EXTEL_HEADER_LSN_LB] = item & 0xF0; // Device ID uint8_t *p = &exMessage[EXTEL_HEADER_ID]; while (item < sensorItemMaxGroup) { @@ -303,7 +427,11 @@ uint8_t createExTelemetryValueMessage(uint8_t *exMessage, uint8_t item) sensorValue = sensorValue >> 8; iCount--; } - *p++ = (sensorValue & 0x9F) | jetiExSensors[item].decimals; + if (jetiExSensors[item].exDataType != EX_TYPE_GPS) { + *p++ = (sensorValue & 0x9F) | jetiExSensors[item].decimals; + } else { + *p++ = sensorValue; + } item = getNextActiveSensor(item); @@ -359,9 +487,7 @@ void handleJetiExBusTelemetry(void) } if ((jetiExBusRequestFrame[EXBUS_HEADER_DATA_ID] == EXBUS_EX_REQUEST) && (jetiExBusCalcCRC16(jetiExBusRequestFrame, jetiExBusRequestFrame[EXBUS_HEADER_MSG_LEN]) == 0)) { - // switch to TX mode if (serialRxBytesWaiting(jetiExBusPort) == 0) { - serialSetMode(jetiExBusPort, MODE_TX); jetiExBusTransceiveState = EXBUS_TRANS_TX; item = sendJetiExBusTelemetry(jetiExBusRequestFrame[EXBUS_HEADER_PACKET_ID], item); jetiExBusRequestState = EXBUS_STATE_PROCESSED; @@ -376,7 +502,6 @@ void handleJetiExBusTelemetry(void) // check the state if transmit is ready if (jetiExBusTransceiveState == EXBUS_TRANS_IS_TX_COMPLETED) { if (isSerialTransmitBufferEmpty(jetiExBusPort)) { - serialSetMode(jetiExBusPort, MODE_RX); jetiExBusTransceiveState = EXBUS_TRANS_RX; jetiExBusRequestState = EXBUS_STATE_ZERO; } @@ -387,6 +512,7 @@ uint8_t sendJetiExBusTelemetry(uint8_t packetID, uint8_t item) { static uint8_t sensorDescriptionCounter = 0xFF; static uint8_t requestLoop = 0xFF; + static bool allSensorsActive = true; uint8_t *jetiExTelemetryFrame = &jetiExBusTelemetryFrame[EXBUS_HEADER_DATA]; if (requestLoop) { @@ -402,12 +528,23 @@ uint8_t sendJetiExBusTelemetry(uint8_t packetID, uint8_t item) createExTelemetryTextMessage(jetiExTelemetryFrame, sensorDescriptionCounter, &jetiExSensors[sensorDescriptionCounter]); createExBusMessage(jetiExBusTelemetryFrame, jetiExTelemetryFrame, packetID); requestLoop--; - if (requestLoop == 0){ + if (requestLoop == 0) { item = firstActiveSensor; + if (featureIsEnabled(FEATURE_GPS)) { + enableGpsTelemetry(false); + allSensorsActive = false; + } } } else { item = createExTelemetryValueMessage(jetiExTelemetryFrame, item); createExBusMessage(jetiExBusTelemetryFrame, jetiExTelemetryFrame, packetID); + + if (!allSensorsActive) { + if (sensors(SENSOR_GPS)) { + enableGpsTelemetry(true); + allSensorsActive = true; + } + } } serialWriteBuf(jetiExBusPort, jetiExBusTelemetryFrame, jetiExBusTelemetryFrame[EXBUS_HEADER_MSG_LEN]); @@ -415,7 +552,4 @@ uint8_t sendJetiExBusTelemetry(uint8_t packetID, uint8_t item) return item; } -#endif // TELEMETRY -#endif // SERIAL_RX - - +#endif diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index 17412e13fb..9eba4d8bb8 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -36,7 +36,7 @@ #include "platform.h" -#ifdef USE_TELEMETRY +#ifdef USE_TELEMETRY_LTM #include "build/build_config.h" @@ -148,9 +148,9 @@ static void ltm_gframe(void) ltm_serialise_8((uint8_t)(gpsSol.groundSpeed / 100)); #if defined(USE_BARO) || defined(USE_RANGEFINDER) - ltm_alt = (sensors(SENSOR_RANGEFINDER) || sensors(SENSOR_BARO)) ? getEstimatedAltitude() : gpsSol.llh.alt * 100; + ltm_alt = (sensors(SENSOR_RANGEFINDER) || sensors(SENSOR_BARO)) ? getEstimatedAltitudeCm() : gpsSol.llh.altCm; #else - ltm_alt = gpsSol.llh.alt * 100; + ltm_alt = gpsSol.llh.altCm; #endif ltm_serialise_32(ltm_alt); ltm_serialise_8((gpsSol.numSat << 2) | gps_fix_type); @@ -175,14 +175,8 @@ static void ltm_sframe(void) uint8_t lt_statemode; if (FLIGHT_MODE(PASSTHRU_MODE)) lt_flightmode = 0; - else if (FLIGHT_MODE(GPS_HOME_MODE)) - lt_flightmode = 13; - else if (FLIGHT_MODE(GPS_HOLD_MODE)) - lt_flightmode = 9; else if (FLIGHT_MODE(HEADFREE_MODE)) lt_flightmode = 4; - else if (FLIGHT_MODE(BARO_MODE)) - lt_flightmode = 8; else if (FLIGHT_MODE(ANGLE_MODE)) lt_flightmode = 2; else if (FLIGHT_MODE(HORIZON_MODE)) diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index ea8764f8d9..adcf980a6e 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -29,7 +29,7 @@ #include "platform.h" -#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_MAVLINK) +#if defined(USE_TELEMETRY_MAVLINK) #include "common/maths.h" #include "common/axis.h" @@ -136,6 +136,17 @@ static void mavlinkSerialWrite(uint8_t * buf, uint16_t length) serialWrite(mavlinkPort, buf[i]); } +static int16_t headingOrScaledMilliAmpereHoursDrawn(void) +{ + if (isAmperageConfigured() && telemetryConfig()->mavlink_mah_as_heading_divisor > 0) { + // In the Connex Prosight OSD, this goes between 0 and 999, so it will need to be scaled in that range. + return getMAhDrawn() / telemetryConfig()->mavlink_mah_as_heading_divisor; + } + // heading Current heading in degrees, in compass units (0..360, 0=north) + return DECIDEGREES_TO_DEGREES(attitude.values.yaw); +} + + void freeMAVLinkTelemetryPort(void) { closeSerialPort(mavlinkPort); @@ -317,7 +328,7 @@ void mavlinkSendPosition(void) // lon Longitude in 1E7 degrees gpsSol.llh.lon, // alt Altitude in 1E3 meters (millimeters) above MSL - gpsSol.llh.alt * 1000, + gpsSol.llh.altCm * 10, // eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 65535, // epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 @@ -340,12 +351,12 @@ void mavlinkSendPosition(void) // lon Longitude in 1E7 degrees gpsSol.llh.lon, // alt Altitude in 1E3 meters (millimeters) above MSL - gpsSol.llh.alt * 1000, + gpsSol.llh.altCm * 10, // relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters) #if defined(USE_BARO) || defined(USE_RANGEFINDER) - (sensors(SENSOR_RANGEFINDER) || sensors(SENSOR_BARO)) ? getEstimatedAltitude() * 10 : gpsSol.llh.alt * 1000, + (sensors(SENSOR_RANGEFINDER) || sensors(SENSOR_BARO)) ? getEstimatedAltitudeCm() * 10 : gpsSol.llh.altCm * 10, #else - gpsSol.llh.alt * 1000, + gpsSol.llh.altCm * 10, #endif // Ground X Speed (Latitude), expressed as m/s * 100 0, @@ -354,7 +365,7 @@ void mavlinkSendPosition(void) // Ground Z Speed (Altitude), expressed as m/s * 100 0, // heading Current heading in degrees, in compass units (0..360, 0=north) - DECIDEGREES_TO_DEGREES(attitude.values.yaw) + headingOrScaledMilliAmpereHoursDrawn() ); msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg); mavlinkSerialWrite(mavBuffer, msgLength); @@ -412,18 +423,18 @@ void mavlinkSendHUDAndHeartbeat(void) #if defined(USE_BARO) || defined(USE_RANGEFINDER) if (sensors(SENSOR_RANGEFINDER) || sensors(SENSOR_BARO)) { // Baro or sonar generally is a better estimate of altitude than GPS MSL altitude - mavAltitude = getEstimatedAltitude() / 100.0; + mavAltitude = getEstimatedAltitudeCm() / 100.0; } #if defined(USE_GPS) else if (sensors(SENSOR_GPS)) { // No sonar or baro, just display altitude above MLS - mavAltitude = gpsSol.llh.alt; + mavAltitude = gpsSol.llh.altCm / 100.0; } #endif #elif defined(USE_GPS) if (sensors(SENSOR_GPS)) { // No sonar or baro, just display altitude above MLS - mavAltitude = gpsSol.llh.alt; + mavAltitude = gpsSol.llh.altCm / 100.0; } #endif @@ -433,7 +444,7 @@ void mavlinkSendHUDAndHeartbeat(void) // groundspeed Current ground speed in m/s mavGroundSpeed, // heading Current heading in degrees, in compass units (0..360, 0=north) - DECIDEGREES_TO_DEGREES(attitude.values.yaw), + headingOrScaledMilliAmpereHoursDrawn(), // throttle Current throttle setting in integer percent, 0 to 100 scaleRange(constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX), PWM_RANGE_MIN, PWM_RANGE_MAX, 0, 100), // alt Current altitude (MSL), in meters, if we have sonar or baro use them, otherwise use GPS (less accurate) @@ -491,12 +502,6 @@ void mavlinkSendHUDAndHeartbeat(void) mavCustomMode = 0; //Stabilize mavModes |= MAV_MODE_FLAG_STABILIZE_ENABLED; } - if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(RANGEFINDER_MODE)) - mavCustomMode = 2; //Alt Hold - if (FLIGHT_MODE(GPS_HOME_MODE)) - mavCustomMode = 6; //Return to Launch - if (FLIGHT_MODE(GPS_HOLD_MODE)) - mavCustomMode = 16; //Position Hold (Earlier called Hybrid) uint8_t mavSystemState = 0; if (ARMING_FLAG(ARMED)) { diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 59c3a5a29d..acb0a90e8d 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -30,7 +30,7 @@ #include "platform.h" -#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SMARTPORT) +#if defined(USE_TELEMETRY_SMARTPORT) #include "common/axis.h" #include "common/color.h" @@ -167,14 +167,13 @@ typedef struct frSkyTableInfo_s { uint8_t index; } frSkyTableInfo_t; -static frSkyTableInfo_t frSkyDataIdTableInfo = {frSkyDataIdTable, 0, 0}; +static frSkyTableInfo_t frSkyDataIdTableInfo = { frSkyDataIdTable, 0, 0 }; #ifdef USE_ESC_SENSOR -#define ESC_DATAID_COUNT sizeof(frSkyEscDataIdTable)/sizeof(uint16_t) +#define ESC_DATAID_COUNT ( sizeof(frSkyEscDataIdTable) / sizeof(uint16_t) ) static frSkyTableInfo_t frSkyEscDataIdTableInfo = {frSkyEscDataIdTable, ESC_DATAID_COUNT, 0}; #endif -#define __USE_C99_MATH // for roundf() #define SMARTPORT_BAUD 57600 #define SMARTPORT_UART_MODE MODE_RXTX #define SMARTPORT_SERVICE_TIMEOUT_MS 1 // max allowed time to find a value to send @@ -263,6 +262,7 @@ smartPortPayload_t *smartPortDataReceive(uint16_t c, bool *clearToSend, smartPor checksum += c; checksum = (checksum & 0xFF) + (checksum >> 8); if (checksum == 0xFF) { + return (smartPortPayload_t *)&rxBuffer; } } @@ -317,6 +317,12 @@ static void smartPortSendPackage(uint16_t id, uint32_t val) smartPortWriteFrame(&payload); } +#ifdef USE_ESC_SENSOR +static bool reportExtendedEscSensors(void) { + return featureIsEnabled(FEATURE_ESC_SENSOR) && telemetryConfig()->smartport_use_extra_sensors; +} +#endif + #define ADD_SENSOR(dataId) frSkyDataIdTableInfo.table[frSkyDataIdTableInfo.index++] = dataId static void initSmartPortSensors(void) @@ -328,23 +334,23 @@ static void initSmartPortSensors(void) if (isBatteryVoltageConfigured()) { #ifdef USE_ESC_SENSOR - if (!feature(FEATURE_ESC_SENSOR)) { + if (!reportExtendedEscSensors()) #endif + { ADD_SENSOR(FSSP_DATAID_VFAS); -#ifdef USE_ESC_SENSOR } -#endif + ADD_SENSOR(FSSP_DATAID_A4); } if (isAmperageConfigured()) { #ifdef USE_ESC_SENSOR - if (!feature(FEATURE_ESC_SENSOR)) { + if (!reportExtendedEscSensors()) #endif + { ADD_SENSOR(FSSP_DATAID_CURRENT); -#ifdef USE_ESC_SENSOR } -#endif + ADD_SENSOR(FSSP_DATAID_FUEL); } @@ -361,7 +367,7 @@ static void initSmartPortSensors(void) } #ifdef USE_GPS - if (sensors(SENSOR_GPS)) { + if (featureIsEnabled(FEATURE_GPS)) { ADD_SENSOR(FSSP_DATAID_SPEED); ADD_SENSOR(FSSP_DATAID_LATLONG); ADD_SENSOR(FSSP_DATAID_LATLONG); // twice (one for lat, one for long) @@ -374,7 +380,7 @@ static void initSmartPortSensors(void) frSkyDataIdTableInfo.index = 0; #ifdef USE_ESC_SENSOR - if (feature(FEATURE_ESC_SENSOR)) { + if (reportExtendedEscSensors()) { frSkyEscDataIdTableInfo.size = ESC_DATAID_COUNT; } else { frSkyEscDataIdTableInfo.size = 0; @@ -472,6 +478,8 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear uint8_t *frameStart = (uint8_t *)&payload->valueId; smartPortMspReplyPending = handleMspFrame(frameStart, SMARTPORT_MSP_PAYLOAD_SIZE); } +#else + UNUSED(payload); #endif bool doRun = true; @@ -629,7 +637,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear break; #endif case FSSP_DATAID_ALTITUDE : - smartPortSendPackage(id, getEstimatedAltitude()); // unknown given unit, requested 100 = 1 meter + smartPortSendPackage(id, getEstimatedAltitudeCm()); // unknown given unit, requested 100 = 1 meter *clearToSend = false; break; case FSSP_DATAID_FUEL : @@ -645,15 +653,15 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear *clearToSend = false; break; case FSSP_DATAID_ACCX : - smartPortSendPackage(id, lrintf(100 * acc.accADC[X] / acc.dev.acc_1G)); // Multiply by 100 to show as x.xx g on Taranis + smartPortSendPackage(id, lrintf(100 * acc.accADC[X] * acc.dev.acc_1G_rec)); // Multiply by 100 to show as x.xx g on Taranis *clearToSend = false; break; case FSSP_DATAID_ACCY : - smartPortSendPackage(id, lrintf(100 * acc.accADC[Y] / acc.dev.acc_1G)); + smartPortSendPackage(id, lrintf(100 * acc.accADC[Y] * acc.dev.acc_1G_rec)); *clearToSend = false; break; case FSSP_DATAID_ACCZ : - smartPortSendPackage(id, lrintf(100 * acc.accADC[Z] / acc.dev.acc_1G)); + smartPortSendPackage(id, lrintf(100 * acc.accADC[Z] * acc.dev.acc_1G_rec)); *clearToSend = false; break; case FSSP_DATAID_T1 : @@ -673,46 +681,43 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear } else { tmpi += 2; } - if (ARMING_FLAG(ARMED)) + if (ARMING_FLAG(ARMED)) { tmpi += 4; + } - if (FLIGHT_MODE(ANGLE_MODE)) + if (FLIGHT_MODE(ANGLE_MODE)) { tmpi += 10; - if (FLIGHT_MODE(HORIZON_MODE)) + } + if (FLIGHT_MODE(HORIZON_MODE)) { tmpi += 20; - if (FLIGHT_MODE(UNUSED_MODE)) - tmpi += 40; - if (FLIGHT_MODE(PASSTHRU_MODE)) + } + if (FLIGHT_MODE(PASSTHRU_MODE)) { tmpi += 40; + } - if (FLIGHT_MODE(MAG_MODE)) + if (FLIGHT_MODE(MAG_MODE)) { tmpi += 100; - if (FLIGHT_MODE(BARO_MODE)) - tmpi += 200; - if (FLIGHT_MODE(RANGEFINDER_MODE)) - tmpi += 400; + } - if (FLIGHT_MODE(GPS_HOLD_MODE)) - tmpi += 1000; - if (FLIGHT_MODE(GPS_HOME_MODE)) - tmpi += 2000; - if (FLIGHT_MODE(HEADFREE_MODE)) + if (FLIGHT_MODE(HEADFREE_MODE)) { tmpi += 4000; + } smartPortSendPackage(id, (uint32_t)tmpi); *clearToSend = false; break; case FSSP_DATAID_T2 : - if (sensors(SENSOR_GPS)) { #ifdef USE_GPS + if (sensors(SENSOR_GPS)) { // provide GPS lock status smartPortSendPackage(id, (STATE(GPS_FIX) ? 1000 : 0) + (STATE(GPS_FIX_HOME) ? 2000 : 0) + gpsSol.numSat); *clearToSend = false; -#endif - } else if (feature(FEATURE_GPS)) { + } else if (featureIsEnabled(FEATURE_GPS)) { smartPortSendPackage(id, 0); *clearToSend = false; - } else if (telemetryConfig()->pidValuesAsTelemetry) { + } else +#endif + if (telemetryConfig()->pidValuesAsTelemetry) { switch (t2Cnt) { case 0: tmp2 = currentPidProfile->pid[PID_ROLL].P; @@ -782,7 +787,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear break; case FSSP_DATAID_GPS_ALT : if (STATE(GPS_FIX)) { - smartPortSendPackage(id, gpsSol.llh.alt * 100); // given in 0.1m , requested in 10 = 1m (should be in mm, probably a bug in opentx, tested on 2.0.1.7) + smartPortSendPackage(id, gpsSol.llh.altCm * 10); // given in 0.01m , requested in 10 = 1m (should be in mm, probably a bug in opentx, tested on 2.0.1.7) *clearToSend = false; } break; @@ -807,25 +812,17 @@ static bool serialCheckQueueEmpty(void) void handleSmartPortTelemetry(void) { - static bool clearToSend = false; - static volatile timeUs_t lastTelemetryFrameReceivedUs; - static smartPortPayload_t *payload = NULL; - const uint32_t requestTimeout = millis() + SMARTPORT_SERVICE_TIMEOUT_MS; if (telemetryState == TELEMETRY_STATE_INITIALIZED_SERIAL && smartPortSerialPort) { + smartPortPayload_t *payload = NULL; + bool clearToSend = false; while (serialRxBytesWaiting(smartPortSerialPort) > 0 && !payload) { uint8_t c = serialRead(smartPortSerialPort); payload = smartPortDataReceive(c, &clearToSend, serialCheckQueueEmpty, true); - if (payload) { - lastTelemetryFrameReceivedUs = micros(); - } } - if (cmpTimeUs(micros(), lastTelemetryFrameReceivedUs) >= SMARTPORT_MIN_TELEMETRY_RESPONSE_DELAY_US) { processSmartPortTelemetry(payload, &clearToSend, &requestTimeout); - payload = NULL; - } } } #endif diff --git a/src/main/telemetry/srxl.c b/src/main/telemetry/srxl.c index 5e60b3fbdc..7eae1e1a2d 100644 --- a/src/main/telemetry/srxl.c +++ b/src/main/telemetry/srxl.c @@ -24,7 +24,7 @@ #include "platform.h" -#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SRXL) +#if defined(USE_TELEMETRY_SRXL) #include "build/version.h" @@ -76,13 +76,6 @@ static bool srxlTelemetryEnabled; static uint8_t srxlFrame[SRXL_FRAME_SIZE_MAX]; -static bool srxlTelemetryNow = false; - -void srxlCollectTelemetryNow(void) -{ - srxlTelemetryNow = true; -} - static void srxlInitializeFrame(sbuf_t *dst) { @@ -517,11 +510,8 @@ bool checkSrxlTelemetryState(void) */ void handleSrxlTelemetry(timeUs_t currentTimeUs) { - if (!srxlTelemetryNow) { - return; - } - - srxlTelemetryNow = false; - processSrxl(currentTimeUs); + if (srxlTelemetryBufferEmpty()) { + processSrxl(currentTimeUs); + } } #endif diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index b6f8a84b52..fce7bc8111 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -75,7 +75,9 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, IBUS_SENSOR_TYPE_TEMPERATURE, IBUS_SENSOR_TYPE_RPM_FLYSKY, IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE - } + }, + .smartport_use_extra_sensors = false, + .mavlink_mah_as_heading_divisor = 0, ); void telemetryInit(void) @@ -100,6 +102,9 @@ void telemetryInit(void) #endif #ifdef USE_TELEMETRY_CRSF initCrsfTelemetry(); +#if defined(USE_MSP_OVER_TELEMETRY) + initCrsfMspBuffer(); +#endif #endif #ifdef USE_TELEMETRY_SRXL initSrxlTelemetry(); @@ -109,7 +114,6 @@ void telemetryInit(void) #endif #if defined(USE_MSP_OVER_TELEMETRY) initSharedMsp(); - initCrsfMspBuffer(); #endif telemetryCheckState(); diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index 7f5fa4a7dd..dd58cb7a1a 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -53,6 +53,8 @@ typedef struct telemetryConfig_s { uint8_t pidValuesAsTelemetry; uint8_t report_cell_voltage; uint8_t flysky_sensors[IBUS_SENSOR_COUNT]; + uint8_t smartport_use_extra_sensors; + uint16_t mavlink_mah_as_heading_divisor; } telemetryConfig_t; PG_DECLARE(telemetryConfig_t, telemetryConfig); diff --git a/src/main/vcp_hal/usbd_cdc_interface.c b/src/main/vcp_hal/usbd_cdc_interface.c index fd9388f7d2..4d39821882 100644 --- a/src/main/vcp_hal/usbd_cdc_interface.c +++ b/src/main/vcp_hal/usbd_cdc_interface.c @@ -46,13 +46,15 @@ */ /* Includes ------------------------------------------------------------------*/ +#include "drivers/serial_usb_vcp.h" +#include "drivers/time.h" + #include "stm32f7xx_hal.h" #include "usbd_core.h" #include "usbd_desc.h" #include "usbd_cdc.h" #include "usbd_cdc_interface.h" #include "stdbool.h" -#include "drivers/time.h" /* Private typedef -----------------------------------------------------------*/ /* Private define ------------------------------------------------------------*/ @@ -82,8 +84,6 @@ uint8_t* rxBuffPtr = NULL; /* TIM handler declaration */ TIM_HandleTypeDef TimHandle; -/* USB handler declaration */ -extern USBD_HandleTypeDef USBD_Device; static void (*ctrlLineStateCb)(void *context, uint16_t ctrlLineState); static void *ctrlLineStateCbContext; diff --git a/src/main/vcp_hal/usbd_cdc_interface.h b/src/main/vcp_hal/usbd_cdc_interface.h index a4f3d80809..3674dc410f 100644 --- a/src/main/vcp_hal/usbd_cdc_interface.h +++ b/src/main/vcp_hal/usbd_cdc_interface.h @@ -50,6 +50,8 @@ #define __USBD_CDC_IF_H /* Includes ------------------------------------------------------------------*/ +#include "common/maths.h" + #include "usbd_cdc.h" #include "stm32f7xx_hal.h" #include "usbd_core.h" diff --git a/src/main/vcp_hal/usbd_conf.c b/src/main/vcp_hal/usbd_conf.c index 2258b13418..6c96763fbd 100644 --- a/src/main/vcp_hal/usbd_conf.c +++ b/src/main/vcp_hal/usbd_conf.c @@ -46,6 +46,8 @@ */ /* Includes ------------------------------------------------------------------*/ +#include "common/maths.h" + #include "stm32f7xx_hal.h" #include "usbd_core.h" #include "usbd_desc.h" diff --git a/src/main/vcpf4/usbd_cdc_vcp.c b/src/main/vcpf4/usbd_cdc_vcp.c index b9cdab30e6..40605aa614 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.c +++ b/src/main/vcpf4/usbd_cdc_vcp.c @@ -29,6 +29,8 @@ #include "stdbool.h" #include "drivers/time.h" +__ALIGN_BEGIN USB_OTG_CORE_HANDLE USB_OTG_dev __ALIGN_END; + LINE_CODING g_lc; extern __IO uint8_t USB_Tx_State; diff --git a/src/main/vcpf4/usbd_cdc_vcp.h b/src/main/vcpf4/usbd_cdc_vcp.h index 0a046ad1b7..fc27a6d98e 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.h +++ b/src/main/vcpf4/usbd_cdc_vcp.h @@ -34,7 +34,7 @@ #include "usbd_usr.h" #include "usbd_desc.h" -__ALIGN_BEGIN USB_OTG_CORE_HANDLE USB_OTG_dev __ALIGN_END; +extern USB_OTG_CORE_HANDLE USB_OTG_dev; uint32_t CDC_Send_DATA(const uint8_t *ptrBuffer, uint32_t sendLength); uint32_t CDC_Send_FreeBytes(void); diff --git a/src/test/Makefile b/src/test/Makefile index f7f89443b0..3f41de48a2 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -13,22 +13,27 @@ USER_DIR = ../main TEST_DIR = unit ROOT = ../.. +OBJECT_DIR = ../../obj/test +TARGET_DIR = $(USER_DIR)/target include $(ROOT)/make/system-id.mk +include $(ROOT)/make/targets_list.mk # specify which files that are included in the test in addition to the unittest file. # variables available: # _SRC # _DEFINES - +# _INCLUDE_DIRS +# _EXPAND (run for each target, call the above with target as $1) +# _BLACKLIST (targets to exclude from an expanded test's run) alignsensor_unittest_SRC := \ $(USER_DIR)/sensors/boardalignment.c \ $(USER_DIR)/common/maths.c arming_prevention_unittest_SRC := \ - $(USER_DIR)/fc/fc_core.c \ - $(USER_DIR)/fc/fc_dispatch.c \ + $(USER_DIR)/fc/core.c \ + $(USER_DIR)/fc/dispatch.c \ $(USER_DIR)/fc/rc_controls.c \ $(USER_DIR)/fc/rc_modes.c \ $(USER_DIR)/fc/runtime_config.c \ @@ -38,9 +43,12 @@ atomic_unittest_SRC := \ $(USER_DIR)/build/atomic.c \ $(TEST_DIR)/atomic_unittest_c.c -baro_bmp085_unittest_SRC := \ - $(USER_DIR)/drivers/barometer/barometer_bmp085.c \ - $(USER_DIR)/drivers/io.c +# This test is disabled due to build errors. +# Its source code is archived in unit/baro_bmp085_unittest.cc.txt +# +#baro_bmp085_unittest_SRC := \ +# $(USER_DIR)/drivers/barometer/barometer_bmp085.c \ +# $(USER_DIR)/drivers/io.c baro_bmp280_unittest_SRC := \ @@ -57,9 +65,12 @@ baro_ms5611_unittest_DEFINES := \ USE_BARO_MS5611 \ USE_BARO_SPI_MS5611 -battery_unittest_SRC := \ - $(USER_DIR)/sensors/battery.c \ - $(USER_DIR)/common/maths.c +# This test is disabled due to build errors. +# Its source code is archived in unit/battery_unittest.cc.txt +# +#battery_unittest_SRC := \ +# $(USER_DIR)/sensors/battery.c \ +# $(USER_DIR)/common/maths.c blackbox_unittest_SRC := \ @@ -217,7 +228,8 @@ sensor_gyro_unittest_SRC := \ $(USER_DIR)/common/maths.c \ $(USER_DIR)/drivers/accgyro/accgyro_fake.c \ $(USER_DIR)/drivers/accgyro/gyro_sync.c \ - $(USER_DIR)/pg/pg.c + $(USER_DIR)/pg/pg.c \ + $(USER_DIR)/pg/gyrodev.c telemetry_crsf_unittest_SRC := \ $(USER_DIR)/rx/crsf.c \ @@ -264,15 +276,26 @@ telemetry_ibus_unittest_SRC := \ $(USER_DIR)/telemetry/ibus_shared.c \ $(USER_DIR)/telemetry/ibus.c +timer_definition_unittest_EXPAND := yes + +# NERO and STM32F7X2 are universal targets with dynamic timer management. +# SITL is a simulator with empty timerHardware and many hearders in target.c. +timer_definition_unittest_BLACKLIST := NERO SITL STM32F7X2 + +timer_definition_unittest_SRC = \ + $(TARGET_DIR)/$(call get_base_target,$1)/target.c + +timer_definition_unittest_DEFINES = \ + TARGET=$1 \ + BASE_TARGET=$(call get_base_target,$1) + +timer_definition_unittest_INCLUDE_DIRS = \ + $(TEST_DIR)/timer_definition_unittest.include \ + $(TARGET_DIR)/$(call get_base_target,$1) transponder_ir_unittest_SRC := \ - $(USER_DIR)/drivers/transponder_ir_ilap.c \ - $(USER_DIR)/drivers/transponder_ir_arcitimer.c - - -type_conversion_unittest_SRC := \ - $(USER_DIR)/common/typeconversion.c - + $(USER_DIR)/drivers/transponder_ir_ilap.c \ + $(USER_DIR)/drivers/transponder_ir_arcitimer.c ws2811_unittest_SRC := \ $(USER_DIR)/drivers/light_ws2811strip.c @@ -289,7 +312,6 @@ rcdevice_unittest_SRC := \ $(USER_DIR)/common/bitarray.c \ $(USER_DIR)/fc/rc_modes.c \ $(USER_DIR)/io/rcdevice.c \ - $(USER_DIR)/io/rcdevice_osd.c \ $(USER_DIR)/io/rcdevice_cam.c \ pid_unittest_SRC := \ @@ -300,9 +322,31 @@ pid_unittest_SRC := \ $(USER_DIR)/pg/pg.c \ $(USER_DIR)/fc/runtime_config.c +pid_unittest_DEFINES := \ + USE_ITERM_RELAX \ + USE_RC_SMOOTHING_FILTER \ + USE_ABSOLUTE_CONTROL + rcdevice_unittest_DEFINES := \ USE_RCDEVICE +vtx_unittest_SRC := \ + $(USER_DIR)/fc/core.c \ + $(USER_DIR)/fc/dispatch.c \ + $(USER_DIR)/fc/rc_controls.c \ + $(USER_DIR)/fc/rc_modes.c \ + $(USER_DIR)/fc/runtime_config.c \ + $(USER_DIR)/drivers/vtx_common.c \ + $(USER_DIR)/io/vtx_control.c \ + $(USER_DIR)/io/vtx_string.c \ + $(USER_DIR)/io/vtx.c \ + $(USER_DIR)/common/bitarray.c + +vtx_unittest_DEFINES := \ + USE_VTX_COMMON \ + USE_VTX_CONTROL \ + USE_VTX_SMARTAUDIO + # Please tweak the following variable definitions as needed by your # project, except GTEST_HEADERS, which you can use in your own targets # but shouldn't modify. @@ -311,11 +355,6 @@ rcdevice_unittest_DEFINES := \ # Remember to tweak this if you move this file. GTEST_DIR = ../../lib/test/gtest - -USER_INCLUDE_DIR = $(USER_DIR) - -OBJECT_DIR = ../../obj/test - # Use clang/clang++ by default CC := clang CXX := clang++ @@ -366,8 +405,10 @@ LDFLAGS += -Wl,-T,$(TEST_DIR)/pg.ld -Wl,-Map,$(OBJECT_DIR)/$@.map endif # Gather up all of the tests. -TEST_SRC = $(sort $(wildcard $(TEST_DIR)/*.cc)) -TESTS = $(TEST_SRC:$(TEST_DIR)/%.cc=%) +TEST_SRCS = $(sort $(wildcard $(TEST_DIR)/*.cc)) +TEST_BASENAMES = $(TEST_SRCS:$(TEST_DIR)/%.cc=%) +TESTS = $(foreach test,$(TEST_BASENAMES),$(if $($(test)_EXPAND),$(foreach \ + target,$(filter-out $($(test)_BLACKLIST),$(VALID_TARGETS)),$(test).$(target)),$(test))) # All Google Test headers. Usually you shouldn't change this # definition. @@ -448,66 +489,119 @@ $(OBJECT_DIR)/gtest_main.a : $(OBJECT_DIR)/gtest-all.o $(OBJECT_DIR)/gtest_main. $(OBJECT_DIR)/gtest_main.d -# includes in test dir must override includes in user dir -TEST_INCLUDE_DIRS := $(TEST_DIR) \ - $(USER_INCLUDE_DIR) - -TEST_CFLAGS = $(addprefix -I,$(TEST_INCLUDE_DIRS)) - +# includes in test dir must override includes in user dir, unless the user +# specifies a list of endorsed directories in ${target}_INCLUDE_DIRS. +test_include_dirs = $1 $(TEST_DIR) $(USER_DIR) +test_cflags = $(addprefix -I,$(call test_include_dirs,$1)) +# target name extractor +# param $1 = expanded test name in the form of test.target +target = $(1:$(basename $1).%=%) # canned recipe for all test builds -# param $1 = testname +# +# variable expansion rules of thumb (number of $'s): +# * parameters: one $, e.g. $1 +# * statically accessed variables set elsewhere: one $, e.g. $(C_FLAGS) +# * dynamically accessed variables set elsewhere: one $, e.g. $($1_SRC) +# * make functions accessing only the above: one $, e.g. $(basename $1) +# * dynamically set and accessed variables: two $, e.g. $$($1_OBJS) +# * make functions accessing dynamically set variables: two $, +# e.g. $$(call test_cflags,$$($1_INCLUDE_DIRS)) +# +# param $1 = plain test name for global tests, test.target for per-target tests define test-specific-stuff -$$1_OBJS = $$(patsubst $$(TEST_DIR)%,$$(OBJECT_DIR)/$1%, $$(patsubst $$(USER_DIR)%,$$(OBJECT_DIR)/$1%,$$($1_SRC:=.o))) +ifeq ($1,$(basename $1)) +# standard global test +$1_OBJS = $(patsubst \ + $(TEST_DIR)/%,$(OBJECT_DIR)/$1/%,$(patsubst \ + $(USER_DIR)/%,$(OBJECT_DIR)/$1/%,$($1_SRC:=.o))) +else +# test executed for each target, $1 has the form of test.target +$1_SRC = $(addsuffix .o,$(call $(basename $1)_SRC,$(call target,$1))) +$1_OBJS = $$(patsubst \ + $(TEST_DIR)/%,$(OBJECT_DIR)/$1/%,$$(patsubst \ + $(USER_DIR)/%,$(OBJECT_DIR)/$1/%,$$(patsubst \ + $(TARGET_DIR)/$(call get_base_target,$(call target,$1))/%,$(OBJECT_DIR)/$1/%,$$($1_SRC)))) +$1_DEFINES = $(call $(basename $1)_DEFINES,$(call target,$1)) +$1_INCLUDE_DIRS = $(call $(basename $1)_INCLUDE_DIRS,$(call target,$1)) +endif # $$(info $1 -v-v-------) -# $$(info $1_SRC: $($1_SRC)) -# $$(info $1_OBJS: $$($$1_OBJS)) +# $$(info $1_SRC: $$($1_SRC)) +# $$(info $1_OBJS: $$($1_OBJS)) # $$(info $1 -^-^-------) - -#include generated dependencies --include $$($$1_OBJS:.o=.d) --include $(OBJECT_DIR)/$1/$1.d - +# include generated dependencies +-include $$($1_OBJS:.o=.d) +-include $(OBJECT_DIR)/$1/$(basename $1).d $(OBJECT_DIR)/$1/%.c.o: $(USER_DIR)/%.c @echo "compiling $$<" "$(STDOUT)" $(V1) mkdir -p $$(dir $$@) - $(V1) $(CC) $(C_FLAGS) $(TEST_CFLAGS) \ - $(foreach def,$($1_DEFINES),-D $(def)) \ + $(V1) $(CC) $(C_FLAGS) $$(call test_cflags,$$($1_INCLUDE_DIRS)) \ + $$(foreach def,$$($1_DEFINES),-D $$(def)) \ -c $$< -o $$@ $(OBJECT_DIR)/$1/%.c.o: $(TEST_DIR)/%.c @echo "compiling test c file: $$<" "$(STDOUT)" $(V1) mkdir -p $$(dir $$@) - $(V1) $(CC) $(C_FLAGS) $(TEST_CFLAGS) \ - $(foreach def,$($1_DEFINES),-D $(def)) \ + $(V1) $(CC) $(C_FLAGS) $$(call test_cflags,$$($1_INCLUDE_DIRS)) \ + $$(foreach def,$$($1_DEFINES),-D $$(def)) \ -c $$< -o $$@ -$(OBJECT_DIR)/$1/$1.o: $(TEST_DIR)/$1.cc +ifneq ($1,$(basename $1)) +# per-target tests may compile files from the target directory +$(OBJECT_DIR)/$1/%.c.o: $(TARGET_DIR)/$(call get_base_target,$(call target,$1))/%.c + @echo "compiling target c file: $$<" "$(STDOUT)" + $(V1) mkdir -p $$(dir $$@) + $(V1) $(CC) $(C_FLAGS) $$(call test_cflags,$$($1_INCLUDE_DIRS)) \ + $$(foreach def,$$($1_DEFINES),-D $$(def)) \ + -c $$< -o $$@ +endif + +$(OBJECT_DIR)/$1/$(basename $1).o: $(TEST_DIR)/$(basename $1).cc @echo "compiling $$<" "$(STDOUT)" $(V1) mkdir -p $$(dir $$@) - $(V1) $(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) \ - $(foreach def,$($1_DEFINES),-D $(def)) \ - -c $$< -o $$@ + $(V1) $(CXX) $(CXX_FLAGS) $$(call test_cflags,$$($1_INCLUDE_DIRS)) \ + $$(foreach def,$$($1_DEFINES),-D $$(def)) \ + -c $$< -o $$@ - -$(OBJECT_DIR)/$1/$1 : $$($$1_OBJS) \ - $(OBJECT_DIR)/$1/$1.o \ +$(OBJECT_DIR)/$1/$(basename $1): $$($1_OBJS) \ + $(OBJECT_DIR)/$1/$(basename $1).o \ $(OBJECT_DIR)/gtest_main.a @echo "linking $$@" "$(STDOUT)" $(V1) mkdir -p $(dir $$@) $(V1) $(CXX) $(CXX_FLAGS) $(LDFLAGS) $$^ -o $$@ -test_$1: $(OBJECT_DIR)/$1/$1 +test_$1: $(OBJECT_DIR)/$1/$(basename $1) $(V1) $$< $$(EXEC_OPTS) "$(STDOUT)" && echo "running $$@: PASS" endef -#apply the canned recipe above to all tests + $(eval $(foreach test,$(TESTS),$(call test-specific-stuff,$(test)))) + +$(foreach test,$(TESTS),$(if $($(basename $(test))_SRC),,$(error \ + Test 'unit/$(basename $(test)).cc' has no '$(basename $(test))_SRC' variable defined))) +$(foreach var,$(filter-out TARGET_SRC,$(filter %_SRC,$(.VARIABLES))),$(if $(filter $(var:_SRC=)%,$(TESTS)),,$(error \ + Variable '$(var)' has no 'unit/$(var:_SRC=).cc' test))) + + +target_list: + @echo ========== BASE TARGETS ========== + @echo $(BASE_TARGETS) + @echo ========== ALT TARGETS ========== + @echo $(ALT_TARGETS) + @echo ========== VALID_TARGETS ========== + @echo $(VALID_TARGETS) + @echo ========== BASE/ALT PAIRS ========== + @echo $(BASE_ALT_PAIRS) + @echo ========== ALT/BASE MAPPING ========== + @echo $(foreach target,$(ALT_TARGETS),$(target)\>$(call get_base_target,$(target))) + @echo ========== ALT/BASE FULL MAPPING ========== + @echo $(foreach target,$(VALID_TARGETS),$(target)\>$(call get_base_target,$(target))) + diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc index 86e1ea2aa9..a979c0d4fa 100644 --- a/src/test/unit/arming_prevention_unittest.cc +++ b/src/test/unit/arming_prevention_unittest.cc @@ -27,7 +27,7 @@ extern "C" { #include "pg/rx.h" #include "fc/config.h" #include "fc/controlrate_profile.h" - #include "fc/fc_core.h" + #include "fc/core.h" #include "fc/rc_controls.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" @@ -612,11 +612,55 @@ TEST(ArmingPreventionTest, WhenUsingSwitched3DModeThenNormalThrottleArmingCondit EXPECT_EQ(0, getArmingDisableFlags()); } +TEST(ArmingPreventionTest, ParalyzeOnAtBoot) +{ + // given + simulationFeatureFlags = 0; + simulationTime = 0; + gyroCalibDone = true; + + // and + modeActivationConditionsMutable(0)->auxChannelIndex = 0; + modeActivationConditionsMutable(0)->modeId = BOXARM; + modeActivationConditionsMutable(0)->range.startStep = CHANNEL_VALUE_TO_STEP(1750); + modeActivationConditionsMutable(0)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX); + modeActivationConditionsMutable(1)->auxChannelIndex = 1; + modeActivationConditionsMutable(1)->modeId = BOXPARALYZE; + modeActivationConditionsMutable(1)->range.startStep = CHANNEL_VALUE_TO_STEP(1750); + modeActivationConditionsMutable(1)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX); + useRcControlsConfig(NULL); + + // and + rxConfigMutable()->mincheck = 1050; + + // given + rcData[THROTTLE] = 1000; + rcData[AUX1] = 1000; + rcData[AUX2] = 1800; // Paralyze on at boot + ENABLE_STATE(SMALL_ANGLE); + + // when + updateActivatedModes(); + updateArmingStatus(); + + // expect + EXPECT_FALSE(ARMING_FLAG(ARMED)); + EXPECT_FALSE(isArmingDisabled()); + EXPECT_EQ(0, getArmingDisableFlags()); + EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXPARALYZE)); + + // when + updateActivatedModes(); + + // expect + EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXPARALYZE)); +} + TEST(ArmingPreventionTest, Paralyze) { // given simulationFeatureFlags = 0; - simulationTime = 30e6; // 30 seconds after boot + simulationTime = 0; gyroCalibDone = true; // and @@ -632,10 +676,8 @@ TEST(ArmingPreventionTest, Paralyze) modeActivationConditionsMutable(2)->modeId = BOXBEEPERON; modeActivationConditionsMutable(2)->range.startStep = CHANNEL_VALUE_TO_STEP(1750); modeActivationConditionsMutable(2)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX); - modeActivationConditionsMutable(3)->auxChannelIndex = 3; modeActivationConditionsMutable(3)->modeId = BOXVTXPITMODE; - modeActivationConditionsMutable(3)->range.startStep = CHANNEL_VALUE_TO_STEP(1750); - modeActivationConditionsMutable(3)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX); + modeActivationConditionsMutable(3)->linkedTo = BOXPARALYZE; useRcControlsConfig(NULL); // and @@ -644,9 +686,8 @@ TEST(ArmingPreventionTest, Paralyze) // given rcData[THROTTLE] = 1000; rcData[AUX1] = 1000; - rcData[AUX2] = 1000; + rcData[AUX2] = 1800; // Start out with paralyze enabled rcData[AUX3] = 1000; - rcData[AUX4] = 1000; ENABLE_STATE(SMALL_ANGLE); // when @@ -685,11 +726,29 @@ TEST(ArmingPreventionTest, Paralyze) EXPECT_FALSE(ARMING_FLAG(ARMED)); EXPECT_FALSE(isArmingDisabled()); EXPECT_EQ(0, getArmingDisableFlags()); + EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXPARALYZE)); // given - // paraylze and enter pit mode + simulationTime = 10e6; // 10 seconds after boot + + // when + updateActivatedModes(); + + // expect + EXPECT_FALSE(ARMING_FLAG(ARMED)); + EXPECT_FALSE(isArmingDisabled()); + EXPECT_EQ(0, getArmingDisableFlags()); + EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXPARALYZE)); + + // given + // disable paralyze once after the startup timer + rcData[AUX2] = 1000; + + // when + updateActivatedModes(); + + // enable paralyze again rcData[AUX2] = 1800; - rcData[AUX4] = 1800; // when updateActivatedModes(); @@ -698,15 +757,12 @@ TEST(ArmingPreventionTest, Paralyze) // expect EXPECT_TRUE(isArmingDisabled()); EXPECT_EQ(ARMING_DISABLED_PARALYZE, getArmingDisableFlags()); + EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXPARALYZE)); EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXVTXPITMODE)); EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXBEEPERON)); - // and - preventModeChanges(); - // given - // Try exiting pit mode and enable beeper - rcData[AUX4] = 1000; + // enable beeper rcData[AUX3] = 1800; // when @@ -717,7 +773,7 @@ TEST(ArmingPreventionTest, Paralyze) EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXBEEPERON)); // given - // exit paralyze mode and ensure arming is still disabled + // try exiting paralyze mode and ensure arming and pit mode are still disabled rcData[AUX2] = 1000; // when @@ -727,6 +783,8 @@ TEST(ArmingPreventionTest, Paralyze) // expect EXPECT_TRUE(isArmingDisabled()); EXPECT_EQ(ARMING_DISABLED_PARALYZE, getArmingDisableFlags()); + EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXPARALYZE)); + EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXVTXPITMODE)); } // STUBS @@ -735,7 +793,7 @@ extern "C" { uint32_t millis(void) { return micros() / 1000; } bool rxIsReceivingSignal(void) { return simulationHaveRx; } - bool feature(uint32_t f) { return simulationFeatureFlags & f; } + bool featureIsEnabled(uint32_t f) { return simulationFeatureFlags & f; } void warningLedFlash(void) {} void warningLedDisable(void) {} void warningLedUpdate(void) {} @@ -765,7 +823,7 @@ extern "C" { void failsafeStartMonitoring(void) {} void failsafeUpdateState(void) {} bool failsafeIsActive(void) { return false; } - void pidResetITerm(void) {} + void pidResetIterm(void) {} void updateAdjustmentStates(void) {} void processRcAdjustments(controlRateConfig_t *) {} void updateGpsWaypointsAndMode(void) {} @@ -792,4 +850,5 @@ extern "C" { void rescheduleTask(cfTaskId_e, uint32_t) {} bool usbCableIsInserted(void) { return false; } bool usbVcpIsConnected(void) { return false; } + void pidSetAntiGravityState(bool) {} } diff --git a/src/test/unit/battery_unittest.cc.txt b/src/test/unit/battery_unittest.cc.txt index d3a8cb889d..d38299be4f 100644 --- a/src/test/unit/battery_unittest.cc.txt +++ b/src/test/unit/battery_unittest.cc.txt @@ -273,7 +273,7 @@ uint8_t armingFlags = 0; float rcCommand[4] = {0,0,0,0}; -bool feature(uint32_t mask) +bool featureIsEnabled(uint32_t mask) { UNUSED(mask); return false; diff --git a/src/test/unit/blackbox_unittest.cc b/src/test/unit/blackbox_unittest.cc index 7716314916..02601b410f 100644 --- a/src/test/unit/blackbox_unittest.cc +++ b/src/test/unit/blackbox_unittest.cc @@ -388,7 +388,7 @@ bool sensors(uint32_t) {return false;} void serialWrite(serialPort_t *, uint8_t) {} uint32_t serialTxBytesFree(const serialPort_t *) {return 0;} bool isSerialTransmitBufferEmpty(const serialPort_t *) {return false;} -bool feature(uint32_t) {return false;} +bool featureIsEnabled(uint32_t) {return false;} void mspSerialReleasePortIfAllocated(serialPort_t *) {} serialPortConfig_t *findSerialPortConfig(serialPortFunction_e ) {return NULL;} serialPort_t *findSharedSerialPort(uint16_t , serialPortFunction_e ) {return NULL;} @@ -398,5 +398,6 @@ portSharing_e determinePortSharing(const serialPortConfig_t *, serialPortFunctio failsafePhase_e failsafePhase(void) {return FAILSAFE_IDLE;} bool rxAreFlightChannelsValid(void) {return false;} bool rxIsReceivingSignal(void) {return false;} +bool isRssiConfigured(void) {return false;} } diff --git a/src/test/unit/cli_unittest.cc b/src/test/unit/cli_unittest.cc index 75d82a7399..d585962ebe 100644 --- a/src/test/unit/cli_unittest.cc +++ b/src/test/unit/cli_unittest.cc @@ -210,6 +210,7 @@ uint8_t getCurrentControlRateProfileIndex(void){ return 1; } void changeControlRateProfile(uint8_t) {} void resetAllRxChannelRangeConfigurations(rxChannelRangeConfig_t *) {} void writeEEPROM() {} +void writeEEPROMWithFeatures(uint32_t) {} serialPortConfig_t *serialFindPortConfiguration(serialPortIdentifier_e) {return NULL; } baudRate_e lookupBaudRateIndex(uint32_t){return BAUD_9600; } serialPortUsage_t *findSerialPortUsageByIdentifier(serialPortIdentifier_e){ return NULL; } @@ -240,6 +241,7 @@ const char *armingDisableFlagNames[]= { void getTaskInfo(cfTaskId_e, cfTaskInfo_t *) {} void getCheckFuncInfo(cfCheckFuncInfo_t *) {} +void schedulerResetTaskMaxExecutionTime(cfTaskId_e) {} const char * const targetName = "UNITTEST"; const char* const buildDate = "Jan 01 2017"; @@ -280,4 +282,6 @@ bool boardInformationIsSet(void) { return true; }; bool setBoardName(char *newBoardName) { UNUSED(newBoardName); return true; }; bool setManufacturerId(char *newManufacturerId) { UNUSED(newManufacturerId); return true; }; bool persistBoardInformation(void) { return true; }; + +void activeAdjustmentRangeReset(void) {} } diff --git a/src/test/unit/flight_failsafe_unittest.cc b/src/test/unit/flight_failsafe_unittest.cc index d40539e4fa..b98df40c4d 100644 --- a/src/test/unit/flight_failsafe_unittest.cc +++ b/src/test/unit/flight_failsafe_unittest.cc @@ -555,6 +555,11 @@ uint32_t millis(void) return sysTickUptime; } +uint32_t micros(void) +{ + return millis() * 1000; +} + throttleStatus_e calculateThrottleStatus() { return throttleStatus; @@ -562,7 +567,7 @@ throttleStatus_e calculateThrottleStatus() void delay(uint32_t) {} -bool feature(uint32_t mask) { +bool featureIsEnabled(uint32_t mask) { return (mask & testFeatureMask); } diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc index c3975cbf25..ce751678a0 100644 --- a/src/test/unit/flight_imu_unittest.cc +++ b/src/test/unit/flight_imu_unittest.cc @@ -243,4 +243,5 @@ void performBaroCalibrationCycle(void) {} int32_t baroCalculateAltitude(void) { return 0; } bool gyroGetAccumulationAverage(float *) { return false; } bool accGetAccumulationAverage(float *) { return false; } +void mixerSetThrottleAngleCorrection(int) {}; } diff --git a/src/test/unit/flight_mixer_unittest.cc.txt b/src/test/unit/flight_mixer_unittest.cc.txt index 8a30af72fd..868f4d72eb 100644 --- a/src/test/unit/flight_mixer_unittest.cc.txt +++ b/src/test/unit/flight_mixer_unittest.cc.txt @@ -392,7 +392,7 @@ uint8_t armingFlags; void delay(uint32_t) {} -bool feature(uint32_t mask) { +bool featureIsEnabled(uint32_t mask) { return (mask & testFeatureMask); } diff --git a/src/test/unit/ledstrip_unittest.cc b/src/test/unit/ledstrip_unittest.cc index 0d6b8256a9..eb7594d441 100644 --- a/src/test/unit/ledstrip_unittest.cc +++ b/src/test/unit/ledstrip_unittest.cc @@ -352,7 +352,7 @@ void delay(uint32_t ms) uint32_t micros(void) { return 0; } bool shouldSoundBatteryAlarm(void) { return false; } -bool feature(uint32_t mask) { +bool featureIsEnabled(uint32_t mask) { UNUSED(mask); return false; } diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc index 718f10b274..c35a03ad57 100644 --- a/src/test/unit/osd_unittest.cc +++ b/src/test/unit/osd_unittest.cc @@ -26,6 +26,7 @@ extern "C" { #include "build/debug.h" #include "blackbox/blackbox.h" + #include "blackbox/blackbox_io.h" #include "pg/pg.h" #include "pg/pg_ids.h" @@ -37,7 +38,7 @@ extern "C" { #include "drivers/serial.h" #include "fc/config.h" - #include "fc/fc_core.h" + #include "fc/core.h" #include "fc/rc_controls.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" @@ -45,9 +46,11 @@ extern "C" { #include "flight/pid.h" #include "flight/imu.h" + #include "io/beeper.h" #include "io/gps.h" #include "io/osd.h" + #include "sensors/acceleration.h" #include "sensors/battery.h" #include "rx/rx.h" @@ -56,7 +59,7 @@ extern "C" { void osdRefresh(timeUs_t currentTimeUs); void osdFormatTime(char * buff, osd_timer_precision_e precision, timeUs_t time); void osdFormatTimer(char *buff, bool showSymbol, int timerIndex); - int osdConvertTemperatureToSelectedUnit(int tempInDeciDegrees); + int osdConvertTemperatureToSelectedUnit(int tempInDegreesCelcius); uint16_t rssi; attitudeEulerAngles_t attitude; @@ -73,6 +76,9 @@ extern "C" { float motorOutputLow = 1000; + acc_t acc; + float accAverage[XYZ_AXIS_COUNT]; + PG_REGISTER(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 0); PG_REGISTER(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0); PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0); @@ -303,6 +309,9 @@ TEST(OsdTest, TestStatsImperial) osdStatSetState(OSD_STAT_RTC_DATE_TIME, true); osdStatSetState(OSD_STAT_MAX_DISTANCE, true); osdStatSetState(OSD_STAT_BLACKBOX_NUMBER, false); + osdStatSetState(OSD_STAT_MAX_G_FORCE, false); + osdStatSetState(OSD_STAT_MAX_ESC_TEMP, false); + osdStatSetState(OSD_STAT_MAX_ESC_RPM, false); // and // using imperial unit system @@ -937,11 +946,15 @@ TEST(OsdTest, TestConvertTemperatureUnits) { /* In Celsius */ osdConfigMutable()->units = OSD_UNIT_METRIC; - EXPECT_EQ(osdConvertTemperatureToSelectedUnit(330), 330); + EXPECT_EQ(osdConvertTemperatureToSelectedUnit(40), 40); /* In Fahrenheit */ osdConfigMutable()->units = OSD_UNIT_IMPERIAL; - EXPECT_EQ(osdConvertTemperatureToSelectedUnit(330), 914); + EXPECT_EQ(osdConvertTemperatureToSelectedUnit(40), 104); + + /* In Fahrenheit with rounding */ + osdConfigMutable()->units = OSD_UNIT_IMPERIAL; + EXPECT_EQ(osdConvertTemperatureToSelectedUnit(41), 106); } // STUBS @@ -968,7 +981,7 @@ extern "C" { return false; } - bool isAirmodeActive() { + bool airmodeIsEnabled() { return false; } @@ -1004,7 +1017,7 @@ extern "C" { return simulationMahDrawn; } - int32_t getEstimatedAltitude() { + int32_t getEstimatedAltitudeCm() { return simulationAltitude; } @@ -1016,6 +1029,14 @@ extern "C" { return 0; } + bool isBlackboxDeviceWorking() { + return true; + } + + bool isBlackboxDeviceFull() { + return false; + } + bool isSerialTransmitBufferEmpty(const serialPort_t *) { return false; } @@ -1039,4 +1060,6 @@ extern "C" { float pidItermAccelerator(void) { return 1.0; } uint8_t getMotorCount(void){ return 4; } bool areMotorsRunning(void){ return true; } + bool pidOsdAntiGravityActive(void) { return false; } + bool failsafeIsActive(void) { return false; } } diff --git a/src/test/unit/pid_unittest.cc b/src/test/unit/pid_unittest.cc index 46dc7cede3..6804ec663f 100644 --- a/src/test/unit/pid_unittest.cc +++ b/src/test/unit/pid_unittest.cc @@ -24,7 +24,7 @@ #include "gtest/gtest.h" #include "build/debug.h" -bool simulateMixerSaturated = false; +bool simulatedAirmodeEnabled = true; float simulatedSetpointRate[3] = { 0,0,0 }; float simulatedRcDeflection[3] = { 0,0,0 }; float simulatedThrottlePIDAttenuation = 1.0f; @@ -46,8 +46,8 @@ extern "C" { #include "drivers/sound_beeper.h" #include "drivers/time.h" - #include "fc/fc_core.h" - #include "fc/fc_rc.h" + #include "fc/core.h" + #include "fc/rc.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" @@ -67,7 +67,7 @@ extern "C" { float getThrottlePIDAttenuation(void) { return simulatedThrottlePIDAttenuation; } float getMotorMixRange(void) { return simulatedMotorMixRange; } float getSetpointRate(int axis) { return simulatedSetpointRate[axis]; } - bool mixerIsOutputSaturated(int, float) { return simulateMixerSaturated; } + bool isAirmodeActivated() { return simulatedAirmodeEnabled; } float getRcDeflectionAbs(int axis) { return ABS(simulatedRcDeflection[axis]); } void systemBeep(bool) { } bool gyroOverflowDetected(void) { return false; } @@ -84,10 +84,10 @@ int loopIter = 0; void setDefaultTestSettings(void) { pgResetAll(); pidProfile = pidProfilesMutable(1); - pidProfile->pid[PID_ROLL] = { 40, 40, 30 }; - pidProfile->pid[PID_PITCH] = { 58, 50, 35 }; - pidProfile->pid[PID_YAW] = { 70, 45, 20 }; - pidProfile->pid[PID_LEVEL] = { 50, 50, 75 }; + pidProfile->pid[PID_ROLL] = { 40, 40, 30, 65 }; + pidProfile->pid[PID_PITCH] = { 58, 50, 35, 60 }; + pidProfile->pid[PID_YAW] = { 70, 45, 20, 60 }; + pidProfile->pid[PID_LEVEL] = { 50, 50, 75, 0 }; pidProfile->pidSumLimit = PIDSUM_LIMIT; pidProfile->pidSumLimitYaw = PIDSUM_LIMIT_YAW; @@ -101,11 +101,11 @@ void setDefaultTestSettings(void) { pidProfile->vbatPidCompensation = 0; pidProfile->pidAtMinThrottle = PID_STABILISATION_ON; pidProfile->levelAngleLimit = 55; - pidProfile->setpointRelaxRatio = 100; - pidProfile->dtermSetpointWeight = 0; + pidProfile->feedForwardTransition = 100; pidProfile->yawRateAccelLimit = 100; pidProfile->rateAccelLimit = 0; - pidProfile->itermThrottleThreshold = 350; + pidProfile->antiGravityMode = ANTI_GRAVITY_SMOOTH; + pidProfile->itermThrottleThreshold = 250; pidProfile->itermAcceleratorGain = 1000; pidProfile->crash_time = 500; pidProfile->crash_delay = 0; @@ -122,6 +122,11 @@ void setDefaultTestSettings(void) { pidProfile->throttle_boost = 0; pidProfile->throttle_boost_cutoff = 15; pidProfile->iterm_rotation = false; + pidProfile->smart_feedforward = false, + pidProfile->iterm_relax = ITERM_RELAX_OFF, + pidProfile->iterm_relax_cutoff = 11, + pidProfile->iterm_relax_type = ITERM_RELAX_SETPOINT, + pidProfile->abs_control_gain = 0, gyro.targetLooptime = 4000; } @@ -132,7 +137,6 @@ timeUs_t currentTestTime(void) { void resetTest(void) { loopIter = 0; - simulateMixerSaturated = false; simulatedThrottlePIDAttenuation = 1.0f; simulatedMotorMixRange = 0.0f; @@ -144,6 +148,7 @@ void resetTest(void) { pidData[axis].P = 0; pidData[axis].I = 0; pidData[axis].D = 0; + pidData[axis].F = 0; pidData[axis].Sum = 0; simulatedSetpointRate[axis] = 0; simulatedRcDeflection[axis] = 0; @@ -266,7 +271,15 @@ TEST(pidControllerTest, testPidLoop) { ASSERT_NEAR(-8.7, pidData[FD_YAW].I, calculateTolerance(-8.7)); EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D); EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D); - EXPECT_FLOAT_EQ(0, pidData[FD_YAW].D); + ASSERT_NEAR(-132.25, pidData[FD_YAW].D, calculateTolerance(-132.25)); + + // Simulate Iterm behaviour during mixer saturation + simulatedMotorMixRange = 1.2f; + pidController(pidProfile, &rollAndPitchTrims, currentTestTime()); + ASSERT_NEAR(-23.5, pidData[FD_ROLL].I, calculateTolerance(-23.5)); + ASSERT_NEAR(19.6, pidData[FD_PITCH].I, calculateTolerance(19.6)); + ASSERT_NEAR(-8.8, pidData[FD_YAW].I, calculateTolerance(-8.8)); + simulatedMotorMixRange = 0; // Match the stick to gyro to stop error simulatedSetpointRate[FD_ROLL] = 100; @@ -276,14 +289,13 @@ TEST(pidControllerTest, testPidLoop) { for(int loop = 0; loop < 5; loop++) { pidController(pidProfile, &rollAndPitchTrims, currentTestTime()); } - // Iterm is stalled as it is not accumulating anymore EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].P); EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].P); EXPECT_FLOAT_EQ(0, pidData[FD_YAW].P); ASSERT_NEAR(-23.5, pidData[FD_ROLL].I, calculateTolerance(-23.5)); ASSERT_NEAR(19.6, pidData[FD_PITCH].I, calculateTolerance(19.6)); - ASSERT_NEAR(-10.6, pidData[FD_YAW].I, calculateTolerance(-10.5)); + ASSERT_NEAR(-10.6, pidData[FD_YAW].I, calculateTolerance(-10.6)); EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D); EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D); EXPECT_FLOAT_EQ(0, pidData[FD_YAW].D); @@ -312,53 +324,42 @@ TEST(pidControllerTest, testPidLevel) { // Test Angle mode response enableFlightMode(ANGLE_MODE); - pidController(pidProfile, &rollAndPitchTrims, currentTestTime()); + float currentPidSetpoint = 30; + rollAndPitchTrims_t angleTrim = { { 0, 0 } }; - // Loop 1 - EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].P); - EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].P); - EXPECT_FLOAT_EQ(0, pidData[FD_YAW].P); - EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].I); - EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].I); - EXPECT_FLOAT_EQ(0, pidData[FD_YAW].I); - EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D); - EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D); - EXPECT_FLOAT_EQ(0, pidData[FD_YAW].D); + currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint); + EXPECT_FLOAT_EQ(0, currentPidSetpoint); + currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint); + EXPECT_FLOAT_EQ(0, currentPidSetpoint); // Test attitude response setStickPosition(FD_ROLL, 1.0f); setStickPosition(FD_PITCH, -1.0f); - attitude.values.roll = 550; - attitude.values.pitch = -550; - pidController(pidProfile, &rollAndPitchTrims, currentTestTime()); - pidController(pidProfile, &rollAndPitchTrims, currentTestTime()); - pidController(pidProfile, &rollAndPitchTrims, currentTestTime()); + currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint); + EXPECT_FLOAT_EQ(275, currentPidSetpoint); + currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint); + EXPECT_FLOAT_EQ(-275, currentPidSetpoint); - // Loop 2 - EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].P); - EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].P); - EXPECT_FLOAT_EQ(0, pidData[FD_YAW].P); - EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].I); - EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].I); - EXPECT_FLOAT_EQ(0, pidData[FD_YAW].I); - EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D); - EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D); - EXPECT_FLOAT_EQ(0, pidData[FD_YAW].D); + setStickPosition(FD_ROLL, -0.5f); + setStickPosition(FD_PITCH, 0.5f); + currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint); + EXPECT_FLOAT_EQ(-137.5, currentPidSetpoint); + currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint); + EXPECT_FLOAT_EQ(137.5, currentPidSetpoint); - // Disable ANGLE_MODE on full stick inputs + attitude.values.roll = -275; + attitude.values.pitch = 275; + currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint); + EXPECT_FLOAT_EQ(0, currentPidSetpoint); + currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint); + EXPECT_FLOAT_EQ(0, currentPidSetpoint); + + // Disable ANGLE_MODE disableFlightMode(ANGLE_MODE); - pidController(pidProfile, &rollAndPitchTrims, currentTestTime()); - - // Expect full rate output - ASSERT_NEAR(2559.8, pidData[FD_ROLL].P, calculateTolerance(2559.8)); - ASSERT_NEAR(-3711.6, pidData[FD_PITCH].P, calculateTolerance(-3711.6)); - EXPECT_FLOAT_EQ(0, pidData[FD_YAW].P); - ASSERT_NEAR(150, pidData[FD_ROLL].I, calculateTolerance(150)); - ASSERT_NEAR(-150, pidData[FD_PITCH].I, calculateTolerance(-150)); - EXPECT_FLOAT_EQ(0, pidData[FD_YAW].I); - EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D); - EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D); - EXPECT_FLOAT_EQ(0, pidData[FD_YAW].D); + currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint); + EXPECT_FLOAT_EQ(0, currentPidSetpoint); + currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint); + EXPECT_FLOAT_EQ(0, currentPidSetpoint); } @@ -368,51 +369,21 @@ TEST(pidControllerTest, testPidHorizon) { pidStabilisationState(PID_STABILISATION_ON); enableFlightMode(HORIZON_MODE); - // Loop 1 - EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].P); - EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].P); - EXPECT_FLOAT_EQ(0, pidData[FD_YAW].P); - EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].I); - EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].I); - EXPECT_FLOAT_EQ(0, pidData[FD_YAW].I); - EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D); - EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D); - EXPECT_FLOAT_EQ(0, pidData[FD_YAW].D); - // Test full stick response setStickPosition(FD_ROLL, 1.0f); setStickPosition(FD_PITCH, -1.0f); - attitude.values.roll = 550; - attitude.values.pitch = -550; - pidController(pidProfile, &rollAndPitchTrims, currentTestTime()); + EXPECT_FLOAT_EQ(0, calcHorizonLevelStrength()); // Expect full rate output on full stick - ASSERT_NEAR(2559.8, pidData[FD_ROLL].P, calculateTolerance(2559.8)); - ASSERT_NEAR(-3711.6, pidData[FD_PITCH].P, calculateTolerance(-3711.6)); - EXPECT_FLOAT_EQ(0, pidData[FD_YAW].P); - ASSERT_NEAR(150, pidData[FD_ROLL].I, calculateTolerance(150)); - ASSERT_NEAR(-150, pidData[FD_PITCH].I, calculateTolerance(-150)); - EXPECT_FLOAT_EQ(0, pidData[FD_YAW].I); - EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D); - EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D); - EXPECT_FLOAT_EQ(0, pidData[FD_YAW].D); + // Test zero stick response + setStickPosition(FD_ROLL, 0); + setStickPosition(FD_PITCH, 0); + EXPECT_FLOAT_EQ(1, calcHorizonLevelStrength()); - // Test full stick response + // Test small stick response setStickPosition(FD_ROLL, 0.1f); setStickPosition(FD_PITCH, -0.1f); - attitude.values.roll = 536; - attitude.values.pitch = -536; - pidController(pidProfile, &rollAndPitchTrims, currentTestTime()); - - ASSERT_NEAR(0.75, pidData[FD_ROLL].P, calculateTolerance(0.75)); - ASSERT_NEAR(-1.09, pidData[FD_PITCH].P, calculateTolerance(-1.09)); - EXPECT_FLOAT_EQ(0, pidData[FD_YAW].P); - ASSERT_NEAR(150, pidData[FD_ROLL].I, calculateTolerance(150)); - ASSERT_NEAR(-150, pidData[FD_PITCH].I, calculateTolerance(-150)); - EXPECT_FLOAT_EQ(0, pidData[FD_YAW].I); - EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D); - EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D); - EXPECT_FLOAT_EQ(0, pidData[FD_YAW].D); + ASSERT_NEAR(0.82, calcHorizonLevelStrength(), calculateTolerance(0.82)); } TEST(pidControllerTest, testMixerSaturation) { @@ -423,14 +394,41 @@ TEST(pidControllerTest, testMixerSaturation) { // Test full stick response setStickPosition(FD_ROLL, 1.0f); setStickPosition(FD_PITCH, -1.0f); - simulateMixerSaturated = true; + setStickPosition(FD_YAW, 1.0f); + simulatedMotorMixRange = 2.0f; pidController(pidProfile, &rollAndPitchTrims, currentTestTime()); // Expect no iterm accumulation - EXPECT_FLOAT_EQ(0, pidData[FD_YAW].P); EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].I); EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].I); EXPECT_FLOAT_EQ(0, pidData[FD_YAW].I); + + // Test itermWindup limit + // First store values without exceeding iterm windup limit + resetTest(); + ENABLE_ARMING_FLAG(ARMED); + pidStabilisationState(PID_STABILISATION_ON); + setStickPosition(FD_ROLL, 0.1f); + setStickPosition(FD_PITCH, -0.1f); + setStickPosition(FD_YAW, 0.1f); + simulatedMotorMixRange = 0.0f; + pidController(pidProfile, &rollAndPitchTrims, currentTestTime()); + float rollTestIterm = pidData[FD_ROLL].I; + float pitchTestIterm = pidData[FD_PITCH].I; + float yawTestIterm = pidData[FD_YAW].I; + + // Now compare values when exceeding the limit + resetTest(); + ENABLE_ARMING_FLAG(ARMED); + pidStabilisationState(PID_STABILISATION_ON); + setStickPosition(FD_ROLL, 0.1f); + setStickPosition(FD_PITCH, -0.1f); + setStickPosition(FD_YAW, 0.1f); + simulatedMotorMixRange = (pidProfile->itermWindupPointPercent + 1) / 100.0f; + pidController(pidProfile, &rollAndPitchTrims, currentTestTime()); + ASSERT_LT(pidData[FD_ROLL].I, rollTestIterm); + ASSERT_GE(pidData[FD_PITCH].I, pitchTestIterm); + ASSERT_LT(pidData[FD_YAW].I, yawTestIterm); } // TODO - Add more scenarios @@ -458,8 +456,155 @@ TEST(pidControllerTest, testCrashRecoveryMode) { // Add additional verifications } -TEST(pidControllerTest, pidSetpointTransition) { -// TODO +TEST(pidControllerTest, testFeedForward) { + resetTest(); + ENABLE_ARMING_FLAG(ARMED); + pidStabilisationState(PID_STABILISATION_ON); + + EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].F); + EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].F); + EXPECT_FLOAT_EQ(0, pidData[FD_YAW].F); + + // Match the stick to gyro to stop error + setStickPosition(FD_ROLL, 1.0f); + setStickPosition(FD_PITCH, -1.0f); + setStickPosition(FD_YAW, -1.0f); + + pidController(pidProfile, &rollAndPitchTrims, currentTestTime()); + + ASSERT_NEAR(2232.78, pidData[FD_ROLL].F, calculateTolerance(2232.78)); + ASSERT_NEAR(-2061.03, pidData[FD_PITCH].F, calculateTolerance(-2061.03)); + ASSERT_NEAR(-82.52, pidData[FD_YAW].F, calculateTolerance(-82.5)); + + // Match the stick to gyro to stop error + setStickPosition(FD_ROLL, 0.5f); + setStickPosition(FD_PITCH, -0.5f); + setStickPosition(FD_YAW, -0.5f); + + pidController(pidProfile, &rollAndPitchTrims, currentTestTime()); + + ASSERT_NEAR(-558.20, pidData[FD_ROLL].F, calculateTolerance(-558.20)); + ASSERT_NEAR(515.26, pidData[FD_PITCH].F, calculateTolerance(515.26)); + ASSERT_NEAR(-41.26, pidData[FD_YAW].F, calculateTolerance(-41.26)); + + for (int loop =0; loop <= 15; loop++) { + gyro.gyroADCf[FD_ROLL] += gyro.gyroADCf[FD_ROLL]; + pidController(pidProfile, &rollAndPitchTrims, currentTestTime()); + } + + EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].F); + EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].F); + EXPECT_FLOAT_EQ(0, pidData[FD_YAW].F); + +} + +TEST(pidControllerTest, testItermRelax) { + resetTest(); + pidProfile->iterm_relax = ITERM_RELAX_RP; + ENABLE_ARMING_FLAG(ARMED); + pidStabilisationState(PID_STABILISATION_ON); + + pidProfile->iterm_relax_type = ITERM_RELAX_SETPOINT; + pidInit(pidProfile); + + float itermErrorRate = 0; + float currentPidSetpoint = 0; + float gyroRate = 0; + + applyItermRelax(FD_PITCH, 0, gyroRate, &itermErrorRate, ¤tPidSetpoint); + EXPECT_FLOAT_EQ(itermErrorRate, 0); + itermErrorRate = -10; + currentPidSetpoint = 10; + pidData[FD_PITCH].I = 10; + + applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, ¤tPidSetpoint); + + ASSERT_NEAR(-6.66, itermErrorRate, calculateTolerance(-6.66)); + currentPidSetpoint += ITERM_RELAX_SETPOINT_THRESHOLD; + applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, ¤tPidSetpoint); + EXPECT_FLOAT_EQ(itermErrorRate, 0); + applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, ¤tPidSetpoint); + EXPECT_FLOAT_EQ(itermErrorRate, 0); + + pidProfile->iterm_relax_type = ITERM_RELAX_GYRO; + pidInit(pidProfile); + + currentPidSetpoint = 100; + applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, ¤tPidSetpoint); + EXPECT_FLOAT_EQ(itermErrorRate, 0); + gyroRate = 10; + itermErrorRate = -10; + applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, ¤tPidSetpoint); + ASSERT_NEAR(7, itermErrorRate, calculateTolerance(7)); + gyroRate += 100; + applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, ¤tPidSetpoint); + ASSERT_NEAR(-10, itermErrorRate, calculateTolerance(-10)); + + pidProfile->iterm_relax = ITERM_RELAX_RP_INC; + pidInit(pidProfile); + + itermErrorRate = -10; + pidData[FD_PITCH].I = 10; + currentPidSetpoint = 10; + applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, ¤tPidSetpoint); + EXPECT_FLOAT_EQ(itermErrorRate, -10); + itermErrorRate = 10; + pidData[FD_PITCH].I = -10; + applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, ¤tPidSetpoint); + EXPECT_FLOAT_EQ(itermErrorRate, 10); + itermErrorRate = -10; + currentPidSetpoint = 10; + applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, ¤tPidSetpoint); + EXPECT_FLOAT_EQ(itermErrorRate, -100); + + pidProfile->iterm_relax_type = ITERM_RELAX_SETPOINT; + pidInit(pidProfile); + + itermErrorRate = -10; + currentPidSetpoint = ITERM_RELAX_SETPOINT_THRESHOLD; + applyItermRelax(FD_YAW, pidData[FD_YAW].I, gyroRate, &itermErrorRate, ¤tPidSetpoint); + EXPECT_FLOAT_EQ(itermErrorRate, -10); + + pidProfile->iterm_relax = ITERM_RELAX_RPY; + pidInit(pidProfile); + applyItermRelax(FD_YAW, pidData[FD_YAW].I, gyroRate, &itermErrorRate, ¤tPidSetpoint); + ASSERT_NEAR(-3.6, itermErrorRate, calculateTolerance(-3.6)); +} + +// TODO - Add more tests +TEST(pidControllerTest, testAbsoluteControl) { + resetTest(); + pidProfile->abs_control_gain = 10; + pidInit(pidProfile); + ENABLE_ARMING_FLAG(ARMED); + pidStabilisationState(PID_STABILISATION_ON); + + float gyroRate = 0; + bool itermRelaxIsEnabled = false; + float setpointLpf = 6; + float setpointHpf = 30; + + float itermErrorRate = 10; + float currentPidSetpoint = 10; + + applyAbsoluteControl(FD_PITCH, gyroRate, itermRelaxIsEnabled, setpointLpf, setpointHpf, + ¤tPidSetpoint, &itermErrorRate); + + ASSERT_NEAR(10.8, itermErrorRate, calculateTolerance(10.8)); + ASSERT_NEAR(10.8, currentPidSetpoint, calculateTolerance(10.8)); + + itermRelaxIsEnabled = true; + applyAbsoluteControl(FD_PITCH, gyroRate, itermRelaxIsEnabled, setpointLpf, setpointHpf, + ¤tPidSetpoint, &itermErrorRate); + ASSERT_NEAR(10.8, itermErrorRate, calculateTolerance(10.8)); + ASSERT_NEAR(10.8, currentPidSetpoint, calculateTolerance(10.8)); + + gyroRate = -53; + axisError[FD_PITCH] = -60; + applyAbsoluteControl(FD_PITCH, gyroRate, itermRelaxIsEnabled, setpointLpf, setpointHpf, + ¤tPidSetpoint, &itermErrorRate); + ASSERT_NEAR(-79.2, itermErrorRate, calculateTolerance(-79.2)); + ASSERT_NEAR(-79.2, currentPidSetpoint, calculateTolerance(-79.2)); } TEST(pidControllerTest, testDtermFiltering) { @@ -467,5 +612,43 @@ TEST(pidControllerTest, testDtermFiltering) { } TEST(pidControllerTest, testItermRotationHandling) { -// TODO + resetTest(); + pidInit(pidProfile); + + rotateItermAndAxisError(); + EXPECT_FLOAT_EQ(pidData[FD_ROLL].I, 0); + EXPECT_FLOAT_EQ(pidData[FD_PITCH].I, 0); + EXPECT_FLOAT_EQ(pidData[FD_YAW].I, 0); + + pidProfile->iterm_rotation = true; + pidInit(pidProfile); + + rotateItermAndAxisError(); + EXPECT_FLOAT_EQ(pidData[FD_ROLL].I, 0); + EXPECT_FLOAT_EQ(pidData[FD_PITCH].I, 0); + EXPECT_FLOAT_EQ(pidData[FD_YAW].I, 0); + + pidData[FD_ROLL].I = 10; + pidData[FD_PITCH].I = 1000; + pidData[FD_YAW].I = 1000; + gyro.gyroADCf[FD_ROLL] = -1000; + rotateItermAndAxisError(); + EXPECT_FLOAT_EQ(pidData[FD_ROLL].I, 10); + ASSERT_NEAR(860.37, pidData[FD_PITCH].I, calculateTolerance(860.37)); + ASSERT_NEAR(1139.6, pidData[FD_YAW].I, calculateTolerance(1139.6)); + + pidProfile->abs_control_gain = 10; + pidInit(pidProfile); + pidData[FD_ROLL].I = 10; + pidData[FD_PITCH].I = 1000; + pidData[FD_YAW].I = 1000; + + gyro.gyroADCf[FD_ROLL] = -1000; + // FIXME - axisError changes don't affect the system. This is a potential bug or intendend behaviour? + axisError[FD_PITCH] = 1000; + axisError[FD_YAW] = 1000; + rotateItermAndAxisError(); + EXPECT_FLOAT_EQ(pidData[FD_ROLL].I, 10); + ASSERT_NEAR(860.37, pidData[FD_PITCH].I, calculateTolerance(860.37)); + ASSERT_NEAR(1139.6, pidData[FD_YAW].I, calculateTolerance(1139.6)); } diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index bf20ccaf28..a01d6a70ba 100644 --- a/src/test/unit/rc_controls_unittest.cc +++ b/src/test/unit/rc_controls_unittest.cc @@ -52,7 +52,7 @@ extern "C" { #include "fc/rc_controls.h" #include "fc/runtime_config.h" - #include "fc/fc_core.h" + #include "fc/core.h" #include "scheduler/scheduler.h" } @@ -224,6 +224,10 @@ extern "C" { uint32_t millis(void) { return fixedMillis; } + +uint32_t micros(void) { + return fixedMillis * 1000; +} } void resetMillis(void) { @@ -705,7 +709,7 @@ void gyroStartCalibration(bool isFirstArmingCalibration) } void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t*) {} void handleInflightCalibrationStickPosition(void) {} -bool feature(uint32_t) { return false;} +bool featureIsEnabled(uint32_t) { return false;} bool sensors(uint32_t) { return false;} void tryArm(void) {} void disarm(void) {} diff --git a/src/test/unit/rcdevice_unittest.cc b/src/test/unit/rcdevice_unittest.cc index fb67d99ff5..0a40e15c4a 100644 --- a/src/test/unit/rcdevice_unittest.cc +++ b/src/test/unit/rcdevice_unittest.cc @@ -42,12 +42,12 @@ extern "C" { #include "io/rcdevice_cam.h" #include "io/osd.h" #include "io/rcdevice.h" - #include "io/rcdevice_osd.h" #include "pg/pg.h" #include "pg/pg_ids.h" #include "pg/vcd.h" #include "pg/rx.h" + #include "pg/rcdevice.h" #include "rx/rx.h" @@ -55,13 +55,28 @@ extern "C" { extern rcdeviceSwitchState_t switchStates[BOXCAMERA3 - BOXCAMERA1 + 1]; extern runcamDevice_t *camDevice; - extern bool needRelease; + extern bool isButtonPressed; + extern bool rcdeviceInMenu; + extern rcdeviceWaitingResponseQueue watingResponseQueue; + PG_REGISTER_WITH_RESET_FN(rcdeviceConfig_t, rcdeviceConfig, PG_RCDEVICE_CONFIG, 0); bool unitTestIsSwitchActivited(boxId_e boxId) { uint8_t adjustBoxID = boxId - BOXCAMERA1; rcdeviceSwitchState_s switchState = switchStates[adjustBoxID]; return switchState.isActivated; } + + void pgResetFn_rcdeviceConfig(rcdeviceConfig_t *rcdeviceConfig) +{ + rcdeviceConfig->initDeviceAttempts = 4; + rcdeviceConfig->initDeviceAttemptInterval = 1000; +} + + uint32_t millis(void); + int minTimeout = 180; + + void rcdeviceSend5KeyOSDCableSimualtionEvent(rcdeviceCamSimulationKeyEvent_e key); + rcdeviceResponseParseContext_t* rcdeviceRespCtxQueueShift(rcdeviceWaitingResponseQueue *queue); } #define MAX_RESPONSES_COUNT 10 @@ -83,6 +98,13 @@ typedef struct testData_s { } testData_t; static testData_t testData; +extern rcdeviceWaitingResponseQueue watingResponseQueue; + +static void resetRCDeviceStatus() +{ + isButtonPressed = false; + rcdeviceInMenu = false; +} static void clearResponseBuff() { @@ -90,17 +112,15 @@ static void clearResponseBuff() testData.responseBufCount = 0; memset(testData.responseBufsLen, 0, MAX_RESPONSES_COUNT); memset(testData.responesBufs, 0, MAX_RESPONSES_COUNT * 60); + + while (rcdeviceRespCtxQueueShift(&watingResponseQueue)) { + + } } static void addResponseData(uint8_t *data, uint8_t dataLen, bool withDataForFlushSerial) { - if (withDataForFlushSerial) { - memcpy(testData.responesBufs[testData.responseBufCount], "0", 1); - testData.responseBufsLen[testData.responseBufCount] = 1; - testData.responseBufCount++; - } - - + UNUSED(withDataForFlushSerial); memcpy(testData.responesBufs[testData.responseBufCount], data, dataLen); testData.responseBufsLen[testData.responseBufCount] = dataLen; testData.responseBufCount++; @@ -109,29 +129,44 @@ static void addResponseData(uint8_t *data, uint8_t dataLen, bool withDataForFlus TEST(RCDeviceTest, TestRCSplitInitWithoutPortConfigurated) { runcamDevice_t device; + + resetRCDeviceStatus(); + watingResponseQueue.headPos = 0; + watingResponseQueue.tailPos = 0; + watingResponseQueue.itemCount = 0; memset(&testData, 0, sizeof(testData)); - bool result = runcamDeviceInit(&device); - EXPECT_EQ(false, result); + runcamDeviceInit(&device); + EXPECT_EQ(false, device.isReady); } TEST(RCDeviceTest, TestRCSplitInitWithoutOpenPortConfigurated) { runcamDevice_t device; + resetRCDeviceStatus(); + + watingResponseQueue.headPos = 0; + watingResponseQueue.tailPos = 0; + watingResponseQueue.itemCount = 0; memset(&testData, 0, sizeof(testData)); testData.isRunCamSplitOpenPortSupported = false; testData.isRunCamSplitPortConfigurated = true; - bool result = runcamDeviceInit(&device); - EXPECT_EQ(false, result); + runcamDeviceInit(&device); + EXPECT_EQ(false, device.isReady); } TEST(RCDeviceTest, TestInitDevice) { runcamDevice_t device; + resetRCDeviceStatus(); + // test correct response + watingResponseQueue.headPos = 0; + watingResponseQueue.tailPos = 0; + watingResponseQueue.itemCount = 0; memset(&testData, 0, sizeof(testData)); testData.isRunCamSplitOpenPortSupported = true; testData.isRunCamSplitPortConfigurated = true; @@ -139,15 +174,26 @@ TEST(RCDeviceTest, TestInitDevice) uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBD }; addResponseData(responseData, sizeof(responseData), true); - bool result = runcamDeviceInit(&device); - EXPECT_EQ(result, true); + runcamDeviceInit(&device); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + testData.responseDataReadPos = 0; + testData.indexOfCurrentRespBuf = 0; + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(device.isReady, true); } TEST(RCDeviceTest, TestInitDeviceWithInvalidResponse) { runcamDevice_t device; + resetRCDeviceStatus(); + // test correct response data with incorrect len + watingResponseQueue.headPos = 0; + watingResponseQueue.tailPos = 0; + watingResponseQueue.itemCount = 0; memset(&testData, 0, sizeof(testData)); testData.isRunCamSplitOpenPortSupported = true; testData.isRunCamSplitPortConfigurated = true; @@ -155,46 +201,82 @@ TEST(RCDeviceTest, TestInitDeviceWithInvalidResponse) uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBD, 0x33 }; addResponseData(responseData, sizeof(responseData), true); - bool result = runcamDeviceInit(&device); - EXPECT_EQ(result, true); + runcamDeviceInit(&device); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + testData.responseDataReadPos = 0; + testData.indexOfCurrentRespBuf = 0; + rcdeviceReceive(millis() * 1000); + EXPECT_EQ(device.isReady, true); clearResponseBuff(); + testData.millis += minTimeout; // invalid crc uint8_t responseDataWithInvalidCRC[] = { 0xCC, 0x01, 0x37, 0x00, 0xBE }; addResponseData(responseDataWithInvalidCRC, sizeof(responseDataWithInvalidCRC), true); - result = runcamDeviceInit(&device); - EXPECT_EQ(result, false); + runcamDeviceInit(&device); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + testData.responseDataReadPos = 0; + testData.indexOfCurrentRespBuf = 0; + rcdeviceReceive(millis() * 1000); + EXPECT_EQ(device.isReady, false); clearResponseBuff(); + testData.millis += minTimeout; // incomplete response data uint8_t incompleteResponseData[] = { 0xCC, 0x01, 0x37 }; addResponseData(incompleteResponseData, sizeof(incompleteResponseData), true); - result = runcamDeviceInit(&device); - EXPECT_EQ(result, false); + runcamDeviceInit(&device); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + testData.responseDataReadPos = 0; + testData.indexOfCurrentRespBuf = 0; + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(device.isReady, false); clearResponseBuff(); + testData.millis += minTimeout; // test timeout memset(&testData, 0, sizeof(testData)); testData.isRunCamSplitOpenPortSupported = true; testData.isRunCamSplitPortConfigurated = true; testData.isAllowBufferReadWrite = true; - result = runcamDeviceInit(&device); - EXPECT_EQ(result, false); + runcamDeviceInit(&device); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + testData.responseDataReadPos = 0; + testData.indexOfCurrentRespBuf = 0; + rcdeviceReceive(millis() * 1000); + EXPECT_EQ(device.isReady, false); clearResponseBuff(); + testData.millis += minTimeout; } TEST(RCDeviceTest, TestWifiModeChangeWithDeviceUnready) { + resetRCDeviceStatus(); + // test correct response + watingResponseQueue.headPos = 0; + watingResponseQueue.tailPos = 0; + watingResponseQueue.itemCount = 0; memset(&testData, 0, sizeof(testData)); testData.isRunCamSplitOpenPortSupported = true; testData.isRunCamSplitPortConfigurated = true; testData.isAllowBufferReadWrite = true; testData.maxTimesOfRespDataAvailable = 0; - uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBC }; + uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBC }; // wrong response addResponseData(responseData, sizeof(responseData), true); - bool result = rcdeviceInit(); - EXPECT_EQ(result, false); + rcdeviceInit(); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + testData.responseDataReadPos = 0; + testData.indexOfCurrentRespBuf = 0; + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(camDevice->isReady, false); // bind aux1, aux2, aux3 channel to wifi button, power button and change mode for (uint8_t i = 0; i <= (BOXCAMERA3 - BOXCAMERA1); i++) { @@ -236,6 +318,8 @@ TEST(RCDeviceTest, TestWifiModeChangeWithDeviceUnready) TEST(RCDeviceTest, TestWifiModeChangeWithDeviceReady) { + resetRCDeviceStatus(); + // test correct response memset(&testData, 0, sizeof(testData)); testData.isRunCamSplitOpenPortSupported = true; @@ -244,8 +328,15 @@ TEST(RCDeviceTest, TestWifiModeChangeWithDeviceReady) testData.maxTimesOfRespDataAvailable = 0; uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBD }; addResponseData(responseData, sizeof(responseData), true); - bool result = rcdeviceInit(); - EXPECT_EQ(result, true); + camDevice->info.features = 15; + rcdeviceInit(); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + testData.responseDataReadPos = 0; + testData.indexOfCurrentRespBuf = 0; + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(camDevice->isReady, true); // bind aux1, aux2, aux3 channel to wifi button, power button and change mode for (uint8_t i = 0; i <= BOXCAMERA3 - BOXCAMERA1; i++) { @@ -289,6 +380,8 @@ TEST(RCDeviceTest, TestWifiModeChangeWithDeviceReady) TEST(RCDeviceTest, TestWifiModeChangeCombine) { + resetRCDeviceStatus(); + memset(&testData, 0, sizeof(testData)); testData.isRunCamSplitOpenPortSupported = true; testData.isRunCamSplitPortConfigurated = true; @@ -296,8 +389,14 @@ TEST(RCDeviceTest, TestWifiModeChangeCombine) testData.maxTimesOfRespDataAvailable = 0; uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBD }; addResponseData(responseData, sizeof(responseData), true); - bool result = rcdeviceInit(); - EXPECT_EQ(true, result); + rcdeviceInit(); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + testData.responseDataReadPos = 0; + testData.indexOfCurrentRespBuf = 0; + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(true, camDevice->isReady); // bind aux1, aux2, aux3 channel to wifi button, power button and change mode for (uint8_t i = 0; i <= BOXCAMERA3 - BOXCAMERA1; i++) { @@ -365,6 +464,8 @@ TEST(RCDeviceTest, TestWifiModeChangeCombine) TEST(RCDeviceTest, Test5KeyOSDCableSimulationProtocol) { + resetRCDeviceStatus(); + memset(&testData, 0, sizeof(testData)); testData.isRunCamSplitOpenPortSupported = true; testData.isRunCamSplitPortConfigurated = true; @@ -372,122 +473,197 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationProtocol) testData.maxTimesOfRespDataAvailable = 0; uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBD }; addResponseData(responseData, sizeof(responseData), true); - bool result = rcdeviceInit(); - EXPECT_EQ(true, result); + rcdeviceInit(); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + testData.responseDataReadPos = 0; + testData.indexOfCurrentRespBuf = 0; + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(true, camDevice->isReady); + clearResponseBuff(); // test timeout of open connection - result = runcamDeviceOpen5KeyOSDCableConnection(camDevice); - EXPECT_EQ(false, result); + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_CONNECTION_OPEN); + rcdeviceReceive(millis() * 1000); + testData.millis += 3000; + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(false, rcdeviceInMenu); clearResponseBuff(); // open connection with correct response uint8_t responseDataOfOpenConnection[] = { 0xCC, 0x11, 0xe7 }; addResponseData(responseDataOfOpenConnection, sizeof(responseDataOfOpenConnection), true); - result = runcamDeviceOpen5KeyOSDCableConnection(camDevice); - EXPECT_EQ(true, result); + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_CONNECTION_OPEN); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(true, rcdeviceInMenu); clearResponseBuff(); // open connection with correct response but wrong data length uint8_t incorrectResponseDataOfOpenConnection1[] = { 0xCC, 0x11, 0xe7, 0x55 }; addResponseData(incorrectResponseDataOfOpenConnection1, sizeof(incorrectResponseDataOfOpenConnection1), true); - result = runcamDeviceOpen5KeyOSDCableConnection(camDevice); - EXPECT_EQ(true, result); + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_CONNECTION_OPEN); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(true, rcdeviceInMenu); clearResponseBuff(); // open connection with invalid crc uint8_t incorrectResponseDataOfOpenConnection2[] = { 0xCC, 0x10, 0x42 }; addResponseData(incorrectResponseDataOfOpenConnection2, sizeof(incorrectResponseDataOfOpenConnection2), true); - result = runcamDeviceOpen5KeyOSDCableConnection(camDevice); - EXPECT_EQ(false, result); + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_CONNECTION_OPEN); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(true, rcdeviceInMenu); // when crc wrong won't change the menu state clearResponseBuff(); // test timeout of close connection - runcamDeviceClose5KeyOSDCableConnection(camDevice, NULL); - EXPECT_EQ(false, result); + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_CONNECTION_CLOSE); + rcdeviceReceive(millis() * 1000); + testData.millis += 3000; + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(true, rcdeviceInMenu); // close menu timeout won't change the menu state clearResponseBuff(); // close connection with correct response uint8_t responseDataOfCloseConnection[] = { 0xCC, 0x21, 0x11 }; addResponseData(responseDataOfCloseConnection, sizeof(responseDataOfCloseConnection), true); - result = runcamDeviceClose5KeyOSDCableConnection(camDevice, NULL); - EXPECT_EQ(true, result); + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_CONNECTION_CLOSE); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(false, rcdeviceInMenu); clearResponseBuff(); // close connection with correct response but wrong data length + addResponseData(responseDataOfOpenConnection, sizeof(responseDataOfOpenConnection), true); + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_CONNECTION_OPEN); // open menu again + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(true, rcdeviceInMenu); + clearResponseBuff(); + uint8_t responseDataOfCloseConnection1[] = { 0xCC, 0x21, 0x11, 0xC1 }; addResponseData(responseDataOfCloseConnection1, sizeof(responseDataOfCloseConnection1), true); - result = runcamDeviceClose5KeyOSDCableConnection(camDevice, NULL); - EXPECT_EQ(true, result); + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_CONNECTION_CLOSE); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(false, rcdeviceInMenu); clearResponseBuff(); // close connection with response that invalid crc + addResponseData(responseDataOfOpenConnection, sizeof(responseDataOfOpenConnection), true); + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_CONNECTION_OPEN); // open menu again + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(true, rcdeviceInMenu); + clearResponseBuff(); + uint8_t responseDataOfCloseConnection2[] = { 0xCC, 0x21, 0xA1 }; addResponseData(responseDataOfCloseConnection2, sizeof(responseDataOfCloseConnection2), true); - result = runcamDeviceClose5KeyOSDCableConnection(camDevice, NULL); - EXPECT_EQ(false, result); + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_CONNECTION_CLOSE); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(true, rcdeviceInMenu); + clearResponseBuff(); + + // release button first + uint8_t responseDataOfSimulation4[] = { 0xCC, 0xA5 }; + addResponseData(responseDataOfSimulation4, sizeof(responseDataOfSimulation4), true); + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_RELEASE); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(false, isButtonPressed); clearResponseBuff(); // simulate press button with no response - result = runcamDeviceSimulate5KeyOSDCableButtonPress(camDevice, RCDEVICE_PROTOCOL_5KEY_SIMULATION_SET); - EXPECT_EQ(false, result); + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_ENTER); + testData.millis += 2000; + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(false, isButtonPressed); clearResponseBuff(); // simulate press button with correct response uint8_t responseDataOfSimulation1[] = { 0xCC, 0xA5 }; addResponseData(responseDataOfSimulation1, sizeof(responseDataOfSimulation1), true); - result = runcamDeviceSimulate5KeyOSDCableButtonPress(camDevice, RCDEVICE_PROTOCOL_5KEY_SIMULATION_SET); - EXPECT_EQ(true, result); + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_ENTER); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(true, isButtonPressed); clearResponseBuff(); // simulate press button with correct response but wrong data length + addResponseData(responseDataOfSimulation4, sizeof(responseDataOfSimulation4), true); // release first + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_RELEASE); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(false, isButtonPressed); + clearResponseBuff(); + uint8_t responseDataOfSimulation2[] = { 0xCC, 0xA5, 0x22 }; addResponseData(responseDataOfSimulation2, sizeof(responseDataOfSimulation2), true); - result = runcamDeviceSimulate5KeyOSDCableButtonPress(camDevice, RCDEVICE_PROTOCOL_5KEY_SIMULATION_SET); - EXPECT_EQ(true, result); + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_ENTER); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(true, isButtonPressed); clearResponseBuff(); // simulate press button event with incorrect response uint8_t responseDataOfSimulation3[] = { 0xCC, 0xB5, 0x22 }; addResponseData(responseDataOfSimulation3, sizeof(responseDataOfSimulation3), true); - result = runcamDeviceSimulate5KeyOSDCableButtonPress(camDevice, RCDEVICE_PROTOCOL_5KEY_SIMULATION_SET); - EXPECT_EQ(false, result); - clearResponseBuff(); - - // simulate release button event - result = runcamDeviceSimulate5KeyOSDCableButtonRelease(camDevice); - EXPECT_EQ(false, result); + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_ENTER); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(true, isButtonPressed); clearResponseBuff(); // simulate release button with correct response - uint8_t responseDataOfSimulation4[] = { 0xCC, 0xA5 }; addResponseData(responseDataOfSimulation4, sizeof(responseDataOfSimulation4), true); - result = runcamDeviceSimulate5KeyOSDCableButtonRelease(camDevice); - EXPECT_EQ(true, result); + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_RELEASE); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(false, isButtonPressed); clearResponseBuff(); // simulate release button with correct response but wrong data length + addResponseData(responseDataOfSimulation1, sizeof(responseDataOfSimulation1), true); // press first + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_ENTER); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(true, isButtonPressed); + clearResponseBuff(); + uint8_t responseDataOfSimulation5[] = { 0xCC, 0xA5, 0xFF }; addResponseData(responseDataOfSimulation5, sizeof(responseDataOfSimulation5), true); - result = runcamDeviceSimulate5KeyOSDCableButtonRelease(camDevice); - EXPECT_EQ(true, result); + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_RELEASE); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(false, isButtonPressed); clearResponseBuff(); // simulate release button with incorrect response uint8_t responseDataOfSimulation6[] = { 0xCC, 0x31, 0xFF }; addResponseData(responseDataOfSimulation6, sizeof(responseDataOfSimulation6), true); - result = runcamDeviceSimulate5KeyOSDCableButtonRelease(camDevice); - EXPECT_EQ(false, result); + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_RELEASE); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(false, isButtonPressed); clearResponseBuff(); } TEST(RCDeviceTest, Test5KeyOSDCableSimulationWithout5KeyFeatureSupport) { + resetRCDeviceStatus(); + // test simulation without device init rcData[THROTTLE] = FIVE_KEY_JOYSTICK_MID; // THROTTLE Mid rcData[ROLL] = FIVE_KEY_JOYSTICK_MID; // ROLL Mid rcData[PITCH] = FIVE_KEY_JOYSTICK_MID; // PITCH Mid rcData[YAW] = FIVE_KEY_JOYSTICK_MAX; // Yaw High - rcdeviceUpdate(0); + rcdeviceUpdate(millis() * 1000); EXPECT_EQ(false, rcdeviceInMenu); // init device that have not 5 key OSD cable simulation feature @@ -498,889 +674,24 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationWithout5KeyFeatureSupport) testData.maxTimesOfRespDataAvailable = 0; uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBD }; addResponseData(responseData, sizeof(responseData), true); - bool result = rcdeviceInit(); - EXPECT_EQ(result, true); + rcdeviceInit(); + rcdeviceReceive(millis() * 1000); + testData.millis += 200; + testData.responseDataReadPos = 0; + testData.indexOfCurrentRespBuf = 0; + rcdeviceReceive(millis() * 1000); + testData.millis += 200; + EXPECT_EQ(camDevice->isReady, true); clearResponseBuff(); // open connection, rcdeviceInMenu will be false if the codes is right uint8_t responseDataOfOpenConnection[] = { 0xCC, 0x11, 0xe7 }; addResponseData(responseDataOfOpenConnection, sizeof(responseDataOfOpenConnection), false); - rcdeviceUpdate(0); + rcdeviceUpdate(millis() * 1000); EXPECT_EQ(false, rcdeviceInMenu); clearResponseBuff(); } -TEST(RCDeviceTest, Test5KeyOSDCableSimulationWith5KeyFeatureSupport) -{ - // test simulation without device init - rcData[THROTTLE] = FIVE_KEY_JOYSTICK_MID; // THROTTLE Mid - rcData[ROLL] = FIVE_KEY_JOYSTICK_MID; // ROLL Mid - rcData[PITCH] = FIVE_KEY_JOYSTICK_MID; // PITCH Mid - rcData[YAW] = FIVE_KEY_JOYSTICK_MAX; // Yaw High - rcdeviceUpdate(0); - EXPECT_EQ(false, rcdeviceInMenu); - - // init device that have not 5 key OSD cable simulation feature - memset(&testData, 0, sizeof(testData)); - testData.isRunCamSplitOpenPortSupported = true; - testData.isRunCamSplitPortConfigurated = true; - testData.isAllowBufferReadWrite = true; - testData.maxTimesOfRespDataAvailable = 0; - uint8_t responseData[] = { 0xCC, 0x01, 0x3F, 0x00, 0xE5 }; - addResponseData(responseData, sizeof(responseData), true); - bool result = rcdeviceInit(); - EXPECT_EQ(result, true); - clearResponseBuff(); - - // open connection - uint8_t responseDataOfOpenConnection[] = { 0xCC, 0x11, 0xe7 }; - addResponseData(responseDataOfOpenConnection, sizeof(responseDataOfOpenConnection), true); - rcdeviceUpdate(0); - EXPECT_EQ(true, rcdeviceInMenu); - EXPECT_EQ(true, needRelease); - clearResponseBuff(); - // relase button - rcData[ROLL] = FIVE_KEY_JOYSTICK_MID; // ROLL Mid - rcData[PITCH] = FIVE_KEY_JOYSTICK_MID; // PITCH Mid - rcData[YAW] = FIVE_KEY_JOYSTICK_MID; // Yaw Mid - uint8_t responseDataOfReleaseButton[] = { 0xCC, 0xA5 }; - addResponseData(responseDataOfReleaseButton, sizeof(responseDataOfReleaseButton), true); - rcdeviceUpdate(0); - EXPECT_EQ(false, needRelease); - clearResponseBuff(); - - // close connection - rcData[THROTTLE] = FIVE_KEY_JOYSTICK_MID; // THROTTLE Mid - rcData[ROLL] = FIVE_KEY_JOYSTICK_MID; // ROLL Mid - rcData[PITCH] = FIVE_KEY_JOYSTICK_MID; // PITCH Mid - rcData[YAW] = FIVE_KEY_JOYSTICK_MIN; // Yaw Low - uint8_t responseDataOfCloseConnection[] = { 0xCC, 0x21, 0x11 }; - addResponseData(responseDataOfCloseConnection, sizeof(responseDataOfCloseConnection), true); - rcdeviceUpdate(0); - EXPECT_EQ(false, rcdeviceInMenu); - EXPECT_EQ(true, needRelease); - clearResponseBuff(); - // relase button - rcData[ROLL] = FIVE_KEY_JOYSTICK_MID; // ROLL Mid - rcData[PITCH] = FIVE_KEY_JOYSTICK_MID; // PITCH Mid - rcData[YAW] = FIVE_KEY_JOYSTICK_MID; // Yaw Mid - addResponseData(responseDataOfReleaseButton, sizeof(responseDataOfReleaseButton), true); - rcdeviceUpdate(0); - EXPECT_EQ(false, needRelease); - clearResponseBuff(); - - // open osd menu again - rcData[THROTTLE] = FIVE_KEY_JOYSTICK_MID; // THROTTLE Mid - rcData[ROLL] = FIVE_KEY_JOYSTICK_MID; // ROLL Mid - rcData[PITCH] = FIVE_KEY_JOYSTICK_MID; // PITCH Mid - rcData[YAW] = FIVE_KEY_JOYSTICK_MAX; // Yaw High - addResponseData(responseDataOfOpenConnection, sizeof(responseDataOfOpenConnection), true); - rcdeviceUpdate(0); - EXPECT_EQ(true, rcdeviceInMenu); - EXPECT_EQ(true, needRelease); - clearResponseBuff(); - // relase button - rcData[ROLL] = FIVE_KEY_JOYSTICK_MID; // ROLL Mid - rcData[PITCH] = FIVE_KEY_JOYSTICK_MID; // PITCH Mid - rcData[YAW] = FIVE_KEY_JOYSTICK_MID; // Yaw Mid - addResponseData(responseDataOfReleaseButton, sizeof(responseDataOfReleaseButton), true); - rcdeviceUpdate(0); - EXPECT_EQ(false, needRelease); - clearResponseBuff(); - - // send down button event - rcData[PITCH] = FIVE_KEY_JOYSTICK_MIN; - addResponseData(responseDataOfReleaseButton, sizeof(responseDataOfReleaseButton), true); - rcdeviceUpdate(0); - EXPECT_EQ(true, needRelease); - clearResponseBuff(); - // relase button - rcData[ROLL] = FIVE_KEY_JOYSTICK_MID; // ROLL Mid - rcData[PITCH] = FIVE_KEY_JOYSTICK_MID; // PITCH Mid - rcData[YAW] = FIVE_KEY_JOYSTICK_MID; // Yaw Mid - addResponseData(responseDataOfReleaseButton, sizeof(responseDataOfReleaseButton), true); - rcdeviceUpdate(0); - EXPECT_EQ(false, needRelease); - rcData[PITCH] = FIVE_KEY_JOYSTICK_MID; // rest down button - clearResponseBuff(); - - // simulate right button long press - rcData[ROLL] = FIVE_KEY_JOYSTICK_MAX; - addResponseData(responseDataOfReleaseButton, sizeof(responseDataOfReleaseButton), true); - rcdeviceUpdate(0); - EXPECT_EQ(true, needRelease); - rcdeviceUpdate(0); - EXPECT_EQ(true, needRelease); - rcdeviceUpdate(0); - EXPECT_EQ(true, needRelease); - clearResponseBuff(); - // send relase button event - rcData[ROLL] = FIVE_KEY_JOYSTICK_MID; // ROLL Mid - rcData[PITCH] = FIVE_KEY_JOYSTICK_MID; // PITCH Mid - rcData[YAW] = FIVE_KEY_JOYSTICK_MID; // Yaw Mid - addResponseData(responseDataOfReleaseButton, sizeof(responseDataOfReleaseButton), true); - rcdeviceUpdate(0); - EXPECT_EQ(false, needRelease); - rcData[ROLL] = FIVE_KEY_JOYSTICK_MID; // reset right button - clearResponseBuff(); - - // simulate right button and get failed response, then FC should release the controller of joysticks - rcData[ROLL] = FIVE_KEY_JOYSTICK_MAX; - addResponseData(responseDataOfReleaseButton, sizeof(responseDataOfReleaseButton), true); - rcdeviceUpdate(0); - EXPECT_EQ(true, needRelease); - clearResponseBuff(); - // send relase button with empty response, so the release will failed - rcData[ROLL] = FIVE_KEY_JOYSTICK_MID; // ROLL Mid - rcData[PITCH] = FIVE_KEY_JOYSTICK_MID; // PITCH Mid - rcData[YAW] = FIVE_KEY_JOYSTICK_MID; // Yaw Mid - rcdeviceUpdate(0); - EXPECT_EQ(true, needRelease); - EXPECT_EQ(false, rcdeviceInMenu); // if failed on release button event, then FC side need release the controller of joysticks - rcData[ROLL] = FIVE_KEY_JOYSTICK_MID; // rest right button - // send again release button with correct response - rcData[ROLL] = FIVE_KEY_JOYSTICK_MID; // ROLL Mid - rcData[PITCH] = FIVE_KEY_JOYSTICK_MID; // PITCH Mid - rcData[YAW] = FIVE_KEY_JOYSTICK_MID; // Yaw Mid - clearResponseBuff(); - addResponseData(responseDataOfReleaseButton, sizeof(responseDataOfReleaseButton), true); - rcdeviceUpdate(0); - EXPECT_EQ(false, needRelease); - EXPECT_EQ(false, rcdeviceInMenu); - clearResponseBuff(); - - // open OSD menu again - rcData[THROTTLE] = FIVE_KEY_JOYSTICK_MID; // THROTTLE Mid - rcData[ROLL] = FIVE_KEY_JOYSTICK_MID; // ROLL Mid - rcData[PITCH] = FIVE_KEY_JOYSTICK_MID; // PITCH Mid - rcData[YAW] = FIVE_KEY_JOYSTICK_MAX; // Yaw High - clearResponseBuff(); - addResponseData(responseDataOfOpenConnection, sizeof(responseDataOfOpenConnection), true); - rcdeviceUpdate(0); - EXPECT_EQ(true, rcdeviceInMenu); - EXPECT_EQ(true, needRelease); - clearResponseBuff(); - - // relase button - rcData[ROLL] = FIVE_KEY_JOYSTICK_MID; // ROLL Mid - rcData[PITCH] = FIVE_KEY_JOYSTICK_MID; // PITCH Mid - rcData[YAW] = FIVE_KEY_JOYSTICK_MID; // Yaw Mid - addResponseData(responseDataOfReleaseButton, sizeof(responseDataOfReleaseButton), true); - rcdeviceUpdate(0); - EXPECT_EQ(false, needRelease); - clearResponseBuff(); - - // send left event - rcData[ROLL] = FIVE_KEY_JOYSTICK_MIN; - addResponseData(responseDataOfReleaseButton, sizeof(responseDataOfReleaseButton), true); - rcdeviceUpdate(0); - EXPECT_EQ(true, needRelease); - clearResponseBuff(); - // send relase button - rcData[ROLL] = FIVE_KEY_JOYSTICK_MID; // ROLL Mid - rcData[PITCH] = FIVE_KEY_JOYSTICK_MID; // PITCH Mid - rcData[YAW] = FIVE_KEY_JOYSTICK_MID; // Yaw Mid - addResponseData(responseDataOfReleaseButton, sizeof(responseDataOfReleaseButton), true); - rcdeviceUpdate(0); - EXPECT_EQ(false, needRelease); - EXPECT_EQ(true, rcdeviceInMenu); - clearResponseBuff(); - rcData[ROLL] = FIVE_KEY_JOYSTICK_MID; // rest right button - - // close connection - rcData[THROTTLE] = FIVE_KEY_JOYSTICK_MID; // THROTTLE Mid - rcData[ROLL] = FIVE_KEY_JOYSTICK_MID; // ROLL Mid - rcData[PITCH] = FIVE_KEY_JOYSTICK_MID; // PITCH Mid - rcData[YAW] = FIVE_KEY_JOYSTICK_MIN; // Yaw High - addResponseData(responseDataOfCloseConnection, sizeof(responseDataOfCloseConnection), true); - rcdeviceUpdate(0); - EXPECT_EQ(false, rcdeviceInMenu); - EXPECT_EQ(true, needRelease); - clearResponseBuff(); - // relase button - rcData[ROLL] = FIVE_KEY_JOYSTICK_MID; // ROLL Mid - rcData[PITCH] = FIVE_KEY_JOYSTICK_MID; // PITCH Mid - rcData[YAW] = FIVE_KEY_JOYSTICK_MID; // Yaw Mid - addResponseData(responseDataOfReleaseButton, sizeof(responseDataOfReleaseButton), true); - rcdeviceUpdate(0); - EXPECT_EQ(false, needRelease); - clearResponseBuff(); -} - -TEST(RCDeviceTest, TestOSDDeviceSupportWithAutoDetectVideoSystem) -{ - memset(&testData, 0, sizeof(testData)); - testData.isRunCamSplitOpenPortSupported = true; - testData.isRunCamSplitPortConfigurated = true; - testData.isAllowBufferReadWrite = true; - testData.maxTimesOfRespDataAvailable = 0; - uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBD }; - uint8_t respDataOfGettingColumnsDetail[] = { 0xCC, 0x00, 0x05, 0x00, 0x1E, 0x1E, 0x1E, 0x01, 0xB0 }; - uint8_t respDataOfGettingTVModeDetail[] = { 0xCC, 0x00, 0x0B, 0x09, 0x00, 0x4E, 0x54, 0x53, 0x43, 0x3B, 0x50, 0x41, 0x4C, 0x00, 0x08 }; - uint8_t respDataOfWriteBFCharset[] = { 0xCC, 0x00, 0x00, 0x39 }; - addResponseData(responseData, sizeof(responseData), true); // device init response - addResponseData(respDataOfGettingColumnsDetail, sizeof(respDataOfGettingColumnsDetail), false); // get column counts response - addResponseData(respDataOfGettingTVModeDetail, sizeof(respDataOfGettingTVModeDetail), false); // get tv mode response - addResponseData(respDataOfWriteBFCharset, sizeof(respDataOfWriteBFCharset), false); // update charset with BF - vcdProfile_t profile; - profile.video_system = VIDEO_SYSTEM_AUTO; // use auto detect - memset(&profile, 0, sizeof(vcdProfile_t)); - bool r = rcdeviceOSDInit(&profile); - EXPECT_EQ(true, r); -} - -TEST(RCDeviceTest, TestOSDDeviceSupportWithGetColumnsFailed) -{ - memset(&testData, 0, sizeof(testData)); - testData.isRunCamSplitOpenPortSupported = true; - testData.isRunCamSplitPortConfigurated = true; - testData.isAllowBufferReadWrite = true; - testData.maxTimesOfRespDataAvailable = 0; - uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBD }; - uint8_t respDataOfGettingColumnsDetail[] = { 0xCC, 0x00, 0x00, 0x1E, 0x1E, 0x1E, 0x01, 0xFf }; - uint8_t respDataOfWriteDataFailed[] = { 0xCC, 0x01, 0x00, 0x32 }; - uint8_t respDataOfWriteDataSuccess[] = { 0xCC, 0x00, 0x00, 0x39 }; - addResponseData(responseData, sizeof(responseData), true); // device init response - addResponseData(respDataOfGettingColumnsDetail, sizeof(respDataOfGettingColumnsDetail), false); // get column counts response - addResponseData(respDataOfWriteDataSuccess, sizeof(respDataOfWriteDataSuccess), false); // update TV mode with Pal - addResponseData(respDataOfWriteDataFailed, sizeof(respDataOfWriteDataFailed), false); // update charset with BF - vcdProfile_t profile; - memset(&profile, 0, sizeof(vcdProfile_t)); - profile.video_system = VIDEO_SYSTEM_PAL; // use auto detect - bool r = rcdeviceOSDInit(&profile); - EXPECT_EQ(false, r); -} - -TEST(RCDeviceTest, TestOSDDeviceSupportWithPal) -{ - memset(&testData, 0, sizeof(testData)); - testData.isRunCamSplitOpenPortSupported = true; - testData.isRunCamSplitPortConfigurated = true; - testData.isAllowBufferReadWrite = true; - testData.maxTimesOfRespDataAvailable = 0; - uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBD }; - uint8_t respDataOfGettingColumnsDetail[] = { 0xCC, 0x00, 0x05, 0x00, 0x1E, 0x1E, 0x1E, 0x01, 0xB0 }; - uint8_t respDataOfWriteDataSuccess[] = { 0xCC, 0x00, 0x00, 0x39 }; - addResponseData(responseData, sizeof(responseData), true); // device init response - addResponseData(respDataOfGettingColumnsDetail, sizeof(respDataOfGettingColumnsDetail), false); // get column counts response - addResponseData(respDataOfWriteDataSuccess, sizeof(respDataOfWriteDataSuccess), false); // update TV mode with Pal - addResponseData(respDataOfWriteDataSuccess, sizeof(respDataOfWriteDataSuccess), false); // update charset with BF - vcdProfile_t profile; - memset(&profile, 0, sizeof(vcdProfile_t)); - profile.video_system = VIDEO_SYSTEM_PAL; // use auto detect - bool r = rcdeviceOSDInit(&profile); - EXPECT_EQ(true, r); -} - -TEST(RCDeviceTest, TestOSDDeviceSupportWithWriteTVModeFailed) -{ - memset(&testData, 0, sizeof(testData)); - testData.isRunCamSplitOpenPortSupported = true; - testData.isRunCamSplitPortConfigurated = true; - testData.isAllowBufferReadWrite = true; - testData.maxTimesOfRespDataAvailable = 0; - uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBD }; - uint8_t respDataOfGettingColumnsDetail[] = { 0xCC, 0x00, 0x05, 0x00, 0x1E, 0x1E, 0x1E, 0x01, 0xB0 }; - uint8_t respDataOfWriteDataFailed[] = { 0xCC, 0x01, 0x00, 0x32 }; - uint8_t respDataOfWriteDataSuccess[] = { 0xCC, 0x00, 0x00, 0x39 }; - addResponseData(responseData, sizeof(responseData), true); // device init response - addResponseData(respDataOfGettingColumnsDetail, sizeof(respDataOfGettingColumnsDetail), false); // get column counts response - addResponseData(respDataOfWriteDataFailed, sizeof(respDataOfWriteDataFailed), false); // update TV mode with Pal - addResponseData(respDataOfWriteDataSuccess, sizeof(respDataOfWriteDataSuccess), false); // update charset with BF - vcdProfile_t profile; - memset(&profile, 0, sizeof(vcdProfile_t)); - profile.video_system = VIDEO_SYSTEM_PAL; // use auto detect - bool r = rcdeviceOSDInit(&profile); - EXPECT_EQ(false, r); -} - -TEST(RCDeviceTest, TestOSDDeviceSupportWithWriteCharsetFailed) -{ - memset(&testData, 0, sizeof(testData)); - testData.isRunCamSplitOpenPortSupported = true; - testData.isRunCamSplitPortConfigurated = true; - testData.isAllowBufferReadWrite = true; - testData.maxTimesOfRespDataAvailable = 0; - uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBD }; - uint8_t respDataOfGettingColumnsDetail[] = { 0xCC, 0x00, 0x05, 0x00, 0x1E, 0x1E, 0x1E, 0x01, 0xB0 }; - uint8_t respDataOfWriteDataFailed[] = { 0xCC, 0x01, 0x00, 0x32 }; - uint8_t respDataOfWriteDataSuccess[] = { 0xCC, 0x00, 0x00, 0x39 }; - addResponseData(responseData, sizeof(responseData), true); // device init response - addResponseData(respDataOfGettingColumnsDetail, sizeof(respDataOfGettingColumnsDetail), false); // get column counts response - addResponseData(respDataOfWriteDataSuccess, sizeof(respDataOfWriteDataSuccess), false); // update TV mode with Pal - addResponseData(respDataOfWriteDataFailed, sizeof(respDataOfWriteDataFailed), false); // update charset with BF - vcdProfile_t profile; - memset(&profile, 0, sizeof(vcdProfile_t)); - profile.video_system = VIDEO_SYSTEM_PAL; // use auto detect - bool r = rcdeviceOSDInit(&profile); - EXPECT_EQ(false, r); -} - - -TEST(RCDeviceTest, TestDSAGetSettings) -{ - // init device that have not 5 key OSD cable simulation feature - memset(&testData, 0, sizeof(testData)); - testData.isRunCamSplitOpenPortSupported = true; - testData.isRunCamSplitPortConfigurated = true; - testData.isAllowBufferReadWrite = true; - testData.maxTimesOfRespDataAvailable = 0; - uint8_t responseData[] = { 0xCC, 0x01, 0x3F, 0x00, 0xE5 }; - addResponseData(responseData, sizeof(responseData), true); - bool result = rcdeviceInit(); - EXPECT_EQ(result, true); - clearResponseBuff(); - - // test getSettings with correct responses - runcamDeviceSetting_t outSettingList[RCDEVICE_PROTOCOL_MAX_MENUITEM_PER_PAGE]; - uint8_t responseDataForGetSettings1[] = { 0xCC, 0x03, 0x18, 0x00, 0x63, 0x68, 0x61, 0x72, 0x73, 0x65, 0x74, 0x00, 0x42, 0x46, 0x00, 0x01, 0x43, 0x4F, 0x4C, 0x55, 0x4D, 0x4E, 0x53, 0x00, 0x33, 0x30, 0x00, 0x22 }; - uint8_t responseDataForGetSettings2[] = { 0xCC, 0x02, 0x23, 0x02, 0x54, 0x56, 0x5F, 0x4D, 0x4F, 0x44, 0x45, 0x00, 0x4E, 0x54, 0x53, 0x43, 0x00, 0x03, 0x53, 0x44, 0x43, 0x41, 0x52, 0x44, 0x5F, 0x43, 0x41, 0x50, 0x41, 0x43, 0x49, 0x54, 0x59, 0x00, 0x30, 0x2F, 0x30, 0x00, 0x9B }; - uint8_t responseDataForGetSettings3[] = { 0xCC, 0x01, 0x33, 0x04, 0x52, 0x45, 0x4D, 0x41, 0x49, 0x4E, 0x49, 0x4E, 0x47, 0x5F, 0x52, 0x45, 0x43, 0x4F, 0x52, 0x44, 0x49, 0x4E, 0x47, 0x5F, 0x54, 0x49, 0x4D, 0x45, 0x00, 0x31, 0x00, 0x05, 0x52, 0x45, 0x53, 0x4F, 0x4C, 0x55, 0x54, 0x49, 0x4F, 0x4E, 0x00, 0x31, 0x30, 0x38, 0x30, 0x70, 0x36, 0x30, 0x66, 0x70, 0x73, 0x00, 0x95 }; - uint8_t responseDataForGetSettings4[] = { 0xCC, 0x00, 0x1F, 0x06, 0x43, 0x41, 0x4D, 0x45, 0x52, 0x41, 0x5F, 0x54, 0x49, 0x4D, 0x45, 0x00, 0x32, 0x30, 0x31, 0x37, 0x31, 0x30, 0x31, 0x30, 0x54, 0x31, 0x37, 0x30, 0x35, 0x31, 0x34, 0x2E, 0x30, 0x00, 0x7C }; - addResponseData(responseDataForGetSettings1, sizeof(responseDataForGetSettings1), true); - addResponseData(responseDataForGetSettings2, sizeof(responseDataForGetSettings2), false); - addResponseData(responseDataForGetSettings3, sizeof(responseDataForGetSettings3), false); - addResponseData(responseDataForGetSettings4, sizeof(responseDataForGetSettings4), false); - result = runcamDeviceGetSettings(camDevice, 0, (runcamDeviceSetting_t*)outSettingList, RCDEVICE_PROTOCOL_MAX_MENUITEM_PER_PAGE); - uint8_t i = 0; - printf("get settings:\n"); - while (i < RCDEVICE_PROTOCOL_MAX_MENUITEM_PER_PAGE && outSettingList[i].name[0] != 0) { - printf("%d\t%-20s\t%-20s\n", outSettingList[i].id, outSettingList[i].name, outSettingList[i].value); - i++; - } - EXPECT_EQ(result, true); - EXPECT_STREQ("charset", outSettingList[0].name); - EXPECT_STREQ("BF", outSettingList[0].value); - EXPECT_STREQ("COLUMNS", outSettingList[1].name); - EXPECT_STREQ("30", outSettingList[1].value); - EXPECT_STREQ("TV_MODE", outSettingList[2].name); - EXPECT_STREQ("NTSC", outSettingList[2].value); - EXPECT_STREQ("SDCARD_CAPACITY", outSettingList[3].name); - EXPECT_STREQ("0/0", outSettingList[3].value); - EXPECT_STREQ("REMAINING_RECORDING_1", outSettingList[4].name); - EXPECT_STREQ("1", outSettingList[4].value); - EXPECT_STREQ("RESOLUTION", outSettingList[5].name); - EXPECT_STREQ("1080p60fps", outSettingList[5].value); - EXPECT_STREQ("CAMERA_TIME", outSettingList[6].name); - EXPECT_STREQ("20171010T170514.0", outSettingList[6].value); - clearResponseBuff(); - - // test get settings with incorrect response - uint8_t responseDataForGetSettings1WithIncorrectCRC[] = { 0xCC, 0x03, 0x18, 0x00, 0x63, 0x68, 0x61, 0x72, 0x73, 0x65, 0x74, 0x00, 0x42, 0x46, 0x00, 0x01, 0x43, 0x4F, 0x4C, 0x55, 0x4D, 0x4E, 0x53, 0x00, 0x33, 0x30, 0x00, 0xA2 }; - addResponseData(responseDataForGetSettings1WithIncorrectCRC, sizeof(responseDataForGetSettings1WithIncorrectCRC), true); - result = runcamDeviceGetSettings(camDevice, 0, (runcamDeviceSetting_t*)outSettingList, RCDEVICE_PROTOCOL_MAX_MENUITEM_PER_PAGE); - EXPECT_EQ(result, false); - clearResponseBuff(); - - // test get settings with incomplete response again - uint8_t responseDataForGetSettings1WithIncorrectLength[] = { 0xCC, 0x03, 0x18, 0x00, 0x63, 0x68, 0x61, 0x72, 0x73, 0x01, 0x43, 0x4F, 0x4C, 0x55, 0x4D, 0x4E, 0x53, 0x00, 0x33, 0x30, 0x00, 0xA2 }; - addResponseData(responseDataForGetSettings1, sizeof(responseDataForGetSettings1), true); - addResponseData(responseDataForGetSettings1WithIncorrectLength, sizeof(responseDataForGetSettings1WithIncorrectLength), false); - result = runcamDeviceGetSettings(camDevice, 0, (runcamDeviceSetting_t*)outSettingList, RCDEVICE_PROTOCOL_MAX_MENUITEM_PER_PAGE); - EXPECT_EQ(result, false); - clearResponseBuff(); - - // test get settings with incorrect length response again - uint8_t responseDataForGetSettings1WithIncorrectLength2[] = { 0xCC, 0x03, 0x3D, 0x00, 0x63, 0x68, 0x61, 0x72, 0x73, 0x65, 0x74, 0x00, 0x42, 0x46, 0x00, 0x01, 0x43, 0x4F, 0x4C, 0x55, 0x4D, 0x4E, 0x53, 0x00, 0x33, 0x30, 0x00, 0xA2 }; - addResponseData(responseDataForGetSettings1, sizeof(responseDataForGetSettings1), true); - addResponseData(responseDataForGetSettings1WithIncorrectLength2, sizeof(responseDataForGetSettings1WithIncorrectLength2), false); - result = runcamDeviceGetSettings(camDevice, 0, (runcamDeviceSetting_t*)outSettingList, RCDEVICE_PROTOCOL_MAX_MENUITEM_PER_PAGE); - EXPECT_EQ(result, false); - clearResponseBuff(); -} - -TEST(RCDeviceTest, TestDSATextSelectionAccess) -{ - // init device that have not 5 key OSD cable simulation feature - memset(&testData, 0, sizeof(testData)); - testData.isRunCamSplitOpenPortSupported = true; - testData.isRunCamSplitPortConfigurated = true; - testData.isAllowBufferReadWrite = true; - testData.maxTimesOfRespDataAvailable = 0; - uint8_t responseData[] = { 0xCC, 0x01, 0x3F, 0x00, 0xE5 }; - addResponseData(responseData, sizeof(responseData), true); - bool result = rcdeviceInit(); - EXPECT_EQ(result, true); - clearResponseBuff(); - - // get setting detail that type is text selection - runcamDeviceSettingDetail_t charsetDetail; - uint8_t responseDataForCharSetDetail1[] = { 0xCC, 0x00, 0x08, 0x09, 0x00, 0x42, 0x46, 0x3B, 0x43, 0x46, 0x00, 0x1C }; - addResponseData(responseDataForCharSetDetail1, sizeof(responseDataForCharSetDetail1), true); - result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_CHARSET, &charsetDetail); - EXPECT_EQ(result, true); - EXPECT_EQ(0, charsetDetail.value); - EXPECT_EQ(RCDEVICE_PROTOCOL_SETTINGTYPE_TEXT_SELECTION, charsetDetail.type); - if (result) { - printf("setting detail:\n"); - printf("\tcurrentValue = %d\n", charsetDetail.value); - printf("\tavailable selections = "); - bool foundCurValue = false; - for (uint32_t i = 0; i < RCDEVICE_PROTOCOL_MAX_TEXT_SELECTIONS; i++) { - if (charsetDetail.textSelections[i].text[0] == 0) { - break; - } - - if (i == charsetDetail.value) { - printf("%s[*], ", charsetDetail.textSelections[i].text); - foundCurValue = true; - } else { - printf("%s, ", charsetDetail.textSelections[i].text); - } - } - printf("\n"); - EXPECT_EQ(foundCurValue, true); - } - clearResponseBuff(); - - // get setting detail that type is text selection with multiple chunk - uint8_t responseDataForCharSetDetailChunk1[] = { 0xcc, 0x02, 0x3a, 0x09, 0x02, 0x4f, 0x4e, 0x45, 0x3b, 0x54, 0x57, 0x4f, 0x3b, 0x54, 0x48, 0x52, 0x45, 0x45, 0x3b, 0x46, 0x4f, 0x55, 0x52, 0x3b, 0x46, 0x49, 0x56, 0x45, 0x3b, 0x53, 0x49, 0x58, 0x3b, 0x53, 0x45, 0x56, 0x45, 0x4e, 0x3b, 0x45, 0x49, 0x47, 0x48, 0x54, 0x3b, 0x4e, 0x49, 0x4e, 0x45, 0x3b, 0x54, 0x45, 0x4e, 0x3b, 0x45, 0x6c, 0x65, 0x76, 0x65, 0x6e, 0x3b, 0x8e }; - uint8_t responseDataForCharSetDetailChunk2[] = { 0xcc, 0x01, 0x38, 0x54, 0x77, 0x65, 0x6c, 0x76, 0x65, 0x3b, 0x20, 0x74, 0x68, 0x69, 0x72, 0x74, 0x65, 0x65, 0x6e, 0x3b, 0x20, 0x66, 0x6f, 0x75, 0x72, 0x74, 0x65, 0x65, 0x6e, 0x3b, 0x20, 0x66, 0x69, 0x66, 0x74, 0x65, 0x65, 0x6e, 0x3b, 0x20, 0x73, 0x69, 0x78, 0x74, 0x65, 0x65, 0x6e, 0x3b, 0x20, 0x73, 0x65, 0x76, 0x65, 0x6e, 0x74, 0x65, 0x65, 0x6e, 0x3b, 0x96 }; - uint8_t responseDataForCharSetDetailChunk3[] = { 0xcc, 0x00, 0x34, 0x45, 0x69, 0x67, 0x68, 0x74, 0x65, 0x65, 0x6e, 0x3b, 0x20, 0x4e, 0x69, 0x6e, 0x65, 0x74, 0x65, 0x65, 0x6e, 0x3b, 0x20, 0x74, 0x77, 0x65, 0x6e, 0x74, 0x79, 0x3b, 0x20, 0x74, 0x77, 0x65, 0x6e, 0x74, 0x79, 0x20, 0x6f, 0x6e, 0x65, 0x3b, 0x20, 0x74, 0x77, 0x65, 0x6e, 0x74, 0x79, 0x20, 0x74, 0x77, 0x6f, 0x3b, 0x00, 0x31 }; - addResponseData(responseDataForCharSetDetailChunk1, sizeof(responseDataForCharSetDetailChunk1), true); - addResponseData(responseDataForCharSetDetailChunk2, sizeof(responseDataForCharSetDetailChunk2), false); - addResponseData(responseDataForCharSetDetailChunk3, sizeof(responseDataForCharSetDetailChunk3), false); - result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_CHARSET, &charsetDetail); - EXPECT_EQ(result, true); - EXPECT_EQ(2, charsetDetail.value); - EXPECT_EQ(RCDEVICE_PROTOCOL_SETTINGTYPE_TEXT_SELECTION, charsetDetail.type); - if (result) { - printf("setting detail:\n"); - printf("\tcurrentValue = %d\n", charsetDetail.value); - printf("\tavailable selections = "); - bool foundCurValue = false; - for (uint32_t i = 0; i < RCDEVICE_PROTOCOL_MAX_TEXT_SELECTIONS; i++) { - if (charsetDetail.textSelections[i].text[0] == 0) { - break; - } - - if (i == charsetDetail.value) { - printf("%s[*], ", charsetDetail.textSelections[i].text); - foundCurValue = true; - } else { - printf("%s, ", charsetDetail.textSelections[i].text); - } - } - printf("\n"); - EXPECT_EQ(foundCurValue, true); - } - clearResponseBuff(); - - // test get setting detail that type is textselection, but the response is incorrect - uint8_t incorrectResponseDataForCharSetDetail1[] = { 0xCC, 0x00, 0x08, 0x09, 0x00, 0x42, 0x46, 0x3B, 0x43, 0x46, 0x00, 0x1A }; - addResponseData(incorrectResponseDataForCharSetDetail1, sizeof(incorrectResponseDataForCharSetDetail1), true); - result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_CHARSET, &charsetDetail); - EXPECT_EQ(result, false); - clearResponseBuff(); - - // test get setting detail that type is textselection, but the response length is incorrect - uint8_t incorrectResponseDataForCharSetDetail2[] = { 0xCC, 0x00, 0x08, 0x09, 0x00, 0x42 }; - addResponseData(incorrectResponseDataForCharSetDetail2, sizeof(incorrectResponseDataForCharSetDetail2), true); - result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_CHARSET, &charsetDetail); - EXPECT_EQ(result, false); - clearResponseBuff(); - - // test get setting detail that type is textselection multiple chunk, but the response length is incorrect - uint8_t incorrectResponseDataForCharSetDetailChunk2[] = { 0xcc, 0x01, 0x38, 0x54, 0x77, 0x65, 0x6c, 0x76, 0x65, 0x3b, 0x20, 0x74, 0x68, 0x69, 0x72, 0x74, 0x65, 0x65, 0x6e, 0x3b, 0x20, 0x66, 0x6f, 0x75, 0x72, 0x74, 0x65, 0x65, 0x6e, 0x3b, 0x20, 0x66, 0x69, 0x66, 0x74, 0x65, 0x65, 0x6e, 0x3b, 0x20, 0x73, 0x74, 0x65, 0x65, 0x6e, 0x3b, 0x20, 0x73, 0x65, 0x76, 0x65, 0x6e, 0x74, 0x65, 0x65, 0x6e, 0x3b, 0x96 }; - addResponseData(responseDataForCharSetDetailChunk1, sizeof(responseDataForCharSetDetailChunk1), true); - addResponseData(incorrectResponseDataForCharSetDetailChunk2, sizeof(incorrectResponseDataForCharSetDetailChunk2), false); - result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_CHARSET, &charsetDetail); - EXPECT_EQ(result, false); - clearResponseBuff(); - - // test update setting - runcamDeviceWriteSettingResponse_t updateSettingResponse; - uint8_t responweDataForUpdateCharSet1[] = { 0xCC, 0x00, 0x00, 0x39 }; - addResponseData(responweDataForUpdateCharSet1, sizeof(responweDataForUpdateCharSet1), true); - uint8_t newValue = 1; - result = runcamDeviceWriteSetting(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_CHARSET, &newValue, sizeof(uint8_t), &updateSettingResponse); - EXPECT_EQ(result, true); - EXPECT_EQ(0, updateSettingResponse.resultCode); - EXPECT_EQ(0, updateSettingResponse.needUpdateMenuItems); - clearResponseBuff(); - - // test update setting with response that needUpdatemenuItems's value is 1 - uint8_t responweDataForUpdateCharSet2[] = { 0xCC, 0x00, 0x01, 0xec }; - addResponseData(responweDataForUpdateCharSet2, sizeof(responweDataForUpdateCharSet2), true); - newValue = 1; - result = runcamDeviceWriteSetting(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_CHARSET, &newValue, sizeof(uint8_t), &updateSettingResponse); - EXPECT_EQ(result, true); - EXPECT_EQ(0, updateSettingResponse.resultCode); - EXPECT_EQ(1, updateSettingResponse.needUpdateMenuItems); - clearResponseBuff(); - - // test update setting with response that resultCode's value is 1 - uint8_t responweDataForUpdateCharSet3[] = { 0xCC, 0x01, 0x00, 0x32 }; - addResponseData(responweDataForUpdateCharSet3, sizeof(responweDataForUpdateCharSet3), true); - newValue = 1; - result = runcamDeviceWriteSetting(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_CHARSET, &newValue, sizeof(uint8_t), &updateSettingResponse); - EXPECT_EQ(result, true); - EXPECT_EQ(1, updateSettingResponse.resultCode); - EXPECT_EQ(0, updateSettingResponse.needUpdateMenuItems); - clearResponseBuff(); -} - -TEST(RCDeviceTest, TestDSAUint8AccessProtocol) -{ - // init device that have not 5 key OSD cable simulation feature - memset(&testData, 0, sizeof(testData)); - testData.isRunCamSplitOpenPortSupported = true; - testData.isRunCamSplitPortConfigurated = true; - testData.isAllowBufferReadWrite = true; - testData.maxTimesOfRespDataAvailable = 0; - uint8_t responseData[] = { 0xCC, 0x01, 0x3F, 0x00, 0xE5 }; - addResponseData(responseData, sizeof(responseData), true); - bool result = rcdeviceInit(); - EXPECT_EQ(result, true); - clearResponseBuff(); - - // get setting detail that type is uint8 - runcamDeviceSettingDetail_t charsetDetail; - uint8_t responseDataForColumnsDetail1[] = { 0xCC, 0x00, 0x05, 0x00, 0x1E, 0x1E, 0x1E, 0x01, 0xB0 }; - addResponseData(responseDataForColumnsDetail1, sizeof(responseDataForColumnsDetail1), true); - result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &charsetDetail); - EXPECT_EQ(result, true); - EXPECT_EQ(RCDEVICE_PROTOCOL_SETTINGTYPE_UINT8, charsetDetail.type); - EXPECT_EQ(30, charsetDetail.value); - EXPECT_EQ(30, charsetDetail.minValue); - EXPECT_EQ(30, charsetDetail.maxValue); - EXPECT_EQ(1, charsetDetail.stepSize); - clearResponseBuff(); - - // get setting detail that type is uint8, but the response length is incorrect - uint8_t incorrectResponseDataForColumnsDetail1[] = { 0xCC, 0x00, 0x05, 0x00, 0x1E, 0x1E }; - addResponseData(incorrectResponseDataForColumnsDetail1, sizeof(incorrectResponseDataForColumnsDetail1), true); - result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &charsetDetail); - EXPECT_EQ(result, false); - clearResponseBuff(); - - // get setting detail that type is uint8, but the response crc is incorrect - uint8_t incorrectResponseDataForColumnsDetail2[] = { 0xCC, 0x00, 0x05, 0x00, 0x1E, 0x1E, 0x1E, 0x01, 0xC2 }; - addResponseData(incorrectResponseDataForColumnsDetail2, sizeof(incorrectResponseDataForColumnsDetail2), true); - result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &charsetDetail); - EXPECT_EQ(result, false); - clearResponseBuff(); - - // get setting detail that type is uint8, but the response crc is incorrect - uint8_t incorrectResponseDataForColumnsDetail3[] = { 0xCC, 0x00, 0x05, 0x00, 0x1E, 0x1E, 0x1E, 0x01, 0xB0, 0xAA, 0xBB, 0xCC }; - addResponseData(incorrectResponseDataForColumnsDetail3, sizeof(incorrectResponseDataForColumnsDetail3), true); - result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &charsetDetail); - EXPECT_EQ(result, true); - clearResponseBuff(); - - // test update setting - runcamDeviceWriteSettingResponse_t updateSettingResponse; - uint8_t responweDataForUpdateCharSet1[] = { 0xCC, 0x00, 0x00, 0x39 }; - addResponseData(responweDataForUpdateCharSet1, sizeof(responweDataForUpdateCharSet1), true); - uint8_t newValue = 1; - result = runcamDeviceWriteSetting(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &newValue, sizeof(uint8_t), &updateSettingResponse); - EXPECT_EQ(result, true); - EXPECT_EQ(0, updateSettingResponse.resultCode); - EXPECT_EQ(0, updateSettingResponse.needUpdateMenuItems); - clearResponseBuff(); - - // test update setting with response that needUpdatemenuItems's value is 1 - uint8_t responweDataForUpdateCharSet2[] = { 0xCC, 0x00, 0x01, 0xec }; - addResponseData(responweDataForUpdateCharSet2, sizeof(responweDataForUpdateCharSet2), true); - newValue = 1; - result = runcamDeviceWriteSetting(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &newValue, sizeof(uint8_t), &updateSettingResponse); - EXPECT_EQ(result, true); - EXPECT_EQ(0, updateSettingResponse.resultCode); - EXPECT_EQ(1, updateSettingResponse.needUpdateMenuItems); - clearResponseBuff(); - - // test update setting with response that resultCode's value is 1 - uint8_t responweDataForUpdateCharSet3[] = { 0xCC, 0x01, 0x00, 0x32 }; - addResponseData(responweDataForUpdateCharSet3, sizeof(responweDataForUpdateCharSet3), true); - newValue = 1; - result = runcamDeviceWriteSetting(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &newValue, sizeof(uint8_t), &updateSettingResponse); - EXPECT_EQ(result, true); - EXPECT_EQ(1, updateSettingResponse.resultCode); - EXPECT_EQ(0, updateSettingResponse.needUpdateMenuItems); - clearResponseBuff(); -} - -TEST(RCDeviceTest, TestDSAUint16AccessProtocol) -{ - // init device that have not 5 key OSD cable simulation feature - memset(&testData, 0, sizeof(testData)); - testData.isRunCamSplitOpenPortSupported = true; - testData.isRunCamSplitPortConfigurated = true; - testData.isAllowBufferReadWrite = true; - testData.maxTimesOfRespDataAvailable = 0; - uint8_t responseData[] = { 0xCC, 0x01, 0x3F, 0x00, 0xE5 }; - addResponseData(responseData, sizeof(responseData), true); - bool result = rcdeviceInit(); - EXPECT_EQ(result, true); - clearResponseBuff(); - - // get setting detail that type is uint16 - runcamDeviceSettingDetail_t charsetDetail; - uint8_t responseDataForColumnsDetail1[] = { 0xcc, 0x00, 0x09, 0x02, 0x1e, 0x3a, 0x1e, 0x00, 0xff, 0xff, 0x64, 0x00, 0x6c }; - addResponseData(responseDataForColumnsDetail1, sizeof(responseDataForColumnsDetail1), true); - result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &charsetDetail); - EXPECT_EQ(result, true); - EXPECT_EQ(RCDEVICE_PROTOCOL_SETTINGTYPE_UINT16, charsetDetail.type); - EXPECT_EQ(14878, charsetDetail.value); - EXPECT_EQ(30, charsetDetail.minValue); - EXPECT_EQ(65535, charsetDetail.maxValue); - EXPECT_EQ(100, charsetDetail.stepSize); - clearResponseBuff(); - - // get setting detail that type is uint16, but the response length is incorrect - uint8_t incorrectResponseDataForColumnsDetail1[] = { 0xcc, 0x00, 0x09, 0x02, 0x1e, 0x3a, 0x1e, 0x00, 0xff, 0xff }; - addResponseData(incorrectResponseDataForColumnsDetail1, sizeof(incorrectResponseDataForColumnsDetail1), true); - result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &charsetDetail); - EXPECT_EQ(result, false); - clearResponseBuff(); - - // get setting detail that type is uint16, but the response crc is incorrect - uint8_t incorrectResponseDataForColumnsDetail2[] = { 0xcc, 0x00, 0x09, 0x02, 0x1e, 0x3a, 0x1e, 0x00, 0xff, 0xff, 0x64, 0x00, 0x7c }; - addResponseData(incorrectResponseDataForColumnsDetail2, sizeof(incorrectResponseDataForColumnsDetail2), true); - result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &charsetDetail); - EXPECT_EQ(result, false); - clearResponseBuff(); - - // get setting detail that type is uint16, but the response has some randome data at trailing - uint8_t incorrectResponseDataForColumnsDetail3[] = { 0xcc, 0x00, 0x09, 0x02, 0x1e, 0x3a, 0x1e, 0x00, 0xff, 0xff, 0x64, 0x00, 0x6c, 0xAA, 0xBB, 0xCC }; - addResponseData(incorrectResponseDataForColumnsDetail3, sizeof(incorrectResponseDataForColumnsDetail3), true); - result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &charsetDetail); - EXPECT_EQ(result, true); - clearResponseBuff(); - - // test update setting - runcamDeviceWriteSettingResponse_t updateSettingResponse; - uint8_t responweDataForUpdateCharSet1[] = { 0xCC, 0x00, 0x00, 0x39 }; - addResponseData(responweDataForUpdateCharSet1, sizeof(responweDataForUpdateCharSet1), true); - uint16_t newValue = 55; - result = runcamDeviceWriteSetting(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, (uint8_t*)&newValue, sizeof(uint16_t), &updateSettingResponse); - EXPECT_EQ(result, true); - EXPECT_EQ(0, updateSettingResponse.resultCode); - EXPECT_EQ(0, updateSettingResponse.needUpdateMenuItems); - clearResponseBuff(); - - // test update setting with response that needUpdatemenuItems's value is 1 - uint8_t responweDataForUpdateCharSet2[] = { 0xCC, 0x00, 0x01, 0xec }; - addResponseData(responweDataForUpdateCharSet2, sizeof(responweDataForUpdateCharSet2), true); - newValue = 190; - result = runcamDeviceWriteSetting(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, (uint8_t*)&newValue, sizeof(uint16_t), &updateSettingResponse); - EXPECT_EQ(result, true); - EXPECT_EQ(0, updateSettingResponse.resultCode); - EXPECT_EQ(1, updateSettingResponse.needUpdateMenuItems); - clearResponseBuff(); - - // test update setting with response that resultCode's value is 1 - uint8_t responweDataForUpdateCharSet3[] = { 0xCC, 0x01, 0x00, 0x32 }; - addResponseData(responweDataForUpdateCharSet3, sizeof(responweDataForUpdateCharSet3), true); - newValue = 10241; - result = runcamDeviceWriteSetting(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, (uint8_t*)&newValue, sizeof(uint16_t), &updateSettingResponse); - EXPECT_EQ(result, true); - EXPECT_EQ(1, updateSettingResponse.resultCode); - EXPECT_EQ(0, updateSettingResponse.needUpdateMenuItems); - clearResponseBuff(); -} - -TEST(RCDeviceTest, TestDSAFloatAccessProtocol) -{ - // init device that have not 5 key OSD cable simulation feature - memset(&testData, 0, sizeof(testData)); - testData.isRunCamSplitOpenPortSupported = true; - testData.isRunCamSplitPortConfigurated = true; - testData.isAllowBufferReadWrite = true; - testData.maxTimesOfRespDataAvailable = 0; - uint8_t responseData[] = { 0xCC, 0x01, 0x3F, 0x00, 0xE5 }; - addResponseData(responseData, sizeof(responseData), true); - bool result = rcdeviceInit(); - EXPECT_EQ(result, true); - clearResponseBuff(); - - // get setting detail that type is float - runcamDeviceSettingDetail_t charsetDetail; - uint8_t responseDataForColumnsDetail1[] = { 0xcc, 0x00, 0x12, 0x08, 0x1e, 0x33, 0x00, 0x00, 0x3a, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0xff, 0x04, 0xf4, 0x01, 0x00, 0x00, 0xc9 }; - addResponseData(responseDataForColumnsDetail1, sizeof(responseDataForColumnsDetail1), true); - result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &charsetDetail); - EXPECT_EQ(result, true); - EXPECT_EQ(RCDEVICE_PROTOCOL_SETTINGTYPE_FLOAT, charsetDetail.type); - EXPECT_EQ(13086, charsetDetail.value); - EXPECT_EQ(58, charsetDetail.minValue); - EXPECT_EQ(4294967295, charsetDetail.maxValue); - EXPECT_EQ(4, charsetDetail.decimalPoint); - EXPECT_EQ(500, charsetDetail.stepSize); - clearResponseBuff(); - - // get setting detail that type is float, but the response length is incorrect - uint8_t incorrectResponseDataForColumnsDetail1[] = { 0xcc, 0x00, 0x07, 0x08, 0x1e, 0x3a, 0xff, 0x0a }; - addResponseData(incorrectResponseDataForColumnsDetail1, sizeof(incorrectResponseDataForColumnsDetail1), true); - result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &charsetDetail); - EXPECT_EQ(result, false); - clearResponseBuff(); - - // get setting detail that type is float, but the response crc is incorrect - uint8_t incorrectResponseDataForColumnsDetail2[] = { 0xcc, 0x00, 0x07, 0x08, 0x1e, 0x3a, 0xff, 0x0a, 0x00, 0x01, 0xf2 }; - addResponseData(incorrectResponseDataForColumnsDetail2, sizeof(incorrectResponseDataForColumnsDetail2), true); - result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &charsetDetail); - EXPECT_EQ(result, false); - clearResponseBuff(); - - // get setting detail that type is float, but the response has some randome data at trailing - uint8_t incorrectResponseDataForColumnsDetail3[] = { 0xcc, 0x00, 0x12, 0x08, 0x1e, 0x33, 0x00, 0x00, 0x3a, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0xff, 0x04, 0xf4, 0x01, 0x00, 0x00, 0xc9, 0xAA, 0xBB, 0xCC }; - addResponseData(incorrectResponseDataForColumnsDetail3, sizeof(incorrectResponseDataForColumnsDetail3), true); - result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &charsetDetail); - EXPECT_EQ(result, true); - clearResponseBuff(); - - // test update setting - runcamDeviceWriteSettingResponse_t updateSettingResponse; - uint8_t responweDataForUpdateCharSet1[] = { 0xCC, 0x00, 0x00, 0x39 }; - addResponseData(responweDataForUpdateCharSet1, sizeof(responweDataForUpdateCharSet1), true); - uint16_t newValue[5] = { 0x1E, 0x1E, 0x00, 0x00, 0x2 }; // the data struct is UINT32_T + UINT8(Decimal Point) - result = runcamDeviceWriteSetting(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, (uint8_t*)newValue, sizeof(newValue), &updateSettingResponse); - EXPECT_EQ(result, true); - EXPECT_EQ(0, updateSettingResponse.resultCode); - EXPECT_EQ(0, updateSettingResponse.needUpdateMenuItems); - clearResponseBuff(); - - // test update setting with response that needUpdatemenuItems's value is 1 - uint8_t responweDataForUpdateCharSet2[] = { 0xCC, 0x00, 0x01, 0xec }; - addResponseData(responweDataForUpdateCharSet2, sizeof(responweDataForUpdateCharSet2), true); - result = runcamDeviceWriteSetting(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, (uint8_t*)newValue, sizeof(newValue), &updateSettingResponse); - EXPECT_EQ(result, true); - EXPECT_EQ(0, updateSettingResponse.resultCode); - EXPECT_EQ(1, updateSettingResponse.needUpdateMenuItems); - clearResponseBuff(); - - // test update setting with response that resultCode's value is 1 - uint8_t responweDataForUpdateCharSet3[] = { 0xCC, 0x01, 0x00, 0x32 }; - addResponseData(responweDataForUpdateCharSet3, sizeof(responweDataForUpdateCharSet3), true); - result = runcamDeviceWriteSetting(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, (uint8_t*)newValue, sizeof(newValue), &updateSettingResponse); - EXPECT_EQ(result, true); - EXPECT_EQ(1, updateSettingResponse.resultCode); - EXPECT_EQ(0, updateSettingResponse.needUpdateMenuItems); - clearResponseBuff(); -} - -TEST(RCDeviceTest, TestDSAStringAccessProtocol) -{ - // init device that have not 5 key OSD cable simulation feature - memset(&testData, 0, sizeof(testData)); - testData.isRunCamSplitOpenPortSupported = true; - testData.isRunCamSplitPortConfigurated = true; - testData.isAllowBufferReadWrite = true; - testData.maxTimesOfRespDataAvailable = 0; - uint8_t responseData[] = { 0xCC, 0x01, 0x3F, 0x00, 0xE5 }; - addResponseData(responseData, sizeof(responseData), true); - bool result = rcdeviceInit(); - EXPECT_EQ(result, true); - clearResponseBuff(); - - // get setting detail that type is string - runcamDeviceSettingDetail_t charsetDetail; - uint8_t responseDataForColumnsDetail1[] = { 0xCC, 0x00, 0x14, 0x0A, 0x31, 0x39, 0x37, 0x32, 0x30, 0x32, 0x31, 0x36, 0x54, 0x31, 0x39, 0x31, 0x35, 0x33, 0x32, 0x2E, 0x30, 0x00, 0x14, 0x20 }; - addResponseData(responseDataForColumnsDetail1, sizeof(responseDataForColumnsDetail1), true); - result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &charsetDetail); - EXPECT_EQ(result, true); - EXPECT_EQ(RCDEVICE_PROTOCOL_SETTINGTYPE_STRING, charsetDetail.type); - EXPECT_STREQ("19720216T191532.0", charsetDetail.stringValue); - EXPECT_EQ(20, charsetDetail.maxStringSize); - clearResponseBuff(); - - // get setting detail that type is string, but the response length is incorrect - uint8_t incorrectResponseDataForColumnsDetail1[] = { 0xCC, 0x00, 0x14, 0x0A, 0x31, 0x39, 0x37, 0x32, 0x30, 0x32, 0x31, 0x36, 0x54, 0x31, 0x39, 0x31, 0x35, 0x33, 0x32, 0x2E, 0x30 }; - addResponseData(incorrectResponseDataForColumnsDetail1, sizeof(incorrectResponseDataForColumnsDetail1), true); - result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &charsetDetail); - EXPECT_EQ(result, false); - clearResponseBuff(); - - // get setting detail that type is string, but the response crc is incorrect - uint8_t incorrectResponseDataForColumnsDetail2[] = { 0xCC, 0x00, 0x14, 0x0A, 0x31, 0x39, 0x37, 0x32, 0x30, 0x32, 0x31, 0x36, 0x54, 0x31, 0x39, 0x31, 0x35, 0x33, 0x32, 0x2E, 0x30, 0x00, 0x14, 0x21 }; - addResponseData(incorrectResponseDataForColumnsDetail2, sizeof(incorrectResponseDataForColumnsDetail2), true); - result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &charsetDetail); - EXPECT_EQ(result, false); - clearResponseBuff(); - - // get setting detail that type is string, but the response has some randome data at trailing - uint8_t incorrectResponseDataForColumnsDetail3[] = { 0xCC, 0x00, 0x14, 0x0A, 0x31, 0x39, 0x37, 0x32, 0x30, 0x32, 0x31, 0x36, 0x54, 0x31, 0x39, 0x31, 0x35, 0x33, 0x32, 0x2E, 0x30, 0x00, 0x14, 0x20, 0xAA, 0xBB, 0xCC }; - addResponseData(incorrectResponseDataForColumnsDetail3, sizeof(incorrectResponseDataForColumnsDetail3), true); - result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &charsetDetail); - EXPECT_EQ(result, true); - clearResponseBuff(); - - // test update setting - runcamDeviceWriteSettingResponse_t updateSettingResponse; - uint8_t responweDataForUpdateCharSet1[] = { 0xCC, 0x00, 0x00, 0x39 }; - addResponseData(responweDataForUpdateCharSet1, sizeof(responweDataForUpdateCharSet1), true); - uint16_t newValue[] = { 0x31, 0x39, 0x37, 0x32, 0x30, 0x32, 0x31, 0x36, 0x54, 0x31, 0x39, 0x31, 0x35, 0x33, 0x32, 0x2E, 0x30, 0x00 }; // the data struct is UINT32_T + UINT8(Decimal Point) - result = runcamDeviceWriteSetting(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, (uint8_t*)newValue, sizeof(newValue), &updateSettingResponse); - EXPECT_EQ(result, true); - EXPECT_EQ(0, updateSettingResponse.resultCode); - EXPECT_EQ(0, updateSettingResponse.needUpdateMenuItems); - clearResponseBuff(); - - // test update setting with response that needUpdatemenuItems's value is 1 - uint8_t responweDataForUpdateCharSet2[] = { 0xCC, 0x00, 0x01, 0xec }; - addResponseData(responweDataForUpdateCharSet2, sizeof(responweDataForUpdateCharSet2), true); - result = runcamDeviceWriteSetting(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, (uint8_t*)newValue, sizeof(newValue), &updateSettingResponse); - EXPECT_EQ(result, true); - EXPECT_EQ(0, updateSettingResponse.resultCode); - EXPECT_EQ(1, updateSettingResponse.needUpdateMenuItems); - clearResponseBuff(); - - // test update setting with response that resultCode's value is 1 - uint8_t responweDataForUpdateCharSet3[] = { 0xCC, 0x01, 0x00, 0x32 }; - addResponseData(responweDataForUpdateCharSet3, sizeof(responweDataForUpdateCharSet3), true); - result = runcamDeviceWriteSetting(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, (uint8_t*)newValue, sizeof(newValue), &updateSettingResponse); - EXPECT_EQ(result, true); - EXPECT_EQ(1, updateSettingResponse.resultCode); - EXPECT_EQ(0, updateSettingResponse.needUpdateMenuItems); - clearResponseBuff(); -} - -TEST(RCDeviceTest, TestDSAInfoAccessProtocol) -{ - // init device that have not 5 key OSD cable simulation feature - memset(&testData, 0, sizeof(testData)); - testData.isRunCamSplitOpenPortSupported = true; - testData.isRunCamSplitPortConfigurated = true; - testData.isAllowBufferReadWrite = true; - testData.maxTimesOfRespDataAvailable = 0; - uint8_t responseData[] = { 0xCC, 0x01, 0x3F, 0x00, 0xE5 }; - addResponseData(responseData, sizeof(responseData), true); - bool result = rcdeviceInit(); - EXPECT_EQ(result, true); - clearResponseBuff(); - - // get setting detail that type is string - runcamDeviceSettingDetail_t charsetDetail; - uint8_t responseDataForColumnsDetail1[] = { 0xCC, 0x00, 0x0B, 0x0C, 0x31, 0x31, 0x35, 0x2F, 0x36, 0x30, 0x38, 0x39, 0x30, 0x00, 0xFE }; - addResponseData(responseDataForColumnsDetail1, sizeof(responseDataForColumnsDetail1), true); - result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &charsetDetail); - EXPECT_EQ(result, true); - EXPECT_EQ(RCDEVICE_PROTOCOL_SETTINGTYPE_INFO, charsetDetail.type); - EXPECT_STREQ("115/60890", charsetDetail.stringValue); - clearResponseBuff(); - - // get setting detail that type is string, but the response length is incorrect - uint8_t incorrectResponseDataForColumnsDetail1[] = { 0xCC, 0x00, 0x0B, 0x0C, 0x31, 0x31, 0x35, 0x2F, 0x36, 0x30, 0x38 }; - addResponseData(incorrectResponseDataForColumnsDetail1, sizeof(incorrectResponseDataForColumnsDetail1), true); - result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &charsetDetail); - EXPECT_EQ(result, false); - clearResponseBuff(); - - // get setting detail that type is string, but the response crc is incorrect - uint8_t incorrectResponseDataForColumnsDetail2[] = { 0xCC, 0x00, 0x0B, 0x0C, 0x31, 0x31, 0x35, 0x2F, 0x36, 0x30, 0x38, 0x39, 0x30, 0x00, 0xFF }; - addResponseData(incorrectResponseDataForColumnsDetail2, sizeof(incorrectResponseDataForColumnsDetail2), true); - result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &charsetDetail); - EXPECT_EQ(result, false); - clearResponseBuff(); - - // get setting detail that type is string, but the response has some randome data at trailing - uint8_t incorrectResponseDataForColumnsDetail3[] = { 0xCC, 0x00, 0x0B, 0x0C, 0x31, 0x31, 0x35, 0x2F, 0x36, 0x30, 0x38, 0x39, 0x30, 0x00, 0xFE, 0xAA, 0xBB, 0xCC }; - addResponseData(incorrectResponseDataForColumnsDetail3, sizeof(incorrectResponseDataForColumnsDetail3), true); - result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &charsetDetail); - EXPECT_EQ(result, true); - clearResponseBuff(); -} - extern "C" { serialPort_t *openSerialPort(serialPortIdentifier_e identifier, serialPortFunction_e functionMask, serialReceiveCallbackPtr callback, void *callbackData, uint32_t baudRate, portMode_e mode, portOptions_e options) { @@ -1438,10 +749,10 @@ extern "C" { uint8_t bufIndex = testData.indexOfCurrentRespBuf; uint8_t leftDataLen = 0; - if (testData.responseDataReadPos >= testData.responseBufsLen[bufIndex]) { + if (testData.responseDataReadPos + 1 > testData.responseBufsLen[bufIndex]) { return 0; } else { - leftDataLen = testData.responseBufsLen[bufIndex] - testData.responseDataReadPos; + leftDataLen = testData.responseBufsLen[bufIndex] - (testData.responseDataReadPos); } if (leftDataLen) { @@ -1567,13 +878,13 @@ extern "C" { sbufWriteU8(dst, (uint8_t)val); } - bool feature(uint32_t) { return false; } + bool featureIsEnabled(uint32_t) { return false; } void serialWriteBuf(serialPort_t *instance, const uint8_t *data, int count) { UNUSED(instance); UNUSED(data); UNUSED(count); - // // reset the input buffer + // reset the input buffer testData.responseDataReadPos = 0; testData.indexOfCurrentRespBuf++; if (testData.indexOfCurrentRespBuf >= testData.responseBufCount) { @@ -1609,9 +920,8 @@ extern "C" { return ret; } - uint32_t millis(void) { return testData.millis++; } - + uint32_t micros(void) { return millis() * 1000; } void beeper(beeperMode_e mode) { UNUSED(mode); } uint8_t armingFlags = 0; bool cmsInMenu; diff --git a/src/test/unit/rx_ranges_unittest.cc b/src/test/unit/rx_ranges_unittest.cc index d45f0490ac..a87b0dc2b9 100644 --- a/src/test/unit/rx_ranges_unittest.cc +++ b/src/test/unit/rx_ranges_unittest.cc @@ -186,11 +186,11 @@ bool rxMspInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadR return true; } -bool feature(uint32_t) { +bool featureIsEnabled(uint32_t) { return false; } -void featureClear(uint32_t) { +void featureDisable(uint32_t) { } bool rxMspFrameComplete(void) diff --git a/src/test/unit/scheduler_unittest.cc b/src/test/unit/scheduler_unittest.cc index 731d5f1532..a4a51e85e6 100644 --- a/src/test/unit/scheduler_unittest.cc +++ b/src/test/unit/scheduler_unittest.cc @@ -69,7 +69,7 @@ extern "C" { cfTask_t cfTasks[TASK_COUNT] = { [TASK_SYSTEM] = { .taskName = "SYSTEM", - .taskFunc = taskSystem, + .taskFunc = taskSystemLoad, .desiredPeriod = TASK_PERIOD_HZ(10), .staticPriority = TASK_PRIORITY_MEDIUM_HIGH, }, @@ -122,8 +122,6 @@ extern "C" { TEST(SchedulerUnittest, TestPriorites) { - EXPECT_EQ(20, TASK_COUNT); - EXPECT_EQ(TASK_PRIORITY_MEDIUM_HIGH, cfTasks[TASK_SYSTEM].staticPriority); EXPECT_EQ(TASK_PRIORITY_REALTIME, cfTasks[TASK_GYROPID].staticPriority); EXPECT_EQ(TASK_PRIORITY_MEDIUM, cfTasks[TASK_ACCEL].staticPriority); @@ -238,64 +236,68 @@ TEST(SchedulerUnittest, TestQueueArray) queueClear(); taskQueueArray[TASK_COUNT_UNITTEST + 1] = deadBeefPtr; // note, must set deadBeefPtr after queueClear + unsigned enqueuedTasks = 0; + EXPECT_EQ(enqueuedTasks, taskQueueSize); + for (int taskId = 0; taskId < TASK_COUNT_UNITTEST - 1; ++taskId) { - setTaskEnabled(static_cast(taskId), true); - EXPECT_EQ(taskId + 1, taskQueueSize); - EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT_UNITTEST + 1]); + if (cfTasks[taskId].taskFunc) { + setTaskEnabled(static_cast(taskId), true); + enqueuedTasks++; + EXPECT_EQ(enqueuedTasks, taskQueueSize); + EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT_UNITTEST + 1]); + } } - EXPECT_EQ(TASK_COUNT_UNITTEST - 1, taskQueueSize); - EXPECT_NE(static_cast(0), taskQueueArray[TASK_COUNT_UNITTEST - 2]); - const cfTask_t *lastTaskPrev = taskQueueArray[TASK_COUNT_UNITTEST - 2]; - EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT_UNITTEST - 1]); - EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT_UNITTEST]); + EXPECT_NE(static_cast(0), taskQueueArray[enqueuedTasks - 1]); + const cfTask_t *lastTaskPrev = taskQueueArray[enqueuedTasks - 1]; + EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks]); + EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks + 1]); EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT_UNITTEST + 1]); setTaskEnabled(TASK_SYSTEM, false); - EXPECT_EQ(TASK_COUNT_UNITTEST - 2, taskQueueSize); - EXPECT_EQ(lastTaskPrev, taskQueueArray[TASK_COUNT_UNITTEST - 3]); - EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT_UNITTEST - 2]); // NULL at end of queue - EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT_UNITTEST - 1]); - EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT_UNITTEST]); + EXPECT_EQ(enqueuedTasks - 1, taskQueueSize); + EXPECT_EQ(lastTaskPrev, taskQueueArray[enqueuedTasks - 2]); + EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks - 1]); // NULL at end of queue + EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks]); + EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks + 1]); EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT_UNITTEST + 1]); - taskQueueArray[TASK_COUNT_UNITTEST - 2] = 0; + taskQueueArray[enqueuedTasks - 1] = 0; setTaskEnabled(TASK_SYSTEM, true); - EXPECT_EQ(TASK_COUNT_UNITTEST - 1, taskQueueSize); - EXPECT_EQ(lastTaskPrev, taskQueueArray[TASK_COUNT_UNITTEST - 2]); - EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT_UNITTEST - 1]); - EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT_UNITTEST]); + EXPECT_EQ(enqueuedTasks, taskQueueSize); + EXPECT_EQ(lastTaskPrev, taskQueueArray[enqueuedTasks - 1]); + EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks]); + EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks + 1]); EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT_UNITTEST + 1]); cfTaskInfo_t taskInfo; - getTaskInfo(static_cast(TASK_COUNT_UNITTEST - 1), &taskInfo); + getTaskInfo(static_cast(enqueuedTasks + 1), &taskInfo); EXPECT_EQ(false, taskInfo.isEnabled); - setTaskEnabled(static_cast(TASK_COUNT_UNITTEST - 1), true); - EXPECT_EQ(TASK_COUNT_UNITTEST, taskQueueSize); - EXPECT_EQ(lastTaskPrev, taskQueueArray[TASK_COUNT_UNITTEST - 1]); - EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT_UNITTEST]); // check no buffer overrun + setTaskEnabled(static_cast(enqueuedTasks), true); + EXPECT_EQ(enqueuedTasks, taskQueueSize); + EXPECT_EQ(lastTaskPrev, taskQueueArray[enqueuedTasks - 1]); + EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks + 1]); // check no buffer overrun EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT_UNITTEST + 1]); setTaskEnabled(TASK_SYSTEM, false); - EXPECT_EQ(TASK_COUNT_UNITTEST - 1, taskQueueSize); - //EXPECT_EQ(lastTaskPrev, taskQueueArray[TASK_COUNT_UNITTEST - 3]); - EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT_UNITTEST - 1]); - EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT_UNITTEST]); + EXPECT_EQ(enqueuedTasks - 1, taskQueueSize); + EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks]); + EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks + 1]); EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT_UNITTEST + 1]); setTaskEnabled(TASK_ACCEL, false); - EXPECT_EQ(TASK_COUNT_UNITTEST - 2, taskQueueSize); - EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT_UNITTEST - 2]); - EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT_UNITTEST - 1]); - EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT_UNITTEST]); + EXPECT_EQ(enqueuedTasks - 2, taskQueueSize); + EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks - 1]); + EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks]); + EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks + 1]); EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT_UNITTEST + 1]); setTaskEnabled(TASK_BATTERY_VOLTAGE, false); - EXPECT_EQ(TASK_COUNT_UNITTEST - 3, taskQueueSize); - EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT_UNITTEST - 3]); - EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT_UNITTEST - 2]); - EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT_UNITTEST - 1]); - EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT_UNITTEST]); + EXPECT_EQ(enqueuedTasks - 2, taskQueueSize); + EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks - 2]); + EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks - 1]); + EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks]); + EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks + 1]); EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT_UNITTEST + 1]); } diff --git a/src/test/unit/sensor_gyro_unittest.cc b/src/test/unit/sensor_gyro_unittest.cc index a88cdaf276..b132e44683 100644 --- a/src/test/unit/sensor_gyro_unittest.cc +++ b/src/test/unit/sensor_gyro_unittest.cc @@ -40,7 +40,7 @@ extern "C" { #include "sensors/acceleration.h" #include "sensors/sensors.h" - STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev); + STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev); struct gyroSensor_s; STATIC_UNIT_TESTED void performGyroCalibration(struct gyroSensor_s *gyroSensor, uint8_t gyroMovementCalibrationThreshold); STATIC_UNIT_TESTED bool fakeGyroRead(gyroDev_t *gyro); @@ -57,7 +57,7 @@ extern gyroDev_t * const gyroDevPtr; TEST(SensorGyro, Detect) { - const gyroSensor_e detected = gyroDetect(gyroDevPtr); + const gyroHardware_e detected = gyroDetect(gyroDevPtr); EXPECT_EQ(GYRO_FAKE, detected); EXPECT_EQ(GYRO_FAKE, detectedSensors[SENSOR_INDEX_GYRO]); } diff --git a/src/test/unit/target.h b/src/test/unit/target.h index 18209cb638..cb76419923 100644 --- a/src/test/unit/target.h +++ b/src/test/unit/target.h @@ -59,6 +59,7 @@ #define USE_UART5 #define USE_SOFTSERIAL1 #define USE_SOFTSERIAL2 +#define USE_TASK_STATISTICS #define SERIAL_PORT_COUNT 8 diff --git a/src/test/unit/telemetry_crsf_msp_unittest.cc b/src/test/unit/telemetry_crsf_msp_unittest.cc index 40374e6209..ddd5225daf 100644 --- a/src/test/unit/telemetry_crsf_msp_unittest.cc +++ b/src/test/unit/telemetry_crsf_msp_unittest.cc @@ -69,6 +69,7 @@ extern "C" { int sbufBytesRemaining(sbuf_t *buf); void initSharedMsp(); uint16_t testBatteryVoltage = 0; + int32_t testAmperage = 0; uint8_t mspTxData[64]; //max frame size sbuf_t mspTxDataBuf; @@ -259,6 +260,9 @@ extern "C" { uint16_t getBatteryVoltage(void) { return testBatteryVoltage; } + uint16_t getBatteryAverageCellVoltage(void) { + return 0; + } bool isAmperageConfigured(void) { return true; } int32_t getAmperage(void) { return testAmperage; @@ -268,9 +272,13 @@ extern "C" { return 67; } - bool feature(uint32_t) {return false;} + int32_t getEstimatedAltitudeCm(void) { + return 0; + } - bool isAirmodeActive(void) {return true;} + bool featureIsEnabled(uint32_t) {return false;} + + bool airmodeIsEnabled(void) {return true;} mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn) { diff --git a/src/test/unit/telemetry_crsf_unittest.cc b/src/test/unit/telemetry_crsf_unittest.cc index 23d73d1dfd..fe2cf8332b 100644 --- a/src/test/unit/telemetry_crsf_unittest.cc +++ b/src/test/unit/telemetry_crsf_unittest.cc @@ -104,7 +104,7 @@ TEST(TelemetryCrsfTest, TestGPS) int frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_GPS); EXPECT_EQ(CRSF_FRAME_GPS_PAYLOAD_SIZE + FRAME_HEADER_FOOTER_LEN, frameLen); - EXPECT_EQ(CRSF_ADDRESS_BROADCAST, frame[0]); // address + EXPECT_EQ(CRSF_SYNC_BYTE, frame[0]); // address EXPECT_EQ(17, frame[1]); // length EXPECT_EQ(0x02, frame[2]); // type int32_t lattitude = frame[3] << 24 | frame[4] << 16 | frame[5] << 8 | frame[6]; @@ -124,8 +124,8 @@ TEST(TelemetryCrsfTest, TestGPS) gpsSol.llh.lat = 56 * GPS_DEGREES_DIVIDER; gpsSol.llh.lon = 163 * GPS_DEGREES_DIVIDER; ENABLE_STATE(GPS_FIX); - gpsSol.llh.alt = 2345; // altitude in m - gpsSol.groundSpeed = 163; // speed in 0.1m/s, 16.3 m/s = 58.68 km/h, so CRSF (km/h *10) value is 587 + gpsSol.llh.altCm = 2345 * 100; // altitude in cm / 100 + 1000m offset, so CRSF value should be 3345 + gpsSol.groundSpeed = 1630; // speed in cm/s, 16.3 m/s = 58.68 km/h, so CRSF (km/h *10) value is 587 gpsSol.numSat = 9; gpsSol.groundCourse = 1479; // degrees * 10 frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_GPS); @@ -151,7 +151,7 @@ TEST(TelemetryCrsfTest, TestBattery) testBatteryVoltage = 0; // 0.1V units int frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_BATTERY_SENSOR); EXPECT_EQ(CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE + FRAME_HEADER_FOOTER_LEN, frameLen); - EXPECT_EQ(CRSF_ADDRESS_BROADCAST, frame[0]); // address + EXPECT_EQ(CRSF_SYNC_BYTE, frame[0]); // address EXPECT_EQ(10, frame[1]); // length EXPECT_EQ(0x08, frame[2]); // type uint16_t voltage = frame[3] << 8 | frame[4]; // mV * 100 @@ -188,7 +188,7 @@ TEST(TelemetryCrsfTest, TestAttitude) attitude.values.yaw = 0; int frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_ATTITUDE); EXPECT_EQ(CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE + FRAME_HEADER_FOOTER_LEN, frameLen); - EXPECT_EQ(CRSF_ADDRESS_BROADCAST, frame[0]); // address + EXPECT_EQ(CRSF_SYNC_BYTE, frame[0]); // address EXPECT_EQ(8, frame[1]); // length EXPECT_EQ(0x1e, frame[2]); // type int16_t pitch = frame[3] << 8 | frame[4]; // rad / 10000 @@ -220,7 +220,7 @@ TEST(TelemetryCrsfTest, TestFlightMode) airMode = false; int frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_FLIGHT_MODE); EXPECT_EQ(5 + FRAME_HEADER_FOOTER_LEN, frameLen); - EXPECT_EQ(CRSF_ADDRESS_BROADCAST, frame[0]); // address + EXPECT_EQ(CRSF_SYNC_BYTE, frame[0]); // address EXPECT_EQ(7, frame[1]); // length EXPECT_EQ(0x21, frame[2]); // type EXPECT_EQ('A', frame[3]); @@ -235,7 +235,7 @@ TEST(TelemetryCrsfTest, TestFlightMode) EXPECT_EQ(ANGLE_MODE, FLIGHT_MODE(ANGLE_MODE)); frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_FLIGHT_MODE); EXPECT_EQ(5 + FRAME_HEADER_FOOTER_LEN, frameLen); - EXPECT_EQ(CRSF_ADDRESS_BROADCAST, frame[0]); // address + EXPECT_EQ(CRSF_SYNC_BYTE, frame[0]); // address EXPECT_EQ(7, frame[1]); // length EXPECT_EQ(0x21, frame[2]); // type EXPECT_EQ('S', frame[3]); @@ -250,7 +250,7 @@ TEST(TelemetryCrsfTest, TestFlightMode) EXPECT_EQ(HORIZON_MODE, FLIGHT_MODE(HORIZON_MODE)); frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_FLIGHT_MODE); EXPECT_EQ(4 + FRAME_HEADER_FOOTER_LEN, frameLen); - EXPECT_EQ(CRSF_ADDRESS_BROADCAST, frame[0]); // address + EXPECT_EQ(CRSF_SYNC_BYTE, frame[0]); // address EXPECT_EQ(6, frame[1]); // length EXPECT_EQ(0x21, frame[2]); // type EXPECT_EQ('H', frame[3]); @@ -263,7 +263,7 @@ TEST(TelemetryCrsfTest, TestFlightMode) airMode = true; frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_FLIGHT_MODE); EXPECT_EQ(4 + FRAME_HEADER_FOOTER_LEN, frameLen); - EXPECT_EQ(CRSF_ADDRESS_BROADCAST, frame[0]); // address + EXPECT_EQ(CRSF_SYNC_BYTE, frame[0]); // address EXPECT_EQ(6, frame[1]); // length EXPECT_EQ(0x21, frame[2]); // type EXPECT_EQ('A', frame[3]); @@ -293,7 +293,7 @@ void beeperConfirmationBeeps(uint8_t beepCount) {UNUSED(beepCount);} uint32_t micros(void) {return 0;} -bool feature(uint32_t) {return true;} +bool featureIsEnabled(uint32_t) {return true;} uint32_t serialRxBytesWaiting(const serialPort_t *) {return 0;} uint32_t serialTxBytesFree(const serialPort_t *) {return 0;} @@ -312,7 +312,7 @@ bool telemetryCheckRxPortShared(const serialPortConfig_t *) {return true;} portSharing_e determinePortSharing(const serialPortConfig_t *, serialPortFunction_e) {return PORTSHARING_NOT_SHARED;} -bool isAirmodeActive(void) {return airMode;} +bool airmodeIsEnabled(void) {return airMode;} int32_t getAmperage(void) { return testAmperage; @@ -322,6 +322,10 @@ uint16_t getBatteryVoltage(void) { return testBatteryVoltage; } +uint16_t getBatteryAverageCellVoltage(void) { + return 0; +} + batteryState_e getBatteryState(void) { return BATTERY_OK; } @@ -330,6 +334,10 @@ uint8_t calculateBatteryPercentageRemaining(void) { return 67; } +int32_t getEstimatedAltitudeCm(void) { + return gpsSol.llh.altCm; // function returns cm not m. +} + int32_t getMAhDrawn(void){ return testmAhDrawn; } diff --git a/src/test/unit/telemetry_hott_unittest.cc b/src/test/unit/telemetry_hott_unittest.cc index 8a00bce26d..80454c7031 100644 --- a/src/test/unit/telemetry_hott_unittest.cc +++ b/src/test/unit/telemetry_hott_unittest.cc @@ -180,8 +180,8 @@ uint32_t fixedMillis = 0; baro_t baro; -uint32_t getEstimatedAltitude() { return 0; } -uint32_t getEstimatedVario() { return 0; } +int32_t getEstimatedAltitudeCm() { return 0; } +int16_t getEstimatedVario() { return 0; } uint32_t millis(void) { return fixedMillis; @@ -265,6 +265,16 @@ batteryState_e getBatteryState(void) return BATTERY_OK; } +batteryState_e getVoltageState(void) +{ + return BATTERY_OK; +} + +batteryState_e getConsumptionState(void) +{ + return BATTERY_OK; +} + uint16_t getBatteryVoltage(void) { return testBatteryVoltage; diff --git a/src/test/unit/telemetry_ibus_unittest.cc b/src/test/unit/telemetry_ibus_unittest.cc index e1c093b0c5..e889fad8ce 100644 --- a/src/test/unit/telemetry_ibus_unittest.cc +++ b/src/test/unit/telemetry_ibus_unittest.cc @@ -35,7 +35,7 @@ extern "C" { #include "sensors/barometer.h" #include "sensors/acceleration.h" #include "scheduler/scheduler.h" -#include "fc/fc_tasks.h" +#include "fc/tasks.h" } #include "unittest_macros.h" @@ -108,7 +108,7 @@ throttleStatus_e calculateThrottleStatus(void) return throttleStatus; } -bool feature(uint32_t mask) +bool featureIsEnabled(uint32_t mask) { return (definedFeatures & mask) != 0; } diff --git a/src/test/unit/timer_definition_unittest.cc b/src/test/unit/timer_definition_unittest.cc new file mode 100644 index 0000000000..41acd81b97 --- /dev/null +++ b/src/test/unit/timer_definition_unittest.cc @@ -0,0 +1,103 @@ +/* + * This file is part of Betaflight. + * + * Betaflight 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. + * + * Betaflight 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 Betaflight. If not, see . + */ + +extern "C" { + #include + #include + extern const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT]; +} + +#include +#include +#include +#include +#include +#include "gtest/gtest.h" + +TEST(TimerDefinitionTest, Test_counterMismatch) { + for (const timerHardware_t &t : timerHardware) + ASSERT_EQ(&t - timerHardware, t.def_tim_counter) + << "Counter mismatch in timerHardware (in target.c) at position " + << &t - timerHardware << "; the array may have uninitialized " + << "trailing elements. This happens when USABLE_TIMER_CHANNEL_COUNT" + << " is not equal to the number of array initializers. Current " + << "value is " << USABLE_TIMER_CHANNEL_COUNT << ", last initialized" + << " array element appears to be " << &t - timerHardware - 1 << '.'; +} + +TEST(TimerDefinitionTest, Test_duplicatePin) { + std::set usedPins; + for (const timerHardware_t &t : timerHardware) + EXPECT_TRUE(usedPins.emplace(t.pin).second) + << "Pin " << TEST_PIN_NAMES[t.pin] << " is used more than once. " + << "This is a problem with the timerHardware array (in target.c). " + << "Check the array for typos. Then check the size of the array " + << "initializers; it must be USABLE_TIMER_CHANNEL_COUNT."; + EXPECT_EQ(USABLE_TIMER_CHANNEL_COUNT, usedPins.size()); +} + +namespace { +std::string writeUsedTimers(const std::bitset &tset) { + std::stringstream used_timers; + if (tset.any()) { + unsigned int timer{0}; + for (; timer < TEST_TIMER_SIZE; ++timer) + if (tset[timer]) { + used_timers << "( TIM_N(" << timer << ')'; + break; + } + for (++timer; timer < TEST_TIMER_SIZE; ++timer) + if (tset[timer]) used_timers << " | TIM_N(" << timer << ')'; + used_timers << " )"; + } else { + used_timers << "(0)"; + } + return used_timers.str(); +} +} + +TEST(TimerDefinitionTest, Test_usedTimers) +{ + std::bitset expected; + for (const timerHardware_t &t : timerHardware) + expected |= TIM_N(t.timer); + const std::bitset actual{USED_TIMERS}; + EXPECT_EQ(expected, actual) + << "Used timers mismatch. target.c says " << expected << ", but " + << "target.h says " << actual << ". This has two possible causes: " + << "(1) The USED_TIMERS bitmap (in target.h) is outdated and out of " + << "sync with timerHardware (in target.c). (2) There is an " + << "inconsistency between USABLE_TIMER_CHANNEL_COUNT and the length " + << "of timerHardware's initializer list."; + std::cerr + << "USED_TIMERS definition based on timerHardware:" << std::endl + << writeUsedTimers(expected) << std::endl; +} + +// STUBS + +extern "C" { + void spiPinConfigure(int) {} + int spiPinConfig(int) { return 0; } + void spiInit(int) {} + + int i2cConfig(int) { return 0; } + void i2cHardwareConfigure(int) {} + void i2cInit(int) {} + + void bstInit(int) {} +} diff --git a/src/test/unit/timer_definition_unittest.include/drivers/dma.h b/src/test/unit/timer_definition_unittest.include/drivers/dma.h new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/test/unit/timer_definition_unittest.include/drivers/io.h b/src/test/unit/timer_definition_unittest.include/drivers/io.h new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/test/unit/timer_definition_unittest.include/drivers/timer.h b/src/test/unit/timer_definition_unittest.include/drivers/timer.h new file mode 100644 index 0000000000..2becfe977f --- /dev/null +++ b/src/test/unit/timer_definition_unittest.include/drivers/timer.h @@ -0,0 +1,21 @@ +#include + +typedef struct timerHardware_s { + enum TestTimerEnum timer; + enum TestChannelEnum channel; + enum TestPinEnum pin; + enum TestTimUseEnum purpose; + unsigned int def_tim_counter; +} timerHardware_t; + +// F7 and F4 have 6 arguments, F3 and F1 have 5 arguments. +#define DEF_TIM(timer_, channel_, pin_, purpose_, ...) \ +{ \ + .timer = timer_, \ + .channel = channel_, \ + .pin = pin_, \ + .purpose = purpose_, \ + .def_tim_counter = __COUNTER__, \ +} + +#define TIM_N(n) (1 << (n)) diff --git a/src/test/unit/timer_definition_unittest.include/drivers/timer_def.h b/src/test/unit/timer_definition_unittest.include/drivers/timer_def.h new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/test/unit/timer_definition_unittest.include/mock_enums.h b/src/test/unit/timer_definition_unittest.include/mock_enums.h new file mode 100644 index 0000000000..782aefce1b --- /dev/null +++ b/src/test/unit/timer_definition_unittest.include/mock_enums.h @@ -0,0 +1,66 @@ +#pragma once + +enum TestTimerEnum { + TIM0, TIM1, TIM2, TIM3, TIM4, TIM5, TIM6, TIM7, TIM8, TIM9, + TIM10, TIM11, TIM12, TIM13, TIM14, TIM15, TIM16, TIM17, TIM18, TIM19, TIM20, + TEST_TIMER_SIZE, +}; + +enum TestChannelEnum { + CH0, CH1, CH2, CH3, CH4, CH5, CH6, CH7, CH9, CH10, CH1N, CH2N, CH3N, + TEST_CHANNEL_SIZE, +}; + +// Keep this in sync with TEST_PIN_NAMES below. +enum TestPinEnum { + PA0, PA1, PA2, PA3, PA4, PA5, PA6, PA7, PA8, PA9, + PA10, PA11, PA12, PA13, PA14, PA15, + PB0, PB1, PB2, PB3, PB4, PB5, PB6, PB7, PB8, PB9, + PB10, PB11, PB12, PB13, PB14, PB15, + PC0, PC1, PC2, PC3, PC4, PC5, PC6, PC7, PC8, PC9, + PC10, PC11, PC12, PC13, PC14, PC15, + PD0, PD1, PD2, PD3, PD4, PD5, PD6, PD7, PD8, PD9, + PD10, PD11, PD12, PD13, PD14, PD15, + PE0, PE1, PE2, PE3, PE4, PE5, PE6, PE7, PE8, PE9, + PE10, PE11, PE12, PE13, PE14, PE15, + PF0, PF1, PF2, PF3, PF4, PF5, PF6, PF7, PF8, PF9, + PF10, PF11, PF12, PF13, PF14, PF15, + PG0, PG1, PG2, PG3, PG4, PG5, PG6, PG7, PG8, PG9, + PG10, PG11, PG12, PG13, PG14, PG15, + PH0, PH1, PH2, PH3, PH4, PH5, PH6, PH7, PH8, PH9, + PH10, PH11, PH12, PH13, PH14, PH15, TEST_PIN_SIZE, +}; + +// Keep this in sync with TestPinEnum above. +const char *const TEST_PIN_NAMES[TEST_PIN_SIZE] = { + "PA0", "PA1", "PA2", "PA3", "PA4", "PA5", "PA6", "PA7", "PA8", "PA9", + "PA10", "PA11", "PA12", "PA13", "PA14", "PA15", + "PB0", "PB1", "PB2", "PB3", "PB4", "PB5", "PB6", "PB7", "PB8", "PB9", + "PB10", "PB11", "PB12", "PB13", "PB14", "PB15", + "PC0", "PC1", "PC2", "PC3", "PC4", "PC5", "PC6", "PC7", "PC8", "PC9", + "PC10", "PC11", "PC12", "PC13", "PC14", "PC15", + "PD0", "PD1", "PD2", "PD3", "PD4", "PD5", "PD6", "PD7", "PD8", "PD9", + "PD10", "PD11", "PD12", "PD13", "PD14", "PD15", + "PE0", "PE1", "PE2", "PE3", "PE4", "PE5", "PE6", "PE7", "PE8", "PE9", + "PE10", "PE11", "PE12", "PE13", "PE14", "PE15", + "PF0", "PF1", "PF2", "PF3", "PF4", "PF5", "PF6", "PF7", "PF8", "PF9", + "PF10", "PF11", "PF12", "PF13", "PF14", "PF15", + "PG0", "PG1", "PG2", "PG3", "PG4", "PG5", "PG6", "PG7", "PG8", "PG9", + "PG10", "PG11", "PG12", "PG13", "PG14", "PG15", + "PH0", "PH1", "PH2", "PH3", "PH4", "PH5", "PH6", "PH7", "PH8", "PH9", + "PH10", "PH11", "PH12", "PH13", "PH14", "PH15", +}; + +enum TestTimUseEnum { + TIM_USE_ANY, + TIM_USE_BEEPER, + TIM_USE_CAMERA_CONTROL, + TIM_USE_LED, + TIM_USE_MOTOR, + TIM_USE_NONE, + TIM_USE_PPM, + TIM_USE_PWM, + TIM_USE_SERVO, + TIM_USE_TRANSPONDER, + TEST_TIM_USE_SIZE, +}; diff --git a/src/test/unit/timer_definition_unittest.include/pg/bus_bst.h b/src/test/unit/timer_definition_unittest.include/pg/bus_bst.h new file mode 100644 index 0000000000..7b40e9d91e --- /dev/null +++ b/src/test/unit/timer_definition_unittest.include/pg/bus_bst.h @@ -0,0 +1 @@ +void bstInit(int); diff --git a/src/test/unit/timer_definition_unittest.include/pg/bus_i2c.h b/src/test/unit/timer_definition_unittest.include/pg/bus_i2c.h new file mode 100644 index 0000000000..b822be69bf --- /dev/null +++ b/src/test/unit/timer_definition_unittest.include/pg/bus_i2c.h @@ -0,0 +1,5 @@ +#define I2CDEV_2 (1) + +int i2cConfig(int); +void i2cHardwareConfigure(int); +void i2cInit(int); diff --git a/src/test/unit/timer_definition_unittest.include/pg/bus_spi.h b/src/test/unit/timer_definition_unittest.include/pg/bus_spi.h new file mode 100644 index 0000000000..910d373e66 --- /dev/null +++ b/src/test/unit/timer_definition_unittest.include/pg/bus_spi.h @@ -0,0 +1,5 @@ +#define SPIDEV_1 (0) + +void spiPinConfigure(int); +int spiPinConfig(int); +void spiInit(int); diff --git a/src/test/unit/timer_definition_unittest.include/platform.h b/src/test/unit/timer_definition_unittest.include/platform.h new file mode 100644 index 0000000000..c4fbc30462 --- /dev/null +++ b/src/test/unit/timer_definition_unittest.include/platform.h @@ -0,0 +1,5 @@ +#include +#include "target/common_pre.h" +#include "target.h" +#include "target/common_post.h" +#include "target/common_defaults_post.h" diff --git a/src/test/unit/vtx_unittest.cc b/src/test/unit/vtx_unittest.cc new file mode 100644 index 0000000000..0ebd069169 --- /dev/null +++ b/src/test/unit/vtx_unittest.cc @@ -0,0 +1,176 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include + +extern "C" { + #include "blackbox/blackbox.h" + #include "build/debug.h" + #include "common/maths.h" + #include "config/feature.h" + #include "pg/pg.h" + #include "pg/pg_ids.h" + #include "pg/rx.h" + #include "fc/config.h" + #include "fc/controlrate_profile.h" + #include "fc/core.h" + #include "fc/rc_controls.h" + #include "fc/rc_modes.h" + #include "fc/runtime_config.h" + #include "flight/failsafe.h" + #include "flight/imu.h" + #include "flight/mixer.h" + #include "flight/pid.h" + #include "flight/servos.h" + #include "io/beeper.h" + #include "io/gps.h" + #include "io/vtx.h" + #include "rx/rx.h" + #include "scheduler/scheduler.h" + #include "sensors/acceleration.h" + #include "sensors/gyro.h" + #include "telemetry/telemetry.h" + + vtxSettingsConfig_t vtxGetSettings(void); + + PG_REGISTER(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 0); + PG_REGISTER(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0); + PG_REGISTER(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 0); + PG_REGISTER(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 0); + PG_REGISTER(pidConfig_t, pidConfig, PG_PID_CONFIG, 0); + PG_REGISTER(rxConfig_t, rxConfig, PG_RX_CONFIG, 0); + PG_REGISTER(servoConfig_t, servoConfig, PG_SERVO_CONFIG, 0); + PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0); + PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0); + PG_REGISTER(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 0); + + float rcCommand[4]; + int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; + uint16_t averageSystemLoadPercent = 0; + uint8_t cliMode = 0; + uint8_t debugMode = 0; + int16_t debug[DEBUG16_VALUE_COUNT]; + pidProfile_t *currentPidProfile; + controlRateConfig_t *currentControlRateProfile; + attitudeEulerAngles_t attitude; + gpsSolutionData_t gpsSol; + uint32_t targetPidLooptime; + bool cmsInMenu = false; + float axisPID_P[3], axisPID_I[3], axisPID_D[3], axisPIDSum[3]; + rxRuntimeConfig_t rxRuntimeConfig = {}; +} + +uint32_t simulationFeatureFlags = 0; +uint32_t simulationTime = 0; +bool gyroCalibDone = false; +bool simulationHaveRx = false; + +#include "gtest/gtest.h" + +TEST(VtxTest, PitMode) +{ + // given + modeActivationConditionsMutable(0)->auxChannelIndex = 0; + modeActivationConditionsMutable(0)->modeId = BOXVTXPITMODE; + modeActivationConditionsMutable(0)->range.startStep = CHANNEL_VALUE_TO_STEP(1750); + modeActivationConditionsMutable(0)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX); + + // and + vtxSettingsConfigMutable()->band = 0; + vtxSettingsConfigMutable()->freq = 5800; + vtxSettingsConfigMutable()->pitModeFreq = 5300; + + // expect + EXPECT_EQ(5800, vtxGetSettings().freq); + + // and + // enable vtx pit mode + rcData[AUX1] = 1800; + + // when + updateActivatedModes(); + + // expect + EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXVTXPITMODE)); + EXPECT_EQ(5300, vtxGetSettings().freq); +} + +// STUBS +extern "C" { + uint32_t micros(void) { return simulationTime; } + uint32_t millis(void) { return micros() / 1000; } + bool rxIsReceivingSignal(void) { return simulationHaveRx; } + + bool featureIsEnabled(uint32_t f) { return simulationFeatureFlags & f; } + void warningLedFlash(void) {} + void warningLedDisable(void) {} + void warningLedUpdate(void) {} + void beeper(beeperMode_e) {} + void beeperConfirmationBeeps(uint8_t) {} + void beeperWarningBeeps(uint8_t) {} + void beeperSilence(void) {} + void systemBeep(bool) {} + void saveConfigAndNotify(void) {} + void blackboxFinish(void) {} + bool accIsCalibrationComplete(void) { return true; } + bool isBaroCalibrationComplete(void) { return true; } + bool isGyroCalibrationComplete(void) { return gyroCalibDone; } + void gyroStartCalibration(bool) {} + bool isFirstArmingGyroCalibrationRunning(void) { return false; } + void pidController(const pidProfile_t *, const rollAndPitchTrims_t *, timeUs_t) {} + void pidStabilisationState(pidStabilisationState_e) {} + void mixTable(timeUs_t , uint8_t) {}; + void writeMotors(void) {}; + void writeServos(void) {}; + bool calculateRxChannelsAndUpdateFailsafe(timeUs_t) { return true; } + bool isMixerUsingServos(void) { return false; } + void gyroUpdate(timeUs_t) {} + timeDelta_t getTaskDeltaTime(cfTaskId_e) { return 0; } + void updateRSSI(timeUs_t) {} + bool failsafeIsMonitoring(void) { return false; } + void failsafeStartMonitoring(void) {} + void failsafeUpdateState(void) {} + bool failsafeIsActive(void) { return false; } + void pidResetIterm(void) {} + void updateAdjustmentStates(void) {} + void processRcAdjustments(controlRateConfig_t *) {} + void updateGpsWaypointsAndMode(void) {} + void mspSerialReleaseSharedTelemetryPorts(void) {} + void telemetryCheckState(void) {} + void mspSerialAllocatePorts(void) {} + void gyroReadTemperature(void) {} + void updateRcCommands(void) {} + void applyAltHold(void) {} + void resetYawAxis(void) {} + int16_t calculateThrottleAngleCorrection(uint8_t) { return 0; } + void processRcCommand(void) {} + void updateGpsStateForHomeAndHoldMode(void) {} + void blackboxUpdate(timeUs_t) {} + void transponderUpdate(timeUs_t) {} + void GPS_reset_home_position(void) {} + void accSetCalibrationCycles(uint16_t) {} + void baroSetCalibrationCycles(uint16_t) {} + void changePidProfile(uint8_t) {} + void changeControlRateProfile(uint8_t) {} + void dashboardEnablePageCycling(void) {} + void dashboardDisablePageCycling(void) {} + bool imuQuaternionHeadfreeOffsetSet(void) { return true; } + void rescheduleTask(cfTaskId_e, uint32_t) {} + bool usbCableIsInserted(void) { return false; } + bool usbVcpIsConnected(void) { return false; } + void pidSetAntiGravityState(bool newState) { UNUSED(newState); } +}