Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-15 04:15:38 +03:00
Code Issues Wiki Activity

Merge branch 'iNavFlight:master' into add-azimuth-to-S.Port/F.Port-telemetry

Browse source
This commit is contained in:
Darren Lines 2021-07-13 12:57:44 +01:00 • committed by GitHub
commit 6ddfdcf7c8
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
110 changed files with 2255 additions and 1129 deletions
Download patch file Download diff file Expand all files Collapse all files

2
.gitattributes vendored
View file

@ -10,4 +10,4 @@
Makefile text Makefile text
*.bat eol=crlf *.bat eol=crlf
*.txt text *.txt text
*.sh text *.sh text eol=lf

2
CMakeLists.txt
View file

@ -37,7 +37,7 @@ else()
include("${CMAKE_CURRENT_SOURCE_DIR}/cmake/${TOOLCHAIN}-checks.cmake") include("${CMAKE_CURRENT_SOURCE_DIR}/cmake/${TOOLCHAIN}-checks.cmake")
endif() endif()
project(INAV VERSION 3.0.1) project(INAV VERSION 3.1.0)
enable_language(ASM) enable_language(ASM)

7
README.md
View file

@ -1,11 +1,6 @@
# INAV - navigation capable flight controller # INAV - navigation capable flight controller
## F3 based flight controllers
> STM32 F3 flight controllers like Omnibus F3 or SP Racing F3 are deprecated and soon they will reach the end of support in INAV. If you are still using F3 boards, please migrate to F4 or F7.
![INAV](http://static.rcgroups.net/forums/attachments/6/1/0/3/7/6/a9088858-102-inav.png) ![INAV](http://static.rcgroups.net/forums/attachments/6/1/0/3/7/6/a9088858-102-inav.png)
![Travis CI status](https://travis-ci.org/iNavFlight/inav.svg?branch=master)
## Features ## Features
@ -16,9 +11,9 @@
* Fully configurable mixer that allows to run any hardware you want: multirotor, fixed wing, rovers, boats and other experimental devices * Fully configurable mixer that allows to run any hardware you want: multirotor, fixed wing, rovers, boats and other experimental devices
* Multiple sensor support: GPS, Pitot tube, sonar, lidar, temperature, ESC with BlHeli_32 telemetry * Multiple sensor support: GPS, Pitot tube, sonar, lidar, temperature, ESC with BlHeli_32 telemetry
* SmartAudio and IRC Tramp VTX support * SmartAudio and IRC Tramp VTX support
* DSHOT and Multishot ESCs
* Blackbox flight recorder logging * Blackbox flight recorder logging
* On Screen Display (OSD) - both character and pixel style * On Screen Display (OSD) - both character and pixel style
* DJI OSD integration: all elements, system messages and warnings
* Telemetry: SmartPort, FPort, MAVlink, LTM * Telemetry: SmartPort, FPort, MAVlink, LTM
* Multi-color RGB LED Strip support * Multi-color RGB LED Strip support
* Advanced gyro filtering: Matrix Filter and RPM filter * Advanced gyro filtering: Matrix Filter and RPM filter

12
docs/Board - BetFPVF722.md Normal file
View file

@ -0,0 +1,12 @@
# BetaFPVF722
## I2C bus for magnetometer
BetFPVF722 does not have I2C pads broken out. I2C is shared with UART3
* TX3 - SCL
* RX3 - SDA
> I2C and UART3 can not be used at the same time! Connect the serial receiver to a different serial port when I2C device like MAG is used.
![BetaFPVF722 I2C](assets/betafpvf722.png)

2
docs/Board - Matek F411-WSE.md
View file

@ -6,7 +6,7 @@
* STM32F411 CPU * STM32F411 CPU
* OSD * OSD
* BMP280 barometer * BMP280 barometer (DSP310 with new FCs from around June 2021)
* Integrated PDB for 2 motors * Integrated PDB for 2 motors
* 2 UART ports * 2 UART ports
* 6 servos * 6 servos

5
docs/Controls.md
View file

@ -34,14 +34,13 @@ The stick positions are combined to activate different functions:
| Trim Acc Forwards | HIGH | CENTER | HIGH | CENTER | | Trim Acc Forwards | HIGH | CENTER | HIGH | CENTER |
| Trim Acc Backwards | HIGH | CENTER | LOW | CENTER | | Trim Acc Backwards | HIGH | CENTER | LOW | CENTER |
| Save current waypoint mission | LOW | CENTER | HIGH | LOW | | Save current waypoint mission | LOW | CENTER | HIGH | LOW |
| Load/unload current waypoint mission | LOW | CENTER | HIGH | HIGH | | Load current waypoint mission | LOW | CENTER | HIGH | HIGH |
| Unload waypoint mission | LOW | CENTER | LOW | HIGH |
| Save setting | LOW | LOW | LOW | HIGH | | Save setting | LOW | LOW | LOW | HIGH |
| Enter OSD Menu (CMS) | CENTER | LOW | HIGH | CENTER | | Enter OSD Menu (CMS) | CENTER | LOW | HIGH | CENTER |
![Stick Positions](assets/images/StickPositions.png) ![Stick Positions](assets/images/StickPositions.png)
"Load/unload Mission" is a toggle. If no mission is loaded to RAM, the EEPROM mission is loaded; if a mission is in RAM, it is cleared. Successful loading is indicated by the `ACTION_SUCCESS` beep, otherwise the `ACTION_FAIL` beep is played.
## Yaw control ## Yaw control
While arming/disarming with sticks, your yaw stick will be moving to extreme values. In order to prevent your craft from trying to yaw during arming/disarming while on the ground, your yaw input will not cause the craft to yaw when the throttle is LOW (i.e. below the `min_check` setting). While arming/disarming with sticks, your yaw stick will be moving to extreme values. In order to prevent your craft from trying to yaw during arming/disarming while on the ground, your yaw input will not cause the craft to yaw when the throttle is LOW (i.e. below the `min_check` setting).

90
docs/Settings.md
View file

@ -452,6 +452,16 @@ If the remaining battery capacity goes below this threshold the beeper will emit
--- ---
### beeper_pwm_mode
Allows disabling PWM mode for beeper on some targets. Switch from ON to OFF if the external beeper sound is weak. Do not switch from OFF to ON without checking if the board supports PWM beeper mode
| Default | Min | Max |
| --- | --- | --- |
| OFF | | |
---
### blackbox_device ### blackbox_device
Selection of where to write blackbox data Selection of where to write blackbox data
@ -492,6 +502,16 @@ Stick deadband in [r/c points], applied after r/c deadband and expo. Used to che
--- ---
### controlrate_profile
Control rate profile to switch to when the battery profile is selected, 0 to disable and keep the currently selected control rate profile
| Default | Min | Max |
| --- | --- | --- |
| 0 | 0 | 3 |
---
### cpu_underclock ### cpu_underclock
This option is only available on certain architectures (F3 CPUs at the moment). It makes CPU clock lower to reduce interference to long-range RC systems working at 433MHz This option is only available on certain architectures (F3 CPUs at the moment). It makes CPU clock lower to reduce interference to long-range RC systems working at 433MHz
@ -622,6 +642,16 @@ OFF = OSD hardware blink / ON = OSD software blink. If OSD warning text/values a
--- ---
### dji_cn_alternating_duration
Alternating duration of craft name elements, in tenths of a second
| Default | Min | Max |
| --- | --- | --- |
| 30 | 1 | 150 |
---
### dji_esc_temp_source ### dji_esc_temp_source
Re-purpose the ESC temperature field for IMU/BARO temperature Re-purpose the ESC temperature field for IMU/BARO temperature
@ -632,9 +662,39 @@ Re-purpose the ESC temperature field for IMU/BARO temperature
--- ---
### dji_message_speed_source
Sets the speed type displayed by the DJI OSD in craft name: GROUND, 3D, AIR
| Default | Min | Max |
| --- | --- | --- |
| 3D | | |
---
### dji_rssi_source
Source of the DJI RSSI field: RSSI, CRSF_LQ
| Default | Min | Max |
| --- | --- | --- |
| RSSI | | |
---
### dji_use_adjustments
Show inflight adjustments in craft name field
| Default | Min | Max |
| --- | --- | --- |
| OFF | | |
---
### dji_use_name_for_messages ### dji_use_name_for_messages
Re-purpose the craft name field for messages. Replace craft name with :WTSED for Warnings|Throttle|Speed|Efficiency|Trip distance Re-purpose the craft name field for messages.
| Default | Min | Max | | Default | Min | Max |
| --- | --- | --- | | --- | --- | --- |
@ -1108,13 +1168,13 @@ The target percentage of maximum mixer output used for determining the rates in
| Default | Min | Max | | Default | Min | Max |
| --- | --- | --- | | --- | --- | --- |
| 90 | 50 | 100 | | 80 | 50 | 100 |
--- ---
### fw_autotune_min_stick ### fw_autotune_min_stick
Minimum stick input [%] to consider overshoot/undershoot detection Minimum stick input [%], after applying deadband and expo, to start recording the plane's response to stick input.
| Default | Min | Max | | Default | Min | Max |
| --- | --- | --- | | --- | --- | --- |
@ -5064,7 +5124,7 @@ Output frequency (in Hz) servo pins. When using tricopters or gimbal with digita
### setpoint_kalman_enabled ### setpoint_kalman_enabled
Enable Kalman filter on the PID controller setpoint Enable Kalman filter on the gyro data
| Default | Min | Max | | Default | Min | Max |
| --- | --- | --- | | --- | --- | --- |
@ -5082,26 +5142,6 @@ Quality factor of the setpoint Kalman filter. Higher values means less filtering
--- ---
### setpoint_kalman_sharpness
Dynamic factor for the setpoint Kalman filter. In general, the higher the value, the more dynamic Kalman filter gets
| Default | Min | Max |
| --- | --- | --- |
| 100 | 1 | 16000 |
---
### setpoint_kalman_w
Window size for the setpoint Kalman filter. Wider the window, more samples are used to compute variance. In general, wider window results in smoother filter response
| Default | Min | Max |
| --- | --- | --- |
| 4 | 1 | 40 |
---
### sim_ground_station_number ### sim_ground_station_number
Number of phone that is used to communicate with SIM module. Messages / calls from other numbers are ignored. If undefined, can be set by calling or sending a message to the module. Number of phone that is used to communicate with SIM module. Messages / calls from other numbers are ignored. If undefined, can be set by calling or sending a message to the module.
@ -5508,7 +5548,7 @@ Battery voltage calibration value. 1100 = 11:1 voltage divider (10k:1k) x 100. A
| Default | Min | Max | | Default | Min | Max |
| --- | --- | --- | | --- | --- | --- |
| _target default_ | VBAT_SCALE_MIN | VBAT_SCALE_MAX | | _target default_ | 0 | 65535 |
--- ---

BIN
docs/assets/betafpvf722.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 221 KiB

BIN
docs/assets/images/StickPositions.png
View file

Binary file not shown.
Side by side Swipe Overlay

Before

Width:  |  Height:  |  Size: 478 KiB

After

Width:  |  Height:  |  Size: 666 KiB

Before After
Before After

69
docs/assets/images/StickPositions.svg
View file

@ -29,7 +29,7 @@
inkscape:pageopacity="0.0" inkscape:pageopacity="0.0"
inkscape:pageshadow="2" inkscape:pageshadow="2"
inkscape:zoom="0.13995495" inkscape:zoom="0.13995495"
inkscape:cx="2157.8372" inkscape:cx="6594.9793"
inkscape:cy="3265.3365" inkscape:cy="3265.3365"
inkscape:document-units="px" inkscape:document-units="px"
inkscape:current-layer="g4157" inkscape:current-layer="g4157"
@ -627,6 +627,20 @@
id="path5923" id="path5923"
d="m 5992.6852,1082.4789 c 0,10.5728 -14.6765,19.1438 -32.7809,19.1438 -18.1044,0 -32.7809,-8.571 -32.7809,-19.1438 0,-10.5729 14.6765,-163.59574 32.7809,-163.59574 18.1044,0 32.7809,153.02284 32.7809,163.59574 z" d="m 5992.6852,1082.4789 c 0,10.5728 -14.6765,19.1438 -32.7809,19.1438 -18.1044,0 -32.7809,-8.571 -32.7809,-19.1438 0,-10.5729 14.6765,-163.59574 32.7809,-163.59574 18.1044,0 32.7809,153.02284 32.7809,163.59574 z"
style="display:inline;opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:8.07780743;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" /> style="display:inline;opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:8.07780743;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
<rect
ry="0"
y="1145.4941"
x="5811.6289"
height="288.94339"
width="288.94339"
id="rect5919-7"
style="display:inline;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.0566;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
<path
sodipodi:nodetypes="sssss"
inkscape:connector-curvature="0"
id="path5923-5"
d="m 5988.8813,1455.7655 c 0,10.5728 -14.6765,19.1438 -32.7809,19.1438 -18.1044,0 -32.7809,-8.571 -32.7809,-19.1438 0,-10.5729 14.6765,-163.5957 32.7809,-163.5957 18.1044,0 32.7809,153.0228 32.7809,163.5957 z"
style="display:inline;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:8.07781;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
<rect <rect
style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.05660725;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.05660725;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
id="rect5941" id="rect5941"
@ -653,48 +667,75 @@
id="tspan5963" id="tspan5963"
y="994.89398" y="994.89398"
x="5680.7969" x="5680.7969"
sodipodi:role="line">(Un)/load waypoint mission</tspan></text> sodipodi:role="line">Load waypoint mission</tspan></text>
<text
id="text5961-3"
y="1369.7102"
x="5681.5464"
style="font-style:normal;font-weight:normal;line-height:0%;font-family:sans-serif;letter-spacing:0px;word-spacing:0px;display:inline;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
xml:space="preserve"><tspan
style="font-size:286.599px;line-height:1.25;text-align:end;text-anchor:end"
id="tspan5963-6"
y="1369.7102"
x="5681.5464"
sodipodi:role="line">Unload waypoint mission</tspan></text>
<rect <rect
style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.05660725;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.0566;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
id="rect5971" id="rect5971"
width="288.94339" width="288.94339"
height="288.94339" height="288.94339"
x="5815.4326" x="5815.4326"
y="1192.6158" y="1526.3658"
ry="0" /> ry="0" />
<path <path
sodipodi:nodetypes="sssss" sodipodi:nodetypes="sssss"
inkscape:connector-curvature="0" inkscape:connector-curvature="0"
id="path5977" id="path5977"
d="m 5863.6503,1478.825 c -7.4762,7.4762 -23.9146,3.1589 -36.7164,-9.6428 -12.8017,-12.8018 -17.119,-29.2402 -9.6428,-36.7164 7.4761,-7.4761 126.0574,-105.3017 138.8592,-92.5 12.8017,12.8018 -85.0239,131.3831 -92.5,138.8592 z" d="m 5863.6503,1812.575 c -7.4762,7.4762 -23.9146,3.1589 -36.7164,-9.6428 -12.8017,-12.8018 -17.119,-29.2402 -9.6428,-36.7164 7.4761,-7.4761 126.0574,-105.3017 138.8592,-92.5 12.8017,12.8018 -85.0239,131.3831 -92.5,138.8592 z"
style="display:inline;opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:8.07780743;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" style="display:inline;opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:8.07781;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
inkscape:transform-center-y="73.935654" inkscape:transform-center-y="73.935654"
inkscape:transform-center-x="73.935653" /> inkscape:transform-center-x="73.935653" />
<rect <rect
ry="0" ry="0"
y="1192.6158" y="1526.3658"
x="6221.147" x="6221.147"
height="288.94339" height="288.94339"
width="288.94339" width="288.94339"
id="rect5993" id="rect5993"
style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.05660725;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" /> style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.0566;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
<path <path
sodipodi:nodetypes="sssss" sodipodi:nodetypes="sssss"
inkscape:connector-curvature="0" inkscape:connector-curvature="0"
id="path6001" id="path6001"
d="m 6460.9887,1478.825 c 7.4761,7.4762 23.9146,3.1589 36.7163,-9.6428 12.8017,-12.8018 17.119,-29.2402 9.6428,-36.7164 -7.4761,-7.4761 -126.0574,-105.3017 -138.8591,-92.5 -12.8018,12.8018 85.0238,131.3831 92.5,138.8592 z" d="m 6460.9887,1812.575 c 7.4761,7.4762 23.9146,3.1589 36.7163,-9.6428 12.8017,-12.8018 17.119,-29.2402 9.6428,-36.7164 -7.4761,-7.4761 -126.0574,-105.3017 -138.8591,-92.5 -12.8018,12.8018 85.0238,131.3831 92.5,138.8592 z"
style="display:inline;opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:8.07780743;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" style="display:inline;opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:8.07781;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
inkscape:transform-center-y="73.935654"
inkscape:transform-center-x="-73.935657" />
<rect
ry="0"
y="1140.2471"
x="6228.9062"
height="288.94339"
width="288.94339"
id="rect5993-3"
style="display:inline;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.0566;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
<path
sodipodi:nodetypes="sssss"
inkscape:connector-curvature="0"
id="path6001-5"
d="m 6468.7482,1426.4562 c 7.4761,7.4762 23.9146,3.1589 36.7163,-9.6428 12.8017,-12.8018 17.119,-29.2402 9.6428,-36.7164 -7.4761,-7.4761 -126.0574,-105.3017 -138.8591,-92.5 -12.8018,12.8018 85.0238,131.3831 92.5,138.8592 z"
style="display:inline;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:8.07781;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
inkscape:transform-center-y="73.935654" inkscape:transform-center-y="73.935654"
inkscape:transform-center-x="-73.935657" /> inkscape:transform-center-x="-73.935657" />
<text <text
xml:space="preserve" xml:space="preserve"
style="font-style:normal;font-weight:normal;line-height:0%;font-family:sans-serif;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" style="font-style:normal;font-weight:normal;line-height:0%;font-family:sans-serif;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
x="5680.7969" x="5714.6821"
y="1415.3022" y="1775.6128"
id="text6013"><tspan id="text6013"><tspan
sodipodi:role="line" sodipodi:role="line"
x="5680.7969" x="5714.6821"
y="1415.3022" y="1775.6128"
id="tspan6015" id="tspan6015"
style="font-size:286.599px;line-height:1.25;text-align:end;text-anchor:end">Save Setting</tspan></text> style="font-size:286.599px;line-height:1.25;text-align:end;text-anchor:end">Save Setting</tspan></text>
<text <text

Side by side Swipe Overlay

Before

Width:  |  Height:  |  Size: 44 KiB

After

Width:  |  Height:  |  Size: 46 KiB

Before After
Before After

4
docs/development/Building in Docker.md
View file

@ -23,12 +23,14 @@ Where `<TARGET>` must be replaced with the name of the target that you want to b
## Windows 10 ## Windows 10
Docker on Windows requires full paths for mounting volumes in `docker run` commands. For example: `c:\Users\pspyc\Documents\Projects\inav` becomes `//c/Users/pspyc/Documents/Projects/inav` . Docker on Windows requires full paths for mounting volumes in `docker run` commands. For example: `c:\Users\pspyc\Documents\Projects\inav` becomes `//c/Users/pspyc/Documents/Projects/inav` .
If you are getting error "standard_init_linux.go:219: exec user process caused: no such file or directory", make sure `\cmake\docker.sh` has lf (not crlf) line endings.
You'll have to manually execute the same steps that the build script does: You'll have to manually execute the same steps that the build script does:
1. `docker build -t inav-build .` 1. `docker build -t inav-build .`
+ This step is only needed the first time. + This step is only needed the first time.
2. `docker run --rm -it -v <PATH_TO_REPO>:/src inav-build <TARGET>` 2. `docker run --rm -it -u root -v <PATH_TO_REPO>:/src inav-build <TARGET>`
+ Where `<PATH_TO_REPO>` must be replaced with the absolute path of where you cloned this repo (see above), and `<TARGET>` with the name of the target that you want to build. + Where `<PATH_TO_REPO>` must be replaced with the absolute path of where you cloned this repo (see above), and `<TARGET>` 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.
Refer to the [Linux](#Linux) instructions or the [build script](/build.sh) for more details. Refer to the [Linux](#Linux) instructions or the [build script](/build.sh) for more details.

16
docs/development/Building in Linux.md
View file

@ -65,7 +65,7 @@ The `git clone` creates an `inav` directory; we can enter this directory, config
## Build tooling ## Build tooling
For 2.6 and later, inav uses `cmake` as its primary build tool. `cmake` simplies various platform and hardware dependencies required to cross compile multiple targets. `cmake` still uses GNU `make` to invoke the actual compiler. It is necessary to configure the build enviroment with `cmake` before we can build any firmware. For 2.6 and later, inav uses `cmake` as its primary build tool. `cmake` simplifies various platform and hardware dependencies required to cross compile multiple targets. `cmake` still uses GNU `make` to invoke the actual compiler. It is necessary to configure the build environment with `cmake` before we can build any firmware.
## Using `cmake` ## Using `cmake`
@ -96,14 +96,14 @@ Once `cmake` has generated the `build/Makefile`, this `Makfile` (with `make`) is
The generated `Makefile` uses different a target selection mechanism from the older (pre 2.6) top level `Makefile`; you can generate a list of targets with `make help` (or, as the list is extremely long), pipe this into a pager, e.g. `make help | less`. The generated `Makefile` uses different a target selection mechanism from the older (pre 2.6) top level `Makefile`; you can generate a list of targets with `make help` (or, as the list is extremely long), pipe this into a pager, e.g. `make help | less`.
Typically, to build a single target, just pass the target name to `make`; note that unlike eariler releases, `make` without a target specified will build **all** targets. Typically, to build a single target, just pass the target name to `make`; note that unlike earlier releases, `make` without a target specified will build **all** targets.
``` ```
# Build the MATEKF405 firmware # Build the MATEKF405 firmware
make MATEKF405 make MATEKF405
``` ```
One can also build multiple targets from a sinlge `make` command: One can also build multiple targets from a single `make` command:
``` ```
make MATEKF405 MATEKF722 make MATEKF405 MATEKF722
@ -128,7 +128,7 @@ make clean_MATEKF405
make clean_MATEKF405 clean_MATEKF722 make clean_MATEKF405 clean_MATEKF722
``` ```
### `cmake` cache maintainance ### `cmake` cache maintenance
`cmake` caches the build environment, so you don't need to rerun `cmake` each time you build a target. Two `make` options are provided to maintain the `cmake` cache. `cmake` caches the build environment, so you don't need to rerun `cmake` each time you build a target. Two `make` options are provided to maintain the `cmake` cache.
@ -141,7 +141,7 @@ It is unlikely that the typical user will need to employ these options, other th
In order to update your local firmware build: In order to update your local firmware build:
* Navigate to the local iNav repository * Navigate to the local inav repository
* Use the following steps to pull the latest changes and rebuild your local version of inav firmware from the `build` directory: * Use the following steps to pull the latest changes and rebuild your local version of inav firmware from the `build` directory:
``` ```
@ -154,3 +154,9 @@ $ make <TARGET>
## Advanced Usage ## Advanced Usage
For more advanced development information and `git` usage, please refer to the [development guide](https://github.com/iNavFlight/inav/blob/master/docs/development/Development.md). For more advanced development information and `git` usage, please refer to the [development guide](https://github.com/iNavFlight/inav/blob/master/docs/development/Development.md).
## Unsupported platforms
If you're using a host platform for which Arm does not supply a cross-compiler (Arm32, IA32), and the distro either does not package a suitable compiler or it's too old, then you can usually find a suitable compiler in the [xpack devtools collection](https://github.com/xpack-dev-tools/arm-none-eabi-gcc-xpack).
You will need to configure `cmake` to use the external compiler.

16
docs/development/Building in Windows 10 with Linux Subsystem.md → docs/development/Building in Windows 10 or 11 with Linux Subsystem.md
View file

@ -20,13 +20,13 @@ Install Ubuntu:
NOTE: from this point all commands are entered into the Ubunto shell command window NOTE: from this point all commands are entered into the Ubunto shell command window
Update the repo packages: Update the repo packages:
1. `sudo apt update` - `sudo apt update`
Install Git, Make, gcc and Ruby Install Git, Make, gcc and Ruby
1. `sudo apt-get install git` - `sudo apt-get install git make cmake ruby`
1. `sudo apt-get install make`
1. `sudo apt-get install cmake` Install python and python-yaml to allow updates to settings.md
1. `sudo apt-get install ruby` - `sudo apt-get install python3 python-yaml`
### CMAKE and Ubuntu 18_04 ### CMAKE and Ubuntu 18_04
@ -78,6 +78,12 @@ cd build
make MATEKF722 make MATEKF722
``` ```
## Updating the documents
```
cd /mnt/c/inav
python3 src/utils/update_cli_docs.py
```
## Flashing: ## Flashing:
Launch windows configurator GUI and from within the firmware flasher select `Load firmware[Local]` Launch windows configurator GUI and from within the firmware flasher select `Load firmware[Local]`
Hex files can be found in the folder `c:\inav\build` Hex files can be found in the folder `c:\inav\build`

80
docs/development/Building in Windows 10 or 11 with MSYS2.md Normal file
View file

@ -0,0 +1,80 @@
# General Info
This is a guide on how to use Windows MSYS2 distribution and building platform to build iNav firmware. This environment is very simple to manage and does not require installing docker for Windows which may get in the way of VMWare or any other virtualization software you already have running for other reasons. Another benefit of this approach is that the compiler runs natively on Windows, so performance is much better than compiling in a virtual environment or a container. You can also integrate with whatever IDE you are using to make code edits and work with github, which makes the entire development and testing workflow a lot more efficient. In addition to MSYS2, this build environment also uses Arm Embedded GCC tolkit from The xPack Project, which provides many benefits over the toolkits maintained by arm.com
Some of those benefits are described here:
https://xpack.github.io/arm-none-eabi-gcc/
## Setting up build environment
Download MSYS2 for your architecture (most likely 64-bit)
https://www.msys2.org/wiki/MSYS2-installation/
Click on 64-bit, scroll all the way down for the latest release
pacman is the package manager which makes it a lot easier to install and maintain all the dependencies
## Installing dependencies
Once MSYS2 is installed, you can open up a new terminal window by running:
"C:\msys64\mingw64.exe"
You can also make a shortcut of this somewhere on your taskbar, desktop, etc, or even setup a shortcut key to open it up every time you need to get a terminal window. If you right click on the window you can customize the font and layout to make it more comfortable to work with. This is very similar to cygwin or any other terminal program you might have used before
This is the best part:
```
pacman -S git ruby make cmake gcc mingw-w64-x86_64-libwinpthread-git unzip wget
```
Now, each release needs a different version of arm toolchain. To setup the xPack ARM toolchain, use the following process:
First, setup your work path, get the release you want to build or master if you want the latest/greatest
```
mkdir /c/Workspace
cd /c/Workspace
# you can also check out your own fork here which makes contributing easier
git clone https://github.com/iNavFlight/inav
cd inav
# switch to release you want or skip next 2 lines if you want latest
git fetch origin
git checkout -b release_2.6.1 origin/release_2.6.1
```
Now create the build and xpack directories and get the toolkit version you need for your inav version
```
mkdir build
cd build
mkdir /c/Workspace/xpack
cd /c/Workspace/xpack
cat /c/Workspace/inav/cmake/arm-none-eabi-checks.cmake | grep "set(arm_none_eabi_gcc_version" | cut -d\" -f2
```
This will give you the version you need for any given release or master branch. You can get to all the releases here and find the version you need
https://github.com/xpack-dev-tools/arm-none-eabi-gcc-xpack/releases/
```
# for version 2.6.1, version needed is 9.2.1
wget https://github.com/xpack-dev-tools/arm-none-eabi-gcc-xpack/releases/download/v9.2.1-1.1/xpack-arm-none-eabi-gcc-9.2.1-1.1-win32-x64.zip
unzip xpack-arm-none-eabi-gcc-9.2.1-1.1-win32-x64.zip
```
This is important, put the toolkit first before your path so that it is picked up ahead of any other versions that may be present on your system
```
export PATH=/c/Workspace/xpack/xpack-arm-none-eabi-gcc-9.2.1-1.1/bin:$PATH
cd /c/Workspace/inav/build
```
You may need to run rm -rf * in build directory if you had any failed previous runs now run cmake
```
# while inside the build directory
cmake ..
```
Once that's done you can compile the firmware for your controller
```
make DALRCF405
```
To get a list of available targets in iNav, see the target src folder
https://github.com/tednv/inav/tree/master/src/main/target
The generated hex file will be in /c/Workspace/inav/build folder
At the time of writting this document, I believe this is the fastest, easiest, and most efficient Windows build environment that is available. I have used this approach several years ago and was very happy with it building iNav 2.1 and 2.2, and now I'm getting back into it so figured I would share my method

55
docs/development/IDE - Visual Studio Code with Windows 10.md
View file

@ -79,30 +79,6 @@ Edit `./.vscode/tasks.json` to enable Building with `Ctrl + Shift + B` keyboard
// for the documentation about the tasks.json format // for the documentation about the tasks.json format
"version": "2.0.0", "version": "2.0.0",
"tasks": [ "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": "Install/Update CMAKE", "label": "Install/Update CMAKE",
"type": "shell", "type": "shell",
@ -112,6 +88,37 @@ Edit `./.vscode/tasks.json` to enable Building with `Ctrl + Shift + B` keyboard
"options": { "options": {
"cwd": "${workspaceFolder}" "cwd": "${workspaceFolder}"
} }
},
{
"label": "Compile autogenerated docs",
"type": "shell",
"command": "python3 src/utils/update_cli_docs.py",
"problemMatcher": [],
"options": {
"cwd": "${workspaceFolder}"
}
},
// Example of building a single target
{
"label": "Build Matek F722-WPX",
"type": "shell",
"command": "make MATEKF722WPX",
"group": "build",
"problemMatcher": [],
"options": {
"cwd": "${workspaceFolder}/build"
}
},
// Example of building multiple targets
{
"label": "Build Matek F405-STD & WING",
"type": "shell",
"command": "make MATEKF405 MATEKF405SE",
"group": "build",
"problemMatcher": [],
"options": {
"cwd": "${workspaceFolder}/build"
}
} }
] ]
} }

5
lib/main/STM32H7/Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_ll_exti.c
View file

