diff --git a/.gitignore b/.gitignore index 3cbe312ed07..b2c3b9ff546 100644 --- a/.gitignore +++ b/.gitignore @@ -15,11 +15,12 @@ startup_stm32f10x_md_gcc.s #.vscode/ cov-int* /build/ +/build_SITL/ /obj/ /patches/ /tools/ /downloads/ -/debug/ +/debug/ /release/ # script-generated files @@ -32,3 +33,5 @@ README.pdf # local changes only make/local.mk launch.json +.vscode/tasks.json +.vscode/c_cpp_properties.json diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100755 index 00000000000..6e7d914b25a --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,66 @@ +{ + "configurations": [ + { + "name": "Linux", + "includePath": [ + "${workspaceRoot}/src/main/**", + "${workspaceRoot}/lib/main/**", + "/usr/include/**" + ], + "browse": { + "limitSymbolsToIncludedHeaders": false, + "path": [ + "${workspaceRoot}/src/main/**", + "${workspaceRoot}/lib/main/**" + ] + }, + "intelliSenseMode": "linux-gcc-arm", + "cStandard": "c11", + "cppStandard": "c++17", + "defines": [ + "MCU_FLASH_SIZE 512", + "USE_NAV", + "NAV_FIXED_WING_LANDING", + "USE_OSD", + "USE_GYRO_NOTCH_1", + "USE_GYRO_NOTCH_2", + "USE_DTERM_NOTCH", + "USE_ACC_NOTCH", + "USE_GYRO_BIQUAD_RC_FIR2", + "USE_D_BOOST", + "USE_SERIALSHOT", + "USE_ANTIGRAVITY", + "USE_ASYNC_GYRO_PROCESSING", + "USE_RPM_FILTER", + "USE_GLOBAL_FUNCTIONS", + "USE_DYNAMIC_FILTERS", + "USE_IMU_BNO055", + "USE_SECONDARY_IMU", + "USE_DSHOT", + "FLASH_SIZE 480", + "USE_I2C_IO_EXPANDER", + "USE_PCF8574", + "USE_ESC_SENSOR", + "USE_PROGRAMMING_FRAMEWORK", + "USE_SERIALRX_GHST", + "USE_TELEMETRY_GHST", + "USE_CMS", + "USE_DJI_HD_OSD", + "USE_GYRO_KALMAN", + "USE_RANGEFINDER", + "USE_RATE_DYNAMICS", + "USE_SMITH_PREDICTOR", + "USE_ALPHA_BETA_GAMMA_FILTER", + "USE_MAG_VCM5883", + "USE_TELEMETRY_JETIEXBUS", + "USE_NAV", + "USE_SDCARD_SDIO", + "USE_SDCARD", + "USE_Q_TUNE", + "USE_GYRO_FFT_FILTER" + ], + "configurationProvider": "ms-vscode.cmake-tools" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/.vscode/tasks.json b/.vscode/tasks.json new file mode 100755 index 00000000000..3ca90b787d3 --- /dev/null +++ b/.vscode/tasks.json @@ -0,0 +1,41 @@ +{ + // See https://go.microsoft.com/fwlink/?LinkId=733558 + // for the documentation about the tasks.json format + "version": "2.0.0", + "tasks": [ + { + "label": "Build Matek F722-SE", + "type": "shell", + "command": "make MATEKF722SE", + "group": "build", + "problemMatcher": [], + "options": { + "cwd": "${workspaceFolder}/build" + } + }, + { + "label": "Build Matek F722", + "type": "shell", + "command": "make MATEKF722", + "group": { + "kind": "build", + "isDefault": true + }, + "problemMatcher": [], + "options": { + "cwd": "${workspaceFolder}/build" + } + } + , + { + "label": "CMAKE Update", + "type": "shell", + "command": "cmake ..", + "group": "build", + "problemMatcher": [], + "options": { + "cwd": "${workspaceFolder}/build" + } + } + ] +} \ No newline at end of file diff --git a/AUTHORS b/AUTHORS index 2a1f2744c48..7c2f94fcdb1 100644 --- a/AUTHORS +++ b/AUTHORS @@ -56,6 +56,7 @@ Krzysztof Rosinski Kyle Manna Larry Davis Marc Egli +Marcelo Bezerra Mark Williams Martin Budden Matthew Evans diff --git a/Dockerfile b/Dockerfile index 9c816b0c183..71077c8ed96 100644 --- a/Dockerfile +++ b/Dockerfile @@ -4,15 +4,15 @@ ARG USER_ID ARG GROUP_ID ENV DEBIAN_FRONTEND noninteractive -RUN apt-get update && apt-get install -y git cmake make ruby gcc python3 python3-pip gcc-arm-none-eabi +RUN apt-get update && apt-get install -y git cmake make ruby gcc python3 python3-pip gcc-arm-none-eabi ninja-build gdb RUN pip install pyyaml # if either of these are already set the same as the user's machine, leave them be and ignore the error -RUN addgroup --gid $GROUP_ID inav; exit 0; -RUN adduser --disabled-password --gecos '' --uid $USER_ID --gid $GROUP_ID inav; exit 0; +RUN if [ -n "$USER_ID" ]; then RUN addgroup --gid $GROUP_ID inav; exit 0; fi +RUN if [ -n "$USER_ID" ]; then RUN adduser --disabled-password --gecos '' --uid $USER_ID --gid $GROUP_ID inav; exit 0; fi -USER inav +RUN if [ -n "$USER_ID" ]; then USER inav; fi RUN git config --global --add safe.directory /src VOLUME /src diff --git a/cmake/docker.sh b/cmake/docker.sh index 3c10ebc8e1d..843e03a48a2 100755 --- a/cmake/docker.sh +++ b/cmake/docker.sh @@ -6,7 +6,7 @@ CURR_REV="$(git rev-parse HEAD)" initialize_cmake() { echo -e "*** CMake was not initialized yet, doing it now.\n" - cmake .. + cmake -GNinja .. echo "$CURR_REV" > "$LAST_CMAKE_AT_REV_FILE" } @@ -26,4 +26,4 @@ else fi # Let Make handle the arguments coming from the build script -make "$@" +ninja "$@" diff --git a/cmake/docker_build_sitl.sh b/cmake/docker_build_sitl.sh new file mode 100644 index 00000000000..a79742ae0ff --- /dev/null +++ b/cmake/docker_build_sitl.sh @@ -0,0 +1,7 @@ +#!/bin/bash +rm -r build_SITL +mkdir -p build_SITL +#cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -GNinja -B build_SITL .. +cmake -DSITL=ON -DDEBUG=ON -DWARNINGS_AS_ERRORS=ON -GNinja -B build_SITL .. +cd build_SITL +ninja \ No newline at end of file diff --git a/cmake/docker_run_sitl.sh b/cmake/docker_run_sitl.sh new file mode 100644 index 00000000000..b2089137ccb --- /dev/null +++ b/cmake/docker_run_sitl.sh @@ -0,0 +1,8 @@ +#!/bin/bash +cd build_SITL + +#Lauch SITL - configurator only mode +./inav_7.0.0_SITL + +#Launch SITL - connect to X-Plane. IP address should be host IP address, not 127.0.0.1. Can be found in X-Plane "Network" tab. +#./inav_7.0.0_SITL --sim=xp --simip=192.168.2.105 --simport=49000 \ No newline at end of file diff --git a/cmake/sitl.cmake b/cmake/sitl.cmake index 24a4ae9b27e..78697f98a9d 100644 --- a/cmake/sitl.cmake +++ b/cmake/sitl.cmake @@ -53,6 +53,11 @@ set(SITL_COMPILE_OPTIONS -funsigned-char ) +if(DEBUG) + message(STATUS "Debug mode enabled. Adding -g to SITL_COMPILE_OPTIONS.") + list(APPEND SITL_COMPILE_OPTIONS -g) +endif() + if(NOT MACOSX) set(SITL_COMPILE_OPTIONS ${SITL_COMPILE_OPTIONS} -Wno-return-local-addr diff --git a/docs/Cli.md b/docs/Cli.md index c805fe9250b..b0cf7d20bfc 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -114,6 +114,7 @@ While connected to the CLI, all Logical Switches are temporarily disabled (5.1.0 | `status` | Show status. Error codes can be looked up [here](https://github.com/iNavFlight/inav/wiki/%22Something%22-is-disabled----Reasons) | | `tasks` | Show task stats | | `temp_sensor` | List or configure temperature sensor(s). See [temperature sensors documentation](Temperature-sensors.md) for more information. | +| `timer_output_mode` | Override automatic timer / pwm function allocation. [Additional Information](#timer_outout_mode)| | `version` | Show version | | `wp` | List or configure waypoints. See the [navigation documentation](Navigation.md#cli-command-wp-to-manage-waypoints). | @@ -170,6 +171,66 @@ serial 0 -4 `serial` can also be used without any argument to print the current configuration of all the serial ports. +### `timer_output_mode` + +Since INAV 7, the firmware can dynamically allocate servo and motor outputs. This removes the need for bespoke targets for special cases (e.g. `MATEKF405` and `MATEKF405_SERVOS6`). + +#### Syntax + +``` +timer_output_mode [timer [function]] +``` +where: +* Without parameters, lists the current timers and modes +* With just a `timer` lists the mode for that timer +* With both `timer` and `function`, sets the function for that timers + +Note: + +* `timer` identifies the timer **index** (from 0); thus is one less than the corresponding `TIMn` definition in a target's `target.c`. +* The function is one of `AUTO` (the default), `MOTORS` or `SERVOS`. + +Motors are allocated first, hence having a servo before a motor may require use of `timer_output_mode`. + +#### Example + +The original `MATEKF405` target defined a multi-rotor (MR) servo on output S1. The later `MATEKF405_SERVOS6` target defined (for MR) S1 as a motor and S6 as a servo. This was more logical, but annoying for anyone who had a legacy `MATEKF405` tricopter with the servo on S1. + +#### Solution + +There is now a single `MATEKF405` target. The `target.c` sets the relevant outputs as: + +``` +DEF_TIM(TIM3, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 +DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 1), // S2 UP(2,1) +DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 1), // S3 UP(2,1) +DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 UP(2,1) +DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_LED, 0, 0), // S5 UP(1,7) +DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 UP(2,5) +DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 D(1,7)!S5 UP(2,6) +``` + +Using the "motors first" allocation, the servo would end up on S6, which in the legacy "tricopter servo on S1" case is not desired. + +Forcing the S1 output (`TIM3`) to servo is achieved by: + +``` +timer_output_mode 2 SERVOS +``` + +with resulting `resource` output: + +``` +C06: SERVO4 OUT +C07: MOTOR1 OUT +C08: MOTOR2 OUT +C09: MOTOR3 OUT +``` + +Note that the `timer` **index** in the `timer_output_mode` line is one less than the mnemonic in `target.c`, `timer` of 2 for `TIM3`. + +Note that the usual caveat that one should not share a timer with both a motor and a servo still apply. + ## Flash chip management For targets that have a flash data chip, typically used for blackbox logs, the following additional comamnds are provided. diff --git a/docs/Controls.md b/docs/Controls.md index 1b63f90e2a3..3cc62b4e740 100644 --- a/docs/Controls.md +++ b/docs/Controls.md @@ -44,9 +44,9 @@ The stick positions are combined to activate different functions: | Bypass Nav Arm disable | LOW | HIGH | CENTER | CENTER | | Save setting | LOW | LOW | LOW | HIGH | | Enter OSD Menu (CMS) | CENTER | LOW | HIGH | CENTER | -| Enter Camera OSD(RuncamDevice)| RIGHT | CENTER | CENTER | CENTER | -| Exit Camera OSD (RuncamDevice)| LEFT | CENTER | CENTER | CENTER | -| Confirm - Camera OSD | RIGHT | CENTER | CENTER | CENTER | +| Enter Camera OSD(RuncamDevice)| CENTER | HIGH | CENTER | CENTER | +| Exit Camera OSD (RuncamDevice)| CENTER | LOW | CENTER | CENTER | +| Confirm - Camera OSD | CENTER | HIGH | CENTER | CENTER | | Navigation - Camera OSD | CENTER | CENTER | * | * | For graphical stick position in all transmitter modes, check out [this page](https://www.mrd-rc.com/tutorials-tools-and-testing/inav-flight/inav-stick-commands-for-all-transmitter-modes/). diff --git a/docs/ESC and servo outputs.md b/docs/ESC and servo outputs.md index 78a5056590a..a9821603c06 100644 --- a/docs/ESC and servo outputs.md +++ b/docs/ESC and servo outputs.md @@ -28,8 +28,23 @@ While motors are usually ordered sequentially, here is no standard output layout ## Modifying output mapping -INAV 5 allows the limited output type mapping by allowing to change the function of *ALL* outputs at the same time. It can be done with the `output_mode` CLI setting. Allowed values: +### Modifying all outputs at the same time + +Since INAV 5, it has been possible to force *ALL* outputs to be `MOTORS` or `SERVOS`. + +Traditional ESCs usually can be controlled via a servo output, but would require calibration. + +This can be done with the `output_mode` CLI setting. Allowed values: * `AUTO` assigns outputs according to the default mapping * `SERVOS` assigns all outputs to servos -* `MOTORS` assigns all outputs to motors \ No newline at end of file +* `MOTORS` assigns all outputs to motors + +### Modifying only some outputs + +INAV 7 introduced extra functionality that let you force only some outputs to be either *MOTORS* or *SERVOS*, with some restrictions dictated by the hardware. + +The mains restrictions is that outputs need to be associated with timers, which are usually shared between multiple outputs. Two outputs on the same timer need to have the same function. + +The easiest way to modify outputs, is to use the Mixer tab in the Configurator, as it will clearly show you which timer is used by all outputs, but you can also use `timer_output_mode` on the cli. +This can be used in conjunction to the previous method, in that cass all outputs will follow `output_mode` and `timer_output_mode` overrides are applied after that. diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md new file mode 100644 index 00000000000..f6688feaeec --- /dev/null +++ b/docs/MixerProfile.md @@ -0,0 +1,144 @@ +# MixerProfile + +A MixerProfile is a set of motor mixer, servo-mixer and platform type configuration settings to enable VTOL transitions. + +Currently two profiles are supported on targets other than F411 and F722 (due to resource constraints on these FC). i.e VTOL transition is not available on F411 and F722 targets. + +By default, switching between profiles requires reboot to take affect. However, when all conditions are met, and a suitable [configuration](#configuration) has been applied, `mixer_profile` also allows in-flight profile [switching](#rc-mode-settings) to allow things like VTOL operation. This is the recommended operating mode. + +Please note that this is an emerging / experimental capability that will require some effort by the pilot to implement. + +## Setup for VTOL +- A VTOL specific FC target or `timer_output_mode` overrides was required in the early stage of the development, But since unified mapping introduced in INAV 7.0 It is not needed anymore. +- ~~For mixer profile switching it is necessary to keep motor and servo PWM mapping consistent between Fixed-Wing (FW) and Multi-rotor (MR) profiles~~ +- ~~Traditionally, FW and MR have had different enumerations to define the PWM mappings. For VTOL operation it is necessary to set the `timer_output_mode` overrides to allow a consistent enumeration and thus mapping between MR and FW modes.~~ +- ~~In operation, it is necessary to set the `mixer_profile` and the `pid_profile` separately and to set a [RC mode](#rc-mode-settings) to switch between them.~~ +## Configuration +### Timer overrides +Set the timer overrides for the outputs that you are intended to use. +For SITL builds, is not necessary to set timer overrides. +Please note that there are some display issues on the configurator that will show wrong mapping on the mixer_profile which has less motor/servo compared with the another +### Profile Switch + +Setup the FW mode and MR mode separately in two different mixer profiles: + +In this example, FW mode is `mixer_profile` 1 and MR mode is `mixer_profile` 2. + +Currently, the INAV Configurator does not fully support `mixer_profile`, so some of the settings have to be done in CLI. + +Add `set mixer_pid_profile_linking = ON` in order to enable `pid_profile` auto handling. It will change the `pid profile` index according to the `mixer_profile` index on FC boot and allow `mixer_profile` hot switching (this is recommended usage). + +The following 2 `mixer_profile` sections are added in the CLI: + +``` +#switch to mixer_profile by cli +mixer_profile 1 + +set platform_type = AIRPLANE +set model_preview_type = 26 +# motor stop feature have been moved to here +set motorstop_on_low = ON +# enable pid_profile auto handling (recommended). +set mixer_pid_profile_linking = ON +save +``` +Then finish the aeroplane setting on mixer_profile 1 + +``` +mixer_profile 2 + +set platform_type = TRICOPTER +set model_preview_type = 1 +# also enable pid_profile auto handling +set mixer_pid_profile_linking = ON +save +``` +Then finish the multi-rotor setting on `mixer_profile` 2. + +Note that default profile is profile `1`. + +You can use `MAX` servo input to set a fixed input for the tilting servo. Speed setting for `MAX` input is available in the CLI. + +It is recommended to have some amount of control surface (elevon / elevator) mapped for stabilization even in MR mode to get improved authority when airspeed is high. + +**Double check all settings in CLI with the `diff all` command**; make sure you have set the correct settings. Also check what will change with `mixer_profile`. For example servo output min / max range will not change. But `smix` and `mmix` will change. + +### Mixer Transition input + +Typically, 'transition input' will be useful in MR mode to gain airspeed. +Both the servo mixer and motor mixer can accept transition mode as an input. +The associated motor or servo will then move accordingly when transition mode is activated. +Transition input is disabled when navigation mode is activate + +The use of Transition Mode is recommended to enable further features and future developments like fail-safe support. Mapping motor to servo output, or servo with logic conditions is **not** recommended + +#### Servo + +38 is the input source for transition input; use this to tilt motor to gain airspeed. + +Example: Increase servo 1 output by +45 with speed of 150 when transition mode is activated for tilted motor setup: + +``` +# rule no; servo index; input source; rate; speed; activate logic function number +smix 6 1 38 45 150 -1 +``` +Please note there will be a time window that tilting motors is providing up lift but rear motor isn't. Result in a sudden pitch raise on the entering of the mode. More forward tilting servo position on transition input(you can use 'speed' in servo rules to slowly move to this position), A faster tilting servo speed on `MAX` servo input will reduce the time window. OR lower the throttle on the entering of the FW mode to mitigate the effect. + +#### Motor + +The default `mmix` throttle value is 0.0, It will not show in `diff` command when throttle value is 0.0 (unused); this causes the motor to stop. + +- 0.0:/src inav-build ` + Where `` must be replaced with the absolute path of where you cloned this repo (see above), and `` with the name of the target that you want to build. - + Note that on Windows/WSL 2 mounted /src folder is writeable for root user only. You have to run build under root user. You can achieve this by using `-u root` option in the command line above, or by removing "USER inav" line from the .\DockerFile before building image. + + Note that on Windows/WSL 2 mounted /src folder is writeable for root user only. You have to run build under root user. You can achieve this by using `-u root` option in the command line above. -3. If you need to update `Settings.md`, run `docker run --entrypoint /src/cmake/docker_docs.sh --rm -it -u root -v :/src inav-build` +3. If you need to update `Settings.md`, run: + +`docker run --entrypoint /src/cmake/docker_docs.sh --rm -it -u root -v :/src inav-build` + +4. Building SITL: + +`docker run --rm --entrypoint /src/cmake/docker_build_sitl.sh -it -u root -v :/src inav-build` + +5. Running SITL: + +`docker run -p 5760:5760 -p 5761:5761 -p 5762:5762 -p 5763:5763 -p 5764:5764 -p 5765:5765 -p 5766:5766 -p 5767:5767 --entrypoint /src/cmake/docker_run_sitl.sh --rm -it -u root -v :/src inav-build`. + + SITL command line parameters can be adjusted in `cmake/docker_run_sitl.sh`. Refer to the [Linux](#Linux) instructions or the [build script](/build.sh) for more details. diff --git a/docs/development/Building in Windows 10 or 11 with Linux Subsystem.md b/docs/development/Building in Windows 10 or 11 with Linux Subsystem.md index cf5cc08c061..a2c5894e72f 100644 --- a/docs/development/Building in Windows 10 or 11 with Linux Subsystem.md +++ b/docs/development/Building in Windows 10 or 11 with Linux Subsystem.md @@ -26,7 +26,7 @@ Install Git, Make, gcc and Ruby - `sudo apt-get install git make cmake ruby` Install python and python-yaml to allow updates to settings.md -- `sudo apt-get install python3 python-yaml` +- `sudo apt-get install python3` ### CMAKE and Ubuntu 18_04 diff --git a/docs/development/Software In The Loop (HITL) plugin for X-Plane 11.md b/docs/development/Hardware In The Loop (HITL) plugin for X-Plane.md similarity index 83% rename from docs/development/Software In The Loop (HITL) plugin for X-Plane 11.md rename to docs/development/Hardware In The Loop (HITL) plugin for X-Plane.md index 0b7d106e0d1..c166c5117ac 100644 --- a/docs/development/Software In The Loop (HITL) plugin for X-Plane 11.md +++ b/docs/development/Hardware In The Loop (HITL) plugin for X-Plane.md @@ -1,4 +1,4 @@ -# Software In The Loop (HITL) plugin for X-Plane 11 +# Hardware In The Loop (HITL) plugin for X-Plane 11/12 **Hardware-in-the-loop (HITL) simulation**, is a technique that is used in the development and testing of complex real-time embedded systems. @@ -6,6 +6,6 @@ **INAV-X-Plane-HITL** is plugin for **X-Plane** for testing and developing flight controllers with **INAV flight controller firmware** -https://github.com/iNavFlight/inav. +https://github.com/RomanLut/INAV-X-Plane-HITL HITL technique can be used to test features during development. Please check page above for installation instructions. diff --git a/docs/development/How to test a pull request.md b/docs/development/How to test a pull request.md index cb7abc57a44..4da0c27e732 100644 --- a/docs/development/How to test a pull request.md +++ b/docs/development/How to test a pull request.md @@ -16,7 +16,7 @@ Building the pull request manually or using custom/unofficial targets is not the - Make sure the pull request has passed all checks, otherwise you may not have pre-compiled firmware images. - Make a diff all backup of your existing INAV configuration. - Take notes of what INAV target you are using. -- You will need a recent version of INAV Configurator from master, or even a specific branch. If you don't need a specific branch, [inav-configurator-next](http://seyrsnys.myzen.co.uk/inav-configurator-next/) usually has recent unofficial pre-built versions of INAV Configurator. If your pull requests refers to an inav-configruator pull request, you are likely to need a specific branch of the configurator. In that case you can try to build it from source by following the build [``Instructions``](https://github.com/iNavFlight/inav-configurator#building-and-running-inav-configurator-locally-for-development) or follow instructions on how to do any needed configuration changes using the CLI. +- You will need a recent version of INAV Configurator from master, or even a specific branch. If you don't need a specific branch, [inav-configurator-next](https://seyrsnys-inav-cfg-next.surge.sh/) usually has recent unofficial pre-built versions of INAV Configurator. If your pull requests refers to an inav-configruator pull request, you are likely to need a specific branch of the configurator. In that case you can try to build it from source by following the build [``Instructions``](https://github.com/iNavFlight/inav-configurator#building-and-running-inav-configurator-locally-for-development) or follow instructions on how to do any needed configuration changes using the CLI. # Finding the pull request This is easy, but you will need to be logged in to your GitHub account. @@ -31,18 +31,11 @@ Once you find the one you are looking for, go ahead an open it! Click on the ``Checks`` tab -Click on ``Build firmware``, it should take you to the ``Actions`` tab. -![Search results](assets/pr_testing/build_firmware.png) +Click on the down arrow next to the number of artifacts +![Artifact list](assets/pr_testing/artifacts_download.png) -You should see a summary with a column saying ``Artifacts`` and a number. Click on the number to be taken to the list of artifacts. - -![Search results](assets/pr_testing/actions_summary.png) - -On the ``Artifacts`` list, there should be an artifact without SITL in its name. - -![Search results](assets/pr_testing/artifact_listing.png) - - Click on it to download the zip file containing pre-compiled firmware images for all INAV official targets. Extract all files and select the firmware for your target using the configurator by clicking on ``Load Firmware [Local]`` button. Don't forget to use the ``Full chip erase`` option, as there are no guarantees the firmware will be compatible with your existing settings. +You should see a list of files. The one without SITL in the name, the biggest one, will be a zip file with all official target .hex files. Click on it to download it to your computer. +Extract all files and select the firmware for your target using the configurator by clicking on ``Load Firmware [Local]`` button. Don't forget to use the ``Full chip erase`` option, as there are no guarantees the firmware will be compatible with your existing settings. # I have flashed the new firmware, what should I do next? diff --git a/docs/development/assets/pr_testing/actions_summary.png b/docs/development/assets/pr_testing/actions_summary.png deleted file mode 100644 index 88673a49bee..00000000000 Binary files a/docs/development/assets/pr_testing/actions_summary.png and /dev/null differ diff --git a/docs/development/assets/pr_testing/artifact_listing.png b/docs/development/assets/pr_testing/artifact_listing.png deleted file mode 100644 index 083e73060f8..00000000000 Binary files a/docs/development/assets/pr_testing/artifact_listing.png and /dev/null differ diff --git a/docs/development/assets/pr_testing/artifacts_download.png b/docs/development/assets/pr_testing/artifacts_download.png new file mode 100644 index 00000000000..4b6a13f5f08 Binary files /dev/null and b/docs/development/assets/pr_testing/artifacts_download.png differ diff --git a/docs/development/assets/pr_testing/build_firmware.png b/docs/development/assets/pr_testing/build_firmware.png deleted file mode 100644 index f4116508a37..00000000000 Binary files a/docs/development/assets/pr_testing/build_firmware.png and /dev/null differ diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index c6d445900a1..313de401868 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -327,6 +327,8 @@ main_sources(COMMON_SRC flight/rth_estimator.h flight/servos.c flight/servos.h + flight/mixer_profile.c + flight/mixer_profile.h flight/wind_estimator.c flight/wind_estimator.h flight/gyroanalyse.c @@ -339,6 +341,8 @@ main_sources(COMMON_SRC flight/secondary_dynamic_gyro_notch.h flight/dynamic_lpf.c flight/dynamic_lpf.h + flight/ez_tune.c + flight/ez_tune.h io/beeper.c io/beeper.h @@ -504,7 +508,6 @@ main_sources(COMMON_SRC io/gps.h io/gps_ublox.c io/gps_ublox_utils.c - io/gps_nmea.c io/gps_msp.c io/gps_fake.c io/gps_private.h diff --git a/src/main/cms/cms.h b/src/main/cms/cms.h index f1f8d8017c8..7084df8b50c 100644 --- a/src/main/cms/cms.h +++ b/src/main/cms/cms.h @@ -37,9 +37,9 @@ void cmsYieldDisplay(displayPort_t *pPort, timeMs_t duration); 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" -#define CMS_STARTUP_HELP_TEXT3 "+ PITCH UP" +#define CMS_STARTUP_HELP_TEXT1 "MENU: THR MID" +#define CMS_STARTUP_HELP_TEXT2 "+ YAW LEFT" +#define CMS_STARTUP_HELP_TEXT3 "+ PITCH UP" // cmsMenuExit special ptr values #define CMS_EXIT (0) diff --git a/src/main/cms/cms_menu_mixer_servo.c b/src/main/cms/cms_menu_mixer_servo.c index 92d18ab3a0a..e23229e7acd 100644 --- a/src/main/cms/cms_menu_mixer_servo.c +++ b/src/main/cms/cms_menu_mixer_servo.c @@ -31,6 +31,7 @@ #include "build/version.h" +#include "flight/mixer_profile.h" #include "flight/mixer.h" #include "flight/servos.h" @@ -40,6 +41,7 @@ #include "fc/runtime_config.h" #include "fc/settings.h" +#include "fc/config.h" static uint8_t currentMotorMixerIndex = 0; static uint8_t tmpcurrentMotorMixerIndex = 1; diff --git a/src/main/common/maths.h b/src/main/common/maths.h index cc3d1bc5176..ff0505fb531 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -25,15 +25,15 @@ #endif // Undefine this for use libc sinf/cosf. Keep this defined to use fast sin/cos approximations -#define FAST_MATH // order 9 approximation -//#define VERY_FAST_MATH // order 7 approximation +#define FAST_MATH // order 9 approximation +//#define VERY_FAST_MATH // order 7 approximation // Use floating point M_PI instead explicitly. -#define M_PIf 3.14159265358979323846f -#define M_LN2f 0.69314718055994530942f -#define M_Ef 2.71828182845904523536f +#define M_PIf 3.14159265358979323846f +#define M_LN2f 0.69314718055994530942f +#define M_Ef 2.71828182845904523536f -#define RAD (M_PIf / 180.0f) +#define RAD (M_PIf / 180.0f) #define DEGREES_TO_CENTIDEGREES(angle) ((angle) * 100) #define CENTIDEGREES_TO_DEGREES(angle) ((angle) / 100.0f) @@ -56,46 +56,46 @@ #define RADIANS_TO_CENTIDEGREES(angle) (((angle) * 100.0f) / RAD) #define CENTIDEGREES_TO_RADIANS(angle) (((angle) / 100.0f) * RAD) -#define CENTIMETERS_TO_CENTIFEET(cm) (cm / 0.3048f) -#define CENTIMETERS_TO_FEET(cm) (cm / 30.48f) -#define CENTIMETERS_TO_METERS(cm) (cm / 100.0f) +#define CENTIMETERS_TO_CENTIFEET(cm) (cm / 0.3048f) +#define CENTIMETERS_TO_FEET(cm) (cm / 30.48f) +#define CENTIMETERS_TO_METERS(cm) (cm / 100.0f) -#define METERS_TO_CENTIMETERS(m) (m * 100) +#define METERS_TO_CENTIMETERS(m) (m * 100) -#define CMSEC_TO_CENTIMPH(cms) (cms * 2.2369363f) -#define CMSEC_TO_CENTIKPH(cms) (cms * 3.6f) -#define CMSEC_TO_CENTIKNOTS(cms) (cms * 1.943845f) +#define CMSEC_TO_CENTIMPH(cms) (cms * 2.2369363f) +#define CMSEC_TO_CENTIKPH(cms) (cms * 3.6f) +#define CMSEC_TO_CENTIKNOTS(cms) (cms * 1.943845f) #define C_TO_KELVIN(temp) (temp + 273.15f) // Standard Sea Level values // Ref:https://en.wikipedia.org/wiki/Standard_sea_level -#define SSL_AIR_DENSITY 1.225f // kg/m^3 -#define SSL_AIR_PRESSURE 101325.01576f // Pascal -#define SSL_AIR_TEMPERATURE 288.15f // K +#define SSL_AIR_DENSITY 1.225f // kg/m^3 +#define SSL_AIR_PRESSURE 101325.01576f // Pascal +#define SSL_AIR_TEMPERATURE 288.15f // K // copied from https://code.google.com/p/cxutil/source/browse/include/cxutil/utility.h#70 -#define _CHOOSE2(binoper, lexpr, lvar, rexpr, rvar) \ - ( __extension__ ({ \ - __typeof__(lexpr) lvar = (lexpr); \ - __typeof__(rexpr) rvar = (rexpr); \ - lvar binoper rvar ? lvar : rvar; \ - })) +#define _CHOOSE2(binoper, lexpr, lvar, rexpr, rvar) \ + ( __extension__ ({ \ + __typeof__(lexpr) lvar = (lexpr); \ + __typeof__(rexpr) rvar = (rexpr); \ + lvar binoper rvar ? lvar : rvar; \ + })) #define _CHOOSE_VAR2(prefix, unique) prefix##unique #define _CHOOSE_VAR(prefix, unique) _CHOOSE_VAR2(prefix, unique) -#define _CHOOSE(binoper, lexpr, rexpr) \ - _CHOOSE2( \ - binoper, \ - lexpr, _CHOOSE_VAR(_left, __COUNTER__), \ - rexpr, _CHOOSE_VAR(_right, __COUNTER__) \ +#define _CHOOSE(binoper, lexpr, rexpr) \ + _CHOOSE2( \ + binoper, \ + lexpr, _CHOOSE_VAR(_left, __COUNTER__), \ + rexpr, _CHOOSE_VAR(_right, __COUNTER__) \ ) #define MIN(a, b) _CHOOSE(<, a, b) #define MAX(a, b) _CHOOSE(>, a, b) -#define _ABS_II(x, var) \ - ( __extension__ ({ \ - __typeof__(x) var = (x); \ - var < 0 ? -var : var; \ +#define _ABS_II(x, var) \ + ( __extension__ ({ \ + __typeof__(x) var = (x); \ + var < 0 ? -var : var; \ })) #define _ABS_I(x, var) _ABS_II(x, var) #define ABS(x) _ABS_I(x, _CHOOSE_VAR(_abs, __COUNTER__)) @@ -114,8 +114,7 @@ typedef union { fp_angles_def angles; } fp_angles_t; -typedef struct stdev_s -{ +typedef struct stdev_s { float m_oldM, m_newM, m_oldS, m_newS; int m_n; } stdev_t; diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 46293524b14..368ea8c887e 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -36,8 +36,8 @@ //#define PG_TRANSPONDER_CONFIG 17 #define PG_SYSTEM_CONFIG 18 #define PG_FEATURE_CONFIG 19 -#define PG_MIXER_CONFIG 20 -#define PG_SERVO_MIXER 21 +#define PG_MIXER_PROFILE 20 +// #define PG_SERVO_MIXER 21 #define PG_IMU_CONFIG 22 //#define PG_PROFILE_SELECTION 23 #define PG_RX_CONFIG 24 @@ -119,7 +119,9 @@ #define PG_UNUSED_1 1029 #define PG_POWER_LIMITS_CONFIG 1030 #define PG_OSD_COMMON_CONFIG 1031 -#define PG_INAV_END 1031 +#define PG_TIMER_OVERRIDE_CONFIG 1032 +#define PG_EZ_TUNE 1033 +#define PG_INAV_END PG_EZ_TUNE // OSD configuration (subject to change) //#define PG_OSD_FONT_CONFIG 2047 diff --git a/src/main/drivers/osd_symbols.h b/src/main/drivers/osd_symbols.h index 147725ab463..d645184e5b4 100644 --- a/src/main/drivers/osd_symbols.h +++ b/src/main/drivers/osd_symbols.h @@ -58,7 +58,7 @@ // 0x26 // 038 ASCII & #define SYM_VTX_POWER 0x27 // 039 VTx Power // 0x28 // 040 to 062 ASCII -#define SYM_AH_NM 0x3F // 063 Ah/NM +#define SYM_AH_NM 0x3F // 063 Ah/NM // 0x40 // 064 to 095 ASCII #define SYM_MAH_NM_0 0x60 // 096 mAh/NM left #define SYM_MAH_NM_1 0x61 // 097 mAh/NM right @@ -78,7 +78,7 @@ #define SYM_WH 0x6D // 109 WH #define SYM_WH_KM 0x6E // 110 WH/KM #define SYM_WH_MI 0x6F // 111 WH/MI -#define SYM_WH_NM 0x70 // 112 Wh/NM +#define SYM_WH_NM 0x70 // 112 Wh/NM #define SYM_WATT 0x71 // 113 WATTS #define SYM_MW 0x72 // 114 mW #define SYM_KILOWATT 0x73 // 115 kW @@ -159,6 +159,7 @@ #define SYM_HEADING_W 0xCB // 203 Heading Graphic west #define SYM_HEADING_DIVIDED_LINE 0xCC // 204 Heading Graphic #define SYM_HEADING_LINE 0xCD // 205 Heading Graphic + #define SYM_MAX 0xCE // 206 MAX symbol #define SYM_PROFILE 0xCF // 207 Profile symbol #define SYM_SWITCH_INDICATOR_LOW 0xD0 // 208 Switch High @@ -175,10 +176,10 @@ #define SYM_FLIGHT_HOURS_REMAINING 0xDB // 219 Flight time (hours) remaining #define SYM_GROUND_COURSE 0xDC // 220 Ground course #define SYM_ALERT 0xDD // 221 General alert symbol - #define SYM_TERRAIN_FOLLOWING 0xFB // 251 Terrain following (also Alt adjust) #define SYM_CROSS_TRACK_ERROR 0xFC // 252 Cross track error + #define SYM_LOGO_START 0x101 // 257 to 297, INAV logo #define SYM_LOGO_WIDTH 10 #define SYM_LOGO_HEIGHT 4 @@ -225,9 +226,10 @@ #define SYM_HUD_SIGNAL_3 0x163 // 355 Hud signal icon 75% #define SYM_HUD_SIGNAL_4 0x164 // 356 Hud signal icon 100% -#define SYM_HOME_DIST 0x165 // 357 DIST +#define SYM_HOME_DIST 0x165 // 357 DIST #define SYM_AH_CH_CENTER 0x166 // 358 Crossair center #define SYM_FLIGHT_DIST_REMAINING 0x167 // 359 Flight distance reminaing +#define SYM_ODOMETER 0x168 // 360 Odometer #define SYM_AH_CH_TYPE3 0x190 // 400 to 402, crosshair 3 #define SYM_AH_CH_TYPE4 0x193 // 403 to 405, crosshair 4 @@ -261,8 +263,13 @@ #define SYM_SERVO_PAN_IS_OFFSET_L 0x1C7 // 455 Pan servo is offset left #define SYM_SERVO_PAN_IS_OFFSET_R 0x1C8 // 456 Pan servo is offset right +#define SYM_PILOT_LOGO_SML_L 0x1D5 // 469 small Pilot logo Left +#define SYM_PILOT_LOGO_SML_C 0x1D6 // 470 small Pilot logo Centre +#define SYM_PILOT_LOGO_SML_R 0x1D7 // 471 small Pilot logo Right +#define SYM_PILOT_LOGO_LRG_START 0x1D8 // 472 to 511, Pilot logo + #else -#define TEMP_SENSOR_SYM_COUNT 0 +#define TEMP_SENSOR_SYM_COUNT 0 #endif // USE_OSD diff --git a/src/main/drivers/pwm_esc_detect.c b/src/main/drivers/pwm_esc_detect.c index 1cd3c72cd08..8abebbb7b1d 100644 --- a/src/main/drivers/pwm_esc_detect.c +++ b/src/main/drivers/pwm_esc_detect.c @@ -38,7 +38,7 @@ uint8_t hardwareMotorType = MOTOR_UNKNOWN; void detectBrushedESC(void) { for (int i = 0; i < timerHardwareCount; i++) { - if (timerHardware[i].usageFlags & TIM_USE_MC_MOTOR) { + if (timerHardware[i].usageFlags & TIM_USE_MOTOR) { IO_t MotorDetectPin = IOGetByTag(timerHardware[i].tag); IOInit(MotorDetectPin, OWNER_SYSTEM, RESOURCE_INPUT, 0); IOConfigGPIO(MotorDetectPin, IOCFG_IPU); diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 4fc5a00b83e..0486eb5ac19 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -211,46 +211,111 @@ static bool checkPwmTimerConflicts(const timerHardware_t *timHw) } static void timerHardwareOverride(timerHardware_t * timer) { - if (mixerConfig()->outputMode == OUTPUT_MODE_SERVOS) { - - //Motors are rewritten as servos - if (timer->usageFlags & TIM_USE_MC_MOTOR) { - timer->usageFlags = timer->usageFlags & ~TIM_USE_MC_MOTOR; - timer->usageFlags = timer->usageFlags | TIM_USE_MC_SERVO; + switch (timerOverrides(timer2id(timer->tim))->outputMode) { + case OUTPUT_MODE_MOTORS: + if (TIM_IS_SERVO(timer->usageFlags)) { + timer->usageFlags &= ~TIM_USE_SERVO; + timer->usageFlags |= TIM_USE_MOTOR; + } + break; + case OUTPUT_MODE_SERVOS: + if (TIM_IS_MOTOR(timer->usageFlags)) { + timer->usageFlags &= ~TIM_USE_MOTOR; + timer->usageFlags |= TIM_USE_SERVO; + } + break; + } +} + +bool pwmHasMotorOnTimer(timMotorServoHardware_t * timOutputs, HAL_Timer_t *tim) +{ + for (int i = 0; i < timOutputs->maxTimMotorCount; ++i) { + if (timOutputs->timMotors[i]->tim == tim) { + return true; + } + } + + return false; +} + +bool pwmHasServoOnTimer(timMotorServoHardware_t * timOutputs, HAL_Timer_t *tim) +{ + for (int i = 0; i < timOutputs->maxTimServoCount; ++i) { + if (timOutputs->timServos[i]->tim == tim) { + return true; + } + } + + return false; +} + +uint8_t pwmClaimTimer(HAL_Timer_t *tim, uint32_t usageFlags) { + uint8_t changed = 0; + for (int idx = 0; idx < timerHardwareCount; idx++) { + timerHardware_t *timHw = &timerHardware[idx]; + if (timHw->tim == tim && timHw->usageFlags != usageFlags) { + timHw->usageFlags = usageFlags; + changed++; + } + } + + return changed; +} + +void pwmEnsureEnoughtMotors(uint8_t motorCount) +{ + uint8_t motorOnlyOutputs = 0; + + for (int idx = 0; idx < timerHardwareCount; idx++) { + timerHardware_t *timHw = &timerHardware[idx]; + + timerHardwareOverride(timHw); + + if (checkPwmTimerConflicts(timHw)) { + continue; } - if (timer->usageFlags & TIM_USE_FW_MOTOR) { - timer->usageFlags = timer->usageFlags & ~TIM_USE_FW_MOTOR; - timer->usageFlags = timer->usageFlags | TIM_USE_FW_SERVO; + + if (TIM_IS_MOTOR_ONLY(timHw->usageFlags)) { + motorOnlyOutputs++; + motorOnlyOutputs += pwmClaimTimer(timHw->tim, timHw->usageFlags); } - - } else if (mixerConfig()->outputMode == OUTPUT_MODE_MOTORS) { - - // Servos are rewritten as motors - if (timer->usageFlags & TIM_USE_MC_SERVO) { - timer->usageFlags = timer->usageFlags & ~TIM_USE_MC_SERVO; - timer->usageFlags = timer->usageFlags | TIM_USE_MC_MOTOR; + } + + for (int idx = 0; idx < timerHardwareCount; idx++) { + timerHardware_t *timHw = &timerHardware[idx]; + + if (checkPwmTimerConflicts(timHw)) { + continue; } - if (timer->usageFlags & TIM_USE_FW_SERVO) { - timer->usageFlags = timer->usageFlags & ~TIM_USE_FW_SERVO; - timer->usageFlags = timer->usageFlags | TIM_USE_FW_MOTOR; + + if (TIM_IS_MOTOR(timHw->usageFlags) && !TIM_IS_MOTOR_ONLY(timHw->usageFlags)) { + if (motorOnlyOutputs < motorCount) { + timHw->usageFlags &= ~TIM_USE_SERVO; + timHw->usageFlags |= TIM_USE_MOTOR; + motorOnlyOutputs++; + motorOnlyOutputs += pwmClaimTimer(timHw->tim, timHw->usageFlags); + } else { + timHw->usageFlags &= ~TIM_USE_MOTOR; + pwmClaimTimer(timHw->tim, timHw->usageFlags); + } } } } void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerUsingServos) { + UNUSED(isMixerUsingServos); timOutputs->maxTimMotorCount = 0; timOutputs->maxTimServoCount = 0; uint8_t motorCount = getMotorCount(); uint8_t motorIdx = 0; - for (int idx = 0; idx < timerHardwareCount; idx++) { + pwmEnsureEnoughtMotors(motorCount); + for (int idx = 0; idx < timerHardwareCount; idx++) { timerHardware_t *timHw = &timerHardware[idx]; - timerHardwareOverride(timHw); - int type = MAP_TO_NONE; // Check for known conflicts (i.e. UART, LEDSTRIP, Rangefinder and ADC) @@ -259,39 +324,29 @@ void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerU continue; } - // Determine if timer belongs to motor/servo - if (mixerConfig()->platformType == PLATFORM_MULTIROTOR || mixerConfig()->platformType == PLATFORM_TRICOPTER) { - // Multicopter - - // Make sure first motorCount outputs get assigned to motor - if ((timHw->usageFlags & TIM_USE_MC_MOTOR) && (motorIdx < motorCount)) { - timHw->usageFlags = timHw->usageFlags & ~TIM_USE_MC_SERVO; - motorIdx += 1; - } + // Make sure first motorCount motor outputs get assigned to motor + if (TIM_IS_MOTOR(timHw->usageFlags) && (motorIdx < motorCount)) { + timHw->usageFlags &= ~TIM_USE_SERVO; + pwmClaimTimer(timHw->tim, timHw->usageFlags); + motorIdx += 1; + } - // We enable mapping to servos if mixer is actually using them - if (isMixerUsingServos && timHw->usageFlags & TIM_USE_MC_SERVO) { - type = MAP_TO_SERVO_OUTPUT; - } - else if (timHw->usageFlags & TIM_USE_MC_MOTOR) { - type = MAP_TO_MOTOR_OUTPUT; - } - } else { - // Fixed wing or HELI (one/two motors and a lot of servos - if (timHw->usageFlags & TIM_USE_FW_SERVO) { - type = MAP_TO_SERVO_OUTPUT; - } - else if (timHw->usageFlags & TIM_USE_FW_MOTOR) { - type = MAP_TO_MOTOR_OUTPUT; - } + if (TIM_IS_SERVO(timHw->usageFlags) && !pwmHasMotorOnTimer(timOutputs, timHw->tim)) { + type = MAP_TO_SERVO_OUTPUT; + } else if (TIM_IS_MOTOR(timHw->usageFlags) && !pwmHasServoOnTimer(timOutputs, timHw->tim)) { + type = MAP_TO_MOTOR_OUTPUT; } switch(type) { case MAP_TO_MOTOR_OUTPUT: + timHw->usageFlags &= TIM_USE_MOTOR; timOutputs->timMotors[timOutputs->maxTimMotorCount++] = timHw; + pwmClaimTimer(timHw->tim, timHw->usageFlags); break; case MAP_TO_SERVO_OUTPUT: + timHw->usageFlags &= TIM_USE_SERVO; timOutputs->timServos[timOutputs->maxTimServoCount++] = timHw; + pwmClaimTimer(timHw->tim, timHw->usageFlags); break; default: break; diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index 14c3f9293ba..e9b3a0f9573 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -19,6 +19,7 @@ #include "drivers/io_types.h" #include "flight/mixer.h" +#include "flight/mixer_profile.h" #include "flight/servos.h" #if defined(TARGET_MOTOR_COUNT) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index bf38ec93404..d898b1595f1 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -119,6 +119,7 @@ static bool pwmMotorsEnabled = true; static timeUs_t digitalMotorUpdateIntervalUs = 0; static timeUs_t digitalMotorLastUpdateUs; static timeUs_t lastCommandSent = 0; +static timeUs_t commandPostDelay = 0; static circularBuffer_t commandsCircularBuffer; static uint8_t commandsBuff[DHSOT_COMMAND_QUEUE_SIZE]; @@ -419,7 +420,7 @@ static int getDShotCommandRepeats(dshotCommands_e cmd) { return repeats; } -static void executeDShotCommands(void){ +static bool executeDShotCommands(void){ timeUs_t tNow = micros(); @@ -430,18 +431,30 @@ static void executeDShotCommands(void){ dshotCommands_e cmd; circularBufferPopHead(&commandsCircularBuffer, (uint8_t *) &cmd); currentExecutingCommand.cmd = cmd; - currentExecutingCommand.remainingRepeats = getDShotCommandRepeats(cmd); + currentExecutingCommand.remainingRepeats = getDShotCommandRepeats(cmd); + commandPostDelay = DSHOT_COMMAND_INTERVAL_US; } else { - return; + if (commandPostDelay) { + if (tNow - lastCommandSent < commandPostDelay) { + return false; + } + commandPostDelay = 0; + } + + return true; } } - delayMicroseconds(DSHOT_COMMAND_DELAY_US); for (uint8_t i = 0; i < getMotorCount(); i++) { motors[i].requestTelemetry = true; motors[i].value = currentExecutingCommand.cmd; } - currentExecutingCommand.remainingRepeats--; - lastCommandSent = tNow; + if (tNow - lastCommandSent >= DSHOT_COMMAND_DELAY_US) { + currentExecutingCommand.remainingRepeats--; + lastCommandSent = tNow; + return true; + } else { + return false; + } } #endif @@ -464,7 +477,9 @@ void pwmCompleteMotorUpdate(void) { #ifdef USE_DSHOT if (isMotorProtocolDshot()) { - executeDShotCommands(); + if (!executeDShotCommands()) { + return; + } #ifdef USE_DSHOT_DMAR for (int index = 0; index < motorCount; index++) { diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index 664bae3ca59..c03db7e57b9 100644 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -38,8 +38,18 @@ timHardwareContext_t * timerCtx[HARDWARE_TIMER_DEFINITION_COUNT]; + +uint8_t timer2id(const HAL_Timer_t *tim) +{ + for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; ++i) { + if (timerDefinitions[i].tim == tim) return i; + } + + return (uint8_t)-1; +} + #if defined(AT32F43x) -uint8_t lookupTimerIndex(const tmr_type *tim) +uint8_t lookupTimerIndex(const HAL_Timer_t *tim) { int i; @@ -54,7 +64,7 @@ uint8_t lookupTimerIndex(const tmr_type *tim) } #else // return index of timer in timer table. Lowest timer has index 0 -uint8_t lookupTimerIndex(const TIM_TypeDef *tim) +uint8_t lookupTimerIndex(const HAL_Timer_t *tim) { int i; diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index b4b3ed8da03..a207aff0c56 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -25,6 +25,8 @@ #include "drivers/rcc_types.h" #include "drivers/timer_def.h" +#include "platform.h" + #define CC_CHANNELS_PER_TIMER 4 // TIM_Channel_1..4 typedef uint16_t captureCompare_t; // 16 bit on both 103 and 303, just register access must be 32bit sometimes (use timCCR_t) @@ -63,8 +65,9 @@ typedef uint32_t timCNT_t; #endif // tmr_type instead in AT32 #if defined(AT32F43x) +typedef tmr_type HAL_Timer_t; typedef struct timerDef_s { - tmr_type * tim; + HAL_Timer_t * tim; rccPeriphTag_t rcc; uint8_t irq; uint8_t secondIrq; @@ -82,8 +85,9 @@ typedef struct timerHardware_s { uint32_t dmaMuxid; //DMAMUX ID } timerHardware_t; #else +typedef TIM_TypeDef HAL_Timer_t; typedef struct timerDef_s { - TIM_TypeDef * tim; + HAL_Timer_t * tim; rccPeriphTag_t rcc; uint8_t irq; uint8_t secondIrq; @@ -106,15 +110,22 @@ typedef enum { TIM_USE_ANY = 0, TIM_USE_PPM = (1 << 0), TIM_USE_PWM = (1 << 1), - TIM_USE_MC_MOTOR = (1 << 2), // Multicopter motor output - TIM_USE_MC_SERVO = (1 << 3), // Multicopter servo output (i.e. TRI) + TIM_USE_MOTOR = (1 << 2), // Motor output + TIM_USE_SERVO = (1 << 3), // Servo output TIM_USE_MC_CHNFW = (1 << 4), // Deprecated and not used after removal of CHANNEL_FORWARDING feature - TIM_USE_FW_MOTOR = (1 << 5), - TIM_USE_FW_SERVO = (1 << 6), + //TIM_USE_FW_MOTOR = (1 << 5), // We no longer differentiate mc from fw on pwm allocation + //TIM_USE_FW_SERVO = (1 << 6), TIM_USE_LED = (1 << 24), TIM_USE_BEEPER = (1 << 25), } timerUsageFlag_e; +#define TIM_USE_OUTPUT_AUTO (TIM_USE_MOTOR | TIM_USE_SERVO) + +#define TIM_IS_MOTOR(flags) ((flags) & TIM_USE_MOTOR) +#define TIM_IS_SERVO(flags) ((flags) & TIM_USE_SERVO) + +#define TIM_IS_MOTOR_ONLY(flags) (TIM_IS_MOTOR(flags) && !TIM_IS_SERVO(flags)) +#define TIM_IS_SERVO_ONLY(flags) (!TIM_IS_MOTOR(flags) && TIM_IS_SERVO(flags)) enum { TIMER_OUTPUT_NONE = 0x00, @@ -248,7 +259,9 @@ bool timerPWMDMAInProgress(TCH_t * tch); volatile timCCR_t *timerCCR(TCH_t * tch); +uint8_t timer2id(const HAL_Timer_t *tim); + #ifdef USE_DSHOT_DMAR bool timerPWMConfigDMABurst(burstDmaTimer_t *burstDmaTimer, TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount); void pwmBurstDMAStart(burstDmaTimer_t * burstDmaTimer, uint32_t BurstLength); -#endif \ No newline at end of file +#endif diff --git a/src/main/drivers/timer_impl_stdperiph.c b/src/main/drivers/timer_impl_stdperiph.c index 5cb3457c77b..620d6f6993a 100644 --- a/src/main/drivers/timer_impl_stdperiph.c +++ b/src/main/drivers/timer_impl_stdperiph.c @@ -301,7 +301,12 @@ bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint8_t dmaBuf TIM_CtrlPWMOutputs(timer, ENABLE); TIM_ARRPreloadConfig(timer, ENABLE); - TIM_CCxCmd(timer, lookupTIMChannelTable[tch->timHw->channelIndex], TIM_CCx_Enable); + if (tch->timHw->output & TIMER_OUTPUT_N_CHANNEL) { + TIM_CCxNCmd(timer, lookupTIMChannelTable[tch->timHw->channelIndex], TIM_CCxN_Enable); + } else { + TIM_CCxCmd(timer, lookupTIMChannelTable[tch->timHw->channelIndex], TIM_CCx_Enable); + } + TIM_Cmd(timer, ENABLE); dmaInit(tch->dma, OWNER_TIMER, 0); @@ -382,7 +387,12 @@ bool impl_timerPWMConfigDMABurst(burstDmaTimer_t *burstDmaTimer, TCH_t * tch, vo TIM_CtrlPWMOutputs(timer, ENABLE); TIM_ARRPreloadConfig(timer, ENABLE); - TIM_CCxCmd(timer, lookupTIMChannelTable[tch->timHw->channelIndex], TIM_CCx_Enable); + if (tch->timHw->output & TIMER_OUTPUT_N_CHANNEL) { + TIM_CCxNCmd(timer, lookupTIMChannelTable[tch->timHw->channelIndex], TIM_CCxN_Enable); + } else { + TIM_CCxCmd(timer, lookupTIMChannelTable[tch->timHw->channelIndex], TIM_CCx_Enable); + } + TIM_Cmd(timer, ENABLE); if (!tch->timCtx->dmaBurstRef) { diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index a14aac38a28..f2ccec9ec07 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -81,7 +81,7 @@ bool cliMode = false; #include "flight/failsafe.h" #include "flight/imu.h" -#include "flight/mixer.h" +#include "flight/mixer_profile.h" #include "flight/pid.h" #include "flight/servos.h" @@ -156,6 +156,13 @@ static const char * const featureNames[] = { "OSD", "FW_LAUNCH", "FW_AUTOTRIM", NULL }; +static const char * outputModeNames[] = { + "AUTO", + "MOTORS", + "SERVOS", + NULL +}; + #ifdef USE_BLACKBOX static const char * const blackboxIncludeFlagNames[] = { "NAV_ACC", @@ -282,7 +289,7 @@ typedef enum { DUMP_MASTER = (1 << 0), DUMP_PROFILE = (1 << 1), DUMP_BATTERY_PROFILE = (1 << 2), - DUMP_RATES = (1 << 3), + DUMP_MIXER_PROFILE = (1 << 3), DUMP_ALL = (1 << 4), DO_DIFF = (1 << 5), SHOW_DEFAULTS = (1 << 6), @@ -1038,7 +1045,7 @@ static void cliAdjustmentRange(char *cmdline) } static void printMotorMix(uint8_t dumpMask, const motorMixer_t *primaryMotorMixer, const motorMixer_t *defaultprimaryMotorMixer) -{ +{ const char *format = "mmix %d %s %s %s %s"; char buf0[FTOA_BUFFER_SIZE]; char buf1[FTOA_BUFFER_SIZE]; @@ -1862,7 +1869,7 @@ static void cliServoMix(char *cmdline) printServoMix(DUMP_MASTER, customServoMixers(0), NULL); } else if (sl_strncasecmp(cmdline, "reset", 5) == 0) { // erase custom mixer - pgResetCopy(customServoMixersMutable(0), PG_SERVO_MIXER); + Reset_servoMixers(customServoMixersMutable(0)); } else { enum {RULE = 0, TARGET, INPUT, RATE, SPEED, CONDITION, ARGS_COUNT}; char *ptr = strtok_r(cmdline, " ", &saveptr); @@ -2514,6 +2521,82 @@ static void cliOsdLayout(char *cmdline) #endif +static void printTimerOutputModes(dumpFlags_e dumpFlags, const timerOverride_t* to, const timerOverride_t* defaultTimerOverride, int timer) +{ + const char *format = "timer_output_mode %d %s"; + + for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; ++i) { + if (timer < 0 || timer == i) { + outputMode_e mode = to[i].outputMode; + bool equalsDefault = false; + if(defaultTimerOverride) { + outputMode_e defaultMode = defaultTimerOverride[i].outputMode; + equalsDefault = mode == defaultMode; + cliDefaultPrintLinef(dumpFlags, equalsDefault, format, i, outputModeNames[defaultMode]); + } + cliDumpPrintLinef(dumpFlags, equalsDefault, format, i, outputModeNames[mode]); + } + } +} + +static void cliTimerOutputMode(char *cmdline) +{ + char * saveptr; + + int timer = -1; + uint8_t mode; + char *tok = strtok_r(cmdline, " ", &saveptr); + + int ii; + + for (ii = 0; tok != NULL; ii++, tok = strtok_r(NULL, " ", &saveptr)) { + switch (ii) { + case 0: + timer = fastA2I(tok); + if (timer < 0 || timer >= HARDWARE_TIMER_DEFINITION_COUNT) { + cliShowParseError(); + return; + } + break; + case 1: + if(!sl_strcasecmp("AUTO", tok)) { + mode = OUTPUT_MODE_AUTO; + } else if(!sl_strcasecmp("MOTORS", tok)) { + mode = OUTPUT_MODE_MOTORS; + } else if(!sl_strcasecmp("SERVOS", tok)) { + mode = OUTPUT_MODE_SERVOS; + } else { + cliShowParseError(); + return; + } + break; + default: + cliShowParseError(); + return; + } + } + + switch (ii) { + case 0: + FALLTHROUGH; + case 1: + // No args, or just timer. If any of them not provided, + // it will be the -1 that we used during initialization, so printOsdLayout() + // won't use them for filtering. + printTimerOutputModes(DUMP_MASTER, timerOverrides(0), NULL, timer); + break; + case 2: + timerOverridesMutable(timer)->outputMode = mode; + printTimerOutputModes(DUMP_MASTER, timerOverrides(0), NULL, timer); + break; + default: + // Unhandled + cliShowParseError(); + return; + } + +} + static void printFeature(uint8_t dumpMask, const featureConfig_t *featureConfig, const featureConfig_t *featureConfigDefault) { uint32_t mask = featureConfig->enabledFeatures; @@ -3049,6 +3132,39 @@ static void cliDumpBatteryProfile(uint8_t profileIndex, uint8_t dumpMask) dumpAllValues(BATTERY_CONFIG_VALUE, dumpMask); } +static void cliMixerProfile(char *cmdline) +{ + // CLI profile index is 1-based + if (isEmpty(cmdline)) { + cliPrintLinef("mixer_profile %d", getConfigMixerProfile() + 1); + return; + } else { + const int i = fastA2I(cmdline) - 1; + if (i >= 0 && i < MAX_MIXER_PROFILE_COUNT) { + setConfigMixerProfileAndWriteEEPROM(i); + cliMixerProfile(""); + } + } +} + +static void cliDumpMixerProfile(uint8_t profileIndex, uint8_t dumpMask) +{ + if (profileIndex >= MAX_MIXER_PROFILE_COUNT) { + // Faulty values + return; + } + setConfigMixerProfile(profileIndex); + cliPrintHashLine("mixer_profile"); + cliPrintLinef("mixer_profile %d\r\n", getConfigMixerProfile() + 1); + dumpAllValues(MIXER_CONFIG_VALUE, dumpMask); + cliPrintHashLine("Mixer: motor mixer"); + cliDumpPrintLinef(dumpMask, primaryMotorMixer_CopyArray()[0].throttle == 0.0f, "\r\nmmix reset\r\n"); + printMotorMix(dumpMask, primaryMotorMixer_CopyArray(), primaryMotorMixer(0)); + cliPrintHashLine("Mixer: servo mixer"); + cliDumpPrintLinef(dumpMask, customServoMixers_CopyArray()[0].rate == 0, "smix reset\r\n"); + printServoMix(dumpMask, customServoMixers_CopyArray(), customServoMixers(0)); +} + #ifdef USE_CLI_BATCH static void cliPrintCommandBatchWarning(const char *warning) { @@ -3604,6 +3720,8 @@ static void printConfig(const char *cmdline, bool doDiff) dumpMask = DUMP_PROFILE; // only } else if ((options = checkCommand(cmdline, "battery_profile"))) { dumpMask = DUMP_BATTERY_PROFILE; // only + } else if ((options = checkCommand(cmdline, "mixer_profile"))) { + dumpMask = DUMP_MIXER_PROFILE; // only } else if ((options = checkCommand(cmdline, "all"))) { dumpMask = DUMP_ALL; // all profiles and rates } else { @@ -3616,12 +3734,14 @@ static void printConfig(const char *cmdline, bool doDiff) const int currentProfileIndexSave = getConfigProfile(); const int currentBatteryProfileIndexSave = getConfigBatteryProfile(); + const int currentMixerProfileIndexSave = getConfigMixerProfile(); backupConfigs(); // reset all configs to defaults to do differencing resetConfigs(); // restore the profile indices, since they should not be reset for proper comparison setConfigProfile(currentProfileIndexSave); setConfigBatteryProfile(currentBatteryProfileIndexSave); + setConfigMixerProfile(currentMixerProfileIndexSave); if (checkCommand(options, "showdefaults")) { dumpMask = dumpMask | SHOW_DEFAULTS; // add default values as comments for changed values @@ -3652,15 +3772,8 @@ static void printConfig(const char *cmdline, bool doDiff) cliPrintHashLine("resources"); //printResource(dumpMask, &defaultConfig); - cliPrintHashLine("Mixer: motor mixer"); - cliDumpPrintLinef(dumpMask, primaryMotorMixer_CopyArray[0].throttle == 0.0f, "\r\nmmix reset\r\n"); - - printMotorMix(dumpMask, primaryMotorMixer_CopyArray, primaryMotorMixer(0)); - - // print custom servo mixer if exists - cliPrintHashLine("Mixer: servo mixer"); - cliDumpPrintLinef(dumpMask, customServoMixers_CopyArray[0].rate == 0, "smix reset\r\n"); - printServoMix(dumpMask, customServoMixers_CopyArray, customServoMixers(0)); + cliPrintHashLine("Timer overrides"); + printTimerOutputModes(dumpMask, timerOverrides_CopyArray, timerOverrides(0), -1); // print servo parameters cliPrintHashLine("Outputs [servo]"); @@ -3743,6 +3856,10 @@ static void printConfig(const char *cmdline, bool doDiff) // dump all profiles const int currentProfileIndexSave = getConfigProfile(); const int currentBatteryProfileIndexSave = getConfigBatteryProfile(); + const int currentMixerProfileIndexSave = getConfigMixerProfile(); + for (int ii = 0; ii < MAX_MIXER_PROFILE_COUNT; ++ii) { + cliDumpMixerProfile(ii, dumpMask); + } for (int ii = 0; ii < MAX_PROFILE_COUNT; ++ii) { cliDumpProfile(ii, dumpMask); } @@ -3751,8 +3868,10 @@ static void printConfig(const char *cmdline, bool doDiff) } setConfigProfile(currentProfileIndexSave); setConfigBatteryProfile(currentBatteryProfileIndexSave); + setConfigMixerProfile(currentMixerProfileIndexSave); cliPrintHashLine("restore original profile selection"); + cliPrintLinef("mixer_profile %d", currentMixerProfileIndexSave + 1); cliPrintLinef("profile %d", currentProfileIndexSave + 1); cliPrintLinef("battery_profile %d", currentBatteryProfileIndexSave + 1); @@ -3761,11 +3880,15 @@ static void printConfig(const char *cmdline, bool doDiff) #endif } else { // dump just the current profiles + cliDumpMixerProfile(getConfigMixerProfile(), dumpMask); cliDumpProfile(getConfigProfile(), dumpMask); cliDumpBatteryProfile(getConfigBatteryProfile(), dumpMask); } } - + if (dumpMask & DUMP_MIXER_PROFILE) { + cliDumpMixerProfile(getConfigMixerProfile(), dumpMask); + } + if (dumpMask & DUMP_PROFILE) { cliDumpProfile(getConfigProfile(), dumpMask); } @@ -3925,6 +4048,8 @@ const clicmd_t cmdTable[] = { "[]", cliProfile), CLI_COMMAND_DEF("battery_profile", "change battery profile", "[]", cliBatteryProfile), + CLI_COMMAND_DEF("mixer_profile", "change mixer profile", + "[]", cliMixerProfile), CLI_COMMAND_DEF("resource", "view currently used resources", NULL, cliResource), CLI_COMMAND_DEF("rxrange", "configure rx channel ranges", NULL, cliRxRange), #if defined(USE_SAFE_HOME) @@ -3968,6 +4093,7 @@ const clicmd_t cmdTable[] = { #ifdef USE_OSD CLI_COMMAND_DEF("osd_layout", "get or set the layout of OSD items", "[ [ [ []]]]", cliOsdLayout), #endif + CLI_COMMAND_DEF("timer_output_mode", "get or set the outputmode for a given timer.", "[ []]", cliTimerOutputMode), }; static void cliHelp(char *cmdline) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 3b884db7474..52cb06f81a6 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -64,6 +64,7 @@ #include "flight/pid.h" #include "flight/imu.h" #include "flight/failsafe.h" +#include "flight/ez_tune.h" #include "fc/config.h" #include "fc/controlrate_profile.h" @@ -107,6 +108,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, PG_RESET_TEMPLATE(systemConfig_t, systemConfig, .current_profile_index = 0, .current_battery_profile_index = 0, + .current_mixer_profile_index = 0, .debug_mode = SETTING_DEBUG_MODE_DEFAULT, #ifdef USE_DEV_TOOLS .groundTestMode = SETTING_GROUND_TEST_MODE_DEFAULT, // disables motors, set heading trusted for FW (for dev use) @@ -304,6 +306,7 @@ static void activateConfig(void) { activateControlRateConfig(); activateBatteryProfile(); + activateMixerConfig(); resetAdjustmentStates(); @@ -330,6 +333,7 @@ void readEEPROM(void) setConfigProfile(getConfigProfile()); setConfigBatteryProfile(getConfigBatteryProfile()); + setConfigMixerProfile(getConfigMixerProfile()); validateAndFixConfig(); activateConfig(); @@ -423,6 +427,9 @@ bool setConfigProfile(uint8_t profileIndex) systemConfigMutable()->current_profile_index = profileIndex; // set the control rate profile to match setControlRateProfile(profileIndex); +#ifdef USE_EZ_TUNE + ezTuneUpdate(); +#endif return ret; } @@ -469,6 +476,36 @@ void setConfigBatteryProfileAndWriteEEPROM(uint8_t profileIndex) beeperConfirmationBeeps(profileIndex + 1); } +uint8_t getConfigMixerProfile(void) +{ + return systemConfig()->current_mixer_profile_index; +} + +bool setConfigMixerProfile(uint8_t profileIndex) +{ + bool ret = true; // return true if current_mixer_profile_index has changed + if (systemConfig()->current_mixer_profile_index == profileIndex) { + ret = false; + } + if (profileIndex >= MAX_MIXER_PROFILE_COUNT) {// sanity check + profileIndex = 0; + } + systemConfigMutable()->current_mixer_profile_index = profileIndex; + return ret; +} + +void setConfigMixerProfileAndWriteEEPROM(uint8_t profileIndex) +{ + if (setConfigMixerProfile(profileIndex)) { + // profile has changed, so ensure current values saved before new profile is loaded + suspendRxSignal(); + writeEEPROM(); + readEEPROM(); + resumeRxSignal(); + } + beeperConfirmationBeeps(profileIndex + 1); +} + void setGyroCalibration(float getGyroZero[XYZ_AXIS_COUNT]) { gyroConfigMutable()->gyro_zero_cal[X] = (int16_t) getGyroZero[X]; diff --git a/src/main/fc/config.h b/src/main/fc/config.h index e32a705e1a1..bafa5c0cbe0 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -36,7 +36,7 @@ typedef enum { FEATURE_VBAT = 1 << 1, FEATURE_TX_PROF_SEL = 1 << 2, // Profile selection by TX stick command FEATURE_BAT_PROFILE_AUTOSWITCH = 1 << 3, - FEATURE_MOTOR_STOP = 1 << 4, + FEATURE_UNUSED_12 = 1 << 4, //was FEATURE_MOTOR_STOP FEATURE_UNUSED_1 = 1 << 5, // was FEATURE_SERVO_TILT was FEATURE_DYNAMIC_FILTERS FEATURE_SOFTSERIAL = 1 << 6, FEATURE_GPS = 1 << 7, @@ -69,6 +69,7 @@ typedef enum { typedef struct systemConfig_s { uint8_t current_profile_index; uint8_t current_battery_profile_index; + uint8_t current_mixer_profile_index; uint8_t debug_mode; #ifdef USE_DEV_TOOLS bool groundTestMode; // Disables motor ouput, sets heading trusted on FW (for dev use) @@ -138,6 +139,10 @@ uint8_t getConfigBatteryProfile(void); bool setConfigBatteryProfile(uint8_t profileIndex); void setConfigBatteryProfileAndWriteEEPROM(uint8_t profileIndex); +uint8_t getConfigMixerProfile(void); +bool setConfigMixerProfile(uint8_t profileIndex); +void setConfigMixerProfileAndWriteEEPROM(uint8_t profileIndex); + void setGyroCalibration(float getGyroZero[XYZ_AXIS_COUNT]); void setGravityCalibration(float getGravity); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index fc9b57dea67..7b227705757 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -81,6 +81,7 @@ #include "telemetry/telemetry.h" +#include "flight/mixer_profile.h" #include "flight/mixer.h" #include "flight/servos.h" #include "flight/pid.h" @@ -618,7 +619,7 @@ void processRx(timeUs_t currentTimeUs) const bool throttleIsLow = throttleStickIsLow(); // When armed and motors aren't spinning, do beeps periodically - if (ARMING_FLAG(ARMED) && feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING_LEGACY)) { + if (ARMING_FLAG(ARMED) && ifMotorstopFeatureEnabled() && !STATE(FIXED_WING_LEGACY)) { static bool armedBeeperOn = false; if (throttleIsLow) { @@ -705,14 +706,14 @@ void processRx(timeUs_t currentTimeUs) #if defined(USE_MAG) if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) { - if (IS_RC_MODE_ACTIVE(BOXHEADFREE)) { + if (IS_RC_MODE_ACTIVE(BOXHEADFREE) && STATE(MULTIROTOR)) { if (!FLIGHT_MODE(HEADFREE_MODE)) { ENABLE_FLIGHT_MODE(HEADFREE_MODE); } } else { DISABLE_FLIGHT_MODE(HEADFREE_MODE); } - if (IS_RC_MODE_ACTIVE(BOXHEADADJ)) { + if (IS_RC_MODE_ACTIVE(BOXHEADADJ) && STATE(MULTIROTOR)) { headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); // acquire new heading } } @@ -726,6 +727,8 @@ void processRx(timeUs_t currentTimeUs) } else { DISABLE_FLIGHT_MODE(MANUAL_MODE); } + }else{ + DISABLE_FLIGHT_MODE(MANUAL_MODE); } /* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered. @@ -745,8 +748,8 @@ void processRx(timeUs_t currentTimeUs) } else if (rcControlsConfig()->airmodeHandlingType == STICK_CENTER) { if (throttleIsLow) { - if (STATE(AIRMODE_ACTIVE) && !failsafeIsActive()) { - if ((rollPitchStatus == CENTERED) || (feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING_LEGACY))) { + if (STATE(AIRMODE_ACTIVE)) { + if ((rollPitchStatus == CENTERED) || (ifMotorstopFeatureEnabled() && !STATE(FIXED_WING_LEGACY))) { ENABLE_STATE(ANTI_WINDUP); } else { @@ -764,7 +767,7 @@ void processRx(timeUs_t currentTimeUs) } else if (rcControlsConfig()->airmodeHandlingType == STICK_CENTER_ONCE) { if (throttleIsLow) { - if (STATE(AIRMODE_ACTIVE) && !failsafeIsActive()) { + if (STATE(AIRMODE_ACTIVE)) { if ((rollPitchStatus == CENTERED) && !STATE(ANTI_WINDUP_DEACTIVATED)) { ENABLE_STATE(ANTI_WINDUP); } @@ -792,7 +795,7 @@ void processRx(timeUs_t currentTimeUs) } } //--------------------------------------------------------- - if (mixerConfig()->platformType == PLATFORM_AIRPLANE) { + if (currentMixerConfig.platformType == PLATFORM_AIRPLANE) { DISABLE_FLIGHT_MODE(HEADFREE_MODE); } @@ -833,13 +836,25 @@ void FAST_CODE taskGyro(timeUs_t currentTimeUs) { #endif } -static float calculateThrottleTiltCompensationFactor(uint8_t throttleTiltCompensationStrength) +static void applyThrottleTiltCompensation(void) { - if (throttleTiltCompensationStrength) { - float tiltCompFactor = 1.0f / constrainf(calculateCosTiltAngle(), 0.6f, 1.0f); // max tilt about 50 deg - return 1.0f + (tiltCompFactor - 1.0f) * (throttleTiltCompensationStrength / 100.f); - } else { - return 1.0f; + if (STATE(MULTIROTOR)) { + int16_t thrTiltCompStrength = 0; + + if (navigationRequiresThrottleTiltCompensation()) { + thrTiltCompStrength = 100; + } + else if (systemConfig()->throttle_tilt_compensation_strength && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { + thrTiltCompStrength = systemConfig()->throttle_tilt_compensation_strength; + } + + if (thrTiltCompStrength) { + const int throttleIdleValue = getThrottleIdleValue(); + float tiltCompFactor = 1.0f / constrainf(calculateCosTiltAngle(), 0.6f, 1.0f); // max tilt about 50 deg + tiltCompFactor = 1.0f + (tiltCompFactor - 1.0f) * (thrTiltCompStrength / 100.f); + + rcCommand[THROTTLE] = setDesiredThrottle(throttleIdleValue + (rcCommand[THROTTLE] - throttleIdleValue) * tiltCompFactor, false); + } } } @@ -891,26 +906,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs) applyWaypointNavigationAndAltitudeHold(); // Apply throttle tilt compensation - if (!STATE(FIXED_WING_LEGACY)) { - int16_t thrTiltCompStrength = 0; - - if (navigationRequiresThrottleTiltCompensation()) { - thrTiltCompStrength = 100; - } - else if (systemConfig()->throttle_tilt_compensation_strength && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { - thrTiltCompStrength = systemConfig()->throttle_tilt_compensation_strength; - } - - if (thrTiltCompStrength) { - rcCommand[THROTTLE] = constrain(getThrottleIdleValue() - + (rcCommand[THROTTLE] - getThrottleIdleValue()) * calculateThrottleTiltCompensationFactor(thrTiltCompStrength), - getThrottleIdleValue(), - motorConfig()->maxthrottle); - } - } - else { - // FIXME: throttle pitch comp for FW - } + applyThrottleTiltCompensation(); #ifdef USE_POWER_LIMITS powerLimiterApply(&rcCommand[THROTTLE]); diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 1b6d2df4715..849cdc2b7c0 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -96,6 +96,7 @@ #include "flight/power_limits.h" #include "flight/rpm_filter.h" #include "flight/servos.h" +#include "flight/ez_tune.h" #include "io/asyncfatfs/asyncfatfs.h" #include "io/beeper.h" @@ -304,9 +305,7 @@ void init(void) // Initialize servo and motor mixers // This needs to be called early to set up platform type correctly and count required motors & servos - servosInit(); - mixerUpdateStateFlags(); - mixerInit(); + mixerConfigInit(); // Some sanity checking if (motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) { @@ -517,6 +516,10 @@ void init(void) owInit(); #endif +#ifdef USE_EZ_TUNE + ezTuneUpdate(); +#endif + if (!sensorsAutodetect()) { // if gyro was not detected due to whatever reason, we give up now. failureMode(FAILURE_MISSING_ACC); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 821e4526337..cdaac609baa 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -74,9 +74,11 @@ #include "flight/failsafe.h" #include "flight/imu.h" +#include "flight/mixer_profile.h" #include "flight/mixer.h" #include "flight/pid.h" #include "flight/servos.h" +#include "flight/ez_tune.h" #include "config/config_eeprom.h" #include "config/feature.h" @@ -696,6 +698,9 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, constrain(pidBank()->pid[i].D, 0, 255)); sbufWriteU8(dst, constrain(pidBank()->pid[i].FF, 0, 255)); } + #ifdef USE_EZ_TUNE + ezTuneUpdate(); + #endif break; case MSP_PIDNAMES: @@ -1465,7 +1470,8 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF case MSP2_INAV_MIXER: sbufWriteU8(dst, mixerConfig()->motorDirectionInverted); - sbufWriteU16(dst, 0); + sbufWriteU8(dst, 0); + sbufWriteU8(dst, mixerConfig()->motorstopOnLow); sbufWriteU8(dst, mixerConfig()->platformType); sbufWriteU8(dst, mixerConfig()->hasFlaps); sbufWriteU16(dst, mixerConfig()->appliedMixerPreset); @@ -1516,6 +1522,18 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF } break; + case MSP2_INAV_OUTPUT_MAPPING_EXT: + for (uint8_t i = 0; i < timerHardwareCount; ++i) + if (!(timerHardware[i].usageFlags & (TIM_USE_PPM | TIM_USE_PWM))) { + #if defined(SITL_BUILD) + sbufWriteU8(dst, i); + #else + sbufWriteU8(dst, timer2id(timerHardware[i].tim)); + #endif + sbufWriteU8(dst, timerHardware[i].usageFlags); + } + break; + case MSP2_INAV_MC_BRAKING: #ifdef USE_MR_BRAKING_MODE sbufWriteU16(dst, navConfig()->mc.braking_speed_threshold); @@ -1568,6 +1586,38 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF break; #endif +#ifdef USE_EZ_TUNE + + case MSP2_INAV_EZ_TUNE: + { + sbufWriteU8(dst, ezTune()->enabled); + sbufWriteU16(dst, ezTune()->filterHz); + sbufWriteU8(dst, ezTune()->axisRatio); + sbufWriteU8(dst, ezTune()->response); + sbufWriteU8(dst, ezTune()->damping); + sbufWriteU8(dst, ezTune()->stability); + sbufWriteU8(dst, ezTune()->aggressiveness); + sbufWriteU8(dst, ezTune()->rate); + sbufWriteU8(dst, ezTune()->expo); + } + break; +#endif + +#ifdef USE_RATE_DYNAMICS + + case MSP2_INAV_RATE_DYNAMICS: + { + sbufWriteU8(dst, currentControlRateProfile->rateDynamics.sensitivityCenter); + sbufWriteU8(dst, currentControlRateProfile->rateDynamics.sensitivityEnd); + sbufWriteU8(dst, currentControlRateProfile->rateDynamics.correctionCenter); + sbufWriteU8(dst, currentControlRateProfile->rateDynamics.correctionEnd); + sbufWriteU8(dst, currentControlRateProfile->rateDynamics.weightCenter); + sbufWriteU8(dst, currentControlRateProfile->rateDynamics.weightEnd); + } + break; + +#endif + default: return false; } @@ -2854,7 +2904,8 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) case MSP2_INAV_SET_MIXER: if (dataSize == 9) { mixerConfigMutable()->motorDirectionInverted = sbufReadU8(src); - sbufReadU16(src); // Was yaw_jump_prevention_limit + sbufReadU8(src); // Was yaw_jump_prevention_limit + mixerConfigMutable()->motorstopOnLow = sbufReadU8(src); mixerConfigMutable()->platformType = sbufReadU8(src); mixerConfigMutable()->hasFlaps = sbufReadU8(src); mixerConfigMutable()->appliedMixerPreset = sbufReadU16(src); @@ -3025,6 +3076,51 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) break; #endif +#ifdef USE_EZ_TUNE + + case MSP2_INAV_EZ_TUNE_SET: + { + if (dataSize == 10) { + ezTuneMutable()->enabled = sbufReadU8(src); + ezTuneMutable()->filterHz = sbufReadU16(src); + ezTuneMutable()->axisRatio = sbufReadU8(src); + ezTuneMutable()->response = sbufReadU8(src); + ezTuneMutable()->damping = sbufReadU8(src); + ezTuneMutable()->stability = sbufReadU8(src); + ezTuneMutable()->aggressiveness = sbufReadU8(src); + ezTuneMutable()->rate = sbufReadU8(src); + ezTuneMutable()->expo = sbufReadU8(src); + + ezTuneUpdate(); + } else { + return MSP_RESULT_ERROR; + } + } + break; + +#endif + +#ifdef USE_RATE_DYNAMICS + + case MSP2_INAV_SET_RATE_DYNAMICS: + + if (dataSize == 6) { + ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.sensitivityCenter = sbufReadU8(src); + ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.sensitivityEnd = sbufReadU8(src); + ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.correctionCenter = sbufReadU8(src); + ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.correctionEnd = sbufReadU8(src); + ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.weightCenter = sbufReadU8(src); + ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.weightEnd = sbufReadU8(src); + + } else { + return MSP_RESULT_ERROR; + } + + break; + +#endif + + default: return MSP_RESULT_ERROR; } @@ -3208,6 +3304,8 @@ static bool mspSettingInfoCommand(sbuf_t *dst, sbuf_t *src) sbufWriteU8(dst, 0); sbufWriteU8(dst, 0); break; + case EZ_TUNE_VALUE: + FALLTHROUGH; case PROFILE_VALUE: FALLTHROUGH; case CONTROL_RATE_VALUE: @@ -3218,6 +3316,10 @@ static bool mspSettingInfoCommand(sbuf_t *dst, sbuf_t *src) sbufWriteU8(dst, getConfigBatteryProfile()); sbufWriteU8(dst, MAX_BATTERY_PROFILE_COUNT); break; + case MIXER_CONFIG_VALUE: + sbufWriteU8(dst, getConfigMixerProfile()); + sbufWriteU8(dst, MAX_MIXER_PROFILE_COUNT); + break; } // If the setting uses a table, send each possible string (null terminated) @@ -3661,7 +3763,43 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu *ret = MSP_RESULT_ACK; break; #endif - +#ifndef SITL_BUILD + case MSP2_INAV_TIMER_OUTPUT_MODE: + if (dataSize == 0) { + for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; ++i) { + sbufWriteU8(dst, i); + sbufWriteU8(dst, timerOverrides(i)->outputMode); + } + *ret = MSP_RESULT_ACK; + } else if(dataSize == 1) { + uint8_t timer = sbufReadU8(src); + if(timer < HARDWARE_TIMER_DEFINITION_COUNT) { + sbufWriteU8(dst, timer); + sbufWriteU8(dst, timerOverrides(timer)->outputMode); + *ret = MSP_RESULT_ACK; + } else { + *ret = MSP_RESULT_ERROR; + } + } else { + *ret = MSP_RESULT_ERROR; + } + break; + case MSP2_INAV_SET_TIMER_OUTPUT_MODE: + if(dataSize == 2) { + uint8_t timer = sbufReadU8(src); + uint8_t outputMode = sbufReadU8(src); + if(timer < HARDWARE_TIMER_DEFINITION_COUNT) { + timerOverridesMutable(timer)->outputMode = outputMode; + *ret = MSP_RESULT_ACK; + } else { + *ret = MSP_RESULT_ERROR; + } + } else { + *ret = MSP_RESULT_ERROR; + } + break; +#endif + default: // Not handled return false; diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 0890b1d6056..5b6b8656d8e 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -30,6 +30,7 @@ #include "fc/fc_msp_box.h" #include "fc/runtime_config.h" #include "flight/mixer.h" +#include "flight/mixer_profile.h" #include "io/osd.h" @@ -99,6 +100,8 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { .boxId = BOXCHANGEMISSION, .boxName = "MISSION CHANGE", .permanentId = 59 }, { .boxId = BOXBEEPERMUTE, .boxName = "BEEPER MUTE", .permanentId = 60 }, { .boxId = BOXMULTIFUNCTION, .boxName = "MULTI FUNCTION", .permanentId = 61 }, + { .boxId = BOXMIXERPROFILE, .boxName = "MIXER PROFILE 2", .permanentId = 62 }, + { .boxId = BOXMIXERTRANSITION, .boxName = "MIXER TRANSITION", .permanentId = 63 }, { .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL, .permanentId = 0xFF } }; @@ -199,7 +202,7 @@ void initActiveBoxIds(void) //Camstab mode is enabled always ADD_ACTIVE_BOX(BOXCAMSTAB); - if (STATE(MULTIROTOR)) { + if (STATE(MULTIROTOR) || platformTypeConfigured(PLATFORM_MULTIROTOR) || platformTypeConfigured(PLATFORM_TRICOPTER)) { if ((sensors(SENSOR_ACC) || sensors(SENSOR_MAG))) { ADD_ACTIVE_BOX(BOXHEADFREE); ADD_ACTIVE_BOX(BOXHEADADJ); @@ -231,6 +234,8 @@ void initActiveBoxIds(void) if (!STATE(ALTITUDE_CONTROL) || (STATE(ALTITUDE_CONTROL) && navReadyAltControl)) { ADD_ACTIVE_BOX(BOXNAVRTH); ADD_ACTIVE_BOX(BOXNAVWP); + ADD_ACTIVE_BOX(BOXNAVCRUISE); + ADD_ACTIVE_BOX(BOXNAVCOURSEHOLD); ADD_ACTIVE_BOX(BOXHOMERESET); ADD_ACTIVE_BOX(BOXGCSNAV); ADD_ACTIVE_BOX(BOXPLANWPMISSION); @@ -239,15 +244,13 @@ void initActiveBoxIds(void) #endif } - if (STATE(AIRPLANE)) { - ADD_ACTIVE_BOX(BOXNAVCRUISE); - ADD_ACTIVE_BOX(BOXNAVCOURSEHOLD); + if (STATE(AIRPLANE) || platformTypeConfigured(PLATFORM_AIRPLANE)) { ADD_ACTIVE_BOX(BOXSOARING); } } #ifdef USE_MR_BRAKING_MODE - if (mixerConfig()->platformType == PLATFORM_MULTIROTOR) { + if (mixerConfig()->platformType == PLATFORM_MULTIROTOR || platformTypeConfigured(PLATFORM_MULTIROTOR)) { ADD_ACTIVE_BOX(BOXBRAKING); } #endif @@ -256,11 +259,12 @@ void initActiveBoxIds(void) ADD_ACTIVE_BOX(BOXNAVALTHOLD); } - if (STATE(AIRPLANE) || STATE(ROVER) || STATE(BOAT)) { + if (STATE(AIRPLANE) || STATE(ROVER) || STATE(BOAT) || + platformTypeConfigured(PLATFORM_AIRPLANE) || platformTypeConfigured(PLATFORM_ROVER) || platformTypeConfigured(PLATFORM_BOAT)) { ADD_ACTIVE_BOX(BOXMANUAL); } - if (STATE(AIRPLANE)) { + if (STATE(AIRPLANE) || platformTypeConfigured(PLATFORM_AIRPLANE)) { if (!feature(FEATURE_FW_LAUNCH)) { ADD_ACTIVE_BOX(BOXNAVLAUNCH); } @@ -350,6 +354,11 @@ void initActiveBoxIds(void) ADD_ACTIVE_BOX(BOXTURTLE); } #endif + +#if (MAX_MIXER_PROFILE_COUNT > 1) + ADD_ACTIVE_BOX(BOXMIXERPROFILE); + ADD_ACTIVE_BOX(BOXMIXERTRANSITION); +#endif } #define IS_ENABLED(mask) ((mask) == 0 ? 0 : 1) @@ -418,6 +427,10 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) #endif #ifdef USE_MULTI_FUNCTIONS CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMULTIFUNCTION)), BOXMULTIFUNCTION); +#endif +#if (MAX_MIXER_PROFILE_COUNT > 1) + CHECK_ACTIVE_BOX(IS_ENABLED(currentMixerProfileIndex), BOXMIXERPROFILE); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION)), BOXMIXERTRANSITION); #endif memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t)); for (uint32_t i = 0; i < activeBoxIdCount; i++) { diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index c396fa8814f..8a03ed81284 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -51,6 +51,7 @@ #include "flight/pid.h" #include "flight/failsafe.h" +#include "flight/mixer.h" #include "io/gps.h" #include "io/beeper.h" @@ -213,7 +214,7 @@ void processRcStickPositions(bool isThrottleLow) // perform actions bool armingSwitchIsActive = IS_RC_MODE_ACTIVE(BOXARM); - if (STATE(AIRPLANE) && feature(FEATURE_MOTOR_STOP) && armingConfig()->fixed_wing_auto_arm) { + if (STATE(AIRPLANE) && ifMotorstopFeatureEnabled() && armingConfig()->fixed_wing_auto_arm) { // Auto arm on throttle when using fixedwing and motorstop if (!isThrottleLow) { tryArm(); diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 674fdded8d2..6b56fa556b1 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -79,6 +79,8 @@ typedef enum { BOXCHANGEMISSION = 50, BOXBEEPERMUTE = 51, BOXMULTIFUNCTION = 52, + BOXMIXERPROFILE = 53, + BOXMIXERTRANSITION = 54, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index 8dc17edc631..109f430f843 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -237,8 +237,12 @@ static uint16_t getValueOffset(const setting_t *value) return value->offset + sizeof(pidProfile_t) * getConfigProfile(); case CONTROL_RATE_VALUE: return value->offset + sizeof(controlRateConfig_t) * getConfigProfile(); + case EZ_TUNE_VALUE: + return value->offset + sizeof(ezTuneSettings_t) * getConfigProfile(); case BATTERY_CONFIG_VALUE: return value->offset + sizeof(batteryProfile_t) * getConfigBatteryProfile(); + case MIXER_CONFIG_VALUE: + return value->offset + sizeof(mixerProfile_t) * getConfigMixerProfile(); } return 0; } diff --git a/src/main/fc/settings.h b/src/main/fc/settings.h index b14289ba27c..75e8bdbef32 100644 --- a/src/main/fc/settings.h +++ b/src/main/fc/settings.h @@ -14,11 +14,11 @@ typedef struct lookupTableEntry_s { } lookupTableEntry_t; #define SETTING_TYPE_OFFSET 0 -#define SETTING_SECTION_OFFSET 4 +#define SETTING_SECTION_OFFSET 3 #define SETTING_MODE_OFFSET 6 typedef enum { - // value type, bits 0-3 + // value type, bits 0-2 VAR_UINT8 = (0 << SETTING_TYPE_OFFSET), VAR_INT8 = (1 << SETTING_TYPE_OFFSET), VAR_UINT16 = (2 << SETTING_TYPE_OFFSET), @@ -29,11 +29,13 @@ typedef enum { } setting_type_e; typedef enum { - // value section, bits 4-5 + // value section, bits 3-5 MASTER_VALUE = (0 << SETTING_SECTION_OFFSET), PROFILE_VALUE = (1 << SETTING_SECTION_OFFSET), - CONTROL_RATE_VALUE = (2 << SETTING_SECTION_OFFSET), // 0x20 + CONTROL_RATE_VALUE = (2 << SETTING_SECTION_OFFSET), BATTERY_CONFIG_VALUE = (3 << SETTING_SECTION_OFFSET), + MIXER_CONFIG_VALUE = (4 << SETTING_SECTION_OFFSET), + EZ_TUNE_VALUE = (5 << SETTING_SECTION_OFFSET) } setting_section_e; typedef enum { @@ -42,8 +44,9 @@ typedef enum { MODE_LOOKUP = (1 << SETTING_MODE_OFFSET), // 0x40 } setting_mode_e; -#define SETTING_TYPE_MASK (0x0F) -#define SETTING_SECTION_MASK (0x30) + +#define SETTING_TYPE_MASK (0x07) +#define SETTING_SECTION_MASK (0x38) #define SETTING_MODE_MASK (0xC0) typedef struct settingMinMaxConfig_s { diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 127d10a8775..87daf753cf2 100755 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -44,10 +44,10 @@ tables: values: ["VELNED", "TURNRATE","ADAPTIVE"] enum: imu_inertia_comp_method_e - name: gps_provider - values: ["NMEA", "UBLOX", "UBLOX7", "MSP", "FAKE"] + values: ["UBLOX", "UBLOX7", "MSP", "FAKE"] enum: gpsProvider_e - name: gps_sbas_mode - values: ["AUTO", "EGNOS", "WAAS", "MSAS", "GAGAN", "NONE"] + values: ["AUTO", "EGNOS", "WAAS", "MSAS", "GAGAN", "SPAN", "NONE"] enum: sbasMode_e - name: gps_dyn_model values: ["PEDESTRIAN", "AIR_1G", "AIR_4G"] @@ -112,9 +112,6 @@ tables: enum: smartportFuelUnit_e - name: platform_type values: ["MULTIROTOR", "AIRPLANE", "HELICOPTER", "TRICOPTER", "ROVER", "BOAT"] - - name: output_mode - values: ["AUTO", "MOTORS", "SERVOS"] - enum: outputMode_e - name: tz_automatic_dst values: ["OFF", "EU", "USA"] enum: tz_automatic_dst_e @@ -588,7 +585,7 @@ groups: members: - name: pitot_hardware description: "Selection of pitot hardware." - default_value: "NONE" + default_value: "VIRTUAL" table: pitot_hardware - name: pitot_lpf_milli_hz min: 0 @@ -1048,7 +1045,7 @@ groups: max: PWM_RANGE_MAX - name: nav_mc_hover_thr description: "Multicopter hover throttle hint for altitude controller. Should be set to approximate throttle value when drone is hovering." - default_value: 1500 + default_value: 1300 field: nav.mc.hover_throttle min: 1000 max: 2000 @@ -1137,36 +1134,56 @@ groups: field: powerLimits.burstPowerFalldownTime max: 3000 - - name: PG_MIXER_CONFIG - type: mixerConfig_t + - name: PG_MIXER_PROFILE + type: mixerProfile_t + headers: ["flight/mixer_profile.h"] + value_type: MIXER_CONFIG_VALUE members: - name: motor_direction_inverted description: "Use if you need to inverse yaw motor direction." default_value: OFF - field: motorDirectionInverted + field: mixer_config.motorDirectionInverted type: bool - name: platform_type description: "Defines UAV platform type. Allowed values: \"MULTIROTOR\", \"AIRPLANE\", \"HELICOPTER\", \"TRICOPTER\", \"ROVER\", \"BOAT\". Currently only MULTIROTOR, AIRPLANE and TRICOPTER types are implemented" default_value: "MULTIROTOR" - field: platformType + field: mixer_config.platformType type: uint8_t table: platform_type - name: has_flaps description: "Defines is UAV is capable of having flaps. If ON and AIRPLANE `platform_type` is used, **FLAPERON** flight mode will be available for the pilot" default_value: OFF - field: hasFlaps + field: mixer_config.hasFlaps type: bool - name: model_preview_type description: "ID of mixer preset applied in a Configurator. **Do not modify manually**. Used only for backup/restore reasons." default_value: -1 - field: appliedMixerPreset + field: mixer_config.appliedMixerPreset min: -1 max: INT16_MAX - - name: output_mode - description: "Output function assignment mode. AUTO assigns outputs according to the default mapping, SERVOS assigns all outputs to servos, MOTORS assigns all outputs to motors" - default_value: "AUTO" - field: outputMode - table: output_mode + - name: motorstop_on_low + description: "If enabled, motor will stop when throttle is low on this mixer_profile" + default_value: OFF + field: mixer_config.motorstopOnLow + type: bool + - name: mixer_pid_profile_linking + description: "If enabled, pid profile_index will follow mixer_profile index. Set to OFF(default) if you want to handle PID profile by your self. Recommend to set to ON on all mixer_profiles to let the mixer_profile handle the PID profile switching on a VTOL or mixed platform type setup." + default_value: OFF + field: mixer_config.PIDProfileLinking + type: bool + - name: mixer_automated_switch + description: "If set to on, This mixer_profile will try to switch to another mixer_profile when 1.RTH heading home is requested and distance to home is lager than 3*nav_fw_loiter_radius on mixer_profile is a MULTIROTOR or TRICOPTER platform_type. 2. RTH landing is requested on this mixer_profile is a AIRPLANE platform_type" + default_value: OFF + field: mixer_config.automated_switch + type: bool + - name: mixer_switch_trans_timer + description: "If switch another mixer_profile is scheduled by mixer_automated_switch or mixer_automated_switch. Activate Mixertransion motor/servo mixing for this many decisecond(0.1s) before the actual mixer_profile switch." + default_value: 0 + field: mixer_config.switchTransitionTimer + min: 0 + max: 200 + + - name: PG_REVERSIBLE_MOTORS_CONFIG type: reversibleMotorsConfig_t @@ -1435,7 +1452,7 @@ groups: type: bool - name: ahrs_inertia_comp_method description: "Inertia force compensation method when gps is avaliable, VELNED use the accleration from gps, TURNRATE calculates accleration by turnrate multiplied by speed, ADAPTIVE choose best result from two in each ahrs loop" - default_value: VELNED + default_value: ADAPTIVE field: inertia_comp_method table: imu_inertia_comp_method @@ -1475,6 +1492,65 @@ groups: min: 0 max: 99 + - name: PG_EZ_TUNE + headers: ["flight/ez_tune.h"] + type: ezTuneSettings_t + value_type: EZ_TUNE_VALUE + members: + - name: ez_enabled + description: "Enables EzTune feature" + default_value: OFF + field: enabled + type: bool + - name: ez_filter_hz + description: "EzTune filter cutoff frequency" + default_value: 110 + field: filterHz + min: 10 + max: 300 + - name: ez_axis_ratio + description: "EzTune axis ratio" + default_value: 110 + field: axisRatio + min: 25 + max: 175 + - name: ez_response + description: "EzTune response" + default_value: 100 + field: response + min: 0 + max: 200 + - name: ez_damping + description: "EzTune damping" + default_value: 100 + field: damping + min: 0 + max: 200 + - name: ez_stability + description: "EzTune stability" + default_value: 100 + field: stability + min: 0 + max: 200 + - name: ez_aggressiveness + description: "EzTune aggressiveness" + default_value: 100 + field: aggressiveness + min: 0 + max: 200 + - name: ez_rate + description: "EzTune rate" + default_value: 100 + field: rate + min: 0 + max: 200 + - name: ez_expo + description: "EzTune expo" + default_value: 100 + field: expo + min: 0 + max: 200 + - name: PG_RPM_FILTER_CONFIG headers: ["flight/rpm_filter.h"] condition: USE_RPM_FILTER @@ -2250,7 +2326,7 @@ groups: max: 10 default_value: 0.2 - name: inav_w_z_gps_v - description: "Weight of GPS climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors. If GPS doesn't support native climb rate reporting (i.e. NMEA GPS) you may consider setting this to zero" + description: "Weight of GPS climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors." field: w_z_gps_v min: 0 max: 10 @@ -2584,6 +2660,12 @@ groups: default_value: ON field: general.flags.mission_planner_reset type: bool + - name: nav_cruise_yaw_rate + description: "Max YAW rate when NAV COURSE HOLD/CRUISE mode is enabled. Set to 0 to disable on fixed wing (Note: On multirotor setting to 0 will disable Course Hold/Cruise mode completely) [dps]" + default_value: 20 + field: general.cruise_yaw_rate + min: 0 + max: 120 - name: nav_mc_bank_angle description: "Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude" default_value: 30 @@ -2819,12 +2901,6 @@ groups: field: fw.launch_abort_deadband min: 2 max: 250 - - name: nav_fw_cruise_yaw_rate - description: "Max YAW rate when NAV CRUISE mode is enabled (0=disable control via yaw stick) [dps]" - default_value: 20 - field: fw.cruise_yaw_rate - min: 0 - max: 60 - name: nav_fw_allow_manual_thr_increase description: "Enable the possibility to manually increase the throttle in auto throttle controlled modes for fixed wing" default_value: OFF @@ -3483,59 +3559,88 @@ groups: max: 6 default_value: 4 + - name: osd_use_pilot_logo + description: Use custom pilot logo with/instead of the INAV logo. The pilot logo must be characters 473 to 511 + field: use_pilot_logo + type: bool + default_value: OFF + + - name: osd_inav_to_pilot_logo_spacing + description: The space between the INAV and pilot logos, if `osd_use_pilot_logo` is `ON`. This number may be adjusted so that it fits the odd/even col width displays. For example, if using an odd column width display, such as Walksnail, and this is set to 4. 1 will be added so that the logos are equally spaced from the centre of the screen. + field: inav_to_pilot_logo_spacing + min: 0 + max: 20 + default_value: 8 + + - name: osd_arm_screen_display_time + description: Amount of time to display the arm screen [ms] + field: arm_screen_display_time + min: 1000 + max: 5000 + default_value: 1500 + - name: osd_switch_indicator_zero_name description: "Character to use for OSD switch incicator 0." field: osd_switch_indicator0_name type: string max: 5 default_value: "FLAP" + - name: osd_switch_indicator_one_name description: "Character to use for OSD switch incicator 1." field: osd_switch_indicator1_name type: string max: 5 default_value: "GEAR" + - name: osd_switch_indicator_two_name description: "Character to use for OSD switch incicator 2." field: osd_switch_indicator2_name type: string max: 5 default_value: "CAM" + - name: osd_switch_indicator_three_name description: "Character to use for OSD switch incicator 3." field: osd_switch_indicator3_name type: string max: 5 default_value: "LIGT" + - name: osd_switch_indicator_zero_channel description: "RC Channel to use for OSD switch indicator 0." field: osd_switch_indicator0_channel min: 5 max: MAX_SUPPORTED_RC_CHANNEL_COUNT default_value: 5 + - name: osd_switch_indicator_one_channel description: "RC Channel to use for OSD switch indicator 1." field: osd_switch_indicator1_channel min: 5 max: MAX_SUPPORTED_RC_CHANNEL_COUNT default_value: 5 + - name: osd_switch_indicator_two_channel description: "RC Channel to use for OSD switch indicator 2." field: osd_switch_indicator2_channel min: 5 max: MAX_SUPPORTED_RC_CHANNEL_COUNT default_value: 5 + - name: osd_switch_indicator_three_channel description: "RC Channel to use for OSD switch indicator 3." field: osd_switch_indicator3_channel min: 5 max: MAX_SUPPORTED_RC_CHANNEL_COUNT default_value: 5 + - name: osd_switch_indicators_align_left description: "Align text to left of switch indicators" field: osd_switch_indicators_align_left type: bool default_value: ON + - name: osd_system_msg_display_time description: System message display cycle time for multiple messages (milliseconds). field: system_msg_display_time diff --git a/src/main/fc/stats.h b/src/main/fc/stats.h index 275460643c3..54b676f6577 100644 --- a/src/main/fc/stats.h +++ b/src/main/fc/stats.h @@ -3,8 +3,8 @@ #ifdef USE_STATS typedef struct statsConfig_s { - uint32_t stats_total_time; // [s] - uint32_t stats_total_dist; // [m] + uint32_t stats_total_time; // [Seconds] + uint32_t stats_total_dist; // [Metres] #ifdef USE_ADC uint32_t stats_total_energy; // deciWatt hour (x0.1Wh) #endif diff --git a/src/main/flight/ez_tune.c b/src/main/flight/ez_tune.c new file mode 100644 index 00000000000..c6c57afbfe5 --- /dev/null +++ b/src/main/flight/ez_tune.c @@ -0,0 +1,143 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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 file 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 program. If not, see http://www.gnu.org/licenses/. + */ + +#include "fc/config.h" +#include "config/config_reset.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + +#include "flight/ez_tune.h" + +#include "fc/settings.h" +#include "flight/pid.h" +#include "sensors/gyro.h" +#include "fc/controlrate_profile.h" + +PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(ezTuneSettings_t, ezTune, PG_EZ_TUNE, 0); + +PG_RESET_TEMPLATE(ezTuneSettings_t, ezTune, + .enabled = SETTING_EZ_ENABLED_DEFAULT, + .filterHz = SETTING_EZ_FILTER_HZ_DEFAULT, + .axisRatio = SETTING_EZ_AXIS_RATIO_DEFAULT, + .response = SETTING_EZ_RESPONSE_DEFAULT, + .damping = SETTING_EZ_DAMPING_DEFAULT, + .stability = SETTING_EZ_STABILITY_DEFAULT, + .aggressiveness = SETTING_EZ_AGGRESSIVENESS_DEFAULT, + .rate = SETTING_EZ_RATE_DEFAULT, + .expo = SETTING_EZ_EXPO_DEFAULT, +); + +#define EZ_TUNE_PID_RP_DEFAULT { 40, 75, 23, 100 } +#define EZ_TUNE_PID_YAW_DEFAULT { 45, 80, 0, 100 } + +#define EZ_TUNE_YAW_SCALE 0.5f + +static float computePt1FilterDelayMs(uint8_t filterHz) { + return 1000.0f / (2.0f * M_PIf * filterHz); +} + +static float getYawPidScale(float input) { + const float normalized = (input - 100) * 0.01f; + + return 1.0f + (normalized * 0.5f); +} + +/** + * Update INAV settings based on current EZTune settings + * This has to be called every time control profile is changed, or EZTune settings are changed + */ +void ezTuneUpdate(void) { + if (ezTune()->enabled) { + + // Setup filtering + //Set Dterm LPF + pidProfileMutable()->dterm_lpf_hz = MAX(ezTune()->filterHz - 5, 50); + pidProfileMutable()->dterm_lpf_type = FILTER_PT2; + + //Set main gyro filter + gyroConfigMutable()->gyro_main_lpf_hz = ezTune()->filterHz; + gyroConfigMutable()->gyro_main_lpf_type = FILTER_PT1; + + //Set anti-aliasing filter + gyroConfigMutable()->gyro_anti_aliasing_lpf_hz = SETTING_GYRO_ANTI_ALIASING_LPF_HZ_DEFAULT; + gyroConfigMutable()->gyro_anti_aliasing_lpf_type = FILTER_PT1; + + //Enable Smith predictor + pidProfileMutable()->smithPredictorDelay = computePt1FilterDelayMs(ezTune()->filterHz); + +#ifdef USE_DYNAMIC_FILTERS + //Enable dynamic notch + gyroConfigMutable()->dynamicGyroNotchEnabled = 1; + gyroConfigMutable()->dynamicGyroNotchQ = 250; + gyroConfigMutable()->dynamicGyroNotchMinHz = MAX(ezTune()->filterHz * 0.667f, SETTING_DYNAMIC_GYRO_NOTCH_MIN_HZ_DEFAULT); + gyroConfigMutable()->dynamicGyroNotchMode = DYNAMIC_NOTCH_MODE_3D; +#endif + +#ifdef USE_GYRO_KALMAN + //Make sure Kalman filter is enabled + gyroConfigMutable()->kalmanEnabled = 1; + if (ezTune()->filterHz < 150) { + gyroConfigMutable()->kalman_q = 200; + } else { + gyroConfigMutable()->kalman_q = scaleRangef(ezTune()->filterHz, 150, 300, 200, 400); + } +#endif + + //Disable dynamic LPF + gyroConfigMutable()->useDynamicLpf = 0; + + //Setup PID controller + + const uint8_t pidDefaults[4] = EZ_TUNE_PID_RP_DEFAULT; + const uint8_t pidDefaultsYaw[4] = EZ_TUNE_PID_YAW_DEFAULT; + const float pitchRatio = ezTune()->axisRatio / 100.0f; + + //Roll + pidProfileMutable()->bank_mc.pid[PID_ROLL].P = pidDefaults[0] * ezTune()->response / 100.0f; + pidProfileMutable()->bank_mc.pid[PID_ROLL].I = pidDefaults[1] * ezTune()->stability / 100.0f; + pidProfileMutable()->bank_mc.pid[PID_ROLL].D = pidDefaults[2] * ezTune()->damping / 100.0f; + pidProfileMutable()->bank_mc.pid[PID_ROLL].FF = pidDefaults[3] * ezTune()->aggressiveness / 100.0f; + + //Pitch + pidProfileMutable()->bank_mc.pid[PID_PITCH].P = pidDefaults[0] * ezTune()->response / 100.0f * pitchRatio; + pidProfileMutable()->bank_mc.pid[PID_PITCH].I = pidDefaults[1] * ezTune()->stability / 100.0f * pitchRatio; + pidProfileMutable()->bank_mc.pid[PID_PITCH].D = pidDefaults[2] * ezTune()->damping / 100.0f * pitchRatio; + pidProfileMutable()->bank_mc.pid[PID_PITCH].FF = pidDefaults[3] * ezTune()->aggressiveness / 100.0f * pitchRatio; + + //Yaw + pidProfileMutable()->bank_mc.pid[PID_YAW].P = pidDefaultsYaw[0] * getYawPidScale(ezTune()->response); + pidProfileMutable()->bank_mc.pid[PID_YAW].I = pidDefaultsYaw[1] * getYawPidScale(ezTune()->stability); + pidProfileMutable()->bank_mc.pid[PID_YAW].D = pidDefaultsYaw[2] * getYawPidScale(ezTune()->damping); + pidProfileMutable()->bank_mc.pid[PID_YAW].FF = pidDefaultsYaw[3] * getYawPidScale(ezTune()->aggressiveness); + + //Setup rates + ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[FD_ROLL] = scaleRange(ezTune()->rate, 0, 200, 30, 90); + ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[FD_PITCH] = scaleRange(ezTune()->rate, 0, 200, 30, 90); + ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[FD_YAW] = scaleRange(ezTune()->rate, 0, 200, 30, 90) - 10; + + ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rcExpo8 = scaleRange(ezTune()->rate, 0, 200, 40, 100); + ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rcYawExpo8 = scaleRange(ezTune()->rate, 0, 200, 40, 100); + + } +} \ No newline at end of file diff --git a/src/main/flight/ez_tune.h b/src/main/flight/ez_tune.h new file mode 100644 index 00000000000..5fcc1ef6f7f --- /dev/null +++ b/src/main/flight/ez_tune.h @@ -0,0 +1,43 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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 file 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 program. If not, see http://www.gnu.org/licenses/. + */ + +#pragma once + +#include "config/parameter_group.h" + +typedef struct ezTuneSettings_s { + uint8_t enabled; + uint16_t filterHz; + uint8_t axisRatio; + uint8_t response; + uint8_t damping; + uint8_t stability; + uint8_t aggressiveness; + uint8_t rate; + uint8_t expo; +} ezTuneSettings_t; + +PG_DECLARE_PROFILE(ezTuneSettings_t, ezTune); + +void ezTuneUpdate(void); \ No newline at end of file diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 2e179f67bb1..36f4d86f8e2 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -146,6 +146,10 @@ static const failsafeProcedureLogic_t failsafeProcedureLogic[] = { */ void failsafeReset(void) { + if (failsafeState.active) { // can't reset if still active + return; + } + failsafeState.rxDataFailurePeriod = PERIOD_RXDATA_FAILURE + failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND; failsafeState.rxDataRecoveryPeriod = PERIOD_RXDATA_RECOVERY + failsafeConfig()->failsafe_recovery_delay * MILLIS_PER_TENTH_SECOND; failsafeState.validRxDataReceivedAt = 0; @@ -157,6 +161,7 @@ void failsafeReset(void) failsafeState.phase = FAILSAFE_IDLE; failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN; failsafeState.activeProcedure = failsafeConfig()->failsafe_procedure; + failsafeState.controlling = false; failsafeState.lastGoodRcCommand[ROLL] = 0; failsafeState.lastGoodRcCommand[PITCH] = 0; @@ -211,14 +216,6 @@ bool failsafeRequiresAngleMode(void) failsafeProcedureLogic[failsafeState.activeProcedure].forceAngleMode; } -bool failsafeRequiresMotorStop(void) -{ - return failsafeState.active && - failsafeState.activeProcedure == FAILSAFE_PROCEDURE_AUTO_LANDING && - posControl.flags.estAltStatus < EST_USABLE && - currentBatteryProfile->failsafe_throttle < getThrottleIdleValue(); -} - void failsafeStartMonitoring(void) { failsafeState.monitoring = true; diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index 7cd88acd591..c3bc2953a1e 100644 --- a/src/main/flight/failsafe.h +++ b/src/main/flight/failsafe.h @@ -170,7 +170,6 @@ void failsafeOnRxResume(void); bool failsafeMayRequireNavigationMode(void); void failsafeApplyControlInput(void); bool failsafeRequiresAngleMode(void); -bool failsafeRequiresMotorStop(void); bool failsafeShouldApplyControlInput(void); bool failsafeBypassNavigation(void); void failsafeUpdateRcCommandValues(void); diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index c9d0f0aacbf..ed5cbcbfaa4 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -719,28 +719,25 @@ static void imuCalculateEstimatedAttitude(float dT) } if (imuConfig()->inertia_comp_method == COMPMETHOD_ADAPTIVE && isGPSTrustworthy() && STATE(AIRPLANE)) { + //pick the best centrifugal acceleration between velned and turnrate fpVector3_t compansatedGravityBF_velned; vectorAdd(&compansatedGravityBF_velned, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_velned); - float velned_magnitude = fabsf(fast_fsqrtf(vectorNormSquared(&compansatedGravityBF_velned)) - GRAVITY_CMSS); + float velned_error = fabsf(fast_fsqrtf(vectorNormSquared(&compansatedGravityBF_velned)) - GRAVITY_CMSS); fpVector3_t compansatedGravityBF_turnrate; vectorAdd(&compansatedGravityBF_turnrate, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_turnrate); - float turnrate_magnitude = fabsf(fast_fsqrtf(vectorNormSquared(&compansatedGravityBF_turnrate)) - GRAVITY_CMSS); - if (velned_magnitude > turnrate_magnitude) - { - compansatedGravityBF = compansatedGravityBF_turnrate; - } - else - { - compansatedGravityBF = compansatedGravityBF_velned; - } + float turnrate_error = fabsf(fast_fsqrtf(vectorNormSquared(&compansatedGravityBF_turnrate)) - GRAVITY_CMSS); + + compansatedGravityBF = velned_error > turnrate_error? compansatedGravityBF_turnrate:compansatedGravityBF_velned; } - else if (imuConfig()->inertia_comp_method == COMPMETHOD_VELNED && isGPSTrustworthy()) + else if (((imuConfig()->inertia_comp_method == COMPMETHOD_VELNED) || (imuConfig()->inertia_comp_method == COMPMETHOD_ADAPTIVE)) && isGPSTrustworthy()) { + //velned centrifugal force compensation, quad will use this method vectorAdd(&compansatedGravityBF, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_velned); } else if (STATE(AIRPLANE)) { + //turnrate centrifugal force compensation vectorAdd(&compansatedGravityBF, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_turnrate); } else diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 765c2f1637d..35674c1f5dc 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -28,9 +28,11 @@ #include "common/maths.h" #include "common/utils.h" +#include "config/config_reset.h" #include "config/feature.h" #include "config/parameter_group.h" #include "config/parameter_group_ids.h" +#include "config/config_reset.h" #include "drivers/pwm_output.h" #include "drivers/pwm_mapping.h" @@ -81,17 +83,7 @@ PG_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig, .neutral = SETTING_3D_NEUTRAL_DEFAULT ); -PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 5); - -PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig, - .motorDirectionInverted = SETTING_MOTOR_DIRECTION_INVERTED_DEFAULT, - .platformType = SETTING_PLATFORM_TYPE_DEFAULT, - .hasFlaps = SETTING_HAS_FLAPS_DEFAULT, - .appliedMixerPreset = SETTING_MODEL_PREVIEW_TYPE_DEFAULT, //This flag is not available in CLI and used by Configurator only - .outputMode = SETTING_OUTPUT_MODE_DEFAULT, -); - -PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 9); +PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 10); PG_RESET_TEMPLATE(motorConfig_t, motorConfig, .motorPwmProtocol = SETTING_MOTOR_PWM_PROTOCOL_DEFAULT, @@ -100,11 +92,17 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig, .mincommand = SETTING_MIN_COMMAND_DEFAULT, .motorPoleCount = SETTING_MOTOR_POLES_DEFAULT, // Most brushless motors that we use are 14 poles ); - -PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTOR_MIXER, 0); +PG_REGISTER_ARRAY_WITH_RESET_FN(timerOverride_t, HARDWARE_TIMER_DEFINITION_COUNT, timerOverrides, PG_TIMER_OVERRIDE_CONFIG, 0); #define CRASH_OVER_AFTER_CRASH_FLIP_STICK_MIN 0.15f +void pgResetFn_timerOverrides(timerOverride_t *instance) +{ + for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; ++i) { + RESET_CONFIG(timerOverride_t, &instance[i], .outputMode = OUTPUT_MODE_AUTO); + } +} + int getThrottleIdleValue(void) { if (!throttleIdleValue) { @@ -116,14 +114,29 @@ int getThrottleIdleValue(void) static void computeMotorCount(void) { + static bool firstRun = true; + if (!firstRun) { + return; + } motorCount = 0; for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { + bool isMotorUsed = false; + for(int j = 0; j< MAX_MIXER_PROFILE_COUNT; j++){ + if (mixerMotorMixersByIndex(j)[i].throttle != 0.0f) { + isMotorUsed = true; + } + } // check if done - if (primaryMotorMixer(i)->throttle == 0.0f) { + if (!isMotorUsed) { break; } motorCount++; } + firstRun = false; +} + +bool ifMotorstopFeatureEnabled(void){ + return currentMixerConfig.motorstopOnLow; } uint8_t getMotorCount(void) { @@ -149,35 +162,44 @@ void mixerUpdateStateFlags(void) DISABLE_STATE(AIRPLANE); DISABLE_STATE(MOVE_FORWARD_ONLY); - if (mixerConfig()->platformType == PLATFORM_AIRPLANE) { + if (currentMixerConfig.platformType == PLATFORM_AIRPLANE) { ENABLE_STATE(FIXED_WING_LEGACY); ENABLE_STATE(AIRPLANE); ENABLE_STATE(ALTITUDE_CONTROL); ENABLE_STATE(MOVE_FORWARD_ONLY); - } if (mixerConfig()->platformType == PLATFORM_ROVER) { + } if (currentMixerConfig.platformType == PLATFORM_ROVER) { ENABLE_STATE(ROVER); ENABLE_STATE(FIXED_WING_LEGACY); ENABLE_STATE(MOVE_FORWARD_ONLY); - } if (mixerConfig()->platformType == PLATFORM_BOAT) { + } if (currentMixerConfig.platformType == PLATFORM_BOAT) { ENABLE_STATE(BOAT); ENABLE_STATE(FIXED_WING_LEGACY); ENABLE_STATE(MOVE_FORWARD_ONLY); - } else if (mixerConfig()->platformType == PLATFORM_MULTIROTOR) { + } else if (currentMixerConfig.platformType == PLATFORM_MULTIROTOR) { ENABLE_STATE(MULTIROTOR); ENABLE_STATE(ALTITUDE_CONTROL); - } else if (mixerConfig()->platformType == PLATFORM_TRICOPTER) { + } else if (currentMixerConfig.platformType == PLATFORM_TRICOPTER) { ENABLE_STATE(MULTIROTOR); ENABLE_STATE(ALTITUDE_CONTROL); - } else if (mixerConfig()->platformType == PLATFORM_HELICOPTER) { + } else if (currentMixerConfig.platformType == PLATFORM_HELICOPTER) { ENABLE_STATE(MULTIROTOR); ENABLE_STATE(ALTITUDE_CONTROL); } - if (mixerConfig()->hasFlaps) { + if (currentMixerConfig.hasFlaps) { ENABLE_STATE(FLAPERON_AVAILABLE); } else { DISABLE_STATE(FLAPERON_AVAILABLE); } + if ( + currentMixerConfig.platformType == PLATFORM_BOAT || + currentMixerConfig.platformType == PLATFORM_ROVER || + navConfig()->fw.useFwNavYawControl + ) { + ENABLE_STATE(FW_HEADING_USE_YAW); + } else { + DISABLE_STATE(FW_HEADING_USE_YAW); + } } void nullMotorRateLimiting(const float dT) @@ -199,7 +221,7 @@ void mixerInit(void) mixerResetDisarmedMotors(); - if (mixerConfig()->motorDirectionInverted) { + if (currentMixerConfig.motorDirectionInverted) { motorYawMultiplier = -1; } else { motorYawMultiplier = 1; @@ -208,6 +230,7 @@ void mixerInit(void) void mixerResetDisarmedMotors(void) { + getThrottleIdleValue(); if (feature(FEATURE_REVERSIBLE_MOTORS)) { motorZeroCommand = reversibleMotorsConfig()->neutral; @@ -215,16 +238,16 @@ void mixerResetDisarmedMotors(void) throttleRangeMax = motorConfig()->maxthrottle; } else { motorZeroCommand = motorConfig()->mincommand; - throttleRangeMin = getThrottleIdleValue(); + throttleRangeMin = throttleIdleValue; throttleRangeMax = motorConfig()->maxthrottle; } reversibleMotorsThrottleState = MOTOR_DIRECTION_FORWARD; - if (feature(FEATURE_MOTOR_STOP)) { + if (ifMotorstopFeatureEnabled()) { motorValueWhenStopped = motorZeroCommand; } else { - motorValueWhenStopped = getThrottleIdleValue(); + motorValueWhenStopped = throttleIdleValue; } // set disarmed motor values @@ -279,7 +302,7 @@ static void applyTurtleModeToMotors(void) { float signPitch = rcCommand[PITCH] < 0 ? 1 : -1; float signRoll = rcCommand[ROLL] < 0 ? 1 : -1; - float signYaw = (float)((rcCommand[YAW] < 0 ? 1 : -1) * (mixerConfig()->motorDirectionInverted ? 1 : -1)); + float signYaw = (float)((rcCommand[YAW] < 0 ? 1 : -1) * (currentMixerConfig.motorDirectionInverted ? 1 : -1)); float stickDeflectionLength = calc_length_pythagorean_2D(stickDeflectionPitchAbs, stickDeflectionRollAbs); float stickDeflectionExpoLength = calc_length_pythagorean_2D(stickDeflectionPitchExpo, stickDeflectionRollExpo); @@ -458,7 +481,7 @@ static int getReversibleMotorsThrottleDeadband(void) directionValue = reversibleMotorsConfig()->deadband_high; } - return feature(FEATURE_MOTOR_STOP) ? reversibleMotorsConfig()->neutral : directionValue; + return ifMotorstopFeatureEnabled() ? reversibleMotorsConfig()->neutral : directionValue; } void FAST_CODE mixTable(void) @@ -469,6 +492,19 @@ void FAST_CODE mixTable(void) return; } #endif +#ifdef USE_DEV_TOOLS + bool isDisarmed = !ARMING_FLAG(ARMED) || systemConfig()->groundTestMode; +#else + bool isDisarmed = !ARMING_FLAG(ARMED); +#endif + bool motorStopIsActive = getMotorStatus() != MOTOR_RUNNING && !isDisarmed; + if (isDisarmed || motorStopIsActive) { + for (int i = 0; i < motorCount; i++) { + motor[i] = isDisarmed ? motor_disarmed[i] : motorValueWhenStopped; + } + mixerThrottleCommand = motor[0]; + return; + } int16_t input[3]; // RPY, range [-500:+500] // Allow direct stick input to motors in passthrough mode on airplanes @@ -551,11 +587,11 @@ void FAST_CODE mixTable(void) throttleRangeMax = motorConfig()->maxthrottle; // Throttle scaling to limit max throttle when battery is full - #ifdef USE_PROGRAMMING_FRAMEWORK +#ifdef USE_PROGRAMMING_FRAMEWORK mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * getThrottleScale(currentBatteryProfile->motor.throttleScale)) + throttleRangeMin; - #else +#else mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * currentBatteryProfile->motor.throttleScale) + throttleRangeMin; - #endif +#endif // Throttle compensation based on battery voltage if (feature(FEATURE_THR_VBAT_COMP) && isAmperageConfigured() && feature(FEATURE_VBAT)) { mixerThrottleCommand = MIN(throttleRangeMin + (mixerThrottleCommand - throttleRangeMin) * calculateThrottleCompensationFactor(), throttleRangeMax); @@ -564,7 +600,6 @@ void FAST_CODE mixTable(void) throttleMin = throttleRangeMin; throttleMax = throttleRangeMax; - throttleRange = throttleMax - throttleMin; #define THROTTLE_CLIPPING_FACTOR 0.33f @@ -584,54 +619,53 @@ void FAST_CODE mixTable(void) // Now add in the desired throttle, but keep in a range that doesn't clip adjusted // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips. - if (ARMING_FLAG(ARMED)) { - const motorStatus_e currentMotorStatus = getMotorStatus(); - for (int i = 0; i < motorCount; i++) { - motor[i] = rpyMix[i] + constrain(mixerThrottleCommand * currentMixer[i].throttle, throttleMin, throttleMax); - - if (failsafeIsActive()) { - motor[i] = constrain(motor[i], motorConfig()->mincommand, motorConfig()->maxthrottle); - } else { - motor[i] = constrain(motor[i], throttleRangeMin, throttleRangeMax); - } + for (int i = 0; i < motorCount; i++) { + motor[i] = rpyMix[i] + constrain(mixerThrottleCommand * currentMixer[i].throttle, throttleMin, throttleMax); - // Motor stop handling - if (currentMotorStatus != MOTOR_RUNNING) { - motor[i] = motorValueWhenStopped; - } -#ifdef USE_DEV_TOOLS - if (systemConfig()->groundTestMode) { - motor[i] = motorZeroCommand; - } -#endif + if (failsafeIsActive()) { + motor[i] = constrain(motor[i], motorConfig()->mincommand, motorConfig()->maxthrottle); + } else { + motor[i] = constrain(motor[i], throttleRangeMin, throttleRangeMax); } - } else { - for (int i = 0; i < motorCount; i++) { - motor[i] = motor_disarmed[i]; + + //stop motors + if (currentMixer[i].throttle <= 0.0f) { + motor[i] = motorZeroCommand; + } + //spin stopped motors only in mixer transition mode + if (isMixerTransitionMixing && currentMixer[i].throttle <= -1.05f && currentMixer[i].throttle >= -2.0f && (!feature(FEATURE_REVERSIBLE_MOTORS))) { + motor[i] = -currentMixer[i].throttle * 1000; + motor[i] = constrain(motor[i], throttleRangeMin, throttleRangeMax); } } } int16_t getThrottlePercent(bool useScaled) { - int16_t thr = constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX); - const int idleThrottle = getThrottleIdleValue(); - + int16_t thr = constrain(mixerThrottleCommand, PWM_RANGE_MIN, PWM_RANGE_MAX); + if (useScaled) { - thr = (thr - idleThrottle) * 100 / (motorConfig()->maxthrottle - idleThrottle); + thr = (thr - throttleIdleValue) * 100 / (motorConfig()->maxthrottle - throttleIdleValue); } else { thr = (rxGetChannelValue(THROTTLE) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN); } return thr; } -motorStatus_e getMotorStatus(void) +uint16_t setDesiredThrottle(uint16_t throttle, bool allowMotorStop) { - if (failsafeRequiresMotorStop()) { - return MOTOR_STOPPED_AUTO; + const uint16_t throttleIdleValue = getThrottleIdleValue(); + + if (allowMotorStop && throttle < throttleIdleValue) { + ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); + return throttle; } + return constrain(throttle, throttleIdleValue, motorConfig()->maxthrottle); +} - if (!failsafeIsActive() && STATE(NAV_MOTOR_STOP_OR_IDLE)) { +motorStatus_e getMotorStatus(void) +{ + if (STATE(NAV_MOTOR_STOP_OR_IDLE)) { return MOTOR_STOPPED_AUTO; } diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index f638d7335fd..9ee6a20654e 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -19,6 +19,8 @@ #include "config/parameter_group.h" +#include "drivers/timer.h" + #if defined(TARGET_MOTOR_COUNT) #define MAX_SUPPORTED_MOTORS TARGET_MOTOR_COUNT #else @@ -62,17 +64,12 @@ typedef struct motorMixer_s { float yaw; } motorMixer_t; -PG_DECLARE_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer); -typedef struct mixerConfig_s { - int8_t motorDirectionInverted; - uint8_t platformType; - bool hasFlaps; - int16_t appliedMixerPreset; +typedef struct timerOverride_s { uint8_t outputMode; -} mixerConfig_t; +} timerOverride_t; -PG_DECLARE(mixerConfig_t, mixerConfig); +PG_DECLARE_ARRAY(timerOverride_t, HARDWARE_TIMER_DEFINITION_COUNT, timerOverrides); typedef struct reversibleMotorsConfig_s { uint16_t deadband_low; // min 3d value @@ -110,8 +107,10 @@ extern int16_t motor[MAX_SUPPORTED_MOTORS]; extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; extern int mixerThrottleCommand; +bool ifMotorstopFeatureEnabled(void); int getThrottleIdleValue(void); int16_t getThrottlePercent(bool); +uint16_t setDesiredThrottle(uint16_t throttle, bool allowMotorStop); uint8_t getMotorCount(void); float getMotorMixRange(void); bool mixerIsOutputSaturated(void); @@ -127,7 +126,8 @@ void processServoAutotrim(const float dT); void processServoAutotrimMode(void); void processContinuousServoAutotrim(const float dT); void stopMotors(void); +void stopMotorsNoDelay(void); void stopPwmAllMotors(void); void loadPrimaryMotorMixer(void); -bool areMotorsRunning(void); +bool areMotorsRunning(void); \ No newline at end of file diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c new file mode 100644 index 00000000000..6f2b6b0920b --- /dev/null +++ b/src/main/flight/mixer_profile.c @@ -0,0 +1,253 @@ +#include +#include +#include + +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" +#include "config/config_reset.h" + +#include "drivers/pwm_output.h" +#include "drivers/pwm_mapping.h" +#include "drivers/time.h" +#include "flight/mixer.h" +#include "common/axis.h" +#include "flight/pid.h" +#include "flight/servos.h" +#include "flight/failsafe.h" +#include "navigation/navigation.h" +#include "navigation/navigation_private.h" + +#include "fc/fc_core.h" +#include "fc/config.h" +#include "fc/runtime_config.h" +#include "fc/settings.h" +#include "fc/rc_modes.h" +#include "fc/cli.h" + +#include "programming/logic_condition.h" +#include "navigation/navigation.h" + +#include "common/log.h" + +mixerConfig_t currentMixerConfig; +int currentMixerProfileIndex; +bool isMixerTransitionMixing; +bool isMixerTransitionMixing_requested; +mixerProfileAT_t mixerProfileAT; +int nextProfileIndex; + +PG_REGISTER_ARRAY_WITH_RESET_FN(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 1); + +void pgResetFn_mixerProfiles(mixerProfile_t *instance) +{ + for (int i = 0; i < MAX_MIXER_PROFILE_COUNT; i++) + { + RESET_CONFIG(mixerProfile_t, &instance[i], + .mixer_config = { + .motorDirectionInverted = SETTING_MOTOR_DIRECTION_INVERTED_DEFAULT, + .platformType = SETTING_PLATFORM_TYPE_DEFAULT, + .hasFlaps = SETTING_HAS_FLAPS_DEFAULT, + .appliedMixerPreset = SETTING_MODEL_PREVIEW_TYPE_DEFAULT, // This flag is not available in CLI and used by Configurator only + .motorstopOnLow = SETTING_MOTORSTOP_ON_LOW_DEFAULT, + .PIDProfileLinking = SETTING_MIXER_PID_PROFILE_LINKING_DEFAULT, + .automated_switch = SETTING_MIXER_AUTOMATED_SWITCH_DEFAULT, + .switchTransitionTimer = SETTING_MIXER_SWITCH_TRANS_TIMER_DEFAULT, + }); + for (int j = 0; j < MAX_SUPPORTED_MOTORS; j++) + { + RESET_CONFIG(motorMixer_t, &instance[i].MotorMixers[j], + .throttle = 0, + .roll = 0, + .pitch = 0, + .yaw = 0); + } + for (int j = 0; j < MAX_SERVO_RULES; j++) + { + RESET_CONFIG(servoMixer_t, &instance[i].ServoMixers[j], + .targetChannel = 0, + .inputSource = 0, + .rate = 0, + .speed = 0 +#ifdef USE_PROGRAMMING_FRAMEWORK + , + .conditionId = -1, +#endif + ); + } + } +} + +void activateMixerConfig(){ + currentMixerProfileIndex = getConfigMixerProfile(); + currentMixerConfig = *mixerConfig(); + nextProfileIndex = (currentMixerProfileIndex + 1) % MAX_MIXER_PROFILE_COUNT; +} + +void mixerConfigInit(void) +{ + activateMixerConfig(); + servosInit(); + mixerUpdateStateFlags(); + mixerInit(); + if (currentMixerConfig.PIDProfileLinking) + { + // LOG_INFO(PWM, "mixer switch pidInit"); + setConfigProfile(getConfigMixerProfile()); + pidInit(); + pidInitFilters(); + pidResetErrorAccumulators(); //should be safer to reset error accumulators + schedulePidGainsUpdate(); + navigationUsePIDs(); // set navigation pid gains + } +} + +void setMixerProfileAT(void) +{ + mixerProfileAT.transitionStartTime = millis(); + mixerProfileAT.transitionTransEndTime = mixerProfileAT.transitionStartTime + (timeMs_t)currentMixerConfig.switchTransitionTimer * 100; +} + +bool platformTypeConfigured(flyingPlatformType_e platformType) +{ + if (!isModeActivationConditionPresent(BOXMIXERPROFILE)){ + return false; + } + return mixerConfigByIndex(nextProfileIndex)->platformType == platformType; +} + +bool checkMixerATRequired(mixerProfileATRequest_e required_action) +{ + //return false if mixerAT condition is not required or setting is not valid + if ((!STATE(AIRPLANE)) && (!STATE(MULTIROTOR))) + { + return false; + } + if (!isModeActivationConditionPresent(BOXMIXERPROFILE)) + { + return false; + } + + if(currentMixerConfig.automated_switch){ + if ((required_action == MIXERAT_REQUEST_RTH) && STATE(MULTIROTOR)) + { + return true; + } + if ((required_action == MIXERAT_REQUEST_LAND) && STATE(AIRPLANE)) + { + return true; + } + } + return false; +} + +bool mixerATUpdateState(mixerProfileATRequest_e required_action) +{ + //return true if mixerAT is done or not required + bool reprocessState; + do + { + reprocessState=false; + if (required_action==MIXERAT_REQUEST_ABORT){ + isMixerTransitionMixing_requested = false; + mixerProfileAT.phase = MIXERAT_PHASE_IDLE; + return true; + } + switch (mixerProfileAT.phase){ + case MIXERAT_PHASE_IDLE: + //check if mixerAT is required + if (checkMixerATRequired(required_action)){ + mixerProfileAT.phase=MIXERAT_PHASE_TRANSITION_INITIALIZE; + reprocessState = true; + } + break; + case MIXERAT_PHASE_TRANSITION_INITIALIZE: + // LOG_INFO(PWM, "MIXERAT_PHASE_IDLE"); + setMixerProfileAT(); + mixerProfileAT.phase = MIXERAT_PHASE_TRANSITIONING; + reprocessState = true; + break; + case MIXERAT_PHASE_TRANSITIONING: + isMixerTransitionMixing_requested = true; + if (millis() > mixerProfileAT.transitionTransEndTime){ + isMixerTransitionMixing_requested = false; + outputProfileHotSwitch(nextProfileIndex); + mixerProfileAT.phase = MIXERAT_PHASE_IDLE; + reprocessState = true; + //transition is done + } + return false; + break; + default: + break; + } + } + while (reprocessState); + return true; +} + +bool checkMixerProfileHotSwitchAvalibility(void) +{ + if (MAX_MIXER_PROFILE_COUNT != 2) + { + return false; + } + return true; +} + +void outputProfileUpdateTask(timeUs_t currentTimeUs) +{ + UNUSED(currentTimeUs); + if(cliMode) return; + bool mixerAT_inuse = mixerProfileAT.phase != MIXERAT_PHASE_IDLE; + // transition mode input for servo mix and motor mix + if (!FLIGHT_MODE(FAILSAFE_MODE) && (!mixerAT_inuse)) + { + if (isModeActivationConditionPresent(BOXMIXERPROFILE)){ + outputProfileHotSwitch(IS_RC_MODE_ACTIVE(BOXMIXERPROFILE) == 0 ? 0 : 1); + } + isMixerTransitionMixing_requested = IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION); + } + isMixerTransitionMixing = isMixerTransitionMixing_requested && ((posControl.navState == NAV_STATE_IDLE) || mixerAT_inuse ||(posControl.navState == NAV_STATE_ALTHOLD_IN_PROGRESS)); +} + +// switch mixerprofile without reboot +bool outputProfileHotSwitch(int profile_index) +{ + static bool allow_hot_switch = true; + // LOG_INFO(PWM, "OutputProfileHotSwitch"); + if (!allow_hot_switch) + { + return false; + } + if (currentMixerProfileIndex == profile_index) + { + return false; + } + if (profile_index < 0 || profile_index >= MAX_MIXER_PROFILE_COUNT) + { // sanity check + // LOG_INFO(PWM, "invalid mixer profile index"); + return false; + } + if (areSensorsCalibrating()) + { // it seems like switching before sensors calibration complete will cause pid stops to respond, especially in D + return false; + } + if (!checkMixerProfileHotSwitchAvalibility()) + { + // LOG_INFO(PWM, "mixer switch failed, checkMixerProfileHotSwitchAvalibility"); + return false; + } + if ((posControl.navState != NAV_STATE_IDLE) && (posControl.navState != NAV_STATE_MIXERAT_IN_PROGRESS)) + { + // LOG_INFO(PWM, "mixer switch failed, navState != NAV_STATE_IDLE"); + return false; + } + if (!setConfigMixerProfile(profile_index)) + { + // LOG_INFO(PWM, "mixer switch failed to set config"); + return false; + } + mixerConfigInit(); + return true; +} diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h new file mode 100644 index 00000000000..d2208d7b8c0 --- /dev/null +++ b/src/main/flight/mixer_profile.h @@ -0,0 +1,79 @@ +#pragma once + +#include "config/parameter_group.h" +#include "flight/failsafe.h" +#include "flight/mixer.h" +#include "flight/servos.h" + +#ifndef MAX_MIXER_PROFILE_COUNT +#define MAX_MIXER_PROFILE_COUNT 2 +#endif + +typedef struct mixerConfig_s { + int8_t motorDirectionInverted; + uint8_t platformType; + bool hasFlaps; + int16_t appliedMixerPreset; + bool motorstopOnLow; + bool PIDProfileLinking; + bool automated_switch; + int16_t switchTransitionTimer; +} mixerConfig_t; +typedef struct mixerProfile_s { + mixerConfig_t mixer_config; + motorMixer_t MotorMixers[MAX_SUPPORTED_MOTORS]; + servoMixer_t ServoMixers[MAX_SERVO_RULES]; +} mixerProfile_t; + +PG_DECLARE_ARRAY(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles); +typedef enum { + MIXERAT_REQUEST_NONE, //no request, stats checking only + MIXERAT_REQUEST_RTH, + MIXERAT_REQUEST_LAND, + MIXERAT_REQUEST_ABORT, +} mixerProfileATRequest_e; + +//mixerProfile Automated Transition PHASE +typedef enum { + MIXERAT_PHASE_IDLE, + MIXERAT_PHASE_TRANSITION_INITIALIZE, + MIXERAT_PHASE_TRANSITIONING, + MIXERAT_PHASE_DONE, +} mixerProfileATState_e; + +typedef struct mixerProfileAT_s { + mixerProfileATState_e phase; + bool transitionInputMixing; + timeMs_t transitionStartTime; + timeMs_t transitionStabEndTime; + timeMs_t transitionTransEndTime; +} mixerProfileAT_t; +extern mixerProfileAT_t mixerProfileAT; +bool checkMixerATRequired(mixerProfileATRequest_e required_action); +bool mixerATUpdateState(mixerProfileATRequest_e required_action); + +extern mixerConfig_t currentMixerConfig; +extern int currentMixerProfileIndex; +extern bool isMixerTransitionMixing; +#define mixerConfig() (&(mixerProfiles(systemConfig()->current_mixer_profile_index)->mixer_config)) +#define mixerConfigMutable() ((mixerConfig_t *) mixerConfig()) + +#define primaryMotorMixer(_index) (&(mixerProfiles(systemConfig()->current_mixer_profile_index)->MotorMixers)[_index]) +#define primaryMotorMixerMutable(_index) ((motorMixer_t *)primaryMotorMixer(_index)) +#define customServoMixers(_index) (&(mixerProfiles(systemConfig()->current_mixer_profile_index)->ServoMixers)[_index]) +#define customServoMixersMutable(_index) ((servoMixer_t *)customServoMixers(_index)) + +static inline const mixerProfile_t* mixerProfiles_CopyArray_by_index(int _index) { return &mixerProfiles_CopyArray[_index]; } +#define primaryMotorMixer_CopyArray() (mixerProfiles_CopyArray_by_index(systemConfig()->current_mixer_profile_index)->MotorMixers) +#define customServoMixers_CopyArray() (mixerProfiles_CopyArray_by_index(systemConfig()->current_mixer_profile_index)->ServoMixers) + +#define mixerConfigByIndex(index) (&(mixerProfiles(index)->mixer_config)) +#define mixerMotorMixersByIndex(index) (mixerProfiles(index)->MotorMixers) +#define mixerServoMixersByIndex(index) (mixerProfiles(index)->ServoMixers) + +bool platformTypeConfigured(flyingPlatformType_e platformType); +bool outputProfileHotSwitch(int profile_index); +bool checkMixerProfileHotSwitchAvalibility(void); +void activateMixerConfig(void); +void mixerConfigInit(void); +void outputProfileUpdateTask(timeUs_t currentTimeUs); \ No newline at end of file diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 16d0ee2d9eb..3049fbb8224 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -43,6 +43,7 @@ #include "flight/pid.h" #include "flight/imu.h" #include "flight/mixer.h" +#include "flight/mixer_profile.h" #include "flight/rpm_filter.h" #include "flight/kalman.h" #include "flight/smith_predictor.h" @@ -898,17 +899,14 @@ float pidHeadingHold(float dT) { float headingHoldRate; + /* Convert absolute error into relative to current heading */ int16_t error = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headingHoldTarget; - /* - * Convert absolute error into relative to current heading - */ - if (error <= -180) { - error += 360; - } - - if (error >= +180) { + /* Convert absolute error into relative to current heading */ + if (error > 180) { error -= 360; + } else if (error < -180) { + error += 360; } /* @@ -1068,7 +1066,7 @@ void FAST_CODE pidController(float dT) return; } - bool canUseFpvCameraMix = true; + bool canUseFpvCameraMix = STATE(MULTIROTOR); uint8_t headingHoldState = getHeadingHoldState(); // In case Yaw override is active, we engage the Heading Hold state @@ -1132,7 +1130,7 @@ void FAST_CODE pidController(float dT) canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with TURN_ASSISTANT } - if (canUseFpvCameraMix && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && currentControlRateProfile->misc.fpvCamAngleDegrees) { + if (canUseFpvCameraMix && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && currentControlRateProfile->misc.fpvCamAngleDegrees && STATE(MULTIROTOR)) { pidApplyFpvCameraAngleMix(pidState, currentControlRateProfile->misc.fpvCamAngleDegrees); } @@ -1216,9 +1214,9 @@ void pidInit(void) if (pidProfile()->pidControllerType == PID_TYPE_AUTO) { if ( - mixerConfig()->platformType == PLATFORM_AIRPLANE || - mixerConfig()->platformType == PLATFORM_BOAT || - mixerConfig()->platformType == PLATFORM_ROVER + currentMixerConfig.platformType == PLATFORM_AIRPLANE || + currentMixerConfig.platformType == PLATFORM_BOAT || + currentMixerConfig.platformType == PLATFORM_ROVER ) { usedPidControllerType = PID_TYPE_PIFF; } else { diff --git a/src/main/flight/pid_autotune.c b/src/main/flight/pid_autotune.c index 2eaef1175ca..140c4ab61d9 100755 --- a/src/main/flight/pid_autotune.c +++ b/src/main/flight/pid_autotune.c @@ -130,7 +130,7 @@ void autotuneStart(void) void autotuneUpdateState(void) { - if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE) && ARMING_FLAG(ARMED)) { + if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE) && STATE(AIRPLANE) && ARMING_FLAG(ARMED)) { if (!FLIGHT_MODE(AUTO_TUNE)) { autotuneStart(); ENABLE_FLIGHT_MODE(AUTO_TUNE); diff --git a/src/main/flight/rth_estimator.c b/src/main/flight/rth_estimator.c index eed9adbae36..708c71bd1cf 100644 --- a/src/main/flight/rth_estimator.c +++ b/src/main/flight/rth_estimator.c @@ -209,8 +209,8 @@ float calculateRemainingFlightTimeBeforeRTH(bool takeWindIntoAccount) { // returns meters float calculateRemainingDistanceBeforeRTH(bool takeWindIntoAccount) { - // Fixed wing only for now - if (!(STATE(FIXED_WING_LEGACY) || ARMING_FLAG(ARMED))) { + // Fixed wing only for now, and must be armed + if (!STATE(AIRPLANE) || !ARMING_FLAG(ARMED)) { return -1; } diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 67235a1133f..d3c5da8c75b 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -70,11 +70,10 @@ PG_RESET_TEMPLATE(servoConfig_t, servoConfig, .servo_autotrim_rotation_limit = SETTING_SERVO_AUTOTRIM_ROTATION_LIMIT_DEFAULT ); -PG_REGISTER_ARRAY_WITH_RESET_FN(servoMixer_t, MAX_SERVO_RULES, customServoMixers, PG_SERVO_MIXER, 1); -void pgResetFn_customServoMixers(servoMixer_t *instance) +void Reset_servoMixers(servoMixer_t *instance) { - for (int i = 0; i < MAX_SERVO_RULES; i++) { + for (int i = 0; i < MAX_SERVO_RULES; i++){ RESET_CONFIG(servoMixer_t, &instance[i], .targetChannel = 0, .inputSource = 0, @@ -83,7 +82,7 @@ void pgResetFn_customServoMixers(servoMixer_t *instance) #ifdef USE_PROGRAMMING_FRAMEWORK ,.conditionId = -1 #endif - ); + ); } } @@ -105,6 +104,13 @@ int16_t servo[MAX_SUPPORTED_SERVOS]; static uint8_t servoRuleCount = 0; static servoMixer_t currentServoMixer[MAX_SERVO_RULES]; + +/* +//Was used to keep track of servo rules in all mixer_profile, In order to Apply mixer speed limit when rules turn off +static servoMixer_t currentServoMixer[MAX_SERVO_RULES*MAX_MIXER_PROFILE_COUNT]; +static bool currentServoMixerActivative[MAX_SERVO_RULES*MAX_MIXER_PROFILE_COUNT]; // if true, the rule is used by current servo mixer +*/ + static bool servoOutputEnabled; static bool mixerUsesServos; @@ -137,6 +143,33 @@ void servoComputeScalingFactors(uint8_t servoIndex) { servoMetadata[servoIndex].scaleMin = (servoParams(servoIndex)->middle - servoParams(servoIndex)->min) / 500.0f; } +void computeServoCount(void) +{ + static bool firstRun = true; + if (!firstRun) { + return; + } + minServoIndex = 255; + maxServoIndex = 0; + for (int j = 0; j < MAX_MIXER_PROFILE_COUNT; j++) { + for (int i = 0; i < MAX_SERVO_RULES; i++) { + // check if done + if (mixerServoMixersByIndex(j)[i].rate == 0){ + break; + } + if (mixerServoMixersByIndex(j)[i].targetChannel < minServoIndex) { + minServoIndex = mixerServoMixersByIndex(j)[i].targetChannel; + } + + if (mixerServoMixersByIndex(j)[i].targetChannel > maxServoIndex) { + maxServoIndex = mixerServoMixersByIndex(j)[i].targetChannel; + } + mixerUsesServos = true; + } + } + firstRun = false; +} + void servosInit(void) { // give all servos a default command @@ -147,12 +180,12 @@ void servosInit(void) /* * load mixer */ + computeServoCount(); loadCustomServoMixer(); // If there are servo rules after all, update variables - if (servoRuleCount > 0) { + if (mixerUsesServos) { servoOutputEnabled = true; - mixerUsesServos = true; } for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { @@ -161,8 +194,8 @@ void servosInit(void) } int getServoCount(void) -{ - if (servoRuleCount) { +{ + if (mixerUsesServos) { return 1 + maxServoIndex - minServoIndex; } else { @@ -172,27 +205,17 @@ int getServoCount(void) void loadCustomServoMixer(void) { - // reset settings servoRuleCount = 0; - minServoIndex = 255; - maxServoIndex = 0; memset(currentServoMixer, 0, sizeof(currentServoMixer)); // load custom mixer into currentServoMixer for (int i = 0; i < MAX_SERVO_RULES; i++) { // check if done - if (customServoMixers(i)->rate == 0) + if (customServoMixers(i)->rate == 0){ break; - - if (customServoMixers(i)->targetChannel < minServoIndex) { - minServoIndex = customServoMixers(i)->targetChannel; } - - if (customServoMixers(i)->targetChannel > maxServoIndex) { - maxServoIndex = customServoMixers(i)->targetChannel; - } - - memcpy(¤tServoMixer[i], customServoMixers(i), sizeof(servoMixer_t)); + currentServoMixer[servoRuleCount] = *customServoMixers(i); + servoSpeedLimitFilter[servoRuleCount].state = 0; servoRuleCount++; } } @@ -231,7 +254,7 @@ void writeServos(void) /* * in case of tricopters, there might me a need to zero servo output when unarmed */ - if (mixerConfig()->platformType == PLATFORM_TRICOPTER && !ARMING_FLAG(ARMED) && !servoConfig()->tri_unarmed_servo) { + if (currentMixerConfig.platformType == PLATFORM_TRICOPTER && !ARMING_FLAG(ARMED) && !servoConfig()->tri_unarmed_servo) { zeroServoValue = true; } @@ -261,7 +284,7 @@ void servoMixer(float dT) // Reverse yaw servo when inverted in 3D mode only for multirotor and tricopter if (feature(FEATURE_REVERSIBLE_MOTORS) && (rxGetChannelValue(THROTTLE) < PWM_RANGE_MIDDLE) && - (mixerConfig()->platformType == PLATFORM_MULTIROTOR || mixerConfig()->platformType == PLATFORM_TRICOPTER)) { + (currentMixerConfig.platformType == PLATFORM_MULTIROTOR || currentMixerConfig.platformType == PLATFORM_TRICOPTER)) { input[INPUT_STABILIZED_YAW] *= -1; } } @@ -297,6 +320,8 @@ void servoMixer(float dT) input[INPUT_STABILIZED_THROTTLE] = mixerThrottleCommand - 1000 - 500; // Since it derives from rcCommand or mincommand and must be [-500:+500] + input[INPUT_MIXER_TRANSITION] = isMixerTransitionMixing * 500; //fixed value + // center the RC input value around the RC middle value // by subtracting the RC middle value from the RC input value, we get: // data - middle = input @@ -335,19 +360,19 @@ void servoMixer(float dT) // mix servos according to rules for (int i = 0; i < servoRuleCount; i++) { + const uint8_t target = currentServoMixer[i].targetChannel; + const uint8_t from = currentServoMixer[i].inputSource; + + int16_t inputRaw = input[from]; /* * Check if conditions for a rule are met, not all conditions apply all the time */ #ifdef USE_PROGRAMMING_FRAMEWORK if (!logicConditionGetValue(currentServoMixer[i].conditionId)) { - continue; + inputRaw = 0; } #endif - - const uint8_t target = currentServoMixer[i].targetChannel; - const uint8_t from = currentServoMixer[i].inputSource; - /* * Apply mixer speed limit. 1 [one] speed unit is defined as 10us/s: * 0 = no limiting @@ -355,13 +380,13 @@ void servoMixer(float dT) * 10 = 100us/s -> full sweep (from 1000 to 2000) is performed in 10s * 100 = 1000us/s -> full sweep in 1s */ - int16_t inputLimited = (int16_t) rateLimitFilterApply4(&servoSpeedLimitFilter[i], input[from], currentServoMixer[i].speed * 10, dT); + int16_t inputLimited = (int16_t) rateLimitFilterApply4(&servoSpeedLimitFilter[i], inputRaw, currentServoMixer[i].speed * 10, dT); servo[target] += ((int32_t)inputLimited * currentServoMixer[i].rate) / 100; } /* - * When not armed, apply servo low position to all outputs that include a throttle or stabilizet throttle in the mix + * When not armed, apply servo low position to all outputs that include a throttle or stabilized throttle in the mix */ if (!ARMING_FLAG(ARMED)) { for (int i = 0; i < servoRuleCount; i++) { @@ -430,6 +455,7 @@ void processServoAutotrimMode(void) if (ARMING_FLAG(ARMED)) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int i = 0; i < servoRuleCount; i++) { + // Reset servo middle accumulator const uint8_t target = currentServoMixer[i].targetChannel; const uint8_t source = currentServoMixer[i].inputSource; if (source == axis) { @@ -593,6 +619,10 @@ void processServoAutotrim(const float dT) { return; } #endif + if(!STATE(AIRPLANE)) + { + return; + } if (feature(FEATURE_FW_AUTOTRIM)) { processContinuousServoAutotrim(dT); } else { diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h index 841415ea234..1dd65912218 100644 --- a/src/main/flight/servos.h +++ b/src/main/flight/servos.h @@ -62,7 +62,7 @@ typedef enum { INPUT_GVAR_5 = 35, INPUT_GVAR_6 = 36, INPUT_GVAR_7 = 37, - + INPUT_MIXER_TRANSITION = 38, INPUT_SOURCE_COUNT } inputSource_e; @@ -151,6 +151,7 @@ typedef struct servoMetadata_s { extern int16_t servo[MAX_SUPPORTED_SERVOS]; +void Reset_servoMixers(servoMixer_t* instance); bool isServoOutputEnabled(void); void setServoOutputEnabled(bool flag); bool isMixerUsingServos(void); diff --git a/src/main/io/displayport_msp_bf_compat.c b/src/main/io/displayport_msp_bf_compat.c index 7cfa8f1bf63..315ac9a90c0 100644 --- a/src/main/io/displayport_msp_bf_compat.c +++ b/src/main/io/displayport_msp_bf_compat.c @@ -187,13 +187,9 @@ uint8_t getBfCharacter(uint8_t ch, uint8_t page) case SYM_ALT_M: return BF_SYM_M; -/* - case SYM_TRIP_DIST: - return BF_SYM_TRIP_DIST; - case SYM_TOTAL: - return BF_SYM_TOTAL; - + return BF_SYM_TOTAL_DISTANCE; +/* case SYM_ALT_KM: return BF_SYM_ALT_KM; @@ -334,10 +330,12 @@ uint8_t getBfCharacter(uint8_t ch, uint8_t page) case SYM_PITCH_DOWN: return BF_SYM_PITCH_DOWN; + */ case SYM_GFORCE: - return BF_SYM_GFORCE; + return 'G'; +/* case SYM_GFORCE_X: return BF_SYM_GFORCE_X; diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 4d7380e84df..0fae3a7f318 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -77,13 +77,6 @@ gpsSolutionData_t gpsSol; baudRate_e gpsToSerialBaudRate[GPS_BAUDRATE_COUNT] = { BAUD_115200, BAUD_57600, BAUD_38400, BAUD_19200, BAUD_9600, BAUD_230400, BAUD_460800, BAUD_921600 }; static gpsProviderDescriptor_t gpsProviders[GPS_PROVIDER_COUNT] = { - /* NMEA GPS */ -#ifdef USE_GPS_PROTO_NMEA - { false, MODE_RX, gpsRestartNMEA, &gpsHandleNMEA }, -#else - { false, 0, NULL, NULL }, -#endif - /* UBLOX binary */ #ifdef USE_GPS_PROTO_UBLOX { false, MODE_RXTX, &gpsRestartUBLOX, &gpsHandleUBLOX }, @@ -113,7 +106,7 @@ static gpsProviderDescriptor_t gpsProviders[GPS_PROVIDER_COUNT] = { }; -PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 4); +PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 5); PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig, .provider = SETTING_GPS_PROVIDER_DEFAULT, @@ -264,7 +257,7 @@ void gpsPreInit(void) { // Make sure gpsProvider is known when gpsMagDetect is called gpsState.gpsConfig = gpsConfig(); - gpsState.baseTimeoutMs = (gpsState.gpsConfig->provider == GPS_NMEA) ? GPS_TIMEOUT*2 : GPS_TIMEOUT; + gpsState.baseTimeoutMs = GPS_TIMEOUT; } void gpsInit(void) diff --git a/src/main/io/gps.h b/src/main/io/gps.h index 99b6aafbdf0..199652a3a24 100755 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -33,8 +33,7 @@ #define GPS_DEGREES_DIVIDER 10000000L typedef enum { - GPS_NMEA = 0, - GPS_UBLOX, + GPS_UBLOX = 0, GPS_UBLOX7PLUS, GPS_MSP, GPS_FAKE, @@ -47,6 +46,7 @@ typedef enum { SBAS_WAAS, SBAS_MSAS, SBAS_GAGAN, + SBAS_SPAN, SBAS_NONE } sbasMode_e; diff --git a/src/main/io/gps_nmea.c b/src/main/io/gps_nmea.c deleted file mode 100755 index c5c3b420db7..00000000000 --- a/src/main/io/gps_nmea.c +++ /dev/null @@ -1,340 +0,0 @@ -/* - * 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 -#include - -#include "platform.h" - -#if defined(USE_GPS) && (defined(USE_GPS_PROTO_NMEA)) - -#include "build/build_config.h" -#include "build/debug.h" - -#include "common/axis.h" -#include "common/gps_conversion.h" -#include "common/maths.h" -#include "common/utils.h" - -#include "drivers/serial.h" -#include "drivers/time.h" - -#include "fc/config.h" -#include "fc/runtime_config.h" - -#include "io/serial.h" -#include "io/gps.h" -#include "io/gps_private.h" - -#include "scheduler/protothreads.h" - -/* This is a light implementation of a GPS frame decoding - This should work with most of modern GPS devices configured to output 5 frames. - It assumes there are some NMEA GGA frames to decode on the serial bus - Now verifies checksum correctly before applying data - - Here we use only the following data : - - latitude - - longitude - - GPS fix is/is not ok - - GPS num sat (4 is enough to be +/- reliable) - // added by Mis - - GPS altitude (for OSD displaying) - - GPS speed (for OSD displaying) -*/ - -#define NO_FRAME 0 -#define FRAME_GGA 1 -#define FRAME_RMC 2 - -static uint32_t grab_fields(char *src, uint8_t mult) -{ // convert string to uint32 - uint32_t i; - uint32_t tmp = 0; - for (i = 0; src[i] != 0; i++) { - if (src[i] == '.') { - i++; - if (mult == 0) - break; - else - src[i + mult] = 0; - } - tmp *= 10; - if (src[i] >= '0' && src[i] <= '9') - tmp += src[i] - '0'; - if (i >= 15) - return 0; // out of bounds - } - return tmp; -} - -typedef struct gpsDataNmea_s { - bool fix; - int32_t latitude; - int32_t longitude; - uint8_t numSat; - int32_t altitude; - uint16_t speed; - uint16_t ground_course; - uint16_t hdop; - uint32_t time; - uint32_t date; -} gpsDataNmea_t; - -#define NMEA_BUFFER_SIZE 16 - -static bool gpsNewFrameNMEA(char c) -{ - static gpsDataNmea_t gps_Msg; - - uint8_t frameOK = 0; - static uint8_t param = 0, offset = 0, parity = 0; - static char string[NMEA_BUFFER_SIZE]; - static uint8_t checksum_param, gps_frame = NO_FRAME; - - switch (c) { - case '$': - param = 0; - offset = 0; - parity = 0; - break; - case ',': - case '*': - string[offset] = 0; - if (param == 0) { //frame identification - gps_frame = NO_FRAME; - if (strcmp(string, "GPGGA") == 0 || strcmp(string, "GNGGA") == 0) { - gps_frame = FRAME_GGA; - } - else if (strcmp(string, "GPRMC") == 0 || strcmp(string, "GNRMC") == 0) { - gps_frame = FRAME_RMC; - } - } - - switch (gps_frame) { - case FRAME_GGA: //************* GPGGA FRAME parsing - switch (param) { - // case 1: // Time information - // break; - case 2: - gps_Msg.latitude = GPS_coord_to_degrees(string); - break; - case 3: - if (string[0] == 'S') - gps_Msg.latitude *= -1; - break; - case 4: - gps_Msg.longitude = GPS_coord_to_degrees(string); - break; - case 5: - if (string[0] == 'W') - gps_Msg.longitude *= -1; - break; - case 6: - if (string[0] > '0') { - gps_Msg.fix = true; - } else { - gps_Msg.fix = false; - } - break; - case 7: - gps_Msg.numSat = grab_fields(string, 0); - break; - case 8: - gps_Msg.hdop = grab_fields(string, 1) * 10; // hdop - break; - case 9: - gps_Msg.altitude = grab_fields(string, 1) * 10; // altitude in cm - break; - } - break; - case FRAME_RMC: //************* GPRMC FRAME parsing - // $GNRMC,130059.00,V,,,,,,,110917,,,N*62 - switch (param) { - case 1: - gps_Msg.time = grab_fields(string, 2); - break; - case 7: - gps_Msg.speed = ((grab_fields(string, 1) * 5144L) / 1000L); // speed in cm/s added by Mis - break; - case 8: - gps_Msg.ground_course = (grab_fields(string, 1)); // ground course deg * 10 - break; - case 9: - gps_Msg.date = grab_fields(string, 0); - break; - } - break; - } - - param++; - offset = 0; - if (c == '*') - checksum_param = 1; - else - parity ^= c; - break; - case '\r': - case '\n': - if (checksum_param) { //parity checksum - uint8_t checksum = 16 * ((string[0] >= 'A') ? string[0] - 'A' + 10 : string[0] - '0') + ((string[1] >= 'A') ? string[1] - 'A' + 10 : string[1] - '0'); - if (checksum == parity) { - gpsStats.packetCount++; - switch (gps_frame) { - case FRAME_GGA: - frameOK = 1; - gpsSol.numSat = gps_Msg.numSat; - if (gps_Msg.fix) { - gpsSol.fixType = GPS_FIX_3D; // NMEA doesn't report fix type, assume 3D - - gpsSol.llh.lat = gps_Msg.latitude; - gpsSol.llh.lon = gps_Msg.longitude; - gpsSol.llh.alt = gps_Msg.altitude; - - // EPH/EPV are unreliable for NMEA as they are not real accuracy - gpsSol.hdop = gpsConstrainHDOP(gps_Msg.hdop); - gpsSol.eph = gpsConstrainEPE(gps_Msg.hdop * GPS_HDOP_TO_EPH_MULTIPLIER); - gpsSol.epv = gpsConstrainEPE(gps_Msg.hdop * GPS_HDOP_TO_EPH_MULTIPLIER); - gpsSol.flags.validEPE = false; - } - else { - gpsSol.fixType = GPS_NO_FIX; - } - - // NMEA does not report VELNED - gpsSol.flags.validVelNE = false; - gpsSol.flags.validVelD = false; - break; - case FRAME_RMC: - gpsSol.groundSpeed = gps_Msg.speed; - gpsSol.groundCourse = gps_Msg.ground_course; - - // This check will miss 00:00:00.00, but we shouldn't care - next report will be valid - if (gps_Msg.date != 0 && gps_Msg.time != 0) { - gpsSol.time.year = (gps_Msg.date % 100) + 2000; - gpsSol.time.month = (gps_Msg.date / 100) % 100; - gpsSol.time.day = (gps_Msg.date / 10000) % 100; - gpsSol.time.hours = (gps_Msg.time / 1000000) % 100; - gpsSol.time.minutes = (gps_Msg.time / 10000) % 100; - gpsSol.time.seconds = (gps_Msg.time / 100) % 100; - gpsSol.time.millis = (gps_Msg.time & 100) * 10; - gpsSol.flags.validTime = true; - } - else { - gpsSol.flags.validTime = false; - } - - break; - } // end switch - } - else { - gpsStats.errors++; - } - } - checksum_param = 0; - break; - default: - if (offset < (NMEA_BUFFER_SIZE-1)) { // leave 1 byte to trailing zero - string[offset++] = c; - - // only checksum if character is recorded and used (will cause checksum failure on dropped characters) - if (!checksum_param) - parity ^= c; - } - } - return frameOK; -} - -static ptSemaphore_t semNewDataReady; - -STATIC_PROTOTHREAD(gpsProtocolReceiverThread) -{ - ptBegin(gpsProtocolReceiverThread); - - while (1) { - // Wait until there are bytes to consume - ptWait(serialRxBytesWaiting(gpsState.gpsPort)); - - // Consume bytes until buffer empty of until we have full message received - while (serialRxBytesWaiting(gpsState.gpsPort)) { - uint8_t newChar = serialRead(gpsState.gpsPort); - if (gpsNewFrameNMEA(newChar)) { - gpsSol.flags.validVelNE = false; - gpsSol.flags.validVelD = false; - ptSemaphoreSignal(semNewDataReady); - break; - } - } - } - - ptEnd(0); -} - -STATIC_PROTOTHREAD(gpsProtocolStateThreadNMEA) -{ - ptBegin(gpsProtocolStateThreadNMEA); - - // Change baud rate - ptWait(isSerialTransmitBufferEmpty(gpsState.gpsPort)); - if (gpsState.gpsConfig->autoBaud != GPS_AUTOBAUD_OFF) { - // Cycle through available baud rates and hope that we will match GPS - serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.autoBaudrateIndex]]); - gpsState.autoBaudrateIndex = (gpsState.autoBaudrateIndex + 1) % GPS_BAUDRATE_COUNT; - ptDelayMs(GPS_BAUD_CHANGE_DELAY); - } - else { - // Set baud rate - serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.baudrateIndex]]); - } - - // No configuration is done for pure NMEA modules - - // GPS setup done, reset timeout - gpsSetProtocolTimeout(gpsState.baseTimeoutMs); - - // GPS is ready - execute the gpsProcessNewSolutionData() based on gpsProtocolReceiverThread semaphore - while (1) { - ptSemaphoreWait(semNewDataReady); - gpsProcessNewSolutionData(); - } - - ptEnd(0); -} - -void gpsHandleNMEA(void) -{ - // Run the protocol threads - gpsProtocolReceiverThread(); - gpsProtocolStateThreadNMEA(); - - // If thread stopped - signal communication loss and restart - if (ptIsStopped(ptGetHandle(gpsProtocolReceiverThread)) || ptIsStopped(ptGetHandle(gpsProtocolStateThreadNMEA))) { - gpsSetState(GPS_LOST_COMMUNICATION); - } -} - -void gpsRestartNMEA(void) -{ - ptSemaphoreInit(semNewDataReady); - ptRestart(ptGetHandle(gpsProtocolReceiverThread)); - ptRestart(ptGetHandle(gpsProtocolStateThreadNMEA)); -} - -#endif diff --git a/src/main/io/gps_private.h b/src/main/io/gps_private.h index 765318d1fce..9136fdcf837 100755 --- a/src/main/io/gps_private.h +++ b/src/main/io/gps_private.h @@ -76,9 +76,6 @@ void gpsSetProtocolTimeout(timeMs_t timeoutMs); extern void gpsRestartUBLOX(void); extern void gpsHandleUBLOX(void); -extern void gpsRestartNMEA(void); -extern void gpsHandleNMEA(void); - extern void gpsRestartMSP(void); extern void gpsHandleMSP(void); diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 2baa97ce9b7..d03c3390776 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -69,6 +69,7 @@ static const uint32_t ubloxScanMode1[] = { (SBASMASK1_BITS(129) | SBASMASK1_BITS(137) | SBASMASK1_BITS(139)), // MSAS (SBASMASK1_BITS(127) | SBASMASK1_BITS(128) | SBASMASK1_BITS(132)), // GAGAN + (SBASMASK1_BITS(122)), // SPAN 0x00000000, // NONE }; diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h index bf057e830cd..ab5b72f3885 100644 --- a/src/main/io/ledstrip.h +++ b/src/main/io/ledstrip.h @@ -159,12 +159,12 @@ typedef struct ledStripConfig_s { PG_DECLARE(ledStripConfig_t, ledStripConfig); #define DEFINE_LED(ledConfigPtr, x, y, col, dir, func, ol, params) { \ - ledConfigPtr->led_position = CALCULATE_LED_XY(x, y); \ - ledConfigPtr->led_color = (col); \ - ledConfigPtr->led_direction = (dir); \ - ledConfigPtr->led_function = (func); \ - ledConfigPtr->led_overlay = (ol); \ - ledConfigPtr->led_params = (params); } + (ledConfigPtr)->led_position = CALCULATE_LED_XY(x, y); \ + (ledConfigPtr)->led_color = (col); \ + (ledConfigPtr)->led_direction = (dir); \ + (ledConfigPtr)->led_function = (func); \ + (ledConfigPtr)->led_overlay = (ol); \ + (ledConfigPtr)->led_params = (params); } static inline uint8_t ledGetXY(const ledConfig_t *lcfg) { return (lcfg->led_position); } static inline uint8_t ledGetX(const ledConfig_t *lcfg) { return ((lcfg->led_position >> (LED_X_BIT_OFFSET)) & LED_XY_MASK); } diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 8983fc5792d..24a6bc6cb24 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -131,7 +131,6 @@ #define STATS_PAGE1 (checkStickPosition(ROL_LO)) #define SPLASH_SCREEN_DISPLAY_TIME 4000 // ms -#define ARMED_SCREEN_DISPLAY_TIME 1500 // ms #define STATS_SCREEN_DISPLAY_TIME 60000 // ms #define EFFICIENCY_UPDATE_INTERVAL (5 * 1000) @@ -209,7 +208,7 @@ static bool osdDisplayHasCanvas; #define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees) -PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 8); +PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 9); PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 1); void osdStartedSaveProcess(void) { @@ -316,7 +315,7 @@ static void osdFormatDistanceSymbol(char *buff, int32_t dist, uint8_t decimals) case OSD_UNIT_UK: FALLTHROUGH; case OSD_UNIT_IMPERIAL: - if (osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(dist), FEET_PER_MILE, decimals, 3, digits)) { + if (osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(dist), FEET_PER_MILE, decimals, 3, digits, false)) { buff[sym_index] = symbol_mi; } else { buff[sym_index] = symbol_ft; @@ -326,7 +325,7 @@ static void osdFormatDistanceSymbol(char *buff, int32_t dist, uint8_t decimals) case OSD_UNIT_METRIC_MPH: FALLTHROUGH; case OSD_UNIT_METRIC: - if (osdFormatCentiNumber(buff, dist, METERS_PER_KILOMETER, decimals, 3, digits)) { + if (osdFormatCentiNumber(buff, dist, METERS_PER_KILOMETER, decimals, 3, digits, false)) { buff[sym_index] = symbol_km; } else { buff[sym_index] = symbol_m; @@ -334,7 +333,7 @@ static void osdFormatDistanceSymbol(char *buff, int32_t dist, uint8_t decimals) buff[sym_index + 1] = '\0'; break; case OSD_UNIT_GA: - if (osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(dist), (uint32_t)FEET_PER_NAUTICALMILE, decimals, 3, digits)) { + if (osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(dist), (uint32_t)FEET_PER_NAUTICALMILE, decimals, 3, digits, false)) { buff[sym_index] = symbol_nm; } else { buff[sym_index] = symbol_ft; @@ -489,7 +488,7 @@ static void osdFormatWindSpeedStr(char *buff, int32_t ws, bool isValid) break; } - osdFormatCentiNumber(buff, centivalue, 0, 2, 0, 3); + osdFormatCentiNumber(buff, centivalue, 0, 2, 0, 3, false); if (!isValid && ((millis() / 1000) % 4 < 2)) suffix = '*'; @@ -562,7 +561,7 @@ void osdFormatAltitudeSymbol(char *buff, int32_t alt) case OSD_UNIT_GA: FALLTHROUGH; case OSD_UNIT_IMPERIAL: - if (osdFormatCentiNumber(buff + totalDigits - digits, CENTIMETERS_TO_CENTIFEET(alt), 1000, 0, 2, digits)) { + if (osdFormatCentiNumber(buff + totalDigits - digits, CENTIMETERS_TO_CENTIFEET(alt), 1000, 0, 2, digits, false)) { // Scaled to kft buff[symbolIndex++] = symbolKFt; } else { @@ -575,7 +574,7 @@ void osdFormatAltitudeSymbol(char *buff, int32_t alt) FALLTHROUGH; case OSD_UNIT_METRIC: // alt is alredy in cm - if (osdFormatCentiNumber(buff + totalDigits - digits, alt, 1000, 0, 2, digits)) { + if (osdFormatCentiNumber(buff + totalDigits - digits, alt, 1000, 0, 2, digits, false)) { // Scaled to km buff[symbolIndex++] = SYM_ALT_KM; } else { @@ -831,13 +830,14 @@ static const char * osdArmingDisabledReasonMessage(void) // See handling of FAILSAFE_RX_LOSS_MONITORING in failsafe.c if (failsafePhase() == FAILSAFE_RX_LOSS_MONITORING) { if (failsafeIsReceivingRxData()) { - // If we're not using sticks, it means the ARM switch - // hasn't been off since entering FAILSAFE_RX_LOSS_MONITORING - // yet - return OSD_MESSAGE_STR(OSD_MSG_TURN_ARM_SW_OFF); + // reminder to disarm to exit FAILSAFE_RX_LOSS_MONITORING once timeout period ends + if (IS_RC_MODE_ACTIVE(BOXARM)) { + return OSD_MESSAGE_STR(OSD_MSG_TURN_ARM_SW_OFF); + } + } else { + // Not receiving RX data + return OSD_MESSAGE_STR(OSD_MSG_RC_RX_LINK_LOST); } - // Not receiving RX data - return OSD_MESSAGE_STR(OSD_MSG_RC_RX_LINK_LOST); } return OSD_MESSAGE_STR(OSD_MSG_DISABLED_BY_FS); case ARMING_DISABLED_NOT_LEVEL: @@ -991,10 +991,10 @@ static const char * osdFailsafeInfoMessage(void) #if defined(USE_SAFE_HOME) static const char * divertingToSafehomeMessage(void) { - if (NAV_Status.state != MW_NAV_STATE_HOVER_ABOVE_HOME && posControl.safehomeState.isApplied) { - return OSD_MESSAGE_STR(OSD_MSG_DIVERT_SAFEHOME); - } - return NULL; + if (NAV_Status.state != MW_NAV_STATE_HOVER_ABOVE_HOME && posControl.safehomeState.isApplied) { + return OSD_MESSAGE_STR(OSD_MSG_DIVERT_SAFEHOME); + } + return NULL; } #endif @@ -1116,11 +1116,7 @@ bool osdUsingScaledThrottle(void) **/ static void osdFormatThrottlePosition(char *buff, bool useScaled, textAttributes_t *elemAttr) { - if (useScaled) { - buff[0] = SYM_SCALE; - } else { - buff[0] = SYM_BLANK; - } + buff[0] = SYM_BLANK; buff[1] = SYM_THR; if (navigationIsControllingThrottle()) { buff[0] = SYM_AUTO_THR0; @@ -1135,7 +1131,14 @@ static void osdFormatThrottlePosition(char *buff, bool useScaled, textAttributes TEXT_ATTRIBUTES_ADD_BLINK(*elemAttr); } #endif - tfp_sprintf(buff + 2, "%3d", getThrottlePercent(useScaled)); + int8_t throttlePercent = getThrottlePercent(useScaled); + if ((useScaled && throttlePercent <= 0) || !ARMING_FLAG(ARMED)) { + const char* message = ARMING_FLAG(ARMED) ? throttlePercent == 0 ? "IDLE" : "STOP" : "DARM"; + buff[0] = SYM_THR; + strcpy(buff + 1, message); + return; + } + tfp_sprintf(buff + 2, "%3d", throttlePercent); } /** @@ -1147,7 +1150,7 @@ static void osdFormatGVar(char *buff, uint8_t index) buff[1] = '0'+index; buff[2] = ':'; #ifdef USE_PROGRAMMING_FRAMEWORK - osdFormatCentiNumber(buff + 3, (int32_t)gvGet(index)*(int32_t)100, 1, 0, 0, 5); + osdFormatCentiNumber(buff + 3, (int32_t)gvGet(index)*(int32_t)100, 1, 0, 0, 5, false); #endif } @@ -1158,7 +1161,7 @@ static void osdFormatRpm(char *buff, uint32_t rpm) if (rpm) { if ( digitCount(rpm) > osdConfig()->esc_rpm_precision) { uint8_t rpmMaxDecimals = (osdConfig()->esc_rpm_precision - 3); - osdFormatCentiNumber(buff + 1, rpm / 10, 0, rpmMaxDecimals, rpmMaxDecimals, osdConfig()->esc_rpm_precision-1); + osdFormatCentiNumber(buff + 1, rpm / 10, 0, rpmMaxDecimals, rpmMaxDecimals, osdConfig()->esc_rpm_precision-1, false); buff[osdConfig()->esc_rpm_precision] = 'K'; buff[osdConfig()->esc_rpm_precision+1] = '\0'; } @@ -1482,13 +1485,13 @@ static void osdFormatPidControllerOutput(char *buff, const char *label, const pi strcpy(buff, label); for (uint8_t i = strlen(label); i < 5; ++i) buff[i] = ' '; uint8_t decimals = showDecimal ? 1 : 0; - osdFormatCentiNumber(buff + 5, pidController->proportional * scale, 0, decimals, 0, 4); + osdFormatCentiNumber(buff + 5, pidController->proportional * scale, 0, decimals, 0, 4, false); buff[9] = ' '; - osdFormatCentiNumber(buff + 10, pidController->integrator * scale, 0, decimals, 0, 4); + osdFormatCentiNumber(buff + 10, pidController->integrator * scale, 0, decimals, 0, 4, false); buff[14] = ' '; - osdFormatCentiNumber(buff + 15, pidController->derivative * scale, 0, decimals, 0, 4); + osdFormatCentiNumber(buff + 15, pidController->derivative * scale, 0, decimals, 0, 4, false); buff[19] = ' '; - osdFormatCentiNumber(buff + 20, pidController->output_constrained * scale, 0, decimals, 0, 4); + osdFormatCentiNumber(buff + 20, pidController->output_constrained * scale, 0, decimals, 0, 4, false); buff[24] = '\0'; } @@ -1504,7 +1507,7 @@ static void osdDisplayBatteryVoltage(uint8_t elemPosX, uint8_t elemPosY, uint16_ elemAttr = TEXT_ATTRIBUTES_NONE; digits = MIN(digits, 5); - osdFormatCentiNumber(buff, voltage, 0, decimals, 0, digits); + osdFormatCentiNumber(buff, voltage, 0, decimals, 0, digits, false); buff[digits] = SYM_VOLT; buff[digits+1] = '\0'; const batteryState_e batteryVoltageState = checkBatteryVoltageState(); @@ -1598,7 +1601,7 @@ static void osdDisplayAdjustableDecimalValue(uint8_t elemPosX, uint8_t elemPosY, displayWrite(osdDisplayPort, elemPosX, elemPosY, str); elemAttr = TEXT_ATTRIBUTES_NONE; - osdFormatCentiNumber(buff, value * 100, 0, maxDecimals, 0, MIN(valueLength, 8)); + osdFormatCentiNumber(buff, value * 100, 0, maxDecimals, 0, MIN(valueLength, 8), false); if (isAdjustmentFunctionSelected(adjFunc)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + strlen(str) + 1 + valueOffset, elemPosY, buff, elemAttr); @@ -1703,7 +1706,7 @@ static bool osdDrawSingleElement(uint8_t item) } case OSD_CURRENT_DRAW: { - osdFormatCentiNumber(buff, getAmperage(), 0, 2, 0, 3); + osdFormatCentiNumber(buff, getAmperage(), 0, 2, 0, 3, false); buff[3] = SYM_AMP; buff[4] = '\0'; @@ -1730,7 +1733,7 @@ static bool osdDrawSingleElement(uint8_t item) buff[5] = SYM_MAH; buff[6] = '\0'; } else { - if (osdFormatCentiNumber(buff, getMAhDrawn() * 100, 1000, 0, (mah_digits - 2), mah_digits)) { + if (osdFormatCentiNumber(buff, getMAhDrawn() * 100, 1000, 0, (mah_digits - 2), mah_digits, false)) { // Shown in Ah buff[mah_digits] = SYM_AH; } else { @@ -1745,7 +1748,7 @@ static bool osdDrawSingleElement(uint8_t item) } case OSD_WH_DRAWN: - osdFormatCentiNumber(buff, getMWhDrawn() / 10, 0, 2, 0, 3); + osdFormatCentiNumber(buff, getMWhDrawn() / 10, 0, 2, 0, 3, false); osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr); buff[3] = SYM_WH; buff[4] = '\0'; @@ -1760,7 +1763,7 @@ static bool osdDrawSingleElement(uint8_t item) else if (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH) tfp_sprintf(buff, "%4lu", (unsigned long)getBatteryRemainingCapacity()); else // currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH - osdFormatCentiNumber(buff + 1, getBatteryRemainingCapacity() / 10, 0, 2, 0, 3); + osdFormatCentiNumber(buff + 1, getBatteryRemainingCapacity() / 10, 0, 2, 0, 3, false); buff[4] = currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH ? SYM_MAH : SYM_WH; buff[5] = '\0'; @@ -1830,7 +1833,7 @@ static bool osdDrawSingleElement(uint8_t item) buff[0] = SYM_GLIDESLOPE; if (glideSlope > 0.0f && glideSlope < 100.0f) { - osdFormatCentiNumber(buff + 1, glideSlope * 100.0f, 0, 2, 0, 3); + osdFormatCentiNumber(buff + 1, glideSlope * 100.0f, 0, 2, 0, 3, false); } else { buff[1] = buff[2] = buff[3] = '-'; } @@ -1908,6 +1911,39 @@ static bool osdDrawSingleElement(uint8_t item) osdFormatDistanceSymbol(buff + 1, getTotalTravelDistance(), 0); break; + case OSD_ODOMETER: + { + displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_ODOMETER); + uint32_t odometerDist = (uint32_t)(getTotalTravelDistance() / 100); +#ifdef USE_STATS + odometerDist+= statsConfig()->stats_total_dist; +#endif + odometerDist = odometerDist / 10; + + switch (osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(odometerDist), FEET_PER_MILE, 1, 0, 6, true); + buff[6] = SYM_MI; + break; + default: + case OSD_UNIT_GA: + osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(odometerDist), (uint32_t)FEET_PER_NAUTICALMILE, 1, 0, 6, true); + buff[6] = SYM_NM; + break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + osdFormatCentiNumber(buff, odometerDist, METERS_PER_KILOMETER, 1, 0, 6, true); + buff[6] = SYM_KM; + break; + } + buff[7] = '\0'; + elemPosX++; + } + break; + case OSD_GROUND_COURSE: { buff[0] = SYM_GROUND_COURSE; @@ -1990,7 +2026,7 @@ static bool osdDrawSingleElement(uint8_t item) digits = 3U; } #endif - osdFormatCentiNumber(&buff[2], centiHDOP, 0, 1, 0, digits); + osdFormatCentiNumber(&buff[2], centiHDOP, 0, 1, 0, digits, false); break; } @@ -2143,37 +2179,38 @@ static bool osdDrawSingleElement(uint8_t item) updatedTimestamp = currentTimeUs; } #endif - //buff[0] = SYM_TRIP_DIST; displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_FLIGHT_DIST_REMAINING); + if ((!ARMING_FLAG(ARMED)) || (distanceMeters == -1)) { - buff[4] = SYM_BLANK; - buff[5] = '\0'; - strcpy(buff + 1, "---"); + buff[3] = SYM_BLANK; + buff[4] = '\0'; + strcpy(buff, "---"); } else if (distanceMeters == -2) { // Wind is too strong to come back with cruise throttle - buff[1] = buff[2] = buff[3] = SYM_WIND_HORIZONTAL; + buff[0] = buff[1] = buff[2] = SYM_WIND_HORIZONTAL; switch ((osd_unit_e)osdConfig()->units){ case OSD_UNIT_UK: FALLTHROUGH; case OSD_UNIT_IMPERIAL: - buff[4] = SYM_DIST_MI; + buff[3] = SYM_DIST_MI; break; case OSD_UNIT_METRIC_MPH: FALLTHROUGH; case OSD_UNIT_METRIC: - buff[4] = SYM_DIST_KM; + buff[3] = SYM_DIST_KM; break; case OSD_UNIT_GA: - buff[4] = SYM_DIST_NM; + buff[3] = SYM_DIST_NM; break; } - buff[5] = '\0'; + buff[4] = '\0'; TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } else { - osdFormatDistanceSymbol(buff + 1, distanceMeters * 100, 0); + osdFormatDistanceSymbol(buff, distanceMeters * 100, 0); if (distanceMeters == 0) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } + elemPosX++; break; case OSD_FLYMODE: @@ -2221,6 +2258,12 @@ static bool osdDrawSingleElement(uint8_t item) osdFormatPilotName(buff); break; + case OSD_PILOT_LOGO: + displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_PILOT_LOGO_SML_L); + displayWriteChar(osdDisplayPort, elemPosX+1, elemPosY, SYM_PILOT_LOGO_SML_C); + displayWriteChar(osdDisplayPort, elemPosX+2, elemPosY, SYM_PILOT_LOGO_SML_R); + break; + case OSD_THROTTLE_POS: { osdFormatThrottlePosition(buff, false, &elemAttr); @@ -2419,7 +2462,7 @@ static bool osdDrawSingleElement(uint8_t item) buff[0] = SYM_ROLL_LEVEL; if (ABS(attitude.values.roll) >= 1) buff[0] += (attitude.values.roll < 0 ? -1 : 1); - osdFormatCentiNumber(buff + 1, DECIDEGREES_TO_CENTIDEGREES(ABS(attitude.values.roll)), 0, 1, 0, 3); + osdFormatCentiNumber(buff + 1, DECIDEGREES_TO_CENTIDEGREES(ABS(attitude.values.roll)), 0, 1, 0, 3, false); break; case OSD_ATTITUDE_PITCH: @@ -2429,7 +2472,7 @@ static bool osdDrawSingleElement(uint8_t item) buff[0] = SYM_PITCH_DOWN; else if (attitude.values.pitch < 0) buff[0] = SYM_PITCH_UP; - osdFormatCentiNumber(buff + 1, DECIDEGREES_TO_CENTIDEGREES(ABS(attitude.values.pitch)), 0, 1, 0, 3); + osdFormatCentiNumber(buff + 1, DECIDEGREES_TO_CENTIDEGREES(ABS(attitude.values.pitch)), 0, 1, 0, 3, false); break; case OSD_ARTIFICIAL_HORIZON: @@ -2490,7 +2533,7 @@ static bool osdDrawSingleElement(uint8_t item) break; } - osdFormatCentiNumber(buff, value, 0, 1, 0, 3); + osdFormatCentiNumber(buff, value, 0, 1, 0, 3, false); buff[3] = sym; buff[4] = '\0'; break; @@ -2523,7 +2566,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_UNIT_IMPERIAL: // mAh/foot if (efficiencyValid) { - osdFormatCentiNumber(buff, (value * METERS_PER_FOOT), 1, 2, 2, 3); + osdFormatCentiNumber(buff, (value * METERS_PER_FOOT), 1, 2, 2, 3, false); tfp_sprintf(buff, "%s%c%c", buff, SYM_AH_V_FT_0, SYM_AH_V_FT_1); } else { buff[0] = buff[1] = buff[2] = '-'; @@ -2537,7 +2580,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_UNIT_METRIC: // mAh/metre if (efficiencyValid) { - osdFormatCentiNumber(buff, value, 1, 2, 2, 3); + osdFormatCentiNumber(buff, value, 1, 2, 2, 3, false); tfp_sprintf(buff, "%s%c%c", buff, SYM_AH_V_M_0, SYM_AH_V_M_1); } else { buff[0] = buff[1] = buff[2] = '-'; @@ -2832,7 +2875,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_POWER: { - bool kiloWatt = osdFormatCentiNumber(buff, getPower(), 1000, 2, 2, 3); + bool kiloWatt = osdFormatCentiNumber(buff, getPower(), 1000, 2, 2, 3, false); buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT; buff[4] = '\0'; @@ -2995,7 +3038,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_UNIT_UK: FALLTHROUGH; case OSD_UNIT_IMPERIAL: - moreThanAh = osdFormatCentiNumber(buff, value * METERS_PER_MILE / 10, 1000, 0, 2, digits); + moreThanAh = osdFormatCentiNumber(buff, value * METERS_PER_MILE / 10, 1000, 0, 2, digits, false); if (!moreThanAh) { tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_MI_0, SYM_MAH_MI_1); } else { @@ -3009,7 +3052,7 @@ static bool osdDrawSingleElement(uint8_t item) } break; case OSD_UNIT_GA: - moreThanAh = osdFormatCentiNumber(buff, value * METERS_PER_NAUTICALMILE / 10, 1000, 0, 2, digits); + moreThanAh = osdFormatCentiNumber(buff, value * METERS_PER_NAUTICALMILE / 10, 1000, 0, 2, digits, false); if (!moreThanAh) { tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_NM_0, SYM_MAH_NM_1); } else { @@ -3025,7 +3068,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_UNIT_METRIC_MPH: FALLTHROUGH; case OSD_UNIT_METRIC: - moreThanAh = osdFormatCentiNumber(buff, value * 100, 1000, 0, 2, digits); + moreThanAh = osdFormatCentiNumber(buff, value * 100, 1000, 0, 2, digits, false); if (!moreThanAh) { tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_KM_0, SYM_MAH_KM_1); } else { @@ -3066,17 +3109,17 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_UNIT_UK: FALLTHROUGH; case OSD_UNIT_IMPERIAL: - osdFormatCentiNumber(buff, value * METERS_PER_MILE / 10000, 0, 2, 0, 3); + osdFormatCentiNumber(buff, value * METERS_PER_MILE / 10000, 0, 2, 0, 3, false); buff[3] = SYM_WH_MI; break; case OSD_UNIT_GA: - osdFormatCentiNumber(buff, value * METERS_PER_NAUTICALMILE / 10000, 0, 2, 0, 3); + osdFormatCentiNumber(buff, value * METERS_PER_NAUTICALMILE / 10000, 0, 2, 0, 3, false); buff[3] = SYM_WH_NM; break; case OSD_UNIT_METRIC_MPH: FALLTHROUGH; case OSD_UNIT_METRIC: - osdFormatCentiNumber(buff, value / 10, 0, 2, 0, 3); + osdFormatCentiNumber(buff, value / 10, 0, 2, 0, 3, false); buff[3] = SYM_WH_KM; break; } @@ -3090,7 +3133,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_GFORCE: { buff[0] = SYM_GFORCE; - osdFormatCentiNumber(buff + 1, GForce, 0, 2, 0, 3); + osdFormatCentiNumber(buff + 1, GForce, 0, 2, 0, 3, false); if (GForce > osdConfig()->gforce_alarm * 100) { TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } @@ -3103,7 +3146,7 @@ static bool osdDrawSingleElement(uint8_t item) { float GForceValue = GForceAxis[item - OSD_GFORCE_X]; buff[0] = SYM_GFORCE_X + item - OSD_GFORCE_X; - osdFormatCentiNumber(buff + 1, GForceValue, 0, 2, 0, 4); + osdFormatCentiNumber(buff + 1, GForceValue, 0, 2, 0, 4, false); if ((GForceValue < osdConfig()->gforce_axis_alarm_min * 100) || (GForceValue > osdConfig()->gforce_axis_alarm_max * 100)) { TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } @@ -3279,7 +3322,7 @@ static bool osdDrawSingleElement(uint8_t item) } buff[0] = SYM_SCALE; if (osdMapData.scale > 0) { - bool scaled = osdFormatCentiNumber(&buff[1], osdMapData.scale * scaleToUnit, scaleUnitDivisor, maxDecimals, 2, 3); + bool scaled = osdFormatCentiNumber(&buff[1], osdMapData.scale * scaleToUnit, scaleUnitDivisor, maxDecimals, 2, 3, false); buff[4] = scaled ? symScaled : symUnscaled; // Make sure this is cleared if the map stops being drawn osdMapData.scale = 0; @@ -3448,14 +3491,14 @@ static bool osdDrawSingleElement(uint8_t item) #ifdef USE_POWER_LIMITS case OSD_PLIMIT_REMAINING_BURST_TIME: - osdFormatCentiNumber(buff, powerLimiterGetRemainingBurstTime() * 100, 0, 1, 0, 3); + osdFormatCentiNumber(buff, powerLimiterGetRemainingBurstTime() * 100, 0, 1, 0, 3, false); buff[3] = 'S'; buff[4] = '\0'; break; case OSD_PLIMIT_ACTIVE_CURRENT_LIMIT: if (currentBatteryProfile->powerLimits.continuousCurrent) { - osdFormatCentiNumber(buff, powerLimiterGetActiveCurrentLimit(), 0, 2, 0, 3); + osdFormatCentiNumber(buff, powerLimiterGetActiveCurrentLimit(), 0, 2, 0, 3, false); buff[3] = SYM_AMP; buff[4] = '\0'; @@ -3469,7 +3512,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_PLIMIT_ACTIVE_POWER_LIMIT: { if (currentBatteryProfile->powerLimits.continuousPower) { - bool kiloWatt = osdFormatCentiNumber(buff, powerLimiterGetActivePowerLimit(), 1000, 2, 2, 3); + bool kiloWatt = osdFormatCentiNumber(buff, powerLimiterGetActivePowerLimit(), 1000, 2, 2, 3, false); buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT; buff[4] = '\0'; @@ -3711,6 +3754,9 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .system_msg_display_time = SETTING_OSD_SYSTEM_MSG_DISPLAY_TIME_DEFAULT, .units = SETTING_OSD_UNITS_DEFAULT, .main_voltage_decimals = SETTING_OSD_MAIN_VOLTAGE_DECIMALS_DEFAULT, + .use_pilot_logo = SETTING_OSD_USE_PILOT_LOGO_DEFAULT, + .inav_to_pilot_logo_spacing = SETTING_OSD_INAV_TO_PILOT_LOGO_SPACING_DEFAULT, + .arm_screen_display_time = SETTING_OSD_ARM_SCREEN_DISPLAY_TIME_DEFAULT, #ifdef USE_WIND_ESTIMATOR .estimations_wind_compensation = SETTING_OSD_ESTIMATIONS_WIND_COMPENSATION_DEFAULT, @@ -3746,6 +3792,7 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig) //line 2 osdLayoutsConfig->item_pos[0][OSD_HOME_DIST] = OSD_POS(1, 1); osdLayoutsConfig->item_pos[0][OSD_TRIP_DIST] = OSD_POS(1, 2); + osdLayoutsConfig->item_pos[0][OSD_ODOMETER] = OSD_POS(1, 3); osdLayoutsConfig->item_pos[0][OSD_MAIN_BATT_CELL_VOLTAGE] = OSD_POS(12, 1); osdLayoutsConfig->item_pos[0][OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE] = OSD_POS(12, 1); osdLayoutsConfig->item_pos[0][OSD_GPS_SPEED] = OSD_POS(23, 1); @@ -3783,6 +3830,7 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig) osdLayoutsConfig->item_pos[0][OSD_CRAFT_NAME] = OSD_POS(20, 2); osdLayoutsConfig->item_pos[0][OSD_PILOT_NAME] = OSD_POS(20, 3); + osdLayoutsConfig->item_pos[0][OSD_PILOT_LOGO] = OSD_POS(20, 3); osdLayoutsConfig->item_pos[0][OSD_VTX_CHANNEL] = OSD_POS(8, 6); #ifdef USE_SERIALRX_CRSF @@ -3905,79 +3953,78 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig) } } -static void osdSetNextRefreshIn(uint32_t timeMs) { - resumeRefreshAt = micros() + timeMs * 1000; - refreshWaitForResumeCmdRelease = true; -} +/** + * @brief Draws the INAV and/or pilot logos on the display + * + * @param singular If true, only one logo will be drawn. If false, both logos will be drawn, separated by osdConfig()->inav_to_pilot_logo_spacing characters + * @param row The row number to start drawing the logos. If not singular, both logos are drawn on the same rows. + * @return uint8_t The row number after the logo(s). + */ +uint8_t drawLogos(bool singular, uint8_t row) { + uint8_t logoRow = row; + uint8_t logoColOffset = 0; + bool usePilotLogo = (osdConfig()->use_pilot_logo && osdDisplayIsHD()); + +#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is in use, the pilot logo cannot be used, due to font issues. + if (isBfCompatibleVideoSystem(osdConfig())) + usePilotLogo = false; +#endif -static void osdCompleteAsyncInitialization(void) -{ - if (!displayIsReady(osdDisplayPort)) { - // Update the display. - // XXX: Rename displayDrawScreen() and associated functions - // to displayUpdate() - displayDrawScreen(osdDisplayPort); - return; - } + uint8_t logoSpacing = osdConfig()->inav_to_pilot_logo_spacing; - osdDisplayIsReady = true; + if (logoSpacing > 0 && ((osdDisplayPort->cols % 2) != (logoSpacing % 2))) { + logoSpacing++; // Add extra 1 character space between logos, if the odd/even of the OSD cols doesn't match the odd/even of the logo spacing + } -#if defined(USE_CANVAS) - if (osdConfig()->force_grid) { - osdDisplayHasCanvas = false; + // Draw Logo(s) + if (usePilotLogo && !singular) { + logoColOffset = ((osdDisplayPort->cols - (SYM_LOGO_WIDTH * 2)) - logoSpacing) / 2; } else { - osdDisplayHasCanvas = displayGetCanvas(&osdCanvas, osdDisplayPort); + logoColOffset = floorf((osdDisplayPort->cols - SYM_LOGO_WIDTH) / 2.0f); } -#endif - - displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING); - displayClearScreen(osdDisplayPort); - uint8_t y = 1; - displayFontMetadata_t metadata; - bool fontHasMetadata = displayGetFontMetadata(&metadata, osdDisplayPort); - LOG_DEBUG(OSD, "Font metadata version %s: %u (%u chars)", - fontHasMetadata ? "Y" : "N", metadata.version, metadata.charCount); - - if (fontHasMetadata && metadata.charCount > 256) { - hasExtendedFont = true; + // Draw INAV logo + if ((singular && !usePilotLogo) || !singular) { unsigned logo_c = SYM_LOGO_START; - unsigned logo_x = OSD_CENTER_LEN(SYM_LOGO_WIDTH); - for (unsigned ii = 0; ii < SYM_LOGO_HEIGHT; ii++) { - for (unsigned jj = 0; jj < SYM_LOGO_WIDTH; jj++) { - displayWriteChar(osdDisplayPort, logo_x + jj, y, logo_c++); + uint8_t logo_x = logoColOffset; + for (uint8_t lRow = 0; lRow < SYM_LOGO_HEIGHT; lRow++) { + for (uint8_t lCol = 0; lCol < SYM_LOGO_WIDTH; lCol++) { + displayWriteChar(osdDisplayPort, logo_x + lCol, logoRow, logo_c++); } - y++; + logoRow++; } - y++; - } else if (!fontHasMetadata) { - const char *m = "INVALID FONT"; - displayWrite(osdDisplayPort, OSD_CENTER_S(m), 3, m); - y = 4; } - if (fontHasMetadata && metadata.version < OSD_MIN_FONT_VERSION) { - const char *m = "INVALID FONT VERSION"; - displayWrite(osdDisplayPort, OSD_CENTER_S(m), y++, m); + // Draw the pilot logo + if (usePilotLogo) { + unsigned logo_c = SYM_PILOT_LOGO_LRG_START; + uint8_t logo_x = 0; + logoRow = row; + if (singular) { + logo_x = logoColOffset; + } else { + logo_x = logoColOffset + SYM_LOGO_WIDTH + logoSpacing; + } + + for (uint8_t lRow = 0; lRow < SYM_LOGO_HEIGHT; lRow++) { + for (uint8_t lCol = 0; lCol < SYM_LOGO_WIDTH; lCol++) { + displayWriteChar(osdDisplayPort, logo_x + lCol, logoRow, logo_c++); + } + logoRow++; + } } - char string_buffer[30]; - tfp_sprintf(string_buffer, "INAV VERSION: %s", FC_VERSION_STRING); - uint8_t xPos = osdDisplayIsHD() ? 15 : 5; - displayWrite(osdDisplayPort, xPos, y++, string_buffer); -#ifdef USE_CMS - displayWrite(osdDisplayPort, xPos+2, y++, CMS_STARTUP_HELP_TEXT1); - displayWrite(osdDisplayPort, xPos+6, y++, CMS_STARTUP_HELP_TEXT2); - displayWrite(osdDisplayPort, xPos+6, y++, CMS_STARTUP_HELP_TEXT3); -#endif -#ifdef USE_STATS - + return logoRow; +} - uint8_t statNameX = osdDisplayIsHD() ? 14 : 4; - uint8_t statValueX = osdDisplayIsHD() ? 34 : 24; +uint8_t drawStats(uint8_t row) { +#ifdef USE_STATS + char string_buffer[30]; + uint8_t statNameX = (osdDisplayPort->cols - 22) / 2; + uint8_t statValueX = statNameX + 21; if (statsConfig()->stats_enabled) { - displayWrite(osdDisplayPort, statNameX, ++y, "ODOMETER:"); + displayWrite(osdDisplayPort, statNameX, row, "ODOMETER:"); switch (osdConfig()->units) { case OSD_UNIT_UK: FALLTHROUGH; @@ -3998,39 +4045,39 @@ static void osdCompleteAsyncInitialization(void) break; } string_buffer[6] = '\0'; - displayWrite(osdDisplayPort, statValueX-5, y, string_buffer); + displayWrite(osdDisplayPort, statValueX-5, row, string_buffer); - displayWrite(osdDisplayPort, statNameX, ++y, "TOTAL TIME:"); + displayWrite(osdDisplayPort, statNameX, ++row, "TOTAL TIME:"); uint32_t tot_mins = statsConfig()->stats_total_time / 60; - tfp_sprintf(string_buffer, "%2d:%02dHM", (int)(tot_mins / 60), (int)(tot_mins % 60)); - displayWrite(osdDisplayPort, statValueX-5, y, string_buffer); + tfp_sprintf(string_buffer, "%2d:%02dH:M", (int)(tot_mins / 60), (int)(tot_mins % 60)); + displayWrite(osdDisplayPort, statValueX-7, row, string_buffer); #ifdef USE_ADC if (feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER)) { - displayWrite(osdDisplayPort, statNameX, ++y, "TOTAL ENERGY:"); - osdFormatCentiNumber(string_buffer, statsConfig()->stats_total_energy / 10, 0, 2, 0, 4); - strcat(string_buffer, "\xAB"); // SYM_WH - displayWrite(osdDisplayPort, statValueX-4, y, string_buffer); + displayWrite(osdDisplayPort, statNameX, ++row, "TOTAL ENERGY:"); + osdFormatCentiNumber(string_buffer, statsConfig()->stats_total_energy / 10, 0, 2, 0, 4, false); + displayWrite(osdDisplayPort, statValueX-4, row, string_buffer); + displayWriteChar(osdDisplayPort, statValueX, row, SYM_WH); - displayWrite(osdDisplayPort, statNameX, ++y, "AVG EFFICIENCY:"); + displayWrite(osdDisplayPort, statNameX, ++row, "AVG EFFICIENCY:"); if (statsConfig()->stats_total_dist) { uint32_t avg_efficiency = statsConfig()->stats_total_energy / (statsConfig()->stats_total_dist / METERS_PER_KILOMETER); // mWh/km switch (osdConfig()->units) { case OSD_UNIT_UK: FALLTHROUGH; case OSD_UNIT_IMPERIAL: - osdFormatCentiNumber(string_buffer, avg_efficiency / 10, 0, 2, 0, 3); + osdFormatCentiNumber(string_buffer, avg_efficiency / 10, 0, 2, 0, 3, false); string_buffer[3] = SYM_WH_MI; break; case OSD_UNIT_GA: - osdFormatCentiNumber(string_buffer, avg_efficiency / 10, 0, 2, 0, 3); + osdFormatCentiNumber(string_buffer, avg_efficiency / 10, 0, 2, 0, 3, false); string_buffer[3] = SYM_WH_NM; break; default: case OSD_UNIT_METRIC_MPH: FALLTHROUGH; case OSD_UNIT_METRIC: - osdFormatCentiNumber(string_buffer, avg_efficiency / 10000 * METERS_PER_MILE, 0, 2, 0, 3); + osdFormatCentiNumber(string_buffer, avg_efficiency / 10000 * METERS_PER_MILE, 0, 2, 0, 3, false); string_buffer[3] = SYM_WH_KM; break; } @@ -4038,10 +4085,75 @@ static void osdCompleteAsyncInitialization(void) string_buffer[0] = string_buffer[1] = string_buffer[2] = '-'; } string_buffer[4] = '\0'; - displayWrite(osdDisplayPort, statValueX-3, y, string_buffer); + displayWrite(osdDisplayPort, statValueX-3, row++, string_buffer); } #endif // USE_ADC } +#endif // USE_STATS + return row; +} + +static void osdSetNextRefreshIn(uint32_t timeMs) { + resumeRefreshAt = micros() + timeMs * 1000; + refreshWaitForResumeCmdRelease = true; +} + +static void osdCompleteAsyncInitialization(void) +{ + if (!displayIsReady(osdDisplayPort)) { + // Update the display. + // XXX: Rename displayDrawScreen() and associated functions + // to displayUpdate() + displayDrawScreen(osdDisplayPort); + return; + } + + osdDisplayIsReady = true; + +#if defined(USE_CANVAS) + if (osdConfig()->force_grid) { + osdDisplayHasCanvas = false; + } else { + osdDisplayHasCanvas = displayGetCanvas(&osdCanvas, osdDisplayPort); + } +#endif + + displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING); + displayClearScreen(osdDisplayPort); + + uint8_t y = 1; + displayFontMetadata_t metadata; + bool fontHasMetadata = displayGetFontMetadata(&metadata, osdDisplayPort); + LOG_DEBUG(OSD, "Font metadata version %s: %u (%u chars)", + fontHasMetadata ? "Y" : "N", metadata.version, metadata.charCount); + + if (fontHasMetadata && metadata.charCount > 256) { + hasExtendedFont = true; + + y = drawLogos(false, y); + y++; + } else if (!fontHasMetadata) { + const char *m = "INVALID FONT"; + displayWrite(osdDisplayPort, OSD_CENTER_S(m), y++, m); + } + + if (fontHasMetadata && metadata.version < OSD_MIN_FONT_VERSION) { + const char *m = "INVALID FONT VERSION"; + displayWrite(osdDisplayPort, OSD_CENTER_S(m), y++, m); + } + + char string_buffer[30]; + tfp_sprintf(string_buffer, "INAV VERSION: %s", FC_VERSION_STRING); + uint8_t xPos = (osdDisplayPort->cols - 19) / 2; // Automatically centre, regardless of resolution. In the case of odd number screens, bias to the left. + displayWrite(osdDisplayPort, xPos, y++, string_buffer); +#ifdef USE_CMS + displayWrite(osdDisplayPort, xPos+2, y++, CMS_STARTUP_HELP_TEXT1); + displayWrite(osdDisplayPort, xPos+6, y++, CMS_STARTUP_HELP_TEXT2); + displayWrite(osdDisplayPort, xPos+6, y++, CMS_STARTUP_HELP_TEXT3); +#endif + +#ifdef USE_STATS + y = drawStats(++y); #endif displayCommitTransaction(osdDisplayPort); @@ -4245,22 +4357,22 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) if (isSinglePageStatsCompatible || page == 1) { if (osdConfig()->stats_min_voltage_unit == OSD_STATS_MIN_VOLTAGE_UNIT_BATTERY) { displayWrite(osdDisplayPort, statNameX, top, "MIN BATTERY VOLT :"); - osdFormatCentiNumber(buff, stats.min_voltage, 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2); + osdFormatCentiNumber(buff, stats.min_voltage, 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2, false); } else { displayWrite(osdDisplayPort, statNameX, top, "MIN CELL VOLTAGE :"); - osdFormatCentiNumber(buff, stats.min_voltage/getBatteryCellCount(), 0, 2, 0, 3); + osdFormatCentiNumber(buff, stats.min_voltage/getBatteryCellCount(), 0, 2, 0, 3, false); } tfp_sprintf(buff, "%s%c", buff, SYM_VOLT); displayWrite(osdDisplayPort, statValuesX, top++, buff); if (feature(FEATURE_CURRENT_METER)) { displayWrite(osdDisplayPort, statNameX, top, "MAX CURRENT :"); - osdFormatCentiNumber(buff, stats.max_current, 0, 2, 0, 3); + osdFormatCentiNumber(buff, stats.max_current, 0, 2, 0, 3, false); tfp_sprintf(buff, "%s%c", buff, SYM_AMP); displayWrite(osdDisplayPort, statValuesX, top++, buff); displayWrite(osdDisplayPort, statNameX, top, "MAX POWER :"); - bool kiloWatt = osdFormatCentiNumber(buff, stats.max_power, 1000, 2, 2, 3); + bool kiloWatt = osdFormatCentiNumber(buff, stats.max_power, 1000, 2, 2, 3, false); buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT; buff[4] = '\0'; displayWrite(osdDisplayPort, statValuesX, top++, buff); @@ -4269,7 +4381,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { tfp_sprintf(buff, "%d%c", (int)getMAhDrawn(), SYM_MAH); } else { - osdFormatCentiNumber(buff, getMWhDrawn() / 10, 0, 2, 0, 3); + osdFormatCentiNumber(buff, getMWhDrawn() / 10, 0, 2, 0, 3, false); tfp_sprintf(buff, "%s%c", buff, SYM_WH); } displayWrite(osdDisplayPort, statValuesX, top++, buff); @@ -4291,7 +4403,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) FALLTHROUGH; case OSD_UNIT_IMPERIAL: if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { - moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_MILE / totalDistance), 1000, 0, 2, digits); + moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_MILE / totalDistance), 1000, 0, 2, digits, false); if (!moreThanAh) { tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_MI_0, SYM_MAH_MI_1); } else { @@ -4304,7 +4416,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) buff[5] = '\0'; } } else { - osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_MILE / totalDistance), 0, 2, 0, digits); + osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_MILE / totalDistance), 0, 2, 0, digits, false); tfp_sprintf(buff, "%s%c", buff, SYM_WH_MI); if (!efficiencyValid) { buff[0] = buff[1] = buff[2] = '-'; @@ -4313,7 +4425,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) break; case OSD_UNIT_GA: if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { - moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_NAUTICALMILE / totalDistance), 1000, 0, 2, digits); + moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_NAUTICALMILE / totalDistance), 1000, 0, 2, digits, false); if (!moreThanAh) { tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_NM_0, SYM_MAH_NM_1); } else { @@ -4326,7 +4438,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) buff[5] = '\0'; } } else { - osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_NAUTICALMILE / totalDistance), 0, 2, 0, digits); + osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_NAUTICALMILE / totalDistance), 0, 2, 0, digits, false); tfp_sprintf(buff, "%s%c", buff, SYM_WH_NM); if (!efficiencyValid) { buff[0] = buff[1] = buff[2] = '-'; @@ -4337,7 +4449,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) FALLTHROUGH; case OSD_UNIT_METRIC: if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { - moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000000.0f / totalDistance), 1000, 0, 2, digits); + moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000000.0f / totalDistance), 1000, 0, 2, digits, false); if (!moreThanAh) { tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_KM_0, SYM_MAH_KM_1); } else { @@ -4350,7 +4462,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) buff[5] = '\0'; } } else { - osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10000.0f / totalDistance), 0, 2, 0, digits); + osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10000.0f / totalDistance), 0, 2, 0, digits, false); tfp_sprintf(buff, "%s%c", buff, SYM_WH_KM); if (!efficiencyValid) { buff[0] = buff[1] = buff[2] = '-'; @@ -4365,19 +4477,19 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) const float max_gforce = accGetMeasuredMaxG(); displayWrite(osdDisplayPort, statNameX, top, "MAX G-FORCE :"); - osdFormatCentiNumber(buff, max_gforce * 100, 0, 2, 0, 3); + osdFormatCentiNumber(buff, max_gforce * 100, 0, 2, 0, 3, false); displayWrite(osdDisplayPort, statValuesX, top++, buff); const acc_extremes_t *acc_extremes = accGetMeasuredExtremes(); const float acc_extremes_min = acc_extremes[Z].min; const float acc_extremes_max = acc_extremes[Z].max; displayWrite(osdDisplayPort, statNameX, top, "MIN/MAX Z G-FORCE:"); - osdFormatCentiNumber(buff, acc_extremes_min * 100, 0, 2, 0, 4); + osdFormatCentiNumber(buff, acc_extremes_min * 100, 0, 2, 0, 4, false); osdLeftAlignString(buff); strcat(osdFormatTrimWhiteSpace(buff),"/"); multiValueLengthOffset = strlen(buff); displayWrite(osdDisplayPort, statValuesX, top, buff); - osdFormatCentiNumber(buff, acc_extremes_max * 100, 0, 2, 0, 3); + osdFormatCentiNumber(buff, acc_extremes_max * 100, 0, 2, 0, 3, false); osdLeftAlignString(buff); displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff); } @@ -4395,85 +4507,205 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) displayCommitTransaction(osdDisplayPort); } -// called when motors armed -static void osdShowArmed(void) +// HD arming screen. based on the minimum HD OSD grid size of 50 x 18 +static void osdShowHDArmScreen(void) { - dateTime_t dt; - char buf[MAX(32, FORMATTED_DATE_TIME_BUFSIZE)]; - char craftNameBuf[MAX_NAME_LENGTH]; - char versionBuf[30]; - char *date; - char *time; - // We need 12 visible rows, start row never < first fully visible row 1 - uint8_t y = osdDisplayPort->rows > 13 ? (osdDisplayPort->rows - 12) / 2 : 1; + dateTime_t dt; + char buf[MAX(osdDisplayPort->cols, FORMATTED_DATE_TIME_BUFSIZE)]; + char buf2[MAX(osdDisplayPort->cols, FORMATTED_DATE_TIME_BUFSIZE)]; + char craftNameBuf[MAX_NAME_LENGTH]; + char versionBuf[osdDisplayPort->cols]; + uint8_t safehomeRow = 0; + uint8_t armScreenRow = 2; // Start at row 2 - displayClearScreen(osdDisplayPort); - strcpy(buf, "ARMED"); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf); - y += 2; + armScreenRow = drawLogos(false, armScreenRow); + armScreenRow++; if (strlen(systemConfig()->craftName) > 0) { osdFormatCraftName(craftNameBuf); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(systemConfig()->craftName)) / 2, y, craftNameBuf ); - y += 1; + strcpy(buf2, "ARMED!"); + tfp_sprintf(buf, "%s - %s", craftNameBuf, buf2); + } else { + strcpy(buf, "ARMED!"); + } + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); +#if defined(USE_GPS) +#if defined (USE_SAFE_HOME) + if (posControl.safehomeState.distance) { + safehomeRow = armScreenRow; + armScreenRow++; } +#endif // USE_SAFE_HOME +#endif // USE_GPS + armScreenRow++; + if (posControl.waypointListValid && posControl.waypointCount > 0) { #ifdef USE_MULTI_MISSION tfp_sprintf(buf, "MISSION %u/%u (%u WP)", posControl.loadedMultiMissionIndex, posControl.multiMissionCount, posControl.waypointCount); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); #else strcpy(buf, "*MISSION LOADED*"); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); #endif } - y += 1; + armScreenRow++; #if defined(USE_GPS) if (feature(FEATURE_GPS)) { if (STATE(GPS_FIX_HOME)) { - if (osdConfig()->osd_home_position_arm_screen){ + if (osdConfig()->osd_home_position_arm_screen) { osdFormatCoordinate(buf, SYM_LAT, GPS_home.lat); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf); - osdFormatCoordinate(buf, SYM_LON, GPS_home.lon); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y + 1, buf); + osdFormatCoordinate(buf2, SYM_LON, GPS_home.lon); + uint8_t gap = 1; + uint8_t col = strlen(buf) + strlen(buf2) + gap; + + if ((osdDisplayPort->cols %2) != (col %2)) { + gap++; + col++; + } + + col = (osdDisplayPort->cols - col) / 2; + + displayWrite(osdDisplayPort, col, armScreenRow, buf); + displayWrite(osdDisplayPort, col + strlen(buf) + gap, armScreenRow++, buf2); + int digits = osdConfig()->plus_code_digits; olc_encode(GPS_home.lat, GPS_home.lon, digits, buf, sizeof(buf)); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y + 2, buf); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); } - y += 4; + #if defined (USE_SAFE_HOME) if (posControl.safehomeState.distance) { // safehome found during arming if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF) { strcpy(buf, "SAFEHOME FOUND; MODE OFF"); - } else { - char buf2[12]; // format the distance first - osdFormatDistanceStr(buf2, posControl.safehomeState.distance); - tfp_sprintf(buf, "%c - %s -> SAFEHOME %u", SYM_HOME, buf2, posControl.safehomeState.index); - } - textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT; - // write this message above the ARMED message to make it obvious - displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y - 8, buf, elemAttr); + } else { + osdFormatDistanceStr(buf2, posControl.safehomeState.distance); + tfp_sprintf(buf, "%c SAFEHOME %u @ %s", SYM_HOME, posControl.safehomeState.index, buf2); + } + textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT; + // write this message below the ARMED message to make it obvious + displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, safehomeRow, buf, elemAttr); } #endif } else { strcpy(buf, "!NO HOME POSITION!"); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf); - y += 1; + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); } + armScreenRow++; } #endif - if (rtcGetDateTime(&dt)) { - dateTimeFormatLocal(buf, &dt); - dateTimeSplitFormatted(buf, &date, &time); + if (rtcGetDateTimeLocal(&dt)) { + tfp_sprintf(buf, "%04u-%02u-%02u %02u:%02u:%02u", dt.year, dt.month, dt.day, dt.hours, dt.minutes, dt.seconds); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + armScreenRow++; + } - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(date)) / 2, y, date); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(time)) / 2, y + 1, time); - y += 3; + tfp_sprintf(versionBuf, "INAV VERSION: %s", FC_VERSION_STRING); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(versionBuf)) / 2, armScreenRow++, versionBuf); + armScreenRow++; + +#ifdef USE_STATS + if (armScreenRow < (osdDisplayPort->rows - 4)) + armScreenRow = drawStats(armScreenRow); +#endif // USE_STATS +} + +static void osdShowSDArmScreen(void) +{ + dateTime_t dt; + char buf[MAX(osdDisplayPort->cols, FORMATTED_DATE_TIME_BUFSIZE)]; + char buf2[MAX(osdDisplayPort->cols, FORMATTED_DATE_TIME_BUFSIZE)]; + char craftNameBuf[MAX_NAME_LENGTH]; + char versionBuf[osdDisplayPort->cols]; + // We need 12 visible rows, start row never < first fully visible row 1 + uint8_t armScreenRow = osdDisplayPort->rows > 13 ? (osdDisplayPort->rows - 12) / 2 : 1; + uint8_t safehomeRow = 0; + + strcpy(buf, "ARMED!"); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + safehomeRow = armScreenRow; + armScreenRow++; + + if (strlen(systemConfig()->craftName) > 0) { + osdFormatCraftName(craftNameBuf); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(systemConfig()->craftName)) / 2, armScreenRow++, craftNameBuf ); + } + + if (posControl.waypointListValid && posControl.waypointCount > 0) { +#ifdef USE_MULTI_MISSION + tfp_sprintf(buf, "MISSION %u/%u (%u WP)", posControl.loadedMultiMissionIndex, posControl.multiMissionCount, posControl.waypointCount); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow, buf); +#else + strcpy(buf, "*MISSION LOADED*"); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow, buf); +#endif + } + armScreenRow++; + +#if defined(USE_GPS) + if (feature(FEATURE_GPS)) { + if (STATE(GPS_FIX_HOME)) { + if (osdConfig()->osd_home_position_arm_screen) { + osdFormatCoordinate(buf, SYM_LAT, GPS_home.lat); + osdFormatCoordinate(buf2, SYM_LON, GPS_home.lon); + + uint8_t gpsStartCol = (osdDisplayPort->cols - (strlen(buf) + strlen(buf2) + 2)) / 2; + displayWrite(osdDisplayPort, gpsStartCol, armScreenRow, buf); + displayWrite(osdDisplayPort, gpsStartCol + strlen(buf) + 2, armScreenRow++, buf2); + + int digits = osdConfig()->plus_code_digits; + olc_encode(GPS_home.lat, GPS_home.lon, digits, buf, sizeof(buf)); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + } + +#if defined (USE_SAFE_HOME) + if (posControl.safehomeState.distance) { // safehome found during arming + if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF) { + strcpy(buf, "SAFEHOME FOUND; MODE OFF"); + } else { + osdFormatDistanceStr(buf2, posControl.safehomeState.distance); + tfp_sprintf(buf, "%c SAFEHOME %u @ %s", SYM_HOME, posControl.safehomeState.index, buf2); + } + textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT; + // write this message below the ARMED message to make it obvious + displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, safehomeRow, buf, elemAttr); + } +#endif + } else { + strcpy(buf, "!NO HOME POSITION!"); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + } + armScreenRow++; + } +#endif + + if (rtcGetDateTimeLocal(&dt)) { + tfp_sprintf(buf, "%04u-%02u-%02u %02u:%02u:%02u", dt.year, dt.month, dt.day, dt.hours, dt.minutes, dt.seconds); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + armScreenRow++; } tfp_sprintf(versionBuf, "INAV VERSION: %s", FC_VERSION_STRING); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(versionBuf)) / 2, y, versionBuf); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(versionBuf)) / 2, armScreenRow++, versionBuf); + armScreenRow++; + +#ifdef USE_STATS + if (armScreenRow < (osdDisplayPort->rows - 4)) + armScreenRow = drawStats(armScreenRow); +#endif // USE_STATS +} + +// called when motors armed +static void osdShowArmed(void) +{ + displayClearScreen(osdDisplayPort); + + if (osdDisplayIsHD()) { + osdShowHDArmScreen(); + } else { + osdShowSDArmScreen(); + } } static void osdFilterData(timeUs_t currentTimeUs) { @@ -4574,10 +4806,10 @@ static void osdRefresh(timeUs_t currentTimeUs) statsDisplayed = false; osdResetStats(); osdShowArmed(); - uint32_t delay = ARMED_SCREEN_DISPLAY_TIME; + uint32_t delay = osdConfig()->arm_screen_display_time; #if defined(USE_SAFE_HOME) if (posControl.safehomeState.distance) - delay *= 3; + delay+= 3000; #endif osdSetNextRefreshIn(delay); } else { @@ -4905,6 +5137,16 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter // by OSD_FLYMODE. messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ALTITUDE_HOLD); } + if (STATE(MULTIROTOR) && FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { + if (posControl.cruise.multicopterSpeed >= 50.0f) { + char buf[6]; + osdFormatVelocityStr(buf, posControl.cruise.multicopterSpeed, false, false); + tfp_sprintf(messageBuf, "(SPD %s)", buf); + } else { + strcpy(messageBuf, "(HOLD)"); + } + messages[messageCount++] = messageBuf; + } if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM) && !feature(FEATURE_FW_AUTOTRIM)) { messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTRIM); } @@ -5184,4 +5426,5 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff) return elemAttr; } + #endif // OSD diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 04794c6cec2..a45df654fcc 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -274,15 +274,17 @@ typedef enum { OSD_PILOT_NAME, OSD_PAN_SERVO_CENTRED, OSD_MULTI_FUNCTION, + OSD_ODOMETER, + OSD_PILOT_LOGO, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; typedef enum { OSD_UNIT_IMPERIAL, OSD_UNIT_METRIC, - OSD_UNIT_METRIC_MPH, // Old UK units, all metric except speed in mph - OSD_UNIT_UK, // Show everything in imperial, temperature in C - OSD_UNIT_GA, // General Aviation: Knots, Nautical Miles, Feet, Degrees C + OSD_UNIT_METRIC_MPH, // Old UK units, all metric except speed in mph + OSD_UNIT_UK, // Show everything in imperial, temperature in C + OSD_UNIT_GA, // General Aviation: Knots, Nautical Miles, Feet, Degrees C OSD_UNIT_MAX = OSD_UNIT_GA, } osd_unit_e; @@ -343,111 +345,112 @@ PG_DECLARE(osdLayoutsConfig_t, osdLayoutsConfig); typedef struct osdConfig_s { // Alarms - uint8_t rssi_alarm; // rssi % - uint16_t time_alarm; // fly minutes - uint16_t alt_alarm; // positive altitude in m - uint16_t dist_alarm; // home distance in m - uint16_t neg_alt_alarm; // abs(negative altitude) in m - uint8_t current_alarm; // current consumption in A - int16_t imu_temp_alarm_min; - int16_t imu_temp_alarm_max; - int16_t esc_temp_alarm_min; - int16_t esc_temp_alarm_max; - float gforce_alarm; - float gforce_axis_alarm_min; - float gforce_axis_alarm_max; + uint8_t rssi_alarm; // rssi % + uint16_t time_alarm; // fly minutes + uint16_t alt_alarm; // positive altitude in m + uint16_t dist_alarm; // home distance in m + uint16_t neg_alt_alarm; // abs(negative altitude) in m + uint8_t current_alarm; // current consumption in A + int16_t imu_temp_alarm_min; + int16_t imu_temp_alarm_max; + int16_t esc_temp_alarm_min; + int16_t esc_temp_alarm_max; + float gforce_alarm; + float gforce_axis_alarm_min; + float gforce_axis_alarm_max; #ifdef USE_SERIALRX_CRSF - int8_t snr_alarm; //CRSF SNR alarm in dB - int8_t link_quality_alarm; - int16_t rssi_dbm_alarm; // in dBm - int16_t rssi_dbm_max; // Perfect RSSI. Set to High end of curve. RSSI at 100% - int16_t rssi_dbm_min; // Worst RSSI. Set to low end of curve or RX sensitivity level. RSSI at 0% + int8_t snr_alarm; //CRSF SNR alarm in dB + int8_t link_quality_alarm; + int16_t rssi_dbm_alarm; // in dBm + int16_t rssi_dbm_max; // Perfect RSSI. Set to High end of curve. RSSI at 100% + int16_t rssi_dbm_min; // Worst RSSI. Set to low end of curve or RX sensitivity level. RSSI at 0% #endif #ifdef USE_BARO - int16_t baro_temp_alarm_min; - int16_t baro_temp_alarm_max; + int16_t baro_temp_alarm_min; + int16_t baro_temp_alarm_max; #endif #ifdef USE_TEMPERATURE_SENSOR osd_alignment_e temp_label_align; #endif #ifdef USE_PITOT - float airspeed_alarm_min; - float airspeed_alarm_max; + float airspeed_alarm_min; + float airspeed_alarm_max; #endif - videoSystem_e video_system; - uint8_t row_shiftdown; - int16_t msp_displayport_fullframe_interval; + videoSystem_e video_system; + uint8_t row_shiftdown; + int16_t msp_displayport_fullframe_interval; // Preferences - uint8_t main_voltage_decimals; - uint8_t ahi_reverse_roll; - uint8_t ahi_max_pitch; - uint8_t crosshairs_style; // from osd_crosshairs_style_e - int8_t horizon_offset; - int8_t camera_uptilt; - bool ahi_camera_uptilt_comp; - uint8_t camera_fov_h; - uint8_t camera_fov_v; - uint8_t hud_margin_h; - uint8_t hud_margin_v; - bool hud_homing; - bool hud_homepoint; - uint8_t hud_radar_disp; - uint16_t hud_radar_range_min; - uint16_t hud_radar_range_max; - uint8_t hud_radar_alt_difference_display_time; - uint8_t hud_radar_distance_display_time; - uint8_t hud_wp_disp; - - uint8_t left_sidebar_scroll; // from osd_sidebar_scroll_e - uint8_t right_sidebar_scroll; // from osd_sidebar_scroll_e - uint8_t sidebar_scroll_arrows; - - uint8_t units; // from osd_unit_e - uint8_t stats_energy_unit; // from osd_stats_energy_unit_e - uint8_t stats_min_voltage_unit; // from osd_stats_min_voltage_unit_e - uint8_t stats_page_auto_swap_time; // stats page auto swap interval time (seconds) + uint8_t main_voltage_decimals; + uint8_t ahi_reverse_roll; + uint8_t ahi_max_pitch; + uint8_t crosshairs_style; // from osd_crosshairs_style_e + int8_t horizon_offset; + int8_t camera_uptilt; + bool ahi_camera_uptilt_comp; + uint8_t camera_fov_h; + uint8_t camera_fov_v; + uint8_t hud_margin_h; + uint8_t hud_margin_v; + bool hud_homing; + bool hud_homepoint; + uint8_t hud_radar_disp; + uint16_t hud_radar_range_min; + uint16_t hud_radar_range_max; + uint8_t hud_radar_alt_difference_display_time; + uint8_t hud_radar_distance_display_time; + uint8_t hud_wp_disp; + + uint8_t left_sidebar_scroll; // from osd_sidebar_scroll_e + uint8_t right_sidebar_scroll; // from osd_sidebar_scroll_e + uint8_t sidebar_scroll_arrows; + + uint8_t units; // from osd_unit_e + uint8_t stats_energy_unit; // from osd_stats_energy_unit_e + uint8_t stats_min_voltage_unit; // from osd_stats_min_voltage_unit_e + uint8_t stats_page_auto_swap_time; // stats page auto swap interval time (seconds) #ifdef USE_WIND_ESTIMATOR - bool estimations_wind_compensation; // use wind compensation for estimated remaining flight/distance -#endif - - uint8_t coordinate_digits; - - bool osd_failsafe_switch_layout; - uint8_t plus_code_digits; // Number of digits to use in OSD_PLUS_CODE - uint8_t plus_code_short; - uint8_t ahi_style; - uint8_t force_grid; // Force a pixel based OSD to use grid mode. - uint8_t ahi_bordered; // Only used by the AHI widget - uint8_t ahi_width; // In pixels, only used by the AHI widget - uint8_t ahi_height; // In pixels, only used by the AHI widget - int8_t ahi_vertical_offset; // Offset from center in pixels. Positive moves the AHI down. Widget only. - int8_t sidebar_horizontal_offset; // Horizontal offset from default position. Units are grid slots for grid OSDs, pixels for pixel based OSDs. Positive values move sidebars closer to the edges. - uint8_t left_sidebar_scroll_step; // How many units each sidebar step represents. 0 means the default value for the scroll type. - uint8_t right_sidebar_scroll_step; // Same as left_sidebar_scroll_step, but for the right sidebar. - bool osd_home_position_arm_screen; - uint8_t pan_servo_index; // Index of the pan servo used for home direction offset - int8_t pan_servo_pwm2centideg; // Centidegrees of servo rotation per us pwm - uint8_t pan_servo_offcentre_warning; // Degrees around the centre, that is assumed camera is wanted to be facing forwards, but isn't centred - bool pan_servo_indicator_show_degrees; // Show the degrees of offset for the pan servo - uint8_t crsf_lq_format; - uint8_t sidebar_height; // sidebar height in rows, 0 turns off sidebars leaving only level indicator arrows - uint8_t telemetry; // use telemetry on displayed pixel line 0 - uint8_t esc_rpm_precision; // Number of characters used for the RPM numbers. - uint16_t system_msg_display_time; // system message display time for multiple messages (ms) - uint8_t mAh_used_precision; // Number of numbers used for mAh drawn. Plently of packs now are > 9999 mAh - uint8_t ahi_pitch_interval; // redraws AHI at set pitch interval (Not pixel OSD) - char osd_switch_indicator0_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 0. - uint8_t osd_switch_indicator0_channel; // RC Channel to use for switch indicator 0. - char osd_switch_indicator1_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 1. - uint8_t osd_switch_indicator1_channel; // RC Channel to use for switch indicator 1. - char osd_switch_indicator2_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 2. - uint8_t osd_switch_indicator2_channel; // RC Channel to use for switch indicator 2. - char osd_switch_indicator3_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 3. - uint8_t osd_switch_indicator3_channel; // RC Channel to use for switch indicator 3. - bool osd_switch_indicators_align_left; // Align switch indicator name to left of the switch. + bool estimations_wind_compensation; // use wind compensation for estimated remaining flight/distance +#endif + uint8_t coordinate_digits; + bool osd_failsafe_switch_layout; + uint8_t plus_code_digits; // Number of digits to use in OSD_PLUS_CODE + uint8_t plus_code_short; + uint8_t ahi_style; + uint8_t force_grid; // Force a pixel based OSD to use grid mode. + uint8_t ahi_bordered; // Only used by the AHI widget + uint8_t ahi_width; // In pixels, only used by the AHI widget + uint8_t ahi_height; // In pixels, only used by the AHI widget + int8_t ahi_vertical_offset; // Offset from center in pixels. Positive moves the AHI down. Widget only. + int8_t sidebar_horizontal_offset; // Horizontal offset from default position. Units are grid slots for grid OSDs, pixels for pixel based OSDs. Positive values move sidebars closer to the edges. + uint8_t left_sidebar_scroll_step; // How many units each sidebar step represents. 0 means the default value for the scroll type. + uint8_t right_sidebar_scroll_step; // Same as left_sidebar_scroll_step, but for the right sidebar. + bool osd_home_position_arm_screen; + uint8_t pan_servo_index; // Index of the pan servo used for home direction offset + int8_t pan_servo_pwm2centideg; // Centidegrees of servo rotation per us pwm + uint8_t pan_servo_offcentre_warning; // Degrees around the centre, that is assumed camera is wanted to be facing forwards, but isn't centred + bool pan_servo_indicator_show_degrees; // Show the degrees of offset for the pan servo + uint8_t crsf_lq_format; + uint8_t sidebar_height; // sidebar height in rows, 0 turns off sidebars leaving only level indicator arrows + uint8_t telemetry; // use telemetry on displayed pixel line 0 + uint8_t esc_rpm_precision; // Number of characters used for the RPM numbers. + uint16_t system_msg_display_time; // system message display time for multiple messages (ms) + uint8_t mAh_used_precision; // Number of numbers used for mAh drawn. Plently of packs now are > 9999 mAh + uint8_t ahi_pitch_interval; // redraws AHI at set pitch interval (Not pixel OSD) + char osd_switch_indicator0_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 0. + uint8_t osd_switch_indicator0_channel; // RC Channel to use for switch indicator 0. + char osd_switch_indicator1_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 1. + uint8_t osd_switch_indicator1_channel; // RC Channel to use for switch indicator 1. + char osd_switch_indicator2_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 2. + uint8_t osd_switch_indicator2_channel; // RC Channel to use for switch indicator 2. + char osd_switch_indicator3_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 3. + uint8_t osd_switch_indicator3_channel; // RC Channel to use for switch indicator 3. + bool osd_switch_indicators_align_left; // Align switch indicator name to left of the switch. + bool use_pilot_logo; // If enabled, the pilot logo (last 40 characters of page 2 font) will be used with the INAV logo. + uint8_t inav_to_pilot_logo_spacing; // The space between the INAV and pilot logos, if pilot logo is used. This number may be adjusted so that it fits the odd/even col width. + uint16_t arm_screen_display_time; // Length of time the arm screen is displayed } osdConfig_t; PG_DECLARE(osdConfig_t, osdConfig); @@ -483,7 +486,7 @@ void osdStartedSaveProcess(void); void osdShowEEPROMSavedNotification(void); void osdCrosshairPosition(uint8_t *x, uint8_t *y); -bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int maxDecimals, int maxScaledDecimals, int length); +bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int maxDecimals, int maxScaledDecimals, int length, bool leadingZeros); void osdFormatAltitudeSymbol(char *buff, int32_t alt); void osdFormatVelocityStr(char* buff, int32_t vel, bool _3D, bool _max); // Returns a heading angle in degrees normalized to [0, 360). diff --git a/src/main/io/osd_hud.c b/src/main/io/osd_hud.c index 2e209fd2826..47cc96f834b 100644 --- a/src/main/io/osd_hud.c +++ b/src/main/io/osd_hud.c @@ -256,18 +256,18 @@ void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitu case OSD_UNIT_IMPERIAL: { if (poiType == 1) { - osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), FEET_PER_MILE, 0, 4, 4); + osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), FEET_PER_MILE, 0, 4, 4, false); } else { - osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), FEET_PER_MILE, 0, 3, 3); + osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), FEET_PER_MILE, 0, 3, 3, false); } } break; case OSD_UNIT_GA: { if (poiType == 1) { - osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), (uint32_t)FEET_PER_NAUTICALMILE, 0, 4, 4); + osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), (uint32_t)FEET_PER_NAUTICALMILE, 0, 4, 4, false); } else { - osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), (uint32_t)FEET_PER_NAUTICALMILE, 0, 3, 3); + osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), (uint32_t)FEET_PER_NAUTICALMILE, 0, 3, 3, false); } } break; @@ -278,9 +278,9 @@ void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitu case OSD_UNIT_METRIC: { if (poiType == 1) { - osdFormatCentiNumber(buff, poiDistance * 100, METERS_PER_KILOMETER, 0, 4, 4); + osdFormatCentiNumber(buff, poiDistance * 100, METERS_PER_KILOMETER, 0, 4, 4, false); } else { - osdFormatCentiNumber(buff, poiDistance * 100, METERS_PER_KILOMETER, 0, 3, 3); + osdFormatCentiNumber(buff, poiDistance * 100, METERS_PER_KILOMETER, 0, 3, 3, false); } } break; diff --git a/src/main/io/osd_utils.c b/src/main/io/osd_utils.c index 194be36f952..6675be8783b 100644 --- a/src/main/io/osd_utils.c +++ b/src/main/io/osd_utils.c @@ -38,7 +38,7 @@ int digitCount(int32_t value) } -bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int maxDecimals, int maxScaledDecimals, int length) +bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int maxDecimals, int maxScaledDecimals, int length, bool leadingZeros) { char *ptr = buff; char *dec; @@ -86,7 +86,11 @@ bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int ma // Done counting. Time to write the characters. // Write spaces at the start while (remaining > 0) { - *ptr = SYM_BLANK; + if (leadingZeros) + *ptr = '0'; + else + *ptr = SYM_BLANK; + ptr++; remaining--; } @@ -98,7 +102,11 @@ bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int ma // Add any needed remaining leading spaces while(rem_spaces > 0) { - *ptr = SYM_BLANK; + if (leadingZeros) + *ptr = '0'; + else + *ptr = SYM_BLANK; + ptr++; remaining--; rem_spaces--; diff --git a/src/main/io/osd_utils.h b/src/main/io/osd_utils.h index 2f9c61a3202..7f10f2bf8f9 100644 --- a/src/main/io/osd_utils.h +++ b/src/main/io/osd_utils.h @@ -33,6 +33,6 @@ int digitCount(int32_t value); * of the same length. If the value doesn't fit into the provided length * it will be divided by scale and true will be returned. */ -bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int maxDecimals, int maxScaledDecimals, int length); +bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int maxDecimals, int maxScaledDecimals, int length, bool leadingZeros); #endif diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 344b84f22f5..0d7f107fa1e 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -31,6 +31,9 @@ #define MSP2_INAV_OUTPUT_MAPPING 0x200A #define MSP2_INAV_MC_BRAKING 0x200B #define MSP2_INAV_SET_MC_BRAKING 0x200C +#define MSP2_INAV_OUTPUT_MAPPING_EXT 0x200D +#define MSP2_INAV_TIMER_OUTPUT_MODE 0x200E +#define MSP2_INAV_SET_TIMER_OUTPUT_MODE 0x200F #define MSP2_INAV_MIXER 0x2010 #define MSP2_INAV_SET_MIXER 0x2011 @@ -89,3 +92,8 @@ #define MSP2_INAV_LED_STRIP_CONFIG_EX 0x2048 #define MSP2_INAV_SET_LED_STRIP_CONFIG_EX 0x2049 +#define MSP2_INAV_RATE_DYNAMICS 0x2060 +#define MSP2_INAV_SET_RATE_DYNAMICS 0x2061 + +#define MSP2_INAV_EZ_TUNE 0x2070 +#define MSP2_INAV_EZ_TUNE_SET 0x2071 diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 17cfe3a5609..69a9c0479a9 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -47,7 +47,7 @@ #include "fc/settings.h" #include "flight/imu.h" -#include "flight/mixer.h" +#include "flight/mixer_profile.h" #include "flight/pid.h" #include "io/beeper.h" @@ -69,6 +69,7 @@ #define MR_RTH_CLIMB_OVERSHOOT_CM 100 // target this amount of cm *above* the target altitude to ensure it is actually reached (Vz > 0 at target alt) #define MR_RTH_CLIMB_MARGIN_MIN_CM 100 // start cruising home this amount of cm *before* reaching the cruise altitude (while continuing the ascend) #define MR_RTH_CLIMB_MARGIN_PERCENT 15 // on high RTH altitudes use even bigger margin - percent of the altitude set +#define MR_RTH_LAND_MARGIN_CM 2000 // pause landing if this amount of cm *before* remaining to the home point (2D distance) // Planes: #define FW_RTH_CLIMB_OVERSHOOT_CM 100 @@ -95,7 +96,7 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 2); #endif -PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 4); +PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 5); PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { @@ -152,6 +153,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .land_detect_sensitivity = SETTING_NAV_LAND_DETECT_SENSITIVITY_DEFAULT, // Changes sensitivity of landing detection .auto_disarm_delay = SETTING_NAV_AUTO_DISARM_DELAY_DEFAULT, // 2000 ms - time delay to disarm when auto disarm after landing enabled .rth_linear_descent_start_distance = SETTING_NAV_RTH_LINEAR_DESCENT_START_DISTANCE_DEFAULT, + .cruise_yaw_rate = SETTING_NAV_CRUISE_YAW_RATE_DEFAULT, // 20dps }, // MC-specific @@ -207,7 +209,6 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .launch_manual_throttle = SETTING_NAV_FW_LAUNCH_MANUAL_THROTTLE_DEFAULT,// OFF .launch_abort_deadband = SETTING_NAV_FW_LAUNCH_ABORT_DEADBAND_DEFAULT, // 100 us - .cruise_yaw_rate = SETTING_NAV_FW_CRUISE_YAW_RATE_DEFAULT, // 20dps .allow_manual_thr_increase = SETTING_NAV_FW_ALLOW_MANUAL_THR_INCREASE_DEFAULT, .useFwNavYawControl = SETTING_NAV_USE_FW_YAW_CONTROL_DEFAULT, .yawControlDeadband = SETTING_NAV_FW_YAW_DEADBAND_DEFAULT, @@ -308,6 +309,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINIS static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_INITIALIZE(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_WAIT(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_IN_PROGRESS(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_INITIALIZE(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_ABORT(navigationFSMState_t previousState); static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { /** Idle state ******************************************************/ @@ -328,6 +332,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_LAUNCH] = NAV_STATE_LAUNCH_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -422,7 +427,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_COURSE_HOLD_IN_PROGRESS, .onEntry = navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS, .timeoutMs = 10, - .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW, + .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_RC_POS | NAV_RC_YAW, .mapToFlightModes = NAV_COURSE_HOLD_MODE, .mwState = MW_NAV_STATE_NONE, .mwError = MW_NAV_ERROR_NONE, @@ -481,7 +486,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_CRUISE_IN_PROGRESS, .onEntry = navOnEnteringState_NAV_STATE_CRUISE_IN_PROGRESS, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT, .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_COURSE_HOLD_MODE, .mwState = MW_NAV_STATE_NONE, .mwError = MW_NAV_ERROR_NONE, @@ -596,6 +601,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -654,6 +660,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -807,6 +814,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -941,6 +949,52 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, } }, + + /** MIXER AUTOMATED TRANSITION mode, alternated althod ***************************************************/ + [NAV_STATE_MIXERAT_INITIALIZE] = { + .persistentId = NAV_PERSISTENT_ID_MIXERAT_INITIALIZE, + .onEntry = navOnEnteringState_NAV_STATE_MIXERAT_INITIALIZE, + .timeoutMs = 0, + .stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT | NAV_MIXERAT, + .mapToFlightModes = NAV_ALTHOLD_MODE, + .mwState = MW_NAV_STATE_NONE, + .mwError = MW_NAV_ERROR_NONE, + .onEvent = { + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_MIXERAT_IN_PROGRESS, + [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + } + }, + + [NAV_STATE_MIXERAT_IN_PROGRESS] = { + .persistentId = NAV_PERSISTENT_ID_MIXERAT_IN_PROGRESS, + .onEntry = navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS, + .timeoutMs = 10, + .stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT | NAV_MIXERAT, + .mapToFlightModes = NAV_ALTHOLD_MODE, + .mwState = MW_NAV_STATE_NONE, + .mwError = MW_NAV_ERROR_NONE, + .onEvent = { + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_MIXERAT_IN_PROGRESS, // re-process the state + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_MIXERAT_ABORT, + [NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME] = NAV_STATE_RTH_HEAD_HOME, //switch to its pending state + [NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING] = NAV_STATE_RTH_LANDING, //switch to its pending state + } + }, + [NAV_STATE_MIXERAT_ABORT] = { + .persistentId = NAV_PERSISTENT_ID_MIXERAT_ABORT, + .onEntry = navOnEnteringState_NAV_STATE_MIXERAT_ABORT, //will not switch to its pending state + .timeoutMs = 10, + .stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT, + .mapToFlightModes = NAV_ALTHOLD_MODE, + .mwState = MW_NAV_STATE_NONE, + .mwError = MW_NAV_ERROR_NONE, + .onEvent = { + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + + } + }, }; static navigationFSMStateFlags_t navGetStateFlags(navigationFSMState_t state) @@ -948,7 +1002,7 @@ static navigationFSMStateFlags_t navGetStateFlags(navigationFSMState_t state) return navFSM[state].stateFlags; } -static flightModeFlags_e navGetMappedFlightModes(navigationFSMState_t state) +flightModeFlags_e navGetMappedFlightModes(navigationFSMState_t state) { return navFSM[state].mapToFlightModes; } @@ -1055,7 +1109,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE( { UNUSED(previousState); - if (!STATE(FIXED_WING_LEGACY)) { // Only on FW for now + if (STATE(MULTIROTOR) && !navConfig()->general.cruise_yaw_rate) { // course hold not possible on MR without yaw control return NAV_FSM_EVENT_ERROR; } @@ -1067,7 +1121,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE( resetPositionController(); - posControl.cruise.course = posControl.actualState.cog; // Store the course to follow + if (STATE(AIRPLANE)) { + posControl.cruise.course = posControl.actualState.cog; // Store the course to follow + } else { // Multicopter + posControl.cruise.course = posControl.actualState.yaw; + posControl.cruise.multicopterSpeed = constrainf(posControl.actualState.velXY, 10.0f, navConfig()->general.max_manual_speed); + posControl.desiredState.pos = posControl.actualState.abs.pos; + } posControl.cruise.previousCourse = posControl.cruise.course; posControl.cruise.lastCourseAdjustmentTime = 0; @@ -1088,15 +1148,24 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS DEBUG_SET(DEBUG_CRUISE, 0, 2); DEBUG_SET(DEBUG_CRUISE, 2, 0); - if (posControl.flags.isAdjustingPosition) { + if (STATE(AIRPLANE) && posControl.flags.isAdjustingPosition) { // inhibit for MR, pitch/roll only adjusts speed using pitch return NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ; } - // User is yawing. We record the desidered yaw and we change the desidered target in the meanwhile - if (posControl.flags.isAdjustingHeading) { + const bool mcRollStickHeadingAdjustmentActive = STATE(MULTIROTOR) && ABS(rcCommand[ROLL]) > rcControlsConfig()->pos_hold_deadband; + + // User demanding yaw -> yaw stick on FW, yaw or roll sticks on MR + // We record the desired course and change the desired target in the meanwhile + if (posControl.flags.isAdjustingHeading || mcRollStickHeadingAdjustmentActive) { + int16_t cruiseYawRate = DEGREES_TO_CENTIDEGREES(navConfig()->general.cruise_yaw_rate); + int16_t headingAdjustCommand = rcCommand[YAW]; + if (mcRollStickHeadingAdjustmentActive && ABS(rcCommand[ROLL]) > ABS(headingAdjustCommand)) { + headingAdjustCommand = -rcCommand[ROLL]; + } + timeMs_t timeDifference = currentTimeMs - posControl.cruise.lastCourseAdjustmentTime; if (timeDifference > 100) timeDifference = 0; // if adjustment was called long time ago, reset the time difference. - float rateTarget = scaleRangef((float)rcCommand[YAW], -500.0f, 500.0f, -DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate), DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate)); + float rateTarget = scaleRangef((float)headingAdjustCommand, -500.0f, 500.0f, -cruiseYawRate, cruiseYawRate); float centidegsPerIteration = rateTarget * MS2S(timeDifference); posControl.cruise.course = wrap_36000(posControl.cruise.course - centidegsPerIteration); DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.course)); @@ -1129,7 +1198,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING(n static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_INITIALIZE(navigationFSMState_t previousState) { - if (!STATE(FIXED_WING_LEGACY)) { return NAV_FSM_EVENT_ERROR; } // only on FW for now + if (STATE(MULTIROTOR) && !navConfig()->general.cruise_yaw_rate) { // course hold not possible on MR without yaw control + return NAV_FSM_EVENT_ERROR; + } navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(previousState); @@ -1349,6 +1420,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } + if (checkMixerATRequired(MIXERAT_REQUEST_RTH) && (calculateDistanceToDestination(&posControl.rthState.homePosition.pos) > (navConfig()->fw.loiter_radius * 3))){ + return NAV_FSM_EVENT_SWITCH_TO_MIXERAT; + } + if (navConfig()->general.flags.rth_use_linear_descent && navConfig()->general.rth_home_altitude > 0) { // Check linear descent status uint32_t homeDistance = calculateDistanceToDestination(&posControl.rthState.homePosition.pos); @@ -1440,11 +1515,21 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } + if (checkMixerATRequired(MIXERAT_REQUEST_LAND)){ + return NAV_FSM_EVENT_SWITCH_TO_MIXERAT; + } + float descentVelLimited = 0; - int32_t landingElevation = posControl.rthState.homeTmpWaypoint.z; + fpVector3_t tmpHomePos = posControl.rthState.homeTmpWaypoint; + uint32_t remaning_distance = calculateDistanceToDestination(&tmpHomePos); + + int32_t landingElevation = posControl.rthState.homeTmpWaypoint.z; + if(STATE(MULTIROTOR) && (remaning_distance>MR_RTH_LAND_MARGIN_CM)){ + descentVelLimited = navConfig()->general.land_minalt_vspd; + } // A safeguard - if surface altitude sensors is available and it is reading < 50cm altitude - drop to low descend speed - if ((posControl.flags.estAglStatus == EST_TRUSTED) && posControl.actualState.agl.pos.z < 50.0f) { + else if ((posControl.flags.estAglStatus == EST_TRUSTED) && posControl.actualState.agl.pos.z < 50.0f) { // land_descent_rate == 200 : descend speed = 30 cm/s, gentle touchdown // Do not allow descent velocity slower than -30cm/s so the landing detector works. descentVelLimited = navConfig()->general.land_minalt_vspd; @@ -1789,8 +1874,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINIS { UNUSED(previousState); - disarm(DISARM_NAVIGATION); - return NAV_FSM_EVENT_NONE; } @@ -1839,6 +1922,70 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_IN_PROGRESS(navi return NAV_FSM_EVENT_NONE; } +navigationFSMState_t navMixerATPendingState = NAV_STATE_IDLE; +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_INITIALIZE(navigationFSMState_t previousState) +{ + const navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState); + + // Prepare altitude controller if idle, RTH or WP modes active or surface mode status changed + if (!(prevFlags & NAV_CTL_ALT) || (prevFlags & NAV_AUTO_RTH) || (prevFlags & NAV_AUTO_WP)) { + resetAltitudeController(false); + setupAltitudeController(); + } + setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); + navMixerATPendingState = previousState; + return NAV_FSM_EVENT_SUCCESS; +} + +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(navigationFSMState_t previousState) +{ + UNUSED(previousState); + mixerProfileATRequest_e required_action; + switch (navMixerATPendingState) + { + case NAV_STATE_RTH_HEAD_HOME: + required_action = MIXERAT_REQUEST_RTH; + break; + case NAV_STATE_RTH_LANDING: + required_action = MIXERAT_REQUEST_LAND; + break; + default: + required_action = MIXERAT_REQUEST_NONE; + break; + } + if (mixerATUpdateState(required_action)){ + // MixerAT is done, switch to next state + resetPositionController(); + resetAltitudeController(false); // Make sure surface tracking is not enabled uses global altitude, not AGL + mixerATUpdateState(MIXERAT_REQUEST_ABORT); + switch (navMixerATPendingState) + { + case NAV_STATE_RTH_HEAD_HOME: + setupAltitudeController(); + return NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME; + break; + case NAV_STATE_RTH_LANDING: + setupAltitudeController(); + return NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING; + break; + default: + return NAV_FSM_EVENT_SWITCH_TO_IDLE; + break; + } + } + + setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); + + return NAV_FSM_EVENT_NONE; +} + +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_ABORT(navigationFSMState_t previousState) +{ + UNUSED(previousState); + mixerATUpdateState(MIXERAT_REQUEST_ABORT); + return NAV_FSM_EVENT_SUCCESS; +} + static navigationFSMState_t navSetNewFSMState(navigationFSMState_t newState) { navigationFSMState_t previousState; @@ -2448,13 +2595,13 @@ void checkSafeHomeState(bool shouldBeEnabled) safehomeNotApplicable = safehomeNotApplicable || (MULTI_FUNC_FLAG(MF_SUSPEND_SAFEHOMES) && !posControl.flags.forcedRTHActivated); #endif - if (safehomeNotApplicable) { - shouldBeEnabled = false; - } else if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_RTH_FS && shouldBeEnabled) { - // if safehomes are only used with failsafe and we're trying to enable safehome - // then enable the safehome only with failsafe - shouldBeEnabled = posControl.flags.forcedRTHActivated; - } + if (safehomeNotApplicable) { + shouldBeEnabled = false; + } else if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_RTH_FS && shouldBeEnabled) { + // if safehomes are only used with failsafe and we're trying to enable safehome + // then enable the safehome only with failsafe + shouldBeEnabled = posControl.flags.forcedRTHActivated; + } // no safe homes found when arming or safehome feature in the correct state, then we don't need to do anything if (posControl.safehomeState.distance == 0 || posControl.safehomeState.isApplied == shouldBeEnabled) { return; @@ -2485,8 +2632,8 @@ bool findNearestSafeHome(void) gpsLocation_t shLLH; shLLH.alt = 0; for (uint8_t i = 0; i < MAX_SAFE_HOMES; i++) { - if (!safeHomeConfig(i)->enabled) - continue; + if (!safeHomeConfig(i)->enabled) + continue; shLLH.lat = safeHomeConfig(i)->lat; shLLH.lon = safeHomeConfig(i)->lon; @@ -2732,6 +2879,9 @@ static void updateNavigationFlightStatistics(void) } } +/* + * Total travel distance in cm + */ uint32_t getTotalTravelDistance(void) { return lrintf(posControl.totalTripDistance); @@ -2823,7 +2973,7 @@ void updateLandingStatus(timeMs_t currentTimeMs) } } else if (STATE(LANDING_DETECTED)) { pidResetErrorAccumulators(); - if (navConfig()->general.flags.disarm_on_landing) { + if (navConfig()->general.flags.disarm_on_landing && !FLIGHT_MODE(FAILSAFE_MODE)) { ENABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED); disarm(DISARM_LANDING); } else if (!navigationInAutomaticThrottleMode()) { @@ -3426,33 +3576,34 @@ bool isNavHoldPositionActive(void) navigationIsExecutingAnEmergencyLanding(); } -float getActiveWaypointSpeed(void) +float getActiveSpeed(void) { - if (posControl.flags.isAdjustingPosition) { - // In manual control mode use different cap for speed + /* Currently only applicable for multicopter */ + + // Speed limit for modes where speed manually controlled + if (posControl.flags.isAdjustingPosition || FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { return navConfig()->general.max_manual_speed; } - else { - uint16_t waypointSpeed = navConfig()->general.auto_speed; - if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) { - if (posControl.waypointCount > 0 && (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_LAND)) { - float wpSpecificSpeed = 0.0f; - if(posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME) - wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p2; // P1 is hold time - else - wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p1; // default case + uint16_t waypointSpeed = navConfig()->general.auto_speed; - if (wpSpecificSpeed >= 50.0f && wpSpecificSpeed <= navConfig()->general.max_auto_speed) { - waypointSpeed = wpSpecificSpeed; - } else if (wpSpecificSpeed > navConfig()->general.max_auto_speed) { - waypointSpeed = navConfig()->general.max_auto_speed; - } + if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) { + if (posControl.waypointCount > 0 && (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_LAND)) { + float wpSpecificSpeed = 0.0f; + if(posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME) + wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p2; // P1 is hold time + else + wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p1; // default case + + if (wpSpecificSpeed >= 50.0f && wpSpecificSpeed <= navConfig()->general.max_auto_speed) { + waypointSpeed = wpSpecificSpeed; + } else if (wpSpecificSpeed > navConfig()->general.max_auto_speed) { + waypointSpeed = navConfig()->general.max_auto_speed; } } - - return waypointSpeed; } + + return waypointSpeed; } bool isWaypointNavTrackingActive(void) @@ -3471,29 +3622,21 @@ static void processNavigationRCAdjustments(void) { /* Process pilot's RC input. Disable all pilot's input when in FAILSAFE_MODE */ navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState); - if ((navStateFlags & NAV_RC_ALT) && (!FLIGHT_MODE(FAILSAFE_MODE))) { - posControl.flags.isAdjustingAltitude = adjustAltitudeFromRCInput(); - } - else { - posControl.flags.isAdjustingAltitude = false; - } - if (navStateFlags & NAV_RC_POS) { - posControl.flags.isAdjustingPosition = adjustPositionFromRCInput() && !FLIGHT_MODE(FAILSAFE_MODE); - if (STATE(MULTIROTOR) && FLIGHT_MODE(FAILSAFE_MODE)) { + if (FLIGHT_MODE(FAILSAFE_MODE)) { + if (STATE(MULTIROTOR) && navStateFlags & NAV_RC_POS) { resetMulticopterBrakingMode(); } - } - else { + posControl.flags.isAdjustingAltitude = false; posControl.flags.isAdjustingPosition = false; - } - - if ((navStateFlags & NAV_RC_YAW) && (!FLIGHT_MODE(FAILSAFE_MODE))) { - posControl.flags.isAdjustingHeading = adjustHeadingFromRCInput(); - } - else { posControl.flags.isAdjustingHeading = false; + + return; } + + posControl.flags.isAdjustingAltitude = (navStateFlags & NAV_RC_ALT) && adjustAltitudeFromRCInput(); + posControl.flags.isAdjustingPosition = (navStateFlags & NAV_RC_POS) && adjustPositionFromRCInput(); + posControl.flags.isAdjustingHeading = (navStateFlags & NAV_RC_YAW) && adjustHeadingFromRCInput(); } /*----------------------------------------------------------- @@ -3797,8 +3940,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) if ((FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivateAltHold)) return NAV_FSM_EVENT_SWITCH_TO_ALTHOLD; } - } - else { + } else { canActivateWaypoint = false; // Launch mode can be activated if feature FW_LAUNCH is enabled or BOX is turned on prior to arming (avoid switching to LAUNCH in flight) @@ -3850,17 +3992,16 @@ int8_t navigationGetHeadingControlState(void) } // For multirotors it depends on navigation system mode + // Course hold requires Auto Control to update heading hold target whilst RC adjustment active if (navGetStateFlags(posControl.navState) & NAV_REQUIRE_MAGHOLD) { - if (posControl.flags.isAdjustingHeading) { + if (posControl.flags.isAdjustingHeading && !FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { return NAV_HEADING_CONTROL_MANUAL; } - else { - return NAV_HEADING_CONTROL_AUTO; - } - } - else { - return NAV_HEADING_CONTROL_NONE; + + return NAV_HEADING_CONTROL_AUTO; } + + return NAV_HEADING_CONTROL_NONE; } bool navigationTerrainFollowingEnabled(void) @@ -3912,13 +4053,13 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass) } } - /* - * Don't allow arming if any of JUMP waypoint has invalid settings - * First WP can't be JUMP - * Can't jump to immediately adjacent WPs (pointless) - * Can't jump beyond WP list - * Only jump to geo-referenced WP types - */ + /* + * Don't allow arming if any of JUMP waypoint has invalid settings + * First WP can't be JUMP + * Can't jump to immediately adjacent WPs (pointless) + * Can't jump beyond WP list + * Only jump to geo-referenced WP types + */ if (posControl.waypointCount) { for (uint8_t wp = posControl.startWpIndex; wp < posControl.waypointCount + posControl.startWpIndex; wp++){ if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP){ @@ -4198,15 +4339,6 @@ void navigationInit(void) /* Use system config */ navigationUsePIDs(); - if ( - mixerConfig()->platformType == PLATFORM_BOAT || - mixerConfig()->platformType == PLATFORM_ROVER || - navConfig()->fw.useFwNavYawControl - ) { - ENABLE_STATE(FW_HEADING_USE_YAW); - } else { - DISABLE_STATE(FW_HEADING_USE_YAW); - } #if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE) /* configure WP missions at boot */ #ifdef USE_MULTI_MISSION @@ -4267,7 +4399,7 @@ void abortForcedRTH(void) rthState_e getStateOfForcedRTH(void) { /* If forced RTH activated and in AUTO_RTH or EMERG state */ - if (posControl.flags.forcedRTHActivated && (navGetStateFlags(posControl.navState) & (NAV_AUTO_RTH | NAV_CTL_EMERG))) { + if (posControl.flags.forcedRTHActivated && (navGetStateFlags(posControl.navState) & (NAV_AUTO_RTH | NAV_CTL_EMERG | NAV_MIXERAT))) { if (posControl.navState == NAV_STATE_RTH_FINISHED || posControl.navState == NAV_STATE_EMERGENCY_LANDING_FINISHED) { return RTH_HAS_LANDED; } @@ -4362,7 +4494,7 @@ bool navigationRTHAllowsLanding(void) bool isNavLaunchEnabled(void) { - return IS_RC_MODE_ACTIVE(BOXNAVLAUNCH) || feature(FEATURE_FW_LAUNCH); + return (IS_RC_MODE_ACTIVE(BOXNAVLAUNCH) || feature(FEATURE_FW_LAUNCH)) && STATE(AIRPLANE); } bool abortLaunchAllowed(void) @@ -4398,3 +4530,8 @@ bool isAdjustingHeading(void) { int32_t getCruiseHeadingAdjustment(void) { return wrap_18000(posControl.cruise.course - posControl.cruise.previousCourse); } + +int32_t navigationGetHeadingError(void) +{ + return wrap_18000(posControl.desiredState.yaw - posControl.actualState.cog); +} diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 8458a3cd2c0..3a9223ff639 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -267,6 +267,7 @@ typedef struct navConfig_s { uint8_t land_detect_sensitivity; // Sensitivity of landing detector uint16_t auto_disarm_delay; // safety time delay for landing detector uint16_t rth_linear_descent_start_distance; // Distance from home to start the linear descent (0 = immediately) + uint8_t cruise_yaw_rate; // Max yaw rate (dps) when CRUISE MODE is enabled } general; struct { @@ -315,7 +316,6 @@ typedef struct navConfig_s { uint8_t launch_max_angle; // Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg] bool launch_manual_throttle; // Allows launch with manual throttle control uint8_t launch_abort_deadband; // roll/pitch stick movement deadband for launch abort - uint8_t cruise_yaw_rate; // Max yaw rate (dps) when CRUISE MODE is enabled bool allow_manual_thr_increase; bool useFwNavYawControl; uint8_t yawControlDeadband; diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 91354d97aed..3a88e4ea366 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -39,6 +39,7 @@ #include "flight/pid.h" #include "flight/imu.h" #include "flight/mixer.h" +#include "flight/mixer_profile.h" #include "fc/config.h" #include "fc/controlrate_profile.h" @@ -71,7 +72,6 @@ static bool isRollAdjustmentValid = false; static bool isYawAdjustmentValid = false; static float throttleSpeedAdjustment = 0; static bool isAutoThrottleManuallyIncreased = false; -static int32_t navHeadingError; static float navCrossTrackError; static int8_t loiterDirYaw = 1; bool needToCalculateCircularLoiter; @@ -294,6 +294,10 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod) needToCalculateCircularLoiter = isNavHoldPositionActive() && (distanceToActualTarget <= (navLoiterRadius / TAN_15DEG)) && (distanceToActualTarget > 50.0f); + //if vtol landing is required, fly straight to homepoint + if ((posControl.navState == NAV_STATE_RTH_HEAD_HOME) && navigationRTHAllowsLanding() && checkMixerATRequired(MIXERAT_REQUEST_LAND)){ + needToCalculateCircularLoiter = false; + } /* WP turn smoothing with 2 options, 1: pass through WP, 2: cut inside turn missing WP * Works for turns > 30 degs and < 160 degs. @@ -445,7 +449,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta * Calculate NAV heading error * Units are centidegrees */ - navHeadingError = wrap_18000(virtualTargetBearing - posControl.actualState.cog); + int32_t navHeadingError = wrap_18000(virtualTargetBearing - posControl.actualState.cog); // Forced turn direction // If heading error is close to 180 deg we initiate forced turn and only disable it when heading error goes below 90 deg @@ -647,7 +651,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat isAutoThrottleManuallyIncreased = false; } - rcCommand[THROTTLE] = constrain(correctedThrottleValue, getThrottleIdleValue(), motorConfig()->maxthrottle); + rcCommand[THROTTLE] = setDesiredThrottle(correctedThrottleValue, false); } #ifdef NAV_FIXED_WING_LANDING @@ -662,7 +666,6 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat (posControl.flags.estAglStatus == EST_TRUSTED && posControl.actualState.agl.pos.z <= navConfig()->general.land_slowdown_minalt)) { // Set motor to min. throttle and stop it when MOTOR_STOP feature is enabled - rcCommand[THROTTLE] = getThrottleIdleValue(); ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // Stabilize ROLL axis on 0 degrees banking regardless of loiter radius and position @@ -762,7 +765,7 @@ bool isFixedWingLandingDetected(void) *-----------------------------------------------------------*/ void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs) { - rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle; + rcCommand[THROTTLE] = setDesiredThrottle(currentBatteryProfile->failsafe_throttle, true); if (posControl.flags.estAltStatus >= EST_USABLE) { // target min descent rate 10m above takeoff altitude @@ -851,11 +854,6 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags, } } -int32_t navigationGetHeadingError(void) -{ - return navHeadingError; -} - float navigationGetCrossTrackError(void) { return navCrossTrackError; diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index d83b61426e5..fe0af4d55a8 100644 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -234,19 +234,12 @@ static void setCurrentState(fixedWingLaunchState_t nextState, timeUs_t currentTi /* Wing control Helpers */ -static bool isThrottleIdleEnabled(void) -{ - return currentBatteryProfile->nav.fw.launch_idle_throttle > getThrottleIdleValue(); -} - static void applyThrottleIdleLogic(bool forceMixerIdle) { - if (isThrottleIdleEnabled() && !forceMixerIdle) { - rcCommand[THROTTLE] = currentBatteryProfile->nav.fw.launch_idle_throttle; - } - else { - ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped - rcCommand[THROTTLE] = getThrottleIdleValue(); // If MOTOR_STOP is disabled, motors will spin given throttle value + if (forceMixerIdle) { + ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped, otherwise motor will run at idle + } else { + rcCommand[THROTTLE] = setDesiredThrottle(currentBatteryProfile->nav.fw.launch_idle_throttle, true); } } @@ -290,7 +283,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs UNUSED(currentTimeUs); if (!throttleStickIsLow()) { - if (isThrottleIdleEnabled()) { + if (currentBatteryProfile->nav.fw.launch_idle_throttle > getThrottleIdleValue()) { return FW_LAUNCH_EVENT_SUCCESS; } else { @@ -404,7 +397,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_ const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs); const uint16_t motorSpinUpMs = navConfig()->fw.launch_motor_spinup_time; - const uint16_t launchThrottle = constrain(currentBatteryProfile->nav.fw.launch_throttle, getThrottleIdleValue(), motorConfig()->maxthrottle); + const uint16_t launchThrottle = setDesiredThrottle(currentBatteryProfile->nav.fw.launch_throttle, false); if (elapsedTimeMs > motorSpinUpMs) { rcCommand[THROTTLE] = launchThrottle; @@ -438,7 +431,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t } } else { initialTime = navConfig()->fw.launch_motor_timer + navConfig()->fw.launch_motor_spinup_time; - rcCommand[THROTTLE] = constrain(currentBatteryProfile->nav.fw.launch_throttle, getThrottleIdleValue(), motorConfig()->maxthrottle); + rcCommand[THROTTLE] = setDesiredThrottle(currentBatteryProfile->nav.fw.launch_throttle, false); } if (isLaunchMaxAltitudeReached()) { @@ -468,7 +461,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t curr // Make a smooth transition from the launch state to the current state for pitch angle // Do the same for throttle when manual launch throttle isn't used if (!navConfig()->fw.launch_manual_throttle) { - const uint16_t launchThrottle = constrain(currentBatteryProfile->nav.fw.launch_throttle, getThrottleIdleValue(), motorConfig()->maxthrottle); + const uint16_t launchThrottle = setDesiredThrottle(currentBatteryProfile->nav.fw.launch_throttle, false); rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, launchThrottle, rcCommand[THROTTLE]); } fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_climb_angle, rcCommand[PITCH]); diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 7f0b4061ec7..2264d842014 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -121,7 +121,7 @@ static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros) int16_t rcThrottleCorrection = pt1FilterApply4(&altholdThrottleFilterState, velocity_controller, NAV_THROTTLE_CUTOFF_FREQENCY_HZ, US2S(deltaMicros)); rcThrottleCorrection = constrain(rcThrottleCorrection, thrCorrectionMin, thrCorrectionMax); - posControl.rcAdjustment[THROTTLE] = constrain(currentBatteryProfile->nav.mc.hover_throttle + rcThrottleCorrection, getThrottleIdleValue(), motorConfig()->maxthrottle); + posControl.rcAdjustment[THROTTLE] = setDesiredThrottle(currentBatteryProfile->nav.mc.hover_throttle + rcThrottleCorrection, false); } bool adjustMulticopterAltitudeFromRCInput(void) @@ -218,7 +218,7 @@ void resetMulticopterAltitudeController(void) pt1FilterReset(&posControl.pids.vel[Z].dterm_filter_state, 0.0f); if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) { - const float maxSpeed = getActiveWaypointSpeed(); + const float maxSpeed = getActiveSpeed(); nav_speed_up = maxSpeed; nav_accel_z = maxSpeed; nav_speed_down = navConfig()->general.max_auto_climb_rate; @@ -285,14 +285,15 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs) bool adjustMulticopterHeadingFromRCInput(void) { if (ABS(rcCommand[YAW]) > rcControlsConfig()->pos_hold_deadband) { - // Can only allow pilot to set the new heading if doing PH, during RTH copter will target itself to home - posControl.desiredState.yaw = posControl.actualState.yaw; + // Heading during Cruise Hold mode set by Nav function so no adjustment required here + if (!FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { + posControl.desiredState.yaw = posControl.actualState.yaw; + } return true; } - else { - return false; - } + + return false; } /*----------------------------------------------------------- @@ -402,8 +403,44 @@ void resetMulticopterPositionController(void) } } +static bool adjustMulticopterCruiseSpeed(int16_t rcPitchAdjustment) +{ + static timeMs_t lastUpdateTimeMs; + const timeMs_t currentTimeMs = millis(); + const timeMs_t updateDeltaTimeMs = currentTimeMs - lastUpdateTimeMs; + lastUpdateTimeMs = currentTimeMs; + + const float rcVelX = rcPitchAdjustment * navConfig()->general.max_manual_speed / (float)(500 - rcControlsConfig()->pos_hold_deadband); + + if (rcVelX > posControl.cruise.multicopterSpeed) { + posControl.cruise.multicopterSpeed = rcVelX; + } else if (rcVelX < 0 && updateDeltaTimeMs < 100) { + posControl.cruise.multicopterSpeed += MS2S(updateDeltaTimeMs) * rcVelX / 2.0f; + } else { + return false; + } + posControl.cruise.multicopterSpeed = constrainf(posControl.cruise.multicopterSpeed, 10.0f, navConfig()->general.max_manual_speed); + + return true; +} + +static void setMulticopterStopPosition(void) +{ + fpVector3_t stopPosition; + calculateMulticopterInitialHoldPosition(&stopPosition); + setDesiredPosition(&stopPosition, 0, NAV_POS_UPDATE_XY); +} + bool adjustMulticopterPositionFromRCInput(int16_t rcPitchAdjustment, int16_t rcRollAdjustment) { + if (navGetMappedFlightModes(posControl.navState) & NAV_COURSE_HOLD_MODE) { + if (rcPitchAdjustment) { + return adjustMulticopterCruiseSpeed(rcPitchAdjustment); + } + + return false; + } + // Process braking mode processMulticopterBrakingMode(rcPitchAdjustment || rcRollAdjustment); @@ -425,16 +462,12 @@ bool adjustMulticopterPositionFromRCInput(int16_t rcPitchAdjustment, int16_t rcR return true; } - else { + else if (posControl.flags.isAdjustingPosition) { // Adjusting finished - reset desired position to stay exactly where pilot released the stick - if (posControl.flags.isAdjustingPosition) { - fpVector3_t stopPosition; - calculateMulticopterInitialHoldPosition(&stopPosition); - setDesiredPosition(&stopPosition, 0, NAV_POS_UPDATE_XY); - } - - return false; + setMulticopterStopPosition(); } + + return false; } static float getVelocityHeadingAttenuationFactor(void) @@ -463,15 +496,28 @@ static float getVelocityExpoAttenuationFactor(float velTotal, float velMax) static void updatePositionVelocityController_MC(const float maxSpeed) { + if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { + // Position held at cruise speeds below 0.5 m/s, otherwise desired neu velocities set directly from cruise speed + if (posControl.cruise.multicopterSpeed >= 50) { + // Rotate multicopter x velocity from body frame to earth frame + posControl.desiredState.vel.x = posControl.cruise.multicopterSpeed * cos_approx(CENTIDEGREES_TO_RADIANS(posControl.cruise.course)); + posControl.desiredState.vel.y = posControl.cruise.multicopterSpeed * sin_approx(CENTIDEGREES_TO_RADIANS(posControl.cruise.course)); + + return; + } else if (posControl.flags.isAdjustingPosition) { + setMulticopterStopPosition(); + } + } + const float posErrorX = posControl.desiredState.pos.x - navGetCurrentActualPositionAndVelocity()->pos.x; const float posErrorY = posControl.desiredState.pos.y - navGetCurrentActualPositionAndVelocity()->pos.y; // Calculate target velocity - float newVelX = posErrorX * posControl.pids.pos[X].param.kP; - float newVelY = posErrorY * posControl.pids.pos[Y].param.kP; + float neuVelX = posErrorX * posControl.pids.pos[X].param.kP; + float neuVelY = posErrorY * posControl.pids.pos[Y].param.kP; // Scale velocity to respect max_speed - float newVelTotal = calc_length_pythagorean_2D(newVelX, newVelY); + float neuVelTotal = calc_length_pythagorean_2D(neuVelX, neuVelY); /* * We override computed speed with max speed in following cases: @@ -481,26 +527,23 @@ static void updatePositionVelocityController_MC(const float maxSpeed) if ( ((navGetCurrentStateFlags() & NAV_AUTO_WP || posControl.flags.rthTrackbackActive) && !isNavHoldPositionActive() && - newVelTotal < maxSpeed && + neuVelTotal < maxSpeed && !navConfig()->mc.slowDownForTurning - ) || newVelTotal > maxSpeed + ) || neuVelTotal > maxSpeed ) { - newVelX = maxSpeed * (newVelX / newVelTotal); - newVelY = maxSpeed * (newVelY / newVelTotal); - newVelTotal = maxSpeed; + neuVelX = maxSpeed * (neuVelX / neuVelTotal); + neuVelY = maxSpeed * (neuVelY / neuVelTotal); + neuVelTotal = maxSpeed; } - posControl.pids.pos[X].output_constrained = newVelX; - posControl.pids.pos[Y].output_constrained = newVelY; + posControl.pids.pos[X].output_constrained = neuVelX; + posControl.pids.pos[Y].output_constrained = neuVelY; // Apply expo & attenuation if heading in wrong direction - turn first, accelerate later (effective only in WP mode) const float velHeadFactor = getVelocityHeadingAttenuationFactor(); - const float velExpoFactor = getVelocityExpoAttenuationFactor(newVelTotal, maxSpeed); - posControl.desiredState.vel.x = newVelX * velHeadFactor * velExpoFactor; - posControl.desiredState.vel.y = newVelY * velHeadFactor * velExpoFactor; - - navDesiredVelocity[X] = constrain(lrintf(posControl.desiredState.vel.x), -32678, 32767); - navDesiredVelocity[Y] = constrain(lrintf(posControl.desiredState.vel.y), -32678, 32767); + const float velExpoFactor = getVelocityExpoAttenuationFactor(neuVelTotal, maxSpeed); + posControl.desiredState.vel.x = neuVelX * velHeadFactor * velExpoFactor; + posControl.desiredState.vel.y = neuVelY * velHeadFactor * velExpoFactor; } static float computeNormalizedVelocity(const float value, const float maxValue) @@ -660,49 +703,53 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA static void applyMulticopterPositionController(timeUs_t currentTimeUs) { - static timeUs_t previousTimePositionUpdate = 0; // Occurs @ GPS update rate - bool bypassPositionController; - - // We should passthrough rcCommand is adjusting position in GPS_ATTI mode - bypassPositionController = (navConfig()->general.flags.user_control_mode == NAV_GPS_ATTI) && posControl.flags.isAdjustingPosition; - // Apply controller only if position source is valid. In absence of valid pos sensor (GPS loss), we'd stick in forced ANGLE mode // and pilots input would be passed thru to PID controller - if ((posControl.flags.estPosStatus >= EST_USABLE)) { - // If we have new position - update velocity and acceleration controllers - if (posControl.flags.horizontalPositionDataNew) { - const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; - previousTimePositionUpdate = currentTimeUs; - - if (!bypassPositionController) { - // Update position controller - if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { - // Get max speed from generic NAV (waypoint specific), don't allow to move slower than 0.5 m/s - const float maxSpeed = getActiveWaypointSpeed(); - updatePositionVelocityController_MC(maxSpeed); - updatePositionAccelController_MC(deltaMicrosPositionUpdate, NAV_ACCELERATION_XY_MAX, maxSpeed); - } - else { - // Position update has not occurred in time (first start or glitch), reset altitude controller - resetMulticopterPositionController(); - } - } - - // Indicate that information is no longer usable - posControl.flags.horizontalPositionDataConsumed = true; - } - } - else { + if (posControl.flags.estPosStatus < EST_USABLE) { /* No position data, disable automatic adjustment, rcCommand passthrough */ posControl.rcAdjustment[PITCH] = 0; posControl.rcAdjustment[ROLL] = 0; - bypassPositionController = true; + + return; } - if (!bypassPositionController) { - rcCommand[PITCH] = pidAngleToRcCommand(posControl.rcAdjustment[PITCH], pidProfile()->max_angle_inclination[FD_PITCH]); - rcCommand[ROLL] = pidAngleToRcCommand(posControl.rcAdjustment[ROLL], pidProfile()->max_angle_inclination[FD_ROLL]); + // Passthrough rcCommand if adjusting position in GPS_ATTI mode except when Course Hold active + bool bypassPositionController = !FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && + navConfig()->general.flags.user_control_mode == NAV_GPS_ATTI && + posControl.flags.isAdjustingPosition; + + if (posControl.flags.horizontalPositionDataNew) { + // Indicate that information is no longer usable + posControl.flags.horizontalPositionDataConsumed = true; + + static timeUs_t previousTimePositionUpdate = 0; // Occurs @ GPS update rate + const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; + previousTimePositionUpdate = currentTimeUs; + + if (bypassPositionController) { + return; + } + + // If we have new position data - update velocity and acceleration controllers + if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { + // Get max speed for current NAV mode + float maxSpeed = getActiveSpeed(); + updatePositionVelocityController_MC(maxSpeed); + updatePositionAccelController_MC(deltaMicrosPositionUpdate, NAV_ACCELERATION_XY_MAX, maxSpeed); + + navDesiredVelocity[X] = constrain(lrintf(posControl.desiredState.vel.x), -32678, 32767); + navDesiredVelocity[Y] = constrain(lrintf(posControl.desiredState.vel.y), -32678, 32767); + } + else { + // Position update has not occurred in time (first start or glitch), reset position controller + resetMulticopterPositionController(); + } + } else if (bypassPositionController) { + return; } + + rcCommand[PITCH] = pidAngleToRcCommand(posControl.rcAdjustment[PITCH], pidProfile()->max_angle_inclination[FD_PITCH]); + rcCommand[ROLL] = pidAngleToRcCommand(posControl.rcAdjustment[ROLL], pidProfile()->max_angle_inclination[FD_ROLL]); } bool isMulticopterFlying(void) @@ -867,13 +914,14 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs) rcCommand[YAW] = 0; rcCommand[ROLL] = 0; rcCommand[PITCH] = 0; - rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle; /* Altitude sensors gone haywire, attempt to land regardless */ if (posControl.flags.estAltStatus < EST_USABLE) { if (failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_DROP_IT) { rcCommand[THROTTLE] = getThrottleIdleValue(); + return; } + rcCommand[THROTTLE] = setDesiredThrottle(currentBatteryProfile->failsafe_throttle, true); return; } @@ -926,6 +974,10 @@ void resetMulticopterHeadingController(void) static void applyMulticopterHeadingController(void) { + if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { // heading set by Nav during Course Hold so disable yaw stick input + rcCommand[YAW] = 0; + } + updateHeadingHoldTarget(CENTIDEGREES_TO_DEGREES(posControl.desiredState.yaw)); } diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 872f417dddd..c408f109c9b 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -157,6 +157,7 @@ typedef enum { NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD, NAV_FSM_EVENT_SWITCH_TO_CRUISE, NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ, + NAV_FSM_EVENT_SWITCH_TO_MIXERAT, NAV_FSM_EVENT_STATE_SPECIFIC_1, // State-specific event NAV_FSM_EVENT_STATE_SPECIFIC_2, // State-specific event @@ -164,6 +165,7 @@ typedef enum { NAV_FSM_EVENT_STATE_SPECIFIC_4, // State-specific event NAV_FSM_EVENT_STATE_SPECIFIC_5, // State-specific event NAV_FSM_EVENT_STATE_SPECIFIC_6, // State-specific event + NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME = NAV_FSM_EVENT_STATE_SPECIFIC_3, NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING = NAV_FSM_EVENT_STATE_SPECIFIC_1, NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND = NAV_FSM_EVENT_STATE_SPECIFIC_1, NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED = NAV_FSM_EVENT_STATE_SPECIFIC_2, @@ -228,6 +230,9 @@ typedef enum { NAV_PERSISTENT_ID_UNUSED_4 = 37, // was NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME NAV_PERSISTENT_ID_RTH_TRACKBACK = 38, + NAV_PERSISTENT_ID_MIXERAT_INITIALIZE = 39, + NAV_PERSISTENT_ID_MIXERAT_IN_PROGRESS = 40, + NAV_PERSISTENT_ID_MIXERAT_ABORT = 41, } navigationPersistentId_e; typedef enum { @@ -275,6 +280,10 @@ typedef enum { NAV_STATE_CRUISE_IN_PROGRESS, NAV_STATE_CRUISE_ADJUSTING, + NAV_STATE_MIXERAT_INITIALIZE, + NAV_STATE_MIXERAT_IN_PROGRESS, + NAV_STATE_MIXERAT_ABORT, + NAV_STATE_COUNT, } navigationFSMState_t; @@ -304,6 +313,8 @@ typedef enum { /* Additional flags */ NAV_CTL_LAND = (1 << 14), NAV_AUTO_WP_DONE = (1 << 15), //Waypoint mission reached the last waypoint and is idling + + NAV_MIXERAT = (1 << 16), //MIXERAT in progress } navigationFSMStateFlags_t; typedef struct { @@ -327,6 +338,7 @@ typedef struct { int32_t course; int32_t previousCourse; timeMs_t lastCourseAdjustmentTime; + float multicopterSpeed; } navCruise_t; typedef struct { @@ -453,6 +465,7 @@ bool isFixedWingFlying(void); bool isMulticopterFlying(void); navigationFSMStateFlags_t navGetCurrentStateFlags(void); +flightModeFlags_e navGetMappedFlightModes(navigationFSMState_t state); void setHomePosition(const fpVector3_t * pos, int32_t heading, navSetWaypointFlags_t useMask, navigationHomeFlags_t homeFlags); void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask); @@ -462,7 +475,7 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAlt bool isNavHoldPositionActive(void); bool isLastMissionWaypoint(void); -float getActiveWaypointSpeed(void); +float getActiveSpeed(void); bool isWaypointNavTrackingActive(void); void updateActualHeading(bool headingValid, int32_t newHeading, int32_t newGroundCourse); diff --git a/src/main/navigation/navigation_rover_boat.c b/src/main/navigation/navigation_rover_boat.c index f03df681d48..5f8134c6a24 100644 --- a/src/main/navigation/navigation_rover_boat.c +++ b/src/main/navigation/navigation_rover_boat.c @@ -138,7 +138,7 @@ void applyRoverBoatNavigationController(navigationFSMStateFlags_t navStateFlags, rcCommand[ROLL] = 0; rcCommand[PITCH] = 0; rcCommand[YAW] = 0; - rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle; + rcCommand[THROTTLE] = setDesiredThrottle(currentBatteryProfile->failsafe_throttle, true); } else if (navStateFlags & NAV_CTL_POS) { applyRoverBoatPositionController(currentTimeUs); applyRoverBoatPitchRollThrottleController(navStateFlags, currentTimeUs); diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index f07e487f9cb..52e5e909c32 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -46,6 +46,7 @@ #include "sensors/rangefinder.h" #include "flight/imu.h" #include "flight/pid.h" +#include "flight/mixer_profile.h" #include "drivers/io_port_expander.h" #include "io/osd_common.h" #include "sensors/diagnostics.h" @@ -424,6 +425,7 @@ static int logicConditionCompute( pidInit(); pidInitFilters(); schedulePidGainsUpdate(); + navigationUsePIDs(); //set navigation pid gains profileChanged = true; } return profileChanged; @@ -702,6 +704,10 @@ static int logicConditionGetFlightOperandValue(int operand) { return constrain(attitude.values.pitch / 10, -180, 180); break; + case LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_YAW: // deg + return constrain(attitude.values.yaw / 10, 0, 360); + break; + case LOGIC_CONDITION_OPERAND_FLIGHT_IS_ARMED: // 0/1 return ARMING_FLAG(ARMED) ? 1 : 0; break; @@ -769,6 +775,14 @@ static int logicConditionGetFlightOperandValue(int operand) { case LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE: // int return getConfigProfile() + 1; break; + + case LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_MIXER_PROFILE: // int + return currentMixerProfileIndex + 1; + break; + + case LOGIC_CONDITION_OPERAND_FLIGHT_MIXER_TRANSITION_ACTIVE: //0,1 + return isMixerTransitionMixing ? 1 : 0; + break; case LOGIC_CONDITION_OPERAND_FLIGHT_LOITER_RADIUS: return getLoiterRadius(navConfig()->fw.loiter_radius); diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 779dbb1b985..cb87566f81d 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -135,6 +135,9 @@ typedef enum { LOGIC_CONDITION_OPERAND_FLIGHT_AGL_STATUS, //0,1,2 // 35 LOGIC_CONDITION_OPERAND_FLIGHT_AGL, //0,1,2 // 36 LOGIC_CONDITION_OPERAND_FLIGHT_RANGEFINDER_RAW, //int // 37 + LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_MIXER_PROFILE, //int // 38 + LOGIC_CONDITION_OPERAND_FLIGHT_MIXER_TRANSITION_ACTIVE, //0,1 // 39 + LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_YAW, // deg // 40 } logicFlightOperands_e; typedef enum { diff --git a/src/main/programming/programming_task.c b/src/main/programming/programming_task.c index 69f1e929445..e5b6642d243 100644 --- a/src/main/programming/programming_task.c +++ b/src/main/programming/programming_task.c @@ -26,8 +26,10 @@ #include "programming/logic_condition.h" #include "programming/pid.h" +#include "flight/mixer_profile.h" void programmingFrameworkUpdateTask(timeUs_t currentTimeUs) { programmingPidUpdateTask(currentTimeUs); + outputProfileUpdateTask(currentTimeUs); logicConditionUpdateTask(currentTimeUs); } \ No newline at end of file diff --git a/src/main/rx/jetiexbus.c b/src/main/rx/jetiexbus.c index d9b009342e3..ce6ebfe958d 100644 --- a/src/main/rx/jetiexbus.c +++ b/src/main/rx/jetiexbus.c @@ -63,7 +63,12 @@ #define JETIEXBUS_BAUDRATE 125000 // EX Bus 125000; EX Bus HS 250000 not supported #define JETIEXBUS_OPTIONS (SERIAL_STOPBITS_1 | SERIAL_PARITY_NO) #define JETIEXBUS_MIN_FRAME_GAP 1000 -#define JETIEXBUS_CHANNEL_COUNT 16 // most Jeti TX transmit 16 channels + +#ifdef USE_24CHANNELS +#define JETIEXBUS_CHANNEL_COUNT 24 +#else +#define JETIEXBUS_CHANNEL_COUNT 16 +#endif #define EXBUS_START_CHANNEL_FRAME (0x3E) diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 4d5e2b135f8..1aee04ec083 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -83,7 +83,11 @@ typedef enum { SERIALRX_FBUS, } rxSerialReceiverType_e; -#define MAX_SUPPORTED_RC_CHANNEL_COUNT 18 +#ifdef USE_24CHANNELS +#define MAX_SUPPORTED_RC_CHANNEL_COUNT 26 +#else +#define MAX_SUPPORTED_RC_CHANNEL_COUNT 18 +#endif #define NON_AUX_CHANNEL_COUNT 4 #define MAX_AUX_CHANNEL_COUNT (MAX_SUPPORTED_RC_CHANNEL_COUNT - NON_AUX_CHANNEL_COUNT) diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 0afc15b5bef..78e52f75c2a 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -564,7 +564,7 @@ void currentMeterUpdate(timeUs_t timeDelta) if (allNav || autoNav) { // account for motors running in Nav modes with throttle low + motor stop throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000; } else { - throttleOffset = (throttleStickIsLow() && feature(FEATURE_MOTOR_STOP)) ? 0 : (int32_t)rcCommand[THROTTLE] - 1000; + throttleOffset = (throttleStickIsLow() && ifMotorstopFeatureEnabled()) ? 0 : (int32_t)rcCommand[THROTTLE] - 1000; } int32_t throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50); amperage += throttleFactor * batteryMetersConfig()->current.scale / 1000; diff --git a/src/main/target/AIKONF4/config.c b/src/main/target/AIKONF4/config.c new file mode 100644 index 00000000000..c77bbfaf3ac --- /dev/null +++ b/src/main/target/AIKONF4/config.c @@ -0,0 +1,29 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include +#include + +#include + +#include "drivers/pwm_mapping.h" + +void targetConfiguration(void) +{ + // To improve backwards compatibility with INAV versions 6.x and older + timerOverridesMutable(timer2id(TIM3))->outputMode = OUTPUT_MODE_MOTORS; +} diff --git a/src/main/target/AIKONF4/target.c b/src/main/target/AIKONF4/target.c index 43c52dbd233..4268fd1864a 100644 --- a/src/main/target/AIKONF4/target.c +++ b/src/main/target/AIKONF4/target.c @@ -29,13 +29,13 @@ timerHardware_t timerHardware[] = { // DEF_TIM(TIM2, CH2, PB3, TIM_USE_CAMERA_CONTROL, 0, 0), // There is not camera control in INAV - DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S1 - DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S2 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S5 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S6 - DEF_TIM(TIM4, CH1, PB6, TIM_USE_PPM | TIM_USE_LED, 0, 0), // LED & PPM (DMA1_ST0_CH2) + DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_PPM | TIM_USE_LED, 0, 0), // LED & PPM (DMA1_ST0_CH2) }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); \ No newline at end of file diff --git a/src/main/target/AIRBOTF4/target.c b/src/main/target/AIRBOTF4/target.c index 8e324818d8a..988543aa321 100644 --- a/src/main/target/AIRBOTF4/target.c +++ b/src/main/target/AIRBOTF4/target.c @@ -24,20 +24,20 @@ #include "drivers/bus.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1_OUT D1_ST7 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2_OUT D1_ST2 - DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S3_OUT D1_ST6 - DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4_OUT D1_ST1 - DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO | TIM_USE_ANY, 0, 0), // S5_OUT / LED - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6_OUT - DEF_TIM(TIM4, CH1, PB6, TIM_USE_ANY, 0, 0), // LED D1_ST0, n/a on older AIRBOTF4 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1_OUT D1_ST7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S2_OUT D1_ST2 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 1), // S3_OUT D1_ST6 + DEF_TIM(TIM2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0), // S4_OUT D1_ST1 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO | TIM_USE_ANY, 0, 0), // S5_OUT / LED + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // S6_OUT + DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED | TIM_USE_ANY, 0, 0), // LED D1_ST0, n/a on older AIRBOTF4 - DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, 0, 0), // PPM (5th pin on FlexiIO port) - DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0), // S2_IN - DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM | TIM_USE_FW_SERVO, 0, 0), // S3_IN - DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM | TIM_USE_FW_SERVO, 0, 0), // S4_IN - DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM | TIM_USE_FW_SERVO, 0, 0), // S5_IN - DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM | TIM_USE_FW_SERVO, 0, 0), // S6_IN + DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, 0, 0), // PPM (5th pin on FlexiIO port) + DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0), // S2_IN + DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM | TIM_USE_OUTPUT_AUTO, 0, 0), // S3_IN + DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM | TIM_USE_OUTPUT_AUTO, 0, 0), // S4_IN + DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM | TIM_USE_OUTPUT_AUTO, 0, 0), // S5_IN + DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM | TIM_USE_OUTPUT_AUTO, 0, 0), // S6_IN }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/AIRBOTF7/target.c b/src/main/target/AIRBOTF7/target.c index a81988d0a69..22d2564821a 100644 --- a/src/main/target/AIRBOTF7/target.c +++ b/src/main/target/AIRBOTF7/target.c @@ -42,13 +42,13 @@ BUSDEV_REGISTER_SPI_TAG(busdev_imu1_mpu6500, DEVHW_MPU6500, GYRO_1_SPI_BUS, #endif timerHardware_t timerHardware[] = { - DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), // LED - DEF_TIM(TIM1, CH1, PA8, TIM_USE_ANY, 0, 0), // Cam control, SS, UNUSED + DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), // LED + DEF_TIM(TIM1, CH1, PA8, TIM_USE_ANY, 0, 0), // Cam control, SS, UNUSED - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR, 0, 0), //S1 - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR, 0, 0), //S2 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR, 0, 0), //S3 - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR, 0, 0), //S4 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), //S1 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), //S2 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), //S3 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), //S4 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/ALIENFLIGHTF4/target.c b/src/main/target/ALIENFLIGHTF4/target.c index bb600670736..e7ee4cf5520 100644 --- a/src/main/target/ALIENFLIGHTF4/target.c +++ b/src/main/target/ALIENFLIGHTF4/target.c @@ -23,19 +23,19 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM1, CH1, PA8, TIM_USE_PWM | TIM_USE_PPM, 0, 1), // PWM1 - PA8 RC1 - DMA2_ST1 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_PWM, 0, 0), // PWM2 - PB0 RC2 - DMA1_ST5 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_PWM, 0, 0), // PWM3 - PB1 RC3 - DMA1_ST7 - DEF_TIM(TIM1, CH2, PB14, TIM_USE_PWM, 0, 1), // PWM4 - PA14 RC4 - DMA2_ST2 - DEF_TIM(TIM1, CH3, PB15, TIM_USE_PWM | TIM_USE_LED, 0, 0), // PWM5 - PA15 RC5 - DMA2_ST6 - DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // PWM6 - PB8 OUT1 - DMA1_ST7 - DEF_TIM(TIM4, CH4, PB9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // PWM7 - PB9 OUT2 - DMA_NONE - DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // PWM8 - PA0 OUT3 - DMA1_ST2 - DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // PWM9 - PA1 OUT4 - DMA1_ST4 - DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // PWM10 - PC6 OUT5 - DMA2_ST2 - DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // PWM11 - PC7 OUT6 - DMA2_ST3 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // PWM13 - PC8 OUT7 - DMA2_ST4 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // PWM13 - PC9 OUT8 - DMA2_ST7 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_PWM | TIM_USE_PPM, 0, 1), // PWM1 - PA8 RC1 - DMA2_ST1 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_PWM, 0, 0), // PWM2 - PB0 RC2 - DMA1_ST5 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_PWM, 0, 0), // PWM3 - PB1 RC3 - DMA1_ST7 + DEF_TIM(TIM1, CH2, PB14, TIM_USE_PWM, 0, 1), // PWM4 - PA14 RC4 - DMA2_ST2 + DEF_TIM(TIM1, CH3, PB15, TIM_USE_PWM | TIM_USE_LED, 0, 0), // PWM5 - PA15 RC5 - DMA2_ST6 + DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0), // PWM6 - PB8 OUT1 - DMA1_ST7 + DEF_TIM(TIM4, CH4, PB9, TIM_USE_OUTPUT_AUTO, 0, 0), // PWM7 - PB9 OUT2 - DMA_NONE + DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0), // PWM8 - PA0 OUT3 - DMA1_ST2 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0), // PWM9 - PA1 OUT4 - DMA1_ST4 + DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // PWM10 - PC6 OUT5 - DMA2_ST2 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // PWM11 - PC7 OUT6 - DMA2_ST3 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 1), // PWM13 - PC8 OUT7 - DMA2_ST4 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // PWM13 - PC9 OUT8 - DMA2_ST7 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index 09a27576428..5527b45a1d3 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -152,7 +152,7 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT -#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_MOTOR_STOP | FEATURE_BLACKBOX) +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) #define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define SERIALRX_UART SERIAL_PORT_USART2 #define RX_CHANNELS_TAER diff --git a/src/main/target/ALIENFLIGHTNGF7/target.c b/src/main/target/ALIENFLIGHTNGF7/target.c index 45b18c71a38..455747923ef 100644 --- a/src/main/target/ALIENFLIGHTNGF7/target.c +++ b/src/main/target/ALIENFLIGHTNGF7/target.c @@ -23,19 +23,19 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM1, CH1, PA8, TIM_USE_PPM | TIM_USE_LED, 0, 1), // PPM - DMA2_ST1 - DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_SERVO, 0, 0), // PWM1 - DMA2_ST2 - DEF_TIM(TIM3, CH2, PC7, TIM_USE_MC_SERVO, 0, 0), // PWM2 - DMA1_ST5 - DEF_TIM(TIM8, CH2N, PB14, TIM_USE_MC_SERVO, 0, 0), // PWM3 - DMA2_ST3 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_SERVO, 0, 0), // PWM4 - DMA1_ST7 - DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_SERVO, 0, 0), // PWM5 - DMA1_ST2 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // PWM6 - DMA2_ST4 - DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // PWM7 - DMA1_ST4 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // PWM8 - DMA2_ST7 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // PWM9 - DMA1_ST2 - DEF_TIM(TIM1, CH3N, PB15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // PWM10 - DMA2_ST6 - DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // PWM11 - DMA1_ST7 - DEF_TIM(TIM4, CH4, PB9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // PWM12 - DMA_NONE + DEF_TIM(TIM1, CH1, PA8, TIM_USE_PPM | TIM_USE_LED, 0, 1), // PPM - DMA2_ST1 + DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // PWM1 - DMA2_ST2 + DEF_TIM(TIM3, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // PWM2 - DMA1_ST5 + DEF_TIM(TIM8, CH2N, PB14, TIM_USE_OUTPUT_AUTO, 0, 0), // PWM3 - DMA2_ST3 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // PWM4 - DMA1_ST7 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0), // PWM5 - DMA1_ST2 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // PWM6 - DMA2_ST4 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0), // PWM7 - DMA1_ST4 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // PWM8 - DMA2_ST7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // PWM9 - DMA1_ST2 + DEF_TIM(TIM1, CH3N, PB15, TIM_USE_OUTPUT_AUTO, 0, 0), // PWM10 - DMA2_ST6 + DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0), // PWM11 - DMA1_ST7 + DEF_TIM(TIM4, CH4, PB9, TIM_USE_OUTPUT_AUTO, 0, 0), // PWM12 - DMA_NONE }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/ALIENFLIGHTNGF7/target.h b/src/main/target/ALIENFLIGHTNGF7/target.h index 59ad543c3b9..5971f3d8484 100644 --- a/src/main/target/ALIENFLIGHTNGF7/target.h +++ b/src/main/target/ALIENFLIGHTNGF7/target.h @@ -165,7 +165,7 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT -#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_MOTOR_STOP | FEATURE_BLACKBOX) +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) #define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define SERIALRX_UART SERIAL_PORT_USART2 #define RX_CHANNELS_TAER diff --git a/src/main/target/ANYFC/target.c b/src/main/target/ANYFC/target.c index f9dccae0f21..5353b589110 100644 --- a/src/main/target/ANYFC/target.c +++ b/src/main/target/ANYFC/target.c @@ -23,23 +23,23 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - 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 - DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM, 0, 0), // S4_IN - DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM, 0, 0), // S6_IN - DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0, 0), // S5_IN + 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 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM, 0, 0), // S4_IN + DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM, 0, 0), // S6_IN + DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0, 0), // S5_IN - DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S10_OUT 16 - DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6_OUT 12 - DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5_OUT 11 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1_OUT 7 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2_OUT 8 - DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3_OUT 9 - DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4_OUT 10 - DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S7_OUT 13 - DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S8_OUT 14 - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S9_OUT 15 + DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO, 0, 0), // S10_OUT 16 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0), // S6_OUT 12 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0), // S5_OUT 11 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1_OUT 7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S2_OUT 8 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 0), // S3_OUT 9 + DEF_TIM(TIM2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0), // S4_OUT 10 + DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 0, 0), // S7_OUT 13 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // S8_OUT 14 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S9_OUT 15 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/ANYFCF7/target.c b/src/main/target/ANYFCF7/target.c index c14bf1dbe4b..65f807d7984 100644 --- a/src/main/target/ANYFCF7/target.c +++ b/src/main/target/ANYFCF7/target.c @@ -23,23 +23,23 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - 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 | TIM_USE_MC_SERVO, 0, 0 ), // S3_IN DMA2_ST2 DMA2_ST2 - DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM | TIM_USE_MC_SERVO, 0, 0 ), // S4_IN DMA2_ST3 DMA2_ST2 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM | TIM_USE_MC_SERVO, 0, 0 ), // S5_IN DMA2_ST4 DMA2_ST2 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM | TIM_USE_MC_SERVO, 0, 0 ), // S6_IN DMA2_ST7 + 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 | TIM_USE_OUTPUT_AUTO, 0, 0 ), // S3_IN DMA2_ST2 DMA2_ST2 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM | TIM_USE_OUTPUT_AUTO, 0, 0 ), // S4_IN DMA2_ST3 DMA2_ST2 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM | TIM_USE_OUTPUT_AUTO, 0, 0 ), // S5_IN DMA2_ST4 DMA2_ST2 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM | TIM_USE_OUTPUT_AUTO, 0, 0 ), // S6_IN DMA2_ST7 - DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S10_OUT 1 DMA1_ST7 - DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S6_OUT 2 DMA1_ST0 - DEF_TIM(TIM4, CH4, PB9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S5_OUT - DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S1_OUT 4 DMA1_ST1 DMA1_ST3 - DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S2_OUT 3 DMA1_ST4 - DEF_TIM(TIM9, CH2, PE6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S3_OUT - DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S4_OUT DMA1_ST5 - DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S7_OUT DMA1_ST2 - DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_LED, 0, 0 ), // S8_OUT DMA1_ST6 - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S9_OUT DMA1_ST4 + DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S10_OUT 1 DMA1_ST7 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S6_OUT 2 DMA1_ST0 + DEF_TIM(TIM4, CH4, PB9, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S5_OUT + DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S1_OUT 4 DMA1_ST1 DMA1_ST3 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S2_OUT 3 DMA1_ST4 + DEF_TIM(TIM9, CH2, PE6, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S3_OUT + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S4_OUT DMA1_ST5 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S7_OUT DMA1_ST2 + DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO | TIM_USE_LED, 0, 0 ), // S8_OUT DMA1_ST6 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S9_OUT DMA1_ST4 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/ANYFCM7/target.c b/src/main/target/ANYFCM7/target.c index 24f289afa6e..fb69d60a43b 100644 --- a/src/main/target/ANYFCM7/target.c +++ b/src/main/target/ANYFCM7/target.c @@ -33,16 +33,16 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0, 0 ), // 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_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S10_OUT 1 DMA1_ST7 - DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S6_OUT 2 DMA1_ST1 - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S5_OUT 3 DMA1_ST3 - DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1 ), // S1_OUT 4 DMA1_ST7 DMA1_ST6 - DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S2_OUT DMA1_ST4 - DEF_TIM(TIM1, CH2N, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1 ), // S3_OUT DMA2_ST6 DMA2_ST2 - DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_LED, 0, 0 ), // S4_OUT DMA1_ST5 - DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S7_OUT DMA1_ST2 - DEF_TIM(TIM1, CH3N, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S8_OUT DMA2_ST6 - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S9_OUT DMA1_ST4 + DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S10_OUT 1 DMA1_ST7 + DEF_TIM(TIM2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S6_OUT 2 DMA1_ST1 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S5_OUT 3 DMA1_ST3 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 1 ), // S1_OUT 4 DMA1_ST7 DMA1_ST6 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S2_OUT DMA1_ST4 + DEF_TIM(TIM1, CH2N, PB0, TIM_USE_OUTPUT_AUTO, 0, 1 ), // S3_OUT DMA2_ST6 DMA2_ST2 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO | TIM_USE_LED, 0, 0 ), // S4_OUT DMA1_ST5 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S7_OUT DMA1_ST2 + DEF_TIM(TIM1, CH3N, PB1, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S8_OUT DMA2_ST6 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S9_OUT DMA1_ST4 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/AOCODARCF405AIO/target.c b/src/main/target/AOCODARCF405AIO/target.c index 9548cb179a2..be225da1bf6 100644 --- a/src/main/target/AOCODARCF405AIO/target.c +++ b/src/main/target/AOCODARCF405AIO/target.c @@ -30,10 +30,10 @@ BUSDEV_REGISTER_SPI_TAG(busdev_bmi270, DEVHW_BMI270, BMI270_SPI_BUS, BMI270_C timerHardware_t timerHardware[] = { DEF_TIM(TIM2, CH2, PA10, TIM_USE_PPM, 0, 0), // PPM - DEF_TIM(TIM3, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 - DEF_TIM(TIM3, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 - DEF_TIM(TIM3, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 - DEF_TIM(TIM3, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 + DEF_TIM(TIM3, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM3, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + DEF_TIM(TIM3, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 + DEF_TIM(TIM3, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 DEF_TIM(TIM1, CH3N, PB1, TIM_USE_LED, 0, 0), // LED }; diff --git a/src/main/target/AOCODARCF4V2/target.c b/src/main/target/AOCODARCF4V2/target.c index c18898b84cc..8581cc5bf11 100644 --- a/src/main/target/AOCODARCF4V2/target.c +++ b/src/main/target/AOCODARCF4V2/target.c @@ -27,16 +27,16 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0 ), // PPM IN - DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1 ), // S1 - DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1 ), // S2 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1 ), // S3 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S4 + DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 1 ), // S1 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 1 ), // S2 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 1 ), // S3 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S4 - DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S5 - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1 ), // S6 + DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S5 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 1 ), // S6 - DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S7 - DEF_TIM(TIM2, CH4, PB11, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S8 + DEF_TIM(TIM2, CH3, PB10, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S7 + DEF_TIM(TIM2, CH4, PB11, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S8 DEF_TIM(TIM3, CH4, PB1, TIM_USE_LED, 0, 0 ), // LED_STRIP diff --git a/src/main/target/AOCODARCF4V2/target.h b/src/main/target/AOCODARCF4V2/target.h index 5ad27e46570..ed524edc906 100644 --- a/src/main/target/AOCODARCF4V2/target.h +++ b/src/main/target/AOCODARCF4V2/target.h @@ -55,6 +55,7 @@ #define USE_BARO_BMP280 #define USE_BARO_MS5611 +#define USE_BARO_DPS310 //*********** Magnetometer / Compass ************* #define USE_MAG diff --git a/src/main/target/AOCODARCF722AIO/CMakeLists.txt b/src/main/target/AOCODARCF722AIO/CMakeLists.txt new file mode 100644 index 00000000000..87553f152fb --- /dev/null +++ b/src/main/target/AOCODARCF722AIO/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f722xe(AOCODARCF722AIO) diff --git a/src/main/target/AOCODARCF722AIO/config.c b/src/main/target/AOCODARCF722AIO/config.c new file mode 100644 index 00000000000..8996c3c5d30 --- /dev/null +++ b/src/main/target/AOCODARCF722AIO/config.c @@ -0,0 +1,44 @@ +/* + * @Author: g05047 + * @Date: 2023-03-22 17:15:53 + * @LastEditors: g05047 + * @LastEditTime: 2023-03-23 16:21:45 + * @Description: file content + */ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include +#include "platform.h" + +#include "fc/fc_msp_box.h" +#include "io/piniobox.h" +#include "sensors/boardalignment.h" +#include "sensors/barometer.h" +#include "sensors/compass.h" +#include "sensors/acceleration.h" + +void targetConfiguration(void) +{ + + compassConfigMutable()->mag_align = CW90_DEG; + + // barometerConfigMutable()->baro_hardware = BARO_DPS310; + + boardAlignmentMutable()->rollDeciDegrees = -450; + +} diff --git a/src/main/target/AOCODARCF722AIO/target.c b/src/main/target/AOCODARCF722AIO/target.c new file mode 100644 index 00000000000..90d21bbf2f6 --- /dev/null +++ b/src/main/target/AOCODARCF722AIO/target.c @@ -0,0 +1,40 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/sensor.h" + +BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500, DEVHW_MPU6500, MPU6500_SPI_BUS, MPU6500_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_MPU6500_ALIGN); + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM + + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MOTOR, 0, 0), // S1 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_MOTOR, 0, 0), // S2 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0), // S3 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0), // S4 + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/AOCODARCF722AIO/target.h b/src/main/target/AOCODARCF722AIO/target.h new file mode 100644 index 00000000000..fd2a559b934 --- /dev/null +++ b/src/main/target/AOCODARCF722AIO/target.h @@ -0,0 +1,175 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "F722AIO" +#define USBD_PRODUCT_STRING "AocodaRCF722AIO" + +#define LED0 PA4 + +#define USE_BEEPER +#define BEEPER PC13 +#define BEEPER_INVERTED + +/*** UART ***/ +#define USB_IO +#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_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define SERIAL_PORT_COUNT 6 + +/*** Gyro & Acc ***/ +#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS + +#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_IMU_MPU6500 +#define MPU6500_CS_PIN PB2 +#define MPU6500_SPI_BUS BUS_SPI1 +#define MPU6500_EXTI_PIN PC4 + +#define IMU_MPU6500_ALIGN CW180_DEG + +#define USE_IMU_MPU6000 +#define MPU6000_CS_PIN PB2 +#define MPU6000_SPI_BUS BUS_SPI1 +#define MPU6000_EXTI_PIN PC4 + +#define IMU_MPU6000_ALIGN CW180_DEG + +#define USE_IMU_ICM42605 +#define ICM42605_CS_PIN PB2 +#define ICM42605_SPI_BUS BUS_SPI1 +#define ICM42605_EXTI_PIN PC4 + +#define IMU_ICM42605_ALIGN CW180_DEG + +/*** I2C (Baro & Mag) ***/ +#define USE_I2C + +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +// Baro +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_SPL06 +#define USE_BARO_DPS310 + +// Mag +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_IST8308 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL + +/*** Onboard Flash ***/ +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +#define M25P16_SPI_BUS BUS_SPI3 +#define M25P16_CS_PIN PD2 + +#define W25N01G_SPI_BUS BUS_SPI3 +#define W25N01G_CS_PIN PD2 + +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +/*** OSD ***/ +#define USE_OSD +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN PB12 + +/*** ADC ***/ +#define USE_ADC +#define ADC_CHANNEL_1_PIN PC2 +#define ADC_CHANNEL_2_PIN PC0 +#define ADC_CHANNEL_3_PIN PC1 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_3 +#define RSSI_ADC_CHANNEL ADC_CHN_2 + +/*** LED ***/ +#define USE_LED_STRIP +#define WS2811_PIN PA8 + +/*** Optical Flow & Lidar ***/ +#define USE_RANGEFINDER +#define USE_RANGEFINDER_MSP +#define USE_OPFLOW +#define USE_OPFLOW_MSP + +/*** Misc ***/ +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX) +#define USE_DSHOT +#define USE_ESC_SENSOR +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define SERIALRX_UART SERIAL_PORT_USART2 +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF + +#define MAX_PWM_OUTPUT_PORTS 8 + +#define CURRENT_METER_SCALE 250 + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) diff --git a/src/main/target/AOCODARCF7DUAL/target.c b/src/main/target/AOCODARCF7DUAL/target.c index f48fb445aac..d09fafeda21 100644 --- a/src/main/target/AOCODARCF7DUAL/target.c +++ b/src/main/target/AOCODARCF7DUAL/target.c @@ -32,15 +32,15 @@ BUSDEV_REGISTER_SPI_TAG(busdev_bmi270, DEVHW_BMI270, BMI270_SPI_BUS, timerHardware_t timerHardware[] = { DEF_TIM(TIM1, CH3, PA10, TIM_USE_PPM, 0, 0), //PPM - DEF_TIM(TIM3, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 D(2, 2, 7) - DEF_TIM(TIM3, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 D(2, 3, 7) - DEF_TIM(TIM3, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S3 D(2, 4, 7) - DEF_TIM(TIM3, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S4 D(2, 7, 7) - - DEF_TIM(TIM8, CH2N, PB0, TIM_USE_MC_MOTOR |TIM_USE_FW_SERVO, 0, 0), // S5 D(1, 7, 5) - DEF_TIM(TIM8, CH3N, PB1, TIM_USE_MC_MOTOR |TIM_USE_FW_SERVO, 0, 0), // S6 D(1, 2, 5) - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR |TIM_USE_FW_SERVO, 0, 0), // S7 D(1, 0, 2) - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR |TIM_USE_FW_SERVO, 0, 0), // S8 D(1, 3, 2) + DEF_TIM(TIM3, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 D(2, 2, 7) + DEF_TIM(TIM3, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 D(2, 3, 7) + DEF_TIM(TIM3, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 D(2, 4, 7) + DEF_TIM(TIM3, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 D(2, 7, 7) + + DEF_TIM(TIM8, CH2N, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 D(1, 7, 5) + DEF_TIM(TIM8, CH3N, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 D(1, 2, 5) + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 D(1, 0, 2) + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 D(1, 3, 2) DEF_TIM(TIM2, CH1, PA0, TIM_USE_LED, 0, 0), // LED_TRIP }; diff --git a/src/main/target/AOCODARCF7MINI/target.c b/src/main/target/AOCODARCF7MINI/target.c index 7b34bb05df4..34360bbc033 100644 --- a/src/main/target/AOCODARCF7MINI/target.c +++ b/src/main/target/AOCODARCF7MINI/target.c @@ -39,20 +39,20 @@ BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, timerHardware_t timerHardware[] = { DEF_TIM(TIM1, CH3, PA10, TIM_USE_PPM, 0, 0), // PPM, RX1 - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 D(1, 4, 5) - DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 D(1, 5, 4) - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S3 D(1, 7, 5) - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S4 D(1, 2, 5) + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 D(1, 4, 5) + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 D(1, 5, 4) + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 D(1, 7, 5) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 D(1, 2, 5) #if defined(AOCODARCF7MINI_V2) - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 D(2, 4, 7) - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 D(2, 7, 7) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 D(2, 4, 7) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 D(2, 7, 7) #else - DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 D(1, 6, 3) + DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 D(1, 6, 3) #endif - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S7 D(1, 0, 2) - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S8 D(1, 3, 2) + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 D(1, 0, 2) + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 D(1, 3, 2) DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED diff --git a/src/main/target/AOCODARCH7DUAL/target.c b/src/main/target/AOCODARCH7DUAL/target.c index 3bc551f5c4a..85e8b004937 100644 --- a/src/main/target/AOCODARCH7DUAL/target.c +++ b/src/main/target/AOCODARCH7DUAL/target.c @@ -32,20 +32,20 @@ BUSDEV_REGISTER_SPI_TAG(busdev_bmi270_2, DEVHW_BMI270, BMI270_SPI_BUS timerHardware_t timerHardware[] = { - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S2 - DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 2), // S3 - DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 3), // S4 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 1), // S2 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 2), // S3 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 3), // S4 - DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 4), // S5 - DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 5), // S6 - DEF_TIM(TIM4, CH1, PD12, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 6), // S7 - DEF_TIM(TIM4, CH2, PD13, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 7), // S8 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 4), // S5 + DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 5), // S6 + DEF_TIM(TIM4, CH1, PD12, TIM_USE_OUTPUT_AUTO, 0, 6), // S7 + DEF_TIM(TIM4, CH2, PD13, TIM_USE_OUTPUT_AUTO, 0, 7), // S8 - DEF_TIM(TIM4, CH3, PD14, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S9 - DEF_TIM(TIM4, CH4, PD15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S10 DMA_NONE - DEF_TIM(TIM15, CH1, PE5, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S11 - DEF_TIM(TIM15, CH2, PE6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S12 DMA_NONE + DEF_TIM(TIM4, CH3, PD14, TIM_USE_OUTPUT_AUTO, 0, 0), // S9 + DEF_TIM(TIM4, CH4, PD15, TIM_USE_OUTPUT_AUTO, 0, 0), // S10 DMA_NONE + DEF_TIM(TIM15, CH1, PE5, TIM_USE_OUTPUT_AUTO, 0, 0), // S11 + DEF_TIM(TIM15, CH2, PE6, TIM_USE_OUTPUT_AUTO, 0, 0), // S12 DMA_NONE DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 9), // LED_2812 DEF_TIM(TIM2, CH1, PA15, TIM_USE_BEEPER, 0, 0), // BEEPER PWM diff --git a/src/main/target/ASGARD32F4/target.c b/src/main/target/ASGARD32F4/target.c index 0fec1b2c79c..ec69feec389 100644 --- a/src/main/target/ASGARD32F4/target.c +++ b/src/main/target/ASGARD32F4/target.c @@ -33,10 +33,10 @@ timerHardware_t timerHardware[] = { // DEF_TIM(TIM1, CH1, PA8, TIM_USE_PPM, 0, 0), // PPM - timer clash with SS1_TX - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR, 1, 1), // M1 - D(2, 4, 7) - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR, 1, 0), // M2 - D(1, 7, 5) - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR, 1, 0), // M3 - D(1, 2, 5) - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR, 1, 0), // M4 - D(1, 3, 2) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, 1, 1), // M1 - D(2, 4, 7) + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 1, 0), // M2 - D(1, 7, 5) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 1, 0), // M3 - D(1, 2, 5) + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MOTOR, 1, 0), // M4 - D(1, 3, 2) // DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0), // LED strip - timer clash with M4 output diff --git a/src/main/target/ASGARD32F7/target.c b/src/main/target/ASGARD32F7/target.c index 74ff8607856..257523eceb5 100644 --- a/src/main/target/ASGARD32F7/target.c +++ b/src/main/target/ASGARD32F7/target.c @@ -33,10 +33,10 @@ timerHardware_t timerHardware[] = { // DEF_TIM(TIM1, CH1, PA8, TIM_USE_PPM, 0, 0), // PPM - timer clash with SS1_TX - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR, 1, 0), // M1 - D(2, 4, 7) - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR, 1, 0), // M2 - D(1, 7, 5) - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR, 1, 0), // M3 - D(1, 2, 5) - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR, 1, 0), // M4 - D(1, 6, 3) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, 1, 0), // M1 - D(2, 4, 7) + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 1, 0), // M2 - D(1, 7, 5) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 1, 0), // M3 - D(1, 2, 5) + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MOTOR, 1, 0), // M4 - D(1, 6, 3) DEF_TIM(TIM5, CH3, PA2, TIM_USE_LED, 0, 0), // TX2; SA port ---> LED - D(1, 0, 6) DEF_TIM(TIM1, CH2, PA9, TIM_USE_ANY, 0, 0), // SS1 }; diff --git a/src/main/target/ATOMRCF405NAVI/target.c b/src/main/target/ATOMRCF405NAVI/target.c index fa23597e92a..7ff7ac3c39d 100644 --- a/src/main/target/ATOMRCF405NAVI/target.c +++ b/src/main/target/ATOMRCF405NAVI/target.c @@ -34,14 +34,14 @@ #include "drivers/sensor.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 - DEF_TIM(TIM1, CH3N, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 - DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S5 - DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6 - DEF_TIM(TIM2, CH4, PB11, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S7 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S8 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 + DEF_TIM(TIM1, CH3N, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 + DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 + DEF_TIM(TIM2, CH3, PB10, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 + DEF_TIM(TIM2, CH4, PB11, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 DEF_TIM(TIM4, CH2, PB7, TIM_USE_LED, 0, 0), // LED_STRIP }; diff --git a/src/main/target/AXISFLYINGF7PRO/target.c b/src/main/target/AXISFLYINGF7PRO/target.c index fc46a9c0b8f..8d4b966b69c 100644 --- a/src/main/target/AXISFLYINGF7PRO/target.c +++ b/src/main/target/AXISFLYINGF7PRO/target.c @@ -23,17 +23,17 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S3 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S5 - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6 - DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S7 - DEF_TIM(TIM2, CH4, PB11, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 1), // S7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 1), // S3 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 + DEF_TIM(TIM2, CH3, PB10, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 + DEF_TIM(TIM2, CH4, PB11, TIM_USE_OUTPUT_AUTO, 0, 1), // S7 DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED STRIP - DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM + DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM DEF_TIM(TIM12, CH2, PB15, TIM_USE_ANY, 0, 0), // CAM CONTROL }; diff --git a/src/main/target/BEEROTORF4/target.c b/src/main/target/BEEROTORF4/target.c index f359da03c16..28e6a77402a 100644 --- a/src/main/target/BEEROTORF4/target.c +++ b/src/main/target/BEEROTORF4/target.c @@ -26,14 +26,14 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM IN - DEF_TIM(TIM1, CH2N, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // M1 - DMAR: DMA2_ST5 - DEF_TIM(TIM1, CH3N, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // M2 - - DEF_TIM(TIM2, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // M3 - DMAR: DMA1_ST7 - DEF_TIM(TIM2, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // M4 - - DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // M5 - DMAR: DMA2_ST1 - DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // M6 - - DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // M7 - DMAR: DMA1_ST2 - DEF_TIM(TIM4, CH4, PB9, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // M8 - DMAR: DMA1_ST6 + DEF_TIM(TIM1, CH2N, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // M1 - DMAR: DMA2_ST5 + DEF_TIM(TIM1, CH3N, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // M2 - + DEF_TIM(TIM2, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0), // M3 - DMAR: DMA1_ST7 + DEF_TIM(TIM2, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0), // M4 - + DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // M5 - DMAR: DMA2_ST1 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // M6 - + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // M7 - DMAR: DMA1_ST2 + DEF_TIM(TIM4, CH4, PB9, TIM_USE_OUTPUT_AUTO, 0, 0), // M8 - DMAR: DMA1_ST6 DEF_TIM(TIM4, CH3, PB8, TIM_USE_LED, 0, 0), // LED_STRIP / TRANSPONDER - DMA1_ST7 }; diff --git a/src/main/target/BETAFLIGHTF4/target.c b/src/main/target/BETAFLIGHTF4/target.c index 2360b6a3627..0e10cdbf59d 100755 --- a/src/main/target/BETAFLIGHTF4/target.c +++ b/src/main/target/BETAFLIGHTF4/target.c @@ -27,10 +27,10 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM4, CH3, PB8, TIM_USE_PPM, 0, 0), // PPM // Motors - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1_OUT D1_ST7 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2_OUT D1_ST2 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3_OUT D1_ST6 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4_OUT D1_ST1 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1_OUT D1_ST7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S2_OUT D1_ST2 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S3_OUT D1_ST6 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S4_OUT D1_ST1 // LED strip DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), // D1_ST0 diff --git a/src/main/target/BETAFPVF435/CMakeLists.txt b/src/main/target/BETAFPVF435/CMakeLists.txt new file mode 100644 index 00000000000..cfac04ed933 --- /dev/null +++ b/src/main/target/BETAFPVF435/CMakeLists.txt @@ -0,0 +1 @@ +target_at32f43x_xGT7(BETAFPVF435 SKIP_RELEASES) diff --git a/src/main/target/BETAFPVF435/config.c b/src/main/target/BETAFPVF435/config.c new file mode 100644 index 00000000000..895714f46bb --- /dev/null +++ b/src/main/target/BETAFPVF435/config.c @@ -0,0 +1,36 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + * + * This target has been autgenerated by bf2inav.py + */ + +#include + +#include "platform.h" + +#include "fc/fc_msp_box.h" +#include "fc/config.h" + +#include "io/ledstrip.h" + +void targetConfiguration(void) +{ + ledStripConfig_t *config = ledStripConfigMutable(); + ledConfig_t *lc = config->ledConfigs; + DEFINE_LED(lc, 0, 0, COLOR_RED, 0, LED_FUNCTION_COLOR, LED_FLAG_OVERLAY(LED_OVERLAY_STROBE), 0); + DEFINE_LED(lc+1, 0, 1, COLOR_GREEN, 0, LED_FUNCTION_COLOR, LED_FLAG_OVERLAY(LED_OVERLAY_STROBE), 0); +} + diff --git a/src/main/target/BETAFPVF435/target.c b/src/main/target/BETAFPVF435/target.c new file mode 100644 index 00000000000..f065744029d --- /dev/null +++ b/src/main/target/BETAFPVF435/target.c @@ -0,0 +1,44 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/pinio.h" +#include "drivers/sensor.h" + +timerHardware_t timerHardware[] = { + DEF_TIM(TMR3, CH3, PB0, TIM_USE_MOTOR, 0, 1), // TMR3_CH3 motor 1 + DEF_TIM(TMR3, CH4, PB1, TIM_USE_MOTOR, 0, 2), // TMR3_CH4 motor 2 + DEF_TIM(TMR2, CH4, PA3, TIM_USE_MOTOR, 0, 7), // TMR2_CH4 motor 3 + DEF_TIM(TMR2, CH3, PA2, TIM_USE_MOTOR, 0, 6), // TMR2_CH3 motor 4 + + DEF_TIM(TMR8, CH3, PC8, TIM_USE_OUTPUT_AUTO | TIM_USE_ANY, 0, 4), // TMR8_CH3 motor 5 + DEF_TIM(TMR1, CH1, PA8, TIM_USE_OUTPUT_AUTO | TIM_USE_ANY, 0, 5), // TMR1_CH3 motor 6 + + DEF_TIM(TMR4, CH1, PB6, TIM_USE_LED, 0, 0), // TMR4_CH1 LED_STRIP +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/BETAFPVF435/target.h b/src/main/target/BETAFPVF435/target.h new file mode 100644 index 00000000000..c8e17bd7c48 --- /dev/null +++ b/src/main/target/BETAFPVF435/target.h @@ -0,0 +1,201 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 . + */ +//https://www.arterychip.com/download/DS/DS_AT32F435_437_V2.02-EN.pdf +#pragma once + +#define TARGET_BOARD_IDENTIFIER "BHER" +#define USBD_PRODUCT_STRING "BETAFPVF435" + +#define LED0 PB5 + +#define USE_BEEPER +#define BEEPER PB4 +#define BEEPER_INVERTED + +//#define ENABLE_DSHOT_DMAR DSHOT_DMAR_AUTO +//#define DSHOT_BITBANG_DEFAULT DSHOT_BITBANG_AUTO +//#define ENABLE_DSHOT + +// *************** 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 + +#define SPI1_NSS_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 ENSURE_MPU_DATA_READY_IS_LOW + +// MPU6000 +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW270_DEG +#define MPU6000_SPI_BUS BUS_SPI1 +#define MPU6000_CS_PIN SPI1_NSS_PIN + +// ICM42605/ICM42688P +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW270_DEG +#define ICM42605_SPI_BUS BUS_SPI1 +#define ICM42605_CS_PIN SPI1_NSS_PIN + +//#define USE_ACC +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN CW270_DEG +#define BMI270_SPI_BUS BUS_SPI1 +#define BMI270_CS_PIN SPI1_NSS_PIN + +// *************** Baro ************************** +#define USE_BARO +#define USE_BARO_BMP280 +#define SPI3_NSS_PIN PB3 +#define BMP280_SPI_BUS BUS_SPI3 +#define BMP280_CS_PIN SPI3_NSS_PIN +#define USE_BARO_DPS310 +#define DPS310_SPI_BUS BUS_SPI3 +#define DPS310_CS_PIN SPI3_NSS_PIN + +// *************** BLACKBOX ************************** +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +#define USE_FLASHFS +#define FLASH_CS_PIN PB12 + +#define USE_FLASH_M25P16 +#define M25P16_SPI_BUS BUS_SPI2 +#define M25P16_CS_PIN FLASH_CS_PIN +#define USE_FLASH_W25N01G // 1Gb NAND flash support +#define W25N01G_SPI_BUS BUS_SPI2 +#define W25N01G_CS_PIN FLASH_CS_PIN +#define USE_FLASH_W25M // Stacked die support +#define W25M_SPI_BUS BUS_SPI2 +#define W25M_CS_PIN FLASH_CS_PIN +#define USE_FLASH_W25M512 // 512Kb (256Kb x 2 stacked) NOR flash support +#define W25M512_SPI_BUS BUS_SPI2 +#define W25M512_CS_PIN FLASH_CS_PIN +#define USE_FLASH_W25M02G // 2Gb (1Gb x 2 stacked) NAND flash support +#define W25M02G_SPI_BUS BUS_SPI2 +#define W25M02G_CS_PIN FLASH_CS_PIN + +// *************** OSD ***************************** +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +#if 0 +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI3 +#define MAX7456_CS_PIN PA15 +#endif + +#if 0 +// I2C +#define USE_I2C +#define USE_I2C_DEVICE_2 +#define I2C2_SCL PB10 // SCL pad +#define I2C2_SDA PB11 // SDA pad +#define USE_I2C_PULLUP +#endif + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C2 +#define USE_BARO_BMP280 +#define USE_BARO_DPS310 + +// *************** UART ***************************** +#define USE_VCP +#define USE_USB_DETECT +//#define USB_DETECT_PIN PC5 + + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 +#define USE_UART_INVERTER +#define INVERTER_PIN_UART3_RX PC9 +#define INVERTER_PIN_USART3_RX PC9 + +#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 PD2 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define SERIAL_PORT_COUNT 6 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define SERIALRX_UART SERIAL_PORT_USART3 + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_CHANNEL5 +#define ADC_CHANNEL1_PIN PC2 +#define ADC_CHANNEL2_PIN PC1 +#define ADC_CHANNEL3_PIN PC0 +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 + +#define USE_LED_STRIP +#define WS2811_PIN PB6 + +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_LED_STRIP ) +#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC +#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC + +#define VBAT_SCALE_DEFAULT 110 +#define CURRENT_METER_SCALE_DEFAULT 800 + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE BIT(2) + + +#define MAX_PWM_OUTPUT_PORTS 8 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE +//#define USE_DSHOT +//#define USE_ESC_SENSOR +#define USE_ESCSERIAL \ No newline at end of file diff --git a/src/main/target/BETAFPVF722/target.c b/src/main/target/BETAFPVF722/target.c index 6858f7e05b7..e24689c0ca9 100755 --- a/src/main/target/BETAFPVF722/target.c +++ b/src/main/target/BETAFPVF722/target.c @@ -25,20 +25,20 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM1, CH3, PA10, TIM_USE_PPM, 0, 0), // PPM + DEF_TIM(TIM1, CH3, PA10, TIM_USE_PPM, 0, 0), // PPM - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR, 0, 0), // S1 D(1, 4, 5) - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR, 0, 0), // S2 D(2, 3, 7) - DEF_TIM(TIM2, CH2, PA1, TIM_USE_MC_MOTOR, 0, 0), // S3 D(2, 4, 7) - DEF_TIM(TIM2, CH1, PA0, TIM_USE_MC_MOTOR, 0, 0), // S4 D(2, 7, 7) + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0), // S1 D(1, 4, 5) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0), // S2 D(2, 3, 7) + DEF_TIM(TIM2, CH2, PA1, TIM_USE_MOTOR, 0, 0), // S3 D(2, 4, 7) + DEF_TIM(TIM2, CH1, PA0, TIM_USE_MOTOR, 0, 0), // S4 D(2, 7, 7) - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR, 0, 0), // S5 DMA1_ST2 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 DMA1_ST2 - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR, 0, 0), // S6 DMA2_ST6 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 DMA2_ST6 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_ANY, 0, 0), //camera + DEF_TIM(TIM8, CH3, PC8, TIM_USE_ANY, 0, 0), //camera - DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED STRIP D(1, 5, 3) + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED STRIP D(1, 5, 3) }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/BLACKPILL_F411/target.c b/src/main/target/BLACKPILL_F411/target.c index a7f6215215b..d36b8c19c24 100644 --- a/src/main/target/BLACKPILL_F411/target.c +++ b/src/main/target/BLACKPILL_F411/target.c @@ -24,12 +24,12 @@ #include "drivers/pinio.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 D(1,4,5) - DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 D(1,5,5) - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S3 D(2,1,6) - DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S4 D(2,2,6) - DEF_TIM(TIM1, CH3, PA10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S5 D(2,6,6) - DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6 D(1,7,2) + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 D(1,4,5) + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 D(1,5,5) + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 1), // S3 D(2,1,6) + DEF_TIM(TIM1, CH2, PA9, TIM_USE_OUTPUT_AUTO, 0, 1), // S4 D(2,2,6) + DEF_TIM(TIM1, CH3, PA10, TIM_USE_OUTPUT_AUTO, 0, 1), // S5 D(2,6,6) + DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 D(1,7,2) DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), //TX2 pad - softserial_tx2 diff --git a/src/main/target/BLUEJAYF4/target.c b/src/main/target/BLUEJAYF4/target.c index 3b6dbb4bda5..0b968380cf0 100644 --- a/src/main/target/BLUEJAYF4/target.c +++ b/src/main/target/BLUEJAYF4/target.c @@ -25,13 +25,13 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM, 0, 0 ), // PPM IN - DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S1_OUT - DMA1_ST2 - DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S2_OUT - DMA1_ST4 - DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S3_OUT - DMA1_ST1 - DEF_TIM(TIM9, CH2, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S4_OUT - no DMA - // DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1 ), // S4_OUT - DMA1_ST3 (Could be DMA1_ST1 with dmaopt=0) - DEF_TIM(TIM3, CH4, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_LED, 0, 0 ), // S5_OUT - DMA1_ST2 - DEF_TIM(TIM3, CH3, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S6_OUT - DMA1_ST7 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S1_OUT - DMA1_ST2 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S2_OUT - DMA1_ST4 + DEF_TIM(TIM2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S3_OUT - DMA1_ST1 + DEF_TIM(TIM9, CH2, PA3, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S4_OUT - no DMA + //DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 1 ), // S4_OUT - DMA1_ST3 (Could be DMA1_ST1 with dmaopt=0) + DEF_TIM(TIM3, CH4, PB0, TIM_USE_OUTPUT_AUTO | TIM_USE_LED, 0, 0 ), // S5_OUT - DMA1_ST2 + DEF_TIM(TIM3, CH3, PB1, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S6_OUT - DMA1_ST7 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/CLRACINGF4AIR/config.c b/src/main/target/CLRACINGF4AIR/config.c index fa0d0196d70..4e4234cb0a8 100644 --- a/src/main/target/CLRACINGF4AIR/config.c +++ b/src/main/target/CLRACINGF4AIR/config.c @@ -52,4 +52,9 @@ void targetConfiguration(void) { compassConfigMutable()->mag_align = CW90_DEG; + +#if defined(CLRACINGF4AIRV2) || defined(CLRACINGF4AIRV3) + // To improve backwards compatibility with INAV versions 6.x and older + timerOverridesMutable(timer2id(TIM8))->outputMode = OUTPUT_MODE_MOTORS; +#endif } diff --git a/src/main/target/CLRACINGF4AIR/target.c b/src/main/target/CLRACINGF4AIR/target.c index 490372c048e..54fa14c7097 100644 --- a/src/main/target/CLRACINGF4AIR/target.c +++ b/src/main/target/CLRACINGF4AIR/target.c @@ -25,19 +25,19 @@ DEF_TIM(TIM4, CH3, PB8, TIM_USE_LED, 0, 0), #if defined(CLRACINGF4AIRV2) || defined(CLRACINGF4AIRV3) - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM12, CH2, PB15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), + DEF_TIM(TIM12, CH2, PB15, TIM_USE_OUTPUT_AUTO, 0, 0), + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), #else - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), - DEF_TIM(TIM9, CH2, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM12, CH2, PB15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), + DEF_TIM(TIM9, CH2, PA3, TIM_USE_OUTPUT_AUTO, 0, 0), + DEF_TIM(TIM2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0), + DEF_TIM(TIM12, CH2, PB15, TIM_USE_OUTPUT_AUTO, 0, 0), + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), #endif }; diff --git a/src/main/target/COLIBRI/target.c b/src/main/target/COLIBRI/target.c index dbeb9c083fa..932188992bc 100755 --- a/src/main/target/COLIBRI/target.c +++ b/src/main/target/COLIBRI/target.c @@ -33,14 +33,14 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM5, CH1, PA0, TIM_USE_PWM, 0, 0), // S7_IN DEF_TIM(TIM5, CH2, PA1, TIM_USE_PWM, 0, 0), // S8_IN - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S1_OUT - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S2_OUT - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3_OUT - DEF_TIM(TIM12, CH2, PB15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4_OUT - DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5_OUT - DEF_TIM(TIM12, CH1, PB14, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6_OUT - DEF_TIM(TIM10, CH1, PB8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S7_OUT - DEF_TIM(TIM11, CH1, PB9, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S8_OUT + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1_OUT + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S2_OUT + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S3_OUT + DEF_TIM(TIM12, CH2, PB15, TIM_USE_OUTPUT_AUTO, 0, 0), // S4_OUT + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // S5_OUT + DEF_TIM(TIM12, CH1, PB14, TIM_USE_OUTPUT_AUTO, 0, 0), // S6_OUT + DEF_TIM(TIM10, CH1, PB8, TIM_USE_OUTPUT_AUTO, 0, 0), // S7_OUT + DEF_TIM(TIM11, CH1, PB9, TIM_USE_OUTPUT_AUTO, 0, 0), // S8_OUT DEF_TIM(TIM4, CH2, PB7, TIM_USE_LED, 0, 0) }; diff --git a/src/main/target/DALRCF405/target.c b/src/main/target/DALRCF405/target.c index add81e2b284..e1cddea2613 100644 --- a/src/main/target/DALRCF405/target.c +++ b/src/main/target/DALRCF405/target.c @@ -24,14 +24,14 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM12, CH2, PB15, TIM_USE_PPM, 0, 0), // PPM - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S1 (1,7) - DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S2 (2,2) - DEF_TIM(TIM1, CH3, PA10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 (2,6) - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S4 (2,1) (2.3 2.6) - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S5 (2,4) (2.2) - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S6 (1,2) - DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S7 (2,3) - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S8 (2,7) + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 (1,7) + DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 (2,2) + DEF_TIM(TIM1, CH3, PA10, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 (2,6) + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 1), // S4 (2,1) (2.3 2.6) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 1), // S5 (2,4) (2.2) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 (1,2) + DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 1), // S7 (2,3) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 (2,7) DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), // LED STRIP(1,0) DEF_TIM(TIM2, CH1, PA5, TIM_USE_ANY, 0, 0), // FC CAM diff --git a/src/main/target/DALRCF722DUAL/config.c b/src/main/target/DALRCF722DUAL/config.c new file mode 100644 index 00000000000..e9e6c9fce06 --- /dev/null +++ b/src/main/target/DALRCF722DUAL/config.c @@ -0,0 +1,29 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include +#include + +#include + +#include "drivers/pwm_mapping.h" + +void targetConfiguration(void) +{ + // To improve backwards compatibility with INAV versions 6.x and older + timerOverridesMutable(timer2id(TIM1))->outputMode = OUTPUT_MODE_MOTORS; +} diff --git a/src/main/target/DALRCF722DUAL/target.c b/src/main/target/DALRCF722DUAL/target.c index 8ea225e4123..286c00539a3 100644 --- a/src/main/target/DALRCF722DUAL/target.c +++ b/src/main/target/DALRCF722DUAL/target.c @@ -30,12 +30,12 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM4, CH2, PB7, TIM_USE_PPM ,0, 0), - DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO,0,0), //S1 DMA2_ST2 T8CH1 - DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO,0,0), //S2 DMA2_ST3 T8CH2 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO,0,0), //S3 DMA2_ST4 T8CH3 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO,0,0), //S4 DMA2_ST7 T8CH4 - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR,0,0), //S5 DMA2_ST1 T1CH1 - DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR,0,0), //S6 DMA2_ST6 T1CH2 + DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO,0,0), //S1 DMA2_ST2 T8CH1 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO,0,0), //S2 DMA2_ST3 T8CH2 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO,0,0), //S3 DMA2_ST4 T8CH3 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO,0,0), //S4 DMA2_ST7 T8CH4 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO,0,0), //S5 DMA2_ST1 T1CH1 + DEF_TIM(TIM1, CH2, PA9, TIM_USE_OUTPUT_AUTO,0,0), //S6 DMA2_ST6 T1CH2 DEF_TIM(TIM2, CH1, PA15,TIM_USE_LED,0,0), //2812 STRIP DMA1_ST5 }; diff --git a/src/main/target/F4BY/target.c b/src/main/target/F4BY/target.c index 5565713c03c..37d9db89418 100644 --- a/src/main/target/F4BY/target.c +++ b/src/main/target/F4BY/target.c @@ -15,14 +15,14 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM4, CH2, PD13, TIM_USE_PWM, 0, 0), // S7_IN DEF_TIM(TIM4, CH1, PD12, TIM_USE_PWM, 0, 0), // S8_IN - DEF_TIM(TIM2, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1_OUT - DEF_TIM(TIM2, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2_OUT - DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3_OUT - DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4_OUT - DEF_TIM(TIM1, CH1, PE9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5_OUT - DEF_TIM(TIM1, CH2, PE11, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6_OUT - DEF_TIM(TIM1, CH3, PE13, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S7_OUT - DEF_TIM(TIM1, CH4, PE14, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S8_OUT + DEF_TIM(TIM2, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1_OUT + DEF_TIM(TIM2, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0), // S2_OUT + DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0), // S3_OUT + DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 0), // S4_OUT + DEF_TIM(TIM1, CH1, PE9, TIM_USE_OUTPUT_AUTO, 0, 0), // S5_OUT + DEF_TIM(TIM1, CH2, PE11, TIM_USE_OUTPUT_AUTO, 0, 0), // S6_OUT + DEF_TIM(TIM1, CH3, PE13, TIM_USE_OUTPUT_AUTO, 0, 0), // S7_OUT + DEF_TIM(TIM1, CH4, PE14, TIM_USE_OUTPUT_AUTO, 0, 0), // S8_OUT DEF_TIM(TIM9, CH2, PE6, TIM_USE_ANY, 0, 0), // sonar echo if needed }; diff --git a/src/main/target/FF_F35_LIGHTNING/config.c b/src/main/target/FF_F35_LIGHTNING/config.c index d0825a06fe6..cec7b4f8b4c 100644 --- a/src/main/target/FF_F35_LIGHTNING/config.c +++ b/src/main/target/FF_F35_LIGHTNING/config.c @@ -19,7 +19,7 @@ #include #include "config/config_master.h" -#include "flight/mixer.h" +#include "flight/mixer_profile.h" #include "rx/rx.h" #include "io/serial.h" #include "telemetry/telemetry.h" diff --git a/src/main/target/FF_F35_LIGHTNING/target.c b/src/main/target/FF_F35_LIGHTNING/target.c index 61180587f07..83d75497e4d 100644 --- a/src/main/target/FF_F35_LIGHTNING/target.c +++ b/src/main/target/FF_F35_LIGHTNING/target.c @@ -23,12 +23,12 @@ timerHardware_t timerHardware[] = { - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1, 0), - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1, 0), - DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), - DEF_TIM(TIM4, CH4, PB9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 1, 0), + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 1, 0), + DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 1, 0), + DEF_TIM(TIM4, CH4, PB9, TIM_USE_OUTPUT_AUTO, 1, 0), + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 1, 0), + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 1, 0), DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), DEF_TIM(TIM5, CH4, PA3, TIM_USE_ANY, 0, 0), diff --git a/src/main/target/FF_FORTINIF4/target.c b/src/main/target/FF_FORTINIF4/target.c index 853104d01fc..5853bc31391 100644 --- a/src/main/target/FF_FORTINIF4/target.c +++ b/src/main/target/FF_FORTINIF4/target.c @@ -23,10 +23,10 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1, 0 ), // S1_OUT - DMA1_ST7 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1, 0 ), // S2_OUT - DMA1_ST2 - DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 1 ), // S3_OUT - DMA1_ST6 - DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0 ), // S4_OUT - DMA1_ST1 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 1, 0 ), // S1_OUT - DMA1_ST7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 1, 0 ), // S2_OUT - DMA1_ST2 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 1, 1 ), // S3_OUT - DMA1_ST6 + DEF_TIM(TIM2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 1, 0 ), // S4_OUT - DMA1_ST1 DEF_TIM(TIM4, CH4, PB9, TIM_USE_PPM, 0, 0 ), // PPM IN DEF_TIM(TIM4, CH2, PB7, TIM_USE_LED, 0, 0), // LED - DMA1_ST3 }; diff --git a/src/main/target/FF_PIKOF4/target.c b/src/main/target/FF_PIKOF4/target.c index ba3cfc51620..581dcd588ef 100644 --- a/src/main/target/FF_PIKOF4/target.c +++ b/src/main/target/FF_PIKOF4/target.c @@ -26,17 +26,17 @@ timerHardware_t timerHardware[] = { #if defined(FF_PIKOF4OSD) - DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), - DEF_TIM(TIM3, CH3, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), - DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), - DEF_TIM(TIM3, CH4, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), - DEF_TIM(TIM12, CH1, PB14, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), - DEF_TIM(TIM12, CH2, PB15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), + DEF_TIM(TIM2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 0 ), + DEF_TIM(TIM3, CH3, PB1, TIM_USE_OUTPUT_AUTO, 0, 0 ), + DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0 ), + DEF_TIM(TIM3, CH4, PB0, TIM_USE_OUTPUT_AUTO, 0, 0 ), + DEF_TIM(TIM12, CH1, PB14, TIM_USE_OUTPUT_AUTO, 0, 0 ), + DEF_TIM(TIM12, CH2, PB15, TIM_USE_OUTPUT_AUTO, 0, 0 ), #else - DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), - DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), + DEF_TIM(TIM2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 0 ), + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0 ), + DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0 ), + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0 ), #endif DEF_TIM(TIM4, CH2, PB7, TIM_USE_LED, 0, 0 ), }; diff --git a/src/main/target/FIREWORKSV2/config.c b/src/main/target/FIREWORKSV2/config.c index ed075a2a9bc..930df0630df 100644 --- a/src/main/target/FIREWORKSV2/config.c +++ b/src/main/target/FIREWORKSV2/config.c @@ -26,10 +26,14 @@ #include #include "drivers/io.h" +#include "drivers/pwm_mapping.h" #include "rx/rx.h" #include "io/serial.h" void targetConfiguration(void) { serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_SMARTPORT; + + // To improve backwards compatibility with INAV versions 6.x and older + timerOverridesMutable(timer2id(TIM8))->outputMode = OUTPUT_MODE_MOTORS; } diff --git a/src/main/target/FIREWORKSV2/target.c b/src/main/target/FIREWORKSV2/target.c index 133c2bd1d7e..8e7cdb95754 100644 --- a/src/main/target/FIREWORKSV2/target.c +++ b/src/main/target/FIREWORKSV2/target.c @@ -44,20 +44,20 @@ BUSDEV_REGISTER_SPI_TAG(busdev_2_mpu6000, DEVHW_MPU6000, IMU_2_SPI_BUS, IMU_ BUSDEV_REGISTER_SPI_TAG(busdev_2_mpu6500, DEVHW_MPU6500, IMU_2_SPI_BUS, IMU_2_CS_PIN, NONE, 1, DEVFLAGS_NONE, IMU_2_ALIGN); timerHardware_t timerHardware[] = { - DEF_TIM(TIM10, CH1, PB8, TIM_USE_PPM, 0, 0), // PPM + DEF_TIM(TIM10, CH1, PB8, TIM_USE_PPM, 0, 0), // PPM // Motor output 1: use different set of timers for MC and FW - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR, 1, 0), // S1_OUT D(1,7) - DEF_TIM(TIM8, CH2N, PB0, TIM_USE_FW_MOTOR, 1, 1), // S1_OUT D(2,2,0),D(2,3,7) + //DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR | TIM _USE_SERVO, 1, 0), // S1_OUT D(1,7) + DEF_TIM(TIM8, CH2N, PB0, TIM_USE_OUTPUT_AUTO, 1, 1), // S1_OUT D(2,2,0),D(2,3,7) // used to fw motor // Motor output 2: use different set of timers for MC and FW - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR, 1, 0), // S2_OUT D(1,2) - DEF_TIM(TIM8, CH3N, PB1, TIM_USE_FW_MOTOR, 1, 1), // S2_OUT D(2,2,0),D(2,4,7) + //DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 1, 0), // S2_OUT D(1,2) + DEF_TIM(TIM8, CH3N, PB1, TIM_USE_OUTPUT_AUTO, 1, 1), // S2_OUT D(2,2,0),D(2,4,7) // used to be fw motor - DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 1), // S3_OUT D(1,6) - DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S4_OUT D(1,5) - DEF_TIM(TIM3, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5_OUT D(1,7) - DEF_TIM(TIM3, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6_OUT D(1,8) + DEF_TIM(TIM2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 1, 1), // S3_OUT D(1,6) + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 1, 0), // S4_OUT D(1,5) + DEF_TIM(TIM3, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S5_OUT D(1,7) + DEF_TIM(TIM3, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S6_OUT D(1,8) DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), // LED strip D(1,0) @@ -66,6 +66,7 @@ timerHardware_t timerHardware[] = { const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); +/* TODO: add DSHOT_DMAR? #ifdef USE_DSHOT void validateAndFixTargetConfig(void) { @@ -76,4 +77,5 @@ void validateAndFixTargetConfig(void) } } } -#endif \ No newline at end of file +#endif +*/ diff --git a/src/main/target/FISHDRONEF4/target.c b/src/main/target/FISHDRONEF4/target.c index d12423798fa..b0aec49bc4d 100644 --- a/src/main/target/FISHDRONEF4/target.c +++ b/src/main/target/FISHDRONEF4/target.c @@ -30,12 +30,12 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM2, CH4, PB11, TIM_USE_PWM, 0, 0 ), DEF_TIM(TIM1, CH1, PA8, TIM_USE_PWM, 0, 0 ), - DEF_TIM(TIM12, CH1, PB14, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), - DEF_TIM(TIM12, CH2, PB15, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), - DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), - DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), + DEF_TIM(TIM12, CH1, PB14, TIM_USE_OUTPUT_AUTO, 0, 0 ), + DEF_TIM(TIM12, CH2, PB15, TIM_USE_OUTPUT_AUTO, 0, 0 ), + DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0 ), + DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0 ), + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0 ), + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0 ), DEF_TIM(TIM3, CH4, PB1, TIM_USE_LED, 0, 0 ), }; diff --git a/src/main/target/FLYCOLORF7MINI/target.c b/src/main/target/FLYCOLORF7MINI/target.c index 146aa9fe1c5..411a4fbe403 100644 --- a/src/main/target/FLYCOLORF7MINI/target.c +++ b/src/main/target/FLYCOLORF7MINI/target.c @@ -29,12 +29,12 @@ BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500, DEVHW_MPU6500, MPU6500_SPI_BUS, MPU6500_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_MPU6500_ALIGN); timerHardware_t timerHardware[] = { - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 - DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 - DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S5 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 + DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // WS2812B }; diff --git a/src/main/target/FLYWOOF405PRO/target.c b/src/main/target/FLYWOOF405PRO/target.c index 753f9046135..f61d06ca0e1 100644 --- a/src/main/target/FLYWOOF405PRO/target.c +++ b/src/main/target/FLYWOOF405PRO/target.c @@ -22,15 +22,15 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 D(1,3,2) - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 D(1,0,2) + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 D(1,3,2) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 D(1,0,2) - DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S3 D(1,7,5) - DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 D(1,2,5) - DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 D(2,4,7) - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S7 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S8 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 1), // S3 D(1,7,5) + DEF_TIM(TIM2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 D(1,2,5) + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 D(2,4,7) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 DEF_TIM(TIM1, CH2, PA9, TIM_USE_LED, 0, 0), //2812LED D(1,5,3) diff --git a/src/main/target/FLYWOOF411/target.c b/src/main/target/FLYWOOF411/target.c index 56d8e7afb40..965974162b0 100644 --- a/src/main/target/FLYWOOF411/target.c +++ b/src/main/target/FLYWOOF411/target.c @@ -27,20 +27,20 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM9, CH1, PA2, TIM_USE_PPM, 0, 0), // PPM IN #ifdef FLYWOOF411_V2 - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S1 - D(2,1) - DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 - D(1,6) - DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 - D(1,1) - DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 - D(1,5) + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 1), // S1 - D(2,1) + DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 - D(1,6) + DEF_TIM(TIM2, CH3, PB10, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 - D(1,1) + DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 - D(1,5) DEF_TIM(TIM4, CH1, PB6, TIM_USE_ANY, 0, 0), // SOFTSERIAL_1_TX_PIN - D(1,0) DEF_TIM(TIM4, CH2, PB7, TIM_USE_ANY, 0, 0), // SOFTSERIAL_1_RX_PIN - D(1,3) DEF_TIM(TIM3, CH3, PB0, TIM_USE_ANY, 0, 0), // RSSI_ADC_CHANNEL 1-7 DEF_TIM(TIM3, CH1, PB4, TIM_USE_ANY, 0, 0), // 1-4 DEF_TIM(TIM5, CH1, PA0, TIM_USE_LED, 0, 0), // LED 1,2 #else - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S1_OUT 2,1 - DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S2_OUT 2,2 - DEF_TIM(TIM1, CH3, PA10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3_OUT 2,6 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S4_OUT 1,7 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 1), // S1_OUT 2,1 + DEF_TIM(TIM1, CH2, PA9, TIM_USE_OUTPUT_AUTO, 0, 1), // S2_OUT 2,2 + DEF_TIM(TIM1, CH3, PA10, TIM_USE_OUTPUT_AUTO, 0, 0), // S3_OUT 2,6 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR , 0, 0), // S4_OUT 1,7 DEF_TIM(TIM3, CH4, PB1, TIM_USE_ANY, 0, 0), // RSSI 1,2 DEF_TIM(TIM5, CH4, PA3, TIM_USE_ANY, 0, 1), // RX2 1,0 diff --git a/src/main/target/FLYWOOF745/target.c b/src/main/target/FLYWOOF745/target.c index 302add553ec..879b66d01fa 100644 --- a/src/main/target/FLYWOOF745/target.c +++ b/src/main/target/FLYWOOF745/target.c @@ -30,17 +30,17 @@ #include "drivers/pinio.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM1, CH3, PE13, TIM_USE_PPM, 0, 1), // PPM, DMA2_ST6 + DEF_TIM(TIM1, CH3, PE13, TIM_USE_PPM, 0, 1), // PPM, DMA2_ST6 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // M1 , DMA1_ST7 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // M2 , DMA1_ST2 - DEF_TIM(TIM1, CH1, PE9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 2), // M3 , DMA2_ST2 - DEF_TIM(TIM1, CH2, PE11, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // M4 , DMA2_ST4 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), // M5 , DMA2_ST7 - DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), // M6 , DMA1_ST1 - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), // M7 , DMA1_ST4 - DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), // M8 , DMA1_ST5 - DEF_TIM(TIM4, CH1, PD12, TIM_USE_LED, 0, 0), // LED_STRIP, DMA1_ST0 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // M1 , DMA1_ST7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // M2 , DMA1_ST2 + DEF_TIM(TIM1, CH1, PE9, TIM_USE_OUTPUT_AUTO, 0, 2), // M3 , DMA2_ST2 + DEF_TIM(TIM1, CH2, PE11, TIM_USE_OUTPUT_AUTO, 0, 1), // M4 , DMA2_ST4 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // M5 , DMA2_ST7 + DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 0), // M6 , DMA1_ST1 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // M7 , DMA1_ST4 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // M8 , DMA1_ST5 + DEF_TIM(TIM4, CH1, PD12, TIM_USE_LED, 0, 0), // LED_STRIP, DMA1_ST0 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/FLYWOOF7DUAL/target.c b/src/main/target/FLYWOOF7DUAL/target.c index 1af14572493..d9d5482e3a5 100644 --- a/src/main/target/FLYWOOF7DUAL/target.c +++ b/src/main/target/FLYWOOF7DUAL/target.c @@ -40,12 +40,12 @@ BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500_2, DEVHW_MPU6500, GYRO_2_SPI_BUS, GYRO timerHardware_t timerHardware[] = { DEF_TIM(TIM8, CH1, PC6, TIM_USE_PPM, 0, 0), // PPM&SBUS - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 - D(1,2) - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 - D(1,4) - DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 - D(1,6) - DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 - D(1,5) - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 - D(2,4) - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 - D(2,1) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 - D(1,2) + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 - D(1,4) + DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 - D(1,6) + DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 - D(1,5) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 - D(2,4) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 - D(2,1) DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED STRIP(1,5) }; diff --git a/src/main/target/FOXEERF405/target.c b/src/main/target/FOXEERF405/target.c index cf367ba00cb..1ad827bb5ca 100644 --- a/src/main/target/FOXEERF405/target.c +++ b/src/main/target/FOXEERF405/target.c @@ -24,16 +24,16 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM12, CH2, PB15, TIM_USE_PPM, 0, 0), // PPM - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 (1,7) - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 (1,4) - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 (2,4) - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 (2,7) - DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 (2,2) - DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 (2,3) + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 (1,7) + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 (1,4) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 (2,4) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 (2,7) + DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 (2,2) + DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 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_CAMERA_CONTROL, 0, 0), // Reserved (Cam Control for BF) + //DEF_TIM(TIM2, CH2, PB3, TIM_USE_CAMERA_CONTROL, 0, 0), // Reserved (Cam Control for BF) }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/FOXEERF722DUAL/target.c b/src/main/target/FOXEERF722DUAL/target.c index fca041e07ac..1933a5134ee 100644 --- a/src/main/target/FOXEERF722DUAL/target.c +++ b/src/main/target/FOXEERF722DUAL/target.c @@ -31,16 +31,16 @@ BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500, DEVHW_MPU6500, MPU6500_SPI_BUS, timerHardware_t timerHardware[] = { DEF_TIM(TIM4, CH2, PB7, TIM_USE_PPM, 0, 0), // PPM&SBUS - DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S1 - D(2, 1, 6) + DEF_TIM(TIM1, CH2, PA9, TIM_USE_OUTPUT_AUTO, 0, 1), // S1 - D(2, 1, 6) #if defined(FOXEERF722V2) - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S2 - D(2, 1, 6) + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 1), // S2 - D(2, 1, 6) #else - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 - D(2, 6, 0) + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 - D(2, 6, 0) #endif - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 - D(2, 7, 7) - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 - D(2, 4, 7) - DEF_TIM(TIM3, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 - D(1, 4, 5) - DEF_TIM(TIM3, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 - D(1, 5, 5) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 - D(2, 7, 7) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 - D(2, 4, 7) + DEF_TIM(TIM3, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 - D(1, 4, 5) + DEF_TIM(TIM3, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 - D(1, 5, 5) DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), // LED STRIP(1,5) diff --git a/src/main/target/FOXEERF722V4/target.c b/src/main/target/FOXEERF722V4/target.c index 2cadae4d77f..be84ee8b909 100644 --- a/src/main/target/FOXEERF722V4/target.c +++ b/src/main/target/FOXEERF722V4/target.c @@ -25,16 +25,16 @@ timerHardware_t timerHardware[] = { - DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S1 - D(2, 1, 6) - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S2 - D(2, 1, 6) - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 - D(2, 7, 7) - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 - D(2, 4, 7) + DEF_TIM(TIM1, CH2, PA9, TIM_USE_OUTPUT_AUTO, 0, 1), // S1 - D(2, 1, 6) + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 1), // S2 - D(2, 1, 6) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 - D(2, 7, 7) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 - D(2, 4, 7) #ifdef FOXEERF722V4_X8 - DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 - DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 - DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 - DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 + DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 + DEF_TIM(TIM2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 #endif DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), // LED STRIP(1,5) diff --git a/src/main/target/FOXEERF745AIO/target.c b/src/main/target/FOXEERF745AIO/target.c index d3ef6a067d6..f724f65913c 100644 --- a/src/main/target/FOXEERF745AIO/target.c +++ b/src/main/target/FOXEERF745AIO/target.c @@ -27,10 +27,10 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED STRIP - D(2, 6, 0) - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR, 0, 0), // M1 - D(1, 4, 5) - DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR, 0, 0), // M2 - D(1, 5, 5) - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR, 0, 0), // M3 - D(1, 2, 5) - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR, 0, 0), // M4 - D(1, 7, 5) + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MOTOR, 0, 0), // M1 - D(1, 4, 5) + DEF_TIM(TIM3, CH2, PB5, TIM_USE_MOTOR, 0, 0), // M2 - D(1, 5, 5) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0), // M3 - D(1, 2, 5) + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0), // M4 - D(1, 7, 5) }; diff --git a/src/main/target/FOXEERH743/config.c b/src/main/target/FOXEERH743/config.c index b83a0a0d03b..e6e31bace2d 100644 --- a/src/main/target/FOXEERH743/config.c +++ b/src/main/target/FOXEERH743/config.c @@ -28,6 +28,7 @@ #include "drivers/sensor.h" #include "drivers/pwm_esc_detect.h" #include "drivers/pwm_output.h" +#include "drivers/pwm_mapping.h" #include "drivers/serial.h" #include "fc/rc_controls.h" @@ -53,5 +54,7 @@ void targetConfiguration(void) * UART1 is SerialRX */ serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].functionMask = FUNCTION_RX_SERIAL; - + + // To improve backwards compatibility with INAV versions 6.x and older + timerOverridesMutable(timer2id(TIM8))->outputMode = OUTPUT_MODE_MOTORS; } diff --git a/src/main/target/FOXEERH743/target.c b/src/main/target/FOXEERH743/target.c index c8bc9e7adaa..3ab6e195294 100644 --- a/src/main/target/FOXEERH743/target.c +++ b/src/main/target/FOXEERH743/target.c @@ -28,18 +28,18 @@ timerHardware_t timerHardware[] = { - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S1 - DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S2 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 2), // S3 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 3), // S4 - - DEF_TIM(TIM4, CH1, PD12, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 4), // S5 - DEF_TIM(TIM4, CH2, PD13, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 5), // S6 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 6), // S7 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 7), // S8 - - DEF_TIM(TIM15, CH1, PE5, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S9 - DEF_TIM(TIM15, CH2, PE6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S10 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 1), // S2 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 2), // S3 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 3), // S4 + + DEF_TIM(TIM4, CH1, PD12, TIM_USE_OUTPUT_AUTO, 0, 4), // S5 + DEF_TIM(TIM4, CH2, PD13, TIM_USE_OUTPUT_AUTO, 0, 5), // S6 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 6), // S7 // used to be fw motor + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 7), // S8 // used to be fw motor + + DEF_TIM(TIM15, CH1, PE5, TIM_USE_OUTPUT_AUTO, 0, 0), // S9 + DEF_TIM(TIM15, CH2, PE6, TIM_USE_OUTPUT_AUTO, 0, 0), // S10 DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 9), // LED_2812 }; diff --git a/src/main/target/FRSKYF4/target.c b/src/main/target/FRSKYF4/target.c index 39b1be48905..cd83979c505 100755 --- a/src/main/target/FRSKYF4/target.c +++ b/src/main/target/FRSKYF4/target.c @@ -23,12 +23,12 @@ #include "drivers/pwm_mapping.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1, 0), // S1_OUT - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1, 0), // S2_OUT - DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S3_OUT - DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S4_OUT - DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S5_OUT - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S6_OUT + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 1, 0), // S1_OUT + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 1, 0), // S2_OUT + DEF_TIM(TIM2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 1, 0), // S3_OUT + DEF_TIM(TIM2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 1, 0), // S4_OUT + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 1, 0), // S5_OUT + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 1, 0), // S6_OUT DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), // LED strip }; diff --git a/src/main/target/FRSKYPILOT/target.c b/src/main/target/FRSKYPILOT/target.c index 8594a6f7572..50b82d7687e 100644 --- a/src/main/target/FRSKYPILOT/target.c +++ b/src/main/target/FRSKYPILOT/target.c @@ -30,21 +30,21 @@ BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500, DEVHW_MPU6500, MPU6500_SPI_BUS, BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU6000_CS_PIN, NONE, 1, DEVFLAGS_NONE, IMU_MPU6000_ALIGN); timerHardware_t timerHardware[] = { - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S1 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S2 - DEF_TIM(TIM1, CH1, PE9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 - DEF_TIM(TIM1, CH2, PE11, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 - DEF_TIM(TIM1, CH3, PE13, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 - DEF_TIM(TIM1, CH4, PE14, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S7 - DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S8 - DEF_TIM(TIM4, CH4, PB9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S9 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + DEF_TIM(TIM1, CH1, PE9, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 + DEF_TIM(TIM1, CH2, PE11, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 + DEF_TIM(TIM1, CH3, PE13, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 + DEF_TIM(TIM1, CH4, PE14, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 + DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 + DEF_TIM(TIM4, CH4, PB9, TIM_USE_OUTPUT_AUTO, 0, 0), // S9 #ifdef FRSKYPILOT_LED DEF_TIM(TIM2, CH2, PA1, TIM_USE_LED | TIM_USE_LED, 0, 0), // S10 now LED, S11 & S12 UART 3 only #else - DEF_TIM(TIM2, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S10 - DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S11 - DEF_TIM(TIM2, CH4, PB11, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S12 + DEF_TIM(TIM2, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0), // S10 + DEF_TIM(TIM2, CH3, PB10, TIM_USE_OUTPUT_AUTO, 0, 0), // S11 + DEF_TIM(TIM2, CH4, PB11, TIM_USE_OUTPUT_AUTO, 0, 0), // S12 #endif DEF_TIM(TIM5, CH1, PA0, TIM_USE_BEEPER, 0, 0), // beeper diff --git a/src/main/target/FRSKY_ROVERF7/target.c b/src/main/target/FRSKY_ROVERF7/target.c index 25a5889f64f..6d8ecd0271a 100755 --- a/src/main/target/FRSKY_ROVERF7/target.c +++ b/src/main/target/FRSKY_ROVERF7/target.c @@ -25,12 +25,12 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM3, CH3, PC8, TIM_USE_MC_MOTOR, 0, 0), // M1 - DEF_TIM(TIM3, CH4, PC9, TIM_USE_MC_MOTOR, 0, 0), // M2 - DEF_TIM(TIM1, CH2N, PB0, TIM_USE_MC_MOTOR, 0, 0), // M3 + DEF_TIM(TIM3, CH3, PC8, TIM_USE_MOTOR, 0, 0), // M1 + DEF_TIM(TIM3, CH4, PC9, TIM_USE_MOTOR, 0, 0), // M2 + DEF_TIM(TIM1, CH2N, PB0, TIM_USE_MOTOR, 0, 0), // M3 - DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_SERVO, 0, 0), // Servo left - DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_SERVO, 0, 0), // Servo right + DEF_TIM(TIM8, CH2, PC7, TIM_USE_SERVO, 0, 0), // Servo left + DEF_TIM(TIM8, CH1, PC6, TIM_USE_SERVO, 0, 0), // Servo right DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), // LED }; diff --git a/src/main/target/FURYF4OSD/target.c b/src/main/target/FURYF4OSD/target.c index 8ca21cdc7d6..c820d0c378f 100644 --- a/src/main/target/FURYF4OSD/target.c +++ b/src/main/target/FURYF4OSD/target.c @@ -24,10 +24,10 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM8, CH4, PC9, TIM_USE_PPM, 0, 0 ), // PPM IN - DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1 ), // S1_OUT - D(1, 6, 3) - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S2_OUT - D(1, 7, 5) - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S3_OUT - D(1, 2, 5) - DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S4_OUT - D(1, 1, 3) + DEF_TIM(TIM2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 1 ), // S1_OUT - D(1, 6, 3) + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S2_OUT - D(1, 7, 5) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S3_OUT - D(1, 2, 5) + DEF_TIM(TIM2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S4_OUT - D(1, 1, 3) DEF_TIM(TIM2, CH3, PB10, TIM_USE_ANY, 0, 0), DEF_TIM(TIM2, CH4, PB11, TIM_USE_ANY, 0, 0), DEF_TIM(TIM5, CH1, PA0, TIM_USE_LED, 0, 0 ), // LED_STRIP - DMA1_ST2_CH6 diff --git a/src/main/target/GEPRCF405/target.c b/src/main/target/GEPRCF405/target.c index ace9b307366..68a900898c1 100644 --- a/src/main/target/GEPRCF405/target.c +++ b/src/main/target/GEPRCF405/target.c @@ -31,12 +31,12 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM, 0, 0), - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), - DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), - DEF_TIM(TIM1, CH3, PA10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 1), + DEF_TIM(TIM1, CH2, PA9, TIM_USE_OUTPUT_AUTO, 0, 1), + DEF_TIM(TIM1, CH3, PA10, TIM_USE_OUTPUT_AUTO, 0, 1), + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 1), + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), DEF_TIM(TIM3, CH4, PB1, TIM_USE_LED, 0, 0), }; diff --git a/src/main/target/GEPRCF722/target.c b/src/main/target/GEPRCF722/target.c index 8e206e7b199..851ae376a54 100644 --- a/src/main/target/GEPRCF722/target.c +++ b/src/main/target/GEPRCF722/target.c @@ -29,12 +29,12 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), diff --git a/src/main/target/GEPRCF722_BT_HD/config.c b/src/main/target/GEPRCF722_BT_HD/config.c index 1e3f19f4ba0..119559fbe6b 100644 --- a/src/main/target/GEPRCF722_BT_HD/config.c +++ b/src/main/target/GEPRCF722_BT_HD/config.c @@ -58,5 +58,8 @@ void targetConfiguration(void) serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].functionMask = FUNCTION_MSP; serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].msp_baudrateIndex = BLUETOOTH_MSP_BAUDRATE; + + // To improve backwards compatibility with INAV versions 6.x and older + timerOverridesMutable(timer2id(TIM4))->outputMode = OUTPUT_MODE_MOTORS; } diff --git a/src/main/target/GEPRCF722_BT_HD/target.c b/src/main/target/GEPRCF722_BT_HD/target.c index fa80baf92d8..d1f902eb698 100644 --- a/src/main/target/GEPRCF722_BT_HD/target.c +++ b/src/main/target/GEPRCF722_BT_HD/target.c @@ -28,20 +28,20 @@ timerHardware_t timerHardware[] = { - DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), + DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), + DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), + DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR | TIM_USE_MC_SERVO, 0, 0), - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR | TIM_USE_MC_SERVO, 0, 0), + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // used to be fw motor + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // used to be fw motor - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), - DEF_TIM(TIM2, CH2, PA1, TIM_USE_LED, 0, 0), + DEF_TIM(TIM2, CH2, PA1, TIM_USE_LED, 0, 0), }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/GEPRC_F722_AIO/target.c b/src/main/target/GEPRC_F722_AIO/target.c index 6699298db6a..a8ee276fce0 100644 --- a/src/main/target/GEPRC_F722_AIO/target.c +++ b/src/main/target/GEPRC_F722_AIO/target.c @@ -27,14 +27,14 @@ timerHardware_t timerHardware[] = { - DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), + DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MOTOR, 0, 0), + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, 0, 0), + DEF_TIM(TIM8, CH2, PC7, TIM_USE_MOTOR, 0, 0), + DEF_TIM(TIM8, CH1, PC6, TIM_USE_MOTOR, 0, 0), - DEF_TIM(TIM2, CH2, PA1, TIM_USE_LED, 0, 0), + DEF_TIM(TIM2, CH2, PA1, TIM_USE_LED, 0, 0), }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/HAKRCF405D/target.c b/src/main/target/HAKRCF405D/target.c index 3e4e0f763c2..664db476332 100644 --- a/src/main/target/HAKRCF405D/target.c +++ b/src/main/target/HAKRCF405D/target.c @@ -32,12 +32,12 @@ BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU600 timerHardware_t timerHardware[] = { DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), // PPM - DEF_TIM(TIM3, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 - DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S2 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S3 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 - DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 + DEF_TIM(TIM3, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 1), // S2 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 1), // S3 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 + DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 DEF_TIM(TIM4, CH1, PB8, TIM_USE_LED, 0, 0), // LED STRIP(2,6) diff --git a/src/main/target/HAKRCF405V2/target.c b/src/main/target/HAKRCF405V2/target.c index ba55aed0310..a44aabc0142 100644 --- a/src/main/target/HAKRCF405V2/target.c +++ b/src/main/target/HAKRCF405V2/target.c @@ -22,14 +22,14 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S1_OUT D2_ST4 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2_OUT D2_ST7 - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S3_OUT D2_ST1 - DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S4_OUT D2_ST2 - DEF_TIM(TIM1, CH3, PA10, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S5_OUT D2_ST6 - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6_OUT D1_ST4 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S7_OUT D1_ST7 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S8_OUT D1_ST2 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 1), // S1_OUT D2_ST4 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S2_OUT D2_ST7 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 1), // S3_OUT D2_ST1 + DEF_TIM(TIM1, CH2, PA9, TIM_USE_OUTPUT_AUTO, 0, 1), // S4_OUT D2_ST2 + DEF_TIM(TIM1, CH3, PA10, TIM_USE_OUTPUT_AUTO, 0, 0), // S5_OUT D2_ST6 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S6_OUT D1_ST4 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S7_OUT D1_ST7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S8_OUT D1_ST2 DEF_TIM(TIM2, CH2, PB3, TIM_USE_LED, 0, 0), // D1_ST6 }; diff --git a/src/main/target/HAKRCF411D/target.c b/src/main/target/HAKRCF411D/target.c index 7f1abaf76aa..1423d2d1992 100644 --- a/src/main/target/HAKRCF411D/target.c +++ b/src/main/target/HAKRCF411D/target.c @@ -25,10 +25,10 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM IN - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 - DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 DEF_TIM(TIM2, CH1, PA15, TIM_USE_ANY, 0, 0), // SOFTSERIAL_1_TX_PIN DEF_TIM(TIM2, CH2, PB3, TIM_USE_ANY, 0, 0), // SOFTSERIAL_1_RX_PIN diff --git a/src/main/target/HAKRCF722V2/target.c b/src/main/target/HAKRCF722V2/target.c index 6c8aec8d370..f09341633a9 100644 --- a/src/main/target/HAKRCF722V2/target.c +++ b/src/main/target/HAKRCF722V2/target.c @@ -33,14 +33,14 @@ BUSDEV_REGISTER_SPI_TAG(busdev_bmi270, DEVHW_BMI270, GYRO_SPI_BUS, GYRO1_CS_PIN, BUSDEV_REGISTER_SPI_TAG(busdev_bmi270_2, DEVHW_BMI270, GYRO_SPI_BUS, GYRO2_CS_PIN, NONE, 1, DEVFLAGS_NONE, IMU_BMI270_ALIGN); timerHardware_t timerHardware[] = { - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 - DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S4 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S5 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6 - DEF_TIM(TIM1, CH3, PA10, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S7 - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S8 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 + DEF_TIM(TIM1, CH2, PA9, TIM_USE_OUTPUT_AUTO, 0, 1), // S4 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 + DEF_TIM(TIM1, CH3, PA10, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 DEF_TIM(TIM2, CH2, PB3, TIM_USE_LED, 0, 0), // 2812LED diff --git a/src/main/target/HAKRCKD722/target.c b/src/main/target/HAKRCKD722/target.c index eb9b4d87c25..16757ff6416 100644 --- a/src/main/target/HAKRCKD722/target.c +++ b/src/main/target/HAKRCKD722/target.c @@ -32,14 +32,14 @@ BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU60 timerHardware_t timerHardware[] = { DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 - DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 - DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 - DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S7 - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S8 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 + DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 + DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED }; diff --git a/src/main/target/HGLRCF722/config.c b/src/main/target/HGLRCF722/config.c index 4f04f3b4aaa..fd3d57037b9 100644 --- a/src/main/target/HGLRCF722/config.c +++ b/src/main/target/HGLRCF722/config.c @@ -26,6 +26,8 @@ #include "platform.h" +#include "drivers/pwm_mapping.h" + #include "fc/fc_msp_box.h" #include "io/piniobox.h" @@ -34,4 +36,7 @@ void targetConfiguration(void) { pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; + + // To improve backwards compatibility with INAV versions 6.x and older + timerOverridesMutable(timer2id(TIM4))->outputMode = OUTPUT_MODE_MOTORS; } diff --git a/src/main/target/HGLRCF722/target.c b/src/main/target/HGLRCF722/target.c index faf6725705b..1ebbbf63373 100644 --- a/src/main/target/HGLRCF722/target.c +++ b/src/main/target/HGLRCF722/target.c @@ -37,14 +37,14 @@ BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, BUSDEV_REGISTER_SPI_TAG(busdev_bmi270, DEVHW_BMI270, BMI270_SPI_BUS, BMI270_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_BMI270_ALIGN); timerHardware_t timerHardware[] = { - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S1 UP1-2 D(1, 4, 5) - DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S2 UP1-2 D(1, 5, 5) - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 UP1-2 D(1, 7, 5) - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 UP1-2 D(1, 2, 5) - DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 UP1-7 D(1, 5, 3) - clash with S2 - DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 UP1-7 D(1, 6, 3) - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S7 UP1-6 D(1, 0, 2) - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S8 UP1-6 D(1, 3, 2) + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 UP1-2 D(1, 4, 5) + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 UP1-2 D(1, 5, 5) + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 UP1-2 D(1, 7, 5) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 UP1-2 D(1, 2, 5) + DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 UP1-7 D(1, 5, 3) - clash with S2 + DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 UP1-7 D(1, 6, 3) + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 UP1-6 D(1, 0, 2) + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 UP1-6 D(1, 3, 2) DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 2), // LED D(2, 6, 0) DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM, RX2 diff --git a/src/main/target/IFLIGHTF4_SUCCEXD/target.c b/src/main/target/IFLIGHTF4_SUCCEXD/target.c index 32dde9fe877..ad9ab68c9ea 100644 --- a/src/main/target/IFLIGHTF4_SUCCEXD/target.c +++ b/src/main/target/IFLIGHTF4_SUCCEXD/target.c @@ -26,10 +26,10 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), - DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR, 0, 0), - DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_MOTOR, 0, 0), - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR, 0, 0), - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR, 0, 0), + DEF_TIM(TIM5, CH1, PA0, TIM_USE_MOTOR, 0, 0), + DEF_TIM(TIM2, CH3, PB10, TIM_USE_MOTOR, 0, 0), + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MOTOR, 0, 0), + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MOTOR, 0, 0), DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), }; diff --git a/src/main/target/IFLIGHTF4_TWING/target.c b/src/main/target/IFLIGHTF4_TWING/target.c index a321b88ceed..87227cf3b04 100644 --- a/src/main/target/IFLIGHTF4_TWING/target.c +++ b/src/main/target/IFLIGHTF4_TWING/target.c @@ -33,10 +33,10 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM // Motors - DEF_TIM(TIM1, CH2N, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1_OUT D2_ST6 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S2_OUT D1_ST2 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3_OUT D1_ST6 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4_OUT D1_ST1 + DEF_TIM(TIM1, CH2N, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1_OUT D2_ST6 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S2_OUT D1_ST2 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S3_OUT D1_ST6 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S4_OUT D1_ST1 // LED strip DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), // D1_ST0 diff --git a/src/main/target/IFLIGHTF7_TWING/config.c b/src/main/target/IFLIGHTF7_TWING/config.c new file mode 100644 index 00000000000..912c662d057 --- /dev/null +++ b/src/main/target/IFLIGHTF7_TWING/config.c @@ -0,0 +1,29 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include +#include + +#include + +#include "drivers/pwm_mapping.h" + +void targetConfiguration(void) +{ + // To improve backwards compatibility with INAV versions 6.x and older + timerOverridesMutable(timer2id(TIM4))->outputMode = OUTPUT_MODE_MOTORS; +} diff --git a/src/main/target/IFLIGHTF7_TWING/target.c b/src/main/target/IFLIGHTF7_TWING/target.c index d596849dd54..6876f07b204 100644 --- a/src/main/target/IFLIGHTF7_TWING/target.c +++ b/src/main/target/IFLIGHTF7_TWING/target.c @@ -30,20 +30,20 @@ BUSDEV_REGISTER_SPI_TAG(busdev_imu0, DEVHW_MPU6500, MPU6500_0_SPI_BUS, MPU6 BUSDEV_REGISTER_SPI_TAG(busdev_imu1, DEVHW_MPU6500, MPU6500_1_SPI_BUS, MPU6500_1_CS_PIN, NONE, 1, DEVFLAGS_NONE, IMU_1_ALIGN); timerHardware_t timerHardware[] = { - DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), + DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), + DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), + DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR | TIM_USE_MC_SERVO, 0, 0), - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR | TIM_USE_MC_SERVO, 0, 0), + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // used to be fw motor + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // used to be fw motor - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), - DEF_TIM(TIM2, CH2, PA1, TIM_USE_LED, 0, 0), + DEF_TIM(TIM2, CH2, PA1, TIM_USE_LED, 0, 0), }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/IFLIGHT_BLITZ_F722/target.c b/src/main/target/IFLIGHT_BLITZ_F722/target.c index 8bba266c363..ac2af33d203 100644 --- a/src/main/target/IFLIGHT_BLITZ_F722/target.c +++ b/src/main/target/IFLIGHT_BLITZ_F722/target.c @@ -23,17 +23,16 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 DMA1_S2_CH5 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 DMA1_S7_CH5 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 DMA1_S2_CH5 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 DMA1_S7_CH5 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S3 DMA2_S4_CH7 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 DMA2_S7_CH7 - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 DMA1_S0_CH2 - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 DMA1_S3_CH2 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 1), // S3 DMA2_S4_CH7 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 DMA2_S7_CH7 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 DMA1_S0_CH2 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 DMA1_S3_CH2 - DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S7 DMA1_S1_CH3 - DEF_TIM(TIM2, CH4, PB11, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S8 DMA1_S6_CH3 - + DEF_TIM(TIM2, CH3, PB10, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 DMA1_S1_CH3 + DEF_TIM(TIM2, CH4, PB11, TIM_USE_OUTPUT_AUTO, 0, 1), // S8 DMA1_S6_CH3 DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // WS2812B }; diff --git a/src/main/target/IFLIGHT_BLITZ_F7_AIO/target.c b/src/main/target/IFLIGHT_BLITZ_F7_AIO/target.c index 3c26d72f80a..54ba97a4905 100644 --- a/src/main/target/IFLIGHT_BLITZ_F7_AIO/target.c +++ b/src/main/target/IFLIGHT_BLITZ_F7_AIO/target.c @@ -30,20 +30,20 @@ //BUSDEV_REGISTER_SPI_TAG(busdev_icm42605, DEVHW_ICM42605, ICM42605_SPI_BUS, ICM42605_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_ICM42605_ALIGN); timerHardware_t timerHardware[] = { - DEF_TIM(TIM2, CH2, PB3, TIM_USE_PPM, 0, 0), // PPM - - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 - DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S3 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S4 - DEF_TIM(TIM1, CH1, PE9, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S5 - DEF_TIM(TIM1, CH2, PE11, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6 - DEF_TIM(TIM1, CH3, PE13, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S7 - DEF_TIM(TIM1, CH4, PE14, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S8 - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S9 - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S10 - - DEF_TIM(TIM8, CH4, PC9, TIM_USE_LED, 0, 0), // LED + DEF_TIM(TIM2, CH2, PB3, TIM_USE_PPM, 0, 0), // PPM + + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MOTOR, 0, 0), // S1 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0), // S2 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_MOTOR, 0, 0), // S3 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0), // S4 + DEF_TIM(TIM1, CH1, PE9, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 + DEF_TIM(TIM1, CH2, PE11, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 + DEF_TIM(TIM1, CH3, PE13, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 + DEF_TIM(TIM1, CH4, PE14, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S9 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S10 + + DEF_TIM(TIM8, CH4, PC9, TIM_USE_LED, 0, 0), // LED }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/IFLIGHT_BLITZ_F7_PRO/target.c b/src/main/target/IFLIGHT_BLITZ_F7_PRO/target.c index 592fd20b4c1..b4b6f6de0fb 100644 --- a/src/main/target/IFLIGHT_BLITZ_F7_PRO/target.c +++ b/src/main/target/IFLIGHT_BLITZ_F7_PRO/target.c @@ -32,14 +32,14 @@ BUSDEV_REGISTER_SPI_TAG(busdev_bmi270, DEVHW_BMI270, BMI270_SPI_BUS, BMI270_CS_P timerHardware_t timerHardware[] = { DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S1 - DEF_TIM(TIM3, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 - DEF_TIM(TIM3, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S5 - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S7 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S8 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 1), // S1 + DEF_TIM(TIM3, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 + DEF_TIM(TIM3, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 DEF_TIM(TIM2, CH2, PA1, TIM_USE_LED, 0, 0), // LED }; diff --git a/src/main/target/IFLIGHT_H743_AIO_V2/target.c b/src/main/target/IFLIGHT_H743_AIO_V2/target.c index 3e02bff3e38..88dada8ffe1 100644 --- a/src/main/target/IFLIGHT_H743_AIO_V2/target.c +++ b/src/main/target/IFLIGHT_H743_AIO_V2/target.c @@ -27,12 +27,12 @@ #include "drivers/sensor.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // M1 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // M2 - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 2), // M3 - DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 3), // M4 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0), // M1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 1), // M2 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MOTOR, 0, 2), // M3 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_MOTOR, 0, 3), // M4 - DEF_TIM( TIM1, CH1, PA8, TIM_USE_LED, 0, 4) // LED_2812 + DEF_TIM( TIM1, CH1, PA8, TIM_USE_LED, 0, 4) // LED_2812 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/IFLIGHT_JBF7PRO/target.c b/src/main/target/IFLIGHT_JBF7PRO/target.c index 7c8c15a7412..676c3d6dae3 100644 --- a/src/main/target/IFLIGHT_JBF7PRO/target.c +++ b/src/main/target/IFLIGHT_JBF7PRO/target.c @@ -27,14 +27,14 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S5 - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6 - DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S7 - DEF_TIM(TIM2, CH4, PB11, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 1), // S8 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 + DEF_TIM(TIM2, CH3, PB10, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 + DEF_TIM(TIM2, CH4, PB11, TIM_USE_OUTPUT_AUTO, 0, 1), // S8 DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED }; diff --git a/src/main/target/JHEH7AIO/target.c b/src/main/target/JHEH7AIO/target.c index 479816acdcf..757d66a8587 100644 --- a/src/main/target/JHEH7AIO/target.c +++ b/src/main/target/JHEH7AIO/target.c @@ -27,12 +27,12 @@ #include "drivers/sensor.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // M1 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // M2 - DEF_TIM(TIM1, CH1, PE9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 2), // M3 - DEF_TIM(TIM1, CH2, PE11, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 3), // M4 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0), // M1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 1), // M2 + DEF_TIM(TIM1, CH1, PE9, TIM_USE_MOTOR, 0, 2), // M3 + DEF_TIM(TIM1, CH2, PE11, TIM_USE_MOTOR, 0, 3), // M4 - DEF_TIM( TIM4, CH1, PD12, TIM_USE_LED, 0, 4) // LED_2812 + DEF_TIM( TIM4, CH1, PD12, TIM_USE_LED, 0, 4) // LED_2812 }; -const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); \ No newline at end of file +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/KAKUTEF4/target.c b/src/main/target/KAKUTEF4/target.c index 27bded1f2fa..bf58e5bdca8 100755 --- a/src/main/target/KAKUTEF4/target.c +++ b/src/main/target/KAKUTEF4/target.c @@ -31,23 +31,23 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM, 0, 0), // PPM IN - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1_OUT - DMA1_ST7 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2_OUT - DMA1_ST2 - DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S3_OUT - DMA1_ST6 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1_OUT - DMA1_ST7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S2_OUT - DMA1_ST2 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 1), // S3_OUT - DMA1_ST6 #if !defined(KAKUTEF4V23) - DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S4_OUT - DMA1_ST1 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0), // S4_OUT - DMA1_ST1 #else - DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4_OUT - DMA1_ST1 - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S5_OUT - DMA1_ST0 - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6_OUT - DMA1_ST3 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0), // S4_OUT - DMA1_ST1 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S5_OUT - DMA1_ST0 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S6_OUT - DMA1_ST3 #endif #if defined(KAKUTEF4V2) || defined(KAKUTEF4V23) DEF_TIM(TIM8, CH3, PC8, TIM_USE_LED, 0, 0), // LED_STRIP - DMA2_ST2 #else - DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5_OUT - DMA1_ST2 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S6_OUT - DMA2_ST4 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0), // S5_OUT - DMA1_ST2 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 1), // S6_OUT - DMA2_ST4 DEF_TIM(TIM5, CH2, PA1, TIM_USE_LED, 0, 0), // LED_STRIP - DMA1_ST4 #endif }; diff --git a/src/main/target/KAKUTEF7/target.c b/src/main/target/KAKUTEF7/target.c index 249047470a5..53ae9359deb 100755 --- a/src/main/target/KAKUTEF7/target.c +++ b/src/main/target/KAKUTEF7/target.c @@ -29,14 +29,14 @@ #include "drivers/bus.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM1, CH3, PE13, TIM_USE_PPM, 0, 1), // PPM, DMA2_ST6 + DEF_TIM(TIM1, CH3, PE13, TIM_USE_PPM, 0, 1), // PPM, DMA2_ST6 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // M1 , DMA1_ST7 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // M2 , DMA1_ST2 - DEF_TIM(TIM1, CH1, PE9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 2), // M3 , DMA2_ST2 - DEF_TIM(TIM1, CH2, PE11, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // M4 , DMA2_ST4 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), // M5 , DMA2_ST7 - DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), // M6 , DMA1_ST1 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // M1 , DMA1_ST7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // M2 , DMA1_ST2 + DEF_TIM(TIM1, CH1, PE9, TIM_USE_OUTPUT_AUTO, 0, 2), // M3 , DMA2_ST2 + DEF_TIM(TIM1, CH2, PE11, TIM_USE_OUTPUT_AUTO, 0, 1), // M4 , DMA2_ST4 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // M5 , DMA2_ST7 + DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 0), // M6 , DMA1_ST1 DEF_TIM(TIM4, CH1, PD12, TIM_USE_LED, 0, 0), // LED_STRIP, DMA1_ST0 }; diff --git a/src/main/target/KAKUTEF7MINIV3/target.c b/src/main/target/KAKUTEF7MINIV3/target.c index 3415db1edde..095038960de 100644 --- a/src/main/target/KAKUTEF7MINIV3/target.c +++ b/src/main/target/KAKUTEF7MINIV3/target.c @@ -31,14 +31,14 @@ #include "drivers/pwm_mapping.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S3 - DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S4 - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S5 - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED STRIP DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // RX2 }; -const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); \ No newline at end of file +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/KAKUTEH7/target.c b/src/main/target/KAKUTEH7/target.c index 086d941f1c8..8851f95286f 100644 --- a/src/main/target/KAKUTEH7/target.c +++ b/src/main/target/KAKUTEH7/target.c @@ -28,15 +28,15 @@ timerHardware_t timerHardware[] = { - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S2 - DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 2), // S3 - DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 3), // S4 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 1), // S2 + DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO, 0, 2), // S3 + DEF_TIM(TIM2, CH3, PB10, TIM_USE_OUTPUT_AUTO, 0, 3), // S4 - DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 4), // S5 - DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 5), // S6 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 6), // S7 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 7), // S8 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 4), // S5 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 5), // S6 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 6), // S7 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 7), // S8 DEF_TIM(TIM4, CH1, PD12, TIM_USE_LED, 0, 9), // LED_2812 }; diff --git a/src/main/target/KAKUTEH7WING/target.c b/src/main/target/KAKUTEH7WING/target.c index 2b880fc3b77..30b9a4f5def 100644 --- a/src/main/target/KAKUTEH7WING/target.c +++ b/src/main/target/KAKUTEH7WING/target.c @@ -32,23 +32,23 @@ BUSDEV_REGISTER_SPI_TAG(busdev_icm42605, DEVHW_ICM42605, ICM42605_SPI_BUS // BUSDEV_REGISTER_SPI_TAG(busdev_icm42688, DEVHW_ICM42605, ICM42605_SPI_BUS, ICM42605_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_ICM42605_ALIGN); timerHardware_t timerHardware[] = { - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 - DEF_TIM(TIM1, CH2, PE11, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S2 - DEF_TIM(TIM1, CH3, PE13, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 2), // S3 - DEF_TIM(TIM1, CH4, PE14, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 3), // S4 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM1, CH2, PE11, TIM_USE_OUTPUT_AUTO, 0, 1), // S2 + DEF_TIM(TIM1, CH3, PE13, TIM_USE_OUTPUT_AUTO, 0, 2), // S3 + DEF_TIM(TIM1, CH4, PE14, TIM_USE_OUTPUT_AUTO, 0, 3), // S4 - DEF_TIM(TIM4, CH3, PD14, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 4), // S5 - DEF_TIM(TIM4, CH4, PD15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 DMA_NONE - DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 5), // S7 - DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 6), // S8 + DEF_TIM(TIM4, CH3, PD14, TIM_USE_OUTPUT_AUTO, 0, 4), // S5 + DEF_TIM(TIM4, CH4, PD15, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 DMA_NONE + DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 5), // S7 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 6), // S8 - DEF_TIM(TIM15,CH1, PE5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 7), // S9 - DEF_TIM(TIM15,CH2, PE6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S10 DMA_NONE - DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S11 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S12 + DEF_TIM(TIM15,CH1, PE5, TIM_USE_OUTPUT_AUTO, 0, 7), // S9 + DEF_TIM(TIM15,CH2, PE6, TIM_USE_OUTPUT_AUTO, 0, 0), // S10 DMA_NONE + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // S11 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S12 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S13 - //DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S14 / LED_2812 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S13 + //DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 0, 0), // S14 / LED_2812 DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), // S14 / LED_2812 diff --git a/src/main/target/KROOZX/target.c b/src/main/target/KROOZX/target.c index ba2519cefe8..c77a77aaed5 100755 --- a/src/main/target/KROOZX/target.c +++ b/src/main/target/KROOZX/target.c @@ -25,16 +25,16 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM, 0, 0), // PPM IN - DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR, 0, 0), // PWM4 - DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR, 0, 0), // PWM2 - DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR, 0, 0), // PWM3 - DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR, 0, 0), // PWM1 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR, 0, 0), // PWM5 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR, 0, 0), // PWM6 - DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR, 0, 0), // PWM7 - DEF_TIM(TIM4, CH4, PB9, TIM_USE_MC_MOTOR, 0, 0), // PWM8 - DEF_TIM(TIM4, CH1, PB14, TIM_USE_MC_MOTOR, 0, 0), // PWM9 - DEF_TIM(TIM4, CH2, PB15, TIM_USE_MC_MOTOR, 0, 0), // PWM10 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR, 0, 0), // PWM4 + DEF_TIM(TIM5, CH4, PA3, TIM_USE_MOTOR, 0, 0), // PWM2 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_MOTOR, 0, 0), // PWM3 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_MOTOR, 0, 0), // PWM1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0), // PWM5 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0), // PWM6 + DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, 0, 0), // PWM7 + DEF_TIM(TIM4, CH4, PB9, TIM_USE_MOTOR, 0, 0), // PWM8 + DEF_TIM(TIM4, CH1, PB14, TIM_USE_MOTOR, 0, 0), // PWM9 + DEF_TIM(TIM4, CH2, PB15, TIM_USE_MOTOR, 0, 0), // PWM10 DEF_TIM(TIM8, CH1, PC6, TIM_USE_LED, 0, 0), // LED_STRIP }; diff --git a/src/main/target/MAMBAF405US/target.c b/src/main/target/MAMBAF405US/target.c index 06a09a2e407..1781a047e3f 100644 --- a/src/main/target/MAMBAF405US/target.c +++ b/src/main/target/MAMBAF405US/target.c @@ -28,15 +28,15 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM11, CH1, PB9, TIM_USE_PPM, 0, 0 ), // PPM IN #ifdef MAMBAF405US_I2C - DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1 ), // S1 pin A9: DMA2 Stream 6 Channel 0 - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1 ), // S2 pin A8: DMA2 Stream 6 Channel 0 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S3 pin C9: DMA2 Stream 7 Channel 7 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1 ), // S4 pin C8: DMA2 Stream 2 Channel 0 + DEF_TIM(TIM1, CH2, PA9, TIM_USE_OUTPUT_AUTO, 0, 1 ), // S1 pin A9: DMA2 Stream 6 Channel 0 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 1 ), // S2 pin A8: DMA2 Stream 6 Channel 0 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S3 pin C9: DMA2 Stream 7 Channel 7 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 1 ), // S4 pin C8: DMA2 Stream 2 Channel 0 #else - DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1 ), // S4_OUT – D(2, 2, 6) - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1 ), // S3_OUT – D(2, 1, 6) - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S2_OUT – D(2, 7, 7) - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S1_OUT – D(2, 4, 7) + DEF_TIM(TIM1, CH2, PA9, TIM_USE_OUTPUT_AUTO, 0, 1 ), // S4_OUT – D(2, 2, 6) + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 1 ), // S3_OUT – D(2, 1, 6) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S2_OUT – D(2, 7, 7) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S1_OUT – D(2, 4, 7) #endif DEF_TIM(TIM2, CH2, PB3, TIM_USE_LED, 0, 0 ), // LED_STRIP – D(1, 6, 3) diff --git a/src/main/target/MAMBAF405_2022A/config.c b/src/main/target/MAMBAF405_2022A/config.c index 355605fd90e..ba3a5344c20 100644 --- a/src/main/target/MAMBAF405_2022A/config.c +++ b/src/main/target/MAMBAF405_2022A/config.c @@ -28,6 +28,7 @@ #include "drivers/sensor.h" #include "drivers/pwm_esc_detect.h" #include "drivers/pwm_output.h" +#include "drivers/pwm_mapping.h" #include "drivers/serial.h" #include "fc/rc_controls.h" @@ -56,4 +57,7 @@ void targetConfiguration(void) serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].functionMask = FUNCTION_MSP; serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].msp_baudrateIndex = BAUD_115200; + + // To improve backwards compatibility with INAV versions 6.x and older + timerOverridesMutable(timer2id(TIM3))->outputMode = OUTPUT_MODE_MOTORS; } diff --git a/src/main/target/MAMBAF405_2022A/target.c b/src/main/target/MAMBAF405_2022A/target.c index 0e659296e83..d91361a263a 100644 --- a/src/main/target/MAMBAF405_2022A/target.c +++ b/src/main/target/MAMBAF405_2022A/target.c @@ -20,18 +20,17 @@ #include "platform.h" #include "drivers/io.h" -#include "drivers/timer.h" #include "drivers/pwm_mapping.h" #include "drivers/bus.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1 ), // S1 pin A9: DMA2 Stream 6 Channel 0 - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1 ), // S2 pin A8: DMA2 Stream 6 Channel 0 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S3 pin C9: DMA2 Stream 7 Channel 7 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1 ), // S4 pin C8: DMA2 Stream 2 Channel 0 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 1, 0 ), // S4 pin C8: DMA2 Stream 2 Channel 0 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 1, 0 ), // S4 pin C8: DMA2 Stream 2 Channel 0 + DEF_TIM(TIM1, CH2, PA9, TIM_USE_OUTPUT_AUTO, 0, 1 ), // S1 pin A9: DMA2 Stream 6 Channel 0 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 1 ), // S2 pin A8: DMA2 Stream 6 Channel 0 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S3 pin C9: DMA2 Stream 7 Channel 7 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 1 ), // S4 pin C8: DMA2 Stream 2 Channel 0 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 1, 0 ), // S4 pin C8: DMA2 Stream 2 Channel 0 // used to be fw motor + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 1, 0 ), // S4 pin C8: DMA2 Stream 2 Channel 0 // used to be fw motor DEF_TIM(TIM2, CH2, PB3, TIM_USE_LED, 0, 0 ), // LED_STRIP – D(1, 6, 3) }; diff --git a/src/main/target/MAMBAF722/target.c b/src/main/target/MAMBAF722/target.c index f7db2a30394..b97969e57bf 100644 --- a/src/main/target/MAMBAF722/target.c +++ b/src/main/target/MAMBAF722/target.c @@ -27,10 +27,10 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM11, CH1, PB9, TIM_USE_PPM, 0, 0 ), // PPM IN - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S1_OUT – D(2, 4, 7) - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S2_OUT – D(2, 7, 7) - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1 ), // S3_OUT – D(2, 1, 6) - DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1 ), // S4_OUT – D(2, 2, 6) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S1_OUT – D(2, 4, 7) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S2_OUT – D(2, 7, 7) + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 1 ), // S3_OUT – D(2, 1, 6) + DEF_TIM(TIM1, CH2, PA9, TIM_USE_OUTPUT_AUTO, 0, 1 ), // S4_OUT – D(2, 2, 6) DEF_TIM(TIM2, CH2, PB3, TIM_USE_LED, 0, 0 ), // LED_STRIP – D(1, 6, 3) }; diff --git a/src/main/target/MAMBAF722_2022A/target.c b/src/main/target/MAMBAF722_2022A/target.c index 56f5483cbdd..9b51f139453 100644 --- a/src/main/target/MAMBAF722_2022A/target.c +++ b/src/main/target/MAMBAF722_2022A/target.c @@ -27,15 +27,15 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM11, CH1, PB9, TIM_USE_PPM, 0, 0 ), // PPM IN - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S1 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S2 - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1 ), // S3 - DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1 ), // S4 - - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0 ), // S5 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0 ), // S6 - DEF_TIM(TIM1, CH3, PA10, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0 ), // S7 - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0 ), // S8 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S1 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S2 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 1 ), // S3 + DEF_TIM(TIM1, CH2, PA9, TIM_USE_OUTPUT_AUTO, 0, 1 ), // S4 + + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S5 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S6 + DEF_TIM(TIM1, CH3, PA10, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S7 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S8 DEF_TIM(TIM2, CH2, PB3, TIM_USE_LED, 0, 0 ), // LED_STRIP – D(1, 6, 3) }; diff --git a/src/main/target/MAMBAF722_WING/target.c b/src/main/target/MAMBAF722_WING/target.c index c39247afe5c..7ac29dbdc9e 100644 --- a/src/main/target/MAMBAF722_WING/target.c +++ b/src/main/target/MAMBAF722_WING/target.c @@ -25,15 +25,15 @@ #include "drivers/bus.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S1 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S2 - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1 ), // S3 - DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1 ), // S4 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S1 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S2 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 1 ), // S3 + DEF_TIM(TIM1, CH2, PA9, TIM_USE_OUTPUT_AUTO, 0, 1 ), // S4 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0 ), // S5 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0 ), // S6 - DEF_TIM(TIM1, CH3, PA10, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0 ), // S7 - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0 ), // S8 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S5 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S6 + DEF_TIM(TIM1, CH3, PA10, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S7 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S8 DEF_TIM(TIM2, CH2, PB3, TIM_USE_LED, 0, 0 ), // LED_STRIP – D(1, 6, 3) }; diff --git a/src/main/target/MAMBAF722_X8/target.c b/src/main/target/MAMBAF722_X8/target.c index afe6e363597..9b51f139453 100644 --- a/src/main/target/MAMBAF722_X8/target.c +++ b/src/main/target/MAMBAF722_X8/target.c @@ -27,15 +27,15 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM11, CH1, PB9, TIM_USE_PPM, 0, 0 ), // PPM IN - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S1 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S2 - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1 ), // S3 - DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1 ), // S4 - - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S5 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S6 - DEF_TIM(TIM1, CH3, PA10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S7 - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S8 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S1 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S2 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 1 ), // S3 + DEF_TIM(TIM1, CH2, PA9, TIM_USE_OUTPUT_AUTO, 0, 1 ), // S4 + + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S5 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S6 + DEF_TIM(TIM1, CH3, PA10, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S7 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S8 DEF_TIM(TIM2, CH2, PB3, TIM_USE_LED, 0, 0 ), // LED_STRIP – D(1, 6, 3) }; diff --git a/src/main/target/MAMBAH743/config.c b/src/main/target/MAMBAH743/config.c index cc04e10319e..7bc7d7b6fe2 100644 --- a/src/main/target/MAMBAH743/config.c +++ b/src/main/target/MAMBAH743/config.c @@ -46,6 +46,7 @@ #include "telemetry/telemetry.h" #include "io/piniobox.h" +#include "drivers/pwm_mapping.h" void targetConfiguration(void) { @@ -65,4 +66,7 @@ void targetConfiguration(void) */ serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].functionMask = FUNCTION_MSP; serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].msp_baudrateIndex = BAUD_115200; + + // To improve backwards compatibility with INAV versions 6.x and older + timerOverridesMutable(timer2id(TIM8))->outputMode = OUTPUT_MODE_MOTORS; } diff --git a/src/main/target/MAMBAH743/target.c b/src/main/target/MAMBAH743/target.c index f684670a22d..46231fdbd79 100644 --- a/src/main/target/MAMBAH743/target.c +++ b/src/main/target/MAMBAH743/target.c @@ -35,15 +35,15 @@ BUSDEV_REGISTER_SPI_TAG(busdev_icm42605, DEVHW_ICM42605, ICM42605_SPI_BUS, timerHardware_t timerHardware[] = { - DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S1 - DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S2 - DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 2), // S3 - DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 3), // S4 - - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 4), // S5 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 5), // S6 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 6), // S7 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 7), // S8 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 1), // S2 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 2), // S3 + DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 3), // S4 + + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 4), // S5 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 5), // S6 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 6), // S7 // used to be fw motor + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 7), // S8 // used to be fw motor DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 9), // LED_2812 }; diff --git a/src/main/target/MATEKF405/CMakeLists.txt b/src/main/target/MATEKF405/CMakeLists.txt index c4f839c1c1d..ebf457cda25 100644 --- a/src/main/target/MATEKF405/CMakeLists.txt +++ b/src/main/target/MATEKF405/CMakeLists.txt @@ -1,3 +1,2 @@ target_stm32f405xg(MATEKF405) -target_stm32f405xg(MATEKF405_SERVOS6) target_stm32f405xg(MATEKF405OSD) diff --git a/src/main/target/MATEKF405/target.c b/src/main/target/MATEKF405/target.c index 40ebf626e5f..254707887b2 100644 --- a/src/main/target/MATEKF405/target.c +++ b/src/main/target/MATEKF405/target.c @@ -24,23 +24,15 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM -#ifdef MATEKF405_SERVOS6 - DEF_TIM(TIM3, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 -#else - DEF_TIM(TIM3, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S1 -#endif - DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S2 UP(2,1) - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S3 UP(2,1) - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 UP(2,1) - - DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_LED , 0, 0), // S5 UP(1,7) - -#ifdef MATEKF405_SERVOS6 - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6 UP(2,5) -#else - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 UP(2,5) -#endif - DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S7 D(1,7)!S5 UP(2,6) + DEF_TIM(TIM3, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 1), // S2 UP(2,1) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 1), // S3 UP(2,1) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 UP(2,1) + + DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO | TIM_USE_LED , 0, 0), // S5 UP(1,7) + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 UP(2,5) + DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 D(1,7)!S5 UP(2,6) DEF_TIM(TIM5, CH3, PA2, TIM_USE_PWM, 0, 0), // TX2 diff --git a/src/main/target/MATEKF405CAN/config.c b/src/main/target/MATEKF405CAN/config.c index b400704b001..78b1e8c985e 100644 --- a/src/main/target/MATEKF405CAN/config.c +++ b/src/main/target/MATEKF405CAN/config.c @@ -22,6 +22,7 @@ #include "io/serial.h" #include "sensors/compass.h" #include "fc/config.h" +#include "drivers/pwm_mapping.h" void targetConfiguration(void) @@ -30,4 +31,7 @@ void targetConfiguration(void) serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].functionMask = FUNCTION_GPS; // serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].gps_baudrateIndex = BAUD_115200; beeperConfigMutable()->pwmMode = true; + + // To improve backwards compatibility with INAV versions 6.x and older + timerOverridesMutable(timer2id(TIM4))->outputMode = OUTPUT_MODE_MOTORS; } diff --git a/src/main/target/MATEKF405CAN/target.c b/src/main/target/MATEKF405CAN/target.c index f4c9826ac87..2ab40de1dc4 100644 --- a/src/main/target/MATEKF405CAN/target.c +++ b/src/main/target/MATEKF405CAN/target.c @@ -22,15 +22,15 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 1), // S1 D(2,2,7) - DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 1), // S2 D(2,3,7) - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 1), // S3 D(2,4,7) - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S4 D(2,7,7) - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S5 D(1,7,5) - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S6 D(1,2,5) - - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1, 0), // S7 D(1,0,2) - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1, 0), // S8 D(1,3,2) + DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 1, 1), // S1 D(2,2,7) + DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 1, 1), // S2 D(2,3,7) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 1, 1), // S3 D(2,4,7) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 1, 0), // S4 D(2,7,7) + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 1, 0), // S5 D(1,7,5) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 1, 0), // S6 D(1,2,5) + + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 1, 0), // S7 D(1,0,2) // used to be fw motor + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 1, 0), // S8 D(1,3,2) // used to be fw motor DEF_TIM(TIM1, CH1, PA8, TIM_USE_BEEPER, 0, 0), // BEEPER PWM diff --git a/src/main/target/MATEKF405SE/config.c b/src/main/target/MATEKF405SE/config.c index f82e5fa1090..8cd618e3cf0 100644 --- a/src/main/target/MATEKF405SE/config.c +++ b/src/main/target/MATEKF405SE/config.c @@ -21,7 +21,7 @@ #include "config/config_master.h" #include "config/feature.h" -#include "flight/mixer.h" +#include "flight/mixer_profile.h" #include "io/serial.h" diff --git a/src/main/target/MATEKF405SE/target.c b/src/main/target/MATEKF405SE/target.c index c81fe5a7f02..ef6fceaca94 100644 --- a/src/main/target/MATEKF405SE/target.c +++ b/src/main/target/MATEKF405SE/target.c @@ -22,16 +22,16 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1, 0), // S1 D(1,3,2) - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1, 0), // S2 D(1,0,2) + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 1, 0), // S1 D(1,3,2) + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 1, 0), // S2 D(1,0,2) - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S3 D(1,7,5) - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S4 D(1,2,5) - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S5 D(2,4,7) - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S6 D(2,7,7) - DEF_TIM(TIM12, CH1, PB14, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 1, 0), // S7 - DEF_TIM(TIM12, CH2, PB15, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 1, 0), // S8 - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 1, 0), // S9 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 1, 0), // S3 D(1,7,5) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 1, 0), // S4 D(1,2,5) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 1, 0), // S5 D(2,4,7) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 1, 0), // S6 D(2,7,7) + DEF_TIM(TIM12, CH1, PB14, TIM_USE_OUTPUT_AUTO, 1, 0), // S7 + DEF_TIM(TIM12, CH2, PB15, TIM_USE_OUTPUT_AUTO, 1, 0), // S8 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 1, 0), // S9 DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), //2812LED D(1,5,3) diff --git a/src/main/target/MATEKF405TE/target.c b/src/main/target/MATEKF405TE/target.c index 70561e001f3..79f6269a528 100644 --- a/src/main/target/MATEKF405TE/target.c +++ b/src/main/target/MATEKF405TE/target.c @@ -25,19 +25,19 @@ #include "drivers/timer_def_stm32f4xx.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 D(2,7,7) UP217 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 D(2,2,0) UP217 - DEF_TIM(TIM1, CH3N, PB15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 D(2,6,0) UP256 - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S4 D(2,1,6) UP256 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 D(2,7,7) UP217 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 D(2,2,0) UP217 + DEF_TIM(TIM1, CH3N, PB15, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 D(2,6,0) UP256 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 1), // S4 D(2,1,6) UP256 - DEF_TIM(TIM2, CH4, PB11, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 D(1,7,3) UP173 - DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 D(1,1,3) UP173 - DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S7 D(1,6,3) UP173 - DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S8 D(1,5,3) UP173 + DEF_TIM(TIM2, CH4, PB11, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 D(1,7,3) UP173 + DEF_TIM(TIM2, CH3, PB10, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 D(1,1,3) UP173 + DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 D(1,6,3) UP173 + DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 D(1,5,3) UP173 - DEF_TIM(TIM12, CH1, PB14, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S9 DMA NONE - DEF_TIM(TIM13, CH1, PA6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S10 DMA NONE - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S11 D(1,0,2) + DEF_TIM(TIM12, CH1, PB14, TIM_USE_OUTPUT_AUTO, 0, 0), // S9 DMA NONE + DEF_TIM(TIM13, CH1, PA6, TIM_USE_OUTPUT_AUTO, 0, 0), // S10 DMA NONE + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S11 D(1,0,2) DEF_TIM(TIM3, CH4, PB1, TIM_USE_LED, 0, 0), // 2812LED D(1,2,5) DEF_TIM(TIM11, CH1, PB9, TIM_USE_BEEPER, 0, 0), // BEEPER PWM diff --git a/src/main/target/MATEKF405TE/target.h b/src/main/target/MATEKF405TE/target.h index 93ab3345def..acaed9daf23 100644 --- a/src/main/target/MATEKF405TE/target.h +++ b/src/main/target/MATEKF405TE/target.h @@ -178,5 +178,6 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define USE_DSHOT +#define USE_DSHOT_DMAR #define USE_ESC_SENSOR diff --git a/src/main/target/MATEKF411/target.c b/src/main/target/MATEKF411/target.c index c192e8c69dd..fb2ccde3f14 100755 --- a/src/main/target/MATEKF411/target.c +++ b/src/main/target/MATEKF411/target.c @@ -25,14 +25,14 @@ timerHardware_t timerHardware[] = { // DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM IN - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 D(1,4,5) - DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 D(1,5,5) - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 D(1,0,2) - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 D(1,3,2) - - DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S5 D(1,6,3) - DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6 D(1,1,3) - DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S7 D(1,5,3) - clash with S2 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 D(1,4,5) + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 D(1,5,5) + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 D(1,0,2) + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 D(1,3,2) + + DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 D(1,6,3) + DEF_TIM(TIM2, CH3, PB10, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 D(1,1,3) + DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 D(1,5,3) - clash with S2 DEF_TIM(TIM1, CH1, PA8, TIM_USE_ANY, 0, 0), //softserial_tx2 - 2812LED TIM_USE_LED D(2,1,6) DEF_TIM(TIM5, CH1, PA0, TIM_USE_PPM, 0, 0), //use rssi pad for PPM/softserial_tx1 diff --git a/src/main/target/MATEKF411SE/target.c b/src/main/target/MATEKF411SE/target.c index 679df396079..caf6972b8f8 100755 --- a/src/main/target/MATEKF411SE/target.c +++ b/src/main/target/MATEKF411SE/target.c @@ -24,13 +24,13 @@ #include "drivers/pinio.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 D(1,4,5) - DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 D(1,5,5) - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S3 D(2,1,6) - DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S4 D(2,2,6) - DEF_TIM(TIM1, CH3, PA10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S5 D(2,6,6) + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 D(1,4,5) + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 D(1,5,5) + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 1), // S3 D(2,1,6) + DEF_TIM(TIM1, CH2, PA9, TIM_USE_OUTPUT_AUTO, 0, 1), // S4 D(2,2,6) + DEF_TIM(TIM1, CH3, PA10, TIM_USE_OUTPUT_AUTO, 0, 1), // S5 D(2,6,6) #ifndef MATEKF411SE_SS2_CH6 - DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6 D(1,7,2) + DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 D(1,7,2) DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), //TX2 pad - softserial_tx2 #else diff --git a/src/main/target/MATEKF411TE/target.c b/src/main/target/MATEKF411TE/target.c index 9bfad340f18..b873201652e 100644 --- a/src/main/target/MATEKF411TE/target.c +++ b/src/main/target/MATEKF411TE/target.c @@ -25,19 +25,19 @@ timerHardware_t timerHardware[] = { - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 D(1,0,2) - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 D(1,3,6) - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 D(1,7,5) - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 D(1,2,5) - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 D(1,4,5) - DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 D(1,5,5) + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 D(1,0,2) + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 D(1,3,6) + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 D(1,7,5) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 D(1,2,5) + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 D(1,4,5) + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 D(1,5,5) DEF_TIM(TIM5, CH1, PA0, TIM_USE_ANY, 0, 0), //Softserial1_TX DEF_TIM(TIM5, CH2, PA1, TIM_USE_ANY, 0, 0), //Softserial1_RX DEF_TIM(TIM2, CH1, PA15, TIM_USE_ANY, 0, 0), //Softserial2_TX DEF_TIM(TIM2, CH2, PB3, TIM_USE_ANY, 0, 0), //Softserial2_RX - DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED | TIM_USE_MC_SERVO, 0, 0), //2812LED D(2,1,6) + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED | TIM_USE_SERVO, 0, 0), //2812LED D(2,1,6) }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/MATEKF722/CMakeLists.txt b/src/main/target/MATEKF722/CMakeLists.txt index eb384e12815..600a4401a20 100644 --- a/src/main/target/MATEKF722/CMakeLists.txt +++ b/src/main/target/MATEKF722/CMakeLists.txt @@ -1,2 +1 @@ target_stm32f722xe(MATEKF722) -target_stm32f722xe(MATEKF722_HEXSERVO) diff --git a/src/main/target/MATEKF722/target.c b/src/main/target/MATEKF722/target.c old mode 100755 new mode 100644 index dc06c7f508d..d2575f42f2d --- a/src/main/target/MATEKF722/target.c +++ b/src/main/target/MATEKF722/target.c @@ -25,24 +25,20 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), // PPM + DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), // PPM - DEF_TIM(TIM3, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 D(1, 4, 5) - DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S2 D(2, 3, 7) - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 D(2, 4, 7) - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 D(2, 7, 7) + DEF_TIM(TIM3, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 D(1, 4, 5) + DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 D(2, 3, 7) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 D(2, 4, 7) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 D(2, 7, 7) - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S5 DMA1_ST2 -#ifdef MATEKF722_HEXSERVO - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 DMA2_ST6 -#else - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6 DMA2_ST6 -#endif - DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S7 DMA1_ST7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 DMA1_ST2 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 DMA2_ST6 + DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 DMA1_ST7 // DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), // TX2/S8 DMA1_ST0 - DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), // LED STRIP D(1, 5, 3) + DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), // LED STRIP D(1, 5, 3) }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/MATEKF722/target.h b/src/main/target/MATEKF722/target.h old mode 100755 new mode 100644 diff --git a/src/main/target/MATEKF722PX/target.c b/src/main/target/MATEKF722PX/target.c index 1db058f8fd2..170b67d9514 100755 --- a/src/main/target/MATEKF722PX/target.c +++ b/src/main/target/MATEKF722PX/target.c @@ -24,19 +24,19 @@ timerHardware_t timerHardware[] = { - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 UP2-1 D(2, 4, 7) - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 UP2-1 D(2, 7, 7) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 UP2-1 D(2, 4, 7) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 UP2-1 D(2, 7, 7) - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 UP1-2 D(1, 4, 5) - DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 UP1-2 D(1, 5, 5) - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 UP1-2 D(1, 7, 5) - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 UP1-2 D(1, 2, 5) + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 UP1-2 D(1, 4, 5) + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 UP1-2 D(1, 5, 5) + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 UP1-2 D(1, 7, 5) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 UP1-2 D(1, 2, 5) - DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S7 UP1-7 D(1, 1, 3) - DEF_TIM(TIM2, CH4, PB11, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S8 UP1-7 D(1, 6, 3) + DEF_TIM(TIM2, CH3, PB10, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 UP1-7 D(1, 1, 3) + DEF_TIM(TIM2, CH4, PB11, TIM_USE_OUTPUT_AUTO, 0, 1), // S8 UP1-7 D(1, 6, 3) - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S9 UP1-6 D(1, 0, 2) - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S10 UP1-6 D(1, 3, 2) + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S9 UP1-6 D(1, 0, 2) + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S10 UP1-6 D(1, 3, 2) DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 2), // LED D(2, 3, 6) diff --git a/src/main/target/MATEKF722SE/CMakeLists.txt b/src/main/target/MATEKF722SE/CMakeLists.txt index 75aa91ec0a1..a541da229f2 100644 --- a/src/main/target/MATEKF722SE/CMakeLists.txt +++ b/src/main/target/MATEKF722SE/CMakeLists.txt @@ -1,3 +1,2 @@ target_stm32f722xe(MATEKF722SE) target_stm32f722xe(MATEKF722MINI) -target_stm32f722xe(MATEKF722SE_8MOTOR) diff --git a/src/main/target/MATEKF722SE/config.c b/src/main/target/MATEKF722SE/config.c index 7e47e5bf6b7..94de7c47b82 100644 --- a/src/main/target/MATEKF722SE/config.c +++ b/src/main/target/MATEKF722SE/config.c @@ -19,6 +19,8 @@ #include "platform.h" +#include "drivers/pwm_mapping.h" + #include "fc/fc_msp_box.h" #include "io/piniobox.h" @@ -27,4 +29,7 @@ void targetConfiguration(void) { pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; + + // To improve backwards compatibility with INAV versions 6.x and older + timerOverridesMutable(timer2id(TIM4))->outputMode = OUTPUT_MODE_MOTORS; } diff --git a/src/main/target/MATEKF722SE/target.c b/src/main/target/MATEKF722SE/target.c index 1ea322fb829..6e469dfff3a 100644 --- a/src/main/target/MATEKF722SE/target.c +++ b/src/main/target/MATEKF722SE/target.c @@ -30,27 +30,22 @@ BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500, DEVHW_MPU6500, MPU6500_SPI_BUS, MPU6500_CS_PIN, NONE, 1, DEVFLAGS_NONE, IMU_MPU6500_ALIGN); timerHardware_t timerHardware[] = { - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S1 UP1-2 D(1, 4, 5) - DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S2 UP1-2 D(1, 5, 5) - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 UP1-2 D(1, 7, 5) - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 UP1-2 D(1, 2, 5) + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 UP1-2 D(1, 4, 5) + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 UP1-2 D(1, 5, 5) + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 UP1-2 D(1, 7, 5) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 UP1-2 D(1, 2, 5) - DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 UP1-7 D(1, 5, 3) - clash with S2 - DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 UP1-7 D(1, 6, 3) + DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 UP1-7 D(1, 5, 3) - clash with S2 + DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 UP1-7 D(1, 6, 3) -#if (defined(MATEKF722SE_8MOTOR)) - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S7 UP1-6 D(1, 0, 2) - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S8 UP1-6 D(1, 3, 2) -#else - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S7 UP1-6 D(1, 0, 2) - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S8 UP1-6 D(1, 3, 2) -#endif + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 UP1-6 D(1, 0, 2) // used to be fw motor + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 UP1-6 D(1, 3, 2) // used to me fw motor - DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 2), // LED D(2, 6, 0) + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 2), // LED D(2, 6, 0) - DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM, RX2 + DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM, RX2 - DEF_TIM(TIM5, CH3, PA2, TIM_USE_PWM, 0, 0), // TX2 & softserial1 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_PWM, 0, 0), // TX2 & softserial1 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/MATEKF765/target.c b/src/main/target/MATEKF765/target.c index 0a4e42ac9c3..712beb4c8c9 100644 --- a/src/main/target/MATEKF765/target.c +++ b/src/main/target/MATEKF765/target.c @@ -34,21 +34,21 @@ BUSDEV_REGISTER_SPI_TAG(busdev_imu3, DEVHW_ICM42605, ICM42605_SPI_BUS, #endif timerHardware_t timerHardware[] = { - DEF_TIM(TIM2, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 UP(1,7), D(1,5,3) - DEF_TIM(TIM2, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 UP(1,7), D(1,6,3) + DEF_TIM(TIM2, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 UP(1,7), D(1,5,3) + DEF_TIM(TIM2, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 UP(1,7), D(1,6,3) - DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 UP(1,0), D(1,0,6)* - DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 UP(1,0), D(1,1,6) - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 UP(1,2), D(1,7,5)** - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 UP(1,2), D(1,2,5) + DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 UP(1,0), D(1,0,6)* + DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 UP(1,0), D(1,1,6) + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 UP(1,2), D(1,7,5)** + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 UP(1,2), D(1,2,5) - DEF_TIM(TIM4, CH1, PD12, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S7 UP(1,6), D(1,0,2)* - DEF_TIM(TIM4, CH2, PD13, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S8 UP(1,6), D(1,3,2) - DEF_TIM(TIM4, CH3, PD14, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S9 UP(1,6), D(1,7,2)** - DEF_TIM(TIM4, CH4, PD15, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S10 UP(1,6) + DEF_TIM(TIM4, CH1, PD12, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 UP(1,6), D(1,0,2)* + DEF_TIM(TIM4, CH2, PD13, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 UP(1,6), D(1,3,2) + DEF_TIM(TIM4, CH3, PD14, TIM_USE_OUTPUT_AUTO, 0, 0), // S9 UP(1,6), D(1,7,2)** + DEF_TIM(TIM4, CH4, PD15, TIM_USE_OUTPUT_AUTO, 0, 0), // S10 UP(1,6) - DEF_TIM(TIM9, CH1, PE5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S11 - DEF_TIM(TIM9, CH2, PE6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S12 + DEF_TIM(TIM9, CH1, PE5, TIM_USE_OUTPUT_AUTO, 0, 0), // S11 + DEF_TIM(TIM9, CH2, PE6, TIM_USE_OUTPUT_AUTO, 0, 0), // S12 DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED_2812 D(2,6,0) diff --git a/src/main/target/MATEKH743/CMakeLists.txt b/src/main/target/MATEKH743/CMakeLists.txt index 96a65ca5a40..dcc5019a9d1 100644 --- a/src/main/target/MATEKH743/CMakeLists.txt +++ b/src/main/target/MATEKH743/CMakeLists.txt @@ -1 +1,2 @@ target_stm32h743xi(MATEKH743) +target_stm32h743xi(MATEKH743HD) diff --git a/src/main/target/MATEKH743/target.c b/src/main/target/MATEKH743/target.c index 706d5787865..1923e6623a1 100644 --- a/src/main/target/MATEKH743/target.c +++ b/src/main/target/MATEKH743/target.c @@ -32,21 +32,21 @@ BUSDEV_REGISTER_SPI_TAG(busdev_icm20602, DEVHW_MPU6500, MPU6500_SPI_BUS, MPU6 BUSDEV_REGISTER_SPI_TAG(busdev_icm42605, DEVHW_ICM42605, ICM42605_SPI_BUS, ICM42605_CS_PIN, NONE, 2, DEVFLAGS_NONE, IMU_ICM42605_ALIGN); timerHardware_t timerHardware[] = { - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S2 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 1), // S2 - DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 2), // S3 - DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 3), // S4 - DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 4), // S5 - DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 5), // S6 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 2), // S3 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 3), // S4 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 4), // S5 + DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 5), // S6 - DEF_TIM(TIM4, CH1, PD12, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 6), // S7 - DEF_TIM(TIM4, CH2, PD13, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 7), // S8 - DEF_TIM(TIM4, CH3, PD14, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S9 - DEF_TIM(TIM4, CH4, PD15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S10 DMA_NONE + DEF_TIM(TIM4, CH1, PD12, TIM_USE_OUTPUT_AUTO, 0, 6), // S7 + DEF_TIM(TIM4, CH2, PD13, TIM_USE_OUTPUT_AUTO, 0, 7), // S8 + DEF_TIM(TIM4, CH3, PD14, TIM_USE_OUTPUT_AUTO, 0, 0), // S9 + DEF_TIM(TIM4, CH4, PD15, TIM_USE_OUTPUT_AUTO, 0, 0), // S10 DMA_NONE - DEF_TIM(TIM15, CH1, PE5, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S11 - DEF_TIM(TIM15, CH2, PE6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S12 DMA_NONE + DEF_TIM(TIM15, CH1, PE5, TIM_USE_OUTPUT_AUTO, 0, 0), // S11 + DEF_TIM(TIM15, CH2, PE6, TIM_USE_OUTPUT_AUTO, 0, 0), // S12 DMA_NONE DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 9), // LED_2812 DEF_TIM(TIM2, CH1, PA15, TIM_USE_BEEPER, 0, 0), // BEEPER PWM diff --git a/src/main/target/MATEKH743/target.h b/src/main/target/MATEKH743/target.h index 37742a88fcc..25e0217bb0f 100644 --- a/src/main/target/MATEKH743/target.h +++ b/src/main/target/MATEKH743/target.h @@ -19,7 +19,12 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "H743" -#define USBD_PRODUCT_STRING "MATEKH743" + +#if defined(MATEKH743HD) + #define USBD_PRODUCT_STRING "MATEKH743HD" +#else + #define USBD_PRODUCT_STRING "MATEKH743" +#endif #define USE_TARGET_CONFIG @@ -69,14 +74,16 @@ #define ICM42605_CS_PIN PC13 // *************** SPI2 OSD *********************** -#define USE_SPI_DEVICE_2 -#define SPI2_SCK_PIN PB13 -#define SPI2_MISO_PIN PB14 -#define SPI2_MOSI_PIN PB15 + #define USE_SPI_DEVICE_2 + #define SPI2_SCK_PIN PB13 + #define SPI2_MISO_PIN PB14 + #define SPI2_MOSI_PIN PB15 -#define USE_MAX7456 -#define MAX7456_SPI_BUS BUS_SPI2 -#define MAX7456_CS_PIN PB12 +#if defined(MATEKH743) + #define USE_MAX7456 + #define MAX7456_SPI_BUS BUS_SPI2 + #define MAX7456_CS_PIN PB12 +#endif // *************** SPI3 SPARE for external RM3100 *********** #define USE_SPI_DEVICE_3 @@ -159,7 +166,7 @@ #define SERIAL_PORT_COUNT 9 #define DEFAULT_RX_TYPE RX_TYPE_SERIAL -#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_PROVIDER SERIALRX_CRSF #define SERIALRX_UART SERIAL_PORT_USART6 // *************** SDIO SD BLACKBOX******************* diff --git a/src/main/target/NEUTRONRCF435MINI/target.c b/src/main/target/NEUTRONRCF435MINI/target.c index 995bf8f3d1a..472867af578 100644 --- a/src/main/target/NEUTRONRCF435MINI/target.c +++ b/src/main/target/NEUTRONRCF435MINI/target.c @@ -28,12 +28,12 @@ timerHardware_t timerHardware[] = { DEF_TIM(TMR1, CH1, PA8, TIM_USE_ANY |TIM_USE_LED, 0,7), // PWM1 - LED MCO1 DMA1 CH2 - DEF_TIM(TMR4, CH1, PB6, TIM_USE_MC_MOTOR|TIM_USE_FW_MOTOR, 0,0), // motor1 DMA2 CH7 - DEF_TIM(TMR4, CH2, PB7, TIM_USE_MC_MOTOR|TIM_USE_FW_MOTOR, 0,2), // motor2 DMA2 CH6 - DEF_TIM(TMR2, CH4, PA3, TIM_USE_MC_MOTOR|TIM_USE_FW_SERVO, 0,1), // motor3 DMA2 CH5 - DEF_TIM(TMR3, CH4, PB1, TIM_USE_MC_MOTOR|TIM_USE_FW_SERVO, 0,3), // motor4 DMA2 CH4 + DEF_TIM(TMR4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0,0), // motor1 DMA2 CH7 + DEF_TIM(TMR4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0,2), // motor2 DMA2 CH6 + DEF_TIM(TMR2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0,1), // motor3 DMA2 CH5 + DEF_TIM(TMR3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0,3), // motor4 DMA2 CH4 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); - \ No newline at end of file + diff --git a/src/main/target/NEUTRONRCF435SE/target.c b/src/main/target/NEUTRONRCF435SE/target.c index 7eb72ead923..3a8848911fa 100644 --- a/src/main/target/NEUTRONRCF435SE/target.c +++ b/src/main/target/NEUTRONRCF435SE/target.c @@ -26,21 +26,21 @@ timerHardware_t timerHardware[] = { - DEF_TIM(TMR5, CH1, PA0, TIM_USE_ANY, 0, 13), // TIM_USE_CAMERA_CONTROL - DEF_TIM(TMR5, CH2, PA1, TIM_USE_ANY |TIM_USE_LED, 0,7), // PWM1 - LED MCO1 DMA1 CH2 - DEF_TIM(TMR2, CH4, PA3, TIM_USE_ANY |TIM_USE_PPM, 0,6), // PWM2 - PPM DMA1 CH6 - - DEF_TIM(TMR8, CH4, PC9, TIM_USE_MC_MOTOR|TIM_USE_FW_MOTOR, 0,0), // motor1 DMA2 CH7 - DEF_TIM(TMR8, CH3, PC8, TIM_USE_MC_MOTOR|TIM_USE_FW_MOTOR, 0,2), // motor2 DMA2 CH6 - DEF_TIM(TMR3, CH2, PC7, TIM_USE_MC_MOTOR|TIM_USE_FW_SERVO, 0,1), // motor3 DMA2 CH5 - DEF_TIM(TMR3, CH1, PC6, TIM_USE_MC_MOTOR|TIM_USE_FW_SERVO, 0,3), // motor4 DMA2 CH4 - - DEF_TIM(TMR4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0,11), // PWM1 - OUT5 DMA1 CH7 - DEF_TIM(TMR4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0,10), // PWM2 - OUT6 DMA2 CH1 - DEF_TIM(TMR3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0,9), // PWM3 - OUT7 DMA2 CH2 - DEF_TIM(TMR3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0,8), // PWM4 - OUT8 DMA2 CH3 - + DEF_TIM(TMR5, CH1, PA0, TIM_USE_ANY, 0, 13), // TIM_USE_CAMERA_CONTROL + DEF_TIM(TMR5, CH2, PA1, TIM_USE_ANY |TIM_USE_LED, 0,7), // PWM1 - LED MCO1 DMA1 CH2 + DEF_TIM(TMR2, CH4, PA3, TIM_USE_ANY |TIM_USE_PPM, 0,6), // PWM2 - PPM DMA1 CH6 + + DEF_TIM(TMR8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0,0), // motor1 DMA2 CH7 + DEF_TIM(TMR8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0,2), // motor2 DMA2 CH6 + DEF_TIM(TMR3, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0,1), // motor3 DMA2 CH5 + DEF_TIM(TMR3, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0,3), // motor4 DMA2 CH4 + + DEF_TIM(TMR4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0,11), // PWM1 - OUT5 DMA1 CH7 + DEF_TIM(TMR4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0,10), // PWM2 - OUT6 DMA2 CH1 + DEF_TIM(TMR3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0,9), // PWM3 - OUT7 DMA2 CH2 + DEF_TIM(TMR3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0,8), // PWM4 - OUT8 DMA2 CH3 + }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); - \ No newline at end of file + diff --git a/src/main/target/NEUTRONRCF435WING/target.c b/src/main/target/NEUTRONRCF435WING/target.c index b64e1a5c8fd..b1cd9031cc3 100644 --- a/src/main/target/NEUTRONRCF435WING/target.c +++ b/src/main/target/NEUTRONRCF435WING/target.c @@ -28,17 +28,17 @@ timerHardware_t timerHardware[] = { DEF_TIM(TMR1, CH1, PA8, TIM_USE_ANY |TIM_USE_LED, 0,7), // PWM1 - LED MCO1 DMA1 CH2 - DEF_TIM(TMR4, CH1, PB6, TIM_USE_MC_MOTOR|TIM_USE_FW_MOTOR, 0,0), // motor1 DMA2 CH7 - DEF_TIM(TMR1, CH3, PA10, TIM_USE_MC_MOTOR|TIM_USE_FW_MOTOR, 0,2), // motor2 DMA2 CH6 - DEF_TIM(TMR2, CH4, PA3, TIM_USE_MC_MOTOR|TIM_USE_FW_SERVO, 0,1), // motor3 DMA2 CH5 - DEF_TIM(TMR3, CH4, PB1, TIM_USE_MC_MOTOR|TIM_USE_FW_SERVO, 0,3), // motor4 DMA2 CH4 + DEF_TIM(TMR4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0,0), // motor1 DMA2 CH7 + DEF_TIM(TMR1, CH3, PA10, TIM_USE_OUTPUT_AUTO, 0,2), // motor2 DMA2 CH6 + DEF_TIM(TMR2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0,1), // motor3 DMA2 CH5 + DEF_TIM(TMR3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0,3), // motor4 DMA2 CH4 - DEF_TIM(TMR4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0,10), // PWM1 - OUT5 DMA2 CH1 - DEF_TIM(TMR1, CH2, PA9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0,9), // PWM2 - OUT6 DMA2 CH2 - DEF_TIM(TMR3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0,8), // PWM3 - OUT7 DMA2 CH3 - DEF_TIM(TMR2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0,11), // PWM4 - OUT8 DMA1 CH7 + DEF_TIM(TMR4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0,10), // PWM1 - OUT5 DMA2 CH1 + DEF_TIM(TMR1, CH2, PA9, TIM_USE_OUTPUT_AUTO, 0,9), // PWM2 - OUT6 DMA2 CH2 + DEF_TIM(TMR3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0,8), // PWM3 - OUT7 DMA2 CH3 + DEF_TIM(TMR2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0,11), // PWM4 - OUT8 DMA1 CH7 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); - \ No newline at end of file + diff --git a/src/main/target/NEUTRONRCH7BT/target.c b/src/main/target/NEUTRONRCH7BT/target.c index b65dc906344..d15f4c4a9bc 100644 --- a/src/main/target/NEUTRONRCH7BT/target.c +++ b/src/main/target/NEUTRONRCH7BT/target.c @@ -27,15 +27,15 @@ #include "drivers/sensor.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S2 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 1), // S2 - DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 2), // S3 - DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 3), // S4 - DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 4), // S5 - DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 5), // S6 - DEF_TIM(TIM4, CH1, PD12, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 6), // S7 - DEF_TIM(TIM4, CH2, PD13, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 7), // S8 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 2), // S3 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 3), // S4 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 4), // S5 + DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 5), // S6 + DEF_TIM(TIM4, CH1, PD12, TIM_USE_OUTPUT_AUTO, 0, 6), // S7 + DEF_TIM(TIM4, CH2, PD13, TIM_USE_OUTPUT_AUTO, 0, 7), // S8 DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 9), // LED_2812 }; diff --git a/src/main/target/NOX/target.c b/src/main/target/NOX/target.c index 0508a0155b2..e2651956e1c 100755 --- a/src/main/target/NOX/target.c +++ b/src/main/target/NOX/target.c @@ -27,10 +27,10 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM2, CH3, PB10, TIM_USE_PPM, 0, 0), //PPM DEF_TIM(TIM2, CH1, PA0, TIM_USE_LED, 0, 0), //2812LED - DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1_OUT - DEF_TIM(TIM1, CH1N, PA7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2_OUT - DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3_OUT - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4_OUT + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0), // S1_OUT + DEF_TIM(TIM1, CH1N, PA7, TIM_USE_OUTPUT_AUTO, 0, 0), // S2_OUT + DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0), // S3_OUT + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S4_OUT DEF_TIM(TIM9, CH1, PA2, TIM_USE_ANY, 0, 0), //UART2 TX DEF_TIM(TIM9, CH2, PA3, TIM_USE_ANY, 0, 0), //UART2 RX diff --git a/src/main/target/OMNIBUSF4/target.c b/src/main/target/OMNIBUSF4/target.c index 9c7e4b76a83..81b30e11f25 100644 --- a/src/main/target/OMNIBUSF4/target.c +++ b/src/main/target/OMNIBUSF4/target.c @@ -36,27 +36,28 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM8, CH3, PC8, TIM_USE_ANY, 0, 0), // S5_IN // pad labelled CH5 on OMNIBUSF4PRO DEF_TIM(TIM8, CH4, PC9, TIM_USE_ANY, 0, 0), // S6_IN // pad labelled CH6 on OMNIBUSF4PRO - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1_OUT D1_ST7 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2_OUT D1_ST2 - DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S3_OUT D1_ST6 - DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4_OUT D1_ST1 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1_OUT D1_ST7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S2_OUT D1_ST2 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 1), // S3_OUT D1_ST6 + DEF_TIM(TIM2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0), // S4_OUT D1_ST1 - // { TIM9, IO_TAG(PA3), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // MOTOR_3 + // { TIM9, IO_TAG(PA3), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM9, TIM_USE_OUTPUT_AUTO }, // MOTOR_3 #if (defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)) && !(defined(OMNIBUSF4PRO_LEDSTRIPM5) || defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS)) - DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S5_OUT - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6_OUT + + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0), // S5_OUT + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // S6_OUT #elif defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS) DEF_TIM(TIM5, CH2, PA1, TIM_USE_ANY, 0, 0), // S5_OUT SOFTSERIAL DEF_TIM(TIM1, CH1, PA8, TIM_USE_ANY, 0, 0), // S6_OUT SOFTSERIAL #elif defined(OMNIBUSF4V3_S6_SS) - DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S5_OUT + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0), // S5_OUT DEF_TIM(TIM1, CH1, PA8, TIM_USE_ANY, 0, 0), // S6_OUT SOFTSERIAL #elif defined(OMNIBUSF4PRO_LEDSTRIPM5) DEF_TIM(TIM5, CH2, PA1, TIM_USE_LED, 0, 0), // S5_OUT LED strip - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6_OUT + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // S6_OUT #else - DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO | TIM_USE_LED, 0, 0), // S5_OUT MOTOR, SERVO or LED - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6_OUT + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO | TIM_USE_LED, 0, 0), // S5_OUT MOTOR, SERVO or LED + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // S6_OUT #endif #if (defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)) && !defined(OMNIBUSF4PRO_LEDSTRIPM5) diff --git a/src/main/target/OMNIBUSF7/target.c b/src/main/target/OMNIBUSF7/target.c index c11e1629bf9..75497fed8bd 100644 --- a/src/main/target/OMNIBUSF7/target.c +++ b/src/main/target/OMNIBUSF7/target.c @@ -39,10 +39,10 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM2, CH4, PA3, TIM_USE_PPM, 0, 0 ), // UART2_RX, joined with PE13 // DEF_TIM(TIM1, CH3, PE13, TIM_USE_NONE, 0, 0 ), // RC1 / PPM, unusable - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // M1 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // M2 - DEF_TIM(TIM1, CH1, PE9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 2 ), // M3 - DEF_TIM(TIM1, CH2, PE11, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1 ), // M4 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0 ), // M1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0 ), // M2 + DEF_TIM(TIM1, CH1, PE9, TIM_USE_OUTPUT_AUTO, 0, 2 ), // M3 + DEF_TIM(TIM1, CH2, PE11, TIM_USE_OUTPUT_AUTO, 0, 1 ), // M4 DEF_TIM(TIM4, CH1, PD12, TIM_USE_LED, 0, 0 ), // LED }; diff --git a/src/main/target/OMNIBUSF7NXT/config.c b/src/main/target/OMNIBUSF7NXT/config.c index e5aff9dbf40..2fd2607b717 100644 --- a/src/main/target/OMNIBUSF7NXT/config.c +++ b/src/main/target/OMNIBUSF7NXT/config.c @@ -26,6 +26,7 @@ #include #include "drivers/io.h" +#include "drivers/pwm_mapping.h" #include "rx/rx.h" #include "io/serial.h" #include "io/ledstrip.h" @@ -34,4 +35,7 @@ void targetConfiguration(void) { DEFINE_LED(ledStripConfigMutable()->ledConfigs, 0, 0, COLOR_GREEN, 0, LED_FUNCTION_ARM_STATE, LED_FLAG_OVERLAY(LED_OVERLAY_WARNING), 0); // serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_SMARTPORT; + + // To improve backwards compatibility with INAV versions 6.x and older + timerOverridesMutable(timer2id(TIM8))->outputMode = OUTPUT_MODE_MOTORS; } diff --git a/src/main/target/OMNIBUSF7NXT/target.c b/src/main/target/OMNIBUSF7NXT/target.c index 6f92459630c..b1eb7332dca 100644 --- a/src/main/target/OMNIBUSF7NXT/target.c +++ b/src/main/target/OMNIBUSF7NXT/target.c @@ -39,14 +39,14 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM4, CH2, PB7, TIM_USE_PPM, 0, 0), // PPM / UART1_RX // OUTPUT 1-4 - DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // D(1, 5, 5) - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // D(1, 4, 5) - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // D(1, 7, 5) - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // D(1, 2, 5) + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 1, 0), // D(1, 5, 5) + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 1, 0), // D(1, 4, 5) + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 1, 0), // D(1, 7, 5) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 1, 0), // D(1, 2, 5) // OUTPUT 5-6 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1, 0), // D(2, 7, 7) - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1, 1), // D(2, 2, 0) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 1, 0), // D(2, 7, 7) // used to be fw motor + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 1, 1), // D(2, 2, 0) // used to be fw motor // AUXILARY pins DEF_TIM(TIM1, CH2, PA9, TIM_USE_LED, 1, 0), // LED diff --git a/src/main/target/PIXRACER/config.c b/src/main/target/PIXRACER/config.c index d538e04b50b..130ce8538f6 100755 --- a/src/main/target/PIXRACER/config.c +++ b/src/main/target/PIXRACER/config.c @@ -21,12 +21,13 @@ #include "config/feature.h" #include "fc/config.h" -#include "flight/mixer.h" -#include "io/serial.h" -#include "rx/rx.h" #include "sensors/compass.h" +#include "drivers/pwm_mapping.h" void targetConfiguration(void) { compassConfigMutable()->mag_align = CW90_DEG; + + // To improve backwards compatibility with INAV versions 6.x and older + timerOverridesMutable(timer2id(TIM4))->outputMode = OUTPUT_MODE_MOTORS; } diff --git a/src/main/target/PIXRACER/target.c b/src/main/target/PIXRACER/target.c index 294c962f248..73c9d6937a0 100755 --- a/src/main/target/PIXRACER/target.c +++ b/src/main/target/PIXRACER/target.c @@ -37,15 +37,15 @@ BUSDEV_REGISTER_I2C_TAG(busdev_mag3110, DEVHW_MAG3110, MAG_I2C_BUS, BUSDEV_REGISTER_SPI_TAG(busdev_hmc5983_spi, DEVHW_HMC5883, MPU9250_SPI_BUS, PE15, NONE, 1, DEVFLAGS_NONE, 0); timerHardware_t timerHardware[] = { - DEF_TIM(TIM3, CH3, PB0, TIM_USE_PPM, 0, 0 ), // PPM shared uart6 pc7 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_PPM, 0, 0), // PPM shared uart6 pc7 - DEF_TIM(TIM1, CH4, PE14, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S1_OUT - DEF_TIM(TIM1, CH3, PE13, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S2_OUT - DEF_TIM(TIM1, CH2, PE11, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S3_OUT - DEF_TIM(TIM1, CH1, PE9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S4_OUT + DEF_TIM(TIM1, CH4, PE14, TIM_USE_OUTPUT_AUTO, 0, 0), // S1_OUT + DEF_TIM(TIM1, CH3, PE13, TIM_USE_OUTPUT_AUTO, 0, 0), // S2_OUT + DEF_TIM(TIM1, CH2, PE11, TIM_USE_OUTPUT_AUTO, 0, 0), // S3_OUT + DEF_TIM(TIM1, CH1, PE9, TIM_USE_OUTPUT_AUTO, 0, 0), // S4_OUT - DEF_TIM(TIM4, CH2, PD13, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S5_OUT - DEF_TIM(TIM4, CH3, PD14, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S6_OUT + DEF_TIM(TIM4, CH2, PD13, TIM_USE_OUTPUT_AUTO, 0, 0), // S5_OUT + DEF_TIM(TIM4, CH3, PD14, TIM_USE_OUTPUT_AUTO, 0, 0), // S6_OUT }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/RADIX/target.c b/src/main/target/RADIX/target.c index dd05a6b3ce5..db78409cdaf 100644 --- a/src/main/target/RADIX/target.c +++ b/src/main/target/RADIX/target.c @@ -26,12 +26,12 @@ /* TIMERS */ timerHardware_t timerHardware[] = { DEF_TIM(TIM12, CH1, PB14, TIM_USE_PPM, 0, 0), // PPM In - DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR, 0, 0), // S1 - DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR, 0, 0), // S2 - DEF_TIM(TIM1, CH3, PA10, TIM_USE_MC_MOTOR, 0, 0), // S3 - DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR, 0, 0), // S4 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR, 0, 0), // S5 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR, 0, 0), // S6 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + DEF_TIM(TIM1, CH3, PA10, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 + DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 DEF_TIM(TIM11, CH1, PB9, TIM_USE_ANY, 0, 0), // Camera Control }; diff --git a/src/main/target/REVO/target.c b/src/main/target/REVO/target.c index 7cd184b9f62..d1203adadc5 100644 --- a/src/main/target/REVO/target.c +++ b/src/main/target/REVO/target.c @@ -34,19 +34,19 @@ BUSDEV_REGISTER_I2C_TAG(busdev_mag3110, DEVHW_MAG3110, MAG_I2C_BUS_EXT, /* TIMERS */ timerHardware_t timerHardware[] = { - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1_OUT D1_ST7 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2_OUT D1_ST2 - DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S3_OUT D1_ST6 - DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4_OUT D1_ST1 - DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO | TIM_USE_ANY, 0, 0), // S5_OUT / LED - DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6_OUT D1_ST2 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1_OUT D1_ST7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S2_OUT D1_ST2 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 1), // S3_OUT D1_ST6 + DEF_TIM(TIM2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0), // S4_OUT D1_ST1 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO | TIM_USE_ANY, 0, 0), // S5_OUT / LED + DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0), // S6_OUT D1_ST2 - DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, 0, 0), // PPM (5th pin on FlexiIO port) - DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0), // S2_IN - DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM | TIM_USE_FW_SERVO, 0, 0), // S3_IN - DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM | TIM_USE_FW_SERVO, 0, 0), // S4_IN - DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM | TIM_USE_FW_SERVO, 0, 0), // S5_IN - DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM | TIM_USE_FW_SERVO, 0, 0), // S6_IN + DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, 0, 0), // PPM (5th pin on FlexiIO port) + DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0), // S2_IN + DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM | TIM_USE_OUTPUT_AUTO, 0, 0), // S3_IN + DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM | TIM_USE_OUTPUT_AUTO, 0, 0), // S4_IN + DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM | TIM_USE_OUTPUT_AUTO, 0, 0), // S5_IN + DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM | TIM_USE_OUTPUT_AUTO, 0, 0), // S6_IN }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/RUSH_BLADE_F7/target.c b/src/main/target/RUSH_BLADE_F7/target.c index 565c8a23465..c50807f465d 100644 --- a/src/main/target/RUSH_BLADE_F7/target.c +++ b/src/main/target/RUSH_BLADE_F7/target.c @@ -23,14 +23,14 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 - DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S5 - DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S6 - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), // S7 - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), // S8 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 + DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), // LED STRIP #ifdef RUSH_BLADE_F7 diff --git a/src/main/target/SAGEATF4/target.c b/src/main/target/SAGEATF4/target.c index 3d8ed6feb5e..48875414672 100644 --- a/src/main/target/SAGEATF4/target.c +++ b/src/main/target/SAGEATF4/target.c @@ -27,20 +27,20 @@ timerHardware_t timerHardware[] = { DEF_TIM(TMR5, CH4, PA3, TIM_USE_PPM, 0, 4), // PPM UART2_RX_PIN DMA1 CH5 - DEF_TIM(TMR2, CH3, PB10, TIM_USE_ANY |TIM_USE_LED, 0,5), // PWM1 - LED MCO1 DMA1 CH6 - DEF_TIM(TMR2, CH4, PB11, TIM_USE_ANY |TIM_USE_BEEPER, 0,6), // PWM2 - BB DMA1 CH7 + DEF_TIM(TMR2, CH3, PB10, TIM_USE_ANY | TIM_USE_LED, 0,5), // PWM1 - LED MCO1 DMA1 CH6 + DEF_TIM(TMR2, CH4, PB11, TIM_USE_ANY | TIM_USE_BEEPER, 0,6), // PWM2 - BB DMA1 CH7 - DEF_TIM(TMR4, CH1, PB6, TIM_USE_MC_MOTOR|TIM_USE_FW_MOTOR, 0,0), // motor1 DMA1 CH1 - DEF_TIM(TMR4, CH2, PB7, TIM_USE_MC_MOTOR|TIM_USE_FW_MOTOR, 0,1), // motor2 DMA1 CH2 - DEF_TIM(TMR2, CH1, PB8, TIM_USE_MC_MOTOR|TIM_USE_FW_SERVO, 0,2), // motor3 DMA1 CH3 - DEF_TIM(TMR2, CH2, PB9, TIM_USE_MC_MOTOR|TIM_USE_FW_SERVO, 0,3), // motor4 DMA1 CH4 + DEF_TIM(TMR4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0,0), // motor1 DMA1 CH1 + DEF_TIM(TMR4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0,1), // motor2 DMA1 CH2 + DEF_TIM(TMR2, CH1, PB8, TIM_USE_OUTPUT_AUTO, 0,2), // motor3 DMA1 CH3 + DEF_TIM(TMR2, CH2, PB9, TIM_USE_OUTPUT_AUTO, 0,3), // motor4 DMA1 CH4 - DEF_TIM(TMR3, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0,8), // PWM1 - OUT5 DMA2 CH2 DMA2_CHANNEL1->ADC - DEF_TIM(TMR3, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0,9), // PWM2 - OUT6 DMA2 CH3 - DEF_TIM(TMR3, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0,10), // PWM3 - OUT7 DMA2 CH4 - DEF_TIM(TMR3, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0,11), // PWM4 - OUT8 DMA2 CH5 + DEF_TIM(TMR3, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0,8), // PWM1 - OUT5 DMA2 CH2 DMA2_CHANNEL1->ADC + DEF_TIM(TMR3, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0,9), // PWM2 - OUT6 DMA2 CH3 + DEF_TIM(TMR3, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0,10), // PWM3 - OUT7 DMA2 CH4 + DEF_TIM(TMR3, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0,11), // PWM4 - OUT8 DMA2 CH5 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); - \ No newline at end of file + diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c index 4bf53a791a5..0a686f60619 100644 --- a/src/main/target/SITL/target.c +++ b/src/main/target/SITL/target.c @@ -51,6 +51,7 @@ #include "drivers/timer.h" #include "drivers/serial.h" #include "config/config_streamer.h" +#include "build/version.h" #include "target/SITL/sim/realFlight.h" #include "target/SITL/sim/xplane.h" @@ -73,9 +74,12 @@ static int simPort = 0; static char **c_argv; -void systemInit(void) { +static void printVersion(void) { + fprintf(stderr, "INAV %d.%d.%d SITL (%s)\n", FC_VERSION_MAJOR, FC_VERSION_MINOR, FC_VERSION_PATCH_LEVEL, shortGitRevision); +} - fprintf(stderr, "INAV %d.%d.%d SITL\n", FC_VERSION_MAJOR, FC_VERSION_MINOR, FC_VERSION_PATCH_LEVEL); +void systemInit(void) { + printVersion(); clock_gettime(CLOCK_MONOTONIC, &start_time); fprintf(stderr, "[SYSTEM] Init...\n"); @@ -168,6 +172,7 @@ bool parseMapping(char* mapStr) void printCmdLineOptions(void) { + printVersion(); fprintf(stderr, "Avaiable options:\n"); fprintf(stderr, "--path=[path] Path and filename of eeprom.bin. If not specified 'eeprom.bin' in program directory is used.\n"); fprintf(stderr, "--sim=[rf|xp] Simulator interface: rf = RealFligt, xp = XPlane. Example: --sim=rf\n"); @@ -197,6 +202,7 @@ void parseArguments(int argc, char *argv[]) {"simport", required_argument, 0, 'p'}, {"help", no_argument, 0, 'h'}, {"path", required_argument, 0, 'e'}, + {"version", no_argument, 0, 'v'}, {NULL, 0, NULL, 0} }; @@ -236,10 +242,12 @@ void parseArguments(int argc, char *argv[]) fprintf(stderr, "[EEPROM] Invalid path, using eeprom file in program directory\n."); } break; - case 'h': + case 'v': + printVersion(); + exit(0); + default: printCmdLineOptions(); exit(0); - break; } } diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index 78f5022a9eb..3fbe45a5cab 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -69,6 +69,8 @@ #define USE_GPS_FAKE #define USE_RANGEFINDER_FAKE #define USE_RX_SIM +#undef MAX_MIXER_PROFILE_COUNT +#define MAX_MIXER_PROFILE_COUNT 2 #define USE_MSP_OSD #define USE_OSD diff --git a/src/main/target/SKYSTARSF405HD/config.c b/src/main/target/SKYSTARSF405HD/config.c index a327f7a4604..b34785c5b7d 100644 --- a/src/main/target/SKYSTARSF405HD/config.c +++ b/src/main/target/SKYSTARSF405HD/config.c @@ -22,8 +22,12 @@ #include "io/serial.h" #include "rx/rx.h" +#include "drivers/pwm_mapping.h" void targetConfiguration(void) { serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART6)].functionMask = FUNCTION_DJI_HD_OSD; + + // To improve backwards compatibility with INAV versions 6.x and older + timerOverridesMutable(timer2id(TIM2))->outputMode = OUTPUT_MODE_MOTORS; } diff --git a/src/main/target/SKYSTARSF405HD/target.c b/src/main/target/SKYSTARSF405HD/target.c index e6bec48b4c3..40ab18015af 100644 --- a/src/main/target/SKYSTARSF405HD/target.c +++ b/src/main/target/SKYSTARSF405HD/target.c @@ -24,10 +24,10 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM1, CH1, PA8, TIM_USE_PPM, 0, 0), - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), + DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO, 0, 0), DEF_TIM(TIM8, CH3, PC8, TIM_USE_LED, 0, 0), }; diff --git a/src/main/target/SKYSTARSF405HD/target.h b/src/main/target/SKYSTARSF405HD/target.h index ac88a720847..b8a696e14d4 100644 --- a/src/main/target/SKYSTARSF405HD/target.h +++ b/src/main/target/SKYSTARSF405HD/target.h @@ -45,6 +45,11 @@ #define BMI270_SPI_BUS BUS_SPI1 #define BMI270_CS_PIN PA4 +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW180_DEG_FLIP +#define ICM42605_SPI_BUS BUS_SPI1 +#define ICM42605_CS_PIN PA4 + // *************** M25P256 flash ******************** #define USE_FLASHFS #define USE_FLASH_M25P16 diff --git a/src/main/target/SKYSTARSF722HD/target.c b/src/main/target/SKYSTARSF722HD/target.c index cf5906d9151..72ac8974552 100644 --- a/src/main/target/SKYSTARSF722HD/target.c +++ b/src/main/target/SKYSTARSF722HD/target.c @@ -24,10 +24,10 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM3, CH1, PB4, TIM_USE_PPM, 0, 0), - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // TIM3_CH3 / TIM8_CH3 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // TIM3_CH4 / TIM8_CH4 - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // TIM4_CH1 - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // TIM4_CH2 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // TIM3_CH3 / TIM8_CH3 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // TIM3_CH4 / TIM8_CH4 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // TIM4_CH1 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // TIM4_CH2 // used to be fw motor, but would have a timer conflict with the line above DEF_TIM(TIM2, CH2, PB3, TIM_USE_LED, 0, 0), }; diff --git a/src/main/target/SKYSTARSH743HD/target.c b/src/main/target/SKYSTARSH743HD/target.c index 07a2b0ef701..296fc51bd35 100644 --- a/src/main/target/SKYSTARSH743HD/target.c +++ b/src/main/target/SKYSTARSH743HD/target.c @@ -30,16 +30,16 @@ BUSDEV_REGISTER_SPI_TAG(busdev_bmi270_1, DEVHW_BMI270, IMU1_SPI_BUS, IMU1_CS BUSDEV_REGISTER_SPI_TAG(busdev_bmi270_2, DEVHW_BMI270, IMU2_SPI_BUS, IMU2_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU2_ALIGN); timerHardware_t timerHardware[] = { - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S2 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 1), // S2 - DEF_TIM(TIM4, CH1, PD12, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 2), // S3 - DEF_TIM(TIM4, CH2, PD13, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 3), // S4 + DEF_TIM(TIM4, CH1, PD12, TIM_USE_OUTPUT_AUTO, 0, 2), // S3 + DEF_TIM(TIM4, CH2, PD13, TIM_USE_OUTPUT_AUTO, 0, 3), // S4 - DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 4), // S5 - DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 5), // S6 - DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 6), // S7 - DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 7), // S8 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 4), // S5 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 5), // S6 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 6), // S7 + DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 7), // S8 DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 9), // LED_2812 }; diff --git a/src/main/target/SPARKY2/target.c b/src/main/target/SPARKY2/target.c index c5ae5109e9d..b4b4b295047 100644 --- a/src/main/target/SPARKY2/target.c +++ b/src/main/target/SPARKY2/target.c @@ -30,12 +30,12 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0, 0 ), // S4_IN DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM, 0, 0 ), // S5_IN - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S1_OUT - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S2_OUT - DEF_TIM(TIM9, CH2, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S3_OUT - DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S4_OUT - DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0 ), // S5_OUT - GPIO_PartialRemap_TIM3 - DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0 ), // S6_OUT + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S1_OUT + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S2_OUT + DEF_TIM(TIM9, CH2, PA3, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S3_OUT + DEF_TIM(TIM2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S4_OUT + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S5_OUT - GPIO_PartialRemap_TIM3 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S6_OUT }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/SPEEDYBEEF4/config.c b/src/main/target/SPEEDYBEEF4/config.c index 6bf5f8877fa..73ec5ed2898 100644 --- a/src/main/target/SPEEDYBEEF4/config.c +++ b/src/main/target/SPEEDYBEEF4/config.c @@ -28,6 +28,7 @@ #include "drivers/sensor.h" #include "drivers/pwm_esc_detect.h" #include "drivers/pwm_output.h" +#include "drivers/pwm_mapping.h" #include "drivers/serial.h" #include "fc/rc_controls.h" @@ -52,4 +53,8 @@ void targetConfiguration(void) { serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART5)].functionMask = FUNCTION_MSP; serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART5)].msp_baudrateIndex = BLUETOOTH_MSP_BAUDRATE; + + // To improve backwards compatibility with INAV versions 6.x and older + timerOverridesMutable(timer2id(TIM1))->outputMode = OUTPUT_MODE_MOTORS; + timerOverridesMutable(timer2id(TIM2))->outputMode = OUTPUT_MODE_MOTORS; } diff --git a/src/main/target/SPEEDYBEEF4/target.c b/src/main/target/SPEEDYBEEF4/target.c index be21bfbb26e..b45c517bfaa 100644 --- a/src/main/target/SPEEDYBEEF4/target.c +++ b/src/main/target/SPEEDYBEEF4/target.c @@ -26,14 +26,14 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), // PPM - DEF_TIM(TIM3, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S1 UP(1,2) - DEF_TIM(TIM3, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S2 UP(2,1) - DEF_TIM(TIM3, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 UP(2,1) - DEF_TIM(TIM3, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 UP(2,1) + DEF_TIM(TIM3, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 UP(1,2) + DEF_TIM(TIM3, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 UP(2,1) + DEF_TIM(TIM3, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 UP(2,1) + DEF_TIM(TIM3, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 UP(2,1) - DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S5 UP(1,7) - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S6 UP(2,5) - DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S7 D(1,7)!S5 UP(2,6) + DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 UP(1,7) // used to be fw motor + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 UP(2,5) // used to be fw motor + DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 D(1,7)!S5 UP(2,6) DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), // LED D(1,0) UP(2,6) }; diff --git a/src/main/target/SPEEDYBEEF405MINI/CMakeLists.txt b/src/main/target/SPEEDYBEEF405MINI/CMakeLists.txt new file mode 100644 index 00000000000..e18c0cd5ada --- /dev/null +++ b/src/main/target/SPEEDYBEEF405MINI/CMakeLists.txt @@ -0,0 +1,2 @@ +target_stm32f405xg(SPEEDYBEEF405MINI) +target_stm32f405xg(SPEEDYBEEF405MINI_6OUTPUTS) diff --git a/src/main/target/SPEEDYBEEF405MINI/config.c b/src/main/target/SPEEDYBEEF405MINI/config.c new file mode 100644 index 00000000000..46aa7f00dcb --- /dev/null +++ b/src/main/target/SPEEDYBEEF405MINI/config.c @@ -0,0 +1,37 @@ +/* + * 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 "platform.h" + +#include "fc/fc_msp_box.h" + +#include "io/piniobox.h" +#include "io/serial.h" + +void targetConfiguration(void) +{ + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART2)].functionMask = FUNCTION_RX_SERIAL; + + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART5)].functionMask = FUNCTION_ESCSERIAL; + + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].functionMask = FUNCTION_MSP; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].msp_baudrateIndex = BAUD_115200; + + pinioBoxConfigMutable()->permanentId[0] = BOXARM; +} diff --git a/src/main/target/SPEEDYBEEF405MINI/target.c b/src/main/target/SPEEDYBEEF405MINI/target.c new file mode 100644 index 00000000000..3cc7fbbebba --- /dev/null +++ b/src/main/target/SPEEDYBEEF405MINI/target.c @@ -0,0 +1,46 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/pinio.h" +#include "drivers/sensor.h" + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 1, 0), // S1 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 1, 0), // S2 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 1, 0), // S3 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 1, 0), // S4 + +#ifdef SPEEDYBEEF405MINI_6OUTPUTS + DEF_TIM(TIM12, CH1, PB14, TIM_USE_OUTPUT_AUTO, 1, 0), // CAM_CTRL + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // LED +#else + DEF_TIM(TIM12, CH1, PB14, TIM_USE_ANY, 1, 0), // CAM_CTRL + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED +#endif + + DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), //TX2 softserial1_Tx +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/SPEEDYBEEF405MINI/target.h b/src/main/target/SPEEDYBEEF405MINI/target.h new file mode 100644 index 00000000000..dd5b9f4cfb3 --- /dev/null +++ b/src/main/target/SPEEDYBEEF405MINI/target.h @@ -0,0 +1,186 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "SF4M" +#define USBD_PRODUCT_STRING "SPEEDYBEEF405MINI" + +#define LED0 PC13 + +#define BEEPER PC15 +#define BEEPER_INVERTED + +/* + * SPI Buses + */ +#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_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PC2 +#define SPI2_MOSI_PIN PC3 + +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + +/* + * I2C + */ +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +/* + * Serial + */ +#define USE_VCP + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define USE_UART3 +#define UART3_TX_PIN PC10 +#define UART3_RX_PIN PC11 + +#define USE_UART4 //Internally routed to BLE +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#define USE_UART5 +#define UART5_TX_PIN PC12 +#define UART5_RX_PIN PD2 + +#define USE_UART6 +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define USE_SOFTSERIAL1 +#define SOFTSERIAL_1_TX_PIN PA2 +#define SOFTSERIAL_1_RX_PIN PA2 + +#define SERIAL_PORT_COUNT 8 + +/* + * Gyro + */ +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW270_DEG +#define ICM42605_CS_PIN PA4 +#define ICM42605_SPI_BUS BUS_SPI1 + +/* + * Other + */ +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_DPS310 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_AK8975 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_IST8308 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL + +#define TEMPERATURE_I2C_BUS BUS_I2C1 +#define PITOT_I2C_BUS BUS_I2C1 +#define RANGEFINDER_I2C_BUS BUS_I2C1 + +/* + * OSD + */ +#define USE_MAX7456 +#define MAX7456_CS_PIN PB12 +#define MAX7456_SPI_BUS BUS_SPI2 + +/* + * Blackbox + */ +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_SPI_BUS BUS_SPI3 +#define M25P16_CS_PIN PC14 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART2 + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 + +#define ADC_CHANNEL_1_PIN PC0 +#define ADC_CHANNEL_2_PIN PC1 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 + +// *************** PINIO *************************** +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PB11 // RF Switch +#define PINIO1_FLAGS PINIO_FLAGS_INVERTED + +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PA8 + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) +#define CURRENT_METER_SCALE 250 +#define CURRENT_METER_OFFSET 0 + +#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 + +#ifdef SPEEDYBEEF405MINI_6OUTPUTS + +#define MAX_PWM_OUTPUT_PORTS 6 + +#else + +#define MAX_PWM_OUTPUT_PORTS 4 + +#endif + +#define USE_DSHOT +#define USE_SERIALSHOT +#define USE_ESC_SENSOR \ No newline at end of file diff --git a/src/main/target/SPEEDYBEEF405V3/target.c b/src/main/target/SPEEDYBEEF405V3/target.c index fffac265202..05c84f1c807 100644 --- a/src/main/target/SPEEDYBEEF405V3/target.c +++ b/src/main/target/SPEEDYBEEF405V3/target.c @@ -35,15 +35,15 @@ BUSDEV_REGISTER_SPI_TAG(busdev_bmi270, DEVHW_BMI270, BMI270_SPI_BUS, BMI270_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_BMI270_ALIGN); timerHardware_t timerHardware[] = { - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 - DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S3 - DEF_TIM(TIM4, CH4, PB9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S4 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 + DEF_TIM(TIM4, CH4, PB9, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 - DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S7 - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S8 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 DEF_TIM(TIM8, CH4, PC9, TIM_USE_LED, 0, 0), // LED }; diff --git a/src/main/target/SPEEDYBEEF405V4/CMakeLists.txt b/src/main/target/SPEEDYBEEF405V4/CMakeLists.txt new file mode 100644 index 00000000000..6e208a8373d --- /dev/null +++ b/src/main/target/SPEEDYBEEF405V4/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f405xg(SPEEDYBEEF405V4) diff --git a/src/main/target/SPEEDYBEEF405V4/config.c b/src/main/target/SPEEDYBEEF405V4/config.c new file mode 100644 index 00000000000..a21244ed8bc --- /dev/null +++ b/src/main/target/SPEEDYBEEF405V4/config.c @@ -0,0 +1,39 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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 file 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 program. If not, see http://www.gnu.org/licenses/. + */ + +#include +#include "platform.h" +#include "fc/fc_msp_box.h" +#include "io/piniobox.h" +#include "io/serial.h" + +void targetConfiguration(void) +{ + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].functionMask = FUNCTION_MSP; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].msp_baudrateIndex = BAUD_115200; + + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART5)].functionMask = FUNCTION_ESCSERIAL; + + pinioBoxConfigMutable()->permanentId[0] = BOXARM; +} diff --git a/src/main/target/SPEEDYBEEF405V4/target.c b/src/main/target/SPEEDYBEEF405V4/target.c new file mode 100644 index 00000000000..2d398bd1bbe --- /dev/null +++ b/src/main/target/SPEEDYBEEF405V4/target.c @@ -0,0 +1,53 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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 file 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 program. If not, see http://www.gnu.org/licenses/. + */ +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/pinio.h" +#include "drivers/sensor.h" + +BUSDEV_REGISTER_SPI_TAG(busdev_icm42688, DEVHW_ICM42605, ICM42605_SPI_BUS, ICM42605_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_ICM42605_ALIGN); + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 1, 0), // S1 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 1, 0), // S2 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 1, 0), // S3 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 1, 0), // S4 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 1, 0), // S5 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 1, 0), // S6 + DEF_TIM(TIM2, CH3, PB10, TIM_USE_OUTPUT_AUTO, 1, 0), // S7 + DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 1, 0), // S8 + + DEF_TIM(TIM12, CH2, PB15, TIM_USE_OUTPUT_AUTO, 1, 0), // S9 + DEF_TIM(TIM12, CH1, PB14, TIM_USE_OUTPUT_AUTO, 1, 0), // S10 + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), //2812LED D(1,5,3) +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/SPEEDYBEEF405V4/target.h b/src/main/target/SPEEDYBEEF405V4/target.h new file mode 100644 index 00000000000..8f333db3c28 --- /dev/null +++ b/src/main/target/SPEEDYBEEF405V4/target.h @@ -0,0 +1,175 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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 file 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 program. If not, see http://www.gnu.org/licenses/. + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "SB44" +#define USBD_PRODUCT_STRING "SpeedyBeeF405V4" + +/*** Indicators ***/ +#define LED0 PC13 //Blue + +#define BEEPER PC15 +#define BEEPER_INVERTED + +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PB11 +#define PINIO1_FLAGS PINIO_FLAGS_INVERTED + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define USE_UART3 +#define UART3_TX_PIN PC10 +#define UART3_RX_PIN PC11 + +// Internally routed to Bluetooth +#define USE_UART4 +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#define USE_UART5 +#define UART5_TX_PIN PC12 // Not broken out +#define UART5_RX_PIN PD2 //ESC TLM + +#define USE_UART6 +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define USE_SOFTSERIAL1 +#define SOFTSERIAL_1_TX_PIN PA2 +#define SOFTSERIAL_1_RX_PIN PA2 + +#define SERIAL_PORT_COUNT 8 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART2 + +// *************** Gyro & ACC ********************** +#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS + +#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_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW90_DEG +#define ICM42605_CS_PIN PA4 +#define ICM42605_SPI_BUS BUS_SPI1 + +// *************** I2C(Baro & I2C) ************************** +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_BMP085 +#define USE_BARO_DPS310 +#define USE_BARO_SPL06 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_AK8963 +#define USE_MAG_AK8975 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL +#define USE_MAG_VCM5883 + +#define USE_RANGEFINDER +#define TEMPERATURE_I2C_BUS BUS_I2C1 +#define PITOT_I2C_BUS BUS_I2C1 +#define RANGEFINDER_I2C_BUS BUS_I2C1 + +// *************** Internal SD card ************************** +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + +#define USE_SDCARD +#define USE_SDCARD_SPI +#define SDCARD_SPI_BUS BUS_SPI3 +#define SDCARD_CS_PIN PC14 + +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + +// *************** OSD ***************************** +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PC2 +#define SPI2_MOSI_PIN PC3 + +#define USE_OSD +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN PB12 + +// *************** ADC ***************************** + +#define USE_ADC +#define ADC_CHANNEL_1_PIN PC0 +#define ADC_CHANNEL_2_PIN PC1 +#define ADC_CHANNEL_3_PIN PC5 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 + +// *************** LED ***************************** +#define USE_LED_STRIP +#define WS2811_PIN PA8 + +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_BLACKBOX | FEATURE_TELEMETRY) +#define CURRENT_METER_SCALE 400 + +#define USE_DSHOT +#define USE_SERIALSHOT +#define USE_ESC_SENSOR +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define MAX_PWM_OUTPUT_PORTS 10 + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) diff --git a/src/main/target/SPEEDYBEEF405WING/target.c b/src/main/target/SPEEDYBEEF405WING/target.c index a06d7838011..c6141b9a731 100644 --- a/src/main/target/SPEEDYBEEF405WING/target.c +++ b/src/main/target/SPEEDYBEEF405WING/target.c @@ -22,19 +22,19 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1, 0), // S1 D(1,3,2) - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1, 0), // S2 D(1,0,2) + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 1, 0), // S1 D(1,3,2) + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 1, 0), // S2 D(1,0,2) - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S3 D(1,7,5) - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S4 D(1,2,5) - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S5 D(2,4,7) - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S6 D(2,7,7) + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 1, 0), // S3 D(1,7,5) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 1, 0), // S4 D(1,2,5) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 1, 0), // S5 D(2,4,7) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 1, 0), // S6 D(2,7,7) - DEF_TIM(TIM8, CH2N,PB14, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S7 - DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 1, 0), // S8 - DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 1, 0), // S9 - DEF_TIM(TIM2, CH4, PB11, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 1, 0), // S10 - DEF_TIM(TIM1, CH3N,PB15, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 1, 0), // S11 + DEF_TIM(TIM8, CH2N, PB14, TIM_USE_OUTPUT_AUTO, 1, 0), // S7 + DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 1, 0), // S8 + DEF_TIM(TIM2, CH3, PB10, TIM_USE_OUTPUT_AUTO, 1, 0), // S9 + DEF_TIM(TIM2, CH4, PB11, TIM_USE_OUTPUT_AUTO, 1, 0), // S10 + DEF_TIM(TIM12, CH2, PB15, TIM_USE_OUTPUT_AUTO, 1, 0), // S11 DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), //2812LED D(1,5,3) DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), //TX2 softserial1_Tx diff --git a/src/main/target/SPEEDYBEEF7/target.c b/src/main/target/SPEEDYBEEF7/target.c index d4545792048..78821f52594 100644 --- a/src/main/target/SPEEDYBEEF7/target.c +++ b/src/main/target/SPEEDYBEEF7/target.c @@ -26,13 +26,13 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), // PPM - DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 - DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 - DEF_TIM(TIM3, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 - DEF_TIM(TIM3, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 1), // S5 + DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + DEF_TIM(TIM3, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 + DEF_TIM(TIM3, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 1), // S5 - DEF_TIM(TIM4, CH2, PB7, TIM_USE_LED | TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // LED/S6 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_LED | TIM_USE_OUTPUT_AUTO, 0, 0), // LED/S6 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/SPEEDYBEEF745AIO/target.c b/src/main/target/SPEEDYBEEF745AIO/target.c index 8a97ac96bc2..2c03c86e425 100644 --- a/src/main/target/SPEEDYBEEF745AIO/target.c +++ b/src/main/target/SPEEDYBEEF745AIO/target.c @@ -30,13 +30,13 @@ timerHardware_t timerHardware[] = { - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR, 0, 0), // S1 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR, 0, 0), // S2 - DEF_TIM(TIM1, CH1, PE9, TIM_USE_MC_MOTOR, 0, 2), // S3 - DEF_TIM(TIM1, CH2, PE11, TIM_USE_MC_MOTOR, 0, 1), // S4 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0), // S1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0), // S2 + DEF_TIM(TIM1, CH1, PE9, TIM_USE_MOTOR, 0, 2), // S3 + DEF_TIM(TIM1, CH2, PE11, TIM_USE_MOTOR, 0, 1), // S4 DEF_TIM(TIM4, CH1, PD12, TIM_USE_LED, 0, 0), // LED DEF_TIM(TIM2, CH2, PB3, TIM_USE_ANY, 0, 0), // Camera Control }; -const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); \ No newline at end of file +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/SPEEDYBEEF7MINI/CMakeLists.txt b/src/main/target/SPEEDYBEEF7MINI/CMakeLists.txt index c7a92612742..1ae9febd177 100644 --- a/src/main/target/SPEEDYBEEF7MINI/CMakeLists.txt +++ b/src/main/target/SPEEDYBEEF7MINI/CMakeLists.txt @@ -1 +1,2 @@ target_stm32f722xe(SPEEDYBEEF7MINI) +target_stm32f722xe(SPEEDYBEEF7MINIV2) diff --git a/src/main/target/SPEEDYBEEF7MINI/config.c b/src/main/target/SPEEDYBEEF7MINI/config.c index 1e69aaa33c6..3fa87ce9b21 100644 --- a/src/main/target/SPEEDYBEEF7MINI/config.c +++ b/src/main/target/SPEEDYBEEF7MINI/config.c @@ -19,6 +19,8 @@ #include "platform.h" +#include "drivers/pwm_mapping.h" + #include "fc/fc_msp_box.h" #include "io/piniobox.h" @@ -30,4 +32,7 @@ void targetConfiguration(void) serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART6)].msp_baudrateIndex = BAUD_115200; pinioBoxConfigMutable()->permanentId[0] = BOXARM; + + // To improve backwards compatibility with INAV versions 6.x and older + timerOverridesMutable(timer2id(TIM4))->outputMode = OUTPUT_MODE_MOTORS; } diff --git a/src/main/target/SPEEDYBEEF7MINI/target.c b/src/main/target/SPEEDYBEEF7MINI/target.c index 468ac3c2f62..32b5792dda2 100644 --- a/src/main/target/SPEEDYBEEF7MINI/target.c +++ b/src/main/target/SPEEDYBEEF7MINI/target.c @@ -26,19 +26,23 @@ #include "drivers/pinio.h" #include "drivers/sensor.h" +#ifdef SPEEDYBEEF7MINIV2 +BUSDEV_REGISTER_SPI_TAG(busdev_bmi270, DEVHW_BMI270, BMI270_SPI_BUS, BMI270_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_BMI270_ALIGN); +#else BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU6000_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_MPU6000_ALIGN); +#endif timerHardware_t timerHardware[] = { - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S1 UP1-2 D(1, 4, 5) - DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S2 UP1-2 D(1, 5, 5) - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 UP1-2 D(1, 7, 5) - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 UP1-2 D(1, 2, 5) + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 UP1-2 D(1, 4, 5) + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 UP1-2 D(1, 5, 5) + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 UP1-2 D(1, 7, 5) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 UP1-2 D(1, 2, 5) - DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 UP1-7 D(1, 5, 3) - clash with S2 - DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 UP1-7 D(1, 6, 3) + DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 UP1-7 D(1, 5, 3) - clash with S2 + DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 UP1-7 D(1, 6, 3) - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S7 UP1-6 D(1, 0, 2) - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S8 UP1-6 D(1, 3, 2) + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 UP1-6 D(1, 0, 2) // used to be fw motor + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 UP1-6 D(1, 3, 2) // used to be fw motor DEF_TIM(TIM8, CH3, PC8, TIM_USE_LED, 0, 0), // LED D(2, 6, 0) diff --git a/src/main/target/SPEEDYBEEF7MINI/target.h b/src/main/target/SPEEDYBEEF7MINI/target.h index f6db8e86fa4..0dd3a6e7a83 100644 --- a/src/main/target/SPEEDYBEEF7MINI/target.h +++ b/src/main/target/SPEEDYBEEF7MINI/target.h @@ -19,7 +19,12 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "SBMN" + +#ifdef SPEEDYBEEF7MINIV2 +#define USBD_PRODUCT_STRING "SPEEDYBEEF7MINIV2" +#else #define USBD_PRODUCT_STRING "SPEEDYBEEF7MINI" +#endif #define LED0 PA14 //Blue SWCLK @@ -35,11 +40,22 @@ #define SPI1_MISO_PIN PA6 #define SPI1_MOSI_PIN PA7 +#ifdef SPEEDYBEEF7MINIV2 + +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN CW180_DEG +#define BMI270_CS_PIN PB2 +#define BMI270_SPI_BUS BUS_SPI1 + +#else + #define USE_IMU_MPU6000 #define IMU_MPU6000_ALIGN CW0_DEG #define MPU6000_CS_PIN PB2 #define MPU6000_SPI_BUS BUS_SPI1 +#endif + // *************** I2C /Baro/Mag ********************* #define USE_I2C #define USE_I2C_DEVICE_1 diff --git a/src/main/target/SPEEDYBEEF7V2/config.c b/src/main/target/SPEEDYBEEF7V2/config.c index dccd2e8a048..c653b78b293 100644 --- a/src/main/target/SPEEDYBEEF7V2/config.c +++ b/src/main/target/SPEEDYBEEF7V2/config.c @@ -24,6 +24,8 @@ #include "platform.h" +#include "drivers/pwm_mapping.h" + #include "fc/fc_msp_box.h" #include "io/piniobox.h" @@ -32,4 +34,7 @@ void targetConfiguration(void) { pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + + // To improve backwards compatibility with INAV versions 6.x and older + timerOverridesMutable(timer2id(TIM3))->outputMode = OUTPUT_MODE_MOTORS; } diff --git a/src/main/target/SPEEDYBEEF7V2/target.c b/src/main/target/SPEEDYBEEF7V2/target.c index 60b86410735..0914224a05b 100644 --- a/src/main/target/SPEEDYBEEF7V2/target.c +++ b/src/main/target/SPEEDYBEEF7V2/target.c @@ -29,14 +29,14 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM4, CH2, PB7, TIM_USE_PPM, 0, 0), // PPM - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S1 - DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S2 - DEF_TIM(TIM1, CH3, PA10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S3 - DEF_TIM(TIM3, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S4 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 1), // S1 + DEF_TIM(TIM1, CH2, PA9, TIM_USE_OUTPUT_AUTO, 0, 1), // S2 + DEF_TIM(TIM1, CH3, PA10, TIM_USE_OUTPUT_AUTO, 0, 1), // S3 + DEF_TIM(TIM3, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 // used to be fw motor DEF_TIM(TIM8, CH2N, PB0, TIM_USE_LED, 0, 0), // LED DEF_TIM(TIM5, CH1, PA0, TIM_USE_ANY, 0, 0), // Camera Control }; -const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); \ No newline at end of file +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/SPEEDYBEEF7V3/target.c b/src/main/target/SPEEDYBEEF7V3/target.c index ad2eaf1353b..5d5b064f81d 100644 --- a/src/main/target/SPEEDYBEEF7V3/target.c +++ b/src/main/target/SPEEDYBEEF7V3/target.c @@ -33,17 +33,17 @@ BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU600 timerHardware_t timerHardware[] = { - DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 - DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 - DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 Clash with S2, DSHOT does not work - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S7 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S8 + DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 Clash with S2, DSHOT does not work + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 DEF_TIM(TIM8, CH3, PC8, TIM_USE_LED, 0, 0), // LED DEF_TIM(TIM5, CH1, PA0, TIM_USE_ANY, 0, 0), // Camera Control }; -const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); \ No newline at end of file +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/SPRACINGF4EVO/config.c b/src/main/target/SPRACINGF4EVO/config.c index fa1e8022978..31e6b2d0fd0 100755 --- a/src/main/target/SPRACINGF4EVO/config.c +++ b/src/main/target/SPRACINGF4EVO/config.c @@ -23,10 +23,14 @@ #include "io/serial.h" #include "sensors/barometer.h" #include "telemetry/telemetry.h" +#include "drivers/pwm_mapping.h" void targetConfiguration(void) { barometerConfigMutable()->baro_hardware = BARO_BMP280; serialConfigMutable()->portConfigs[1].functionMask = FUNCTION_MSP; // So SPRacingF3OSD users don't have to change anything. serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_SMARTPORT; + + // To improve backwards compatibility with INAV versions 6.x and older + timerOverridesMutable(timer2id(TIM3))->outputMode = OUTPUT_MODE_MOTORS; } diff --git a/src/main/target/SPRACINGF4EVO/target.c b/src/main/target/SPRACINGF4EVO/target.c index a52df8997db..deabe8665d6 100755 --- a/src/main/target/SPRACINGF4EVO/target.c +++ b/src/main/target/SPRACINGF4EVO/target.c @@ -26,21 +26,21 @@ timerHardware_t timerHardware[] = 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 - DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // ESC 1 - DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // ESC 2 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // ESC 3 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // ESC 4 + DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 1), // ESC 1 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 1), // ESC 2 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // ESC 3 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 1), // ESC 4 #if defined(SPRACINGF4EVO_REV) && (SPRACINGF4EVO_REV >= 2) - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // ESC 5 / Conflicts with USART5_RX / SPI3_RX - SPI3_RX can be mapped to DMA1_ST3_CH0 - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // ESC 6 / Conflicts with USART3_RX + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // ESC 5 / Conflicts with USART5_RX / SPI3_RX - SPI3_RX can be mapped to DMA1_ST3_CH0 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // ESC 6 / Conflicts with USART3_RX #else - DEF_TIM(TIM10, CH1, PB8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // ESC 5 - DEF_TIM(TIM11, CH1, PB9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // ESC 6 + DEF_TIM(TIM10, CH1, PB8, TIM_USE_OUTPUT_AUTO, 0, 0), // ESC 5 + DEF_TIM(TIM11, CH1, PB9, TIM_USE_OUTPUT_AUTO, 0, 0), // ESC 6 #endif - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // ESC 7 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // ESC 8 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // ESC 7 // used to be fw motor + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // ESC 8 // used to be fw motor DEF_TIM(TIM2, CH2, PA1, TIM_USE_LED, 0, 0), // LED Strip }; diff --git a/src/main/target/SPRACINGF7DUAL/target.c b/src/main/target/SPRACINGF7DUAL/target.c index 533cc17c58c..f47d1a08355 100644 --- a/src/main/target/SPRACINGF7DUAL/target.c +++ b/src/main/target/SPRACINGF7DUAL/target.c @@ -39,34 +39,34 @@ timerHardware_t timerHardware[] = { 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_MC_MOTOR, 0, 0), // ESC 1 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_MOTOR, 0, 0), // ESC 1 #else - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR, 0, 0), // ESC 1 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, 0, 0), // ESC 1 #endif - DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR, 0, 0), // ESC 2 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR, 0, 0), // ESC 3 + 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_MC_MOTOR, 0, 0), // ESC 4 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, 0, 0), // ESC 4 #else - DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR, 0, 0), // ESC 4 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_MOTOR, 0, 0), // ESC 4 #endif - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR, 0, 0), // ESC 5 / Conflicts with USART5_RX / SPI3_RX - SPI3_RX can be mapped to DMA1_ST3_CH0 - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR, 0, 0), // ESC 6 / Conflicts with USART3_RX - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR, 0, 0), // ESC 7 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR, 0, 0), // ESC 8 + 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 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MOTOR, 0, 0), // ESC 6 / Conflicts with USART3_RX + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0), // ESC 7 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0), // ESC 8 DEF_TIM(TIM2, CH2, PA1, TIM_USE_LED, 0, 0), // LED Strip // Additional 2 PWM channels available on UART3 RX/TX pins // However, when using led strip the timer cannot be used, but no code appears to prevent that right now - DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_MOTOR, 0, 0), // Shared with UART3 TX PIN and SPI3 TX (OSD) - DEF_TIM(TIM2, CH4, PB11, TIM_USE_MC_MOTOR, 0, 0), // Shared with UART3 RX PIN + DEF_TIM(TIM2, CH3, PB10, TIM_USE_MOTOR, 0, 0), // Shared with UART3 TX PIN and SPI3 TX (OSD) + DEF_TIM(TIM2, CH4, PB11, TIM_USE_MOTOR, 0, 0), // Shared with UART3 RX PIN //DEF_TIM(TIM1, CH1, PA8, USE_TRANSPONDER, 0, 0), // Transponder // Additional 2 PWM channels available on UART1 RX/TX pins // However, when using transponder the timer cannot be used, but no code appears to prevent that right now - DEF_TIM(TIM1, CH2, PA9, TIM_USE_FW_SERVO | TIM_USE_PWM, 0, 1), // PWM 3 - DEF_TIM(TIM1, CH3, PA10, TIM_USE_FW_SERVO | TIM_USE_PWM, 0, 1), // PWM 4 + DEF_TIM(TIM1, CH2, PA9, TIM_USE_SERVO | TIM_USE_PWM, 0, 1), // PWM 3 + DEF_TIM(TIM1, CH3, PA10, TIM_USE_SERVO | TIM_USE_PWM, 0, 1), // PWM 4 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/TMOTORF7/target.c b/src/main/target/TMOTORF7/target.c index 966bc54d3aa..2f96517ba15 100644 --- a/src/main/target/TMOTORF7/target.c +++ b/src/main/target/TMOTORF7/target.c @@ -29,13 +29,13 @@ // BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500, DEVHW_MPU6500, MPU6500_SPI_BUS, MPU6500_CS_PIN, NONE, 1, DEVFLAGS_NONE, IMU_MPU6500_ALIGN); timerHardware_t timerHardware[] = { - DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), - DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), + DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 1), + DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 1), + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), - DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), + DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0), DEF_TIM(TIM2, CH1, PA15, TIM_USE_PPM, 0, 0), DEF_TIM(TIM11, CH1, PB9, TIM_USE_ANY, 0, 0), diff --git a/src/main/target/TMOTORF7V2/target.c b/src/main/target/TMOTORF7V2/target.c index 0a995a8ac27..606ac8819dc 100644 --- a/src/main/target/TMOTORF7V2/target.c +++ b/src/main/target/TMOTORF7V2/target.c @@ -26,17 +26,17 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), // PPM - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S3 - DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S4 - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S7 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S8 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED - DEF_TIM(TIM2, CH2, PB3, TIM_USE_PWM | TIM_USE_FW_SERVO, 0, 0), // "C.C" + DEF_TIM(TIM2, CH2, PB3, TIM_USE_PWM | TIM_USE_SERVO, 0, 0), // "C.C" }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/YUPIF4/target.c b/src/main/target/YUPIF4/target.c index d98374c7f1d..310ab66b0aa 100644 --- a/src/main/target/YUPIF4/target.c +++ b/src/main/target/YUPIF4/target.c @@ -24,23 +24,23 @@ #include "drivers/bus.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM8, CH3, PC8, TIM_USE_PPM, 0, 0 ), // PPM IN + DEF_TIM(TIM8, CH3, PC8, TIM_USE_PPM, 0, 0 ), // PPM IN - DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S1_OUT - DMA1_ST2 - DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S2_OUT - DMA1_ST4 - DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S3_OUT - DMA1_ST1 - DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1 ), // S4_OUT - DMA1_ST6 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0 ), // S5_OUT - DMA1_ST7 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S1_OUT - DMA1_ST2 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S2_OUT - DMA1_ST4 + DEF_TIM(TIM2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S3_OUT - DMA1_ST1 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 1 ), // S4_OUT - DMA1_ST6 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S5_OUT - DMA1_ST7 #if defined (YUPIF4R2) - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO | TIM_USE_LED, 0, 0 ), - DEF_TIM(TIM12, CH1, PB14, TIM_USE_BEEPER, 0, 0 ), // BEEPER PWM OPT + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO | TIM_USE_LED, 0, 0 ), + DEF_TIM(TIM12, CH1, PB14, TIM_USE_BEEPER, 0, 0 ), // BEEPER PWM OPT #elif (YUPIF4MINI) - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0 ), - DEF_TIM(TIM3, CH4, PC9, TIM_USE_BEEPER, 0, 0 ), // BEEPER PWM + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0 ), + DEF_TIM(TIM3, CH4, PC9, TIM_USE_BEEPER, 0, 0 ), // BEEPER PWM #else - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO | TIM_USE_LED, 0, 0 ), - DEF_TIM(TIM3, CH4, PC9, TIM_USE_BEEPER, 0, 0 ), // BEEPER PWM + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO | TIM_USE_LED, 0, 0 ), + DEF_TIM(TIM3, CH4, PC9, TIM_USE_BEEPER, 0, 0 ), // BEEPER PWM #endif }; diff --git a/src/main/target/YUPIF7/target.c b/src/main/target/YUPIF7/target.c index 3794e7032c8..cd6a17ee683 100644 --- a/src/main/target/YUPIF7/target.c +++ b/src/main/target/YUPIF7/target.c @@ -24,15 +24,15 @@ #include "drivers/bus.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM8, CH3, PC8, TIM_USE_PPM, 0, 1 ), // PPM IN - DEF_TIM(TIM2, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S1_OUT - DMA1_ST2 - DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S2_OUT - DMA1_ST4 - DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S3_OUT - DMA1_ST1 - DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1 ), // S4_OUT - DMA1_ST6 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0 ), // S5_OUT - DMA1_ST7 - LED Control - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO | TIM_USE_LED, 0, 0 ), // S6_OUT - DEF_TIM(TIM4, CH2, PB7, TIM_USE_ANY, 0, 0 ), // ANY - DEF_TIM(TIM12, CH1, PB14, TIM_USE_BEEPER, 0, 0 ), // BEEPER PWM + DEF_TIM(TIM8, CH3, PC8, TIM_USE_PPM, 0, 1 ), // PPM IN + DEF_TIM(TIM2, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S1_OUT - DMA1_ST2 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S2_OUT - DMA1_ST4 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S3_OUT - DMA1_ST1 + DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 1 ), // S4_OUT - DMA1_ST6 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S5_OUT - DMA1_ST7 - LED Control + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO | TIM_USE_LED, 0, 0 ), // S6_OUT + DEF_TIM(TIM4, CH2, PB7, TIM_USE_ANY, 0, 0 ), // ANY + DEF_TIM(TIM12, CH1, PB14, TIM_USE_BEEPER, 0, 0 ), // BEEPER PWM }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/ZEEZF7/target.c b/src/main/target/ZEEZF7/target.c index 9901d7f83be..f1f5356dcb2 100755 --- a/src/main/target/ZEEZF7/target.c +++ b/src/main/target/ZEEZF7/target.c @@ -27,32 +27,32 @@ timerHardware_t timerHardware[] = { #ifdef ZEEZF7V3 - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 - DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 - DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 1), // S5 - DEF_TIM(TIM3, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), // S6 - DEF_TIM(TIM3, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), // S7 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), // S8 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 + DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 + DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 1), // S5 + DEF_TIM(TIM3, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 + DEF_TIM(TIM3, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 #endif #ifdef ZEEZF7V2 - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 - DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 - DEF_TIM(TIM3, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 - DEF_TIM(TIM3, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 - DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S7 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S8 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 + DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 + DEF_TIM(TIM3, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 + DEF_TIM(TIM3, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 + DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 1), // S7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 #endif #ifdef ZEEZF7 - DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR, 0, 0), // S1 - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR, 0, 0), // S2 - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR, 0, 0), // S3 - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR, 0, 0), // S4 + DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 #endif DEF_TIM(TIM1, CH2N, PB0, TIM_USE_LED , 0, 0), // LED STRIP diff --git a/src/main/target/common.h b/src/main/target/common.h old mode 100755 new mode 100644 index 815b3ae41c8..469750c3627 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -121,8 +121,6 @@ #define USE_I2C_IO_EXPANDER -#define USE_GPS_PROTO_NMEA - #define USE_TELEMETRY_SIM #define USE_TELEMETRY_MAVLINK #define USE_MSP_OVER_TELEMETRY @@ -190,5 +188,9 @@ #define USE_SERIALRX_SUMD #define USE_TELEMETRY_HOTT #define USE_HOTT_TEXTMODE - +#define USE_24CHANNELS +#else +#define MAX_MIXER_PROFILE_COUNT 1 #endif + +#define USE_EZ_TUNE \ No newline at end of file diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 90c1377bc80..30237c28552 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -54,7 +54,7 @@ #include "flight/failsafe.h" #include "flight/imu.h" -#include "flight/mixer.h" +#include "flight/mixer_profile.h" #include "flight/pid.h" #include "flight/servos.h" diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index e6146a03938..c7edad973b8 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -30,12 +30,6 @@ #include "io/serial.h" - -typedef enum { - FRSKY_FORMAT_DMS = 0, - FRSKY_FORMAT_NMEA -} frskyGpsCoordFormat_e; - typedef enum { LTM_RATE_NORMAL, LTM_RATE_MEDIUM, diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc index 6a8aa6f0918..fd126c5c41a 100644 --- a/src/test/unit/osd_unittest.cc +++ b/src/test/unit/osd_unittest.cc @@ -14,55 +14,55 @@ TEST(OSDTest, TestCentiNumber) //bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int maxDecimals, int maxScaledDecimals, int length); char buf[11] = "0123456789"; - osdFormatCentiNumber(buf, 12345, 1, 2, 3, 7); + osdFormatCentiNumber(buf, 12345, 1, 2, 3, 7, false); std::cout << "'" << buf << "'" << std::endl; EXPECT_FALSE(strcmp(buf, " 123.45")); memset(buf, 0, 11); - osdFormatCentiNumber(buf, 12345, 1, 2, 2, 6); + osdFormatCentiNumber(buf, 12345, 1, 2, 2, 6, false); std::cout << "'" << buf << "'" << std::endl; EXPECT_FALSE(strcmp(buf, "123.45")); memset(buf, 0, 11); - osdFormatCentiNumber(buf, 12345, 1, 2, 2, 5); + osdFormatCentiNumber(buf, 12345, 1, 2, 2, 5, false); std::cout << "'" << buf << "'" << std::endl; EXPECT_FALSE(strcmp(buf, "123.4")); memset(buf, 0, 11); - osdFormatCentiNumber(buf, 12345, 1, 2, 2, 4); + osdFormatCentiNumber(buf, 12345, 1, 2, 2, 4, false); std::cout << "'" << buf << "'" << std::endl; EXPECT_FALSE(strcmp(buf, " 123")); // this should be causing #8769 memset(buf, 0, 11); - osdFormatCentiNumber(buf, 12345, 1, 2, 2, 3); + osdFormatCentiNumber(buf, 12345, 1, 2, 2, 3, false); std::cout << "'" << buf << "'" << std::endl; EXPECT_FALSE(strcmp(buf, "123")); std::cout << "'" << buf << "'" << std::endl; memset(buf, 0, 11); - osdFormatCentiNumber(buf, -12345, 1, 2, 2, 8); + osdFormatCentiNumber(buf, -12345, 1, 2, 2, 8, false); std::cout << "'" << buf << "'" << std::endl; EXPECT_FALSE(strcmp(buf, " -123.45")); memset(buf, 0, 11); - osdFormatCentiNumber(buf, -12345, 1, 2, 2, 7); + osdFormatCentiNumber(buf, -12345, 1, 2, 2, 7, false); std::cout << "'" << buf << "'" << std::endl; EXPECT_FALSE(strcmp(buf, "-123.45")); memset(buf, 0, 11); - osdFormatCentiNumber(buf, -12345, 1, 2, 2, 6); + osdFormatCentiNumber(buf, -12345, 1, 2, 2, 6, false); std::cout << "'" << buf << "'" << std::endl; EXPECT_FALSE(strcmp(buf, "-123.4")); memset(buf, 0, 11); - osdFormatCentiNumber(buf, -12345, 1, 2, 2, 5); + osdFormatCentiNumber(buf, -12345, 1, 2, 2, 5, false); std::cout << "'" << buf << "'" << std::endl; EXPECT_FALSE(strcmp(buf, " -123")); memset(buf, 0, 11); - osdFormatCentiNumber(buf, -12345, 1, 2, 2, 4); + osdFormatCentiNumber(buf, -12345, 1, 2, 2, 4, false); std::cout << "'" << buf << "'" << std::endl; EXPECT_FALSE(strcmp(buf, "-123")); diff --git a/src/test/unit/target.h b/src/test/unit/target.h index ac7617a63e2..52725837642 100755 --- a/src/test/unit/target.h +++ b/src/test/unit/target.h @@ -21,7 +21,6 @@ #define USE_MAG #define USE_BARO #define USE_GPS -#define USE_GPS_PROTO_NMEA #define USE_GPS_PROTO_UBLOX #define USE_DASHBOARD #define USE_TELEMETRY diff --git a/src/utils/rename-ifdefs.py b/src/utils/rename-ifdefs.py index bf3f7826d2e..a70a950243f 100644 --- a/src/utils/rename-ifdefs.py +++ b/src/utils/rename-ifdefs.py @@ -27,7 +27,6 @@ 'TELEMETRY_LTM', 'TELEMETRY_FRSKY', 'CMS', - 'GPS_PROTO_NMEA', 'GPS_PROTO_UBLOX_NEO7PLUS', 'TELEMETRY_HOTT', 'TELEMETRY_IBUS',