@ -20,11 +20,6 @@
/* Includes ------------------------------------------------------------------*/ /* Includes ------------------------------------------------------------------*/
#include "stm32h7xx_ll_exti.h" #include "stm32h7xx_ll_exti.h"
#ifdef USE_FULL_ASSERT
#include "stm32_assert.h"
#else
#define assert_param(expr) ((void)0U)
#endif
/** @addtogroup STM32H7xx_LL_Driver /** @addtogroup STM32H7xx_LL_Driver
* @{ * @{

5
lib/main/STM32H7/Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_ll_i2c.c
View file

@ -21,11 +21,6 @@
/* Includes ------------------------------------------------------------------*/ /* Includes ------------------------------------------------------------------*/
#include "stm32h7xx_ll_i2c.h" #include "stm32h7xx_ll_i2c.h"
#include "stm32h7xx_ll_bus.h" #include "stm32h7xx_ll_bus.h"
#ifdef USE_FULL_ASSERT
#include "stm32_assert.h"
#else
#define assert_param(expr) ((void)0U)
#endif
/** @addtogroup STM32H7xx_LL_Driver /** @addtogroup STM32H7xx_LL_Driver
* @{ * @{

4
src/main/CMakeLists.txt
View file

@ -242,7 +242,7 @@ main_sources(COMMON_SRC
drivers/rangefinder/rangefinder_virtual.h drivers/rangefinder/rangefinder_virtual.h
drivers/rangefinder/rangefinder_us42.c drivers/rangefinder/rangefinder_us42.c
drivers/rangefinder/rangefinder_us42.h drivers/rangefinder/rangefinder_us42.h
drivers/resource.c drivers/resource.c
drivers/resource.h drivers/resource.h
drivers/rcc.c drivers/rcc.c
@ -280,6 +280,7 @@ main_sources(COMMON_SRC
fc/config.h fc/config.h
fc/controlrate_profile.c fc/controlrate_profile.c
fc/controlrate_profile.h fc/controlrate_profile.h
fc/controlrate_profile_config_struct.h
fc/fc_core.c fc/fc_core.c
fc/fc_core.h fc/fc_core.h
fc/fc_init.c fc/fc_init.c
@ -440,6 +441,7 @@ main_sources(COMMON_SRC
sensors/acceleration.h sensors/acceleration.h
sensors/battery.c sensors/battery.c
sensors/battery.h sensors/battery.h
sensors/battery_config_structs.h
sensors/boardalignment.c sensors/boardalignment.c
sensors/boardalignment.h sensors/boardalignment.h
sensors/compass.c sensors/compass.c

25
src/main/cms/cms_menu_navigation.c
View file

@ -181,6 +181,30 @@ static const CMS_Menu cmsx_menuFWSettings = {
.entries = cmsx_menuFWSettingsEntries .entries = cmsx_menuFWSettingsEntries
}; };
static const OSD_Entry cmsx_menuMissionSettingsEntries[] =
{
OSD_LABEL_ENTRY("-- MISSIONS --"),
OSD_SETTING_ENTRY("MC WP SLOWDOWN", SETTING_NAV_MC_WP_SLOWDOWN),
OSD_SETTING_ENTRY("MISSION FAILSAFE", SETTING_FAILSAFE_MISSION),
OSD_SETTING_ENTRY("WP LOAD ON BOOT", SETTING_NAV_WP_LOAD_ON_BOOT),
OSD_SETTING_ENTRY("WP REACHED RADIUS", SETTING_NAV_WP_RADIUS),
OSD_SETTING_ENTRY("WP SAFE DISTANCE", SETTING_NAV_WP_SAFE_DISTANCE),
OSD_BACK_AND_END_ENTRY,
};
static const CMS_Menu cmsx_menuMissionSettings = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "MENUMISSIONSETTINGS",
.GUARD_type = OME_MENU,
#endif
.onEnter = NULL,
.onExit = NULL,
.onGlobalExit = NULL,
.entries = cmsx_menuMissionSettingsEntries
};
static const OSD_Entry cmsx_menuNavigationEntries[] = static const OSD_Entry cmsx_menuNavigationEntries[] =
{ {
OSD_LABEL_ENTRY("-- NAVIGATION --"), OSD_LABEL_ENTRY("-- NAVIGATION --"),
@ -188,6 +212,7 @@ static const OSD_Entry cmsx_menuNavigationEntries[] =
OSD_SUBMENU_ENTRY("BASIC SETTINGS", &cmsx_menuNavSettings), OSD_SUBMENU_ENTRY("BASIC SETTINGS", &cmsx_menuNavSettings),
OSD_SUBMENU_ENTRY("RTH", &cmsx_menuRTH), OSD_SUBMENU_ENTRY("RTH", &cmsx_menuRTH),
OSD_SUBMENU_ENTRY("FIXED WING", &cmsx_menuFWSettings), OSD_SUBMENU_ENTRY("FIXED WING", &cmsx_menuFWSettings),
OSD_SUBMENU_ENTRY("MISSIONS", &cmsx_menuMissionSettings),
OSD_BACK_AND_END_ENTRY, OSD_BACK_AND_END_ENTRY,
}; };

66
src/main/common/filter.c
View file

@ -109,6 +109,72 @@ void pt1FilterReset(pt1Filter_t *filter, float input)
filter->state = input; filter->state = input;
} }
/*
* PT2 LowPassFilter
*/
float pt2FilterGain(float f_cut, float dT)
{
const float order = 2.0f;
const float orderCutoffCorrection = 1 / sqrtf(powf(2, 1.0f / order) - 1);
float RC = 1 / (2 * orderCutoffCorrection * M_PIf * f_cut);
// float RC = 1 / (2 * 1.553773974f * M_PIf * f_cut);
// where 1.553773974 = 1 / sqrt( (2^(1 / order) - 1) ) and order is 2
return dT / (RC + dT);
}
void pt2FilterInit(pt2Filter_t *filter, float k)
{
filter->state = 0.0f;
filter->state1 = 0.0f;
filter->k = k;
}
void pt2FilterUpdateCutoff(pt2Filter_t *filter, float k)
{
filter->k = k;
}
FAST_CODE float pt2FilterApply(pt2Filter_t *filter, float input)
{
filter->state1 = filter->state1 + filter->k * (input - filter->state1);
filter->state = filter->state + filter->k * (filter->state1 - filter->state);
return filter->state;
}
/*
* PT3 LowPassFilter
*/
float pt3FilterGain(float f_cut, float dT)
{
const float order = 3.0f;
const float orderCutoffCorrection = 1 / sqrtf(powf(2, 1.0f / order) - 1);
float RC = 1 / (2 * orderCutoffCorrection * M_PIf * f_cut);
// float RC = 1 / (2 * 1.961459177f * M_PIf * f_cut);
// where 1.961459177 = 1 / sqrt( (2^(1 / order) - 1) ) and order is 3
return dT / (RC + dT);
}
void pt3FilterInit(pt3Filter_t *filter, float k)
{
filter->state = 0.0f;
filter->state1 = 0.0f;
filter->state2 = 0.0f;
filter->k = k;
}
void pt3FilterUpdateCutoff(pt3Filter_t *filter, float k)
{
filter->k = k;
}
FAST_CODE float pt3FilterApply(pt3Filter_t *filter, float input)
{
filter->state1 = filter->state1 + filter->k * (input - filter->state1);
filter->state2 = filter->state2 + filter->k * (filter->state1 - filter->state2);
filter->state = filter->state + filter->k * (filter->state2 - filter->state);
return filter->state;
}
// rate_limit = maximum rate of change of the output value in units per second // rate_limit = maximum rate of change of the output value in units per second
void rateLimitFilterInit(rateLimitFilter_t *filter) void rateLimitFilterInit(rateLimitFilter_t *filter)
{ {

27
src/main/common/filter.h
View file

@ -27,6 +27,17 @@ typedef struct pt1Filter_s {
float dT; float dT;
float alpha; float alpha;
} pt1Filter_t; } pt1Filter_t;
typedef struct pt2Filter_s {
float state;
float state1;
float k;
} pt2Filter_t;
typedef struct pt3Filter_s {
float state;
float state1;
float state2;
float k;
} pt3Filter_t;
/* this holds the data required to update samples thru a filter */ /* this holds the data required to update samples thru a filter */
typedef struct biquadFilter_s { typedef struct biquadFilter_s {
@ -87,6 +98,22 @@ float pt1FilterApply3(pt1Filter_t *filter, float input, float dT);
float pt1FilterApply4(pt1Filter_t *filter, float input, float f_cut, float dt); float pt1FilterApply4(pt1Filter_t *filter, float input, float f_cut, float dt);
void pt1FilterReset(pt1Filter_t *filter, float input); void pt1FilterReset(pt1Filter_t *filter, float input);
/*
* PT2 LowPassFilter
*/
float pt2FilterGain(float f_cut, float dT);
void pt2FilterInit(pt2Filter_t *filter, float k);
void pt2FilterUpdateCutoff(pt2Filter_t *filter, float k);
float pt2FilterApply(pt2Filter_t *filter, float input);
/*
* PT3 LowPassFilter
*/
float pt3FilterGain(float f_cut, float dT);
void pt3FilterInit(pt3Filter_t *filter, float k);
void pt3FilterUpdateCutoff(pt3Filter_t *filter, float k);
float pt3FilterApply(pt3Filter_t *filter, float input);
void rateLimitFilterInit(rateLimitFilter_t *filter); void rateLimitFilterInit(rateLimitFilter_t *filter);
float rateLimitFilterApply4(rateLimitFilter_t *filter, float input, float rate_limit, float dT); float rateLimitFilterApply4(rateLimitFilter_t *filter, float input, float rate_limit, float dT);

20
src/main/common/maths.c
View file

@ -25,6 +25,10 @@
#include "quaternion.h" #include "quaternion.h"
#include "platform.h" #include "platform.h"
#ifdef USE_ARM_MATH
#include "arm_math.h"
#endif
FILE_COMPILE_FOR_SPEED FILE_COMPILE_FOR_SPEED
// http://lolengine.net/blog/2011/12/21/better-function-approximations // http://lolengine.net/blog/2011/12/21/better-function-approximations
@ -95,7 +99,7 @@ float atan2_approx(float y, float x)
float acos_approx(float x) float acos_approx(float x)
{ {
float xa = fabsf(x); float xa = fabsf(x);
float result = sqrtf(1.0f - xa) * (1.5707288f + xa * (-0.2121144f + xa * (0.0742610f + (-0.0187293f * xa)))); float result = fast_fsqrtf(1.0f - xa) * (1.5707288f + xa * (-0.2121144f + xa * (0.0742610f + (-0.0187293f * xa))));
if (x < 0.0f) if (x < 0.0f)
return M_PIf - result; return M_PIf - result;
else else
@ -200,7 +204,7 @@ float devVariance(stdev_t *dev)
float devStandardDeviation(stdev_t *dev) float devStandardDeviation(stdev_t *dev)
{ {
return sqrtf(devVariance(dev)); return fast_fsqrtf(devVariance(dev));
} }
float degreesToRadians(int16_t degrees) float degreesToRadians(int16_t degrees)
@ -508,7 +512,7 @@ bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float resu
sensorCalibration_SolveLGS(state->XtX, beta, state->XtY); sensorCalibration_SolveLGS(state->XtX, beta, state->XtY);
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
result[i] = sqrtf(beta[i]); result[i] = fast_fsqrtf(beta[i]);
} }
return sensorCalibrationValidateResult(result); return sensorCalibrationValidateResult(result);
@ -518,3 +522,13 @@ float bellCurve(const float x, const float curveWidth)
{ {
return powf(M_Ef, -sq(x) / (2.0f * sq(curveWidth))); return powf(M_Ef, -sq(x) / (2.0f * sq(curveWidth)));
} }
float fast_fsqrtf(const double value) {
#ifdef USE_ARM_MATH
float squirt;
arm_sqrt_f32(value, &squirt);
return squirt;
#else
return sqrtf(value);
#endif
}

1
src/main/common/maths.h
View file

@ -176,3 +176,4 @@ float acos_approx(float x);
void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count); void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count);
float bellCurve(const float x, const float curveWidth); float bellCurve(const float x, const float curveWidth);
float fast_fsqrtf(const double value);

2
src/main/common/quaternion.h
View file

@ -142,7 +142,7 @@ static inline fpQuaternion_t * quaternionConjugate(fpQuaternion_t * result, cons
static inline fpQuaternion_t * quaternionNormalize(fpQuaternion_t * result, const fpQuaternion_t * q) static inline fpQuaternion_t * quaternionNormalize(fpQuaternion_t * result, const fpQuaternion_t * q)
{ {
float mod = sqrtf(quaternionNormSqared(q)); float mod = fast_fsqrtf(quaternionNormSqared(q));
if (mod < 1e-6f) { if (mod < 1e-6f) {
// Length is too small - re-initialize to zero rotation // Length is too small - re-initialize to zero rotation
result->q0 = 1; result->q0 = 1;

2
src/main/common/vector.h
View file

@ -67,7 +67,7 @@ static inline float vectorNormSquared(const fpVector3_t * v)
static inline fpVector3_t * vectorNormalize(fpVector3_t * result, const fpVector3_t * v) static inline fpVector3_t * vectorNormalize(fpVector3_t * result, const fpVector3_t * v)
{ {
float length = sqrtf(vectorNormSquared(v)); float length = fast_fsqrtf(vectorNormSquared(v));
if (length != 0) { if (length != 0) {
result->x = v->x / length; result->x = v->x / length;
result->y = v->y / length; result->y = v->y / length;

2
src/main/drivers/bus_i2c_stm32f30x.c
View file

@ -87,7 +87,7 @@ typedef struct i2cBusState_s {
I2CDevice device; I2CDevice device;
bool initialized; bool initialized;
i2cState_t state; i2cState_t state;
uint32_t timeout; timeUs_t timeout;
/* Active transfer */ /* Active transfer */
bool allowRawAccess; bool allowRawAccess;

2
src/main/drivers/bus_i2c_stm32f40x.c
View file

@ -121,7 +121,7 @@ typedef struct i2cBusState_s {
I2CDevice device; I2CDevice device;
bool initialized; bool initialized;
i2cState_t state; i2cState_t state;
uint32_t timeout; timeUs_t timeout;
/* Active transfer */ /* Active transfer */
bool allowRawAccess; bool allowRawAccess;

4
src/main/drivers/pwm_output.c
View file

@ -105,11 +105,9 @@ static timeUs_t digitalMotorUpdateIntervalUs = 0;
static timeUs_t digitalMotorLastUpdateUs; static timeUs_t digitalMotorLastUpdateUs;
#endif #endif
#ifdef BEEPER_PWM
static pwmOutputPort_t beeperPwmPort; static pwmOutputPort_t beeperPwmPort;
static pwmOutputPort_t *beeperPwm; static pwmOutputPort_t *beeperPwm;
static uint16_t beeperFrequency = 0; static uint16_t beeperFrequency = 0;
#endif
static uint8_t allocatedOutputPortCount = 0; static uint8_t allocatedOutputPortCount = 0;
@ -634,7 +632,6 @@ void pwmWriteServo(uint8_t index, uint16_t value)
} }
} }
#ifdef BEEPER_PWM
void pwmWriteBeeper(bool onoffBeep) void pwmWriteBeeper(bool onoffBeep)
{ {
if (beeperPwm == NULL) if (beeperPwm == NULL)
@ -666,4 +663,3 @@ void beeperPwmInit(ioTag_t tag, uint16_t frequency)
pwmOutConfigTimer(beeperPwm, tch, PWM_TIMER_HZ, 1000000 / beeperFrequency, (1000000 / beeperFrequency) / 2); pwmOutConfigTimer(beeperPwm, tch, PWM_TIMER_HZ, 1000000 / beeperFrequency, (1000000 / beeperFrequency) / 2);
} }
} }
#endif

28
src/main/drivers/sound_beeper.c
View file

@ -23,11 +23,10 @@
#include "drivers/time.h" #include "drivers/time.h"
#include "drivers/io.h" #include "drivers/io.h"
#ifdef BEEPER_PWM
#include "drivers/timer.h" #include "drivers/timer.h"
#include "drivers/pwm_mapping.h" #include "drivers/pwm_mapping.h"
#include "drivers/pwm_output.h" #include "drivers/pwm_output.h"
#endif #include "fc/config.h"
#include "sound_beeper.h" #include "sound_beeper.h"
@ -44,13 +43,18 @@ void systemBeep(bool onoff)
{ {
#if !defined(BEEPER) #if !defined(BEEPER)
UNUSED(onoff); UNUSED(onoff);
#elif defined(BEEPER_PWM)
pwmWriteBeeper(onoff);
beeperState = onoff;
#else #else
IOWrite(beeperIO, beeperInverted ? onoff : !onoff);
beeperState = onoff; if (beeperConfig()->pwmMode) {
pwmWriteBeeper(onoff);
beeperState = onoff;
} else {
IOWrite(beeperIO, beeperInverted ? onoff : !onoff);
beeperState = onoff;
}
#endif #endif
} }
void systemBeepToggle(void) void systemBeepToggle(void)
@ -70,11 +74,11 @@ void beeperInit(const beeperDevConfig_t *config)
if (beeperIO) { if (beeperIO) {
IOInit(beeperIO, OWNER_BEEPER, RESOURCE_OUTPUT, 0); IOInit(beeperIO, OWNER_BEEPER, RESOURCE_OUTPUT, 0);
#if defined(BEEPER_PWM) if (beeperConfig()->pwmMode) {
beeperPwmInit(config->ioTag, BEEPER_PWM_FREQUENCY); beeperPwmInit(config->ioTag, BEEPER_PWM_FREQUENCY);
#else } else {
IOConfigGPIO(beeperIO, config->isOD ? IOCFG_OUT_OD : IOCFG_OUT_PP); IOConfigGPIO(beeperIO, config->isOD ? IOCFG_OUT_OD : IOCFG_OUT_PP);
#endif }
} }
systemBeep(false); systemBeep(false);

17
src/main/fc/cli.c
View file

@ -119,6 +119,9 @@ uint8_t cliMode = 0;
#include "sensors/opflow.h" #include "sensors/opflow.h"
#include "sensors/sensors.h" #include "sensors/sensors.h"
#include "sensors/temperature.h" #include "sensors/temperature.h"
#ifdef USE_ESC_SENSOR
#include "sensors/esc_sensor.h"
#endif
#include "telemetry/frsky_d.h" #include "telemetry/frsky_d.h"
#include "telemetry/telemetry.h" #include "telemetry/telemetry.h"
@ -1460,7 +1463,7 @@ static void cliWaypoints(char *cmdline)
if (!(validArgumentCount == 6 || validArgumentCount == 8)) { if (!(validArgumentCount == 6 || validArgumentCount == 8)) {
cliShowParseError(); cliShowParseError();
} else if (!(action == 0 || action == NAV_WP_ACTION_WAYPOINT || action == NAV_WP_ACTION_RTH || action == NAV_WP_ACTION_JUMP || action == NAV_WP_ACTION_HOLD_TIME || action == NAV_WP_ACTION_LAND || action == NAV_WP_ACTION_SET_POI || action == NAV_WP_ACTION_SET_HEAD) || !(flag == 0 || flag == NAV_WP_FLAG_LAST)) { } else if (!(action == 0 || action == NAV_WP_ACTION_WAYPOINT || action == NAV_WP_ACTION_RTH || action == NAV_WP_ACTION_JUMP || action == NAV_WP_ACTION_HOLD_TIME || action == NAV_WP_ACTION_LAND || action == NAV_WP_ACTION_SET_POI || action == NAV_WP_ACTION_SET_HEAD) || !(flag == 0 || flag == NAV_WP_FLAG_LAST || flag == NAV_WP_FLAG_HOME)) {
cliShowParseError(); cliShowParseError();
} else { } else {
posControl.waypointList[i].action = action; posControl.waypointList[i].action = action;
@ -3193,6 +3196,18 @@ static void cliStatus(char *cmdline)
hardwareSensorStatusNames[getHwGPSStatus()] hardwareSensorStatusNames[getHwGPSStatus()]
); );
#ifdef USE_ESC_SENSOR
uint8_t motorCount = getMotorCount();
if (STATE(ESC_SENSOR_ENABLED) && motorCount > 0) {
cliPrintLinef("ESC Temperature(s): Motor Count = %d", motorCount);
for (uint8_t i = 0; i < motorCount; i++) {
const escSensorData_t *escState = getEscTelemetry(i); //Get ESC telemetry
cliPrintf("ESC %d: %d\260C, ", i, escState->temperature);
}
cliPrintLinefeed();
}
#endif
#ifdef USE_SDCARD #ifdef USE_SDCARD
cliSdInfo(NULL); cliSdInfo(NULL);
#endif #endif

3
src/main/fc/config.c
View file

@ -123,13 +123,14 @@ PG_RESET_TEMPLATE(systemConfig_t, systemConfig,
.name = SETTING_NAME_DEFAULT .name = SETTING_NAME_DEFAULT
); );
PG_REGISTER_WITH_RESET_TEMPLATE(beeperConfig_t, beeperConfig, PG_BEEPER_CONFIG, 1); PG_REGISTER_WITH_RESET_TEMPLATE(beeperConfig_t, beeperConfig, PG_BEEPER_CONFIG, 2);
PG_RESET_TEMPLATE(beeperConfig_t, beeperConfig, PG_RESET_TEMPLATE(beeperConfig_t, beeperConfig,
.beeper_off_flags = 0, .beeper_off_flags = 0,
.preferred_beeper_off_flags = 0, .preferred_beeper_off_flags = 0,
.dshot_beeper_enabled = SETTING_DSHOT_BEEPER_ENABLED_DEFAULT, .dshot_beeper_enabled = SETTING_DSHOT_BEEPER_ENABLED_DEFAULT,
.dshot_beeper_tone = SETTING_DSHOT_BEEPER_TONE_DEFAULT, .dshot_beeper_tone = SETTING_DSHOT_BEEPER_TONE_DEFAULT,
.pwmMode = SETTING_BEEPER_PWM_MODE_DEFAULT,
); );
PG_REGISTER_WITH_RESET_TEMPLATE(adcChannelConfig_t, adcChannelConfig, PG_ADC_CHANNEL_CONFIG, 0); PG_REGISTER_WITH_RESET_TEMPLATE(adcChannelConfig_t, adcChannelConfig, PG_ADC_CHANNEL_CONFIG, 0);

1
src/main/fc/config.h
View file

@ -86,6 +86,7 @@ typedef struct beeperConfig_s {
uint32_t preferred_beeper_off_flags; uint32_t preferred_beeper_off_flags;
bool dshot_beeper_enabled; bool dshot_beeper_enabled;
uint8_t dshot_beeper_tone; uint8_t dshot_beeper_tone;
bool pwmMode;
} beeperConfig_t; } beeperConfig_t;
PG_DECLARE(beeperConfig_t, beeperConfig); PG_DECLARE(beeperConfig_t, beeperConfig);

31
src/main/fc/controlrate_profile.h
View file

@ -21,35 +21,10 @@
#include "config/parameter_group.h" #include "config/parameter_group.h"
#define MAX_CONTROL_RATE_PROFILE_COUNT 3 #include "fc/controlrate_profile_config_struct.h"
#include "fc/settings.h"
typedef struct controlRateConfig_s { #define MAX_CONTROL_RATE_PROFILE_COUNT SETTING_CONSTANT_MAX_CONTROL_RATE_PROFILE_COUNT
struct {
uint8_t rcMid8;
uint8_t rcExpo8;
uint8_t dynPID;
uint16_t pa_breakpoint; // Breakpoint where TPA is activated
uint16_t fixedWingTauMs; // Time constant of airplane TPA PT1-filter
} throttle;
struct {
uint8_t rcExpo8;
uint8_t rcYawExpo8;
uint8_t rates[3];
} stabilized;
struct {
uint8_t rcExpo8;
uint8_t rcYawExpo8;
uint8_t rates[3];
} manual;
struct {
uint8_t fpvCamAngleDegrees; // Camera angle to treat as "forward" base axis in ACRO (Roll and Yaw sticks will command rotation considering this axis)
} misc;
} controlRateConfig_t;
PG_DECLARE_ARRAY(controlRateConfig_t, MAX_CONTROL_RATE_PROFILE_COUNT, controlRateProfiles); PG_DECLARE_ARRAY(controlRateConfig_t, MAX_CONTROL_RATE_PROFILE_COUNT, controlRateProfiles);

52
src/main/fc/controlrate_profile_config_struct.h Normal file
View file

@ -0,0 +1,52 @@
/*
* This file is part of iNav
*
* iNav 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 distributed in the hope that it
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <stdbool.h>
#include <stdint.h>
typedef struct controlRateConfig_s {
struct {
uint8_t rcMid8;
uint8_t rcExpo8;
uint8_t dynPID;
uint16_t pa_breakpoint; // Breakpoint where TPA is activated
uint16_t fixedWingTauMs; // Time constant of airplane TPA PT1-filter
} throttle;
struct {
uint8_t rcExpo8;
uint8_t rcYawExpo8;
uint8_t rates[3];
} stabilized;
struct {
uint8_t rcExpo8;
uint8_t rcYawExpo8;
uint8_t rates[3];
} manual;
struct {
uint8_t fpvCamAngleDegrees; // Camera angle to treat as "forward" base axis in ACRO (Roll and Yaw sticks will command rotation considering this axis)
} misc;
} controlRateConfig_t;

33
src/main/fc/fc_msp.c
View file

@ -774,7 +774,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU16(dst, motorConfig()->maxthrottle); sbufWriteU16(dst, motorConfig()->maxthrottle);
sbufWriteU16(dst, motorConfig()->mincommand); sbufWriteU16(dst, motorConfig()->mincommand);
sbufWriteU16(dst, failsafeConfig()->failsafe_throttle); sbufWriteU16(dst, currentBatteryProfile->failsafe_throttle);
#ifdef USE_GPS #ifdef USE_GPS
sbufWriteU8(dst, gpsConfig()->provider); // gps_type sbufWriteU8(dst, gpsConfig()->provider); // gps_type
@ -815,7 +815,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU16(dst, motorConfig()->maxthrottle); sbufWriteU16(dst, motorConfig()->maxthrottle);
sbufWriteU16(dst, motorConfig()->mincommand); sbufWriteU16(dst, motorConfig()->mincommand);
sbufWriteU16(dst, failsafeConfig()->failsafe_throttle); sbufWriteU16(dst, currentBatteryProfile->failsafe_throttle);
#ifdef USE_GPS #ifdef USE_GPS
sbufWriteU8(dst, gpsConfig()->provider); // gps_type sbufWriteU8(dst, gpsConfig()->provider); // gps_type
@ -1041,7 +1041,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
case MSP_FAILSAFE_CONFIG: case MSP_FAILSAFE_CONFIG:
sbufWriteU8(dst, failsafeConfig()->failsafe_delay); sbufWriteU8(dst, failsafeConfig()->failsafe_delay);
sbufWriteU8(dst, failsafeConfig()->failsafe_off_delay); sbufWriteU8(dst, failsafeConfig()->failsafe_off_delay);
sbufWriteU16(dst, failsafeConfig()->failsafe_throttle); sbufWriteU16(dst, currentBatteryProfile->failsafe_throttle);
sbufWriteU8(dst, 0); // was failsafe_kill_switch sbufWriteU8(dst, 0); // was failsafe_kill_switch
sbufWriteU16(dst, failsafeConfig()->failsafe_throttle_low_delay); sbufWriteU16(dst, failsafeConfig()->failsafe_throttle_low_delay);
sbufWriteU8(dst, failsafeConfig()->failsafe_procedure); sbufWriteU8(dst, failsafeConfig()->failsafe_procedure);
@ -1322,7 +1322,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU16(dst, navConfig()->general.max_manual_climb_rate); sbufWriteU16(dst, navConfig()->general.max_manual_climb_rate);
sbufWriteU8(dst, navConfig()->mc.max_bank_angle); sbufWriteU8(dst, navConfig()->mc.max_bank_angle);
sbufWriteU8(dst, navConfig()->general.flags.use_thr_mid_for_althold); sbufWriteU8(dst, navConfig()->general.flags.use_thr_mid_for_althold);
sbufWriteU16(dst, navConfig()->mc.hover_throttle); sbufWriteU16(dst, currentBatteryProfile->nav.mc.hover_throttle);
break; break;
case MSP_RTH_AND_LAND_CONFIG: case MSP_RTH_AND_LAND_CONFIG:
@ -1342,13 +1342,13 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
break; break;
case MSP_FW_CONFIG: case MSP_FW_CONFIG:
sbufWriteU16(dst, navConfig()->fw.cruise_throttle); sbufWriteU16(dst, currentBatteryProfile->nav.fw.cruise_throttle);
sbufWriteU16(dst, navConfig()->fw.min_throttle); sbufWriteU16(dst, currentBatteryProfile->nav.fw.min_throttle);
sbufWriteU16(dst, navConfig()->fw.max_throttle); sbufWriteU16(dst, currentBatteryProfile->nav.fw.max_throttle);
sbufWriteU8(dst, navConfig()->fw.max_bank_angle); sbufWriteU8(dst, navConfig()->fw.max_bank_angle);
sbufWriteU8(dst, navConfig()->fw.max_climb_angle); sbufWriteU8(dst, navConfig()->fw.max_climb_angle);
sbufWriteU8(dst, navConfig()->fw.max_dive_angle); sbufWriteU8(dst, navConfig()->fw.max_dive_angle);
sbufWriteU8(dst, navConfig()->fw.pitch_to_throttle); sbufWriteU8(dst, currentBatteryProfile->nav.fw.pitch_to_throttle);
sbufWriteU16(dst, navConfig()->fw.loiter_radius); sbufWriteU16(dst, navConfig()->fw.loiter_radius);
break; break;
#endif #endif
@ -1664,7 +1664,6 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
{ {
uint8_t tmp_u8; uint8_t tmp_u8;
uint16_t tmp_u16; uint16_t tmp_u16;
batteryProfile_t *currentBatteryProfileMutable = (batteryProfile_t*)currentBatteryProfile;
const unsigned int dataSize = sbufBytesRemaining(src); const unsigned int dataSize = sbufBytesRemaining(src);
@ -1867,7 +1866,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
motorConfigMutable()->maxthrottle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX); motorConfigMutable()->maxthrottle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX);
motorConfigMutable()->mincommand = constrain(sbufReadU16(src), 0, PWM_RANGE_MAX); motorConfigMutable()->mincommand = constrain(sbufReadU16(src), 0, PWM_RANGE_MAX);
failsafeConfigMutable()->failsafe_throttle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX); currentBatteryProfileMutable->failsafe_throttle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX);
#ifdef USE_GPS #ifdef USE_GPS
gpsConfigMutable()->provider = sbufReadU8(src); // gps_type gpsConfigMutable()->provider = sbufReadU8(src); // gps_type
@ -1915,7 +1914,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
motorConfigMutable()->maxthrottle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX); motorConfigMutable()->maxthrottle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX);
motorConfigMutable()->mincommand = constrain(sbufReadU16(src), 0, PWM_RANGE_MAX); motorConfigMutable()->mincommand = constrain(sbufReadU16(src), 0, PWM_RANGE_MAX);
failsafeConfigMutable()->failsafe_throttle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX); currentBatteryProfileMutable->failsafe_throttle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX);
#ifdef USE_GPS #ifdef USE_GPS
gpsConfigMutable()->provider = sbufReadU8(src); // gps_type gpsConfigMutable()->provider = sbufReadU8(src); // gps_type
@ -2293,7 +2292,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
navConfigMutable()->general.max_manual_climb_rate = sbufReadU16(src); navConfigMutable()->general.max_manual_climb_rate = sbufReadU16(src);
navConfigMutable()->mc.max_bank_angle = sbufReadU8(src); navConfigMutable()->mc.max_bank_angle = sbufReadU8(src);
navConfigMutable()->general.flags.use_thr_mid_for_althold = sbufReadU8(src); navConfigMutable()->general.flags.use_thr_mid_for_althold = sbufReadU8(src);
navConfigMutable()->mc.hover_throttle = sbufReadU16(src); currentBatteryProfileMutable->nav.mc.hover_throttle = sbufReadU16(src);
} else } else
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
break; break;
@ -2319,13 +2318,13 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
case MSP_SET_FW_CONFIG: case MSP_SET_FW_CONFIG:
if (dataSize >= 12) { if (dataSize >= 12) {
navConfigMutable()->fw.cruise_throttle = sbufReadU16(src); currentBatteryProfileMutable->nav.fw.cruise_throttle = sbufReadU16(src);
navConfigMutable()->fw.min_throttle = sbufReadU16(src); currentBatteryProfileMutable->nav.fw.min_throttle = sbufReadU16(src);
navConfigMutable()->fw.max_throttle = sbufReadU16(src); currentBatteryProfileMutable->nav.fw.max_throttle = sbufReadU16(src);
navConfigMutable()->fw.max_bank_angle = sbufReadU8(src); navConfigMutable()->fw.max_bank_angle = sbufReadU8(src);
navConfigMutable()->fw.max_climb_angle = sbufReadU8(src); navConfigMutable()->fw.max_climb_angle = sbufReadU8(src);
navConfigMutable()->fw.max_dive_angle = sbufReadU8(src); navConfigMutable()->fw.max_dive_angle = sbufReadU8(src);
navConfigMutable()->fw.pitch_to_throttle = sbufReadU8(src); currentBatteryProfileMutable->nav.fw.pitch_to_throttle = sbufReadU8(src);
navConfigMutable()->fw.loiter_radius = sbufReadU16(src); navConfigMutable()->fw.loiter_radius = sbufReadU16(src);
} else } else
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
@ -2695,7 +2694,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
if (dataSize >= 20) { if (dataSize >= 20) {
failsafeConfigMutable()->failsafe_delay = sbufReadU8(src); failsafeConfigMutable()->failsafe_delay = sbufReadU8(src);
failsafeConfigMutable()->failsafe_off_delay = sbufReadU8(src); failsafeConfigMutable()->failsafe_off_delay = sbufReadU8(src);
failsafeConfigMutable()->failsafe_throttle = sbufReadU16(src); currentBatteryProfileMutable->failsafe_throttle = sbufReadU16(src);
sbufReadU8(src); // was failsafe_kill_switch sbufReadU8(src); // was failsafe_kill_switch
failsafeConfigMutable()->failsafe_throttle_low_delay = sbufReadU16(src); failsafeConfigMutable()->failsafe_throttle_low_delay = sbufReadU16(src);
failsafeConfigMutable()->failsafe_procedure = sbufReadU8(src); failsafeConfigMutable()->failsafe_procedure = sbufReadU8(src);

18
src/main/fc/rc_adjustments.c
View file

@ -49,6 +49,7 @@
#include "io/beeper.h" #include "io/beeper.h"
#include "io/vtx.h" #include "io/vtx.h"
#include "sensors/battery.h"
#include "sensors/boardalignment.h" #include "sensors/boardalignment.h"
#include "rx/rx.h" #include "rx/rx.h"
@ -500,10 +501,10 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
schedulePidGainsUpdate(); schedulePidGainsUpdate();
break; break;
case ADJUSTMENT_NAV_FW_CRUISE_THR: case ADJUSTMENT_NAV_FW_CRUISE_THR:
applyAdjustmentU16(ADJUSTMENT_NAV_FW_CRUISE_THR, &navConfigMutable()->fw.cruise_throttle, delta, SETTING_NAV_FW_CRUISE_THR_MIN, SETTING_NAV_FW_CRUISE_THR_MAX); applyAdjustmentU16(ADJUSTMENT_NAV_FW_CRUISE_THR, &currentBatteryProfileMutable->nav.fw.cruise_throttle, delta, SETTING_NAV_FW_CRUISE_THR_MIN, SETTING_NAV_FW_CRUISE_THR_MAX);
break; break;
case ADJUSTMENT_NAV_FW_PITCH2THR: case ADJUSTMENT_NAV_FW_PITCH2THR:
applyAdjustmentU8(ADJUSTMENT_NAV_FW_PITCH2THR, &navConfigMutable()->fw.pitch_to_throttle, delta, SETTING_NAV_FW_PITCH2THR_MIN, SETTING_NAV_FW_PITCH2THR_MAX); applyAdjustmentU8(ADJUSTMENT_NAV_FW_PITCH2THR, &currentBatteryProfileMutable->nav.fw.pitch_to_throttle, delta, SETTING_NAV_FW_PITCH2THR_MIN, SETTING_NAV_FW_PITCH2THR_MAX);
break; break;
case ADJUSTMENT_ROLL_BOARD_ALIGNMENT: case ADJUSTMENT_ROLL_BOARD_ALIGNMENT:
updateBoardAlignment(delta, 0); updateBoardAlignment(delta, 0);
@ -578,7 +579,7 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
navigationUsePIDs(); navigationUsePIDs();
break; break;
case ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE: case ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE:
applyAdjustmentU16(ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE, &mixerConfigMutable()->fwMinThrottleDownPitchAngle, delta, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MIN, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MAX); applyAdjustmentU16(ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE, &currentBatteryProfileMutable->fwMinThrottleDownPitchAngle, delta, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MIN, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MAX);
break; break;
#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) #if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP)
case ADJUSTMENT_VTX_POWER_LEVEL: case ADJUSTMENT_VTX_POWER_LEVEL:
@ -722,3 +723,14 @@ bool isAdjustmentFunctionSelected(uint8_t adjustmentFunction) {
} }
return false; return false;
} }
uint8_t getActiveAdjustmentFunctions(uint8_t *adjustmentFunctions) {
uint8_t adjustmentCount = 0;
for (uint8_t i = 0; i < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT; i++) {
if (adjustmentStates[i].config) {
adjustmentCount++;
adjustmentFunctions[i] = adjustmentStates[i].config->adjustmentFunction;
}
}
return adjustmentCount;
}

1
src/main/fc/rc_adjustments.h
View file

@ -144,3 +144,4 @@ void updateAdjustmentStates(bool canUseRxData);
struct controlRateConfig_s; struct controlRateConfig_s;
void processRcAdjustments(struct controlRateConfig_s *controlRateConfig, bool canUseRxData); void processRcAdjustments(struct controlRateConfig_s *controlRateConfig, bool canUseRxData);
bool isAdjustmentFunctionSelected(uint8_t adjustmentFunction); bool isAdjustmentFunctionSelected(uint8_t adjustmentFunction);
uint8_t getActiveAdjustmentFunctions(uint8_t *adjustmentFunctions);

7
src/main/fc/rc_controls.c
View file

@ -260,9 +260,14 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
// Load waypoint list // Load waypoint list
if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_HI) { if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_HI) {
const bool success = loadNonVolatileWaypointList(true); const bool success = loadNonVolatileWaypointList(false);
beeper(success ? BEEPER_ACTION_SUCCESS : BEEPER_ACTION_FAIL); beeper(success ? BEEPER_ACTION_SUCCESS : BEEPER_ACTION_FAIL);
} }
if (rcSticks == THR_LO + YAW_CE + PIT_LO + ROL_HI) {
resetWaypointList();
beeper(BEEPER_ACTION_FAIL); // The above cannot fail, but traditionally, we play FAIL for not-loading
}
#endif #endif
// Multiple configuration profiles // Multiple configuration profiles

6
src/main/fc/settings.h
View file

@ -67,9 +67,9 @@ typedef struct {
} __attribute__((packed)) setting_t; } __attribute__((packed)) setting_t;
static inline setting_type_e SETTING_TYPE(const setting_t *s) { return s->type & SETTING_TYPE_MASK; } static inline setting_type_e SETTING_TYPE(const setting_t *s) { return (setting_type_e)(s->type & SETTING_TYPE_MASK); }
static inline setting_section_e SETTING_SECTION(const setting_t *s) { return s->type & SETTING_SECTION_MASK; } static inline setting_section_e SETTING_SECTION(const setting_t *s) { return (setting_section_e)(s->type & SETTING_SECTION_MASK); }
static inline setting_mode_e SETTING_MODE(const setting_t *s) { return s->type & SETTING_MODE_MASK; } static inline setting_mode_e SETTING_MODE(const setting_t *s) { return (setting_mode_e)(s->type & SETTING_MODE_MASK); }
void settingGetName(const setting_t *val, char *buf); void settingGetName(const setting_t *val, char *buf);
bool settingNameContains(const setting_t *val, char *buf, const char *cmdline); bool settingNameContains(const setting_t *val, char *buf, const char *cmdline);

338
src/main/fc/settings.yaml
View file

@ -64,7 +64,7 @@ tables:
- name: nav_rth_alt_mode - name: nav_rth_alt_mode
values: ["CURRENT", "EXTRA", "FIXED", "MAX", "AT_LEAST", "AT_LEAST_LINEAR_DESCENT"] values: ["CURRENT", "EXTRA", "FIXED", "MAX", "AT_LEAST", "AT_LEAST_LINEAR_DESCENT"]
- name: osd_unit - name: osd_unit
values: ["IMPERIAL", "METRIC", "UK"] values: ["IMPERIAL", "METRIC", "METRIC_MPH", "UK"]
enum: osd_unit_e enum: osd_unit_e
- name: osd_stats_energy_unit - name: osd_stats_energy_unit
values: ["MAH", "WH"] values: ["MAH", "WH"]
@ -179,6 +179,10 @@ tables:
- name: nav_rth_climb_first - name: nav_rth_climb_first
enum: navRTHClimbFirst_e enum: navRTHClimbFirst_e
values: ["OFF", "ON", "ON_FW_SPIRAL"] values: ["OFF", "ON", "ON_FW_SPIRAL"]
- name: djiRssiSource
values: ["RSSI", "CRSF_LQ"]
enum: djiRssiSource_e
constants: constants:
RPYL_PID_MIN: 0 RPYL_PID_MIN: 0
@ -190,6 +194,10 @@ constants:
ROLL_PITCH_RATE_MIN: 4 ROLL_PITCH_RATE_MIN: 4
ROLL_PITCH_RATE_MAX: 180 ROLL_PITCH_RATE_MAX: 180
MAX_CONTROL_RATE_PROFILE_COUNT: 3
MAX_BATTERY_PROFILE_COUNT: 3
groups: groups:
- name: PG_GYRO_CONFIG - name: PG_GYRO_CONFIG
type: gyroConfig_t type: gyroConfig_t
@ -320,6 +328,19 @@ groups:
condition: USE_ALPHA_BETA_GAMMA_FILTER condition: USE_ALPHA_BETA_GAMMA_FILTER
min: 0 min: 0
max: 10 max: 10
- name: setpoint_kalman_enabled
description: "Enable Kalman filter on the gyro data"
default_value: OFF
condition: USE_GYRO_KALMAN
field: kalmanEnabled
type: bool
- name: setpoint_kalman_q
description: "Quality factor of the setpoint Kalman filter. Higher values means less filtering and lower phase delay. On 3-7 inch multirotors can be usually increased to 200-300 or even higher of clean builds"
default_value: 100
field: kalman_q
condition: USE_GYRO_KALMAN
min: 1
max: 16000
- name: PG_ADC_CHANNEL_CONFIG - name: PG_ADC_CHANNEL_CONFIG
type: adcChannelConfig_t type: adcChannelConfig_t
@ -857,31 +878,12 @@ groups:
default_value: "ONESHOT125" default_value: "ONESHOT125"
field: motorPwmProtocol field: motorPwmProtocol
table: motor_pwm_protocol table: motor_pwm_protocol
- name: throttle_scale
description: "Throttle scaling factor. `1` means no throttle scaling. `0.5` means throttle scaled down by 50%"
default_value: 1.0
field: throttleScale
min: 0
max: 1
- name: throttle_idle
description: "The percentage of the throttle range (`max_throttle` - `min_command`) above `min_command` used for minimum / idle throttle."
default_value: 15
field: throttleIdle
min: 0
max: 30
- name: motor_poles - name: motor_poles
field: motorPoleCount field: motorPoleCount
description: "The number of motor poles. Required to compute motor RPM" description: "The number of motor poles. Required to compute motor RPM"
min: 4 min: 4
max: 255 max: 255
default_value: 14 default_value: 14
- name: turtle_mode_power_factor
field: turtleModePowerFactor
default_value: 55
description: "Turtle mode power factor"
condition: "USE_DSHOT"
min: 0
max: 100
- name: PG_FAILSAFE_CONFIG - name: PG_FAILSAFE_CONFIG
type: failsafeConfig_t type: failsafeConfig_t
@ -902,11 +904,6 @@ groups:
default_value: 200 default_value: 200
min: 0 min: 0
max: 200 max: 200
- name: failsafe_throttle
description: "Throttle level used for landing when failsafe is enabled. See [Failsafe documentation](Failsafe.md#failsafe_throttle)."
default_value: 1000
min: PWM_RANGE_MIN
max: PWM_RANGE_MAX
- name: failsafe_throttle_low_delay - name: failsafe_throttle_low_delay
description: "If failsafe activated when throttle is low for this much time - bypass failsafe and disarm, in 10th of seconds. 0 = No timeout" description: "If failsafe activated when throttle is low for this much time - bypass failsafe and disarm, in 10th of seconds. 0 = No timeout"
default_value: 0 default_value: 0
@ -999,7 +996,7 @@ groups:
- name: PG_BATTERY_METERS_CONFIG - name: PG_BATTERY_METERS_CONFIG
type: batteryMetersConfig_t type: batteryMetersConfig_t
headers: ["sensors/battery.h"] headers: ["sensors/battery_config_structs.h"]
members: members:
- name: vbat_meter_type - name: vbat_meter_type
description: "Vbat voltage source. Possible values: `NONE`, `ADC`, `ESC`. `ESC` required ESC telemetry enebled and running" description: "Vbat voltage source. Possible values: `NONE`, `ADC`, `ESC`. `ESC` required ESC telemetry enebled and running"
@ -1013,8 +1010,8 @@ groups:
default_value: :target default_value: :target
field: voltage.scale field: voltage.scale
condition: USE_ADC condition: USE_ADC
min: VBAT_SCALE_MIN min: 0
max: VBAT_SCALE_MAX max: 65535
- name: current_meter_scale - name: current_meter_scale
description: "This sets the output voltage to current scaling for the current sensor in 0.1 mV/A steps. 400 is 40mV/A such as the ACS756 sensor outputs. 183 is the setting for the uberdistro with a 0.25mOhm shunt." description: "This sets the output voltage to current scaling for the current sensor in 0.1 mV/A steps. 400 is 40mV/A such as the ACS756 sensor outputs. 183 is the setting for the uberdistro with a 0.25mOhm shunt."
default_value: :target default_value: :target
@ -1065,7 +1062,7 @@ groups:
- name: PG_BATTERY_PROFILES - name: PG_BATTERY_PROFILES
type: batteryProfile_t type: batteryProfile_t
headers: ["sensors/battery.h"] headers: ["sensors/battery_config_structs.h"]
value_type: BATTERY_CONFIG_VALUE value_type: BATTERY_CONFIG_VALUE
members: members:
- name: bat_cells - name: bat_cells
@ -1127,6 +1124,133 @@ groups:
field: capacity.unit field: capacity.unit
table: bat_capacity_unit table: bat_capacity_unit
type: uint8_t type: uint8_t
- name: controlrate_profile
description: "Control rate profile to switch to when the battery profile is selected, 0 to disable and keep the currently selected control rate profile"
default_value: 0
field: controlRateProfile
min: 0
max: MAX_CONTROL_RATE_PROFILE_COUNT
- name: throttle_scale
description: "Throttle scaling factor. `1` means no throttle scaling. `0.5` means throttle scaled down by 50%"
default_value: 1.0
field: motor.throttleScale
min: 0
max: 1
- name: throttle_idle
description: "The percentage of the throttle range (`max_throttle` - `min_command`) above `min_command` used for minimum / idle throttle."
default_value: 15
field: motor.throttleIdle
min: 0
max: 30
- name: turtle_mode_power_factor
field: motor.turtleModePowerFactor
default_value: 55
description: "Turtle mode power factor"
condition: USE_DSHOT
min: 0
max: 100
- name: failsafe_throttle
description: "Throttle level used for landing when failsafe is enabled. See [Failsafe documentation](Failsafe.md#failsafe_throttle)."
default_value: 1000
min: PWM_RANGE_MIN
max: PWM_RANGE_MAX
- name: fw_min_throttle_down_pitch
description: "Automatic pitch down angle when throttle is at 0 in angle mode. Progressively applied between cruise throttle and zero throttle (decidegrees)"
default_value: 0
field: fwMinThrottleDownPitchAngle
min: 0
max: 450
- 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
field: nav.mc.hover_throttle
min: 1000
max: 2000
- name: nav_fw_cruise_thr
description: "Cruise throttle in GPS assisted modes, this includes RTH. Should be set high enough to avoid stalling. This values gives INAV a base for throttle when flying straight, and it will increase or decrease throttle based on pitch of airplane and the parameters below. In addition it will increase throttle if GPS speed gets below 7m/s ( hardcoded )"
default_value: 1400
field: nav.fw.cruise_throttle
min: 1000
max: 2000
- name: nav_fw_min_thr
description: "Minimum throttle for flying wing in GPS assisted modes"
default_value: 1200
field: nav.fw.min_throttle
min: 1000
max: 2000
- name: nav_fw_max_thr
description: "Maximum throttle for flying wing in GPS assisted modes"
default_value: 1700
field: nav.fw.max_throttle
min: 1000
max: 2000
- name: nav_fw_pitch2thr
description: "Amount of throttle applied related to pitch attitude in GPS assisted modes. Throttle = nav_fw_cruise_throttle - (nav_fw_pitch2thr * pitch_angle). (notice that pitch_angle is in degrees and is negative when climbing and positive when diving, and throttle value is constrained between nav_fw_min_thr and nav_fw_max_thr)"
default_value: 10
field: nav.fw.pitch_to_throttle
min: 0
max: 100
- name: nav_fw_launch_thr
description: "Launch throttle - throttle to be set during launch sequence (pwm units)"
default_value: 1700
field: nav.fw.launch_throttle
min: 1000
max: 2000
- name: nav_fw_launch_idle_thr
description: "Launch idle throttle - throttle to be set before launch sequence is initiated. If set below minimum throttle it will force motor stop or at idle throttle (depending if the MOTOR_STOP is enabled). If set above minimum throttle it will force throttle to this value (if MOTOR_STOP is enabled it will be handled according to throttle stick position)"
default_value: 1000
field: nav.fw.launch_idle_throttle
min: 1000
max: 2000
- name: limit_cont_current
description: "Continous current limit (dA), set to 0 to disable"
condition: USE_POWER_LIMITS
default_value: 0
field: powerLimits.continuousCurrent
max: 4000
- name: limit_burst_current
description: "Burst current limit (dA): the current which is allowed during `limit_burst_current_time` after which `limit_cont_current` will be enforced, set to 0 to disable"
condition: USE_POWER_LIMITS
default_value: 0
field: powerLimits.burstCurrent
max: 4000
- name: limit_burst_current_time
description: "Allowed current burst time (ds) during which `limit_burst_current` is allowed and after which `limit_cont_current` will be enforced"
condition: USE_POWER_LIMITS
default_value: 0
field: powerLimits.burstCurrentTime
max: 3000
- name: limit_burst_current_falldown_time
description: "Time slice at the end of the burst time during which the current limit will be ramped down from `limit_burst_current` back down to `limit_cont_current`"
condition: USE_POWER_LIMITS
default_value: 0
field: powerLimits.burstCurrentFalldownTime
max: 3000
- name: limit_cont_power
description: "Continous power limit (dW), set to 0 to disable"
condition: USE_POWER_LIMITS && USE_ADC
default_value: 0
field: powerLimits.continuousPower
max: 40000
- name: limit_burst_power
description: "Burst power limit (dW): the current which is allowed during `limit_burst_power_time` after which `limit_cont_power` will be enforced, set to 0 to disable"
condition: USE_POWER_LIMITS && USE_ADC
default_value: 0
field: powerLimits.burstPower
max: 40000
- name: limit_burst_power_time
description: "Allowed power burst time (ds) during which `limit_burst_power` is allowed and after which `limit_cont_power` will be enforced"
condition: USE_POWER_LIMITS && USE_ADC
default_value: 0
field: powerLimits.burstPowerTime
max: 3000
- name: limit_burst_power_falldown_time
description: "Time slice at the end of the burst time during which the power limit will be ramped down from `limit_burst_power` back down to `limit_cont_power`"
condition: USE_POWER_LIMITS && USE_ADC
default_value: 0
field: powerLimits.burstPowerFalldownTime
max: 3000
- name: PG_MIXER_CONFIG - name: PG_MIXER_CONFIG
type: mixerConfig_t type: mixerConfig_t
@ -1153,12 +1277,6 @@ groups:
field: appliedMixerPreset field: appliedMixerPreset
min: -1 min: -1
max: INT16_MAX max: INT16_MAX
- name: fw_min_throttle_down_pitch
description: "Automatic pitch down angle when throttle is at 0 in angle mode. Progressively applied between cruise throttle and zero throttle (decidegrees)"
default_value: 0
field: fwMinThrottleDownPitchAngle
min: 0
max: 450
- name: PG_REVERSIBLE_MOTORS_CONFIG - name: PG_REVERSIBLE_MOTORS_CONFIG
type: reversibleMotorsConfig_t type: reversibleMotorsConfig_t
@ -1226,7 +1344,7 @@ groups:
- name: PG_CONTROL_RATE_PROFILES - name: PG_CONTROL_RATE_PROFILES
type: controlRateConfig_t type: controlRateConfig_t
headers: ["fc/controlrate_profile.h"] headers: ["fc/controlrate_profile_config_struct.h"]
value_type: CONTROL_RATE_VALUE value_type: CONTROL_RATE_VALUE
members: members:
- name: thr_mid - name: thr_mid
@ -2063,33 +2181,6 @@ groups:
field: controlDerivativeLpfHz field: controlDerivativeLpfHz
min: 0 min: 0
max: 200 max: 200
- name: setpoint_kalman_enabled
description: "Enable Kalman filter on the PID controller setpoint"
default_value: OFF
condition: USE_GYRO_KALMAN
field: kalmanEnabled
type: bool
- name: setpoint_kalman_q
description: "Quality factor of the setpoint Kalman filter. Higher values means less filtering and lower phase delay. On 3-7 inch multirotors can be usually increased to 200-300 or even higher of clean builds"
default_value: 100
field: kalman_q
condition: USE_GYRO_KALMAN
min: 1
max: 16000
- name: setpoint_kalman_w
description: "Window size for the setpoint Kalman filter. Wider the window, more samples are used to compute variance. In general, wider window results in smoother filter response"
default_value: 4
field: kalman_w
condition: USE_GYRO_KALMAN
min: 1
max: 40
- name: setpoint_kalman_sharpness
description: "Dynamic factor for the setpoint Kalman filter. In general, the higher the value, the more dynamic Kalman filter gets"
default_value: 100
field: kalman_sharpness
condition: USE_GYRO_KALMAN
min: 1
max: 16000
- name: fw_level_pitch_trim - name: fw_level_pitch_trim
description: "Pitch trim for self-leveling flight modes. In degrees. +5 means airplane nose should be raised 5 deg from level" description: "Pitch trim for self-leveling flight modes. In degrees. +5 means airplane nose should be raised 5 deg from level"
default_value: 0 default_value: 0
@ -2129,7 +2220,7 @@ groups:
condition: USE_AUTOTUNE_FIXED_WING condition: USE_AUTOTUNE_FIXED_WING
members: members:
- name: fw_autotune_min_stick - name: fw_autotune_min_stick
description: "Minimum stick input [%] to consider overshoot/undershoot detection" description: "Minimum stick input [%], after applying deadband and expo, to start recording the plane's response to stick input."
default_value: 50 default_value: 50
field: fw_min_stick field: fw_min_stick
min: 0 min: 0
@ -2160,7 +2251,7 @@ groups:
type: uint8_t type: uint8_t
- name: fw_autotune_max_rate_deflection - name: fw_autotune_max_rate_deflection
description: "The target percentage of maximum mixer output used for determining the rates in `AUTO` and `LIMIT`." description: "The target percentage of maximum mixer output used for determining the rates in `AUTO` and `LIMIT`."
default_value: 90 default_value: 80
field: fw_max_rate_deflection field: fw_max_rate_deflection
min: 50 min: 50
max: 100 max: 100
@ -2482,12 +2573,6 @@ groups:
field: mc.max_bank_angle field: mc.max_bank_angle
min: 15 min: 15
max: 45 max: 45
- 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
field: mc.hover_throttle
min: 1000
max: 2000
- name: nav_mc_auto_disarm_delay - name: nav_mc_auto_disarm_delay
description: "Delay before multi-rotor disarms when `nav_disarm_on_landing` is set (m/s)" description: "Delay before multi-rotor disarms when `nav_disarm_on_landing` is set (m/s)"
default_value: 2000 default_value: 2000
@ -2569,24 +2654,6 @@ groups:
default_value: ON default_value: ON
field: mc.slowDownForTurning field: mc.slowDownForTurning
type: bool type: bool
- name: nav_fw_cruise_thr
description: "Cruise throttle in GPS assisted modes, this includes RTH. Should be set high enough to avoid stalling. This values gives INAV a base for throttle when flying straight, and it will increase or decrease throttle based on pitch of airplane and the parameters below. In addition it will increase throttle if GPS speed gets below 7m/s ( hardcoded )"
default_value: 1400
field: fw.cruise_throttle
min: 1000
max: 2000
- name: nav_fw_min_thr
description: "Minimum throttle for flying wing in GPS assisted modes"
default_value: 1200
field: fw.min_throttle
min: 1000
max: 2000
- name: nav_fw_max_thr
description: "Maximum throttle for flying wing in GPS assisted modes"
default_value: 1700
field: fw.max_throttle
min: 1000
max: 2000
- name: nav_fw_bank_angle - name: nav_fw_bank_angle
description: "Max roll angle when rolling / turning in GPS assisted modes, is also restrained by global max_angle_inclination_rll" description: "Max roll angle when rolling / turning in GPS assisted modes, is also restrained by global max_angle_inclination_rll"
default_value: 35 default_value: 35
@ -2605,12 +2672,6 @@ groups:
field: fw.max_dive_angle field: fw.max_dive_angle
min: 5 min: 5
max: 80 max: 80
- name: nav_fw_pitch2thr
description: "Amount of throttle applied related to pitch attitude in GPS assisted modes. Throttle = nav_fw_cruise_throttle - (nav_fw_pitch2thr * pitch_angle). (notice that pitch_angle is in degrees and is negative when climbing and positive when diving, and throttle value is constrained between nav_fw_min_thr and nav_fw_max_thr)"
default_value: 10
field: fw.pitch_to_throttle
min: 0
max: 100
- name: nav_fw_pitch2thr_smoothing - name: nav_fw_pitch2thr_smoothing
description: "How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh." description: "How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh."
default_value: 6 default_value: 6
@ -2674,18 +2735,6 @@ groups:
field: fw.launch_time_thresh field: fw.launch_time_thresh
min: 10 min: 10
max: 1000 max: 1000
- name: nav_fw_launch_thr
description: "Launch throttle - throttle to be set during launch sequence (pwm units)"
default_value: 1700
field: fw.launch_throttle
min: 1000
max: 2000
- name: nav_fw_launch_idle_thr
description: "Launch idle throttle - throttle to be set before launch sequence is initiated. If set below minimum throttle it will force motor stop or at idle throttle (depending if the MOTOR_STOP is enabled). If set above minimum throttle it will force throttle to this value (if MOTOR_STOP is enabled it will be handled according to throttle stick position)"
default_value: 1000
field: fw.launch_idle_throttle
min: 1000
max: 2000
- name: nav_fw_launch_idle_motor_delay - name: nav_fw_launch_idle_motor_delay
description: "Delay between raising throttle and motor starting at idle throttle (ms)" description: "Delay between raising throttle and motor starting at idle throttle (ms)"
default_value: 0 default_value: 0
@ -3634,7 +3683,7 @@ groups:
max: UINT8_MAX max: UINT8_MAX
default_value: 1 default_value: 1
- name: dji_use_name_for_messages - name: dji_use_name_for_messages
description: "Re-purpose the craft name field for messages. Replace craft name with :WTSED for Warnings|Throttle|Speed|Efficiency|Trip distance" description: "Re-purpose the craft name field for messages."
default_value: ON default_value: ON
field: use_name_for_messages field: use_name_for_messages
type: bool type: bool
@ -3644,6 +3693,30 @@ groups:
field: esc_temperature_source field: esc_temperature_source
table: djiOsdTempSource table: djiOsdTempSource
type: uint8_t type: uint8_t
- name: dji_message_speed_source
description: "Sets the speed type displayed by the DJI OSD in craft name: GROUND, 3D, AIR"
default_value: "3D"
field: messageSpeedSource
table: osdSpeedSource
type: uint8_t
- name: dji_rssi_source
description: "Source of the DJI RSSI field: RSSI, CRSF_LQ"
default_value: "RSSI"
field: rssi_source
table: djiRssiSource
type: uint8_t
- name: dji_use_adjustments
description: "Show inflight adjustments in craft name field"
default_value: OFF
type: bool
field: useAdjustments
- name: dji_cn_alternating_duration
description: "Alternating duration of craft name elements, in tenths of a second"
default_value: 30
min: 1
max: 150
field: craftNameAlternatingDuration
type: uint8_t
- name: PG_BEEPER_CONFIG - name: PG_BEEPER_CONFIG
type: beeperConfig_t type: beeperConfig_t
@ -3661,56 +3734,17 @@ groups:
default_value: 1 default_value: 1
field: dshot_beeper_tone field: dshot_beeper_tone
type: uint8_t type: uint8_t
- name: beeper_pwm_mode
field: pwmMode
type: bool
default_value: OFF
description: "Allows disabling PWM mode for beeper on some targets. Switch from ON to OFF if the external beeper sound is weak. Do not switch from OFF to ON without checking if the board supports PWM beeper mode"
- name: PG_POWER_LIMITS_CONFIG - name: PG_POWER_LIMITS_CONFIG
type: powerLimitsConfig_t type: powerLimitsConfig_t
headers: ["flight/power_limits.h"] headers: ["flight/power_limits.h"]
condition: USE_POWER_LIMITS condition: USE_POWER_LIMITS
members: members:
- name: limit_cont_current
description: "Continous current limit (dA), set to 0 to disable"
default_value: 0
field: continuousCurrent
max: 4000
- name: limit_burst_current
description: "Burst current limit (dA): the current which is allowed during `limit_burst_current_time` after which `limit_cont_current` will be enforced, set to 0 to disable"
default_value: 0
field: burstCurrent
max: 4000
- name: limit_burst_current_time
description: "Allowed current burst time (ds) during which `limit_burst_current` is allowed and after which `limit_cont_current` will be enforced"
default_value: 0
field: burstCurrentTime
max: 3000
- name: limit_burst_current_falldown_time
description: "Time slice at the end of the burst time during which the current limit will be ramped down from `limit_burst_current` back down to `limit_cont_current`"
default_value: 0
field: burstCurrentFalldownTime
max: 3000
- name: limit_cont_power
condition: USE_ADC
description: "Continous power limit (dW), set to 0 to disable"
default_value: 0
field: continuousPower
max: 40000
- name: limit_burst_power
condition: USE_ADC
description: "Burst power limit (dW): the current which is allowed during `limit_burst_power_time` after which `limit_cont_power` will be enforced, set to 0 to disable"
default_value: 0
field: burstPower
max: 40000
- name: limit_burst_power_time
condition: USE_ADC
description: "Allowed power burst time (ds) during which `limit_burst_power` is allowed and after which `limit_cont_power` will be enforced"
default_value: 0
field: burstPowerTime
max: 3000
- name: limit_burst_power_falldown_time
condition: USE_ADC
description: "Time slice at the end of the burst time during which the power limit will be ramped down from `limit_burst_power` back down to `limit_cont_power`"
default_value: 0
field: burstPowerFalldownTime
max: 3000
- name: limit_pi_p - name: limit_pi_p
description: "Throttle attenuation PI control P term" description: "Throttle attenuation PI control P term"
default_value: 100 default_value: 100

10
src/main/flight/failsafe.c
View file

@ -51,6 +51,7 @@
#include "rx/rx.h" #include "rx/rx.h"
#include "sensors/battery.h"
#include "sensors/sensors.h" #include "sensors/sensors.h"
/* /*
@ -66,13 +67,12 @@
static failsafeState_t failsafeState; static failsafeState_t failsafeState;
PG_REGISTER_WITH_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 1); PG_REGISTER_WITH_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 2);
PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig,
.failsafe_delay = SETTING_FAILSAFE_DELAY_DEFAULT, // 0.5 sec .failsafe_delay = SETTING_FAILSAFE_DELAY_DEFAULT, // 0.5 sec
.failsafe_recovery_delay = SETTING_FAILSAFE_RECOVERY_DELAY_DEFAULT, // 0.5 seconds (plus 200ms explicit delay) .failsafe_recovery_delay = SETTING_FAILSAFE_RECOVERY_DELAY_DEFAULT, // 0.5 seconds (plus 200ms explicit delay)
.failsafe_off_delay = SETTING_FAILSAFE_OFF_DELAY_DEFAULT, // 20sec .failsafe_off_delay = SETTING_FAILSAFE_OFF_DELAY_DEFAULT, // 20sec
.failsafe_throttle = SETTING_FAILSAFE_THROTTLE_DEFAULT, // default throttle off.
.failsafe_throttle_low_delay = SETTING_FAILSAFE_THROTTLE_LOW_DELAY_DEFAULT, // default throttle low delay for "just disarm" on failsafe condition .failsafe_throttle_low_delay = SETTING_FAILSAFE_THROTTLE_LOW_DELAY_DEFAULT, // default throttle low delay for "just disarm" on failsafe condition
.failsafe_procedure = SETTING_FAILSAFE_PROCEDURE_DEFAULT, // default full failsafe procedure .failsafe_procedure = SETTING_FAILSAFE_PROCEDURE_DEFAULT, // default full failsafe procedure
.failsafe_fw_roll_angle = SETTING_FAILSAFE_FW_ROLL_ANGLE_DEFAULT, // 20 deg left .failsafe_fw_roll_angle = SETTING_FAILSAFE_FW_ROLL_ANGLE_DEFAULT, // 20 deg left
@ -218,7 +218,7 @@ bool failsafeRequiresMotorStop(void)
{ {
return failsafeState.active && return failsafeState.active &&
failsafeState.activeProcedure == FAILSAFE_PROCEDURE_AUTO_LANDING && failsafeState.activeProcedure == FAILSAFE_PROCEDURE_AUTO_LANDING &&
failsafeConfig()->failsafe_throttle < getThrottleIdleValue(); currentBatteryProfile->failsafe_throttle < getThrottleIdleValue();
} }
void failsafeStartMonitoring(void) void failsafeStartMonitoring(void)
@ -264,13 +264,13 @@ void failsafeApplyControlInput(void)
autoRcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]); autoRcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]);
autoRcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]); autoRcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]);
autoRcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]); autoRcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]);
autoRcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle; autoRcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
} }
else { else {
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
autoRcCommand[i] = 0; autoRcCommand[i] = 0;
} }
autoRcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle; autoRcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
} }
// Apply channel values // Apply channel values

1
src/main/flight/failsafe.h
View file

@ -30,7 +30,6 @@
typedef struct failsafeConfig_s { typedef struct failsafeConfig_s {
uint16_t failsafe_throttle; // Throttle level used for landing - specify value between 1000..2000 (pwm pulse width for slightly below hover). center throttle = 1500.
uint16_t failsafe_throttle_low_delay; // Time throttle stick must have been below 'min_check' to "JustDisarm" instead of "full failsafe procedure" (TENTH_SECOND) uint16_t failsafe_throttle_low_delay; // Time throttle stick must have been below 'min_check' to "JustDisarm" instead of "full failsafe procedure" (TENTH_SECOND)
uint8_t failsafe_delay; // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example (10) uint8_t failsafe_delay; // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example (10)
uint8_t failsafe_recovery_delay; // Time from RC link recovery to failsafe abort. 1 step = 0.1sec - 1sec in example (10) uint8_t failsafe_recovery_delay; // Time from RC link recovery to failsafe abort. 1 step = 0.1sec - 1sec in example (10)

10
src/main/flight/imu.c
View file

@ -241,7 +241,7 @@ static float imuGetPGainScaleFactor(void)
static void imuResetOrientationQuaternion(const fpVector3_t * accBF) static void imuResetOrientationQuaternion(const fpVector3_t * accBF)
{ {
const float accNorm = sqrtf(vectorNormSquared(accBF)); const float accNorm = fast_fsqrtf(vectorNormSquared(accBF));
orientation.q0 = accBF->z + accNorm; orientation.q0 = accBF->z + accNorm;
orientation.q1 = accBF->y; orientation.q1 = accBF->y;
@ -436,12 +436,12 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
// Proper quaternion from axis/angle involves computing sin/cos, but the formula becomes numerically unstable as Theta approaches zero. // Proper quaternion from axis/angle involves computing sin/cos, but the formula becomes numerically unstable as Theta approaches zero.
// For near-zero cases we use the first 3 terms of the Taylor series expansion for sin/cos. We check if fourth term is less than machine precision - // For near-zero cases we use the first 3 terms of the Taylor series expansion for sin/cos. We check if fourth term is less than machine precision -
// then we can safely use the "low angle" approximated version without loss of accuracy. // then we can safely use the "low angle" approximated version without loss of accuracy.
if (thetaMagnitudeSq < sqrtf(24.0f * 1e-6f)) { if (thetaMagnitudeSq < fast_fsqrtf(24.0f * 1e-6f)) {
quaternionScale(&deltaQ, &deltaQ, 1.0f - thetaMagnitudeSq / 6.0f); quaternionScale(&deltaQ, &deltaQ, 1.0f - thetaMagnitudeSq / 6.0f);
deltaQ.q0 = 1.0f - thetaMagnitudeSq / 2.0f; deltaQ.q0 = 1.0f - thetaMagnitudeSq / 2.0f;
} }
else { else {
const float thetaMagnitude = sqrtf(thetaMagnitudeSq); const float thetaMagnitude = fast_fsqrtf(thetaMagnitudeSq);
quaternionScale(&deltaQ, &deltaQ, sin_approx(thetaMagnitude) / thetaMagnitude); quaternionScale(&deltaQ, &deltaQ, sin_approx(thetaMagnitude) / thetaMagnitude);
deltaQ.q0 = cos_approx(thetaMagnitude); deltaQ.q0 = cos_approx(thetaMagnitude);
} }
@ -483,7 +483,7 @@ static float imuCalculateAccelerometerWeight(const float dT)
accMagnitudeSq += acc.accADCf[axis] * acc.accADCf[axis]; accMagnitudeSq += acc.accADCf[axis] * acc.accADCf[axis];
} }
const float accWeight_Nearness = bellCurve(sqrtf(accMagnitudeSq) - 1.0f, MAX_ACC_NEARNESS); const float accWeight_Nearness = bellCurve(fast_fsqrtf(accMagnitudeSq) - 1.0f, MAX_ACC_NEARNESS);
// Experiment: if rotation rate on a FIXED_WING_LEGACY is higher than a threshold - centrifugal force messes up too much and we // Experiment: if rotation rate on a FIXED_WING_LEGACY is higher than a threshold - centrifugal force messes up too much and we
// should not use measured accel for AHRS comp // should not use measured accel for AHRS comp
@ -504,7 +504,7 @@ static float imuCalculateAccelerometerWeight(const float dT)
float accWeight_RateIgnore = 1.0f; float accWeight_RateIgnore = 1.0f;
if (ARMING_FLAG(ARMED) && STATE(FIXED_WING_LEGACY) && imuConfig()->acc_ignore_rate) { if (ARMING_FLAG(ARMED) && STATE(FIXED_WING_LEGACY) && imuConfig()->acc_ignore_rate) {
const float rotRateMagnitude = sqrtf(sq(imuMeasuredRotationBF.y) + sq(imuMeasuredRotationBF.z)); const float rotRateMagnitude = fast_fsqrtf(sq(imuMeasuredRotationBF.y) + sq(imuMeasuredRotationBF.z));
const float rotRateMagnitudeFiltered = pt1FilterApply4(&rotRateFilter, rotRateMagnitude, IMU_CENTRIFUGAL_LPF, dT); const float rotRateMagnitudeFiltered = pt1FilterApply4(&rotRateFilter, rotRateMagnitude, IMU_CENTRIFUGAL_LPF, dT);
if (imuConfig()->acc_ignore_slope) { if (imuConfig()->acc_ignore_slope) {

36
src/main/flight/kalman.c
View file

@ -30,45 +30,35 @@ FILE_COMPILE_FOR_SPEED
kalman_t kalmanFilterStateRate[XYZ_AXIS_COUNT]; kalman_t kalmanFilterStateRate[XYZ_AXIS_COUNT];
static void gyroKalmanInitAxis(kalman_t *filter, uint16_t q, uint16_t w, uint16_t sharpness) static void gyroKalmanInitAxis(kalman_t *filter, uint16_t q)
{ {
memset(filter, 0, sizeof(kalman_t)); memset(filter, 0, sizeof(kalman_t));
filter->q = q * 0.03f; //add multiplier to make tuning easier filter->q = q * 0.03f; //add multiplier to make tuning easier
filter->r = 88.0f; //seeding R at 88.0f filter->r = 88.0f; //seeding R at 88.0f
filter->p = 30.0f; //seeding P at 30.0f filter->p = 30.0f; //seeding P at 30.0f
filter->e = 1.0f; filter->e = 1.0f;
filter->s = sharpness / 10.0f; filter->w = MAX_KALMAN_WINDOW_SIZE;
filter->w = w * 8;
filter->inverseN = 1.0f / (float)(filter->w); filter->inverseN = 1.0f / (float)(filter->w);
} }
void gyroKalmanInitialize(uint16_t q, uint16_t w, uint16_t sharpness) void gyroKalmanInitialize(uint16_t q)
{ {
gyroKalmanInitAxis(&kalmanFilterStateRate[X], q, w, sharpness); gyroKalmanInitAxis(&kalmanFilterStateRate[X], q);
gyroKalmanInitAxis(&kalmanFilterStateRate[Y], q, w, sharpness); gyroKalmanInitAxis(&kalmanFilterStateRate[Y], q);
gyroKalmanInitAxis(&kalmanFilterStateRate[Z], q, w, sharpness); gyroKalmanInitAxis(&kalmanFilterStateRate[Z], q);
} }
float kalman_process(kalman_t *kalmanState, float input, float target) float kalman_process(kalman_t *kalmanState, float input)
{ {
float targetAbs = fabsf(target);
//project the state ahead using acceleration //project the state ahead using acceleration
kalmanState->x += (kalmanState->x - kalmanState->lastX); kalmanState->x += (kalmanState->x - kalmanState->lastX);
//figure out how much to boost or reduce our error in the estimate based on setpoint target.
//this should be close to 0 as we approach the sepoint and really high the futher away we are from the setpoint.
//update last state //update last state
kalmanState->lastX = kalmanState->x; kalmanState->lastX = kalmanState->x;
if (kalmanState->lastX != 0.0f) if (kalmanState->lastX != 0.0f)
{ {
// calculate the error and add multiply sharpness boost kalmanState->e = fabsf(1.0f - (kalmanState->setpoint / kalmanState->lastX));
float errorMultiplier = fabsf(target - kalmanState->x) * kalmanState->s;
// give a boost to the setpoint, used to caluclate the kalman q, based on the error and setpoint/gyrodata
errorMultiplier = constrainf(errorMultiplier * fabsf(1.0f - (target / kalmanState->lastX)) + 1.0f, 1.0f, 50.0f);
kalmanState->e = fabsf(1.0f - (((targetAbs + 1.0f) * errorMultiplier) / fabsf(kalmanState->lastX)));
} }
//prediction update //prediction update
@ -92,7 +82,7 @@ static void updateAxisVariance(kalman_t *kalmanState, float rate)
kalmanState->varianceWindow[kalmanState->windex] = varianceElement; kalmanState->varianceWindow[kalmanState->windex] = varianceElement;
kalmanState->windex++; kalmanState->windex++;
if (kalmanState->windex >= kalmanState->w) { if (kalmanState->windex > kalmanState->w) {
kalmanState->windex = 0; kalmanState->windex = 0;
} }
@ -108,13 +98,17 @@ static void updateAxisVariance(kalman_t *kalmanState, float rate)
kalmanState->r = squirt * VARIANCE_SCALE; kalmanState->r = squirt * VARIANCE_SCALE;
} }
float gyroKalmanUpdate(uint8_t axis, float input, float setpoint) float NOINLINE gyroKalmanUpdate(uint8_t axis, float input)
{ {
updateAxisVariance(&kalmanFilterStateRate[axis], input); updateAxisVariance(&kalmanFilterStateRate[axis], input);
DEBUG_SET(DEBUG_KALMAN_GAIN, axis, kalmanFilterStateRate[axis].k * 1000.0f); //Kalman gain DEBUG_SET(DEBUG_KALMAN_GAIN, axis, kalmanFilterStateRate[axis].k * 1000.0f); //Kalman gain
return kalman_process(&kalmanFilterStateRate[axis], input, setpoint); return kalman_process(&kalmanFilterStateRate[axis], input);
}
void gyroKalmanUpdateSetpoint(uint8_t axis, float setpoint) {
kalmanFilterStateRate[axis].setpoint = setpoint;
} }
#endif #endif

14
src/main/flight/kalman.h
View file

@ -23,7 +23,7 @@
#include "sensors/gyro.h" #include "sensors/gyro.h"
#include "common/filter.h" #include "common/filter.h"
#define MAX_KALMAN_WINDOW_SIZE 512 #define MAX_KALMAN_WINDOW_SIZE 64
#define VARIANCE_SCALE 0.67f #define VARIANCE_SCALE 0.67f
@ -36,12 +36,13 @@ typedef struct kalman
float x; //state float x; //state
float lastX; //previous state float lastX; //previous state
float e; float e;
float s;
float setpoint;
float axisVar; float axisVar;
uint16_t windex; uint16_t windex;
float axisWindow[MAX_KALMAN_WINDOW_SIZE]; float axisWindow[MAX_KALMAN_WINDOW_SIZE + 1];
float varianceWindow[MAX_KALMAN_WINDOW_SIZE]; float varianceWindow[MAX_KALMAN_WINDOW_SIZE + 1];
float axisSumMean; float axisSumMean;
float axisMean; float axisMean;
float axisSumVar; float axisSumVar;
@ -49,5 +50,6 @@ typedef struct kalman
uint16_t w; uint16_t w;
} kalman_t; } kalman_t;
void gyroKalmanInitialize(uint16_t q, uint16_t w, uint16_t sharpness); void gyroKalmanInitialize(uint16_t q);
float gyroKalmanUpdate(uint8_t axis, float input, float setpoint); float gyroKalmanUpdate(uint8_t axis, float input);
void gyroKalmanUpdateSetpoint(uint8_t axis, float setpoint);

26
src/main/flight/mixer.c
View file

@ -83,14 +83,13 @@ PG_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig,
.neutral = SETTING_3D_NEUTRAL_DEFAULT .neutral = SETTING_3D_NEUTRAL_DEFAULT
); );
PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 3); PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 4);
PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
.motorDirectionInverted = SETTING_MOTOR_DIRECTION_INVERTED_DEFAULT, .motorDirectionInverted = SETTING_MOTOR_DIRECTION_INVERTED_DEFAULT,
.platformType = SETTING_PLATFORM_TYPE_DEFAULT, .platformType = SETTING_PLATFORM_TYPE_DEFAULT,
.hasFlaps = SETTING_HAS_FLAPS_DEFAULT, .hasFlaps = SETTING_HAS_FLAPS_DEFAULT,
.appliedMixerPreset = SETTING_MODEL_PREVIEW_TYPE_DEFAULT, //This flag is not available in CLI and used by Configurator only .appliedMixerPreset = SETTING_MODEL_PREVIEW_TYPE_DEFAULT, //This flag is not available in CLI and used by Configurator only
.fwMinThrottleDownPitchAngle = SETTING_FW_MIN_THROTTLE_DOWN_PITCH_DEFAULT
); );
#ifdef BRUSHED_MOTORS #ifdef BRUSHED_MOTORS
@ -103,7 +102,7 @@ PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
#define DEFAULT_MAX_THROTTLE 1850 #define DEFAULT_MAX_THROTTLE 1850
PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 7); PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 8);
PG_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
.motorPwmProtocol = SETTING_MOTOR_PWM_PROTOCOL_DEFAULT, .motorPwmProtocol = SETTING_MOTOR_PWM_PROTOCOL_DEFAULT,
@ -112,12 +111,7 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
.mincommand = SETTING_MIN_COMMAND_DEFAULT, .mincommand = SETTING_MIN_COMMAND_DEFAULT,
.motorAccelTimeMs = SETTING_MOTOR_ACCEL_TIME_DEFAULT, .motorAccelTimeMs = SETTING_MOTOR_ACCEL_TIME_DEFAULT,
.motorDecelTimeMs = SETTING_MOTOR_DECEL_TIME_DEFAULT, .motorDecelTimeMs = SETTING_MOTOR_DECEL_TIME_DEFAULT,
.throttleIdle = SETTING_THROTTLE_IDLE_DEFAULT,
.throttleScale = SETTING_THROTTLE_SCALE_DEFAULT,
.motorPoleCount = SETTING_MOTOR_POLES_DEFAULT, // Most brushless motors that we use are 14 poles .motorPoleCount = SETTING_MOTOR_POLES_DEFAULT, // Most brushless motors that we use are 14 poles
#ifdef USE_DSHOT
.turtleModePowerFactor = SETTING_TURTLE_MODE_POWER_FACTOR_DEFAULT,
#endif
); );
PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTOR_MIXER, 0); PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTOR_MIXER, 0);
@ -130,7 +124,7 @@ static EXTENDED_FASTRAM motorRateLimitingApplyFnPtr motorRateLimitingApplyFn;
int getThrottleIdleValue(void) int getThrottleIdleValue(void)
{ {
if (!throttleIdleValue) { if (!throttleIdleValue) {
throttleIdleValue = motorConfig()->mincommand + (((motorConfig()->maxthrottle - motorConfig()->mincommand) / 100.0f) * motorConfig()->throttleIdle); throttleIdleValue = motorConfig()->mincommand + (((motorConfig()->maxthrottle - motorConfig()->mincommand) / 100.0f) * currentBatteryProfile->motor.throttleIdle);
} }
return throttleIdleValue; return throttleIdleValue;
@ -330,7 +324,7 @@ static uint16_t handleOutputScaling(
static void applyTurtleModeToMotors(void) { static void applyTurtleModeToMotors(void) {
if (ARMING_FLAG(ARMED)) { if (ARMING_FLAG(ARMED)) {
const float flipPowerFactor = ((float)motorConfig()->turtleModePowerFactor)/100.0f; const float flipPowerFactor = ((float)currentBatteryProfile->motor.turtleModePowerFactor)/100.0f;
const float stickDeflectionPitchAbs = ABS(((float) rcCommand[PITCH]) / 500.0f); const float stickDeflectionPitchAbs = ABS(((float) rcCommand[PITCH]) / 500.0f);
const float stickDeflectionRollAbs = ABS(((float) rcCommand[ROLL]) / 500.0f); const float stickDeflectionRollAbs = ABS(((float) rcCommand[ROLL]) / 500.0f);
const float stickDeflectionYawAbs = ABS(((float) rcCommand[YAW]) / 500.0f); const float stickDeflectionYawAbs = ABS(((float) rcCommand[YAW]) / 500.0f);
@ -347,8 +341,8 @@ static void applyTurtleModeToMotors(void) {
float signRoll = rcCommand[ROLL] < 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) * (mixerConfig()->motorDirectionInverted ? 1 : -1));
float stickDeflectionLength = sqrtf(sq(stickDeflectionPitchAbs) + sq(stickDeflectionRollAbs)); float stickDeflectionLength = fast_fsqrtf(sq(stickDeflectionPitchAbs) + sq(stickDeflectionRollAbs));
float stickDeflectionExpoLength = sqrtf(sq(stickDeflectionPitchExpo) + sq(stickDeflectionRollExpo)); float stickDeflectionExpoLength = fast_fsqrtf(sq(stickDeflectionPitchExpo) + sq(stickDeflectionRollExpo));
if (stickDeflectionYawAbs > MAX(stickDeflectionPitchAbs, stickDeflectionRollAbs)) { if (stickDeflectionYawAbs > MAX(stickDeflectionPitchAbs, stickDeflectionRollAbs)) {
// If yaw is the dominant, disable pitch and roll // If yaw is the dominant, disable pitch and roll
@ -362,8 +356,8 @@ static void applyTurtleModeToMotors(void) {
} }
const float cosPhi = (stickDeflectionLength > 0) ? (stickDeflectionPitchAbs + stickDeflectionRollAbs) / const float cosPhi = (stickDeflectionLength > 0) ? (stickDeflectionPitchAbs + stickDeflectionRollAbs) /
(sqrtf(2.0f) * stickDeflectionLength) : 0; (fast_fsqrtf(2.0f) * stickDeflectionLength) : 0;
const float cosThreshold = sqrtf(3.0f) / 2.0f; // cos(PI/6.0f) const float cosThreshold = fast_fsqrtf(3.0f) / 2.0f; // cos(PI/6.0f)
if (cosPhi < cosThreshold) { if (cosPhi < cosThreshold) {
// Enforce either roll or pitch exclusively, if not on diagonal // Enforce either roll or pitch exclusively, if not on diagonal
@ -602,9 +596,9 @@ void FAST_CODE mixTable(const float dT)
// Throttle scaling to limit max throttle when battery is full // Throttle scaling to limit max throttle when battery is full
#ifdef USE_PROGRAMMING_FRAMEWORK #ifdef USE_PROGRAMMING_FRAMEWORK
mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * getThrottleScale(motorConfig()->throttleScale)) + throttleRangeMin; mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * getThrottleScale(currentBatteryProfile->motor.throttleScale)) + throttleRangeMin;
#else #else
mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * motorConfig()->throttleScale) + throttleRangeMin; mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * currentBatteryProfile->motor.throttleScale) + throttleRangeMin;
#endif #endif
// Throttle compensation based on battery voltage // Throttle compensation based on battery voltage
if (feature(FEATURE_THR_VBAT_COMP) && isAmperageConfigured() && feature(FEATURE_VBAT)) { if (feature(FEATURE_THR_VBAT_COMP) && isAmperageConfigured() && feature(FEATURE_VBAT)) {

4
src/main/flight/mixer.h
View file

@ -62,7 +62,6 @@ typedef struct mixerConfig_s {
uint8_t platformType; uint8_t platformType;
bool hasFlaps; bool hasFlaps;
int16_t appliedMixerPreset; int16_t appliedMixerPreset;
uint16_t fwMinThrottleDownPitchAngle;
} mixerConfig_t; } mixerConfig_t;
PG_DECLARE(mixerConfig_t, mixerConfig); PG_DECLARE(mixerConfig_t, mixerConfig);
@ -84,10 +83,7 @@ typedef struct motorConfig_s {
uint16_t motorAccelTimeMs; // Time limit for motor to accelerate from 0 to 100% throttle [ms] uint16_t motorAccelTimeMs; // Time limit for motor to accelerate from 0 to 100% throttle [ms]
uint16_t motorDecelTimeMs; // Time limit for motor to decelerate from 0 to 100% throttle [ms] uint16_t motorDecelTimeMs; // Time limit for motor to decelerate from 0 to 100% throttle [ms]
uint16_t digitalIdleOffsetValue; uint16_t digitalIdleOffsetValue;
float throttleIdle; // Throttle IDLE value based on min_command, max_throttle, in percent
float throttleScale; // Scaling factor for throttle.
uint8_t motorPoleCount; // Magnetic poles in the motors for calculating actual RPM from eRPM provided by ESC telemetry uint8_t motorPoleCount; // Magnetic poles in the motors for calculating actual RPM from eRPM provided by ESC telemetry
uint8_t turtleModePowerFactor; // Power factor from 0 to 100% of flip over after crash
} motorConfig_t; } motorConfig_t;
PG_DECLARE(motorConfig_t, motorConfig); PG_DECLARE(motorConfig_t, motorConfig);

111
src/main/flight/pid.c
View file

@ -56,6 +56,7 @@ FILE_COMPILE_FOR_SPEED
#include "rx/rx.h" #include "rx/rx.h"
#include "sensors/battery.h"
#include "sensors/sensors.h" #include "sensors/sensors.h"
#include "sensors/gyro.h" #include "sensors/gyro.h"
#include "sensors/acceleration.h" #include "sensors/acceleration.h"
@ -109,7 +110,7 @@ typedef struct {
bool itermLimitActive; bool itermLimitActive;
bool itermFreezeActive; bool itermFreezeActive;
biquadFilter_t rateTargetFilter; pt2Filter_t rateTargetFilter;
smithPredictor_t smithPredictor; smithPredictor_t smithPredictor;
} pidState_t; } pidState_t;
@ -169,7 +170,7 @@ static EXTENDED_FASTRAM bool levelingEnabled = false;
static EXTENDED_FASTRAM float fixedWingLevelTrim; static EXTENDED_FASTRAM float fixedWingLevelTrim;
static EXTENDED_FASTRAM pidController_t fixedWingLevelTrimController; static EXTENDED_FASTRAM pidController_t fixedWingLevelTrimController;
PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 2); PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 4);
PG_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.bank_mc = { .bank_mc = {
@ -299,13 +300,6 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.navFwPosHdgPidsumLimit = SETTING_NAV_FW_POS_HDG_PIDSUM_LIMIT_DEFAULT, .navFwPosHdgPidsumLimit = SETTING_NAV_FW_POS_HDG_PIDSUM_LIMIT_DEFAULT,
.controlDerivativeLpfHz = SETTING_MC_CD_LPF_HZ_DEFAULT, .controlDerivativeLpfHz = SETTING_MC_CD_LPF_HZ_DEFAULT,
#ifdef USE_GYRO_KALMAN
.kalman_q = SETTING_SETPOINT_KALMAN_Q_DEFAULT,
.kalman_w = SETTING_SETPOINT_KALMAN_W_DEFAULT,
.kalman_sharpness = SETTING_SETPOINT_KALMAN_SHARPNESS_DEFAULT,
.kalmanEnabled = SETTING_SETPOINT_KALMAN_ENABLED_DEFAULT,
#endif
.fixedWingLevelTrim = SETTING_FW_LEVEL_PITCH_TRIM_DEFAULT, .fixedWingLevelTrim = SETTING_FW_LEVEL_PITCH_TRIM_DEFAULT,
.fixedWingLevelTrimGain = SETTING_FW_LEVEL_PITCH_GAIN_DEFAULT, .fixedWingLevelTrimGain = SETTING_FW_LEVEL_PITCH_GAIN_DEFAULT,
@ -362,30 +356,30 @@ bool pidInitFilters(void)
if (pidProfile()->controlDerivativeLpfHz) { if (pidProfile()->controlDerivativeLpfHz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInitLPF(&pidState[axis].rateTargetFilter, pidProfile()->controlDerivativeLpfHz, getLooptime()); pt2FilterInit(&pidState[axis].rateTargetFilter, pt2FilterGain(pidProfile()->controlDerivativeLpfHz, refreshRate * 1e-6f));
} }
} }
#ifdef USE_SMITH_PREDICTOR #ifdef USE_SMITH_PREDICTOR
smithPredictorInit( smithPredictorInit(
&pidState[FD_ROLL].smithPredictor, &pidState[FD_ROLL].smithPredictor,
pidProfile()->smithPredictorDelay, pidProfile()->smithPredictorDelay,
pidProfile()->smithPredictorStrength, pidProfile()->smithPredictorStrength,
pidProfile()->smithPredictorFilterHz, pidProfile()->smithPredictorFilterHz,
getLooptime() getLooptime()
); );
smithPredictorInit( smithPredictorInit(
&pidState[FD_PITCH].smithPredictor, &pidState[FD_PITCH].smithPredictor,
pidProfile()->smithPredictorDelay, pidProfile()->smithPredictorDelay,
pidProfile()->smithPredictorStrength, pidProfile()->smithPredictorStrength,
pidProfile()->smithPredictorFilterHz, pidProfile()->smithPredictorFilterHz,
getLooptime() getLooptime()
); );
smithPredictorInit( smithPredictorInit(
&pidState[FD_YAW].smithPredictor, &pidState[FD_YAW].smithPredictor,
pidProfile()->smithPredictorDelay, pidProfile()->smithPredictorDelay,
pidProfile()->smithPredictorStrength, pidProfile()->smithPredictorStrength,
pidProfile()->smithPredictorFilterHz, pidProfile()->smithPredictorFilterHz,
getLooptime() getLooptime()
); );
#endif #endif
@ -412,7 +406,7 @@ void pidResetErrorAccumulators(void)
} }
} }
void pidReduceErrorAccumulators(int8_t delta, uint8_t axis) void pidReduceErrorAccumulators(int8_t delta, uint8_t axis)
{ {
pidState[axis].errorGyroIf -= delta; pidState[axis].errorGyroIf -= delta;
pidState[axis].errorGyroIfLimit -= delta; pidState[axis].errorGyroIfLimit -= delta;
@ -420,10 +414,10 @@ void pidReduceErrorAccumulators(int8_t delta, uint8_t axis)
float getTotalRateTarget(void) float getTotalRateTarget(void)
{ {
return sqrtf(sq(pidState[FD_ROLL].rateTarget) + sq(pidState[FD_PITCH].rateTarget) + sq(pidState[FD_YAW].rateTarget)); return fast_fsqrtf(sq(pidState[FD_ROLL].rateTarget) + sq(pidState[FD_PITCH].rateTarget) + sq(pidState[FD_YAW].rateTarget));
} }
float getAxisIterm(uint8_t axis) float getAxisIterm(uint8_t axis)
{ {
return pidState[axis].errorGyroIf; return pidState[axis].errorGyroIf;
} }
@ -528,7 +522,7 @@ void updatePIDCoefficients()
#ifdef USE_ANTIGRAVITY #ifdef USE_ANTIGRAVITY
if (usedPidControllerType == PID_TYPE_PID) { if (usedPidControllerType == PID_TYPE_PID) {
antigravityThrottleHpf = rcCommand[THROTTLE] - pt1FilterApply(&antigravityThrottleLpf, rcCommand[THROTTLE]); antigravityThrottleHpf = rcCommand[THROTTLE] - pt1FilterApply(&antigravityThrottleLpf, rcCommand[THROTTLE]);
iTermAntigravityGain = scaleRangef(fabsf(antigravityThrottleHpf) * antigravityAccelerator, 0.0f, 1000.0f, 1.0f, antigravityGain); iTermAntigravityGain = scaleRangef(fabsf(antigravityThrottleHpf) * antigravityAccelerator, 0.0f, 1000.0f, 1.0f, antigravityGain);
} }
#endif #endif
@ -606,7 +600,7 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h
// Automatically pitch down if the throttle is manually controlled and reduced bellow cruise throttle // Automatically pitch down if the throttle is manually controlled and reduced bellow cruise throttle
if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle()) { if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle()) {
angleTarget += scaleRange(MAX(0, navConfig()->fw.cruise_throttle - rcCommand[THROTTLE]), 0, navConfig()->fw.cruise_throttle - PWM_RANGE_MIN, 0, mixerConfig()->fwMinThrottleDownPitchAngle); angleTarget += scaleRange(MAX(0, currentBatteryProfile->nav.fw.cruise_throttle - rcCommand[THROTTLE]), 0, currentBatteryProfile->nav.fw.cruise_throttle - PWM_RANGE_MIN, 0, currentBatteryProfile->fwMinThrottleDownPitchAngle);
} }
//PITCH trim applied by a AutoLevel flight mode and manual pitch trimming //PITCH trim applied by a AutoLevel flight mode and manual pitch trimming
@ -615,7 +609,7 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h
DEBUG_SET(DEBUG_AUTOLEVEL, 1, fixedWingLevelTrim * 10); DEBUG_SET(DEBUG_AUTOLEVEL, 1, fixedWingLevelTrim * 10);
DEBUG_SET(DEBUG_AUTOLEVEL, 2, getEstimatedActualVelocity(Z)); DEBUG_SET(DEBUG_AUTOLEVEL, 2, getEstimatedActualVelocity(Z));
/* /*
* fixedWingLevelTrim has opposite sign to rcCommand. * fixedWingLevelTrim has opposite sign to rcCommand.
* Positive rcCommand means nose should point downwards * Positive rcCommand means nose should point downwards
* Negative rcCommand mean nose should point upwards * Negative rcCommand mean nose should point upwards
@ -624,7 +618,7 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h
* Positive fixedWingLevelTrim means nose should point upwards * Positive fixedWingLevelTrim means nose should point upwards
* Negative fixedWingLevelTrim means nose should point downwards * Negative fixedWingLevelTrim means nose should point downwards
*/ */
angleTarget -= DEGREES_TO_DECIDEGREES(fixedWingLevelTrim); angleTarget -= DEGREES_TO_DECIDEGREES(fixedWingLevelTrim);
DEBUG_SET(DEBUG_AUTOLEVEL, 3, angleTarget * 10); DEBUG_SET(DEBUG_AUTOLEVEL, 3, angleTarget * 10);
} }
@ -702,23 +696,23 @@ static float pTermProcess(pidState_t *pidState, float rateError, float dT) {
#ifdef USE_D_BOOST #ifdef USE_D_BOOST
static float FAST_CODE applyDBoost(pidState_t *pidState, float dT) { static float FAST_CODE applyDBoost(pidState_t *pidState, float dT) {
float dBoost = 1.0f; float dBoost = 1.0f;
if (dBoostFactor > 1) { if (dBoostFactor > 1) {
const float dBoostGyroDelta = (pidState->gyroRate - pidState->previousRateGyro) / dT; const float dBoostGyroDelta = (pidState->gyroRate - pidState->previousRateGyro) / dT;
const float dBoostGyroAcceleration = fabsf(biquadFilterApply(&pidState->dBoostGyroLpf, dBoostGyroDelta)); const float dBoostGyroAcceleration = fabsf(biquadFilterApply(&pidState->dBoostGyroLpf, dBoostGyroDelta));
const float dBoostRateAcceleration = fabsf((pidState->rateTarget - pidState->previousRateTarget) / dT); const float dBoostRateAcceleration = fabsf((pidState->rateTarget - pidState->previousRateTarget) / dT);
const float acceleration = MAX(dBoostGyroAcceleration, dBoostRateAcceleration); const float acceleration = MAX(dBoostGyroAcceleration, dBoostRateAcceleration);
dBoost = scaleRangef(acceleration, 0.0f, dBoostMaxAtAlleceleration, 1.0f, dBoostFactor); dBoost = scaleRangef(acceleration, 0.0f, dBoostMaxAtAlleceleration, 1.0f, dBoostFactor);
dBoost = pt1FilterApply4(&pidState->dBoostLpf, dBoost, D_BOOST_LPF_HZ, dT); dBoost = pt1FilterApply4(&pidState->dBoostLpf, dBoost, D_BOOST_LPF_HZ, dT);
dBoost = constrainf(dBoost, 1.0f, dBoostFactor); dBoost = constrainf(dBoost, 1.0f, dBoostFactor);
} }
return dBoost; return dBoost;
} }
#else #else
static float applyDBoost(pidState_t *pidState, float dT) { static float applyDBoost(pidState_t *pidState, float dT) {
UNUSED(pidState); UNUSED(pidState);
UNUSED(dT); UNUSED(dT);
@ -747,7 +741,7 @@ static float dTermProcess(pidState_t *pidState, float dT) {
static void applyItermLimiting(pidState_t *pidState) { static void applyItermLimiting(pidState_t *pidState) {
if (pidState->itermLimitActive) { if (pidState->itermLimitActive) {
pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit); pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit);
} else } else
{ {
pidState->errorGyroIfLimit = fabsf(pidState->errorGyroIf); pidState->errorGyroIfLimit = fabsf(pidState->errorGyroIf);
} }
@ -806,7 +800,7 @@ static float FAST_CODE applyItermRelax(const int axis, float currentPidSetpoint,
const float setpointLpf = pt1FilterApply(&windupLpf[axis], currentPidSetpoint); const float setpointLpf = pt1FilterApply(&windupLpf[axis], currentPidSetpoint);
const float setpointHpf = fabsf(currentPidSetpoint - setpointLpf); const float setpointHpf = fabsf(currentPidSetpoint - setpointLpf);
const float itermRelaxFactor = MAX(0, 1 - setpointHpf / MC_ITERM_RELAX_SETPOINT_THRESHOLD); const float itermRelaxFactor = MAX(0, 1 - setpointHpf / MC_ITERM_RELAX_SETPOINT_THRESHOLD);
return itermErrorRate * itermRelaxFactor; return itermErrorRate * itermRelaxFactor;
} }
@ -822,18 +816,12 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid
const float newDTerm = dTermProcess(pidState, dT); const float newDTerm = dTermProcess(pidState, dT);
const float rateTargetDelta = pidState->rateTarget - pidState->previousRateTarget; const float rateTargetDelta = pidState->rateTarget - pidState->previousRateTarget;
const float rateTargetDeltaFiltered = biquadFilterApply(&pidState->rateTargetFilter, rateTargetDelta); const float rateTargetDeltaFiltered = pt2FilterApply(&pidState->rateTargetFilter, rateTargetDelta);
/* /*
* Compute Control Derivative * Compute Control Derivative
* CD is enabled only when ANGLE and HORIZON modes are not enabled!
*/ */
float newCDTerm; const float newCDTerm = rateTargetDeltaFiltered * (pidState->kCD / dT);
if (levelingEnabled) {
newCDTerm = 0.0F;
} else {
newCDTerm = rateTargetDeltaFiltered * (pidState->kCD / dT);
}
DEBUG_SET(DEBUG_CD, axis, newCDTerm); DEBUG_SET(DEBUG_CD, axis, newCDTerm);
// TODO: Get feedback from mixer on available correction range for each axis // TODO: Get feedback from mixer on available correction range for each axis
@ -1051,12 +1039,12 @@ void checkItermLimitingActive(pidState_t *pidState)
bool shouldActivate; bool shouldActivate;
if (usedPidControllerType == PID_TYPE_PIFF) { if (usedPidControllerType == PID_TYPE_PIFF) {
shouldActivate = isFixedWingItermLimitActive(pidState->stickPosition); shouldActivate = isFixedWingItermLimitActive(pidState->stickPosition);
} else } else
{ {
shouldActivate = mixerIsOutputSaturated(); shouldActivate = mixerIsOutputSaturated();
} }
pidState->itermLimitActive = STATE(ANTI_WINDUP) || shouldActivate; pidState->itermLimitActive = STATE(ANTI_WINDUP) || shouldActivate;
} }
void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis) void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis)
@ -1074,7 +1062,7 @@ void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis
{ {
pidState->itermFreezeActive = false; pidState->itermFreezeActive = false;
} }
} }
void FAST_CODE pidController(float dT) void FAST_CODE pidController(float dT)
@ -1102,7 +1090,7 @@ void FAST_CODE pidController(float dT)
} else { } else {
#ifdef USE_PROGRAMMING_FRAMEWORK #ifdef USE_PROGRAMMING_FRAMEWORK
rateTarget = pidRcCommandToRate(getRcCommandOverride(rcCommand, axis), currentControlRateProfile->stabilized.rates[axis]); rateTarget = pidRcCommandToRate(getRcCommandOverride(rcCommand, axis), currentControlRateProfile->stabilized.rates[axis]);
#else #else
rateTarget = pidRcCommandToRate(rcCommand[axis], currentControlRateProfile->stabilized.rates[axis]); rateTarget = pidRcCommandToRate(rcCommand[axis], currentControlRateProfile->stabilized.rates[axis]);
#endif #endif
} }
@ -1111,9 +1099,7 @@ void FAST_CODE pidController(float dT)
pidState[axis].rateTarget = constrainf(rateTarget, -GYRO_SATURATION_LIMIT, +GYRO_SATURATION_LIMIT); pidState[axis].rateTarget = constrainf(rateTarget, -GYRO_SATURATION_LIMIT, +GYRO_SATURATION_LIMIT);
#ifdef USE_GYRO_KALMAN #ifdef USE_GYRO_KALMAN
if (pidProfile()->kalmanEnabled) { gyroKalmanUpdateSetpoint(axis, pidState[axis].rateTarget);
pidState[axis].gyroRate = gyroKalmanUpdate(axis, pidState[axis].gyroRate, pidState[axis].rateTarget);
}
#endif #endif
#ifdef USE_SMITH_PREDICTOR #ifdef USE_SMITH_PREDICTOR
@ -1153,7 +1139,7 @@ void FAST_CODE pidController(float dT)
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
// Apply setpoint rate of change limits // Apply setpoint rate of change limits
pidApplySetpointRateLimiting(&pidState[axis], axis, dT); pidApplySetpointRateLimiting(&pidState[axis], axis, dT);
// Step 4: Run gyro-driven control // Step 4: Run gyro-driven control
checkItermLimitingActive(&pidState[axis]); checkItermLimitingActive(&pidState[axis]);
checkItermFreezingActive(&pidState[axis], axis); checkItermFreezingActive(&pidState[axis], axis);
@ -1165,7 +1151,7 @@ void FAST_CODE pidController(float dT)
pidType_e pidIndexGetType(pidIndex_e pidIndex) pidType_e pidIndexGetType(pidIndex_e pidIndex)
{ {
if (pidIndex == PID_ROLL || pidIndex == PID_PITCH || pidIndex == PID_YAW) { if (pidIndex == PID_ROLL || pidIndex == PID_PITCH || pidIndex == PID_YAW) {
return usedPidControllerType; return usedPidControllerType;
} }
if (STATE(AIRPLANE) || STATE(ROVER) || STATE(BOAT)) { if (STATE(AIRPLANE) || STATE(ROVER) || STATE(BOAT)) {
if (pidIndex == PID_VEL_XY || pidIndex == PID_VEL_Z) { if (pidIndex == PID_VEL_XY || pidIndex == PID_VEL_Z) {
@ -1201,7 +1187,7 @@ void pidInit(void)
antigravityGain = pidProfile()->antigravityGain; antigravityGain = pidProfile()->antigravityGain;
antigravityAccelerator = pidProfile()->antigravityAccelerator; antigravityAccelerator = pidProfile()->antigravityAccelerator;
#endif #endif
for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) { for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) {
if (axis == FD_YAW) { if (axis == FD_YAW) {
pidState[axis].pidSumLimit = pidProfile()->pidSumLimitYaw; pidState[axis].pidSumLimit = pidProfile()->pidSumLimitYaw;
@ -1218,7 +1204,7 @@ void pidInit(void)
if (pidProfile()->pidControllerType == PID_TYPE_AUTO) { if (pidProfile()->pidControllerType == PID_TYPE_AUTO) {
if ( if (
mixerConfig()->platformType == PLATFORM_AIRPLANE || mixerConfig()->platformType == PLATFORM_AIRPLANE ||
mixerConfig()->platformType == PLATFORM_BOAT || mixerConfig()->platformType == PLATFORM_BOAT ||
mixerConfig()->platformType == PLATFORM_ROVER mixerConfig()->platformType == PLATFORM_ROVER
) { ) {
@ -1257,11 +1243,6 @@ void pidInit(void)
} }
pidResetTPAFilter(); pidResetTPAFilter();
#ifdef USE_GYRO_KALMAN
if (pidProfile()->kalmanEnabled) {
gyroKalmanInitialize(pidProfile()->kalman_q, pidProfile()->kalman_w, pidProfile()->kalman_sharpness);
}
#endif
fixedWingLevelTrim = pidProfile()->fixedWingLevelTrim; fixedWingLevelTrim = pidProfile()->fixedWingLevelTrim;
@ -1276,10 +1257,10 @@ void pidInit(void)
} }
const pidBank_t * pidBank(void) { const pidBank_t * pidBank(void) {
return usedPidControllerType == PID_TYPE_PIFF ? &pidProfile()->bank_fw : &pidProfile()->bank_mc; return usedPidControllerType == PID_TYPE_PIFF ? &pidProfile()->bank_fw : &pidProfile()->bank_mc;
} }
pidBank_t * pidBankMutable(void) { pidBank_t * pidBankMutable(void) {
return usedPidControllerType == PID_TYPE_PIFF ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc; return usedPidControllerType == PID_TYPE_PIFF ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc;
} }
@ -1298,14 +1279,14 @@ void updateFixedWingLevelTrim(timeUs_t currentTimeUs)
*/ */
if (ARMING_FLAG(ARMED) && !previousArmingState) { if (ARMING_FLAG(ARMED) && !previousArmingState) {
navPidReset(&fixedWingLevelTrimController); navPidReset(&fixedWingLevelTrimController);
} }
/* /*
* On disarm update the default value * On disarm update the default value
*/ */
if (!ARMING_FLAG(ARMED) && previousArmingState) { if (!ARMING_FLAG(ARMED) && previousArmingState) {
pidProfileMutable()->fixedWingLevelTrim = constrainf(fixedWingLevelTrim, -FIXED_WING_LEVEL_TRIM_MAX_ANGLE, FIXED_WING_LEVEL_TRIM_MAX_ANGLE); pidProfileMutable()->fixedWingLevelTrim = constrainf(fixedWingLevelTrim, -FIXED_WING_LEVEL_TRIM_MAX_ANGLE, FIXED_WING_LEVEL_TRIM_MAX_ANGLE);
} }
/* /*
* Prepare flags for the PID controller * Prepare flags for the PID controller

7
src/main/flight/pid.h
View file

@ -156,13 +156,6 @@ typedef struct pidProfile_s {
uint16_t navFwPosHdgPidsumLimit; uint16_t navFwPosHdgPidsumLimit;
uint8_t controlDerivativeLpfHz; uint8_t controlDerivativeLpfHz;
#ifdef USE_GYRO_KALMAN
uint16_t kalman_q;
uint16_t kalman_w;
uint16_t kalman_sharpness;
uint8_t kalmanEnabled;
#endif
float fixedWingLevelTrim; float fixedWingLevelTrim;
float fixedWingLevelTrimGain; float fixedWingLevelTrimGain;
#ifdef USE_SMITH_PREDICTOR #ifdef USE_SMITH_PREDICTOR

52
src/main/flight/power_limits.c
View file

@ -45,22 +45,12 @@ FILE_COMPILE_FOR_SPEED
#define LIMITING_THR_FILTER_TCONST 50 #define LIMITING_THR_FILTER_TCONST 50
PG_REGISTER_WITH_RESET_TEMPLATE(powerLimitsConfig_t, powerLimitsConfig, PG_POWER_LIMITS_CONFIG, 0); PG_REGISTER_WITH_RESET_TEMPLATE(powerLimitsConfig_t, powerLimitsConfig, PG_POWER_LIMITS_CONFIG, 1);
PG_RESET_TEMPLATE(powerLimitsConfig_t, powerLimitsConfig, PG_RESET_TEMPLATE(powerLimitsConfig_t, powerLimitsConfig,
.continuousCurrent = SETTING_LIMIT_CONT_CURRENT_DEFAULT, // dA
.burstCurrent = SETTING_LIMIT_BURST_CURRENT_DEFAULT, // dA
.burstCurrentTime = SETTING_LIMIT_BURST_CURRENT_TIME_DEFAULT, // dS
.burstCurrentFalldownTime = SETTING_LIMIT_BURST_CURRENT_FALLDOWN_TIME_DEFAULT, // dS
#ifdef USE_ADC
.continuousPower = SETTING_LIMIT_CONT_POWER_DEFAULT, // dW
.burstPower = SETTING_LIMIT_BURST_POWER_DEFAULT, // dW
.burstPowerTime = SETTING_LIMIT_BURST_POWER_TIME_DEFAULT, // dS
.burstPowerFalldownTime = SETTING_LIMIT_BURST_POWER_FALLDOWN_TIME_DEFAULT, // dS
#endif
.piP = SETTING_LIMIT_PI_P_DEFAULT, .piP = SETTING_LIMIT_PI_P_DEFAULT,
.piI = SETTING_LIMIT_PI_I_DEFAULT, .piI = SETTING_LIMIT_PI_I_DEFAULT,
.attnFilterCutoff = SETTING_LIMIT_ATTN_FILTER_CUTOFF_DEFAULT, // Hz .attnFilterCutoff = SETTING_LIMIT_ATTN_FILTER_CUTOFF_DEFAULT, // Hz
); );
static float burstCurrentReserve; // cA.µs static float burstCurrentReserve; // cA.µs
@ -84,29 +74,29 @@ static bool wasLimitingPower = false;
#endif #endif
void powerLimiterInit(void) { void powerLimiterInit(void) {
if (powerLimitsConfig()->burstCurrent < powerLimitsConfig()->continuousCurrent) { if (currentBatteryProfile->powerLimits.burstCurrent < currentBatteryProfile->powerLimits.continuousCurrent) {
powerLimitsConfigMutable()->burstCurrent = powerLimitsConfig()->continuousCurrent; currentBatteryProfileMutable->powerLimits.burstCurrent = currentBatteryProfile->powerLimits.continuousCurrent;
} }
activeCurrentLimit = powerLimitsConfig()->burstCurrent; activeCurrentLimit = currentBatteryProfile->powerLimits.burstCurrent;
uint16_t currentBurstOverContinuous = powerLimitsConfig()->burstCurrent - powerLimitsConfig()->continuousCurrent; uint16_t currentBurstOverContinuous = currentBatteryProfile->powerLimits.burstCurrent - currentBatteryProfile->powerLimits.continuousCurrent;
burstCurrentReserve = burstCurrentReserveMax = currentBurstOverContinuous * powerLimitsConfig()->burstCurrentTime * 1e6; burstCurrentReserve = burstCurrentReserveMax = currentBurstOverContinuous * currentBatteryProfile->powerLimits.burstCurrentTime * 1e6;
burstCurrentReserveFalldown = currentBurstOverContinuous * powerLimitsConfig()->burstCurrentFalldownTime * 1e6; burstCurrentReserveFalldown = currentBurstOverContinuous * currentBatteryProfile->powerLimits.burstCurrentFalldownTime * 1e6;
pt1FilterInit(&currentThrAttnFilter, powerLimitsConfig()->attnFilterCutoff, 0); pt1FilterInit(&currentThrAttnFilter, powerLimitsConfig()->attnFilterCutoff, 0);
pt1FilterInitRC(&currentThrLimitingBaseFilter, LIMITING_THR_FILTER_TCONST, 0); pt1FilterInitRC(&currentThrLimitingBaseFilter, LIMITING_THR_FILTER_TCONST, 0);
#ifdef USE_ADC #ifdef USE_ADC
if (powerLimitsConfig()->burstPower < powerLimitsConfig()->continuousPower) { if (currentBatteryProfile->powerLimits.burstPower < currentBatteryProfile->powerLimits.continuousPower) {
powerLimitsConfigMutable()->burstPower = powerLimitsConfig()->continuousPower; currentBatteryProfileMutable->powerLimits.burstPower = currentBatteryProfile->powerLimits.continuousPower;
} }
activePowerLimit = powerLimitsConfig()->burstPower; activePowerLimit = currentBatteryProfile->powerLimits.burstPower;
uint16_t powerBurstOverContinuous = powerLimitsConfig()->burstPower - powerLimitsConfig()->continuousPower; uint16_t powerBurstOverContinuous = currentBatteryProfile->powerLimits.burstPower - currentBatteryProfile->powerLimits.continuousPower;
burstPowerReserve = burstPowerReserveMax = powerBurstOverContinuous * powerLimitsConfig()->burstPowerTime * 1e6; burstPowerReserve = burstPowerReserveMax = powerBurstOverContinuous * currentBatteryProfile->powerLimits.burstPowerTime * 1e6;
burstPowerReserveFalldown = powerBurstOverContinuous * powerLimitsConfig()->burstPowerFalldownTime * 1e6; burstPowerReserveFalldown = powerBurstOverContinuous * currentBatteryProfile->powerLimits.burstPowerFalldownTime * 1e6;
pt1FilterInit(&powerThrAttnFilter, powerLimitsConfig()->attnFilterCutoff, 0); pt1FilterInit(&powerThrAttnFilter, powerLimitsConfig()->attnFilterCutoff, 0);
pt1FilterInitRC(&powerThrLimitingBaseFilter, LIMITING_THR_FILTER_TCONST, 0); pt1FilterInitRC(&powerThrLimitingBaseFilter, LIMITING_THR_FILTER_TCONST, 0);
@ -118,7 +108,7 @@ static uint32_t calculateActiveLimit(int32_t value, uint32_t continuousLimit, ui
float spentReserveChunk = continuousDiff * timeDelta; float spentReserveChunk = continuousDiff * timeDelta;
*burstReserve = constrainf(*burstReserve - spentReserveChunk, 0, burstReserveMax); *burstReserve = constrainf(*burstReserve - spentReserveChunk, 0, burstReserveMax);
if (powerLimitsConfig()->burstCurrentFalldownTime) { if (currentBatteryProfile->powerLimits.burstCurrentFalldownTime) {
return scaleRangef(MIN(*burstReserve, burstReserveFalldown), 0, burstReserveFalldown, continuousLimit, burstLimit) * 10; return scaleRangef(MIN(*burstReserve, burstReserveFalldown), 0, burstReserveFalldown, continuousLimit, burstLimit) * 10;
} }
@ -127,7 +117,7 @@ static uint32_t calculateActiveLimit(int32_t value, uint32_t continuousLimit, ui
void currentLimiterUpdate(timeDelta_t timeDelta) { void currentLimiterUpdate(timeDelta_t timeDelta) {
activeCurrentLimit = calculateActiveLimit(getAmperage(), activeCurrentLimit = calculateActiveLimit(getAmperage(),
powerLimitsConfig()->continuousCurrent, powerLimitsConfig()->burstCurrent, currentBatteryProfile->powerLimits.continuousCurrent, currentBatteryProfile->powerLimits.burstCurrent,
&burstCurrentReserve, burstCurrentReserveFalldown, burstCurrentReserveMax, &burstCurrentReserve, burstCurrentReserveFalldown, burstCurrentReserveMax,
timeDelta); timeDelta);
} }
@ -135,7 +125,7 @@ void currentLimiterUpdate(timeDelta_t timeDelta) {
#ifdef USE_ADC #ifdef USE_ADC
void powerLimiterUpdate(timeDelta_t timeDelta) { void powerLimiterUpdate(timeDelta_t timeDelta) {
activePowerLimit = calculateActiveLimit(getPower(), activePowerLimit = calculateActiveLimit(getPower(),
powerLimitsConfig()->continuousPower, powerLimitsConfig()->burstPower, currentBatteryProfile->powerLimits.continuousPower, currentBatteryProfile->powerLimits.burstPower,
&burstPowerReserve, burstPowerReserveFalldown, burstPowerReserveMax, &burstPowerReserve, burstPowerReserveFalldown, burstPowerReserveMax,
timeDelta); timeDelta);
} }
@ -254,18 +244,18 @@ bool powerLimiterIsLimitingPower(void) {
// returns seconds // returns seconds
float powerLimiterGetRemainingBurstTime(void) { float powerLimiterGetRemainingBurstTime(void) {
uint16_t currentBurstOverContinuous = powerLimitsConfig()->burstCurrent - powerLimitsConfig()->continuousCurrent; uint16_t currentBurstOverContinuous = currentBatteryProfile->powerLimits.burstCurrent - currentBatteryProfile->powerLimits.continuousCurrent;
float remainingCurrentBurstTime = burstCurrentReserve / currentBurstOverContinuous / 1e7; float remainingCurrentBurstTime = burstCurrentReserve / currentBurstOverContinuous / 1e7;
#ifdef USE_ADC #ifdef USE_ADC
uint16_t powerBurstOverContinuous = powerLimitsConfig()->burstPower - powerLimitsConfig()->continuousPower; uint16_t powerBurstOverContinuous = currentBatteryProfile->powerLimits.burstPower - currentBatteryProfile->powerLimits.continuousPower;
float remainingPowerBurstTime = burstPowerReserve / powerBurstOverContinuous / 1e7; float remainingPowerBurstTime = burstPowerReserve / powerBurstOverContinuous / 1e7;
if (!powerLimitsConfig()->continuousCurrent) { if (!currentBatteryProfile->powerLimits.continuousCurrent) {
return remainingPowerBurstTime; return remainingPowerBurstTime;
} }
if (!powerLimitsConfig()->continuousPower) { if (!currentBatteryProfile->powerLimits.continuousPower) {
return remainingCurrentBurstTime; return remainingCurrentBurstTime;
} }

14
src/main/flight/power_limits.h
View file

@ -25,23 +25,13 @@
#include <common/time.h> #include <common/time.h>
#include "platform.h"
#include "config/parameter_group.h" #include "config/parameter_group.h"
#if defined(USE_POWER_LIMITS) #if defined(USE_POWER_LIMITS)
typedef struct { typedef struct {
uint16_t continuousCurrent; // dA
uint16_t burstCurrent; // dA
uint16_t burstCurrentTime; // ds
uint16_t burstCurrentFalldownTime; // ds
#ifdef USE_ADC
uint16_t continuousPower; // dW
uint16_t burstPower; // dW
uint16_t burstPowerTime; // ds
uint16_t burstPowerFalldownTime; // ds
#endif
uint16_t piP; uint16_t piP;
uint16_t piI; uint16_t piI;

6
src/main/flight/rth_estimator.c
View file

@ -77,9 +77,9 @@ static int8_t RTHInitialAltitudeChangePitchAngle(float altitudeChange) {
// pitch in degrees // pitch in degrees
// output in Watt // output in Watt
static float estimatePitchPower(float pitch) { static float estimatePitchPower(float pitch) {
int16_t altitudeChangeThrottle = (int16_t)pitch * navConfig()->fw.pitch_to_throttle; int16_t altitudeChangeThrottle = (int16_t)pitch * currentBatteryProfile->nav.fw.pitch_to_throttle;
altitudeChangeThrottle = constrain(altitudeChangeThrottle, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle); altitudeChangeThrottle = constrain(altitudeChangeThrottle, currentBatteryProfile->nav.fw.min_throttle, currentBatteryProfile->nav.fw.max_throttle);
const float altitudeChangeThrToCruiseThrRatio = (float)(altitudeChangeThrottle - getThrottleIdleValue()) / (navConfig()->fw.cruise_throttle - getThrottleIdleValue()); const float altitudeChangeThrToCruiseThrRatio = (float)(altitudeChangeThrottle - getThrottleIdleValue()) / (currentBatteryProfile->nav.fw.cruise_throttle - getThrottleIdleValue());
return (float)heatLossesCompensatedPower(batteryMetersConfig()->idle_power + batteryMetersConfig()->cruise_power * altitudeChangeThrToCruiseThrRatio) / 100; return (float)heatLossesCompensatedPower(batteryMetersConfig()->idle_power + batteryMetersConfig()->cruise_power * altitudeChangeThrToCruiseThrRatio) / 100;
} }

2
src/main/flight/servos.c
View file

@ -498,7 +498,7 @@ void processContinuousServoAutotrim(const float dT)
static servoAutotrimState_e trimState = AUTOTRIM_IDLE; static servoAutotrimState_e trimState = AUTOTRIM_IDLE;
static uint32_t servoMiddleUpdateCount; static uint32_t servoMiddleUpdateCount;
const float rotRateMagnitudeFiltered = pt1FilterApply4(&rotRateFilter, sqrtf(vectorNormSquared(&imuMeasuredRotationBF)), SERVO_AUTOTRIM_FILTER_CUTOFF, dT); const float rotRateMagnitudeFiltered = pt1FilterApply4(&rotRateFilter, fast_fsqrtf(vectorNormSquared(&imuMeasuredRotationBF)), SERVO_AUTOTRIM_FILTER_CUTOFF, dT);
const float targetRateMagnitudeFiltered = pt1FilterApply4(&targetRateFilter, getTotalRateTarget(), SERVO_AUTOTRIM_FILTER_CUTOFF, dT); const float targetRateMagnitudeFiltered = pt1FilterApply4(&targetRateFilter, getTotalRateTarget(), SERVO_AUTOTRIM_FILTER_CUTOFF, dT);
if (ARMING_FLAG(ARMED)) { if (ARMING_FLAG(ARMED)) {

8
src/main/flight/wind_estimator.c
View file

@ -72,7 +72,7 @@ float getEstimatedHorizontalWindSpeed(uint16_t *angle)
} }
*angle = RADIANS_TO_CENTIDEGREES(horizontalWindAngle); *angle = RADIANS_TO_CENTIDEGREES(horizontalWindAngle);
} }
return sqrtf(sq(xWindSpeed) + sq(yWindSpeed)); return fast_fsqrtf(sq(xWindSpeed) + sq(yWindSpeed));
} }
void updateWindEstimator(timeUs_t currentTimeUs) void updateWindEstimator(timeUs_t currentTimeUs)
@ -133,7 +133,7 @@ void updateWindEstimator(timeUs_t currentTimeUs)
groundVelocityDiff[Z] = groundVelocity[X] - lastGroundVelocity[Z]; groundVelocityDiff[Z] = groundVelocity[X] - lastGroundVelocity[Z];
// estimate airspeed it using equation 6 // estimate airspeed it using equation 6
float V = (sqrtf(sq(groundVelocityDiff[0]) + sq(groundVelocityDiff[1]) + sq(groundVelocityDiff[2]))) / sqrtf(diffLengthSq); float V = (fast_fsqrtf(sq(groundVelocityDiff[0]) + sq(groundVelocityDiff[1]) + sq(groundVelocityDiff[2]))) / fast_fsqrtf(diffLengthSq);
fuselageDirectionSum[X] = fuselageDirection[X] + lastFuselageDirection[X]; fuselageDirectionSum[X] = fuselageDirection[X] + lastFuselageDirection[X];
fuselageDirectionSum[Y] = fuselageDirection[Y] + lastFuselageDirection[Y]; fuselageDirectionSum[Y] = fuselageDirection[Y] + lastFuselageDirection[Y];
@ -155,8 +155,8 @@ void updateWindEstimator(timeUs_t currentTimeUs)
wind[Y] = (groundVelocitySum[Y] - V * (sintheta * fuselageDirectionSum[X] + costheta * fuselageDirectionSum[Y])) * 0.5f;// equation 11 wind[Y] = (groundVelocitySum[Y] - V * (sintheta * fuselageDirectionSum[X] + costheta * fuselageDirectionSum[Y])) * 0.5f;// equation 11
wind[Z] = (groundVelocitySum[Z] - V * fuselageDirectionSum[Z]) * 0.5f;// equation 12 wind[Z] = (groundVelocitySum[Z] - V * fuselageDirectionSum[Z]) * 0.5f;// equation 12
float prevWindLength = sqrtf(sq(estimatedWind[X]) + sq(estimatedWind[Y]) + sq(estimatedWind[Z])); float prevWindLength = fast_fsqrtf(sq(estimatedWind[X]) + sq(estimatedWind[Y]) + sq(estimatedWind[Z]));
float windLength = sqrtf(sq(wind[X]) + sq(wind[Y]) + sq(wind[Z])); float windLength = fast_fsqrtf(sq(wind[X]) + sq(wind[Y]) + sq(wind[Z]));
if (windLength < prevWindLength + 2000) { if (windLength < prevWindLength + 2000) {
// TODO: Better filtering // TODO: Better filtering

10
src/main/io/beeper.c
View file

@ -157,8 +157,8 @@ static uint8_t beep_multiBeeps[MAX_MULTI_BEEPS + 2];
#define BEEPER_CONFIRMATION_BEEP_GAP_DURATION 20 #define BEEPER_CONFIRMATION_BEEP_GAP_DURATION 20
// Beeper off = 0 Beeper on = 1 // Beeper off = false Beeper on = true
static uint8_t beeperIsOn = 0; static bool beeperIsOn = false;
// Place in current sequence // Place in current sequence
static uint16_t beeperPos = 0; static uint16_t beeperPos = 0;
@ -259,7 +259,7 @@ void beeperSilence(void)
warningLedRefresh(); warningLedRefresh();
beeperIsOn = 0; beeperIsOn = false;
beeperNextToggleTime = 0; beeperNextToggleTime = 0;
beeperPos = 0; beeperPos = 0;
@ -352,7 +352,7 @@ void beeperUpdate(timeUs_t currentTimeUs)
} }
#endif #endif
beeperIsOn = 1; beeperIsOn = true;
if (currentBeeperEntry->sequence[beeperPos] != 0) { if (currentBeeperEntry->sequence[beeperPos] != 0) {
if (!(getBeeperOffMask() & (1 << (currentBeeperEntry->mode - 1)))) if (!(getBeeperOffMask() & (1 << (currentBeeperEntry->mode - 1))))
BEEP_ON; BEEP_ON;
@ -368,7 +368,7 @@ void beeperUpdate(timeUs_t currentTimeUs)
} }
} }
} else { } else {
beeperIsOn = 0; beeperIsOn = false;
if (currentBeeperEntry->sequence[beeperPos] != 0) { if (currentBeeperEntry->sequence[beeperPos] != 0) {
BEEP_OFF; BEEP_OFF;
warningLedDisable(); warningLedDisable();

2
src/main/io/gps_msp.c
View file

@ -89,7 +89,7 @@ void mspGPSReceiveNewData(const uint8_t * bufferPtr)
gpsSol.velNED[X] = pkt->nedVelNorth; gpsSol.velNED[X] = pkt->nedVelNorth;
gpsSol.velNED[Y] = pkt->nedVelEast; gpsSol.velNED[Y] = pkt->nedVelEast;
gpsSol.velNED[Z] = pkt->nedVelDown; gpsSol.velNED[Z] = pkt->nedVelDown;
gpsSol.groundSpeed = sqrtf(sq((float)pkt->nedVelNorth) + sq((float)pkt->nedVelEast)); gpsSol.groundSpeed = fast_fsqrtf(sq((float)pkt->nedVelNorth) + sq((float)pkt->nedVelEast));
gpsSol.groundCourse = pkt->groundCourse / 10; // in deg * 10 gpsSol.groundCourse = pkt->groundCourse / 10; // in deg * 10
gpsSol.eph = gpsConstrainEPE(pkt->horizontalPosAccuracy / 10); gpsSol.eph = gpsConstrainEPE(pkt->horizontalPosAccuracy / 10);
gpsSol.epv = gpsConstrainEPE(pkt->verticalPosAccuracy / 10); gpsSol.epv = gpsConstrainEPE(pkt->verticalPosAccuracy / 10);

2
src/main/io/gps_naza.c
View file

@ -199,7 +199,7 @@ static bool NAZA_parse_gps(void)
gpsSol.eph = gpsConstrainEPE(h_acc / 10); // hAcc in cm gpsSol.eph = gpsConstrainEPE(h_acc / 10); // hAcc in cm
gpsSol.epv = gpsConstrainEPE(v_acc / 10); // vAcc in cm gpsSol.epv = gpsConstrainEPE(v_acc / 10); // vAcc in cm
gpsSol.numSat = _buffernaza.nav.satellites; gpsSol.numSat = _buffernaza.nav.satellites;
gpsSol.groundSpeed = sqrtf(powf(gpsSol.velNED[0], 2)+powf(gpsSol.velNED[1], 2)); //cm/s gpsSol.groundSpeed = fast_fsqrtf(powf(gpsSol.velNED[0], 2)+powf(gpsSol.velNED[1], 2)); //cm/s
// calculate gps heading from VELNE // calculate gps heading from VELNE
gpsSol.groundCourse = (uint16_t) (fmodf(RADIANS_TO_DECIDEGREES(atan2_approx(gpsSol.velNED[1], gpsSol.velNED[0]))+3600.0f,3600.0f)); gpsSol.groundCourse = (uint16_t) (fmodf(RADIANS_TO_DECIDEGREES(atan2_approx(gpsSol.velNED[1], gpsSol.velNED[0]))+3600.0f,3600.0f));

178
src/main/io/osd.c
View file

@ -317,6 +317,8 @@ static void osdLeftAlignString(char *buff)
static void osdFormatDistanceSymbol(char *buff, int32_t dist, uint8_t decimals) static void osdFormatDistanceSymbol(char *buff, int32_t dist, uint8_t decimals)
{ {
switch ((osd_unit_e)osdConfig()->units) { switch ((osd_unit_e)osdConfig()->units) {
case OSD_UNIT_UK:
FALLTHROUGH;
case OSD_UNIT_IMPERIAL: case OSD_UNIT_IMPERIAL:
if (osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(dist), FEET_PER_MILE, decimals, 3, 3)) { if (osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(dist), FEET_PER_MILE, decimals, 3, 3)) {
buff[3] = SYM_DIST_MI; buff[3] = SYM_DIST_MI;
@ -325,7 +327,7 @@ static void osdFormatDistanceSymbol(char *buff, int32_t dist, uint8_t decimals)
} }
buff[4] = '\0'; buff[4] = '\0';
break; break;
case OSD_UNIT_UK: case OSD_UNIT_METRIC_MPH:
FALLTHROUGH; FALLTHROUGH;
case OSD_UNIT_METRIC: case OSD_UNIT_METRIC:
if (osdFormatCentiNumber(buff, dist, METERS_PER_KILOMETER, decimals, 3, 3)) { if (osdFormatCentiNumber(buff, dist, METERS_PER_KILOMETER, decimals, 3, 3)) {
@ -344,32 +346,34 @@ static void osdFormatDistanceSymbol(char *buff, int32_t dist, uint8_t decimals)
*/ */
static void osdFormatDistanceStr(char *buff, int32_t dist) static void osdFormatDistanceStr(char *buff, int32_t dist)
{ {
int32_t centifeet; int32_t centifeet;
switch ((osd_unit_e)osdConfig()->units) { switch ((osd_unit_e)osdConfig()->units) {
case OSD_UNIT_IMPERIAL: case OSD_UNIT_UK:
centifeet = CENTIMETERS_TO_CENTIFEET(dist); FALLTHROUGH;
if (abs(centifeet) < FEET_PER_MILE * 100 / 2) { case OSD_UNIT_IMPERIAL:
// Show feet when dist < 0.5mi centifeet = CENTIMETERS_TO_CENTIFEET(dist);
tfp_sprintf(buff, "%d%c", (int)(centifeet / 100), SYM_FT); if (abs(centifeet) < FEET_PER_MILE * 100 / 2) {
} else { // Show feet when dist < 0.5mi
// Show miles when dist >= 0.5mi tfp_sprintf(buff, "%d%c", (int)(centifeet / 100), SYM_FT);
tfp_sprintf(buff, "%d.%02d%c", (int)(centifeet / (100*FEET_PER_MILE)), } else {
(abs(centifeet) % (100 * FEET_PER_MILE)) / FEET_PER_MILE, SYM_MI); // Show miles when dist >= 0.5mi
tfp_sprintf(buff, "%d.%02d%c", (int)(centifeet / (100*FEET_PER_MILE)),
(abs(centifeet) % (100 * FEET_PER_MILE)) / FEET_PER_MILE, SYM_MI);
}
break;
case OSD_UNIT_METRIC_MPH:
FALLTHROUGH;
case OSD_UNIT_METRIC:
if (abs(dist) < METERS_PER_KILOMETER * 100) {
// Show meters when dist < 1km
tfp_sprintf(buff, "%d%c", (int)(dist / 100), SYM_M);
} else {
// Show kilometers when dist >= 1km
tfp_sprintf(buff, "%d.%02d%c", (int)(dist / (100*METERS_PER_KILOMETER)),
(abs(dist) % (100 * METERS_PER_KILOMETER)) / METERS_PER_KILOMETER, SYM_KM);
}
break;
} }
break;
case OSD_UNIT_UK:
FALLTHROUGH;
case OSD_UNIT_METRIC:
if (abs(dist) < METERS_PER_KILOMETER * 100) {
// Show meters when dist < 1km
tfp_sprintf(buff, "%d%c", (int)(dist / 100), SYM_M);
} else {
// Show kilometers when dist >= 1km
tfp_sprintf(buff, "%d.%02d%c", (int)(dist / (100*METERS_PER_KILOMETER)),
(abs(dist) % (100 * METERS_PER_KILOMETER)) / METERS_PER_KILOMETER, SYM_KM);
}
break;
}
} }
/** /**
@ -381,6 +385,8 @@ static int32_t osdConvertVelocityToUnit(int32_t vel)
switch ((osd_unit_e)osdConfig()->units) { switch ((osd_unit_e)osdConfig()->units) {
case OSD_UNIT_UK: case OSD_UNIT_UK:
FALLTHROUGH; FALLTHROUGH;
case OSD_UNIT_METRIC_MPH:
FALLTHROUGH;
case OSD_UNIT_IMPERIAL: case OSD_UNIT_IMPERIAL:
return (vel * 224) / 10000; // Convert to mph return (vel * 224) / 10000; // Convert to mph
case OSD_UNIT_METRIC: case OSD_UNIT_METRIC:
@ -399,6 +405,8 @@ void osdFormatVelocityStr(char* buff, int32_t vel, bool _3D)
switch ((osd_unit_e)osdConfig()->units) { switch ((osd_unit_e)osdConfig()->units) {
case OSD_UNIT_UK: case OSD_UNIT_UK:
FALLTHROUGH; FALLTHROUGH;
case OSD_UNIT_METRIC_MPH:
FALLTHROUGH;
case OSD_UNIT_IMPERIAL: case OSD_UNIT_IMPERIAL:
tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_MPH : SYM_MPH)); tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_MPH : SYM_MPH));
break; break;
@ -422,6 +430,8 @@ static void osdFormatWindSpeedStr(char *buff, int32_t ws, bool isValid)
switch (osdConfig()->units) { switch (osdConfig()->units) {
case OSD_UNIT_UK: case OSD_UNIT_UK:
FALLTHROUGH; FALLTHROUGH;
case OSD_UNIT_METRIC_MPH:
FALLTHROUGH;
case OSD_UNIT_IMPERIAL: case OSD_UNIT_IMPERIAL:
centivalue = (ws * 224) / 100; centivalue = (ws * 224) / 100;
suffix = SYM_MPH; suffix = SYM_MPH;
@ -449,29 +459,38 @@ static void osdFormatWindSpeedStr(char *buff, int32_t ws, bool isValid)
*/ */
void osdFormatAltitudeSymbol(char *buff, int32_t alt) void osdFormatAltitudeSymbol(char *buff, int32_t alt)
{ {
int digits;
if (alt < 0) {
digits = 4;
} else {
digits = 3;
buff[0] = ' ';
}
switch ((osd_unit_e)osdConfig()->units) { switch ((osd_unit_e)osdConfig()->units) {
case OSD_UNIT_UK: case OSD_UNIT_UK:
FALLTHROUGH; FALLTHROUGH;
case OSD_UNIT_IMPERIAL: case OSD_UNIT_IMPERIAL:
if (osdFormatCentiNumber(buff , CENTIMETERS_TO_CENTIFEET(alt), 1000, 0, 2, 3)) { if (osdFormatCentiNumber(buff + 4 - digits, CENTIMETERS_TO_CENTIFEET(alt), 1000, 0, 2, digits)) {
// Scaled to kft // Scaled to kft
buff[3] = SYM_ALT_KFT; buff[4] = SYM_ALT_KFT;
} else { } else {
// Formatted in feet // Formatted in feet
buff[3] = SYM_ALT_FT; buff[4] = SYM_ALT_FT;
} }
buff[4] = '\0'; buff[5] = '\0';
break; break;
case OSD_UNIT_METRIC_MPH:
FALLTHROUGH;
case OSD_UNIT_METRIC: case OSD_UNIT_METRIC:
// alt is alredy in cm // alt is alredy in cm
if (osdFormatCentiNumber(buff, alt, 1000, 0, 2, 3)) { if (osdFormatCentiNumber(buff + 4 - digits, alt, 1000, 0, 2, digits)) {
// Scaled to km // Scaled to km
buff[3] = SYM_ALT_KM; buff[4] = SYM_ALT_KM;
} else { } else {
// Formatted in m // Formatted in m
buff[3] = SYM_ALT_M; buff[4] = SYM_ALT_M;
} }
buff[4] = '\0'; buff[5] = '\0';
break; break;
} }
} }
@ -484,11 +503,13 @@ static void osdFormatAltitudeStr(char *buff, int32_t alt)
{ {
int32_t value; int32_t value;
switch ((osd_unit_e)osdConfig()->units) { switch ((osd_unit_e)osdConfig()->units) {
case OSD_UNIT_UK:
FALLTHROUGH;
case OSD_UNIT_IMPERIAL: case OSD_UNIT_IMPERIAL:
value = CENTIMETERS_TO_FEET(alt); value = CENTIMETERS_TO_FEET(alt);
tfp_sprintf(buff, "%d%c", (int)value, SYM_FT); tfp_sprintf(buff, "%d%c", (int)value, SYM_FT);
break; break;
case OSD_UNIT_UK: case OSD_UNIT_METRIC_MPH:
FALLTHROUGH; FALLTHROUGH;
case OSD_UNIT_METRIC: case OSD_UNIT_METRIC:
value = CENTIMETERS_TO_METERS(alt); value = CENTIMETERS_TO_METERS(alt);
@ -654,6 +675,9 @@ static void osdFormatCraftName(char *buff)
static const char * osdArmingDisabledReasonMessage(void) static const char * osdArmingDisabledReasonMessage(void)
{ {
const char *message = NULL;
char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH+1)];
switch (isArmingDisabledReason()) { switch (isArmingDisabledReason()) {
case ARMING_DISABLED_FAILSAFE_SYSTEM: case ARMING_DISABLED_FAILSAFE_SYSTEM:
// See handling of FAILSAFE_RX_LOSS_MONITORING in failsafe.c // See handling of FAILSAFE_RX_LOSS_MONITORING in failsafe.c
@ -678,6 +702,7 @@ static const char * osdArmingDisabledReasonMessage(void)
#if defined(USE_NAV) #if defined(USE_NAV)
// Check the exact reason // Check the exact reason
switch (navigationIsBlockingArming(NULL)) { switch (navigationIsBlockingArming(NULL)) {
char buf[6];
case NAV_ARMING_BLOCKER_NONE: case NAV_ARMING_BLOCKER_NONE:
break; break;
case NAV_ARMING_BLOCKER_MISSING_GPS_FIX: case NAV_ARMING_BLOCKER_MISSING_GPS_FIX:
@ -685,7 +710,9 @@ static const char * osdArmingDisabledReasonMessage(void)
case NAV_ARMING_BLOCKER_NAV_IS_ALREADY_ACTIVE: case NAV_ARMING_BLOCKER_NAV_IS_ALREADY_ACTIVE:
return OSD_MESSAGE_STR(OSD_MSG_DISABLE_NAV_FIRST); return OSD_MESSAGE_STR(OSD_MSG_DISABLE_NAV_FIRST);
case NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR: case NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR:
return OSD_MESSAGE_STR(OSD_MSG_1ST_WP_TOO_FAR); osdFormatDistanceSymbol(buf, distanceToFirstWP(), 0);
tfp_sprintf(messageBuf, "FIRST WP TOO FAR (%s)", buf);
return message = messageBuf;
case NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR: case NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR:
return OSD_MESSAGE_STR(OSD_MSG_JUMP_WP_MISCONFIG); return OSD_MESSAGE_STR(OSD_MSG_JUMP_WP_MISCONFIG);
} }
@ -1081,12 +1108,14 @@ static void osdDrawMap(int referenceHeading, uint8_t referenceSym, uint8_t cente
const int scaleReductionMultiplier = MIN(midX - hMargin, midY - vMargin) / 2; const int scaleReductionMultiplier = MIN(midX - hMargin, midY - vMargin) / 2;
switch (osdConfig()->units) { switch (osdConfig()->units) {
case OSD_UNIT_UK:
FALLTHROUGH;
case OSD_UNIT_IMPERIAL: case OSD_UNIT_IMPERIAL:
initialScale = 16; // 16m ~= 0.01miles initialScale = 16; // 16m ~= 0.01miles
break; break;
case OSD_UNIT_UK:
FALLTHROUGH;
default: default:
case OSD_UNIT_METRIC_MPH:
FALLTHROUGH;
case OSD_UNIT_METRIC: case OSD_UNIT_METRIC:
initialScale = 10; // 10m as initial scale initialScale = 10; // 10m as initial scale
break; break;
@ -2117,6 +2146,8 @@ static bool osdDrawSingleElement(uint8_t item)
sym = SYM_FTS; sym = SYM_FTS;
break; break;
default: default:
case OSD_UNIT_METRIC_MPH:
FALLTHROUGH;
case OSD_UNIT_METRIC: case OSD_UNIT_METRIC:
// Already in cm/s // Already in cm/s
sym = SYM_MS; sym = SYM_MS;
@ -2243,15 +2274,15 @@ static bool osdDrawSingleElement(uint8_t item)
return true; return true;
case OSD_NAV_FW_CRUISE_THR: case OSD_NAV_FW_CRUISE_THR:
osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "CRZ", 0, navConfig()->fw.cruise_throttle, 4, 0, ADJUSTMENT_NAV_FW_CRUISE_THR); osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "CRZ", 0, currentBatteryProfile->nav.fw.cruise_throttle, 4, 0, ADJUSTMENT_NAV_FW_CRUISE_THR);
return true; return true;
case OSD_NAV_FW_PITCH2THR: case OSD_NAV_FW_PITCH2THR:
osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "P2T", 0, navConfig()->fw.pitch_to_throttle, 3, 0, ADJUSTMENT_NAV_FW_PITCH2THR); osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "P2T", 0, currentBatteryProfile->nav.fw.pitch_to_throttle, 3, 0, ADJUSTMENT_NAV_FW_PITCH2THR);
return true; return true;
case OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE: case OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE:
osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "0TP", 0, (float)mixerConfig()->fwMinThrottleDownPitchAngle / 10, 3, 1, ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE); osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "0TP", 0, (float)currentBatteryProfile->fwMinThrottleDownPitchAngle / 10, 3, 1, ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE);
return true; return true;
case OSD_FW_ALT_PID_OUTPUTS: case OSD_FW_ALT_PID_OUTPUTS:
@ -2403,6 +2434,8 @@ static bool osdDrawSingleElement(uint8_t item)
} }
bool efficiencyValid = (value > 0) && (gpsSol.groundSpeed > 100); bool efficiencyValid = (value > 0) && (gpsSol.groundSpeed > 100);
switch (osdConfig()->units) { switch (osdConfig()->units) {
case OSD_UNIT_UK:
FALLTHROUGH;
case OSD_UNIT_IMPERIAL: case OSD_UNIT_IMPERIAL:
moreThanAh = osdFormatCentiNumber(buff, value * METERS_PER_MILE / 10, 1000, 0, 2, 3); moreThanAh = osdFormatCentiNumber(buff, value * METERS_PER_MILE / 10, 1000, 0, 2, 3);
if (!moreThanAh) { if (!moreThanAh) {
@ -2417,7 +2450,7 @@ static bool osdDrawSingleElement(uint8_t item)
buff[5] = '\0'; buff[5] = '\0';
} }
break; break;
case OSD_UNIT_UK: case OSD_UNIT_METRIC_MPH:
FALLTHROUGH; FALLTHROUGH;
case OSD_UNIT_METRIC: case OSD_UNIT_METRIC:
moreThanAh = osdFormatCentiNumber(buff, value * 100, 1000, 0, 2, 3); moreThanAh = osdFormatCentiNumber(buff, value * 100, 1000, 0, 2, 3);
@ -2458,11 +2491,13 @@ static bool osdDrawSingleElement(uint8_t item)
} }
bool efficiencyValid = (value > 0) && (gpsSol.groundSpeed > 100); bool efficiencyValid = (value > 0) && (gpsSol.groundSpeed > 100);
switch (osdConfig()->units) { switch (osdConfig()->units) {
case OSD_UNIT_UK:
FALLTHROUGH;
case OSD_UNIT_IMPERIAL: 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);
buff[3] = SYM_WH_MI; buff[3] = SYM_WH_MI;
break; break;
case OSD_UNIT_UK: case OSD_UNIT_METRIC_MPH:
FALLTHROUGH; FALLTHROUGH;
case OSD_UNIT_METRIC: case OSD_UNIT_METRIC:
osdFormatCentiNumber(buff, value / 10, 0, 2, 0, 3); osdFormatCentiNumber(buff, value / 10, 0, 2, 0, 3);
@ -2647,6 +2682,8 @@ static bool osdDrawSingleElement(uint8_t item)
int maxDecimals; int maxDecimals;
switch (osdConfig()->units) { switch (osdConfig()->units) {
case OSD_UNIT_UK:
FALLTHROUGH;
case OSD_UNIT_IMPERIAL: case OSD_UNIT_IMPERIAL:
scaleToUnit = 100 / 1609.3440f; // scale to 0.01mi for osdFormatCentiNumber() scaleToUnit = 100 / 1609.3440f; // scale to 0.01mi for osdFormatCentiNumber()
scaleUnitDivisor = 0; scaleUnitDivisor = 0;
@ -2654,9 +2691,9 @@ static bool osdDrawSingleElement(uint8_t item)
symScaled = SYM_MI; symScaled = SYM_MI;
maxDecimals = 2; maxDecimals = 2;
break; break;
case OSD_UNIT_UK:
FALLTHROUGH;
default: default:
case OSD_UNIT_METRIC_MPH:
FALLTHROUGH;
case OSD_UNIT_METRIC: case OSD_UNIT_METRIC:
scaleToUnit = 100; // scale to cm for osdFormatCentiNumber() scaleToUnit = 100; // scale to cm for osdFormatCentiNumber()
scaleUnitDivisor = 1000; // Convert to km when scale gets bigger than 999m scaleUnitDivisor = 1000; // Convert to km when scale gets bigger than 999m
@ -2779,7 +2816,7 @@ static bool osdDrawSingleElement(uint8_t item)
break; break;
case OSD_PLIMIT_ACTIVE_CURRENT_LIMIT: case OSD_PLIMIT_ACTIVE_CURRENT_LIMIT:
if (powerLimitsConfig()->continuousCurrent) { if (currentBatteryProfile->powerLimits.continuousCurrent) {
osdFormatCentiNumber(buff, powerLimiterGetActiveCurrentLimit(), 0, 2, 0, 3); osdFormatCentiNumber(buff, powerLimiterGetActiveCurrentLimit(), 0, 2, 0, 3);
buff[3] = SYM_AMP; buff[3] = SYM_AMP;
buff[4] = '\0'; buff[4] = '\0';
@ -2793,7 +2830,7 @@ static bool osdDrawSingleElement(uint8_t item)
#ifdef USE_ADC #ifdef USE_ADC
case OSD_PLIMIT_ACTIVE_POWER_LIMIT: case OSD_PLIMIT_ACTIVE_POWER_LIMIT:
{ {
if (powerLimitsConfig()->continuousPower) { if (currentBatteryProfile->powerLimits.continuousPower) {
bool kiloWatt = osdFormatCentiNumber(buff, powerLimiterGetActivePowerLimit(), 1000, 2, 2, 3); bool kiloWatt = osdFormatCentiNumber(buff, powerLimiterGetActivePowerLimit(), 1000, 2, 2, 3);
buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT; buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT;
buff[4] = '\0'; buff[4] = '\0';
@ -3219,12 +3256,20 @@ static void osdCompleteAsyncInitialization(void)
#define STATS_VALUE_X_POS 24 #define STATS_VALUE_X_POS 24
if (statsConfig()->stats_enabled) { if (statsConfig()->stats_enabled) {
displayWrite(osdDisplayPort, STATS_LABEL_X_POS, ++y, "ODOMETER:"); displayWrite(osdDisplayPort, STATS_LABEL_X_POS, ++y, "ODOMETER:");
if (osdConfig()->units == OSD_UNIT_IMPERIAL) { switch (osdConfig()->units) {
tfp_sprintf(string_buffer, "%5d", (int)(statsConfig()->stats_total_dist / METERS_PER_MILE)); case OSD_UNIT_UK:
string_buffer[5] = SYM_MI; FALLTHROUGH;
} else { case OSD_UNIT_IMPERIAL:
tfp_sprintf(string_buffer, "%5d", (int)(statsConfig()->stats_total_dist / METERS_PER_KILOMETER)); tfp_sprintf(string_buffer, "%5d", (int)(statsConfig()->stats_total_dist / METERS_PER_MILE));
string_buffer[5] = SYM_KM; string_buffer[5] = SYM_MI;
break;
default:
case OSD_UNIT_METRIC_MPH:
FALLTHROUGH;
case OSD_UNIT_METRIC:
tfp_sprintf(string_buffer, "%5d", (int)(statsConfig()->stats_total_dist / METERS_PER_KILOMETER));
string_buffer[5] = SYM_KM;
break;
} }
string_buffer[6] = '\0'; string_buffer[6] = '\0';
displayWrite(osdDisplayPort, STATS_VALUE_X_POS-5, y, string_buffer); displayWrite(osdDisplayPort, STATS_VALUE_X_POS-5, y, string_buffer);
@ -3244,12 +3289,20 @@ static void osdCompleteAsyncInitialization(void)
displayWrite(osdDisplayPort, STATS_LABEL_X_POS, ++y, "AVG EFFICIENCY:"); displayWrite(osdDisplayPort, STATS_LABEL_X_POS, ++y, "AVG EFFICIENCY:");
if (statsConfig()->stats_total_dist) { if (statsConfig()->stats_total_dist) {
uint32_t avg_efficiency = statsConfig()->stats_total_energy / (statsConfig()->stats_total_dist / METERS_PER_KILOMETER); // mWh/km uint32_t avg_efficiency = statsConfig()->stats_total_energy / (statsConfig()->stats_total_dist / METERS_PER_KILOMETER); // mWh/km
if (osdConfig()->units == OSD_UNIT_IMPERIAL) { switch (osdConfig()->units) {
osdFormatCentiNumber(string_buffer, avg_efficiency / 10, 0, 2, 0, 3); case OSD_UNIT_UK:
string_buffer[3] = SYM_WH_MI; FALLTHROUGH;
} else { case OSD_UNIT_IMPERIAL:
osdFormatCentiNumber(string_buffer, avg_efficiency / 10000 * METERS_PER_MILE, 0, 2, 0, 3); osdFormatCentiNumber(string_buffer, avg_efficiency / 10, 0, 2, 0, 3);
string_buffer[3] = SYM_WH_KM; string_buffer[3] = SYM_WH_MI;
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);
string_buffer[3] = SYM_WH_KM;
break;
} }
} else { } else {
string_buffer[0] = string_buffer[1] = string_buffer[2] = '-'; string_buffer[0] = string_buffer[1] = string_buffer[2] = '-';
@ -3475,6 +3528,8 @@ static void osdShowStatsPage2(void)
} }
} }
break; break;
case OSD_UNIT_METRIC_MPH:
FALLTHROUGH;
case OSD_UNIT_METRIC: case OSD_UNIT_METRIC:
if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) {
moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000000.0f / totalDistance), 1000, 0, 2, 3); moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000000.0f / totalDistance), 1000, 0, 2, 3);
@ -3597,7 +3652,7 @@ static void osdFilterData(timeUs_t currentTimeUs) {
static timeUs_t lastRefresh = 0; static timeUs_t lastRefresh = 0;
float refresh_dT = US2S(cmpTimeUs(currentTimeUs, lastRefresh)); float refresh_dT = US2S(cmpTimeUs(currentTimeUs, lastRefresh));
GForce = sqrtf(vectorNormSquared(&imuMeasuredAccelBF)) / GRAVITY_MSS; GForce = fast_fsqrtf(vectorNormSquared(&imuMeasuredAccelBF)) / GRAVITY_MSS;
for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) GForceAxis[axis] = imuMeasuredAccelBF.v[axis] / GRAVITY_MSS; for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) GForceAxis[axis] = imuMeasuredAccelBF.v[axis] / GRAVITY_MSS;
if (lastRefresh) { if (lastRefresh) {
@ -3922,6 +3977,9 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
} }
if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) { if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) {
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTUNE); messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTUNE);
if (FLIGHT_MODE(MANUAL_MODE)) {
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTUNE_ACRO);
}
} }
if (FLIGHT_MODE(HEADFREE_MODE)) { if (FLIGHT_MODE(HEADFREE_MODE)) {
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_HEADFREE); messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_HEADFREE);

5
src/main/io/osd.h
View file

@ -53,7 +53,6 @@
#define OSD_MSG_SYS_OVERLOADED "SYSTEM OVERLOADED" #define OSD_MSG_SYS_OVERLOADED "SYSTEM OVERLOADED"
#define OSD_MSG_WAITING_GPS_FIX "WAITING FOR GPS FIX" #define OSD_MSG_WAITING_GPS_FIX "WAITING FOR GPS FIX"
#define OSD_MSG_DISABLE_NAV_FIRST "DISABLE NAVIGATION FIRST" #define OSD_MSG_DISABLE_NAV_FIRST "DISABLE NAVIGATION FIRST"
#define OSD_MSG_1ST_WP_TOO_FAR "FIRST WAYPOINT IS TOO FAR"
#define OSD_MSG_JUMP_WP_MISCONFIG "JUMP WAYPOINT MISCONFIGURED" #define OSD_MSG_JUMP_WP_MISCONFIG "JUMP WAYPOINT MISCONFIGURED"
#define OSD_MSG_MAG_NOT_CAL "COMPASS NOT CALIBRATED" #define OSD_MSG_MAG_NOT_CAL "COMPASS NOT CALIBRATED"
#define OSD_MSG_ACC_NOT_CAL "ACCELEROMETER NOT CALIBRATED" #define OSD_MSG_ACC_NOT_CAL "ACCELEROMETER NOT CALIBRATED"
@ -98,6 +97,7 @@
#define OSD_MSG_ALTITUDE_HOLD "(ALTITUDE HOLD)" #define OSD_MSG_ALTITUDE_HOLD "(ALTITUDE HOLD)"
#define OSD_MSG_AUTOTRIM "(AUTOTRIM)" #define OSD_MSG_AUTOTRIM "(AUTOTRIM)"
#define OSD_MSG_AUTOTUNE "(AUTOTUNE)" #define OSD_MSG_AUTOTUNE "(AUTOTUNE)"
#define OSD_MSG_AUTOTUNE_ACRO "SWITCH TO ACRO"
#define OSD_MSG_HEADFREE "(HEADFREE)" #define OSD_MSG_HEADFREE "(HEADFREE)"
#define OSD_MSG_UNABLE_ARM "UNABLE TO ARM" #define OSD_MSG_UNABLE_ARM "UNABLE TO ARM"
@ -238,7 +238,8 @@ typedef enum {
typedef enum { typedef enum {
OSD_UNIT_IMPERIAL, OSD_UNIT_IMPERIAL,
OSD_UNIT_METRIC, OSD_UNIT_METRIC,
OSD_UNIT_UK, // Show speed in mp/h, other values in 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_MAX = OSD_UNIT_UK, OSD_UNIT_MAX = OSD_UNIT_UK,
} osd_unit_e; } osd_unit_e;

20
src/main/io/osd_canvas.c
View file

@ -502,9 +502,11 @@ static int32_t osdCanvasSidebarGetValue(osd_sidebar_scroll_e scroll)
break; break;
case OSD_SIDEBAR_SCROLL_ALTITUDE: case OSD_SIDEBAR_SCROLL_ALTITUDE:
switch ((osd_unit_e)osdConfig()->units) { switch ((osd_unit_e)osdConfig()->units) {
case OSD_UNIT_UK:
FALLTHROUGH;
case OSD_UNIT_IMPERIAL: case OSD_UNIT_IMPERIAL:
return CENTIMETERS_TO_CENTIFEET(osdGetAltitude()); return CENTIMETERS_TO_CENTIFEET(osdGetAltitude());
case OSD_UNIT_UK: case OSD_UNIT_METRIC_MPH:
FALLTHROUGH; FALLTHROUGH;
case OSD_UNIT_METRIC: case OSD_UNIT_METRIC:
return osdGetAltitude(); return osdGetAltitude();
@ -517,6 +519,8 @@ static int32_t osdCanvasSidebarGetValue(osd_sidebar_scroll_e scroll)
switch ((osd_unit_e)osdConfig()->units) { switch ((osd_unit_e)osdConfig()->units) {
case OSD_UNIT_UK: case OSD_UNIT_UK:
FALLTHROUGH; FALLTHROUGH;
case OSD_UNIT_METRIC_MPH:
FALLTHROUGH;
case OSD_UNIT_IMPERIAL: case OSD_UNIT_IMPERIAL:
// cms/s to (mi/h) * 100 // cms/s to (mi/h) * 100
return speed * 224 / 100; return speed * 224 / 100;
@ -530,9 +534,11 @@ static int32_t osdCanvasSidebarGetValue(osd_sidebar_scroll_e scroll)
case OSD_SIDEBAR_SCROLL_HOME_DISTANCE: case OSD_SIDEBAR_SCROLL_HOME_DISTANCE:
#if defined(USE_GPS) #if defined(USE_GPS)
switch ((osd_unit_e)osdConfig()->units) { switch ((osd_unit_e)osdConfig()->units) {
case OSD_UNIT_UK:
FALLTHROUGH;
case OSD_UNIT_IMPERIAL: case OSD_UNIT_IMPERIAL:
return CENTIMETERS_TO_CENTIFEET(GPS_distanceToHome * 100); return CENTIMETERS_TO_CENTIFEET(GPS_distanceToHome * 100);
case OSD_UNIT_UK: case OSD_UNIT_METRIC_MPH:
FALLTHROUGH; FALLTHROUGH;
case OSD_UNIT_METRIC: case OSD_UNIT_METRIC:
return GPS_distanceToHome * 100; return GPS_distanceToHome * 100;
@ -575,13 +581,15 @@ static void osdCanvasSidebarGetUnit(osdUnit_t *unit, uint16_t *countsPerStep, os
break; break;
case OSD_SIDEBAR_SCROLL_ALTITUDE: case OSD_SIDEBAR_SCROLL_ALTITUDE:
switch ((osd_unit_e)osdConfig()->units) { switch ((osd_unit_e)osdConfig()->units) {
case OSD_UNIT_UK:
FALLTHROUGH;
case OSD_UNIT_IMPERIAL: case OSD_UNIT_IMPERIAL:
unit->symbol = SYM_ALT_FT; unit->symbol = SYM_ALT_FT;
unit->divisor = FEET_PER_KILOFEET; unit->divisor = FEET_PER_KILOFEET;
unit->divided_symbol = SYM_ALT_KFT; unit->divided_symbol = SYM_ALT_KFT;
*countsPerStep = 50; *countsPerStep = 50;
break; break;
case OSD_UNIT_UK: case OSD_UNIT_METRIC_MPH:
FALLTHROUGH; FALLTHROUGH;
case OSD_UNIT_METRIC: case OSD_UNIT_METRIC:
unit->symbol = SYM_ALT_M; unit->symbol = SYM_ALT_M;
@ -595,6 +603,8 @@ static void osdCanvasSidebarGetUnit(osdUnit_t *unit, uint16_t *countsPerStep, os
switch ((osd_unit_e)osdConfig()->units) { switch ((osd_unit_e)osdConfig()->units) {
case OSD_UNIT_UK: case OSD_UNIT_UK:
FALLTHROUGH; FALLTHROUGH;
case OSD_UNIT_METRIC_MPH:
FALLTHROUGH;
case OSD_UNIT_IMPERIAL: case OSD_UNIT_IMPERIAL:
unit->symbol = SYM_MPH; unit->symbol = SYM_MPH;
unit->divisor = 0; unit->divisor = 0;
@ -611,13 +621,15 @@ static void osdCanvasSidebarGetUnit(osdUnit_t *unit, uint16_t *countsPerStep, os
break; break;
case OSD_SIDEBAR_SCROLL_HOME_DISTANCE: case OSD_SIDEBAR_SCROLL_HOME_DISTANCE:
switch ((osd_unit_e)osdConfig()->units) { switch ((osd_unit_e)osdConfig()->units) {
case OSD_UNIT_UK:
FALLTHROUGH;
case OSD_UNIT_IMPERIAL: case OSD_UNIT_IMPERIAL:
unit->symbol = SYM_FT; unit->symbol = SYM_FT;
unit->divisor = FEET_PER_MILE; unit->divisor = FEET_PER_MILE;
unit->divided_symbol = SYM_MI; unit->divided_symbol = SYM_MI;
*countsPerStep = 300; *countsPerStep = 300;
break; break;
case OSD_UNIT_UK: case OSD_UNIT_METRIC_MPH:
FALLTHROUGH; FALLTHROUGH;
case OSD_UNIT_METRIC: case OSD_UNIT_METRIC:
unit->symbol = SYM_M; unit->symbol = SYM_M;

2
src/main/io/osd_common.c
View file

@ -195,6 +195,6 @@ int16_t osdGet3DSpeed(void)
{ {
int16_t vert_speed = getEstimatedActualVelocity(Z); int16_t vert_speed = getEstimatedActualVelocity(Z);
int16_t hor_speed = gpsSol.groundSpeed; int16_t hor_speed = gpsSol.groundSpeed;
return (int16_t)sqrtf(sq(hor_speed) + sq(vert_speed)); return (int16_t)fast_fsqrtf(sq(hor_speed) + sq(vert_speed));
} }
#endif #endif

601
src/main/io/osd_dji_hd.c
View file

@ -49,6 +49,7 @@
#include "fc/fc_msp_box.h" #include "fc/fc_msp_box.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "fc/settings.h" #include "fc/settings.h"
#include "fc/rc_adjustments.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/pid.h" #include "flight/pid.h"
@ -70,6 +71,7 @@
#include "sensors/esc_sensor.h" #include "sensors/esc_sensor.h"
#include "sensors/temperature.h" #include "sensors/temperature.h"
#include "sensors/pitotmeter.h" #include "sensors/pitotmeter.h"
#include "sensors/boardalignment.h"
#include "msp/msp.h" #include "msp/msp.h"
#include "msp/msp_protocol.h" #include "msp/msp_protocol.h"
@ -125,8 +127,25 @@ PG_RESET_TEMPLATE(djiOsdConfig_t, djiOsdConfig,
.use_name_for_messages = SETTING_DJI_USE_NAME_FOR_MESSAGES_DEFAULT, .use_name_for_messages = SETTING_DJI_USE_NAME_FOR_MESSAGES_DEFAULT,
.esc_temperature_source = SETTING_DJI_ESC_TEMP_SOURCE_DEFAULT, .esc_temperature_source = SETTING_DJI_ESC_TEMP_SOURCE_DEFAULT,
.proto_workarounds = SETTING_DJI_WORKAROUNDS_DEFAULT, .proto_workarounds = SETTING_DJI_WORKAROUNDS_DEFAULT,
.messageSpeedSource = SETTING_DJI_MESSAGE_SPEED_SOURCE_DEFAULT,
.rssi_source = SETTING_DJI_RSSI_SOURCE_DEFAULT,
.useAdjustments = SETTING_DJI_USE_ADJUSTMENTS_DEFAULT,
.craftNameAlternatingDuration = SETTING_DJI_CN_ALTERNATING_DURATION_DEFAULT
); );
#define RSSI_BOUNDARY(PERCENT) (RSSI_MAX_VALUE / 100 * PERCENT)
typedef enum {
DJI_OSD_CN_MESSAGES,
DJI_OSD_CN_THROTTLE,
DJI_OSD_CN_THROTTLE_AUTO_THR,
DJI_OSD_CN_AIR_SPEED,
DJI_OSD_CN_EFFICIENCY,
DJI_OSD_CN_DISTANCE,
DJI_OSD_CN_ADJUSTEMNTS,
DJI_OSD_CN_MAX_ELEMENTS
} DjiCraftNameElements_t;
// External dependency on looptime // External dependency on looptime
extern timeDelta_t cycleTime; extern timeDelta_t cycleTime;
@ -413,7 +432,7 @@ static void djiSerializeOSDConfigReply(sbuf_t *dst)
//sbufWriteU8(dst, DJI_OSD_SCREEN_HEIGHT); // osdConfig()->camera_frame_height //sbufWriteU8(dst, DJI_OSD_SCREEN_HEIGHT); // osdConfig()->camera_frame_height
} }
static const char * osdArmingDisabledReasonMessage(void) static char * osdArmingDisabledReasonMessage(void)
{ {
switch (isArmingDisabledReason()) { switch (isArmingDisabledReason()) {
case ARMING_DISABLED_FAILSAFE_SYSTEM: case ARMING_DISABLED_FAILSAFE_SYSTEM:
@ -522,7 +541,7 @@ static const char * osdArmingDisabledReasonMessage(void)
return NULL; return NULL;
} }
static const char * osdFailsafePhaseMessage(void) static char * osdFailsafePhaseMessage(void)
{ {
// See failsafe.h for each phase explanation // See failsafe.h for each phase explanation
switch (failsafePhase()) { switch (failsafePhase()) {
@ -563,7 +582,7 @@ static const char * osdFailsafePhaseMessage(void)
return NULL; return NULL;
} }
static const char * osdFailsafeInfoMessage(void) static char * osdFailsafeInfoMessage(void)
{ {
if (failsafeIsReceivingRxData()) { if (failsafeIsReceivingRxData()) {
// User must move sticks to exit FS mode // User must move sticks to exit FS mode
@ -573,7 +592,7 @@ static const char * osdFailsafeInfoMessage(void)
return OSD_MESSAGE_STR(RC_RX_LINK_LOST_MSG); return OSD_MESSAGE_STR(RC_RX_LINK_LOST_MSG);
} }
static const char * navigationStateMessage(void) static char * navigationStateMessage(void)
{ {
switch (NAV_Status.state) { switch (NAV_Status.state) {
case MW_NAV_STATE_NONE: case MW_NAV_STATE_NONE:
@ -633,10 +652,10 @@ static int32_t osdConvertVelocityToUnit(int32_t vel)
switch (osdConfig()->units) { switch (osdConfig()->units) {
case OSD_UNIT_UK: case OSD_UNIT_UK:
FALLTHROUGH; FALLTHROUGH;
case OSD_UNIT_METRIC_MPH:
FALLTHROUGH;
case OSD_UNIT_IMPERIAL: case OSD_UNIT_IMPERIAL:
return (vel * 224) / 10000; // Convert to mph return (vel * 224) / 10000; // Convert to mph
case OSD_UNIT_METRIC: case OSD_UNIT_METRIC:
return (vel * 36) / 1000; // Convert to kmh return (vel * 36) / 1000; // Convert to kmh
} }
@ -649,16 +668,37 @@ static int32_t osdConvertVelocityToUnit(int32_t vel)
* Converts velocity into a string based on the current unit system. * Converts velocity into a string based on the current unit system.
* @param alt Raw velocity (i.e. as taken from gpsSol.groundSpeed in centimeters/seconds) * @param alt Raw velocity (i.e. as taken from gpsSol.groundSpeed in centimeters/seconds)
*/ */
void osdDJIFormatVelocityStr(char* buff, int32_t vel ) void osdDJIFormatVelocityStr(char* buff)
{ {
char sourceBuf[4];
int vel = 0;
switch (djiOsdConfig()->messageSpeedSource) {
case OSD_SPEED_SOURCE_GROUND:
strcpy(sourceBuf, "GRD");
vel = gpsSol.groundSpeed;
break;
case OSD_SPEED_SOURCE_3D:
strcpy(sourceBuf, "3D");
vel = osdGet3DSpeed();
break;
case OSD_SPEED_SOURCE_AIR:
strcpy(sourceBuf, "AIR");
#ifdef USE_PITOT
vel = pitot.airSpeed;
#endif
break;
}
switch (osdConfig()->units) { switch (osdConfig()->units) {
case OSD_UNIT_UK: case OSD_UNIT_UK:
FALLTHROUGH; FALLTHROUGH;
case OSD_UNIT_METRIC_MPH:
FALLTHROUGH;
case OSD_UNIT_IMPERIAL: case OSD_UNIT_IMPERIAL:
tfp_sprintf(buff, "%3d%s", (int)osdConvertVelocityToUnit(vel), "MPH"); tfp_sprintf(buff, "%s %3d MPH", sourceBuf, (int)osdConvertVelocityToUnit(vel));
break; break;
case OSD_UNIT_METRIC: case OSD_UNIT_METRIC:
tfp_sprintf(buff, "%3d%s", (int)osdConvertVelocityToUnit(vel), "KMH"); tfp_sprintf(buff, "%s %3d KPH", sourceBuf, (int)osdConvertVelocityToUnit(vel));
break; break;
} }
} }
@ -681,6 +721,8 @@ static void osdDJIFormatDistanceStr(char *buff, int32_t dist)
int32_t centifeet; int32_t centifeet;
switch (osdConfig()->units) { switch (osdConfig()->units) {
case OSD_UNIT_UK:
FALLTHROUGH;
case OSD_UNIT_IMPERIAL: case OSD_UNIT_IMPERIAL:
centifeet = CENTIMETERS_TO_CENTIFEET(dist); centifeet = CENTIMETERS_TO_CENTIFEET(dist);
if (abs(centifeet) < FEET_PER_MILE * 100 / 2) { if (abs(centifeet) < FEET_PER_MILE * 100 / 2) {
@ -693,7 +735,7 @@ static void osdDJIFormatDistanceStr(char *buff, int32_t dist)
(abs(centifeet) % (100 * FEET_PER_MILE)) / FEET_PER_MILE, "Mi"); (abs(centifeet) % (100 * FEET_PER_MILE)) / FEET_PER_MILE, "Mi");
} }
break; break;
case OSD_UNIT_UK: case OSD_UNIT_METRIC_MPH:
FALLTHROUGH; FALLTHROUGH;
case OSD_UNIT_METRIC: case OSD_UNIT_METRIC:
if (abs(dist) < METERS_PER_KILOMETER * 100) { if (abs(dist) < METERS_PER_KILOMETER * 100) {
@ -726,175 +768,362 @@ static void osdDJIEfficiencyMahPerKM(char *buff)
1, US2S(efficiencyTimeDelta)); 1, US2S(efficiencyTimeDelta));
efficiencyUpdated = currentTimeUs; efficiencyUpdated = currentTimeUs;
} } else {
else {
value = eFilterState.state; value = eFilterState.state;
} }
} }
if (value > 0 && value <= 999) { if (value > 0 && value <= 999) {
tfp_sprintf(buff, "%3d%s", (int)value, "mAhKM"); tfp_sprintf(buff, "%3d%s", (int)value, "mAhKM");
} } else {
else {
tfp_sprintf(buff, "%s", "---mAhKM"); tfp_sprintf(buff, "%s", "---mAhKM");
} }
} }
static void djiSerializeCraftNameOverride(sbuf_t *dst, const char * name) static void osdDJIAdjustmentMessage(char *buff, uint8_t adjustmentFunction)
{ {
// :W T S E D switch (adjustmentFunction) {
// | | | | Distance Trip case ADJUSTMENT_RC_EXPO:
// | | | Efficiency mA/KM tfp_sprintf(buff, "RCE %d", currentControlRateProfile->stabilized.rcExpo8);
// | | S 3dSpeed break;
// | Throttle case ADJUSTMENT_RC_YAW_EXPO:
// Warnings tfp_sprintf(buff, "RCYE %3d", currentControlRateProfile->stabilized.rcYawExpo8);
const char *message = " "; break;
const char *enabledElements = name + 1; case ADJUSTMENT_MANUAL_RC_EXPO:
char djibuf[24]; tfp_sprintf(buff, "MRCE %3d", currentControlRateProfile->manual.rcExpo8);
break;
// clear name from chars : and leading W case ADJUSTMENT_MANUAL_RC_YAW_EXPO:
if (enabledElements[0] == 'W') { tfp_sprintf(buff, "MRCYE %3d", currentControlRateProfile->manual.rcYawExpo8);
enabledElements += 1; break;
} case ADJUSTMENT_THROTTLE_EXPO:
tfp_sprintf(buff, "TE %3d", currentControlRateProfile->throttle.rcExpo8);
int elemLen = strlen(enabledElements); break;
case ADJUSTMENT_PITCH_ROLL_RATE:
if (elemLen > 0) { tfp_sprintf(buff, "PRR %3d %3d", currentControlRateProfile->stabilized.rates[FD_PITCH], currentControlRateProfile->stabilized.rates[FD_ROLL]);
switch (enabledElements[OSD_ALTERNATING_CHOICES(3000, elemLen)]){ break;
case 'T': case ADJUSTMENT_PITCH_RATE:
osdDJIFormatThrottlePosition(djibuf,true); tfp_sprintf(buff, "PR %3d", currentControlRateProfile->stabilized.rates[FD_PITCH]);
break; break;
case 'S': case ADJUSTMENT_ROLL_RATE:
osdDJIFormatVelocityStr(djibuf, osdGet3DSpeed()); tfp_sprintf(buff, "RR %3d", currentControlRateProfile->stabilized.rates[FD_ROLL]);
break; break;
case 'E': case ADJUSTMENT_MANUAL_PITCH_ROLL_RATE:
osdDJIEfficiencyMahPerKM(djibuf); tfp_sprintf(buff, "MPRR %3d %3d", currentControlRateProfile->manual.rates[FD_PITCH], currentControlRateProfile->manual.rates[FD_ROLL]);
break; break;
case 'D': case ADJUSTMENT_MANUAL_PITCH_RATE:
osdDJIFormatDistanceStr(djibuf, getTotalTravelDistance()); tfp_sprintf(buff, "MPR %3d", currentControlRateProfile->manual.rates[FD_PITCH]);
break; break;
case 'W': case ADJUSTMENT_MANUAL_ROLL_RATE:
tfp_sprintf(djibuf, "%s", "MAKE_W_FIRST"); tfp_sprintf(buff, "MRR %3d", currentControlRateProfile->manual.rates[FD_ROLL]);
break; break;
default: case ADJUSTMENT_YAW_RATE:
tfp_sprintf(djibuf, "%s", "UNKOWN_ELEM"); tfp_sprintf(buff, "YR %3d", currentControlRateProfile->stabilized.rates[FD_YAW]);
break; break;
} case ADJUSTMENT_MANUAL_YAW_RATE:
tfp_sprintf(buff, "MYR %3d", currentControlRateProfile->manual.rates[FD_YAW]);
if (djibuf[0] != '\0') { break;
message = djibuf; case ADJUSTMENT_PITCH_ROLL_P:
} tfp_sprintf(buff, "PRP %3d %3d", pidBankMutable()->pid[PID_PITCH].P, pidBankMutable()->pid[PID_ROLL].P);
} break;
case ADJUSTMENT_PITCH_P:
if (name[1] == 'W') { tfp_sprintf(buff, "PP %3d", pidBankMutable()->pid[PID_PITCH].P);
char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH+1)]; break;
if (ARMING_FLAG(ARMED)) { case ADJUSTMENT_ROLL_P:
// Aircraft is armed. We might have up to 5 tfp_sprintf(buff, "RP %3d", pidBankMutable()->pid[PID_ROLL].P);
// messages to show. break;
const char *messages[5]; case ADJUSTMENT_PITCH_ROLL_I:
unsigned messageCount = 0; tfp_sprintf(buff, "PRI %3d %3d", pidBankMutable()->pid[PID_PITCH].I, pidBankMutable()->pid[PID_ROLL].I);
break;
if (FLIGHT_MODE(FAILSAFE_MODE)) { case ADJUSTMENT_PITCH_I:
// In FS mode while being armed too tfp_sprintf(buff, "PI %3d", pidBankMutable()->pid[PID_PITCH].I);
const char *failsafePhaseMessage = osdFailsafePhaseMessage(); break;
const char *failsafeInfoMessage = osdFailsafeInfoMessage(); case ADJUSTMENT_ROLL_I:
const char *navStateFSMessage = navigationStateMessage(); tfp_sprintf(buff, "RI %3d", pidBankMutable()->pid[PID_ROLL].I);
break;
if (failsafePhaseMessage) { case ADJUSTMENT_PITCH_ROLL_D:
messages[messageCount++] = failsafePhaseMessage; tfp_sprintf(buff, "PRD %3d %3d", pidBankMutable()->pid[PID_PITCH].D, pidBankMutable()->pid[PID_ROLL].D);
} break;
case ADJUSTMENT_PITCH_ROLL_FF:
if (failsafeInfoMessage) { tfp_sprintf(buff, "PRFF %3d %3d", pidBankMutable()->pid[PID_PITCH].FF, pidBankMutable()->pid[PID_ROLL].FF);
messages[messageCount++] = failsafeInfoMessage; break;
} case ADJUSTMENT_PITCH_D:
tfp_sprintf(buff, "PD %3d", pidBankMutable()->pid[PID_PITCH].D);
if (navStateFSMessage) { break;
messages[messageCount++] = navStateFSMessage; case ADJUSTMENT_PITCH_FF:
} tfp_sprintf(buff, "PFF %3d", pidBankMutable()->pid[PID_PITCH].FF);
break;
if (messageCount > 0) { case ADJUSTMENT_ROLL_D:
message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)]; tfp_sprintf(buff, "RD %3d", pidBankMutable()->pid[PID_ROLL].D);
if (message == failsafeInfoMessage) { break;
// failsafeInfoMessage is not useful for recovering case ADJUSTMENT_ROLL_FF:
// a lost model, but might help avoiding a crash. tfp_sprintf(buff, "RFF %3d", pidBankMutable()->pid[PID_ROLL].FF);
// Blink to grab user attention. break;
//doesnt work TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); case ADJUSTMENT_YAW_P:
} tfp_sprintf(buff, "YP %3d", pidBankMutable()->pid[PID_YAW].P);
// We're shoing either failsafePhaseMessage or break;
// navStateFSMessage. Don't BLINK here since case ADJUSTMENT_YAW_I:
// having this text available might be crucial tfp_sprintf(buff, "YI %3d", pidBankMutable()->pid[PID_YAW].I);
// during a lost aircraft recovery and blinking break;
// will cause it to be missing from some frames. case ADJUSTMENT_YAW_D:
} tfp_sprintf(buff, "YD %3d", pidBankMutable()->pid[PID_YAW].D);
} break;
else { case ADJUSTMENT_YAW_FF:
if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) { tfp_sprintf(buff, "YFF %3d", pidBankMutable()->pid[PID_YAW].FF);
const char *navStateMessage = navigationStateMessage(); break;
if (navStateMessage) { case ADJUSTMENT_NAV_FW_CRUISE_THR:
messages[messageCount++] = navStateMessage; tfp_sprintf(buff, "CR %4d", currentBatteryProfileMutable->nav.fw.cruise_throttle);
} break;
} case ADJUSTMENT_NAV_FW_PITCH2THR:
else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) { tfp_sprintf(buff, "P2T %3d", currentBatteryProfileMutable->nav.fw.pitch_to_throttle);
messages[messageCount++] = "AUTOLAUNCH"; break;
} case ADJUSTMENT_ROLL_BOARD_ALIGNMENT:
else { tfp_sprintf(buff, "RBA %3d", boardAlignment()->rollDeciDegrees);
if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) { break;
// ALTHOLD might be enabled alongside ANGLE/HORIZON/ACRO case ADJUSTMENT_PITCH_BOARD_ALIGNMENT:
// when it doesn't require ANGLE mode (required only in FW tfp_sprintf(buff, "PBA %3d", boardAlignment()->pitchDeciDegrees);
// right now). If if requires ANGLE, its display is handled break;
// by OSD_FLYMODE. case ADJUSTMENT_LEVEL_P:
messages[messageCount++] = "(ALT HOLD)"; tfp_sprintf(buff, "LP %3d", pidBankMutable()->pid[PID_LEVEL].P);
} break;
if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM)) { case ADJUSTMENT_LEVEL_I:
messages[messageCount++] = "(AUTOTRIM)"; tfp_sprintf(buff, "LI %3d", pidBankMutable()->pid[PID_LEVEL].I);
} break;
if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) { case ADJUSTMENT_LEVEL_D:
messages[messageCount++] = "(AUTOTUNE)"; tfp_sprintf(buff, "LD %3d", pidBankMutable()->pid[PID_LEVEL].D);
} break;
if (FLIGHT_MODE(HEADFREE_MODE)) { case ADJUSTMENT_POS_XY_P:
messages[messageCount++] = "(HEADFREE)"; tfp_sprintf(buff, "PXYP %3d", pidBankMutable()->pid[PID_POS_XY].P);
} break;
} case ADJUSTMENT_POS_XY_I:
// Pick one of the available messages. Each message lasts tfp_sprintf(buff, "PXYI %3d", pidBankMutable()->pid[PID_POS_XY].I);
// a second. break;
if (messageCount > 0) { case ADJUSTMENT_POS_XY_D:
message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)]; tfp_sprintf(buff, "PXYD %3d", pidBankMutable()->pid[PID_POS_XY].D);
} break;
} case ADJUSTMENT_POS_Z_P:
} tfp_sprintf(buff, "PZP %3d", pidBankMutable()->pid[PID_POS_Z].P);
else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) { break;
unsigned invalidIndex; case ADJUSTMENT_POS_Z_I:
// Check if we're unable to arm for some reason tfp_sprintf(buff, "PZI %3d", pidBankMutable()->pid[PID_POS_Z].I);
if (ARMING_FLAG(ARMING_DISABLED_INVALID_SETTING) && !settingsValidate(&invalidIndex)) { break;
if (OSD_ALTERNATING_CHOICES(1000, 2) == 0) { case ADJUSTMENT_POS_Z_D:
const setting_t *setting = settingGet(invalidIndex); tfp_sprintf(buff, "PZD %3d", pidBankMutable()->pid[PID_POS_Z].D);
settingGetName(setting, messageBuf); break;
for (int ii = 0; messageBuf[ii]; ii++) { case ADJUSTMENT_HEADING_P:
messageBuf[ii] = sl_toupper(messageBuf[ii]); tfp_sprintf(buff, "HP %3d", pidBankMutable()->pid[PID_HEADING].P);
} break;
message = messageBuf; case ADJUSTMENT_VEL_XY_P:
} tfp_sprintf(buff, "VXYP %3d", pidBankMutable()->pid[PID_VEL_XY].P);
else { break;
message = "ERR SETTING"; case ADJUSTMENT_VEL_XY_I:
// TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr); tfp_sprintf(buff, "VXYI %3d", pidBankMutable()->pid[PID_VEL_XY].I);
} break;
} case ADJUSTMENT_VEL_XY_D:
else { tfp_sprintf(buff, "VXYD %3d", pidBankMutable()->pid[PID_VEL_XY].D);
if (OSD_ALTERNATING_CHOICES(1000, 2) == 0) { break;
message = "CANT ARM"; case ADJUSTMENT_VEL_Z_P:
// TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr); tfp_sprintf(buff, "VZP %3d", pidBankMutable()->pid[PID_VEL_Z].P);
} else { break;
// Show the reason for not arming case ADJUSTMENT_VEL_Z_I:
message = osdArmingDisabledReasonMessage(); tfp_sprintf(buff, "VZI %3d", pidBankMutable()->pid[PID_VEL_Z].I);
} break;
} case ADJUSTMENT_VEL_Z_D:
} tfp_sprintf(buff, "VZD %3d", pidBankMutable()->pid[PID_VEL_Z].D);
} break;
case ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE:
if (message[0] != '\0') { tfp_sprintf(buff, "MTDPA %4d", currentBatteryProfileMutable->fwMinThrottleDownPitchAngle);
sbufWriteData(dst, message, strlen(message)); break;
case ADJUSTMENT_TPA:
tfp_sprintf(buff, "TPA %3d", currentControlRateProfile->throttle.dynPID);
break;
case ADJUSTMENT_TPA_BREAKPOINT:
tfp_sprintf(buff, "TPABP %4d", currentControlRateProfile->throttle.pa_breakpoint);
break;
case ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS:
tfp_sprintf(buff, "CSM %3d", navConfigMutable()->fw.control_smoothness);
break;
default:
tfp_sprintf(buff, "UNSUPPORTED");
break;
} }
} }
static bool osdDJIFormatAdjustments(char *buff)
{
uint8_t adjustmentFunctions[MAX_SIMULTANEOUS_ADJUSTMENT_COUNT];
uint8_t adjustmentCount = getActiveAdjustmentFunctions(adjustmentFunctions);
if (adjustmentCount > 0 && buff != NULL) {
osdDJIAdjustmentMessage(buff, adjustmentFunctions[OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_LONG, adjustmentCount)]);
}
return adjustmentCount > 0;
}
static bool djiFormatMessages(char *buff)
{
bool haveMessage = false;
char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH+1)];
if (ARMING_FLAG(ARMED)) {
// Aircraft is armed. We might have up to 6
// messages to show.
char *messages[6];
unsigned messageCount = 0;
if (FLIGHT_MODE(FAILSAFE_MODE)) {
// In FS mode while being armed too
char *failsafePhaseMessage = osdFailsafePhaseMessage();
char *failsafeInfoMessage = osdFailsafeInfoMessage();
char *navStateFSMessage = navigationStateMessage();
if (failsafePhaseMessage) {
messages[messageCount++] = failsafePhaseMessage;
}
if (failsafeInfoMessage) {
messages[messageCount++] = failsafeInfoMessage;
}
if (navStateFSMessage) {
messages[messageCount++] = navStateFSMessage;
}
} else {
#ifdef USE_SERIALRX_CRSF
if (djiOsdConfig()->rssi_source == DJI_CRSF_LQ && rxLinkStatistics.rfMode == 0) {
messages[messageCount++] = "CRSF LOW RF";
}
#endif
if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) {
char *navStateMessage = navigationStateMessage();
if (navStateMessage) {
messages[messageCount++] = navStateMessage;
}
} else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) {
messages[messageCount++] = "AUTOLAUNCH";
} else {
if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) {
// ALTHOLD might be enabled alongside ANGLE/HORIZON/ACRO
// when it doesn't require ANGLE mode (required only in FW
// right now). If if requires ANGLE, its display is handled
// by OSD_FLYMODE.
messages[messageCount++] = "(ALT HOLD)";
}
if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM) && !feature(FEATURE_FW_AUTOTRIM)) {
messages[messageCount++] = "(AUTOTRIM)";
}
if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) {
messages[messageCount++] = "(AUTOTUNE)";
}
if (IS_RC_MODE_ACTIVE(BOXAUTOLEVEL)) {
messages[messageCount++] = "(AUTOLEVEL)";
}
if (FLIGHT_MODE(HEADFREE_MODE)) {
messages[messageCount++] = "(HEADFREE)";
}
if (FLIGHT_MODE(MANUAL_MODE)) {
messages[messageCount++] = "(MANUAL)";
}
}
}
// Pick one of the available messages. Each message lasts
// a second.
if (messageCount > 0) {
strcpy(buff, messages[OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_SHORT, messageCount)]);;
haveMessage = true;
}
} else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) {
unsigned invalidIndex;
// Check if we're unable to arm for some reason
if (ARMING_FLAG(ARMING_DISABLED_INVALID_SETTING) && !settingsValidate(&invalidIndex)) {
if (OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_SHORT, 2) == 0) {
const setting_t *setting = settingGet(invalidIndex);
settingGetName(setting, messageBuf);
for (int ii = 0; messageBuf[ii]; ii++) {
messageBuf[ii] = sl_toupper(messageBuf[ii]);
}
strcpy(buff, messageBuf);
} else {
strcpy(buff, "ERR SETTING");
// TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr);
}
} else {
if (OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_SHORT, 2) == 0) {
strcpy(buff, "CANT ARM");
// TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr);
} else {
// Show the reason for not arming
strcpy(buff, osdArmingDisabledReasonMessage());
}
}
haveMessage = true;
}
return haveMessage;
}
static void djiSerializeCraftNameOverride(sbuf_t *dst)
{
char djibuf[DJI_CRAFT_NAME_LENGTH] = "\0";
uint16_t *osdLayoutConfig = (uint16_t*)(osdLayoutsConfig()->item_pos[0]);
if (!(OSD_VISIBLE(osdLayoutConfig[OSD_MESSAGES]) && djiFormatMessages(djibuf))
&& !(djiOsdConfig()->useAdjustments && osdDJIFormatAdjustments(djibuf))) {
DjiCraftNameElements_t activeElements[DJI_OSD_CN_MAX_ELEMENTS];
uint8_t activeElementsCount = 0;
if (OSD_VISIBLE(osdLayoutConfig[OSD_THROTTLE_POS])) {
activeElements[activeElementsCount++] = DJI_OSD_CN_THROTTLE;
}
if (OSD_VISIBLE(osdLayoutConfig[OSD_THROTTLE_POS_AUTO_THR])) {
activeElements[activeElementsCount++] = DJI_OSD_CN_THROTTLE_AUTO_THR;
}
if (OSD_VISIBLE(osdLayoutConfig[OSD_3D_SPEED])) {
activeElements[activeElementsCount++] = DJI_OSD_CN_AIR_SPEED;
}
if (OSD_VISIBLE(osdLayoutConfig[OSD_EFFICIENCY_MAH_PER_KM])) {
activeElements[activeElementsCount++] = DJI_OSD_CN_EFFICIENCY;
}
if (OSD_VISIBLE(osdLayoutConfig[OSD_TRIP_DIST])) {
activeElements[activeElementsCount++] = DJI_OSD_CN_DISTANCE;
}
switch (activeElements[OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_LONG, activeElementsCount)])
{
case DJI_OSD_CN_THROTTLE:
osdDJIFormatThrottlePosition(djibuf, false);
break;
case DJI_OSD_CN_THROTTLE_AUTO_THR:
osdDJIFormatThrottlePosition(djibuf, true);
break;
case DJI_OSD_CN_AIR_SPEED:
osdDJIFormatVelocityStr(djibuf);
break;
case DJI_OSD_CN_EFFICIENCY:
osdDJIEfficiencyMahPerKM(djibuf);
break;
case DJI_OSD_CN_DISTANCE:
osdDJIFormatDistanceStr(djibuf, getTotalTravelDistance());
break;
default:
break;
}
}
if (djibuf[0] != '\0') {
sbufWriteData(dst, djibuf, strlen(djibuf));
}
}
#endif #endif
@ -931,26 +1160,17 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms
case DJI_MSP_NAME: case DJI_MSP_NAME:
{ {
const char * name = systemConfig()->name;
#if defined(USE_OSD) #if defined(USE_OSD)
if (djiOsdConfig()->use_name_for_messages) { if (djiOsdConfig()->use_name_for_messages) {
if (name[0] == ':') { djiSerializeCraftNameOverride(dst);
// If craft name starts with a semicolon - use it as a template for what we want to show } else {
djiSerializeCraftNameOverride(dst, name);
}
else {
// Otherwise fall back to just warnings
djiSerializeCraftNameOverride(dst, ":W");
}
}
else
#endif #endif
{ sbufWriteData(dst, systemConfig()->name, MAX((int)strlen(systemConfig()->name), OSD_MESSAGE_LENGTH));
int len = strlen(name); #if defined(USE_OSD)
sbufWriteData(dst, name, MAX(len, 12));
break;
} }
#endif
break;
} }
break; break;
@ -1027,7 +1247,22 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms
case DJI_MSP_ANALOG: case DJI_MSP_ANALOG:
sbufWriteU8(dst, constrain(getBatteryVoltage() / 10, 0, 255)); sbufWriteU8(dst, constrain(getBatteryVoltage() / 10, 0, 255));
sbufWriteU16(dst, constrain(getMAhDrawn(), 0, 0xFFFF)); // milliamp hours drawn from battery sbufWriteU16(dst, constrain(getMAhDrawn(), 0, 0xFFFF)); // milliamp hours drawn from battery
sbufWriteU16(dst, getRSSI()); #ifdef USE_SERIALRX_CRSF
// Range of RSSI field: 0-99: 99 = 150 hz , 0 - 98 50 hz / 4 hz
if (djiOsdConfig()->rssi_source == DJI_CRSF_LQ) {
uint16_t scaledLq = 0;
if (rxLinkStatistics.rfMode >= 2) {
scaledLq = RSSI_MAX_VALUE;
} else {
scaledLq = scaleRange(constrain(rxLinkStatistics.uplinkLQ, 0, 100), 0, 100, 0, RSSI_BOUNDARY(98));
}
sbufWriteU16(dst, scaledLq);
} else {
#endif
sbufWriteU16(dst, getRSSI());
#ifdef USE_SERIALRX_CRSF
}
#endif
sbufWriteU16(dst, constrain(getAmperage(), -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A sbufWriteU16(dst, constrain(getAmperage(), -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A
sbufWriteU16(dst, getBatteryVoltage()); sbufWriteU16(dst, getBatteryVoltage());
break; break;

13
src/main/io/osd_dji_hd.h
View file

@ -62,12 +62,21 @@
#define DJI_MSP_SET_PID 202 #define DJI_MSP_SET_PID 202
#define DJI_MSP_SET_RC_TUNING 204 #define DJI_MSP_SET_RC_TUNING 204
#define DJI_CRAFT_NAME_LENGTH 24
#define DJI_ALTERNATING_DURATION_LONG (djiOsdConfig()->craftNameAlternatingDuration * 100)
#define DJI_ALTERNATING_DURATION_SHORT 1000
enum djiOsdTempSource_e { enum djiOsdTempSource_e {
DJI_OSD_TEMP_ESC = 0, DJI_OSD_TEMP_ESC = 0,
DJI_OSD_TEMP_CORE = 1, DJI_OSD_TEMP_CORE = 1,
DJI_OSD_TEMP_BARO = 2 DJI_OSD_TEMP_BARO = 2
}; };
enum djiRssiSource_e {
DJI_RSSI = 0,
DJI_CRSF_LQ = 1
};
enum djiOsdProtoWorkarounds_e { enum djiOsdProtoWorkarounds_e {
DJI_OSD_USE_NON_STANDARD_MSP_ESC_SENSOR_DATA = 1 << 0, DJI_OSD_USE_NON_STANDARD_MSP_ESC_SENSOR_DATA = 1 << 0,
}; };
@ -76,6 +85,10 @@ typedef struct djiOsdConfig_s {
uint8_t use_name_for_messages; uint8_t use_name_for_messages;
uint8_t esc_temperature_source; uint8_t esc_temperature_source;
uint8_t proto_workarounds; uint8_t proto_workarounds;
uint8_t messageSpeedSource;
uint8_t rssi_source;
uint8_t useAdjustments;
uint8_t craftNameAlternatingDuration;
} djiOsdConfig_t; } djiOsdConfig_t;
PG_DECLARE(djiOsdConfig_t, djiOsdConfig); PG_DECLARE(djiOsdConfig_t, djiOsdConfig);

48
src/main/navigation/navigation.c
View file

@ -96,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, 0); PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0);
#endif #endif
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 12); PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 13);
PG_RESET_TEMPLATE(navConfig_t, navConfig, PG_RESET_TEMPLATE(navConfig_t, navConfig,
.general = { .general = {
@ -142,7 +142,6 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
// MC-specific // MC-specific
.mc = { .mc = {
.max_bank_angle = SETTING_NAV_MC_BANK_ANGLE_DEFAULT, // degrees .max_bank_angle = SETTING_NAV_MC_BANK_ANGLE_DEFAULT, // degrees
.hover_throttle = SETTING_NAV_MC_HOVER_THR_DEFAULT,
.auto_disarm_delay = SETTING_NAV_MC_AUTO_DISARM_DELAY_DEFAULT, // milliseconds - time before disarming when auto disarm is enabled and landing is confirmed .auto_disarm_delay = SETTING_NAV_MC_AUTO_DISARM_DELAY_DEFAULT, // milliseconds - time before disarming when auto disarm is enabled and landing is confirmed
#ifdef USE_MR_BRAKING_MODE #ifdef USE_MR_BRAKING_MODE
@ -166,12 +165,8 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.max_bank_angle = SETTING_NAV_FW_BANK_ANGLE_DEFAULT, // degrees .max_bank_angle = SETTING_NAV_FW_BANK_ANGLE_DEFAULT, // degrees
.max_climb_angle = SETTING_NAV_FW_CLIMB_ANGLE_DEFAULT, // degrees .max_climb_angle = SETTING_NAV_FW_CLIMB_ANGLE_DEFAULT, // degrees
.max_dive_angle = SETTING_NAV_FW_DIVE_ANGLE_DEFAULT, // degrees .max_dive_angle = SETTING_NAV_FW_DIVE_ANGLE_DEFAULT, // degrees
.cruise_throttle = SETTING_NAV_FW_CRUISE_THR_DEFAULT,
.cruise_speed = SETTING_NAV_FW_CRUISE_SPEED_DEFAULT, // cm/s .cruise_speed = SETTING_NAV_FW_CRUISE_SPEED_DEFAULT, // cm/s
.control_smoothness = SETTING_NAV_FW_CONTROL_SMOOTHNESS_DEFAULT, .control_smoothness = SETTING_NAV_FW_CONTROL_SMOOTHNESS_DEFAULT,
.max_throttle = SETTING_NAV_FW_MAX_THR_DEFAULT,
.min_throttle = SETTING_NAV_FW_MIN_THR_DEFAULT,
.pitch_to_throttle = SETTING_NAV_FW_PITCH2THR_DEFAULT, // pwm units per degree of pitch (10pwm units ~ 1% throttle)
.pitch_to_throttle_smooth = SETTING_NAV_FW_PITCH2THR_SMOOTHING_DEFAULT, .pitch_to_throttle_smooth = SETTING_NAV_FW_PITCH2THR_SMOOTHING_DEFAULT,
.pitch_to_throttle_thresh = SETTING_NAV_FW_PITCH2THR_THRESHOLD_DEFAULT, .pitch_to_throttle_thresh = SETTING_NAV_FW_PITCH2THR_THRESHOLD_DEFAULT,
.loiter_radius = SETTING_NAV_FW_LOITER_RADIUS_DEFAULT, // 75m .loiter_radius = SETTING_NAV_FW_LOITER_RADIUS_DEFAULT, // 75m
@ -183,8 +178,6 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.launch_velocity_thresh = SETTING_NAV_FW_LAUNCH_VELOCITY_DEFAULT, // 3 m/s .launch_velocity_thresh = SETTING_NAV_FW_LAUNCH_VELOCITY_DEFAULT, // 3 m/s
.launch_accel_thresh = SETTING_NAV_FW_LAUNCH_ACCEL_DEFAULT, // cm/s/s (1.9*G) .launch_accel_thresh = SETTING_NAV_FW_LAUNCH_ACCEL_DEFAULT, // cm/s/s (1.9*G)
.launch_time_thresh = SETTING_NAV_FW_LAUNCH_DETECT_TIME_DEFAULT, // 40ms .launch_time_thresh = SETTING_NAV_FW_LAUNCH_DETECT_TIME_DEFAULT, // 40ms
.launch_throttle = SETTING_NAV_FW_LAUNCH_THR_DEFAULT,
.launch_idle_throttle = SETTING_NAV_FW_LAUNCH_IDLE_THR_DEFAULT, // Motor idle or MOTOR_STOP
.launch_motor_timer = SETTING_NAV_FW_LAUNCH_MOTOR_DELAY_DEFAULT, // ms .launch_motor_timer = SETTING_NAV_FW_LAUNCH_MOTOR_DELAY_DEFAULT, // ms
.launch_idle_motor_timer = SETTING_NAV_FW_LAUNCH_IDLE_MOTOR_DELAY_DEFAULT, // ms .launch_idle_motor_timer = SETTING_NAV_FW_LAUNCH_IDLE_MOTOR_DELAY_DEFAULT, // ms
.launch_motor_spinup_time = SETTING_NAV_FW_LAUNCH_SPINUP_TIME_DEFAULT, // ms, time to gredually increase throttle from idle to launch .launch_motor_spinup_time = SETTING_NAV_FW_LAUNCH_SPINUP_TIME_DEFAULT, // ms, time to gredually increase throttle from idle to launch
@ -1042,7 +1035,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS
timeMs_t timeDifference = currentTimeMs - posControl.cruise.lastYawAdjustmentTime; timeMs_t timeDifference = currentTimeMs - posControl.cruise.lastYawAdjustmentTime;
if (timeDifference > 100) timeDifference = 0; // if adjustment was called long time ago, reset the time difference. 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)rcCommand[YAW], -500.0f, 500.0f, -DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate), DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate));
float centidegsPerIteration = rateTarget * timeDifference / 1000.0f; float centidegsPerIteration = rateTarget * timeDifference * 0.001f;
posControl.cruise.yaw = wrap_36000(posControl.cruise.yaw - centidegsPerIteration); posControl.cruise.yaw = wrap_36000(posControl.cruise.yaw - centidegsPerIteration);
DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.yaw)); DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.yaw));
posControl.cruise.lastYawAdjustmentTime = currentTimeMs; posControl.cruise.lastYawAdjustmentTime = currentTimeMs;
@ -1934,7 +1927,7 @@ void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelVali
posControl.actualState.agl.vel.x = newVelX; posControl.actualState.agl.vel.x = newVelX;
posControl.actualState.agl.vel.y = newVelY; posControl.actualState.agl.vel.y = newVelY;
posControl.actualState.velXY = sqrtf(sq(newVelX) + sq(newVelY)); posControl.actualState.velXY = fast_fsqrtf(sq(newVelX) + sq(newVelY));
// CASE 1: POS & VEL valid // CASE 1: POS & VEL valid
if (estPosValid && estVelValid) { if (estPosValid && estVelValid) {
@ -2068,7 +2061,7 @@ const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void)
*-----------------------------------------------------------*/ *-----------------------------------------------------------*/
static uint32_t calculateDistanceFromDelta(float deltaX, float deltaY) static uint32_t calculateDistanceFromDelta(float deltaX, float deltaY)
{ {
return sqrtf(sq(deltaX) + sq(deltaY)); return fast_fsqrtf(sq(deltaX) + sq(deltaY));
} }
static int32_t calculateBearingFromDelta(float deltaX, float deltaY) static int32_t calculateBearingFromDelta(float deltaX, float deltaY)
@ -2145,8 +2138,8 @@ bool isWaypointReached(const navWaypointPosition_t * waypoint, const bool isWayp
static void updateHomePositionCompatibility(void) static void updateHomePositionCompatibility(void)
{ {
geoConvertLocalToGeodetic(&GPS_home, &posControl.gpsOrigin, &posControl.rthState.homePosition.pos); geoConvertLocalToGeodetic(&GPS_home, &posControl.gpsOrigin, &posControl.rthState.homePosition.pos);
GPS_distanceToHome = posControl.homeDistance / 100; GPS_distanceToHome = posControl.homeDistance * 0.01f;
GPS_directionToHome = posControl.homeDirection / 100; GPS_directionToHome = posControl.homeDirection * 0.01f;
} }
// Backdoor for RTH estimator // Backdoor for RTH estimator
@ -2933,8 +2926,15 @@ static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint
{ {
gpsLocation_t wpLLH; gpsLocation_t wpLLH;
wpLLH.lat = waypoint->lat; /* Default to home position if lat & lon = 0 or HOME flag set
wpLLH.lon = waypoint->lon; * Applicable to WAYPOINT, HOLD_TIME & LANDING WP types */
if ((waypoint->lat == 0 && waypoint->lon == 0) || waypoint->flag == NAV_WP_FLAG_HOME) {
wpLLH.lat = GPS_home.lat;
wpLLH.lon = GPS_home.lon;
} else {
wpLLH.lat = waypoint->lat;
wpLLH.lon = waypoint->lon;
}
wpLLH.alt = waypoint->alt; wpLLH.alt = waypoint->alt;
geoConvertGeodeticToLocal(localPos, &posControl.gpsOrigin, &wpLLH, altConv); geoConvertGeodeticToLocal(localPos, &posControl.gpsOrigin, &wpLLH, altConv);
@ -3339,6 +3339,13 @@ bool navigationTerrainFollowingEnabled(void)
return posControl.flags.isTerrainFollowEnabled; return posControl.flags.isTerrainFollowEnabled;
} }
uint32_t distanceToFirstWP(void)
{
fpVector3_t startingWaypointPos;
mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[0], GEO_ALT_RELATIVE);
return calculateDistanceToDestination(&startingWaypointPos);
}
navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass) navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
{ {
const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) || (STATE(FIXED_WING_LEGACY) && (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCRUISE))); const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) || (STATE(FIXED_WING_LEGACY) && (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCRUISE)));
@ -3355,7 +3362,7 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
// Apply extra arming safety only if pilot has any of GPS modes configured // Apply extra arming safety only if pilot has any of GPS modes configured
if ((isUsingNavigationModes() || failsafeMayRequireNavigationMode()) && !((posControl.flags.estPosStatus >= EST_USABLE) && STATE(GPS_FIX_HOME))) { if ((isUsingNavigationModes() || failsafeMayRequireNavigationMode()) && !((posControl.flags.estPosStatus >= EST_USABLE) && STATE(GPS_FIX_HOME))) {
if (navConfig()->general.flags.extra_arming_safety == NAV_EXTRA_ARMING_SAFETY_ALLOW_BYPASS && if (navConfig()->general.flags.extra_arming_safety == NAV_EXTRA_ARMING_SAFETY_ALLOW_BYPASS &&
(STATE(NAV_EXTRA_ARMING_SAFETY_BYPASSED) || rxGetChannelValue(YAW) > 1750)) { (STATE(NAV_EXTRA_ARMING_SAFETY_BYPASSED) || checkStickPosition(YAW_HI))) {
if (usedBypass) { if (usedBypass) {
*usedBypass = true; *usedBypass = true;
} }
@ -3371,12 +3378,7 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
// Don't allow arming if first waypoint is farther than configured safe distance // Don't allow arming if first waypoint is farther than configured safe distance
if ((posControl.waypointCount > 0) && (navConfig()->general.waypoint_safe_distance != 0)) { if ((posControl.waypointCount > 0) && (navConfig()->general.waypoint_safe_distance != 0)) {
fpVector3_t startingWaypointPos; if (distanceToFirstWP() > navConfig()->general.waypoint_safe_distance && !checkStickPosition(YAW_HI)) {
mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[0], GEO_ALT_RELATIVE);
const bool navWpMissionStartTooFar = calculateDistanceToDestination(&startingWaypointPos) > navConfig()->general.waypoint_safe_distance;
if (navWpMissionStartTooFar) {
return NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR; return NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR;
} }
} }
@ -3742,7 +3744,7 @@ static void GPS_distance_cm_bearing(int32_t currentLat1, int32_t currentLon1, in
const float dLat = destinationLat2 - currentLat1; // difference of latitude in 1/10 000 000 degrees const float dLat = destinationLat2 - currentLat1; // difference of latitude in 1/10 000 000 degrees
const float dLon = (float)(destinationLon2 - currentLon1) * GPS_scaleLonDown; const float dLon = (float)(destinationLon2 - currentLon1) * GPS_scaleLonDown;
*dist = sqrtf(sq(dLat) + sq(dLon)) * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR; *dist = fast_fsqrtf(sq(dLat) + sq(dLon)) * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR;
*bearing = 9000.0f + RADIANS_TO_CENTIDEGREES(atan2_approx(-dLat, dLon)); // Convert the output radians to 100xdeg *bearing = 9000.0f + RADIANS_TO_CENTIDEGREES(atan2_approx(-dLat, dLon)); // Convert the output radians to 100xdeg

9
src/main/navigation/navigation.h
View file

@ -225,7 +225,6 @@ typedef struct navConfig_s {
struct { struct {
uint8_t max_bank_angle; // multicopter max banking angle (deg) uint8_t max_bank_angle; // multicopter max banking angle (deg)
uint16_t hover_throttle; // multicopter hover throttle
uint16_t auto_disarm_delay; // multicopter safety delay for landing detector uint16_t auto_disarm_delay; // multicopter safety delay for landing detector
#ifdef USE_MR_BRAKING_MODE #ifdef USE_MR_BRAKING_MODE
@ -248,12 +247,8 @@ typedef struct navConfig_s {
uint8_t max_bank_angle; // Fixed wing max banking angle (deg) uint8_t max_bank_angle; // Fixed wing max banking angle (deg)
uint8_t max_climb_angle; // Fixed wing max banking angle (deg) uint8_t max_climb_angle; // Fixed wing max banking angle (deg)
uint8_t max_dive_angle; // Fixed wing max banking angle (deg) uint8_t max_dive_angle; // Fixed wing max banking angle (deg)
uint16_t cruise_throttle; // Cruise throttle
uint16_t cruise_speed; // Speed at cruise throttle (cm/s), used for time/distance left before RTH uint16_t cruise_speed; // Speed at cruise throttle (cm/s), used for time/distance left before RTH
uint8_t control_smoothness; // The amount of smoothing to apply to controls for navigation uint8_t control_smoothness; // The amount of smoothing to apply to controls for navigation
uint16_t min_throttle; // Minimum allowed throttle in auto mode
uint16_t max_throttle; // Maximum allowed throttle in auto mode
uint8_t pitch_to_throttle; // Pitch angle (in deg) to throttle gain (in 1/1000's of throttle) (*10)
uint16_t pitch_to_throttle_smooth; // How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh. uint16_t pitch_to_throttle_smooth; // How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh.
uint8_t pitch_to_throttle_thresh; // Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees] uint8_t pitch_to_throttle_thresh; // Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees]
uint16_t loiter_radius; // Loiter radius when executing PH on a fixed wing uint16_t loiter_radius; // Loiter radius when executing PH on a fixed wing
@ -261,8 +256,6 @@ typedef struct navConfig_s {
uint16_t launch_velocity_thresh; // Velocity threshold for swing launch detection uint16_t launch_velocity_thresh; // Velocity threshold for swing launch detection
uint16_t launch_accel_thresh; // Acceleration threshold for launch detection (cm/s/s) uint16_t launch_accel_thresh; // Acceleration threshold for launch detection (cm/s/s)
uint16_t launch_time_thresh; // Time threshold for launch detection (ms) uint16_t launch_time_thresh; // Time threshold for launch detection (ms)
uint16_t launch_idle_throttle; // Throttle to keep at launch idle
uint16_t launch_throttle; // Launch throttle
uint16_t launch_motor_timer; // Time to wait before setting launch_throttle (ms) uint16_t launch_motor_timer; // Time to wait before setting launch_throttle (ms)
uint16_t launch_idle_motor_timer; // Time to wait before motor starts at_idle throttle (ms) uint16_t launch_idle_motor_timer; // Time to wait before motor starts at_idle throttle (ms)
uint16_t launch_motor_spinup_time; // Time to speed-up motors from idle to launch_throttle (ESC desync prevention) uint16_t launch_motor_spinup_time; // Time to speed-up motors from idle to launch_throttle (ESC desync prevention)
@ -306,6 +299,7 @@ typedef enum {
} navWaypointHeadings_e; } navWaypointHeadings_e;
typedef enum { typedef enum {
NAV_WP_FLAG_HOME = 0x48,
NAV_WP_FLAG_LAST = 0xA5 NAV_WP_FLAG_LAST = 0xA5
} navWaypointFlags_e; } navWaypointFlags_e;
@ -514,6 +508,7 @@ geoAltitudeConversionMode_e waypointMissionAltConvMode(geoAltitudeDatumFlag_e da
/* Distance/bearing calculation */ /* Distance/bearing calculation */
bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos); bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos);
uint32_t distanceToFirstWP(void);
/* Failsafe-forced RTH mode */ /* Failsafe-forced RTH mode */
void activateForcedRTH(void); void activateForcedRTH(void);

68
src/main/navigation/navigation_fixedwing.c
View file

@ -51,6 +51,8 @@
#include "rx/rx.h" #include "rx/rx.h"
#include "sensors/battery.h"
// Base frequencies for smoothing pitch and roll // Base frequencies for smoothing pitch and roll
#define NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ 2.0f #define NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ 2.0f
#define NAV_FW_BASE_ROLL_CUTOFF_FREQUENCY_HZ 10.0f #define NAV_FW_BASE_ROLL_CUTOFF_FREQUENCY_HZ 10.0f
@ -132,10 +134,10 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
// velocity error. We use PID controller on altitude error and calculate desired pitch angle // velocity error. We use PID controller on altitude error and calculate desired pitch angle
// Update energies // Update energies
const float demSPE = (posControl.desiredState.pos.z / 100.0f) * GRAVITY_MSS; const float demSPE = (posControl.desiredState.pos.z * 0.01f) * GRAVITY_MSS;
const float demSKE = 0.0f; const float demSKE = 0.0f;
const float estSPE = (navGetCurrentActualPositionAndVelocity()->pos.z / 100.0f) * GRAVITY_MSS; const float estSPE = (navGetCurrentActualPositionAndVelocity()->pos.z * 0.01f) * GRAVITY_MSS;
const float estSKE = 0.0f; const float estSKE = 0.0f;
// speedWeight controls balance between potential and kinetic energy used for pitch controller // speedWeight controls balance between potential and kinetic energy used for pitch controller
@ -234,13 +236,28 @@ void resetFixedWingPositionController(void)
static int8_t loiterDirection(void) { static int8_t loiterDirection(void) {
int8_t dir = 1; //NAV_LOITER_RIGHT int8_t dir = 1; //NAV_LOITER_RIGHT
if (pidProfile()->loiter_direction == NAV_LOITER_LEFT) dir = -1;
if (pidProfile()->loiter_direction == NAV_LOITER_LEFT) {
dir = -1;
}
if (pidProfile()->loiter_direction == NAV_LOITER_YAW) { if (pidProfile()->loiter_direction == NAV_LOITER_YAW) {
if (rcCommand[YAW] < -250) loiterDirYaw = 1; //RIGHT //yaw is contrariwise
if (rcCommand[YAW] > 250) loiterDirYaw = -1; //LEFT //see annexCode in fc_core.c if (rcCommand[YAW] < -250) {
loiterDirYaw = 1; //RIGHT //yaw is contrariwise
}
if (rcCommand[YAW] > 250) {
loiterDirYaw = -1; //LEFT //see annexCode in fc_core.c
}
dir = loiterDirYaw; dir = loiterDirYaw;
} }
if (IS_RC_MODE_ACTIVE(BOXLOITERDIRCHN)) dir *= -1;
if (IS_RC_MODE_ACTIVE(BOXLOITERDIRCHN)) {
dir *= -1;
}
return dir; return dir;
} }
@ -249,7 +266,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
float posErrorX = posControl.desiredState.pos.x - navGetCurrentActualPositionAndVelocity()->pos.x; float posErrorX = posControl.desiredState.pos.x - navGetCurrentActualPositionAndVelocity()->pos.x;
float posErrorY = posControl.desiredState.pos.y - navGetCurrentActualPositionAndVelocity()->pos.y; float posErrorY = posControl.desiredState.pos.y - navGetCurrentActualPositionAndVelocity()->pos.y;
float distanceToActualTarget = sqrtf(sq(posErrorX) + sq(posErrorY)); float distanceToActualTarget = fast_fsqrtf(sq(posErrorX) + sq(posErrorY));
// Limit minimum forward velocity to 1 m/s // Limit minimum forward velocity to 1 m/s
float trackingDistance = trackingPeriod * MAX(posControl.actualState.velXY, 100.0f); float trackingDistance = trackingPeriod * MAX(posControl.actualState.velXY, 100.0f);
@ -272,7 +289,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
// We have temporary loiter target. Recalculate distance and position error // We have temporary loiter target. Recalculate distance and position error
posErrorX = loiterTargetX - navGetCurrentActualPositionAndVelocity()->pos.x; posErrorX = loiterTargetX - navGetCurrentActualPositionAndVelocity()->pos.x;
posErrorY = loiterTargetY - navGetCurrentActualPositionAndVelocity()->pos.y; posErrorY = loiterTargetY - navGetCurrentActualPositionAndVelocity()->pos.y;
distanceToActualTarget = sqrtf(sq(posErrorX) + sq(posErrorY)); distanceToActualTarget = fast_fsqrtf(sq(posErrorX) + sq(posErrorY));
} }
// Calculate virtual waypoint // Calculate virtual waypoint
@ -316,7 +333,7 @@ float processHeadingYawController(timeDelta_t deltaMicros, int32_t navHeadingErr
-limit, -limit,
limit, limit,
yawPidFlags yawPidFlags
) / 100.0f; ) * 0.01f;
DEBUG_SET(DEBUG_NAV_YAW, 0, posControl.pids.fw_heading.proportional); DEBUG_SET(DEBUG_NAV_YAW, 0, posControl.pids.fw_heading.proportional);
DEBUG_SET(DEBUG_NAV_YAW, 1, posControl.pids.fw_heading.integral); DEBUG_SET(DEBUG_NAV_YAW, 1, posControl.pids.fw_heading.integral);
@ -483,18 +500,18 @@ int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs
if (ABS(pitch - filteredPitch) > navConfig()->fw.pitch_to_throttle_thresh) { if (ABS(pitch - filteredPitch) > navConfig()->fw.pitch_to_throttle_thresh) {
// Unfiltered throttle correction outside of pitch deadband // Unfiltered throttle correction outside of pitch deadband
return DECIDEGREES_TO_DEGREES(pitch) * navConfig()->fw.pitch_to_throttle; return DECIDEGREES_TO_DEGREES(pitch) * currentBatteryProfile->nav.fw.pitch_to_throttle;
} }
else { else {
// Filtered throttle correction inside of pitch deadband // Filtered throttle correction inside of pitch deadband
return DECIDEGREES_TO_DEGREES(filteredPitch) * navConfig()->fw.pitch_to_throttle; return DECIDEGREES_TO_DEGREES(filteredPitch) * currentBatteryProfile->nav.fw.pitch_to_throttle;
} }
} }
void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs) void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs)
{ {
int16_t minThrottleCorrection = navConfig()->fw.min_throttle - navConfig()->fw.cruise_throttle; int16_t minThrottleCorrection = currentBatteryProfile->nav.fw.min_throttle - currentBatteryProfile->nav.fw.cruise_throttle;
int16_t maxThrottleCorrection = navConfig()->fw.max_throttle - navConfig()->fw.cruise_throttle; int16_t maxThrottleCorrection = currentBatteryProfile->nav.fw.max_throttle - currentBatteryProfile->nav.fw.cruise_throttle;
if (isRollAdjustmentValid && (navStateFlags & NAV_CTL_POS)) { if (isRollAdjustmentValid && (navStateFlags & NAV_CTL_POS)) {
// ROLL >0 right, <0 left // ROLL >0 right, <0 left
@ -529,15 +546,16 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, maxThrottleCorrection); throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, maxThrottleCorrection);
} }
uint16_t correctedThrottleValue = constrain(navConfig()->fw.cruise_throttle + throttleCorrection, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle); uint16_t correctedThrottleValue = constrain(currentBatteryProfile->nav.fw.cruise_throttle + throttleCorrection, currentBatteryProfile->nav.fw.min_throttle, currentBatteryProfile->nav.fw.max_throttle);
// Manual throttle increase // Manual throttle increase
if (navConfig()->fw.allow_manual_thr_increase && !FLIGHT_MODE(FAILSAFE_MODE)) { if (navConfig()->fw.allow_manual_thr_increase && !FLIGHT_MODE(FAILSAFE_MODE)) {
if (rcCommand[THROTTLE] < PWM_RANGE_MIN + (PWM_RANGE_MAX - PWM_RANGE_MIN) * 0.95) if (rcCommand[THROTTLE] < PWM_RANGE_MIN + (PWM_RANGE_MAX - PWM_RANGE_MIN) * 0.95){
correctedThrottleValue += MAX(0, rcCommand[THROTTLE] - navConfig()->fw.cruise_throttle); correctedThrottleValue += MAX(0, rcCommand[THROTTLE] - currentBatteryProfile->nav.fw.cruise_throttle);
else } else {
correctedThrottleValue = motorConfig()->maxthrottle; correctedThrottleValue = motorConfig()->maxthrottle;
isAutoThrottleManuallyIncreased = (rcCommand[THROTTLE] > navConfig()->fw.cruise_throttle); }
isAutoThrottleManuallyIncreased = (rcCommand[THROTTLE] > currentBatteryProfile->nav.fw.cruise_throttle);
} else { } else {
isAutoThrottleManuallyIncreased = false; isAutoThrottleManuallyIncreased = false;
} }
@ -600,7 +618,7 @@ void applyFixedWingEmergencyLandingController(void)
rcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]); rcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]);
rcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]); rcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]);
rcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]); rcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]);
rcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle; rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
} }
/*----------------------------------------------------------- /*-----------------------------------------------------------
@ -637,24 +655,28 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags,
// Motor has been stopped by user. Update target altitude and bypass navigation pitch/throttle control // Motor has been stopped by user. Update target altitude and bypass navigation pitch/throttle control
resetFixedWingAltitudeController(); resetFixedWingAltitudeController();
setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z);
} else } else {
applyFixedWingAltitudeAndThrottleController(currentTimeUs); applyFixedWingAltitudeAndThrottleController(currentTimeUs);
}
} }
if (navStateFlags & NAV_CTL_POS) if (navStateFlags & NAV_CTL_POS) {
applyFixedWingPositionController(currentTimeUs); applyFixedWingPositionController(currentTimeUs);
}
} else { } else {
posControl.rcAdjustment[PITCH] = 0; posControl.rcAdjustment[PITCH] = 0;
posControl.rcAdjustment[ROLL] = 0; posControl.rcAdjustment[ROLL] = 0;
} }
if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && posControl.flags.isAdjustingPosition) if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && posControl.flags.isAdjustingPosition) {
rcCommand[ROLL] = applyDeadbandRescaled(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband, -500, 500); rcCommand[ROLL] = applyDeadbandRescaled(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband, -500, 500);
}
//if (navStateFlags & NAV_CTL_YAW) //if (navStateFlags & NAV_CTL_YAW)
if ((navStateFlags & NAV_CTL_ALT) || (navStateFlags & NAV_CTL_POS)) if ((navStateFlags & NAV_CTL_ALT) || (navStateFlags & NAV_CTL_POS)) {
applyFixedWingPitchRollThrottleController(navStateFlags, currentTimeUs); applyFixedWingPitchRollThrottleController(navStateFlags, currentTimeUs);
}
} }
} }

16
src/main/navigation/navigation_fw_launch.c
View file

@ -50,6 +50,8 @@
#include "io/gps.h" #include "io/gps.h"
#include "sensors/battery.h"
#define SWING_LAUNCH_MIN_ROTATION_RATE DEGREES_TO_RADIANS(100) // expect minimum 100dps rotation rate #define SWING_LAUNCH_MIN_ROTATION_RATE DEGREES_TO_RADIANS(100) // expect minimum 100dps rotation rate
#define LAUNCH_MOTOR_IDLE_SPINUP_TIME 1500 // ms #define LAUNCH_MOTOR_IDLE_SPINUP_TIME 1500 // ms
#define UNUSED(x) ((void)(x)) #define UNUSED(x) ((void)(x))
@ -225,13 +227,13 @@ static void setCurrentState(fixedWingLaunchState_t nextState, timeUs_t currentTi
static bool isThrottleIdleEnabled(void) static bool isThrottleIdleEnabled(void)
{ {
return navConfig()->fw.launch_idle_throttle > getThrottleIdleValue(); return currentBatteryProfile->nav.fw.launch_idle_throttle > getThrottleIdleValue();
} }
static void applyThrottleIdleLogic(bool forceMixerIdle) static void applyThrottleIdleLogic(bool forceMixerIdle)
{ {
if (isThrottleIdleEnabled() && !forceMixerIdle) { if (isThrottleIdleEnabled() && !forceMixerIdle) {
rcCommand[THROTTLE] = navConfig()->fw.launch_idle_throttle; rcCommand[THROTTLE] = currentBatteryProfile->nav.fw.launch_idle_throttle;
} }
else { else {
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped
@ -331,7 +333,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t
return FW_LAUNCH_EVENT_SUCCESS; return FW_LAUNCH_EVENT_SUCCESS;
} }
else { else {
rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle); rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, getThrottleIdleValue(), currentBatteryProfile->nav.fw.launch_idle_throttle);
fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, 0, navConfig()->fw.launch_climb_angle); fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, 0, navConfig()->fw.launch_climb_angle);
} }
@ -397,14 +399,14 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_
const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs); const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs);
const uint16_t motorSpinUpMs = navConfig()->fw.launch_motor_spinup_time; const uint16_t motorSpinUpMs = navConfig()->fw.launch_motor_spinup_time;
const uint16_t launchThrottle = navConfig()->fw.launch_throttle; const uint16_t launchThrottle = currentBatteryProfile->nav.fw.launch_throttle;
if (elapsedTimeMs > motorSpinUpMs) { if (elapsedTimeMs > motorSpinUpMs) {
rcCommand[THROTTLE] = launchThrottle; rcCommand[THROTTLE] = launchThrottle;
return FW_LAUNCH_EVENT_SUCCESS; return FW_LAUNCH_EVENT_SUCCESS;
} }
else { else {
const uint16_t minIdleThrottle = MAX(getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle); const uint16_t minIdleThrottle = MAX(getThrottleIdleValue(), currentBatteryProfile->nav.fw.launch_idle_throttle);
rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, motorSpinUpMs, minIdleThrottle, launchThrottle); rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, motorSpinUpMs, minIdleThrottle, launchThrottle);
} }
@ -413,7 +415,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t currentTimeUs) static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t currentTimeUs)
{ {
rcCommand[THROTTLE] = navConfig()->fw.launch_throttle; rcCommand[THROTTLE] = currentBatteryProfile->nav.fw.launch_throttle;
if (isLaunchMaxAltitudeReached()) { if (isLaunchMaxAltitudeReached()) {
return FW_LAUNCH_EVENT_SUCCESS; // cancel the launch and do the FW_LAUNCH_STATE_FINISH state return FW_LAUNCH_EVENT_SUCCESS; // cancel the launch and do the FW_LAUNCH_STATE_FINISH state
@ -443,7 +445,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t curr
} }
else { else {
// make a smooth transition from the launch state to the current state for throttle and the pitch angle // make a smooth transition from the launch state to the current state for throttle and the pitch angle
rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_throttle, rcCommand[THROTTLE]); rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, currentBatteryProfile->nav.fw.launch_throttle, rcCommand[THROTTLE]);
fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_climb_angle, rcCommand[PITCH]); fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_climb_angle, rcCommand[PITCH]);
} }

26
src/main/navigation/navigation_multicopter.c
View file

@ -51,6 +51,8 @@
#include "navigation/navigation.h" #include "navigation/navigation.h"
#include "navigation/navigation_private.h" #include "navigation/navigation_private.h"
#include "sensors/battery.h"
/*----------------------------------------------------------- /*-----------------------------------------------------------
* Altitude controller for multicopter aircraft * Altitude controller for multicopter aircraft
*-----------------------------------------------------------*/ *-----------------------------------------------------------*/
@ -104,8 +106,8 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros) static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros)
{ {
// Calculate min and max throttle boundaries (to compensate for integral windup) // Calculate min and max throttle boundaries (to compensate for integral windup)
const int16_t thrAdjustmentMin = (int16_t)getThrottleIdleValue() - (int16_t)navConfig()->mc.hover_throttle; const int16_t thrAdjustmentMin = (int16_t)getThrottleIdleValue() - (int16_t)currentBatteryProfile->nav.mc.hover_throttle;
const int16_t thrAdjustmentMax = (int16_t)motorConfig()->maxthrottle - (int16_t)navConfig()->mc.hover_throttle; const int16_t thrAdjustmentMax = (int16_t)motorConfig()->maxthrottle - (int16_t)currentBatteryProfile->nav.mc.hover_throttle;
posControl.rcAdjustment[THROTTLE] = navPidApply2(&posControl.pids.vel[Z], posControl.desiredState.vel.z, navGetCurrentActualPositionAndVelocity()->vel.z, US2S(deltaMicros), thrAdjustmentMin, thrAdjustmentMax, 0); posControl.rcAdjustment[THROTTLE] = navPidApply2(&posControl.pids.vel[Z], posControl.desiredState.vel.z, navGetCurrentActualPositionAndVelocity()->vel.z, US2S(deltaMicros), thrAdjustmentMin, thrAdjustmentMax, 0);
@ -239,7 +241,7 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs)
} }
// Update throttle controller // Update throttle controller
rcCommand[THROTTLE] = constrain((int16_t)navConfig()->mc.hover_throttle + posControl.rcAdjustment[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle); rcCommand[THROTTLE] = constrain((int16_t)currentBatteryProfile->nav.mc.hover_throttle + posControl.rcAdjustment[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle);
// Save processed throttle for future use // Save processed throttle for future use
rcCommandAdjustedThrottle = rcCommand[THROTTLE]; rcCommandAdjustedThrottle = rcCommand[THROTTLE];
@ -437,17 +439,17 @@ static void updatePositionVelocityController_MC(const float maxSpeed)
float newVelY = posErrorY * posControl.pids.pos[Y].param.kP; float newVelY = posErrorY * posControl.pids.pos[Y].param.kP;
// Scale velocity to respect max_speed // Scale velocity to respect max_speed
float newVelTotal = sqrtf(sq(newVelX) + sq(newVelY)); float newVelTotal = fast_fsqrtf(sq(newVelX) + sq(newVelY));
/* /*
* We override computed speed with max speed in following cases: * We override computed speed with max speed in following cases:
* 1 - computed velocity is > maxSpeed * 1 - computed velocity is > maxSpeed
* 2 - in WP mission when: slowDownForTurning is OFF, we do not fly towards na last waypoint and computed speed is < maxSpeed * 2 - in WP mission when: slowDownForTurning is OFF, we do not fly towards na last waypoint and computed speed is < maxSpeed
*/ */
if ( if (
(navGetCurrentStateFlags() & NAV_AUTO_WP && (navGetCurrentStateFlags() & NAV_AUTO_WP &&
!isApproachingLastWaypoint() && !isApproachingLastWaypoint() &&
newVelTotal < maxSpeed && newVelTotal < maxSpeed &&
!navConfig()->mc.slowDownForTurning !navConfig()->mc.slowDownForTurning
) || newVelTotal > maxSpeed ) || newVelTotal > maxSpeed
) { ) {
@ -497,7 +499,7 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
const float setpointX = posControl.desiredState.vel.x; const float setpointX = posControl.desiredState.vel.x;
const float setpointY = posControl.desiredState.vel.y; const float setpointY = posControl.desiredState.vel.y;
const float setpointXY = sqrtf(powf(setpointX, 2)+powf(setpointY, 2)); const float setpointXY = fast_fsqrtf(powf(setpointX, 2)+powf(setpointY, 2));
// Calculate velocity error // Calculate velocity error
const float velErrorX = setpointX - measurementX; const float velErrorX = setpointX - measurementX;
@ -505,7 +507,7 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
// Calculate XY-acceleration limit according to velocity error limit // Calculate XY-acceleration limit according to velocity error limit
float accelLimitX, accelLimitY; float accelLimitX, accelLimitY;
const float velErrorMagnitude = sqrtf(sq(velErrorX) + sq(velErrorY)); const float velErrorMagnitude = fast_fsqrtf(sq(velErrorX) + sq(velErrorY));
if (velErrorMagnitude > 0.1f) { if (velErrorMagnitude > 0.1f) {
accelLimitX = maxAccelLimit / velErrorMagnitude * fabsf(velErrorX); accelLimitX = maxAccelLimit / velErrorMagnitude * fabsf(velErrorX);
accelLimitY = maxAccelLimit / velErrorMagnitude * fabsf(velErrorY); accelLimitY = maxAccelLimit / velErrorMagnitude * fabsf(velErrorY);
@ -753,7 +755,7 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
rcCommand[THROTTLE] = getThrottleIdleValue(); rcCommand[THROTTLE] = getThrottleIdleValue();
} }
else { else {
rcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle; rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
} }
return; return;
@ -780,7 +782,7 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
} }
// Update throttle controller // Update throttle controller
rcCommand[THROTTLE] = constrain((int16_t)navConfig()->mc.hover_throttle + posControl.rcAdjustment[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle); rcCommand[THROTTLE] = constrain((int16_t)currentBatteryProfile->nav.mc.hover_throttle + posControl.rcAdjustment[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle);
} }
/*----------------------------------------------------------- /*-----------------------------------------------------------

4
src/main/navigation/navigation_pos_estimator.c
View file

@ -164,7 +164,7 @@ static bool detectGPSGlitch(timeUs_t currentTimeUs)
predictedGpsPosition.y = lastKnownGoodPosition.y + lastKnownGoodVelocity.y * dT; predictedGpsPosition.y = lastKnownGoodPosition.y + lastKnownGoodVelocity.y * dT;
/* New pos is within predefined radius of predicted pos, radius is expanded exponentially */ /* New pos is within predefined radius of predicted pos, radius is expanded exponentially */
gpsDistance = sqrtf(sq(predictedGpsPosition.x - lastKnownGoodPosition.x) + sq(predictedGpsPosition.y - lastKnownGoodPosition.y)); gpsDistance = fast_fsqrtf(sq(predictedGpsPosition.x - lastKnownGoodPosition.x) + sq(predictedGpsPosition.y - lastKnownGoodPosition.y));
if (gpsDistance <= (INAV_GPS_GLITCH_RADIUS + 0.5f * INAV_GPS_GLITCH_ACCEL * dT * dT)) { if (gpsDistance <= (INAV_GPS_GLITCH_RADIUS + 0.5f * INAV_GPS_GLITCH_ACCEL * dT * dT)) {
isGlitching = false; isGlitching = false;
} }
@ -640,7 +640,7 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx)
const float gpsPosYResidual = posEstimator.gps.pos.y - posEstimator.est.pos.y; const float gpsPosYResidual = posEstimator.gps.pos.y - posEstimator.est.pos.y;
const float gpsVelXResidual = posEstimator.gps.vel.x - posEstimator.est.vel.x; const float gpsVelXResidual = posEstimator.gps.vel.x - posEstimator.est.vel.x;
const float gpsVelYResidual = posEstimator.gps.vel.y - posEstimator.est.vel.y; const float gpsVelYResidual = posEstimator.gps.vel.y - posEstimator.est.vel.y;
const float gpsPosResidualMag = sqrtf(sq(gpsPosXResidual) + sq(gpsPosYResidual)); const float gpsPosResidualMag = fast_fsqrtf(sq(gpsPosXResidual) + sq(gpsPosYResidual));
//const float gpsWeightScaler = scaleRangef(bellCurve(gpsPosResidualMag, INAV_GPS_ACCEPTANCE_EPE), 0.0f, 1.0f, 0.1f, 1.0f); //const float gpsWeightScaler = scaleRangef(bellCurve(gpsPosResidualMag, INAV_GPS_ACCEPTANCE_EPE), 0.0f, 1.0f, 0.1f, 1.0f);
const float gpsWeightScaler = 1.0f; const float gpsWeightScaler = 1.0f;

2
src/main/navigation/navigation_pos_estimator_flow.c
View file

@ -109,7 +109,7 @@ bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx)
ctx->estPosCorr.x = flowResidualX * positionEstimationConfig()->w_xy_flow_p * ctx->dt; ctx->estPosCorr.x = flowResidualX * positionEstimationConfig()->w_xy_flow_p * ctx->dt;
ctx->estPosCorr.y = flowResidualY * positionEstimationConfig()->w_xy_flow_p * ctx->dt; ctx->estPosCorr.y = flowResidualY * positionEstimationConfig()->w_xy_flow_p * ctx->dt;
ctx->newEPH = updateEPE(posEstimator.est.eph, ctx->dt, sqrtf(sq(flowResidualX) + sq(flowResidualY)), positionEstimationConfig()->w_xy_flow_p); ctx->newEPH = updateEPE(posEstimator.est.eph, ctx->dt, fast_fsqrtf(sq(flowResidualX) + sq(flowResidualY)), positionEstimationConfig()->w_xy_flow_p);
} }
DEBUG_SET(DEBUG_FLOW, 0, RADIANS_TO_DEGREES(posEstimator.flow.flowRate[X])); DEBUG_SET(DEBUG_FLOW, 0, RADIANS_TO_DEGREES(posEstimator.flow.flowRate[X]));

10
src/main/navigation/navigation_rover_boat.c
View file

@ -29,13 +29,19 @@ FILE_COMPILE_FOR_SIZE
#ifdef USE_NAV #ifdef USE_NAV
#include "build/debug.h" #include "build/debug.h"
#include "common/utils.h" #include "common/utils.h"
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "fc/config.h" #include "fc/config.h"
#include "flight/mixer.h" #include "flight/mixer.h"
#include "navigation/navigation.h" #include "navigation/navigation.h"
#include "navigation/navigation_private.h" #include "navigation/navigation_private.h"
#include "sensors/battery.h"
static bool isYawAdjustmentValid = false; static bool isYawAdjustmentValid = false;
static int32_t navHeadingError; static int32_t navHeadingError;
@ -125,7 +131,7 @@ void applyRoverBoatPitchRollThrottleController(navigationFSMStateFlags_t navStat
rcCommand[YAW] = posControl.rcAdjustment[YAW]; rcCommand[YAW] = posControl.rcAdjustment[YAW];
} }
rcCommand[THROTTLE] = constrain(navConfig()->fw.cruise_throttle, motorConfig()->mincommand, motorConfig()->maxthrottle); rcCommand[THROTTLE] = constrain(currentBatteryProfile->nav.fw.cruise_throttle, motorConfig()->mincommand, motorConfig()->maxthrottle);
} }
} }
} }
@ -136,7 +142,7 @@ void applyRoverBoatNavigationController(navigationFSMStateFlags_t navStateFlags,
rcCommand[ROLL] = 0; rcCommand[ROLL] = 0;
rcCommand[PITCH] = 0; rcCommand[PITCH] = 0;
rcCommand[YAW] = 0; rcCommand[YAW] = 0;
rcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle; rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
} else if (navStateFlags & NAV_CTL_POS) { } else if (navStateFlags & NAV_CTL_POS) {
applyRoverBoatPositionController(currentTimeUs); applyRoverBoatPositionController(currentTimeUs);
applyRoverBoatPitchRollThrottleController(navStateFlags, currentTimeUs); applyRoverBoatPitchRollThrottleController(navStateFlags, currentTimeUs);

2
src/main/programming/logic_condition.c
View file

@ -514,7 +514,7 @@ static int logicConditionGetFlightOperandValue(int operand) {
break; break;
case LOGIC_CONDITION_OPERAND_FLIGHT_3D_HOME_DISTANCE: //in m case LOGIC_CONDITION_OPERAND_FLIGHT_3D_HOME_DISTANCE: //in m
return constrain(sqrtf(sq(GPS_distanceToHome) + sq(getEstimatedActualPosition(Z)/100)), 0, INT16_MAX); return constrain(fast_fsqrtf(sq(GPS_distanceToHome) + sq(getEstimatedActualPosition(Z)/100)), 0, INT16_MAX);
break; break;
case LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_LQ: case LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_LQ:

10
src/main/sensors/acceleration.c
View file

@ -616,20 +616,20 @@ void updateAccExtremes(void)
if (acc.accADCf[axis] > acc.extremes[axis].max) acc.extremes[axis].max = acc.accADCf[axis]; if (acc.accADCf[axis] > acc.extremes[axis].max) acc.extremes[axis].max = acc.accADCf[axis];
} }
float gforce = sqrtf(sq(acc.accADCf[0]) + sq(acc.accADCf[1]) + sq(acc.accADCf[2])); float gforce = fast_fsqrtf(sq(acc.accADCf[0]) + sq(acc.accADCf[1]) + sq(acc.accADCf[2]));
if (gforce > acc.maxG) acc.maxG = gforce; if (gforce > acc.maxG) acc.maxG = gforce;
} }
void accGetVibrationLevels(fpVector3_t *accVibeLevels) void accGetVibrationLevels(fpVector3_t *accVibeLevels)
{ {
accVibeLevels->x = sqrtf(acc.accVibeSq[X]); accVibeLevels->x = fast_fsqrtf(acc.accVibeSq[X]);
accVibeLevels->y = sqrtf(acc.accVibeSq[Y]); accVibeLevels->y = fast_fsqrtf(acc.accVibeSq[Y]);
accVibeLevels->z = sqrtf(acc.accVibeSq[Z]); accVibeLevels->z = fast_fsqrtf(acc.accVibeSq[Z]);
} }
float accGetVibrationLevel(void) float accGetVibrationLevel(void)
{ {
return sqrtf(acc.accVibeSq[X] + acc.accVibeSq[Y] + acc.accVibeSq[Z]); return fast_fsqrtf(acc.accVibeSq[X] + acc.accVibeSq[Y] + acc.accVibeSq[Z]);
} }
uint32_t accGetClipCount(void) uint32_t accGetClipCount(void)

53
src/main/sensors/battery.c
View file

@ -36,6 +36,7 @@
#include "drivers/time.h" #include "drivers/time.h"
#include "fc/config.h" #include "fc/config.h"
#include "fc/controlrate_profile.h"
#include "fc/fc_core.h" #include "fc/fc_core.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "fc/stats.h" #include "fc/stats.h"
@ -93,7 +94,7 @@ static int32_t mWhDrawn = 0; // energy (milliWatt hours) draw
batteryState_e batteryState; batteryState_e batteryState;
const batteryProfile_t *currentBatteryProfile; const batteryProfile_t *currentBatteryProfile;
PG_REGISTER_ARRAY_WITH_RESET_FN(batteryProfile_t, MAX_BATTERY_PROFILE_COUNT, batteryProfiles, PG_BATTERY_PROFILES, 0); PG_REGISTER_ARRAY_WITH_RESET_FN(batteryProfile_t, MAX_BATTERY_PROFILE_COUNT, batteryProfiles, PG_BATTERY_PROFILES, 1);
void pgResetFn_batteryProfiles(batteryProfile_t *instance) void pgResetFn_batteryProfiles(batteryProfile_t *instance)
{ {
@ -115,7 +116,54 @@ void pgResetFn_batteryProfiles(batteryProfile_t *instance)
.warning = SETTING_BATTERY_CAPACITY_WARNING_DEFAULT, .warning = SETTING_BATTERY_CAPACITY_WARNING_DEFAULT,
.critical = SETTING_BATTERY_CAPACITY_CRITICAL_DEFAULT, .critical = SETTING_BATTERY_CAPACITY_CRITICAL_DEFAULT,
.unit = SETTING_BATTERY_CAPACITY_UNIT_DEFAULT, .unit = SETTING_BATTERY_CAPACITY_UNIT_DEFAULT,
},
.controlRateProfile = 0,
.motor = {
.throttleIdle = SETTING_THROTTLE_IDLE_DEFAULT,
.throttleScale = SETTING_THROTTLE_SCALE_DEFAULT,
#ifdef USE_DSHOT
.turtleModePowerFactor = SETTING_TURTLE_MODE_POWER_FACTOR_DEFAULT,
#endif
},
.failsafe_throttle = SETTING_FAILSAFE_THROTTLE_DEFAULT, // default throttle off.
.fwMinThrottleDownPitchAngle = SETTING_FW_MIN_THROTTLE_DOWN_PITCH_DEFAULT,
.nav = {
.mc = {
.hover_throttle = SETTING_NAV_MC_HOVER_THR_DEFAULT,
},
.fw = {
.cruise_throttle = SETTING_NAV_FW_CRUISE_THR_DEFAULT,
.max_throttle = SETTING_NAV_FW_MAX_THR_DEFAULT,
.min_throttle = SETTING_NAV_FW_MIN_THR_DEFAULT,
.pitch_to_throttle = SETTING_NAV_FW_PITCH2THR_DEFAULT, // pwm units per degree of pitch (10pwm units ~ 1% throttle)
.launch_throttle = SETTING_NAV_FW_LAUNCH_THR_DEFAULT,
.launch_idle_throttle = SETTING_NAV_FW_LAUNCH_IDLE_THR_DEFAULT, // Motor idle or MOTOR_STOP
}
},
#if defined(USE_POWER_LIMITS)
.powerLimits = {
.continuousCurrent = SETTING_LIMIT_CONT_CURRENT_DEFAULT, // dA
.burstCurrent = SETTING_LIMIT_BURST_CURRENT_DEFAULT, // dA
.burstCurrentTime = SETTING_LIMIT_BURST_CURRENT_TIME_DEFAULT, // dS
.burstCurrentFalldownTime = SETTING_LIMIT_BURST_CURRENT_FALLDOWN_TIME_DEFAULT, // dS
#ifdef USE_ADC
.continuousPower = SETTING_LIMIT_CONT_POWER_DEFAULT, // dW
.burstPower = SETTING_LIMIT_BURST_POWER_DEFAULT, // dW
.burstPowerTime = SETTING_LIMIT_BURST_POWER_TIME_DEFAULT, // dS
.burstPowerFalldownTime = SETTING_LIMIT_BURST_POWER_FALLDOWN_TIME_DEFAULT, // dS
#endif // USE_ADC
} }
#endif // USE_POWER_LIMITS
); );
} }
} }
@ -197,6 +245,9 @@ void setBatteryProfile(uint8_t profileIndex)
profileIndex = 0; profileIndex = 0;
} }
currentBatteryProfile = batteryProfiles(profileIndex); currentBatteryProfile = batteryProfiles(profileIndex);
if ((currentBatteryProfile->controlRateProfile > 0) && (currentBatteryProfile->controlRateProfile < MAX_CONTROL_RATE_PROFILE_COUNT)) {
setConfigProfile(currentBatteryProfile->controlRateProfile - 1);
}
} }
void activateBatteryProfile(void) void activateBatteryProfile(void)

82
src/main/sensors/battery.h
View file

@ -18,13 +18,16 @@
#pragma once #pragma once
#include "config/parameter_group.h" #include "config/parameter_group.h"
#include "drivers/time.h" #include "drivers/time.h"
#include "fc/settings.h"
#include "sensors/battery_config_structs.h"
#ifndef VBAT_SCALE_DEFAULT #ifndef VBAT_SCALE_DEFAULT
#define VBAT_SCALE_DEFAULT 1100 #define VBAT_SCALE_DEFAULT 1100
#endif #endif
#define VBAT_SCALE_MIN 0
#define VBAT_SCALE_MAX 65535
#ifndef CURRENT_METER_SCALE #ifndef CURRENT_METER_SCALE
#define CURRENT_METER_SCALE 400 // for Allegro ACS758LCB-100U (40mV/A) #define CURRENT_METER_SCALE 400 // for Allegro ACS758LCB-100U (40mV/A)
@ -35,90 +38,21 @@
#endif #endif
#ifndef MAX_BATTERY_PROFILE_COUNT #ifndef MAX_BATTERY_PROFILE_COUNT
#define MAX_BATTERY_PROFILE_COUNT 3 #define MAX_BATTERY_PROFILE_COUNT SETTING_CONSTANT_MAX_BATTERY_PROFILE_COUNT
#endif #endif
typedef enum {
CURRENT_SENSOR_NONE = 0,
CURRENT_SENSOR_ADC,
CURRENT_SENSOR_VIRTUAL,
CURRENT_SENSOR_ESC,
CURRENT_SENSOR_MAX = CURRENT_SENSOR_VIRTUAL
} currentSensor_e;
typedef enum {
VOLTAGE_SENSOR_NONE = 0,
VOLTAGE_SENSOR_ADC,
VOLTAGE_SENSOR_ESC
} voltageSensor_e;
typedef enum {
BAT_CAPACITY_UNIT_MAH,
BAT_CAPACITY_UNIT_MWH,
} batCapacityUnit_e;
typedef struct { typedef struct {
uint8_t profile_index; uint8_t profile_index;
uint16_t max_voltage; uint16_t max_voltage;
} profile_comp_t; } profile_comp_t;
typedef enum {
BAT_VOLTAGE_RAW,
BAT_VOLTAGE_SAG_COMP
} batVoltageSource_e;
typedef struct batteryMetersConfig_s {
#ifdef USE_ADC
struct {
uint16_t scale;
voltageSensor_e type;
} voltage;
#endif
struct {
int16_t scale; // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A
int16_t offset; // offset of the current sensor in millivolt steps
currentSensor_e type; // type of current meter used, either ADC or virtual
} current;
batVoltageSource_e voltageSource;
uint32_t cruise_power; // power drawn by the motor(s) at cruise throttle/speed (cW)
uint16_t idle_power; // power drawn by the system when the motor(s) are stopped (cW)
uint8_t rth_energy_margin; // Energy that should be left after RTH (%), used for remaining time/distance before RTH
float throttle_compensation_weight;
} batteryMetersConfig_t;
typedef struct batteryProfile_s {
#ifdef USE_ADC
uint8_t cells;
struct {
uint16_t cellDetect; // maximum voltage per cell, used for auto-detecting battery cell count in 0.01V units, default is 430 (4.3V)
uint16_t cellMax; // maximum voltage per cell, used for auto-detecting battery voltage in 0.01V units, default is 421 (4.21V)
uint16_t cellMin; // minimum voltage per cell, this triggers battery critical alarm, in 0.01V units, default is 330 (3.3V)
uint16_t cellWarning; // warning voltage per cell, this triggers battery warning alarm, in 0.01V units, default is 350 (3.5V)
} voltage;
#endif
struct {
uint32_t value; // mAh or mWh (see capacity.unit)
uint32_t warning; // mAh or mWh (see capacity.unit)
uint32_t critical; // mAh or mWh (see capacity.unit)
batCapacityUnit_e unit; // Describes unit of capacity.value, capacity.warning and capacity.critical
} capacity;
} batteryProfile_t;
PG_DECLARE(batteryMetersConfig_t, batteryMetersConfig); PG_DECLARE(batteryMetersConfig_t, batteryMetersConfig);
PG_DECLARE_ARRAY(batteryProfile_t, MAX_BATTERY_PROFILE_COUNT, batteryProfiles); PG_DECLARE_ARRAY(batteryProfile_t, MAX_BATTERY_PROFILE_COUNT, batteryProfiles);
extern const batteryProfile_t *currentBatteryProfile; extern const batteryProfile_t *currentBatteryProfile;
#define currentBatteryProfileMutable ((batteryProfile_t*)currentBatteryProfile)
typedef enum { typedef enum {
BATTERY_OK = 0, BATTERY_OK = 0,
BATTERY_WARNING, BATTERY_WARNING,

144
src/main/sensors/battery_config_structs.h Normal file
View file

@ -0,0 +1,144 @@
/*
* This file is part of iNav
*
* iNav 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 distributed in the hope that it
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <stdbool.h>
#include <stdint.h>
#include "platform.h"
typedef enum {
CURRENT_SENSOR_NONE = 0,
CURRENT_SENSOR_ADC,
CURRENT_SENSOR_VIRTUAL,
CURRENT_SENSOR_ESC,
CURRENT_SENSOR_MAX = CURRENT_SENSOR_VIRTUAL
} currentSensor_e;
typedef enum {
VOLTAGE_SENSOR_NONE = 0,
VOLTAGE_SENSOR_ADC,
VOLTAGE_SENSOR_ESC
} voltageSensor_e;
typedef enum {
BAT_CAPACITY_UNIT_MAH,
BAT_CAPACITY_UNIT_MWH,
} batCapacityUnit_e;
typedef enum {
BAT_VOLTAGE_RAW,
BAT_VOLTAGE_SAG_COMP
} batVoltageSource_e;
typedef struct batteryMetersConfig_s {
#ifdef USE_ADC
struct {
uint16_t scale;
voltageSensor_e type;
} voltage;
#endif
struct {
int16_t scale; // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A
int16_t offset; // offset of the current sensor in millivolt steps
currentSensor_e type; // type of current meter used, either ADC or virtual
} current;
batVoltageSource_e voltageSource;
uint32_t cruise_power; // power drawn by the motor(s) at cruise throttle/speed (cW)
uint16_t idle_power; // power drawn by the system when the motor(s) are stopped (cW)
uint8_t rth_energy_margin; // Energy that should be left after RTH (%), used for remaining time/distance before RTH
float throttle_compensation_weight;
} batteryMetersConfig_t;
typedef struct batteryProfile_s {
#ifdef USE_ADC
uint8_t cells;
struct {
uint16_t cellDetect; // maximum voltage per cell, used for auto-detecting battery cell count in 0.01V units, default is 430 (4.3V)
uint16_t cellMax; // maximum voltage per cell, used for auto-detecting battery voltage in 0.01V units, default is 421 (4.21V)
uint16_t cellMin; // minimum voltage per cell, this triggers battery critical alarm, in 0.01V units, default is 330 (3.3V)
uint16_t cellWarning; // warning voltage per cell, this triggers battery warning alarm, in 0.01V units, default is 350 (3.5V)
} voltage;
#endif
struct {
uint32_t value; // mAh or mWh (see capacity.unit)
uint32_t warning; // mAh or mWh (see capacity.unit)
uint32_t critical; // mAh or mWh (see capacity.unit)
batCapacityUnit_e unit; // Describes unit of capacity.value, capacity.warning and capacity.critical
} capacity;
uint8_t controlRateProfile;
struct {
float throttleIdle; // Throttle IDLE value based on min_command, max_throttle, in percent
float throttleScale; // Scaling factor for throttle.
#ifdef USE_DSHOT
uint8_t turtleModePowerFactor; // Power factor from 0 to 100% of flip over after crash
#endif
} motor;
uint16_t failsafe_throttle; // Throttle level used for landing - specify value between 1000..2000 (pwm pulse width for slightly below hover). center throttle = 1500.
uint16_t fwMinThrottleDownPitchAngle;
struct {
struct {
uint16_t hover_throttle; // multicopter hover throttle
} mc;
struct {
uint16_t cruise_throttle; // Cruise throttle
uint16_t min_throttle; // Minimum allowed throttle in auto mode
uint16_t max_throttle; // Maximum allowed throttle in auto mode
uint8_t pitch_to_throttle; // Pitch angle (in deg) to throttle gain (in 1/1000's of throttle) (*10)
uint16_t launch_idle_throttle; // Throttle to keep at launch idle
uint16_t launch_throttle; // Launch throttle
} fw;
} nav;
#if defined(USE_POWER_LIMITS)
struct {
uint16_t continuousCurrent; // dA
uint16_t burstCurrent; // dA
uint16_t burstCurrentTime; // ds
uint16_t burstCurrentFalldownTime; // ds
#ifdef USE_ADC
uint16_t continuousPower; // dW
uint16_t burstPower; // dW
uint16_t burstPowerTime; // ds
uint16_t burstPowerFalldownTime; // ds
#endif // USE_ADC
} powerLimits;
#endif // USE_POWER_LIMITS
} batteryProfile_t;

19
src/main/sensors/gyro.c
View file

@ -75,6 +75,7 @@ FILE_COMPILE_FOR_SPEED
#include "flight/gyroanalyse.h" #include "flight/gyroanalyse.h"
#include "flight/rpm_filter.h" #include "flight/rpm_filter.h"
#include "flight/dynamic_gyro_notch.h" #include "flight/dynamic_gyro_notch.h"
#include "flight/kalman.h"
#ifdef USE_HARDWARE_REVISION_DETECTION #ifdef USE_HARDWARE_REVISION_DETECTION
#include "hardware_revision.h" #include "hardware_revision.h"
@ -111,7 +112,7 @@ STATIC_FASTRAM alphaBetaGammaFilter_t abgFilter[XYZ_AXIS_COUNT];
#endif #endif
PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 14); PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 15);
PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_lpf = SETTING_GYRO_HARDWARE_LPF_DEFAULT, .gyro_lpf = SETTING_GYRO_HARDWARE_LPF_DEFAULT,
@ -142,6 +143,10 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.alphaBetaGammaBoost = SETTING_GYRO_ABG_BOOST_DEFAULT, .alphaBetaGammaBoost = SETTING_GYRO_ABG_BOOST_DEFAULT,
.alphaBetaGammaHalfLife = SETTING_GYRO_ABG_HALF_LIFE_DEFAULT, .alphaBetaGammaHalfLife = SETTING_GYRO_ABG_HALF_LIFE_DEFAULT,
#endif #endif
#ifdef USE_GYRO_KALMAN
.kalman_q = SETTING_SETPOINT_KALMAN_Q_DEFAULT,
.kalmanEnabled = SETTING_SETPOINT_KALMAN_ENABLED_DEFAULT,
#endif
); );
STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware) STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware)
@ -318,6 +323,11 @@ static void gyroInitFilters(void)
} }
} }
#endif #endif
#ifdef USE_GYRO_KALMAN
if (gyroConfig()->kalmanEnabled) {
gyroKalmanInitialize(gyroConfig()->kalman_q);
}
#endif
} }
bool gyroInit(void) bool gyroInit(void)
@ -501,6 +511,12 @@ void FAST_CODE NOINLINE gyroFilter()
} }
#endif #endif
#ifdef USE_GYRO_KALMAN
if (gyroConfig()->kalmanEnabled) {
gyroADCf = gyroKalmanUpdate(axis, gyroADCf);
}
#endif
gyro.gyroADCf[axis] = gyroADCf; gyro.gyroADCf[axis] = gyroADCf;
} }
@ -517,6 +533,7 @@ void FAST_CODE NOINLINE gyroFilter()
} }
} }
#endif #endif
} }
void FAST_CODE NOINLINE gyroUpdate() void FAST_CODE NOINLINE gyroUpdate()

4
src/main/sensors/gyro.h
View file

@ -87,6 +87,10 @@ typedef struct gyroConfig_s {
float alphaBetaGammaBoost; float alphaBetaGammaBoost;
float alphaBetaGammaHalfLife; float alphaBetaGammaHalfLife;
#endif #endif
#ifdef USE_GYRO_KALMAN
uint16_t kalman_q;
uint8_t kalmanEnabled;
#endif
} gyroConfig_t; } gyroConfig_t;
PG_DECLARE(gyroConfig_t, gyroConfig); PG_DECLARE(gyroConfig_t, gyroConfig);

4
src/main/sensors/opflow.c
View file

@ -237,8 +237,8 @@ void opflowUpdate(timeUs_t currentTimeUs)
else if (opflow.flowQuality == OPFLOW_QUALITY_VALID) { else if (opflow.flowQuality == OPFLOW_QUALITY_VALID) {
// Ongoing calibration - accumulate body and flow rotation magniture if opflow quality is good enough // Ongoing calibration - accumulate body and flow rotation magniture if opflow quality is good enough
const float invDt = 1.0e6 / opflow.dev.rawData.deltaTime; const float invDt = 1.0e6 / opflow.dev.rawData.deltaTime;
opflowCalibrationBodyAcc += sqrtf(sq(opflow.bodyRate[X]) + sq(opflow.bodyRate[Y])); opflowCalibrationBodyAcc += fast_fsqrtf(sq(opflow.bodyRate[X]) + sq(opflow.bodyRate[Y]));
opflowCalibrationFlowAcc += sqrtf(sq(opflow.dev.rawData.flowRateRaw[X]) + sq(opflow.dev.rawData.flowRateRaw[Y])) * invDt; opflowCalibrationFlowAcc += fast_fsqrtf(sq(opflow.dev.rawData.flowRateRaw[X]) + sq(opflow.dev.rawData.flowRateRaw[Y])) * invDt;
} }
} }

2
src/main/sensors/pitotmeter.c
View file

@ -223,7 +223,7 @@ STATIC_PROTOTHREAD(pitotThread)
// //
// Therefore we shouldn't care about CAS/TAS and only calculate IAS since it's more indicative to the pilot and more useful in calculations // Therefore we shouldn't care about CAS/TAS and only calculate IAS since it's more indicative to the pilot and more useful in calculations
// It also allows us to use pitot_scale to calibrate the dynamic pressure sensor scale // It also allows us to use pitot_scale to calibrate the dynamic pressure sensor scale
pitot.airSpeed = pitotmeterConfig()->pitot_scale * sqrtf(2.0f * fabsf(pitot.pressure - pitot.pressureZero) / AIR_DENSITY_SEA_LEVEL_15C) * 100; pitot.airSpeed = pitotmeterConfig()->pitot_scale * fast_fsqrtf(2.0f * fabsf(pitot.pressure - pitot.pressureZero) / AIR_DENSITY_SEA_LEVEL_15C) * 100;
} else { } else {
performPitotCalibrationCycle(); performPitotCalibrationCycle();
pitot.airSpeed = 0; pitot.airSpeed = 0;

1
src/main/target/BETAFPVF722/CMakeLists.txt Normal file
View file

@ -0,0 +1 @@
target_stm32f722xe(BETAFPVF722)

44
src/main/target/BETAFPVF722/target.c Executable file
View file

@ -0,0 +1,44 @@
/*
* 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 <http://www.gnu.org/licenses/>.
*/
#include <stdint.h>
#include "platform.h"
#include "drivers/bus.h"
#include "drivers/io.h"
#include "drivers/pwm_mapping.h"
#include "drivers/timer.h"
const timerHardware_t timerHardware[] = {
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(TIM4, CH1, PB6, TIM_USE_MC_MOTOR, 0, 0), // S5 DMA1_ST2
DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR, 0, 0), // S6 DMA2_ST6
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)
};
const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

166
src/main/target/BETAFPVF722/target.h Executable file
View file

@ -0,0 +1,166 @@
/*
* 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 <http://www.gnu.org/licenses/>.
*/
#pragma once
#define TARGET_BOARD_IDENTIFIER "BHF7"
#define USBD_PRODUCT_STRING "BETAFPVF722"
#define LED0 PC15
#define BEEPER PC14
#define BEEPER_INVERTED
// *************** 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 MPU6000_CS_PIN PA4
#define MPU6000_SPI_BUS BUS_SPI1
#define USE_IMU_MPU6000
#define IMU_MPU6000_ALIGN CW180_DEG
#define USE_EXTI
#define GYRO_INT_EXTI PC4
#define USE_MPU_DATA_READY_SIGNAL
// *************** I2C/Baro/Mag *********************
#define USE_I2C
#define USE_I2C_DEVICE_1
#define I2C1_SCL PB8
#define I2C1_SDA PB9
#define USE_I2C_DEVICE_2
#define I2C_DEVICE_2_SHARES_UART3
#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_I2C2
#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_I2C2
#define PITOT_I2C_BUS BUS_I2C2
#define RANGEFINDER_I2C_BUS BUS_I2C2
// *************** OSD*************************
#define USE_SPI_DEVICE_3
#define SPI3_SCK_PIN PB3
#define SPI3_MISO_PIN PB4
#define SPI3_MOSI_PIN PB5
#define USE_OSD
#define USE_MAX7456
#define MAX7456_SPI_BUS BUS_SPI3
#define MAX7456_CS_PIN PA15
// *************** UART *****************************
#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 PC11
#define UART4_TX_PIN PC10
#define USE_UART5
#define UART5_RX_PIN PD2
#define UART5_TX_PIN PC12
#define USE_UART6
#define UART6_RX_PIN PC7
#define UART6_TX_PIN PC6
#define SERIAL_PORT_COUNT 7
#define DEFAULT_RX_TYPE RX_TYPE_SERIAL
#define SERIALRX_PROVIDER SERIALRX_SBUS
// *************** 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 ADC_CHANNEL_3_PIN PC2
#define VBAT_ADC_CHANNEL ADC_CHN_1
#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2
#define RSSI_ADC_CHANNEL ADC_CHN_3
// *************** FLASH *****************************
#define USE_SPI_DEVICE_2
#define SPI2_SCK_PIN PB13
#define SPI2_MISO_PIN PB14
#define SPI2_MOSI_PIN PB15
#define USE_FLASHFS
#define USE_FLASH_M25P16
#define M25P16_CS_PIN PB12
#define M25P16_SPI_BUS BUS_SPI2
#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY| FEATURE_VBAT | FEATURE_OSD )
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
#define TARGET_IO_PORTA 0xffff
#define TARGET_IO_PORTB 0xffff
#define TARGET_IO_PORTC 0xffff
#define TARGET_IO_PORTD (BIT(2))
#define MAX_PWM_OUTPUT_PORTS 6
#define TARGET_MOTOR_COUNT 4
// ESC-related features
#define USE_DSHOT
#define USE_ESC_SENSOR
#define USE_SERIALSHOT

6
src/main/target/FALCORE/config.c
View file

@ -89,7 +89,7 @@ void targetConfiguration(void)
failsafeConfigMutable()->failsafe_delay = 5; failsafeConfigMutable()->failsafe_delay = 5;
failsafeConfigMutable()->failsafe_recovery_delay = 5; failsafeConfigMutable()->failsafe_recovery_delay = 5;
failsafeConfigMutable()->failsafe_off_delay = 200; failsafeConfigMutable()->failsafe_off_delay = 200;
failsafeConfigMutable()->failsafe_throttle = 1200; currentBatteryProfile->failsafe_throttle = 1200;
failsafeConfigMutable()->failsafe_procedure = FAILSAFE_PROCEDURE_RTH; failsafeConfigMutable()->failsafe_procedure = FAILSAFE_PROCEDURE_RTH;
boardAlignmentMutable()->rollDeciDegrees = 0; boardAlignmentMutable()->rollDeciDegrees = 0;
@ -120,7 +120,7 @@ void targetConfiguration(void)
navConfigMutable()->general.rth_altitude = 1000; navConfigMutable()->general.rth_altitude = 1000;
navConfigMutable()->mc.max_bank_angle = 30; navConfigMutable()->mc.max_bank_angle = 30;
navConfigMutable()->mc.hover_throttle = 1500; currentBatteryProfile->nav.mc.hover_throttle = 1500;
navConfigMutable()->mc.auto_disarm_delay = 2000; navConfigMutable()->mc.auto_disarm_delay = 2000;
/* /*
@ -192,4 +192,6 @@ void targetConfiguration(void)
((controlRateConfig_t*)currentControlRateProfile)->throttle.rcExpo8 = 0; ((controlRateConfig_t*)currentControlRateProfile)->throttle.rcExpo8 = 0;
((controlRateConfig_t*)currentControlRateProfile)->throttle.dynPID = 10; ((controlRateConfig_t*)currentControlRateProfile)->throttle.dynPID = 10;
((controlRateConfig_t*)currentControlRateProfile)->throttle.pa_breakpoint = 1600; ((controlRateConfig_t*)currentControlRateProfile)->throttle.pa_breakpoint = 1600;
beeperConfigMutable()->pwmMode = true;
} }

1
src/main/target/FALCORE/target.h
View file

@ -25,7 +25,6 @@
#define USE_HARDWARE_PREBOOT_SETUP // FALCORE board requires some hardware to be set up before booting and detecting sensors #define USE_HARDWARE_PREBOOT_SETUP // FALCORE board requires some hardware to be set up before booting and detecting sensors
#define BEEPER PB4 #define BEEPER PB4
#define BEEPER_PWM
#define BEEPER_PWM_FREQUENCY 2700 #define BEEPER_PWM_FREQUENCY 2700
#define MPU6500_CS_PIN PC0 #define MPU6500_CS_PIN PC0

1
src/main/target/FLYWOOF745/target.h
View file

@ -119,7 +119,6 @@
#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
#define USE_I2C #define USE_I2C
#define USE_I2C_DEVICE_1 #define USE_I2C_DEVICE_1
#define I2C1_SCL PB6 #define I2C1_SCL PB6
#define I2C1_SDA PB7 #define I2C1_SDA PB7

3
src/main/target/FRSKYPILOT/config.c
View file

@ -22,6 +22,7 @@
#include "io/serial.h" #include "io/serial.h"
#include "rx/rx.h" #include "rx/rx.h"
#include "fc/config.h"
void targetConfiguration(void) void targetConfiguration(void)
{ {
@ -29,4 +30,6 @@ void targetConfiguration(void)
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART6)].functionMask = FUNCTION_FRSKY_OSD; serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART6)].functionMask = FUNCTION_FRSKY_OSD;
rxConfigMutable()->serialrx_inverted = 1; rxConfigMutable()->serialrx_inverted = 1;
beeperConfigMutable()->pwmMode = true;
} }

1
src/main/target/FRSKYPILOT/target.h
View file

@ -29,7 +29,6 @@
#define BEEPER PA0 #define BEEPER PA0
#define BEEPER_INVERTED #define BEEPER_INVERTED
#define BEEPER_PWM
#define BEEPER_PWM_FREQUENCY 4000 #define BEEPER_PWM_FREQUENCY 4000
#define USE_SPI #define USE_SPI

8
src/main/target/IFLIGHTF7_TWING/target.c
View file

@ -37,11 +37,11 @@ const timerHardware_t timerHardware[] = {
DEF_TIM(TIM8, CH4, PC9, 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, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 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, 0, 0), DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR | TIM_USE_MC_SERVO, 0, 0),
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), 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, 0, 0), DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0),
DEF_TIM(TIM2, CH2, PA1, TIM_USE_LED, 0, 0), DEF_TIM(TIM2, CH2, PA1, TIM_USE_LED, 0, 0),
}; };

7
src/main/target/MAMBAF405US/target.h
View file

@ -57,7 +57,7 @@
#define I2C2_SDA PB9 #define I2C2_SDA PB9
#define DEFAULT_I2C_BUS BUS_I2C1 #define DEFAULT_I2C_BUS BUS_I2C1
#else #else
#define USE_I2C_DEVICE_2 #define USE_I2C_DEVICE_2
#define I2C2_SCL PB10 // SCL pad TX3 #define I2C2_SCL PB10 // SCL pad TX3
@ -186,8 +186,8 @@
#define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTC 0xffff
#define TARGET_IO_PORTD (BIT(2)) #define TARGET_IO_PORTD (BIT(2))
#define MAX_PWM_OUTPUT_PORTS 8 #define MAX_PWM_OUTPUT_PORTS 4
#define TARGET_MOTOR_COUNT 4 #define TARGET_MOTOR_COUNT 4
// ESC-related features // ESC-related features
#define USE_DSHOT #define USE_DSHOT
@ -199,4 +199,3 @@
#define PITOT_I2C_BUS DEFAULT_I2C_BUS #define PITOT_I2C_BUS DEFAULT_I2C_BUS
#define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS #define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS
#define BNO055_I2C_BUS DEFAULT_I2C_BUS #define BNO055_I2C_BUS DEFAULT_I2C_BUS

3
src/main/target/MATEKF405CAN/config.c
View file

@ -21,6 +21,7 @@
#include "config/feature.h" #include "config/feature.h"
#include "io/serial.h" #include "io/serial.h"
#include "sensors/compass.h" #include "sensors/compass.h"
#include "fc/config.h"
void targetConfiguration(void) void targetConfiguration(void)
@ -28,5 +29,5 @@ void targetConfiguration(void)
compassConfigMutable()->mag_align = CW0_DEG_FLIP; compassConfigMutable()->mag_align = CW0_DEG_FLIP;
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].functionMask = FUNCTION_GPS; serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].functionMask = FUNCTION_GPS;
// serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].gps_baudrateIndex = BAUD_115200; // serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].gps_baudrateIndex = BAUD_115200;
beeperConfigMutable()->pwmMode = true;
} }

1
src/main/target/MATEKF405CAN/target.h
View file

@ -25,7 +25,6 @@
#define BEEPER PA8 #define BEEPER PA8
#define BEEPER_INVERTED #define BEEPER_INVERTED
#define BEEPER_PWM
#define BEEPER_PWM_FREQUENCY 2500 #define BEEPER_PWM_FREQUENCY 2500
// *************** SPI1 Gyro & ACC ******************* // *************** SPI1 Gyro & ACC *******************

2
src/main/target/MATEKH743/CMakeLists.txt
View file

@ -1 +1 @@
target_stm32h743xi(MATEKH743 SKIP_RELEASES) target_stm32h743xi(MATEKH743)

2
src/main/target/MATEKH743/config.c
View file

@ -20,6 +20,7 @@
#include "platform.h" #include "platform.h"
#include "fc/fc_msp_box.h" #include "fc/fc_msp_box.h"
#include "fc/config.h"
#include "io/piniobox.h" #include "io/piniobox.h"
@ -27,4 +28,5 @@ void targetConfiguration(void)
{ {
pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1;
pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2;
beeperConfigMutable()->pwmMode = true;
} }

16
src/main/target/MATEKH743/target.c
View file

@ -30,16 +30,16 @@ BUSDEV_REGISTER_SPI_TAG(busdev_imu1_6000, DEVHW_MPU6000, IMU1_SPI_BUS, IMU1
BUSDEV_REGISTER_SPI_TAG(busdev_imu2_6500, DEVHW_MPU6500, IMU2_SPI_BUS, IMU2_CS_PIN, IMU2_EXTI_PIN, 1, DEVFLAGS_NONE, IMU2_ALIGN); BUSDEV_REGISTER_SPI_TAG(busdev_imu2_6500, DEVHW_MPU6500, IMU2_SPI_BUS, IMU2_CS_PIN, IMU2_EXTI_PIN, 1, DEVFLAGS_NONE, IMU2_ALIGN);
const timerHardware_t timerHardware[] = { const timerHardware_t timerHardware[] = {
DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S1 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, 2), // S2 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, 3), // S3 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, 4), // S4 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, 5), // S5 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, 6), // S6 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, 7), // S7 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, 8), // S8 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, 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, CH4, PD15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S10 DMA_NONE

1
src/main/target/MATEKH743/target.h
View file

@ -28,7 +28,6 @@
#define BEEPER PA15 #define BEEPER PA15
#define BEEPER_INVERTED #define BEEPER_INVERTED
#define BEEPER_PWM
#define BEEPER_PWM_FREQUENCY 2500 #define BEEPER_PWM_FREQUENCY 2500
// *************** IMU generic *********************** // *************** IMU generic ***********************

1
src/main/target/SKYSTARSF405HD/CMakeLists.txt Normal file
View file

@ -0,0 +1 @@
target_stm32f405xg(SKYSTARSF405HD)

29
src/main/target/SKYSTARSF405HD/config.c Normal file
View file

@ -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 <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <platform.h>
#include "io/serial.h"
#include "rx/rx.h"
void targetConfiguration(void)
{
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART6)].functionMask = FUNCTION_DJI_HD_OSD;
}

35
src/main/target/SKYSTARSF405HD/target.c Normal file
View file

@ -0,0 +1,35 @@
/*
* 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 <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <platform.h>
#include "drivers/io.h"
#include "drivers/pwm_mapping.h"
#include "drivers/timer.h"
const 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(TIM8, CH3, PC8, TIM_USE_LED, 0, 0),
};
const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

166
src/main/target/SKYSTARSF405HD/target.h Normal file
View file

@ -0,0 +1,166 @@
/*
* 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 <http://www.gnu.org/licenses/>.
*/
#pragma once
#define TARGET_BOARD_IDENTIFIER "SS4D"
#define USBD_PRODUCT_STRING "SkystarsF405HD"
#define LED0 PC14 // green
#define LED1 PC15 // blue
#define BEEPER PC13
#define BEEPER_INVERTED
// *************** 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 MPU6000_CS_PIN PA4
#define MPU6000_SPI_BUS BUS_SPI1
#define USE_EXTI
#define GYRO_INT_EXTI PC4
#define USE_MPU_DATA_READY_SIGNAL
#define USE_IMU_MPU6000
#define IMU_MPU6000_ALIGN CW180_DEG_FLIP
// *************** M25P256 flash ********************
#define USE_FLASHFS
#define USE_FLASH_M25P16
#define USE_SPI_DEVICE_3
#define SPI3_SCK_PIN PC10
#define SPI3_MISO_PIN PC11
#define SPI3_MOSI_PIN PB5
#define M25P16_SPI_BUS BUS_SPI3
#define M25P16_CS_PIN PA15
#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
// *************** OSD & baro ***********************
#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
#define USE_BARO
#define USE_BARO_BMP280
#define BMP280_SPI_BUS BUS_SPI2
#define BMP280_CS_PIN PC5
// *************** UART *****************************
#define USE_VCP
#define USE_UART1
#define UART1_RX_PIN PA10
#define UART1_TX_PIN PA9
#define USE_UART2
#define UART2_RX_PIN PA3
#define UART2_TX_PIN PA2
#define USE_UART3
#define UART3_RX_PIN PB11
#define UART3_TX_PIN PB10
#define USE_UART4
#define UART4_RX_PIN PA1
#define UART4_TX_PIN PA0
#define USE_UART5
#define UART5_RX_PIN PD2
#define UART5_TX_PIN PC12
#define USE_UART6
#define UART6_RX_PIN PC7
#define UART6_TX_PIN PC6
#define SERIAL_PORT_COUNT 7
#define DEFAULT_RX_TYPE RX_TYPE_SERIAL
#define SERIALRX_PROVIDER SERIALRX_SBUS
#define SERIALRX_UART SERIAL_PORT_USART1
// *************** I2C ****************************
#define USE_I2C
#define USE_I2C_DEVICE_1
#define I2C1_SCL PB8
#define I2C1_SDA PB9
#define DEFAULT_I2C_BUS BUS_I2C1
#define USE_MAG
#define MAG_I2C_BUS DEFAULT_I2C_BUS
#define USE_MAG_AK8963
#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 DEFAULT_I2C_BUS
#define BNO055_I2C_BUS DEFAULT_I2C_BUS
#define USE_RANGEFINDER
#define USE_RANGEFINDER_MSP
#define USE_RANGEFINDER_HCSR04_I2C
#define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS
#define PITOT_I2C_BUS DEFAULT_I2C_BUS
#define PCA9685_I2C_BUS DEFAULT_I2C_BUS
// *************** ADC *****************************
#define USE_ADC
#define ADC_CHANNEL_1_PIN PC0
#define ADC_CHANNEL_2_PIN PC1
#define ADC_CHANNEL_3_PIN PC2
#define VBAT_ADC_CHANNEL ADC_CHN_1
#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2
#define RSSI_ADC_CHANNEL ADC_CHN_3
#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY )
#define USE_LED_STRIP
#define WS2811_PIN PC8
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
#define TARGET_IO_PORTA 0xffff
#define TARGET_IO_PORTB 0xffff
#define TARGET_IO_PORTC 0xffff
#define TARGET_IO_PORTD 0xffff
#define USE_DSHOT
#define USE_ESC_SENSOR
#define USE_SERIALSHOT
#define MAX_PWM_OUTPUT_PORTS 4

Some files were not shown because too many files have changed in this diff Show more