Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-12 19:10:27 +03:00
Code Issues Wiki Activity

Merge branch 'master' into dzikuvx-adaptive-filter

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2024-06-07 12:39:31 +02:00
commit e18cbbf76a
114 changed files with 3688 additions and 2097 deletions
Download patch file Download diff file Expand all files Collapse all files

1
.vimrc
View file

@ -5,5 +5,6 @@ set expandtab
set bs=2
set sw=4
set ts=4
syn on

5
cmake/at32.cmake
View file

@ -330,6 +330,11 @@ function(target_at32)
math(EXPR hse_value "${hse_mhz} * 1000000")
list(APPEND target_definitions "HSE_VALUE=${hse_value}")
if (MSP_UART)
list(APPEND target_definitions "MSP_UART=${MSP_UART}")
endif()
if(args_COMPILE_DEFINITIONS)
list(APPEND target_definitions ${args_COMPILE_DEFINITIONS})
endif()

4
cmake/sitl.cmake
View file

@ -99,6 +99,10 @@ function (target_sitl name)
math(EXPR hse_value "${hse_mhz} * 1000000")
list(APPEND target_definitions "HSE_VALUE=${hse_value}")
if (MSP_UART)
list(APPEND target_definitions "MSP_UART=${MSP_UART}")
endif()
string(TOLOWER ${PROJECT_NAME} lowercase_project_name)
set(binary_name ${lowercase_project_name}_${FIRMWARE_VERSION}_${name})
if(DEFINED BUILD_SUFFIX AND NOT "" STREQUAL "${BUILD_SUFFIX}")

5
cmake/stm32.cmake
View file

@ -337,6 +337,11 @@ function(target_stm32)
math(EXPR hse_value "${hse_mhz} * 1000000")
list(APPEND target_definitions "HSE_VALUE=${hse_value}")
if (MSP_UART)
list(APPEND target_definitions "MSP_UART=${MSP_UART}")
endif()
if(args_COMPILE_DEFINITIONS)
list(APPEND target_definitions ${args_COMPILE_DEFINITIONS})
endif()

3
docs/Battery.md
View file

@ -201,7 +201,6 @@ Up to 3 battery profiles are supported. You can select the battery profile from
- `vbat_max_cell_voltage`
- `vbat_warning_cell_voltage`
- `vbat_min_cell_voltage`
- `battery_capacity_unit`
- `battery_capacity`
- `battery_capacity_warning`
- `battery_capacity_critical`
@ -253,7 +252,6 @@ feature BAT_PROF_AUTOSWITCH
battery_profile 1
set bat_cells = 3
set battery_capacity_unit = MAH
set battery_capacity = 2200
set battery_capacity_warning = 440
set battery_capacity_critical = 220
@ -262,7 +260,6 @@ set battery_capacity_critical = 220
battery_profile 2
set bat_cells = 4
set battery_capacity_unit = MAH
set battery_capacity = 1500
set battery_capacity_warning = 300
set battery_capacity_critical = 150

50
docs/Betaflight 4.3 compatible OSD.md
View file

@ -1,50 +0,0 @@
# Betaflight 4.3 compatible MSP DisplayPort OSD (DJI O3 "Canvas Mode")
INAV 6.0 includes a special mode for MSP DisplayPort that supports incomplete implementations of MSP DisplayPort that only support BetaFlight, like the DJI O3 Air Unit. INAV 6.1 expands this to include HD canvas sizes from BetaFlight 4.4.
Different flight controllers have different OSD symbols and elements and require different fonts. BetaFlight's font is a single page and supports a maximum of 256 glyphs, INAV's font is currently 2 pages and supports up to 512 different glyphs.
While there is some overlap between the glyphs in BetaFlight and INAV, it is not possible to perform a 1 to 1 mapping for all the them. In cases where there is no suitable glyph in the BetaFlight font, a question mark `?` will be displayed.
This mode can be enabled by selecting BF43COMPAT or BFHDCOMPAT as video format in the OSD tab of the configurator or by typing the following command on the CLI:
`set osd_video_system = BF43COMPAT`
or
`set osd_video_system = BFHDCOMPAT`
## Limitations
* Canvas size needs to be manually changed to HD on the Display menu in DJI's goggles (you may need a firmware update) and set as BFHDCOMPAT in the OSD tab of the configurator.
* Unsupported Glyphs show up as `?`
## FAQ
### I see a lot of `?` on my OSD.
That is expected, when your INAV OSD widgets use glyphs that don't have a suitable mapping in BetaFlight's font.
### Does it work with the G2 and Original Air Unit/Vista?
Yes.
### Is this a replacement for WTFOS?
Not exactly. WTFOS is a full implementation of MSP-Displayport for rooted Air Unit/Vista/Googles V2 and actually works much better than BetaFlight compatibility mode, being able to display all INAV's glyphs.
### Can INAV fix DJI's product?
No. OSD renderinng happens on the googles/air unit side of things. Please ask DJI to fix their incomplete MSP DisplayPort implemenation. You can probably request it in [DJI's forum](https://forum.dji.com/forum.php?mod=forumdisplay&fid=129&filter=typeid&typeid=767).
### BetaFlight X.Y now has more symbols, can you update INAV?
Maybe. If a future version of BetaFlight includes more Glyphs that can be mapped into INAV it is fairly simple to add the mapping, but the problem with DJI's implemenation persists. Even if we update the mapping, if DJI does not update the fonts on their side the problem will persist.
### Can you replace glyph `X` with text `x description`?
While it might technically be possible to replace some glyphs with text in multiple cells, it will introduce a lot of complexity in the OSD rendering and configuration for something we hope is a temporary workaround.
### Does DJI support Canvas Mode?
Actually, no. What DJI calls Canvas Mode is actually MSP DisplayPort and is a character based OSD.

79
docs/Broken USB recovery.md Normal file
View file

@ -0,0 +1,79 @@
# Broken USB recovery
It is possible to flash INAV without USB over UART 1 or 3.
## Prerequisites:
- USB/UART adapter (FT232, CP2102, etc.)
- STM32 Cube Programmer (https://www.st.com/en/development-tools/stm32cubeprog.html)
To gain access to the FC via Configurator, MSP must be activated on a UART as standard. Some FCs already have this enabled by default, if not a custom firmware must be created.
The following targets have MSP activated on a UART by default:
| Target | Standard MSP Port |
|-----------| ----------- |
| AOCODARCF4V3 | UART5 |
| ATOMRCF405NAVI_DELUXE | UART1 |
| FF_F35_LIGHTNING | UART1 |
| FLYCOLORF7V2 | UART4 |
| GEPRCF405_BT_HD | UART5* |
| GEPRCF722_BT_HD | UART4* |
| IFLIGHT_BLITZ_F7_AIO | UART1 |
| JHEMCUF405WING | UART6 |
| JHEMCUH743HD | UART4 |
| KAKUTEH7 | UART1 and UART2* |
| KAKUTEH7WING | UART6 |
| MAMBAF405_2022A | UART4 |
| MAMBAF405US | UART4 |
| MAMBAF722 | UART4 |
| MAMBAF722 APP | UART4*|
| MAMBAF722WING | UART4 |
| MAMBAF722_X8 | UART4 |
| MAMBAH743 | UART4* |
| MATEKF405SE | UART1 |
| NEUTRONRCH743BT | UART3* |
| SDMODELH7V1 | UART1 and UART2 |
| SKYSTARSH743HD | UART4 |
| SPEEDYBEEF4 | UART5* |
| SPEEDYBEEF405MINI | UART4* |
| SPEEDYBEEF405V3 | UART4* |
| SPEEDYBEEF405V4 | UART4* |
| SPEEDYBEEF405WING | UART6 |
| SPEEDYBEEF7 | UART6 |
| SPRACINGF4EVO | UART1 |
| TMOTORF7V2 | UART5 |
(*) No Pads/Pins, Port is used interally (Bluetooth)
## Custom firmware:
If the FC does not have MSP activated on a UART by default or does not have a connector for it, a custom firmware must be built.
The following procedure describes the process under Windows 10/11:
Please read [Building in Windows 2010 or 11 with Linux Subsystem](https://github.com/iNavFlight/inav/blob/master/docs/development/Building%20in%20Windows%2010%20or%2011%20with%20Linux%20Subsystem.md)
and follow the instructions up to "Building with Make".
In the step 'prepare build environment' add the option `-DMSP_UART=SERIAL_PORT_USARTX` to `cmake`
Replace the X in SERIAL_PORT_USARTX with the number of UART/serial port on which MSP is to be activated.
Example:
For UART 2: `cmake -DMSP_UART=SERIAL_PORT_USART2 ..`
For UART 3: `cmake -DMSP_UART=SERIAL_PORT_USART3 ..`
etc.
Build the firmware as described in the document above (`make [YOUR_TARGET]`).
## Flashing via Uart:
1. Disconnect ALL peripherals and the USB Cable from the FC. To power the FC use a battery or use the 5V provided from the USB/Serial Converter.
2. Connect UART 1 or 3 (other UARTS will not work) and GND to the USB/Serial converter (RX -> TX, TX -> RX)
3. Keep the boot/dfu button pressed
4. Switch on the FC / supply with power
5. Start STM32 CubeProgrammer and go to "Erasing & Programming", second option in the menu.
6. Select UART (blue dropdown field) and select the COM port of the USB/Serial adapter and press "Connect". The corresponding processor should now be displayed below.
7. Click on "Full flash erase". This is also necessary if you are flashing the same firmware version that was previously on the FC, otherwise MSP may not be activated on the UART.
8. Under "Download" load the previously created firmware (`INAV_X.X.X_[Your Target].hex`) or the standard firmware if UART is already activated there. The option "Verify programming" is optional but recommended. Make sure that "Skip flash erase while programming" is NOT activated.
9. Click "Start Programming"
After the process is completed, switch the FC off and on again and then the Configurator can connect to the FC via USB/serial adapter and the previously configured UART.

43
docs/Profiles.md → docs/Control Profiles.md
View file

@ -1,56 +1,59 @@
# Profiles
# Control Profiles
A profile is a set of configuration settings.
Currently three profiles are supported. The default profile is profile `1`.
Currently, INAV gives you three control profiles. The default control profile is `1`.
## Changing profiles
## Changing contorl profiles
### Stick Commands
Profiles can be selected using a GUI or the following stick combinations:
Control profiles can be selected using a GUI or the following stick combinations:
| Profile | Throttle | Yaw | Pitch | Roll |
| ------- | -------- | ----- | ------ | ------ |
| Profile # | Throttle | Yaw | Pitch | Roll |
| -------- | -------- | ----- | ------ | ------ |
| 1 | Down | Left | Middle | Left |
| 2 | Down | Left | Up | Middle |
| 3 | Down | Left | Middle | Right |
### CLI
The CLI `profile` command can also be used to change profiles:
The CLI `control_profile` command can also be used to change control profiles:
```
profile <index>
control_profile <index>
```
### Programming (4.0.0 onwards)
You can change profiles using the programming frame work. This allows a lot of flexability in how you change profiles.
You can change control profiles using the programming frame work. This allows a lot of flexability in how you change profiles.
For example, using a simple switch on channel 15.
[![For example, using a simple switch](https://i.imgur.com/SS9CaaOl.png)](https://i.imgur.com/SS9CaaO.png)
Or using the speed to change profiles. In this example:
- when lower than 25 cm/s (basically not flying), profiles are not effected.
- Below 2682 cm/s (60 mph | 97 Km/h) use Profile 1
- Above 5364 cm/s (120 mph | 193 Km/h) use Profile 3
- Between 2683 and 5364 cm/s, use Profile 2
Or using the speed to change control profiles. In this example:
- when lower than 25 cm/s (basically not flying), control profiles are not effected.
- Below 2682 cm/s (60 mph | 97 Km/h) use control profile 1
- Above 5364 cm/s (120 mph | 193 Km/h) use control profile 3
- Between 2683 and 5364 cm/s, use control profile 2
[![Using speed to change profiles](https://i.imgur.com/WjkuhhWl.png)](https://i.imgur.com/WjkuhhW.png)
#### Configurator use with profile changing logic.
> [!NOTE]
> From INAV 8.0, the programming framework operator is **Set Control Profile** and the **Flight** Operand is **Active Control Profile**. Pre-INAV 8.0, they were **Set Profile** and **Active Profile** respectively.
If you have logic conditions that change the profiles. You may find that if you manually change the profile using the drop down boxes in the top right of Configurator; that they switch back to a different profile. This is because the logic conditions are still running in the background. If this is the case, the simplest solutuion is to temporarily disable the switches that trigger the `set profile` operations. Remember to re-enable these switches after you have made your changes.
#### Configurator use with control profile changing logic.
If you have logic conditions that change the profiles. You may find that if you manually change the control profile; using the drop down boxes in the top right of Configurator. That they switch back to a different control profile. This is because the logic conditions are still running in the background. If this is the case, the simplest solutuion is to temporarily disable the switches that trigger the `Set Control Profile` operations. Remember to re-enable these switches after you have made your changes.
[![Disabled SET PROFILE switches](https://i.imgur.com/AeH9ll7l.png)](https://i.imgur.com/AeH9ll7.png)
## Profile Contents
The values contained within a profile can be seen by using the CLI `dump profile` command.
The values contained within a control profile can be seen by using the CLI `dump control_profile` command.
e.g
```
# dump profile
# dump control_profile
# profile
profile 1
# control_profile
control_profile 1
set mc_p_pitch = 40
set mc_i_pitch = 30

50
docs/DJI compatible OSD.md Normal file
View file

@ -0,0 +1,50 @@
# DJI compatible MSP DisplayPort OSD (DJI O3 "Canvas Mode")
INAV 6.0 includes a special mode for MSP DisplayPort that supports DJI's incomplete implementations of MSP DisplayPort. This can be found on products like the DJI O3 Air Unit. INAV 6.1 expands this to include HD canvas sizes from BetaFlight 4.4.
Different flight controller firmware have different OSD symbols and elements and require different fonts. BetaFlight's font is a single page and supports a maximum of 256 glyphs, INAV's font is currently 2 pages and supports up to 512 different glyphs. DJI's font is single page and based, but not the same as, BetaFlight's font.
While there is some overlap between the glyphs in DJI and INAV, it is not possible to perform a 1 to 1 mapping for all the them. In cases where there is no suitable glyph in the DJI font, a question mark `?` will be displayed.
This mode can be enabled by selecting DJI43COMPAT or DJIHDCOMPAT as video format in the OSD tab of the configurator or by typing the following command on the CLI:
`set osd_video_system = DJI43COMPAT`
or
`set osd_video_system = DJIHDCOMPAT`
## Limitations
* Canvas size needs to be manually changed to HD on the Display menu in DJI's goggles (you may need a firmware update) and set as DJIHDCOMPAT in the OSD tab of the configurator.
* Unsupported Glyphs show up as `?`
## FAQ
### I see a lot of `?` on my OSD.
That is expected. When your INAV OSD widgets use glyphs that don't have a suitable mapping in DJI's font.
### Does it work with the G2 and Original Air Unit/Vista?
Yes.
### Is this a replacement for WTFOS?
Not exactly. WTFOS is a full implementation of MSP-Displayport for rooted Air Unit/Vista/Googles V2 and actually works much better than DJI compatibility mode. It can use all of INAV's OSD elements as intended. If you have the option of WTFOS or DJI compatability mode. WTFOS is the best option.
### Can INAV fix DJI's product?
No. OSD renderinng happens on the googles/air unit side of things. Please ask DJI to fix their incomplete MSP DisplayPort implemenation. You can probably request it in [DJI's forum](https://forum.dji.com/forum.php?mod=forumdisplay&fid=129&filter=typeid&typeid=767). To see what you're missing out on with O3. Check out what WTFOS did with the original system. Not only could the pilots upload the fonts of their choosing (who doesn't want a cool SneakyFPV font on their OSD). But there were no problems supporting and firmware. Plus, there was even an option to save the OSD to a file and overlay that over your DVR video. If you're reading this far. Please recommend to DJI that they fix their product, to at least what was possible with WTFOS.
### DJI's font now has more symbols, can you update INAV?
Maybe. If a future version of DJI's font includes more Glyphs that can be mapped into INAV. It is fairly simple to add the mapping. However, the best solution would be full support of MSP DisplayPort by DJI. Then there will never be an issue with missing icons. As the latest INAV font would be able to be uploaded on to the goggles.
### Can you replace glyph `X` with text `x description`?
While it might technically be possible to replace some glyphs with text in multiple cells, it will introduce a lot of complexity in the OSD rendering and configuration for something we hope is a temporary workaround.
### Does DJI support Canvas Mode?
Actually, no. What DJI calls Canvas Mode is actually MSP DisplayPort and is a character based OSD. Currently, the only true implementaion of Canvas Mode is with FrSKY PixelOSD. This was found on some F722 flight controllers from Matek.

192
docs/Programming Framework.md
View file

@ -44,13 +44,13 @@ IPF can be edited using INAV Configurator user interface, or via CLI. To use COn
| Operation ID | Name | Notes |
|---------------|-------------------------------|-------|
| 0 | TRUE | Always evaluates as true |
| 1 | EQUAL | Evaluates `false` if `false` or `0` |
| 2 | GREATER_THAN | `true` if `Operand A` is a higher value than `Operand B` |
| 3 | LOWER_THAN | `true` if `Operand A` is a lower value than `Operand B` |
| 4 | LOW | `true` if `<1333` |
| 5 | MID | `true` if `>=1333 and <=1666` |
| 6 | HIGH | `true` if `>1666` |
| 0 | True | Always evaluates as true |
| 1 | Equal (A=B) | Evaluates `false` if `false` or `0` |
| 2 | Greater Than (A>B) | `true` if `Operand A` is a higher value than `Operand B` |
| 3 | Lower Than (A<B) | `true` if `Operand A` is a lower value than `Operand B` |
| 4 | Low | `true` if `<1333` |
| 5 | Mid | `true` if `>=1333 and <=1666` |
| 6 | High | `true` if `>1666` |
| 7 | AND | `true` if `Operand A` and `Operand B` are the same value or both `true` |
| 8 | OR | `true` if `Operand A` and/or `OperandB` is `true` |
| 9 | XOR | `true` if `Operand A` or `Operand B` is `true`, but not both |
@ -67,96 +67,100 @@ IPF can be edited using INAV Configurator user interface, or via CLI. To use COn
| 19 | Increase GVAR | Increase the GVAR indexed by `Operand A` (use `Value 1` for Global Variable 1) with value from `Operand B` |
| 20 | Decrease GVAR | Decrease the GVAR indexed by `Operand A` (use `Value 1` for Global Variable 1) with value from `Operand B` |
| 21 | Set IO Port | Set I2C IO Expander pin `Operand A` to value of `Operand B`. `Operand A` accepts values `0-7` and `Operand B` accepts `0` and `1` |
| 22 | OVERRIDE_ARMING_SAFETY | Allows the craft to arm on any angle even without GPS fix. WARNING: This bypasses all safety checks, even that the throttle is low, so use with caution. If you only want to check for certain conditions, such as arm without GPS fix. You will need to add logic conditions to check the throttle is low. |
| 23 | OVERRIDE_THROTTLE_SCALE | Override throttle scale to the value defined by operand. Operand type `0` and value `50` means throttle will be scaled by 50%. |
| 24 | SWAP_ROLL_YAW | basically, when activated, yaw stick will control roll and roll stick will control yaw. Required for tail-sitters VTOL during vertical-horizonral transition when body frame changes |
| 25 | SET_VTX_POWER_LEVEL | Sets VTX power level. Accepted values are `0-3` for SmartAudio and `0-4` for Tramp protocol |
| 26 | INVERT_ROLL | Inverts ROLL axis input for PID/PIFF controller |
| 27 | INVERT_PITCH | Inverts PITCH axis input for PID/PIFF controller |
| 28 | INVERT_YAW | Inverts YAW axis input for PID/PIFF controller |
| 29 | OVERRIDE_THROTTLE | Override throttle value that is fed to the motors by mixer. Operand is scaled in us. `1000` means throttle cut, `1500` means half throttle |
| 30 | SET_VTX_BAND | Sets VTX band. Accepted values are `1-5` |
| 31 | SET_VTX_CHANNEL | Sets VTX channel. Accepted values are `1-8` |
| 32 | SET_OSD_LAYOUT | Sets OSD layout. Accepted values are `0-3` |
| 22 | Override Arming Safety | Allows the craft to arm on any angle even without GPS fix. WARNING: This bypasses all safety checks, even that the throttle is low, so use with caution. If you only want to check for certain conditions, such as arm without GPS fix. You will need to add logic conditions to check the throttle is low. |
| 23 | Override Throttle Scale | Override throttle scale to the value defined by operand. Operand type `0` and value `50` means throttle will be scaled by 50%. |
| 24 | Swap Roll & Yaw | basically, when activated, yaw stick will control roll and roll stick will control yaw. Required for tail-sitters VTOL during vertical-horizonral transition when body frame changes |
| 25 | Set VTx Power Level | Sets VTX power level. Accepted values are `0-3` for SmartAudio and `0-4` for Tramp protocol |
| 26 | Invert Roll | Inverts ROLL axis input for PID/PIFF controller |
| 27 | Invert Pitch | Inverts PITCH axis input for PID/PIFF controller |
| 28 | Invert Yaw | Inverts YAW axis input for PID/PIFF controller |
| 29 | Override Throttlw | Override throttle value that is fed to the motors by mixer. Operand is scaled in us. `1000` means throttle cut, `1500` means half throttle |
| 30 | Set VTx Band | Sets VTX band. Accepted values are `1-5` |
| 31 | Set VTx Channel | Sets VTX channel. Accepted values are `1-8` |
| 32 | Set OSD Layout | Sets OSD layout. Accepted values are `0-3` |
| 33 | Trigonometry: Sine | Computes SIN of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` |
| 34 | Trigonometry: Cosine | Computes COS of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` |
| 35 | Trigonometry: Tangent | Computes TAN of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` |
| 36 | MAP_INPUT | Scales `Operand A` from [`0` : `Operand B`] to [`0` : `1000`]. Note: input will be constrained and then scaled |
| 37 | MAP_OUTPUT | Scales `Operand A` from [`0` : `1000`] to [`0` : `Operand B`]. Note: input will be constrained and then scaled |
| 38 | RC_CHANNEL_OVERRIDE | Overrides channel set by `Operand A` to value of `Operand B`. Note operand A should normally be set as a "Value", NOT as "Get RC Channel"|
| 39 | SET_HEADING_TARGET | Sets heading-hold target to `Operand A`, in degrees. Value wraps-around. |
| 36 | Map Input | Scales `Operand A` from [`0` : `Operand B`] to [`0` : `1000`]. Note: input will be constrained and then scaled |
| 37 | Map Output | Scales `Operand A` from [`0` : `1000`] to [`0` : `Operand B`]. Note: input will be constrained and then scaled |
| 38 | Override RC Channel | Overrides channel set by `Operand A` to value of `Operand B`. Note operand A should normally be set as a "Value", NOT as "Get RC Channel"|
| 39 | Set Heading Target | Sets heading-hold target to `Operand A`, in centidegrees. Value wraps-around. |
| 40 | Modulo | Modulo. Divide `Operand A` by `Operand B` and returns the remainder |
| 41 | LOITER_RADIUS_OVERRIDE | Sets the loiter radius to `Operand A` [`0` : `100000`] in cm. If the value is lower than the loiter radius set in the **Advanced Tuning**, that will be used. |
| 42 | SET_PROFILE | Sets the active config profile (PIDFF/Rates/Filters/etc) to `Operand A`. `Operand A` must be a valid profile number, currently from 1 to 3. If not, the profile will not change |
| 41 | Override Loiter Radius | Sets the loiter radius to `Operand A` [`0` : `100000`] in cm. If the value is lower than the loiter radius set in the **Advanced Tuning**, that will be used. |
| 42 | Set Control Profile | Sets the active config profile (PIDFF/Rates/Filters/etc) to `Operand A`. `Operand A` must be a valid profile number, currently from 1 to 3. If not, the profile will not change |
| 43 | Use Lowest Value | Finds the lowest value of `Operand A` and `Operand B` |
| 44 | Use Highest Value | Finds the highest value of `Operand A` and `Operand B` |
| 45 | FLIGHT_AXIS_ANGLE_OVERRIDE | Sets the target attitude angle for axis. In other words, when active, it enforces Angle mode (Heading Hold for Yaw) on this axis (Angle mode does not have to be active). `Operand A` defines the axis: `0` - Roll, `1` - Pitch, `2` - Yaw. `Operand B` defines the angle in degrees |
| 46 | FLIGHT_AXIS_RATE_OVERRIDE | Sets the target rate (rotation speed) for axis. `Operand A` defines the axis: `0` - Roll, `1` - Pitch, `2` - Yaw. `Operand B` defines the rate in degrees per second |
| 47 | EDGE | Momentarily true when triggered by `Operand A`. `Operand A` is the activation operator [`boolean`], `Operand B` _(Optional)_ is the time for the edge to stay active [ms]. After activation, operator will return `true` until the time in Operand B is reached. If a pure momentary edge is wanted. Just leave `Operand B` as the default `Value: 0` setting. |
| 48 | DELAY | Delays activation after being triggered. This will return `true` when `Operand A` _is_ true, and the delay time in `Operand B` [ms] has been exceeded. |
| 49 | TIMER | A simple on - off timer. `true` for the duration of `Operand A` [ms]. Then `false` for the duration of `Operand B` [ms]. |
| 50 | DELTA | This returns `true` when the value of `Operand A` has changed by the value of `Operand B` or greater within 100ms. |
| 51 | APPROX_EQUAL | `true` if `Operand B` is within 1% of `Operand A`. |
| 52 | LED_PIN_PWM | Value `Operand A` from [`0` : `100`] starts PWM generation on LED Pin. See [LED pin PWM](LED%20pin%20PWM.md). Any other value stops PWM generation (stop to allow ws2812 LEDs updates in shared modes)|
| 45 | Flight Axis Angle Override | Sets the target attitude angle for axis. In other words, when active, it enforces Angle mode (Heading Hold for Yaw) on this axis (Angle mode does not have to be active). `Operand A` defines the axis: `0` - Roll, `1` - Pitch, `2` - Yaw. `Operand B` defines the angle in degrees |
| 46 | Flight Axis Rate Override | Sets the target rate (rotation speed) for axis. `Operand A` defines the axis: `0` - Roll, `1` - Pitch, `2` - Yaw. `Operand B` defines the rate in degrees per second |
| 47 | Edge | Momentarily true when triggered by `Operand A`. `Operand A` is the activation operator [`boolean`], `Operand B` _(Optional)_ is the time for the edge to stay active [ms]. After activation, operator will return `true` until the time in Operand B is reached. If a pure momentary edge is wanted. Just leave `Operand B` as the default `Value: 0` setting. |
| 48 | Delay | Delays activation after being triggered. This will return `true` when `Operand A` _is_ true, and the delay time in `Operand B` [ms] has been exceeded. |
| 49 | Timer | A simple on - off timer. `true` for the duration of `Operand A` [ms]. Then `false` for the duration of `Operand B` [ms]. |
| 50 | Delta (|A| >= B) | This returns `true` when the value of `Operand A` has changed by the value of `Operand B` or greater within 100ms. |
| 51 | Approx Equals (A ~ B) | `true` if `Operand B` is within 1% of `Operand A`. |
| 52 | LED Pin PWM | Value `Operand A` from [`0` : `100`] starts PWM generation on LED Pin. See [LED pin PWM](LED%20pin%20PWM.md). Any other value stops PWM generation (stop to allow ws2812 LEDs updates in shared modes). |
| 53 | Disable GPS Sensor Fix | Disables the GNSS sensor fix. For testing GNSS failure. |
| 54 | Mag calibration | Trigger a magnetometer calibration. |
### Operands
| Operand Type | Name | Notes |
|---------------|-----------------------|-------|
| 0 | VALUE | Value derived from `value` field |
| 1 | GET_RC_CHANNEL | `value` points to RC channel number, indexed from 1 |
| 2 | FLIGHT | `value` points to flight parameter table |
| 3 | FLIGHT_MODE | `value` points to flight modes table |
| 4 | LC | `value` points to other logic condition ID |
| 5 | GVAR | Value stored in Global Variable indexed by `value`. `GVAR 1` means: value in GVAR 1 |
| 5 | PID | Output of a Programming PID indexed by `value`. `PID 1` means: value in PID 1 |
| 0 | Value | Value derived from `value` field |
| 1 | Get RC Channel | `value` points to RC channel number, indexed from 1 |
| 2 | Flight | `value` points to **Flight** Parameters table |
| 3 | Flight Mode | `value` points to **Flight_Mode** table |
| 4 | Logic Condition | `value` points to other logic condition ID |
| 5 | Get Global Variable | Value stored in Global Variable indexed by `value`. `GVAR 1` means: value in GVAR 1 |
| 5 | Programming PID | Output of a Programming PID indexed by `value`. `PID 1` means: value in PID 1 |
| 6 | Waypoints | `value` points to the **Waypoint** parameter table |
#### FLIGHT
#### Flight Parameters
| Operand Value | Name | Notes |
|---------------|-------------------------------|-------|
| 0 | ARM_TIMER | in `seconds` |
| 1 | HOME_DISTANCE | in `meters` |
| 2 | TRIP_DISTANCE | in `meters` |
|---------------|---------------------------------------|-------|
| 0 | ARM Timer [s] | Time since armed in `seconds` |
| 1 | Home Distance [m] | distance from home in `meters` |
| 2 | Trip distance [m] | Trip distance in `meters` |
| 3 | RSSI | |
| 4 | VBAT | in `Volts * 100`, eg. `12.1V` is `1210` |
| 5 | CELL_VOLTAGE | in `Volts * 100`, eg. `12.1V` is `1210` |
| 6 | CURRENT | in `Amps * 100`, eg. `9A` is `900` |
| 7 | MAH_DRAWN | in `mAh` |
| 8 | GPS_SATS | |
| 9 | GROUD_SPEED | in `cm/s` |
| 10 | 3D_SPEED | in `cm/s` |
| 11 | AIR_SPEED | in `cm/s` |
| 12 | ALTITUDE | in `cm` |
| 13 | VERTICAL_SPEED | in `cm/s` |
| 14 | TROTTLE_POS | in `%` |
| 15 | ATTITUDE_ROLL | in `degrees` |
| 16 | ATTITUDE_PITCH | in `degrees` |
| 17 | IS_ARMED | boolean `0`/`1` |
| 18 | IS_AUTOLAUNCH | boolean `0`/`1` |
| 19 | IS_ALTITUDE_CONTROL | boolean `0`/`1` |
| 20 | IS_POSITION_CONTROL | boolean `0`/`1` |
| 21 | IS_EMERGENCY_LANDING | boolean `0`/`1` |
| 22 | IS_RTH | boolean `0`/`1` |
| 23 | IS_LANDING | boolean `0`/`1` |
| 24 | IS_FAILSAFE | boolean `0`/`1` |
| 25 | STABILIZED_ROLL | Roll PID controller output `[-500:500]` |
| 26 | STABILIZED_PITCH | Pitch PID controller output `[-500:500]` |
| 27 | STABILIZED_YAW | Yaw PID controller output `[-500:500]` |
| 28 | 3D HOME_DISTANCE | in `meters`, calculated from HOME_DISTANCE and ALTITUDE using Pythagorean theorem |
| 29 | CROSSFIRE LQ | Crossfire Link quality as returned by the CRSF protocol |
| 30 | CROSSFIRE SNR | Crossfire SNR as returned by the CRSF protocol |
| 31 | GPS_VALID | boolean `0`/`1`. True when the GPS has a valid 3D Fix |
| 32 | LOITER_RADIUS | The current loiter radius in cm. |
| 33 | ACTIVE_PROFILE | integer for the active config profile `[1..MAX_PROFILE_COUNT]` |
| 34 | BATT_CELLS | Number of battery cells detected |
| 35 | AGL_STATUS | boolean `1` when AGL can be trusted, `0` when AGL estimate can not be trusted |
| 36 | AGL | integer Above The Groud Altitude in `cm` |
| 37 | RANGEFINDER_RAW | integer raw distance provided by the rangefinder in `cm` |
| 38 | ACTIVE_MIXER_PROFILE | Which mixers are currently active (for vtol etc) |
| 39 | MIXER_TRANSITION_ACTIVE | Currently switching between mixers (quad to plane etc) |
| 40 | ATTITUDE_YAW | current heading (yaw) in `degrees` |
| 41 | FW Land Sate | integer `1` - `5`, indicates the status of the FW landing, 0 Idle, 1 Downwind, 2 Base Leg, 3 Final Approach, 4 Glide, 5 Flare |
| 4 | Vbat [centi-Volt] [1V = 100] | VBAT Voltage in `Volts * 100`, eg. `12.1V` is `1210` |
| 5 | Cell voltage [centi-Volt] [1V = 100] | Average cell voltage in `Volts * 100`, eg. `12.1V` is `1210` |
| 6 | Current [centi-Amp] [1A = 100] | Current in `Amps * 100`, eg. `9A` is `900` |
| 7 | Current drawn [mAh] | Total used current in `mAh` |
| 8 | GPS Sats | |
| 9 | Ground speed [cm/s] | Ground speed in `cm/s` |
| 10 | 3D speed [cm/s] | 3D speed in `cm/s` |
| 11 | Air speed [cm/s] | Air speed in `cm/s` |
| 12 | Altitude [cm] | Altitude in `cm` |
| 13 | Vertical speed [cm/s] | Vertical speed in `cm/s` |
| 14 | Throttle position [%] | Throttle position in `%` |
| 15 | Roll [deg] | Roll attitude in `degrees` |
| 16 | Pitch [deg] | Pitch attitude in `degrees` |
| 17 | Is Armed | Is the system armed? boolean `0`/`1` |
| 18 | Is Autolaunch | Is auto launch active? boolean `0`/`1` |
| 19 | Is Controlling Altitude | Is altitude being controlled? boolean `0`/`1` |
| 20 | Is Controlling Position | Is the position being controlled? boolean `0`/`1` |
| 21 | Is Emergency Landing | Is the aircraft emergency landing? boolean `0`/`1` |
| 22 | Is RTH | Is RTH active? boolean `0`/`1` |
| 23 | Is Landing | Is the aircaft automatically landing? boolean `0`/`1` |
| 24 | Is Failsafe | Is the flight controller in a failsafe? boolean `0`/`1` |
| 25 | Stabilized Roll | Roll PID controller output `[-500:500]` |
| 26 | Stabilized Pitch | Pitch PID controller output `[-500:500]` |
| 27 | Stabilized Yaw | Yaw PID controller output `[-500:500]` |
| 28 | 3D home distance [m] | 3D distance to home in `meters`. Calculated from Home distance and Altitude using Pythagorean theorem |
| 29 | CRSF LQ | Link quality as returned by the CRSF protocol |
| 30 | CRSF SNR | SNR as returned by the CRSF protocol |
| 31 | GPS Valid Fix | Boolean `0`/`1`. True when the GPS has a valid 3D Fix |
| 32 | Loiter Radius [cm] | The current loiter radius in cm. |
| 33 | Active Control Profile | Integer for the active config profile `[1..MAX_PROFILE_COUNT]` |
| 34 | Battery cells | Number of battery cells detected |
| 35 | AGL status [0/1] | Boolean `1` when AGL can be trusted, `0` when AGL estimate can not be trusted |
| 36 | AGL [cm] | Integer altitude above The Groud Altitude in `cm` |
| 37 | Rangefinder [cm] | Integer raw distance provided by the rangefinder in `cm` |
| 38 | Active Mixer Profile | Which mixer is currently active (for vtol etc) |
| 39 | Mixer Transition Active | Boolean `0`/`1`. Are we currently switching between mixers (quad to plane etc) |
| 40 | Yaw [deg] | Current heading (yaw) in `degrees` |
| 41 | FW Land Sate | Integer `1` - `5`, indicates the status of the FW landing, 0 Idle, 1 Downwind, 2 Base Leg, 3 Final Approach, 4 Glide, 5 Flare |
| 42 | Current battery profile | The active battery profile. Integer `[1..MAX_PROFILE_COUNT]` |
#### FLIGHT_MODE
@ -164,22 +168,22 @@ The flight mode operands return `true` when the mode is active. These are modes
| Operand Value | Name | Notes |
|---------------|-------------------|-------|
| 0 | FAILSAFE | `true` when a **Failsafe** state has been triggered. |
| 1 | MANUAL | `true` when you are in the **Manual** flight mode. |
| 0 | Failsafe | `true` when a **Failsafe** state has been triggered. |
| 1 | Manual | `true` when you are in the **Manual** flight mode. |
| 2 | RTH | `true` when you are in the **Return to Home** flight mode. |
| 3 | POSHOLD | `true` when you are in the **Position Hold** or **Loiter** flight modes. |
| 4 | CRUISE | `true` when you are in the **Cruise** flight mode. |
| 5 | ALTHOLD | `true` when you the **Altitude Hold** flight mode modifier is active. |
| 6 | ANGLE | `true` when you are in the **Angle** flight mode. |
| 7 | HORIZON | `true` when you are in the **Horizon** flight mode. |
| 8 | AIR | `true` when you the **Airmode** flight mode modifier is active. |
| 3 | Position Hold | `true` when you are in the **Position Hold** or **Loiter** flight modes. |
| 4 | Cruise | `true` when you are in the **Cruise** flight mode. |
| 5 | Altitude Hold | `true` when you the **Altitude Hold** flight mode modifier is active. |
| 6 | Angle | `true` when you are in the **Angle** flight mode. |
| 7 | Horizon | `true` when you are in the **Horizon** flight mode. |
| 8 | Air | `true` when you the **Airmode** flight mode modifier is active. |
| 9 | USER 1 | `true` when the **USER 1** mode is active. |
| 10 | USER 2 | `true` when the **USER 2** mode is active. |
| 11 | COURSE_HOLD | `true` when you are in the **Course Hold** flight mode. |
| 11 | Course Hold | `true` when you are in the **Course Hold** flight mode. |
| 12 | USER 3 | `true` when the **USER 3** mode is active. |
| 13 | USER 4 | `true` when the **USER 4** mode is active. |
| 14 | ACRO | `true` when you are in the **Acro** flight mode. |
| 15 | WAYPOINT_MISSION | `true` when you are in the **WP Mission** flight mode. |
| 14 | Acro | `true` when you are in the **Acro** flight mode. |
| 15 | Waypoint Mission | `true` when you are in the **WP Mission** flight mode. |
#### WAYPOINTS
@ -335,5 +339,3 @@ choose *value* and enter the channel number. Choosing "get RC value" is a common
which does something other than what you probably want.
![screenshot of override an RC channel with a value](./assets/images/ipf_set_get_rc_channel.png)

16
docs/Rangefinder.md
View file

@ -21,6 +21,22 @@ Following rangefinders are supported:
* MSP - experimental
* TOF10120 - small & lightweight laser range sensor, usable up to 200cm
* TERARANGER EVO - 30cm to 600cm, depends on version https://www.terabee.com/sensors-modules/lidar-tof-range-finders/#individual-distance-measurement-sensors
* NRA15/NRA24 - experimental, UART version
#### NRA15/NRA24
NRA15/NRA24 from nanoradar use US-D1_V0 or NRA protocol, it depends which firmware you use. Radar can be set by firmware
to two different resolutions. See table below.
| Radar | Protocol | Resolution | Name in configurator |
|-------|----------|-----------------|----------------------|
| NRA15 | US-D1_V0 | 0-30m (+-4cm) | USD1_V0 |
| NRA15 | NRA | 0-30m (+-4cm) | NRA |
| NRA15 | NRA | 0-100m (+-10cm) | NRA |
| NRA24 | US-D1_V0 | 0-50m (+-4cm) | USD1_V0 |
| NRA24 | US-D1_V0 | 0-200m (+-10cm) | USD1_V0 |
| NRA24 | NRA | 0-50m (+-4cm) | NRA |
| NRA24 | NRA | 0-200m (+-10cm) | NRA |
## Connections

68
docs/Settings.md
View file

@ -1302,13 +1302,33 @@ Fixed-wing rate stabilisation I-gain for YAW
---
### fw_iterm_limit_stick_position
### fw_iterm_lock_engage_threshold
Iterm is not allowed to grow when stick position is above threshold. This solves the problem of bounceback or followthrough when full stick deflection is applied on poorely tuned fixed wings. In other words, stabilization is partialy disabled when pilot is actively controlling the aircraft and active when sticks are not touched. `0` mean stick is in center position, `1` means it is fully deflected to either side
Defines error rate (in percents of max rate) when Iterm Lock is engaged when sticks are release. Iterm Lock will stay active until error drops below this number
| Default | Min | Max |
| --- | --- | --- |
| 0.5 | 0 | 1 |
| 10 | 5 | 25 |
---
### fw_iterm_lock_rate_threshold
Defines rate percentage when full P I and D attenuation should happen. 100 disables Iterm Lock for P and D term
| Default | Min | Max |
| --- | --- | --- |
| 40 | 10 | 100 |
---
### fw_iterm_lock_time_max_ms
Defines max time in milliseconds for how long ITerm Lock will shut down Iterm after sticks are release
| Default | Min | Max |
| --- | --- | --- |
| 500 | 100 | 1000 |
---
@ -1882,16 +1902,6 @@ Allows to chose when the home position is reset. Can help prevent resetting home
---
### inav_use_gps_no_baro
_// TODO_
| Default | Min | Max |
| --- | --- | --- |
| OFF | OFF | ON |
---
### inav_w_acc_bias
Weight for accelerometer drift estimation
@ -2842,6 +2852,26 @@ Enable the possibility to manually increase the throttle in auto throttle contro
---
### nav_fw_alt_control_response
Adjusts the deceleration response of fixed wing altitude control as the target altitude is approached. Decrease value to help avoid overshooting the target altitude.
| Default | Min | Max |
| --- | --- | --- |
| 20 | 5 | 100 |
---
### nav_fw_auto_climb_rate
Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s]
| Default | Min | Max |
| --- | --- | --- |
| 500 | 10 | 2000 |
---
### nav_fw_bank_angle
Max roll angle when rolling / turning in GPS assisted modes, is also restrained by global max_angle_inclination_rll
@ -2998,7 +3028,7 @@ Forward acceleration threshold for bungee launch or throw launch [cm/s/s], 1G =
| Default | Min | Max |
| --- | --- | --- |
| 1863 | 1500 | 20000 |
| 1863 | 1350 | 20000 |
---
@ -4462,6 +4492,16 @@ Value under which the OSD axis g force indicators will blink (g)
---
### osd_highlight_djis_missing_font_symbols
Show question marks where there is no symbol in the DJI font to represent the INAV OSD element's symbol. When off, blank spaces will be used. Only relevent for DJICOMPAT modes.
| Default | Min | Max |
| --- | --- | --- |
| ON | OFF | ON |
---
### osd_home_position_arm_screen
Should home position coordinates be displayed on the arming screen.

19
docs/development/Building in Gitpod.md Normal file
View file

@ -0,0 +1,19 @@
# Building in Gitpod
Gitpod offers an online build environment for building INAV targets.
## Setting up the environment and building targets
1. Go to https://gitpod.io/new
1. Paste `https://github.com/iNavFlight/inav/tree/[version]` into the field called "Select a repository".
1. Ensure that you substitute [version] (e.g. 7.1.0) with the version number of INAV that you want to build.
1. Cick on the link that shows in the drop down and Gitpod will atomatically selects the adequate Editor and Browser.
1. Leave the other fields as default and click "Continue". Your build environment will be created.
1. At the bottom of the page, you will see a command line. Type `make [TARGET]` and wait for the target to be built.
1. Once the build has finished, navigate to the build folder using `cd build`.
1. Once in the folder, run `objcopy -O ihex -R .eeprom [TARGET].elf [TARGET].hex` to convert the `.elf` file to a `.hex` file.
1. Your new target `.hex` binary will be located in a folder called `bin`, which can be found at the top left of the page.
NOTE: You can use this method to build your forks as well. Just paste in the link to your fork and follow the rest of the steps.
You are done!

5
readme.md
View file

@ -6,6 +6,10 @@
> INAV 7 is the last INAV official release available for F411 based flight controllers. The next milestone, INAV 8 will not be available for F411 boards.
# ICM426xx IMUs PSA
> The filtering settings for the ICM426xx has changed to match what is used by Ardupilot and Betaflight in INAV 7.1. When upgrading from older versions you may need to recalibrate the Accelerometer and if you are not using INAV's default tune you may also want to check if the tune is still good.
![INAV](http://static.rcgroups.net/forums/attachments/6/1/0/3/7/6/a9088858-102-inav.png)
# PosHold, Navigation and RTH without compass PSA
@ -51,7 +55,6 @@ Fly safe, fly smart with INAV 7.1 and a compass by your side!
* SmartAudio and IRC Tramp VTX support
* Telemetry: SmartPort, FPort, MAVlink, LTM, CRSF
* Multi-color RGB LED Strip support
* On Screen Display (OSD) - both character and pixel style
* And many more!
For a list of features, changes and some discussion please review consult the releases [page](https://github.com/iNavFlight/inav/releases) and the documentation.

7
src/main/CMakeLists.txt
View file

@ -1,3 +1,4 @@
main_sources(COMMON_SRC
main.c
@ -487,6 +488,8 @@ main_sources(COMMON_SRC
io/rangefinder.h
io/rangefinder_msp.c
io/rangefinder_benewake.c
io/rangefinder_usd1_v0.c
io/rangefinder_nanoradar.c
io/rangefinder_fake.c
io/opflow.h
io/opflow_cxof.c
@ -499,8 +502,8 @@ main_sources(COMMON_SRC
io/displayport_max7456.h
io/displayport_msp.c
io/displayport_msp.h
io/displayport_msp_bf_compat.c
io/displayport_msp_bf_compat.h
io/displayport_msp_dji_compat.c
io/displayport_msp_dji_compat.h
io/displayport_oled.c
io/displayport_oled.h
io/displayport_msp_osd.c

55
src/main/blackbox/blackbox.c
View file

@ -402,12 +402,21 @@ static const blackboxSimpleFieldDefinition_t blackboxGpsHFields[] = {
// Rarely-updated fields
static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = {
/* "flightModeFlags" renamed internally to more correct ref of rcModeFlags, since it logs rc boxmode selections,
* but name kept for external compatibility reasons.
* "activeFlightModeFlags" logs actual active flight modes rather than rc boxmodes.
* 'active' should at least distinguish it from the existing "flightModeFlags" */
{"activeWpNumber", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
{"flightModeFlags", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
{"flightModeFlags2", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
{"activeFlightModeFlags", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
{"stateFlags", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
{"failsafePhase", -1, UNSIGNED, PREDICT(0), ENCODING(TAG2_3S32)},
{"rxSignalReceived", -1, UNSIGNED, PREDICT(0), ENCODING(TAG2_3S32)},
{"rxFlightChannelsValid", -1, UNSIGNED, PREDICT(0), ENCODING(TAG2_3S32)},
{"rxUpdateRate", -1, UNSIGNED, PREDICT(PREVIOUS), ENCODING(UNSIGNED_VB)},
{"hwHealthStatus", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
{"powerSupplyImpedance", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
@ -436,8 +445,6 @@ static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = {
{"escRPM", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
{"escTemperature", -1, SIGNED, PREDICT(PREVIOUS), ENCODING(SIGNED_VB)},
#endif
{"rxUpdateRate", -1, UNSIGNED, PREDICT(PREVIOUS), ENCODING(UNSIGNED_VB)},
{"activeWpNumber", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
};
typedef enum BlackboxState {
@ -533,7 +540,9 @@ typedef struct blackboxGpsState_s {
// This data is updated really infrequently:
typedef struct blackboxSlowState_s {
uint32_t flightModeFlags; // extend this data size (from uint16_t)
uint32_t rcModeFlags;
uint32_t rcModeFlags2;
uint32_t activeFlightModeFlags;
uint32_t stateFlags;
uint8_t failsafePhase;
bool rxSignalReceived;
@ -566,7 +575,7 @@ extern boxBitmask_t rcModeActivationMask;
static BlackboxState blackboxState = BLACKBOX_STATE_DISABLED;
static uint32_t blackboxLastArmingBeep = 0;
static uint32_t blackboxLastFlightModeFlags = 0;
static uint32_t blackboxLastRcModeFlags = 0;
static struct {
uint32_t headerIndex;
@ -1260,7 +1269,10 @@ static void writeSlowFrame(void)
blackboxWrite('S');
blackboxWriteUnsignedVB(slowHistory.flightModeFlags);
blackboxWriteUnsignedVB(slowHistory.activeWpNumber);
blackboxWriteUnsignedVB(slowHistory.rcModeFlags);
blackboxWriteUnsignedVB(slowHistory.rcModeFlags2);
blackboxWriteUnsignedVB(slowHistory.activeFlightModeFlags);
blackboxWriteUnsignedVB(slowHistory.stateFlags);
/*
@ -1271,6 +1283,8 @@ static void writeSlowFrame(void)
values[2] = slowHistory.rxFlightChannelsValid ? 1 : 0;
blackboxWriteTag2_3S32(values);
blackboxWriteUnsignedVB(slowHistory.rxUpdateRate);
blackboxWriteUnsignedVB(slowHistory.hwHealthStatus);
blackboxWriteUnsignedVB(slowHistory.powerSupplyImpedance);
@ -1296,8 +1310,6 @@ static void writeSlowFrame(void)
blackboxWriteUnsignedVB(slowHistory.escRPM);
blackboxWriteSignedVB(slowHistory.escTemperature);
#endif
blackboxWriteUnsignedVB(slowHistory.rxUpdateRate);
blackboxWriteUnsignedVB(slowHistory.activeWpNumber);
blackboxSlowFrameIterationTimer = 0;
}
@ -1307,21 +1319,21 @@ static void writeSlowFrame(void)
*/
static void loadSlowState(blackboxSlowState_t *slow)
{
memcpy(&slow->flightModeFlags, &rcModeActivationMask, sizeof(slow->flightModeFlags)); //was flightModeFlags;
// Also log Nav auto selected flight modes rather than just those selected by boxmode
if (!IS_RC_MODE_ACTIVE(BOXANGLE) && FLIGHT_MODE(ANGLE_MODE)) {
slow->flightModeFlags |= (1 << BOXANGLE);
}
slow->activeWpNumber = getActiveWpNumber();
slow->rcModeFlags = rcModeActivationMask.bits[0]; // first 32 bits of boxId_e
slow->rcModeFlags2 = rcModeActivationMask.bits[1]; // remaining bits of boxId_e
// Also log Nav auto enabled flight modes rather than just those selected by boxmode
if (navigationGetHeadingControlState() == NAV_HEADING_CONTROL_AUTO) {
slow->flightModeFlags |= (1 << BOXHEADINGHOLD);
}
if (navigationRequiresTurnAssistance()) {
slow->flightModeFlags |= (1 << BOXTURNASSIST);
slow->rcModeFlags |= (1 << BOXHEADINGHOLD);
}
slow->activeFlightModeFlags = flightModeFlags;
slow->stateFlags = stateFlags;
slow->failsafePhase = failsafePhase();
slow->rxSignalReceived = rxIsReceivingSignal();
slow->rxFlightChannelsValid = rxAreFlightChannelsValid();
slow->rxUpdateRate = getRcUpdateFrequency();
slow->hwHealthStatus = (getHwGyroStatus() << 2 * 0) | // Pack hardware health status into a bit field.
(getHwAccelerometerStatus() << 2 * 1) | // Use raw hardwareSensorStatus_e values and pack them using 2 bits per value
(getHwCompassStatus() << 2 * 2) | // Report GYRO in 2 lowest bits, then ACC, COMPASS, BARO, GPS, RANGEFINDER and PITOT
@ -1373,9 +1385,6 @@ static void loadSlowState(blackboxSlowState_t *slow)
slow->escRPM = escSensor->rpm;
slow->escTemperature = escSensor->temperature;
#endif
slow->rxUpdateRate = getRcUpdateFrequency();
slow->activeWpNumber = getActiveWpNumber();
}
/**
@ -1492,7 +1501,7 @@ void blackboxStart(void)
* it finally plays the beep for this arming event.
*/
blackboxLastArmingBeep = getArmingBeepTimeMicros();
memcpy(&blackboxLastFlightModeFlags, &rcModeActivationMask, sizeof(blackboxLastFlightModeFlags)); // record startup status
memcpy(&blackboxLastRcModeFlags, &rcModeActivationMask, sizeof(blackboxLastRcModeFlags)); // record startup status
blackboxSetState(BLACKBOX_STATE_PREPARE_LOG_FILE);
}
@ -2017,10 +2026,10 @@ static void blackboxCheckAndLogArmingBeep(void)
static void blackboxCheckAndLogFlightMode(void)
{
// Use != so that we can still detect a change if the counter wraps
if (memcmp(&rcModeActivationMask, &blackboxLastFlightModeFlags, sizeof(blackboxLastFlightModeFlags))) {
if (memcmp(&rcModeActivationMask, &blackboxLastRcModeFlags, sizeof(blackboxLastRcModeFlags))) {
flightLogEvent_flightMode_t eventData; // Add new data for current flight mode flags
eventData.lastFlags = blackboxLastFlightModeFlags;
memcpy(&blackboxLastFlightModeFlags, &rcModeActivationMask, sizeof(blackboxLastFlightModeFlags));
eventData.lastFlags = blackboxLastRcModeFlags;
memcpy(&blackboxLastRcModeFlags, &rcModeActivationMask, sizeof(blackboxLastRcModeFlags));
memcpy(&eventData.flags, &rcModeActivationMask, sizeof(eventData.flags));
blackboxLogEvent(FLIGHT_LOG_EVENT_FLIGHTMODE, (flightLogEventData_t *)&eventData);
}

2
src/main/cms/cms_menu_imu.c
View file

@ -222,6 +222,8 @@ static const OSD_Entry cmsx_menuPidAltMagEntries[] =
{
OSD_LABEL_DATA_ENTRY("-- ALT&MAG --", profileIndexString),
OSD_SETTING_ENTRY("FW ALT RESPONSE", SETTING_NAV_FW_ALT_CONTROL_RESPONSE),
OTHER_PIDFF_ENTRY("ALT P", &cmsx_pidPosZ.P),
OTHER_PIDFF_ENTRY("ALT I", &cmsx_pidPosZ.I),
OTHER_PIDFF_ENTRY("ALT D", &cmsx_pidPosZ.D),

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

@ -516,9 +516,25 @@ bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float resu
return sensorCalibrationValidateResult(result);
}
float gaussian(const float x, const float mu, const float sigma) {
return exp(-pow((double)(x - mu), 2) / (2 * pow((double)sigma, 2)));
}
float bellCurve(const float x, const float curveWidth)
{
return powf(M_Ef, -sq(x) / (2.0f * sq(curveWidth)));
return gaussian(x, 0.0f, curveWidth);
}
/**
* @brief Calculate the attenuation of a value using a Gaussian function.
* Retuns 1 for input 0 and ~0 for input width.
* @param input The input value.
* @param width The width of the Gaussian function.
* @return The attenuation of the input value.
*/
float attenuation(const float input, const float width) {
const float sigma = width / 2.35482f; // Approximately width / sqrt(2 * ln(2))
return gaussian(input, 0.0f, sigma);
}
float fast_fsqrtf(const float value) {

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

@ -195,6 +195,8 @@ float acos_approx(float x);
void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count);
float bellCurve(const float x, const float curveWidth);
float attenuation(const float input, const float width);
float gaussian(const float x, const float mu, const float sigma);
float fast_fsqrtf(const float value);
float calc_length_pythagorean_2D(const float firstElement, const float secondElement);
float calc_length_pythagorean_3D(const float firstElement, const float secondElement, const float thirdElement);

3
src/main/drivers/compass/compass_ist8308.c
View file

@ -121,8 +121,9 @@ static bool ist8308Read(magDev_t * mag)
return false;
}
// Invert Y axis to co convert from left to right coordinate system
mag->magADCRaw[X] = (int16_t)(buf[1] << 8 | buf[0]) * LSB2FSV;
mag->magADCRaw[Y] = (int16_t)(buf[3] << 8 | buf[2]) * LSB2FSV;
mag->magADCRaw[Y] = -(int16_t)(buf[3] << 8 | buf[2]) * LSB2FSV;
mag->magADCRaw[Z] = (int16_t)(buf[5] << 8 | buf[4]) * LSB2FSV;
return true;

2
src/main/drivers/compass/compass_ist8310.c
View file

@ -139,7 +139,7 @@ static bool ist8310Read(magDev_t * mag)
return false;
}
// Looks like datasheet is incorrect and we need to invert Y axis to conform to right hand rule
// Invert Y axis to co convert from left to right coordinate system
mag->magADCRaw[X] = (int16_t)(buf[1] << 8 | buf[0]) * LSB2FSV;
mag->magADCRaw[Y] = -(int16_t)(buf[3] << 8 | buf[2]) * LSB2FSV;
mag->magADCRaw[Z] = (int16_t)(buf[5] << 8 | buf[4]) * LSB2FSV;

6
src/main/drivers/light_ws2811strip.c
View file

@ -124,6 +124,10 @@ void ws2811LedStripInit(void)
{
const timerHardware_t * timHw = timerGetByTag(IO_TAG(WS2811_PIN), TIM_USE_ANY);
if (!(timHw->usageFlags & TIM_USE_LED)) { // Check if it has not been reassigned
timHw = timerGetByUsageFlag(TIM_USE_LED); // Get first pin marked as LED
}
if (timHw == NULL) {
return;
}
@ -133,7 +137,7 @@ void ws2811LedStripInit(void)
return;
}
ws2811IO = IOGetByTag(IO_TAG(WS2811_PIN));
ws2811IO = IOGetByTag(timHw->tag); //IOGetByTag(IO_TAG(WS2811_PIN));
IOInit(ws2811IO, OWNER_LED_STRIP, RESOURCE_OUTPUT, 0);
IOConfigGPIOAF(ws2811IO, IOCFG_AF_PP_FAST, timHw->alternateFunction);

4
src/main/drivers/osd.h
View file

@ -49,8 +49,8 @@ typedef enum {
VIDEO_SYSTEM_HDZERO,
VIDEO_SYSTEM_DJIWTF,
VIDEO_SYSTEM_AVATAR,
VIDEO_SYSTEM_BFCOMPAT,
VIDEO_SYSTEM_BFCOMPAT_HD
VIDEO_SYSTEM_DJICOMPAT,
VIDEO_SYSTEM_DJICOMPAT_HD
} videoSystem_e;
typedef enum {

6
src/main/drivers/osd_symbols.h
View file

@ -44,9 +44,9 @@
#define SYM_DBM 0x13 // 019 dBm
#define SYM_SNR 0x14 // 020 SNR
#define SYM_AH_DIRECTION_UP 0x15 // 021 Arrow up AHI
#define SYM_AH_DIRECTION_DOWN 0x16 // 022 Arrow down AHI
#define SYM_DIRECTION 0x17 // 023 to 030, directional little arrows
#define SYM_AH_DECORATION_UP 0x15 // 021 Arrow up AHI
#define SYM_AH_DECORATION_DOWN 0x16 // 022 Arrow down AHI
#define SYM_DECORATION 0x17 // 023 to 030, directional little arrows
#define SYM_VOLT 0x1F // 031 VOLTS
#define SYM_MAH 0x99 // 153 mAh

29
src/main/drivers/pwm_mapping.c
View file

@ -47,6 +47,7 @@ enum {
MAP_TO_NONE,
MAP_TO_MOTOR_OUTPUT,
MAP_TO_SERVO_OUTPUT,
MAP_TO_LED_OUTPUT
};
typedef struct {
@ -167,11 +168,17 @@ static bool checkPwmTimerConflicts(const timerHardware_t *timHw)
#if defined(USE_LED_STRIP)
if (feature(FEATURE_LED_STRIP)) {
const timerHardware_t * ledTimHw = timerGetByTag(IO_TAG(WS2811_PIN), TIM_USE_ANY);
if (ledTimHw != NULL && timHw->tim == ledTimHw->tim) {
for (int i = 0; i < timerHardwareCount; i++) {
if (timHw->tim == timerHardware[i].tim && timerHardware[i].usageFlags & TIM_USE_LED) {
return true;
}
}
//const timerHardware_t * ledTimHw = timerGetByTag(IO_TAG(WS2811_PIN), TIM_USE_ANY);
//if (ledTimHw != NULL && timHw->tim == ledTimHw->tim) {
// return true;
//}
}
#endif
#if defined(USE_ADC)
@ -213,16 +220,16 @@ static bool checkPwmTimerConflicts(const timerHardware_t *timHw)
static void timerHardwareOverride(timerHardware_t * timer) {
switch (timerOverrides(timer2id(timer->tim))->outputMode) {
case OUTPUT_MODE_MOTORS:
if (TIM_IS_SERVO(timer->usageFlags)) {
timer->usageFlags &= ~TIM_USE_SERVO;
timer->usageFlags &= ~(TIM_USE_SERVO|TIM_USE_LED);
timer->usageFlags |= TIM_USE_MOTOR;
}
break;
case OUTPUT_MODE_SERVOS:
if (TIM_IS_MOTOR(timer->usageFlags)) {
timer->usageFlags &= ~TIM_USE_MOTOR;
timer->usageFlags &= ~(TIM_USE_MOTOR|TIM_USE_LED);
timer->usageFlags |= TIM_USE_SERVO;
}
break;
case OUTPUT_MODE_LED:
timer->usageFlags &= ~(TIM_USE_MOTOR|TIM_USE_SERVO);
timer->usageFlags |= TIM_USE_LED;
break;
}
}
@ -335,6 +342,8 @@ void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerU
type = MAP_TO_SERVO_OUTPUT;
} else if (TIM_IS_MOTOR(timHw->usageFlags) && !pwmHasServoOnTimer(timOutputs, timHw->tim)) {
type = MAP_TO_MOTOR_OUTPUT;
} else if (TIM_IS_LED(timHw->usageFlags) && !pwmHasMotorOnTimer(timOutputs, timHw->tim) && !pwmHasServoOnTimer(timOutputs, timHw->tim)) {
type = MAP_TO_LED_OUTPUT;
}
switch(type) {
@ -348,6 +357,10 @@ void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerU
timOutputs->timServos[timOutputs->maxTimServoCount++] = timHw;
pwmClaimTimer(timHw->tim, timHw->usageFlags);
break;
case MAP_TO_LED_OUTPUT:
timHw->usageFlags &= TIM_USE_LED;
pwmClaimTimer(timHw->tim, timHw->usageFlags);
break;
default:
break;
}

5
src/main/drivers/pwm_mapping.h
View file

@ -53,6 +53,11 @@ typedef enum {
SERVO_TYPE_SBUS_PWM
} servoProtocolType_e;
typedef enum {
PIN_LABEL_NONE = 0,
PIN_LABEL_LED
} pinLabel_e;
typedef enum {
PWM_INIT_ERROR_NONE = 0,
PWM_INIT_ERROR_TOO_MANY_MOTORS,

3
src/main/drivers/timer.h
View file

@ -115,7 +115,7 @@ typedef enum {
TIM_USE_MC_CHNFW = (1 << 4), // Deprecated and not used after removal of CHANNEL_FORWARDING feature
//TIM_USE_FW_MOTOR = (1 << 5), // We no longer differentiate mc from fw on pwm allocation
//TIM_USE_FW_SERVO = (1 << 6),
TIM_USE_LED = (1 << 24),
TIM_USE_LED = (1 << 24), // Remapping needs to be in the lower 8 bits.
TIM_USE_BEEPER = (1 << 25),
} timerUsageFlag_e;
@ -123,6 +123,7 @@ typedef enum {
#define TIM_IS_MOTOR(flags) ((flags) & TIM_USE_MOTOR)
#define TIM_IS_SERVO(flags) ((flags) & TIM_USE_SERVO)
#define TIM_IS_LED(flags) ((flags) & TIM_USE_LED)
#define TIM_IS_MOTOR_ONLY(flags) (TIM_IS_MOTOR(flags) && !TIM_IS_SERVO(flags))
#define TIM_IS_SERVO_ONLY(flags) (!TIM_IS_MOTOR(flags) && TIM_IS_SERVO(flags))

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

@ -163,6 +163,7 @@ static const char * outputModeNames[] = {
"AUTO",
"MOTORS",
"SERVOS",
"LED",
NULL
};
@ -292,7 +293,7 @@ static void cliPutp(void *p, char ch)
typedef enum {
DUMP_MASTER = (1 << 0),
DUMP_PROFILE = (1 << 1),
DUMP_CONTROL_PROFILE = (1 << 1),
DUMP_BATTERY_PROFILE = (1 << 2),
DUMP_MIXER_PROFILE = (1 << 3),
DUMP_ALL = (1 << 4),
@ -2821,6 +2822,8 @@ static void cliTimerOutputMode(char *cmdline)
mode = OUTPUT_MODE_MOTORS;
} else if(!sl_strcasecmp("SERVOS", tok)) {
mode = OUTPUT_MODE_SERVOS;
} else if(!sl_strcasecmp("LED", tok)) {
mode = OUTPUT_MODE_LED;
} else {
cliShowParseError();
return;
@ -3333,30 +3336,30 @@ static void cliPlaySound(char *cmdline)
beeper(beeperModeForTableIndex(i));
}
static void cliProfile(char *cmdline)
static void cliControlProfile(char *cmdline)
{
// CLI profile index is 1-based
if (isEmpty(cmdline)) {
cliPrintLinef("profile %d", getConfigProfile() + 1);
cliPrintLinef("control_profile %d", getConfigProfile() + 1);
return;
} else {
const int i = fastA2I(cmdline) - 1;
if (i >= 0 && i < MAX_PROFILE_COUNT) {
setConfigProfileAndWriteEEPROM(i);
cliProfile("");
cliControlProfile("");
}
}
}
static void cliDumpProfile(uint8_t profileIndex, uint8_t dumpMask)
static void cliDumpControlProfile(uint8_t profileIndex, uint8_t dumpMask)
{
if (profileIndex >= MAX_PROFILE_COUNT) {
// Faulty values
return;
}
setConfigProfile(profileIndex);
cliPrintHashLine("profile");
cliPrintLinef("profile %d\r\n", getConfigProfile() + 1);
cliPrintHashLine("control_profile");
cliPrintLinef("control_profile %d\r\n", getConfigProfile() + 1);
dumpAllValues(PROFILE_VALUE, dumpMask);
dumpAllValues(CONTROL_RATE_VALUE, dumpMask);
dumpAllValues(EZ_TUNE_VALUE, dumpMask);
@ -3978,12 +3981,12 @@ static void printConfig(const char *cmdline, bool doDiff)
const char *options;
if ((options = checkCommand(cmdline, "master"))) {
dumpMask = DUMP_MASTER; // only
} else if ((options = checkCommand(cmdline, "profile"))) {
dumpMask = DUMP_PROFILE; // only
} else if ((options = checkCommand(cmdline, "battery_profile"))) {
dumpMask = DUMP_BATTERY_PROFILE; // only
} else if ((options = checkCommand(cmdline, "control_profile"))) {
dumpMask = DUMP_CONTROL_PROFILE; // only
} else if ((options = checkCommand(cmdline, "mixer_profile"))) {
dumpMask = DUMP_MIXER_PROFILE; // only
} else if ((options = checkCommand(cmdline, "battery_profile"))) {
dumpMask = DUMP_BATTERY_PROFILE; // only
} else if ((options = checkCommand(cmdline, "all"))) {
dumpMask = DUMP_ALL; // all profiles and rates
} else {
@ -3994,16 +3997,16 @@ static void printConfig(const char *cmdline, bool doDiff)
dumpMask = dumpMask | DO_DIFF;
}
const int currentProfileIndexSave = getConfigProfile();
const int currentBatteryProfileIndexSave = getConfigBatteryProfile();
const int currentControlProfileIndexSave = getConfigProfile();
const int currentMixerProfileIndexSave = getConfigMixerProfile();
const int currentBatteryProfileIndexSave = getConfigBatteryProfile();
backupConfigs();
// reset all configs to defaults to do differencing
resetConfigs();
// restore the profile indices, since they should not be reset for proper comparison
setConfigProfile(currentProfileIndexSave);
setConfigBatteryProfile(currentBatteryProfileIndexSave);
setConfigProfile(currentControlProfileIndexSave);
setConfigMixerProfile(currentMixerProfileIndexSave);
setConfigBatteryProfile(currentBatteryProfileIndexSave);
if (checkCommand(options, "showdefaults")) {
dumpMask = dumpMask | SHOW_DEFAULTS; // add default values as comments for changed values
@ -4125,25 +4128,25 @@ static void printConfig(const char *cmdline, bool doDiff)
if (dumpMask & DUMP_ALL) {
// dump all profiles
const int currentProfileIndexSave = getConfigProfile();
const int currentBatteryProfileIndexSave = getConfigBatteryProfile();
const int currentControlProfileIndexSave = getConfigProfile();
const int currentMixerProfileIndexSave = getConfigMixerProfile();
const int currentBatteryProfileIndexSave = getConfigBatteryProfile();
for (int ii = 0; ii < MAX_PROFILE_COUNT; ++ii) {
cliDumpControlProfile(ii, dumpMask);
}
for (int ii = 0; ii < MAX_MIXER_PROFILE_COUNT; ++ii) {
cliDumpMixerProfile(ii, dumpMask);
}
for (int ii = 0; ii < MAX_PROFILE_COUNT; ++ii) {
cliDumpProfile(ii, dumpMask);
}
for (int ii = 0; ii < MAX_BATTERY_PROFILE_COUNT; ++ii) {
cliDumpBatteryProfile(ii, dumpMask);
}
setConfigProfile(currentProfileIndexSave);
setConfigBatteryProfile(currentBatteryProfileIndexSave);
setConfigProfile(currentControlProfileIndexSave);
setConfigMixerProfile(currentMixerProfileIndexSave);
setConfigBatteryProfile(currentBatteryProfileIndexSave);
cliPrintHashLine("restore original profile selection");
cliPrintLinef("control_profile %d", currentControlProfileIndexSave + 1);
cliPrintLinef("mixer_profile %d", currentMixerProfileIndexSave + 1);
cliPrintLinef("profile %d", currentProfileIndexSave + 1);
cliPrintLinef("battery_profile %d", currentBatteryProfileIndexSave + 1);
#ifdef USE_CLI_BATCH
@ -4151,17 +4154,18 @@ static void printConfig(const char *cmdline, bool doDiff)
#endif
} else {
// dump just the current profiles
cliDumpControlProfile(getConfigProfile(), dumpMask);
cliDumpMixerProfile(getConfigMixerProfile(), dumpMask);
cliDumpProfile(getConfigProfile(), dumpMask);
cliDumpBatteryProfile(getConfigBatteryProfile(), dumpMask);
}
}
if (dumpMask & DUMP_MIXER_PROFILE) {
cliDumpMixerProfile(getConfigMixerProfile(), dumpMask);
if (dumpMask & DUMP_CONTROL_PROFILE) {
cliDumpControlProfile(getConfigProfile(), dumpMask);
}
if (dumpMask & DUMP_PROFILE) {
cliDumpProfile(getConfigProfile(), dumpMask);
if (dumpMask & DUMP_MIXER_PROFILE) {
cliDumpMixerProfile(getConfigMixerProfile(), dumpMask);
}
if (dumpMask & DUMP_BATTERY_PROFILE) {
@ -4276,9 +4280,9 @@ const clicmd_t cmdTable[] = {
CLI_COMMAND_DEF("defaults", "reset to defaults and reboot", NULL, cliDefaults),
CLI_COMMAND_DEF("dfu", "DFU mode on reboot", NULL, cliDfu),
CLI_COMMAND_DEF("diff", "list configuration changes from default",
"[master|battery_profile|profile|rates|all] {showdefaults}", cliDiff),
"[master|battery_profile|control_profile|mixer_profile|rates|all] {showdefaults}", cliDiff),
CLI_COMMAND_DEF("dump", "dump configuration",
"[master|battery_profile|profile|rates|all] {showdefaults}", cliDump),
"[master|battery_profile|control_profile|mixer_profile|rates|all] {showdefaults}", cliDump),
#ifdef USE_RX_ELERES
CLI_COMMAND_DEF("eleres_bind", NULL, NULL, cliEleresBind),
#endif // USE_RX_ELERES
@ -4319,12 +4323,9 @@ const clicmd_t cmdTable[] = {
CLI_COMMAND_DEF("msc", "switch into msc mode", NULL, cliMsc),
#endif
CLI_COMMAND_DEF("play_sound", NULL, "[<index>]\r\n", cliPlaySound),
CLI_COMMAND_DEF("profile", "change profile",
"[<index>]", cliProfile),
CLI_COMMAND_DEF("battery_profile", "change battery profile",
"[<index>]", cliBatteryProfile),
CLI_COMMAND_DEF("mixer_profile", "change mixer profile",
"[<index>]", cliMixerProfile),
CLI_COMMAND_DEF("control_profile", "change control profile", "[<index>]", cliControlProfile),
CLI_COMMAND_DEF("mixer_profile", "change mixer profile", "[<index>]", cliMixerProfile),
CLI_COMMAND_DEF("battery_profile", "change battery profile", "[<index>]", cliBatteryProfile),
CLI_COMMAND_DEF("resource", "view currently used resources", NULL, cliResource),
CLI_COMMAND_DEF("rxrange", "configure rx channel ranges", NULL, cliRxRange),
#if defined(USE_SAFE_HOME)

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

@ -294,6 +294,14 @@ void createDefaultConfig(void)
featureSet(FEATURE_AIRMODE);
targetConfiguration();
#ifdef MSP_UART
int port = findSerialPortIndexByIdentifier(MSP_UART);
if (port) {
serialConfigMutable()->portConfigs[port].functionMask = FUNCTION_MSP;
serialConfigMutable()->portConfigs[port].msp_baudrateIndex = BAUD_115200;
}
#endif
}
void resetConfigs(void)

8
src/main/fc/fc_core.c
View file

@ -342,6 +342,10 @@ static void updateArmingStatus(void)
DISABLE_ARMING_FLAG(ARMING_DISABLED_NO_PREARM);
}
if (ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED) && !IS_RC_MODE_ACTIVE(BOXARM)) {
DISABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED);
}
/* CHECK: Arming switch */
// If arming is disabled and the ARM switch is on
// Note that this should be last check so all other blockers could be cleared correctly
@ -700,7 +704,7 @@ void processRx(timeUs_t currentTimeUs)
}
/* Turn assistant mode */
if (IS_RC_MODE_ACTIVE(BOXTURNASSIST)) {
if (IS_RC_MODE_ACTIVE(BOXTURNASSIST) || navigationRequiresTurnAssistance()) {
ENABLE_FLIGHT_MODE(TURN_ASSISTANT);
} else {
DISABLE_FLIGHT_MODE(TURN_ASSISTANT);
@ -896,7 +900,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
}
#if defined(SITL_BUILD)
if (lockMainPID()) {
if (ARMING_FLAG(SIMULATOR_MODE_HITL) || lockMainPID()) {
#endif
gyroFilter();

1
src/main/fc/fc_init.c
View file

@ -60,7 +60,6 @@
#include "drivers/pwm_esc_detect.h"
#include "drivers/pwm_mapping.h"
#include "drivers/pwm_output.h"
#include "drivers/pwm_output.h"
#include "drivers/sensor.h"
#include "drivers/serial.h"
#include "drivers/serial_softserial.h"

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

@ -856,7 +856,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU32(dst, currentBatteryProfile->capacity.value);
sbufWriteU32(dst, currentBatteryProfile->capacity.warning);
sbufWriteU32(dst, currentBatteryProfile->capacity.critical);
sbufWriteU8(dst, currentBatteryProfile->capacity.unit);
sbufWriteU8(dst, batteryMetersConfig()->capacity_unit);
break;
case MSP2_INAV_MISC2:
@ -895,7 +895,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU32(dst, currentBatteryProfile->capacity.value);
sbufWriteU32(dst, currentBatteryProfile->capacity.warning);
sbufWriteU32(dst, currentBatteryProfile->capacity.critical);
sbufWriteU8(dst, currentBatteryProfile->capacity.unit);
sbufWriteU8(dst, batteryMetersConfig()->capacity_unit);
break;
#ifdef USE_GPS
@ -1336,9 +1336,9 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
case MSP_NAV_POSHOLD:
sbufWriteU8(dst, navConfig()->general.flags.user_control_mode);
sbufWriteU16(dst, navConfig()->general.max_auto_speed);
sbufWriteU16(dst, navConfig()->mc.max_auto_climb_rate);
sbufWriteU16(dst, mixerConfig()->platformType == PLATFORM_AIRPLANE ? navConfig()->fw.max_auto_climb_rate : navConfig()->mc.max_auto_climb_rate);
sbufWriteU16(dst, navConfig()->general.max_manual_speed);
sbufWriteU16(dst, mixerConfig()->platformType != PLATFORM_AIRPLANE ? navConfig()->mc.max_manual_climb_rate:navConfig()->fw.max_manual_climb_rate);
sbufWriteU16(dst, mixerConfig()->platformType == PLATFORM_AIRPLANE ? navConfig()->fw.max_manual_climb_rate : navConfig()->mc.max_manual_climb_rate);
sbufWriteU8(dst, navConfig()->mc.max_bank_angle);
sbufWriteU8(dst, navConfig()->mc.althold_throttle_type);
sbufWriteU16(dst, currentBatteryProfile->nav.mc.hover_throttle);
@ -1575,6 +1575,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
}
break;
// Obsolete, replaced by MSP2_INAV_OUTPUT_MAPPING_EXT2
case MSP2_INAV_OUTPUT_MAPPING_EXT:
for (uint8_t i = 0; i < timerHardwareCount; ++i)
if (!(timerHardware[i].usageFlags & (TIM_USE_PPM | TIM_USE_PWM))) {
@ -1583,9 +1584,35 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
#else
sbufWriteU8(dst, timer2id(timerHardware[i].tim));
#endif
// usageFlags is u32, cuts out the higher 24bits
sbufWriteU8(dst, timerHardware[i].usageFlags);
}
break;
case MSP2_INAV_OUTPUT_MAPPING_EXT2:
{
#if !defined(SITL_BUILD) && defined(WS2811_PIN)
ioTag_t led_tag = IO_TAG(WS2811_PIN);
#endif
for (uint8_t i = 0; i < timerHardwareCount; ++i)
if (!(timerHardware[i].usageFlags & (TIM_USE_PPM | TIM_USE_PWM))) {
#if defined(SITL_BUILD)
sbufWriteU8(dst, i);
#else
sbufWriteU8(dst, timer2id(timerHardware[i].tim));
#endif
sbufWriteU32(dst, timerHardware[i].usageFlags);
#if defined(SITL_BUILD) || !defined(WS2811_PIN)
sbufWriteU8(dst, 0);
#else
// Extra label to help identify repurposed PINs.
// Eventually, we can try to add more labels for PPM pins, etc.
sbufWriteU8(dst, timerHardware[i].tag == led_tag ? PIN_LABEL_LED : PIN_LABEL_NONE);
#endif
}
}
break;
case MSP2_INAV_MC_BRAKING:
#ifdef USE_MR_BRAKING_MODE
@ -2054,13 +2081,13 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
currentBatteryProfileMutable->capacity.value = sbufReadU32(src);
currentBatteryProfileMutable->capacity.warning = sbufReadU32(src);
currentBatteryProfileMutable->capacity.critical = sbufReadU32(src);
currentBatteryProfileMutable->capacity.unit = sbufReadU8(src);
batteryMetersConfigMutable()->capacity_unit = sbufReadU8(src);
if ((batteryMetersConfig()->voltageSource != BAT_VOLTAGE_RAW) && (batteryMetersConfig()->voltageSource != BAT_VOLTAGE_SAG_COMP)) {
batteryMetersConfigMutable()->voltageSource = BAT_VOLTAGE_RAW;
return MSP_RESULT_ERROR;
}
if ((currentBatteryProfile->capacity.unit != BAT_CAPACITY_UNIT_MAH) && (currentBatteryProfile->capacity.unit != BAT_CAPACITY_UNIT_MWH)) {
currentBatteryProfileMutable->capacity.unit = BAT_CAPACITY_UNIT_MAH;
if ((batteryMetersConfig()->capacity_unit != BAT_CAPACITY_UNIT_MAH) && (batteryMetersConfig()->capacity_unit != BAT_CAPACITY_UNIT_MWH)) {
batteryMetersConfigMutable()->capacity_unit = BAT_CAPACITY_UNIT_MAH;
return MSP_RESULT_ERROR;
}
} else
@ -2093,13 +2120,13 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
currentBatteryProfileMutable->capacity.value = sbufReadU32(src);
currentBatteryProfileMutable->capacity.warning = sbufReadU32(src);
currentBatteryProfileMutable->capacity.critical = sbufReadU32(src);
currentBatteryProfileMutable->capacity.unit = sbufReadU8(src);
batteryMetersConfigMutable()->capacity_unit = sbufReadU8(src);
if ((batteryMetersConfig()->voltageSource != BAT_VOLTAGE_RAW) && (batteryMetersConfig()->voltageSource != BAT_VOLTAGE_SAG_COMP)) {
batteryMetersConfigMutable()->voltageSource = BAT_VOLTAGE_RAW;
return MSP_RESULT_ERROR;
}
if ((currentBatteryProfile->capacity.unit != BAT_CAPACITY_UNIT_MAH) && (currentBatteryProfile->capacity.unit != BAT_CAPACITY_UNIT_MWH)) {
currentBatteryProfileMutable->capacity.unit = BAT_CAPACITY_UNIT_MAH;
if ((batteryMetersConfig()->capacity_unit != BAT_CAPACITY_UNIT_MAH) && (batteryMetersConfig()->capacity_unit != BAT_CAPACITY_UNIT_MWH)) {
batteryMetersConfigMutable()->capacity_unit = BAT_CAPACITY_UNIT_MAH;
return MSP_RESULT_ERROR;
}
} else
@ -2399,12 +2426,16 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
if (dataSize == 13) {
navConfigMutable()->general.flags.user_control_mode = sbufReadU8(src);
navConfigMutable()->general.max_auto_speed = sbufReadU16(src);
navConfigMutable()->mc.max_auto_climb_rate = sbufReadU16(src);
navConfigMutable()->general.max_manual_speed = sbufReadU16(src);
if (mixerConfig()->platformType != PLATFORM_AIRPLANE) {
navConfigMutable()->mc.max_manual_climb_rate = sbufReadU16(src);
if (mixerConfig()->platformType == PLATFORM_AIRPLANE) {
navConfigMutable()->fw.max_auto_climb_rate = sbufReadU16(src);
} else {
navConfigMutable()->mc.max_auto_climb_rate = sbufReadU16(src);
}
navConfigMutable()->general.max_manual_speed = sbufReadU16(src);
if (mixerConfig()->platformType == PLATFORM_AIRPLANE) {
navConfigMutable()->fw.max_manual_climb_rate = sbufReadU16(src);
} else {
navConfigMutable()->mc.max_manual_climb_rate = sbufReadU16(src);
}
navConfigMutable()->mc.max_bank_angle = sbufReadU8(src);
navConfigMutable()->mc.althold_throttle_type = sbufReadU8(src);

2
src/main/fc/fc_msp_box.c
View file

@ -215,7 +215,7 @@ void initActiveBoxIds(void)
bool navReadyAltControl = getHwBarometerStatus() != HW_SENSOR_NONE;
#ifdef USE_GPS
navReadyAltControl = navReadyAltControl || (feature(FEATURE_GPS) && (STATE(AIRPLANE) || positionEstimationConfig()->use_gps_no_baro));
navReadyAltControl = navReadyAltControl || feature(FEATURE_GPS);
const bool navFlowDeadReckoning = sensors(SENSOR_OPFLOW) && sensors(SENSOR_ACC) && positionEstimationConfig()->allow_dead_reckoning;
bool navReadyPosControl = sensors(SENSOR_ACC) && feature(FEATURE_GPS);

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

@ -5,7 +5,7 @@ tables:
values: ["NONE", "AUTO", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "ICM42605", "BMI270","LSM6DXX", "FAKE"]
enum: accelerationSensor_e
- name: rangefinder_hardware
values: ["NONE", "SRF10", "VL53L0X", "MSP", "BENEWAKE", "VL53L1X", "US42", "TOF10120_I2C", "FAKE", "TERARANGER_EVO"]
values: ["NONE", "SRF10", "VL53L0X", "MSP", "BENEWAKE", "VL53L1X", "US42", "TOF10120_I2C", "FAKE", "TERARANGER_EVO", "USD1_V0", "NRA"]
enum: rangefinderType_e
- name: mag_hardware
values: ["NONE", "AUTO", "HMC5883", "AK8975", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "RM3100", "VCM5883", "MLX90393", "FAKE"]
@ -942,6 +942,12 @@ groups:
condition: USE_ADC
min: 0
max: 65535
- name: battery_capacity_unit
description: "Unit used for `battery_capacity`, `battery_capacity_warning` and `battery_capacity_critical` [MAH/MWH] (milliAmpere hour / milliWatt hour)."
default_value: "MAH"
field: capacity_unit
table: bat_capacity_unit
type: uint8_t
- 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."
default_value: :target
@ -1048,12 +1054,6 @@ groups:
field: capacity.critical
min: 0
max: 4294967295
- name: battery_capacity_unit
description: "Unit used for `battery_capacity`, `battery_capacity_warning` and `battery_capacity_critical` [MAH/MWH] (milliAmpere hour / milliWatt hour)."
default_value: "MAH"
field: capacity.unit
table: bat_capacity_unit
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
@ -1986,12 +1986,6 @@ groups:
field: fixedWingCoordinatedPitchGain
min: 0
max: 2
- name: fw_iterm_limit_stick_position
description: "Iterm is not allowed to grow when stick position is above threshold. This solves the problem of bounceback or followthrough when full stick deflection is applied on poorely tuned fixed wings. In other words, stabilization is partialy disabled when pilot is actively controlling the aircraft and active when sticks are not touched. `0` mean stick is in center position, `1` means it is fully deflected to either side"
default_value: 0.5
field: fixedWingItermLimitOnStickPosition
min: 0
max: 1
- name: fw_yaw_iterm_freeze_bank_angle
description: "Yaw Iterm is frozen when bank angle is above this threshold [degrees]. This solves the problem of the rudder counteracting turns by partially disabling yaw stabilization when making banked turns. Setting to 0 (the default) disables this feature. Only applies when autopilot is not active and TURN ASSIST is disabled."
default_value: 0
@ -2127,6 +2121,12 @@ groups:
field: bank_fw.pid[PID_POS_Z].D
min: 0
max: 255
- name: nav_fw_alt_control_response
description: "Adjusts the deceleration response of fixed wing altitude control as the target altitude is approached. Decrease value to help avoid overshooting the target altitude."
default_value: 20
field: fwAltControlResponseFactor
min: 5
max: 100
- name: nav_fw_pos_xy_p
description: "P gain of 2D trajectory PID controller. Play with this to get a straight line between waypoints or a straight RTH"
default_value: 75
@ -2279,6 +2279,24 @@ groups:
field: fixedWingLevelTrimGain
min: 0
max: 20
- name: fw_iterm_lock_time_max_ms
description: Defines max time in milliseconds for how long ITerm Lock will shut down Iterm after sticks are release
default_value: 500
field: fwItermLockTimeMaxMs
min: 100
max: 1000
- name: fw_iterm_lock_rate_threshold
description: Defines rate percentage when full P I and D attenuation should happen. 100 disables Iterm Lock for P and D term
field: fwItermLockRateLimit
default_value: 40
min: 10
max: 100
- name: fw_iterm_lock_engage_threshold
description: Defines error rate (in percents of max rate) when Iterm Lock is engaged when sticks are release. Iterm Lock will stay active until error drops below this number
default_value: 10
min: 5
max: 25
field: fwItermLockEngageThreshold
- name: PG_PID_AUTOTUNE_CONFIG
type: pidAutotuneConfig_t
@ -2317,10 +2335,6 @@ groups:
field: gravity_calibration_tolerance
min: 0
max: 255
- name: inav_use_gps_no_baro
field: use_gps_no_baro
type: bool
default_value: OFF
- name: inav_allow_dead_reckoning
description: "Defines if INAV will dead-reckon over short GPS outages. May also be useful for indoors OPFLOW navigation"
default_value: OFF
@ -2823,6 +2837,12 @@ groups:
field: fw.max_bank_angle
min: 5
max: 80
- name: nav_fw_auto_climb_rate
description: "Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s]"
default_value: 500
field: fw.max_auto_climb_rate
min: 10
max: 2000
- name: nav_fw_manual_climb_rate
description: "Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s]"
default_value: 300
@ -2898,7 +2918,7 @@ groups:
description: "Forward acceleration threshold for bungee launch or throw launch [cm/s/s], 1G = 981 cm/s/s"
default_value: 1863
field: fw.launch_accel_thresh
min: 1500
min: 1350
max: 20000
- name: nav_fw_launch_max_angle
description: "Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg]"
@ -3492,7 +3512,6 @@ groups:
min: 8
max: 11
default_value: 9
- name: osd_adsb_distance_warning
description: "Distance in meters of ADSB aircraft that is displayed"
default_value: 20000
@ -3517,7 +3536,6 @@ groups:
min: 0
max: 64000
type: uint16_t
- name: osd_estimations_wind_compensation
description: "Use wind estimation for remaining flight time/distance estimation"
default_value: ON
@ -3530,12 +3548,10 @@ groups:
condition: USE_WIND_ESTIMATOR
field: estimations_wind_mps
type: bool
- name: osd_failsafe_switch_layout
description: "If enabled the OSD automatically switches to the first layout during failsafe"
default_value: OFF
type: bool
- name: osd_plus_code_digits
description: "Numer of plus code digits before shortening with `osd_plus_code_short`. Precision at the equator: 10=13.9x13.9m; 11=2.8x3.5m; 12=56x87cm; 13=11x22cm."
field: plus_code_digits
@ -3547,213 +3563,186 @@ groups:
field: plus_code_short
default_value: "0"
table: osd_plus_code_short
- name: osd_ahi_style
description: "Sets OSD Artificial Horizon style \"DEFAULT\" or \"LINE\" for the FrSky Graphical OSD."
field: ahi_style
default_value: "DEFAULT"
table: osd_ahi_style
type: uint8_t
- name: osd_force_grid
field: force_grid
type: bool
default_value: OFF
description: Force OSD to work in grid mode even if the OSD device supports pixel level access (mainly used for development)
- name: osd_ahi_bordered
field: ahi_bordered
type: bool
description: Shows a border/corners around the AHI region (pixel OSD only)
default_value: OFF
- name: osd_ahi_width
field: ahi_width
max: 255
description: AHI width in pixels (pixel OSD only)
default_value: 132
- name: osd_ahi_height
field: ahi_height
max: 255
description: AHI height in pixels (pixel OSD only)
default_value: 162
- name: osd_ahi_vertical_offset
field: ahi_vertical_offset
min: -128
max: 127
description: AHI vertical offset from center (pixel OSD only)
default_value: -18
- name: osd_sidebar_horizontal_offset
field: sidebar_horizontal_offset
min: -128
max: 127
default_value: 0
description: Sidebar horizontal offset from default position. Positive values move the sidebars closer to the edges.
- name: osd_left_sidebar_scroll_step
field: left_sidebar_scroll_step
max: 255
default_value: 0
description: How many units each sidebar step represents. 0 means the default value for the scroll type.
- name: osd_right_sidebar_scroll_step
field: right_sidebar_scroll_step
max: 255
default_value: 0
description: Same as left_sidebar_scroll_step, but for the right sidebar
- name: osd_sidebar_height
field: sidebar_height
min: 0
max: 5
default_value: 3
description: Height of sidebars in rows. 0 leaves only the level indicator arrows (Not for pixel OSD)
- name: osd_ahi_pitch_interval
field: ahi_pitch_interval
min: 0
max: 30
default_value: 0
description: Draws AHI at increments of the set pitch interval over the full pitch range. AHI line is drawn with ends offset when pitch first exceeds interval with offset increasing with increasing pitch. Offset direction changes between climb and dive. Set to 0 to disable (Not for pixel OSD)
- name: osd_home_position_arm_screen
type: bool
default_value: ON
description: Should home position coordinates be displayed on the arming screen.
- name: osd_pan_servo_index
description: Index of the pan servo, used to adjust osd home heading direction based on camera pan. Note that this feature does not work with continiously rotating servos.
field: pan_servo_index
min: 0
max: 16
default_value: 0
- name: osd_pan_servo_pwm2centideg
description: Centidegrees of pan servo rotation us PWM signal. A servo with 180 degrees of rotation from 1000 to 2000 us PWM typically needs `18` for this setting. Change sign to inverse direction.
field: pan_servo_pwm2centideg
default_value: 0
min: -36
max: 36
- name: osd_pan_servo_offcentre_warning
description: Degrees either side of the pan servo centre; where it is assumed camera is wanted to be facing forwards, but isn't at 0. If in this range and not 0 for longer than 10 seconds, the pan servo offset OSD element will blink. 0 means the warning is disabled.
field: pan_servo_offcentre_warning
min: 0
max: 45
default_value: 10
- name: osd_pan_servo_indicator_show_degrees
description: Show the degress of offset from centre on the pan servo OSD display element.
field: pan_servo_indicator_show_degrees
type: bool
default_value: OFF
- name: osd_esc_rpm_precision
description: Number of characters used to display the RPM value.
field: esc_rpm_precision
min: 3
max: 6
default_value: 3
- name: osd_mah_precision
description: Number of digits used for mAh precision. Currently used by mAh Used and Battery Remaining Capacity
field: mAh_precision
min: 4
max: 6
default_value: 4
- name: osd_use_pilot_logo
description: Use custom pilot logo with/instead of the INAV logo. The pilot logo must be characters 473 to 511
field: use_pilot_logo
type: bool
default_value: OFF
- name: osd_inav_to_pilot_logo_spacing
description: The space between the INAV and pilot logos, if `osd_use_pilot_logo` is `ON`. This number may be adjusted so that it fits the odd/even col width displays. For example, if using an odd column width display, such as Walksnail, and this is set to 4. 1 will be added so that the logos are equally spaced from the centre of the screen.
field: inav_to_pilot_logo_spacing
min: 0
max: 20
default_value: 8
- name: osd_arm_screen_display_time
description: Amount of time to display the arm screen [ms]
field: arm_screen_display_time
min: 1000
max: 5000
default_value: 1500
- name: osd_switch_indicator_zero_name
description: "Character to use for OSD switch incicator 0."
field: osd_switch_indicator0_name
type: string
max: 5
default_value: "FLAP"
- name: osd_switch_indicator_one_name
description: "Character to use for OSD switch incicator 1."
field: osd_switch_indicator1_name
type: string
max: 5
default_value: "GEAR"
- name: osd_switch_indicator_two_name
description: "Character to use for OSD switch incicator 2."
field: osd_switch_indicator2_name
type: string
max: 5
default_value: "CAM"
- name: osd_switch_indicator_three_name
description: "Character to use for OSD switch incicator 3."
field: osd_switch_indicator3_name
type: string
max: 5
default_value: "LIGT"
- name: osd_switch_indicator_zero_channel
description: "RC Channel to use for OSD switch indicator 0."
field: osd_switch_indicator0_channel
min: 5
max: MAX_SUPPORTED_RC_CHANNEL_COUNT
default_value: 5
- name: osd_switch_indicator_one_channel
description: "RC Channel to use for OSD switch indicator 1."
field: osd_switch_indicator1_channel
min: 5
max: MAX_SUPPORTED_RC_CHANNEL_COUNT
default_value: 5
- name: osd_switch_indicator_two_channel
description: "RC Channel to use for OSD switch indicator 2."
field: osd_switch_indicator2_channel
min: 5
max: MAX_SUPPORTED_RC_CHANNEL_COUNT
default_value: 5
- name: osd_switch_indicator_three_channel
description: "RC Channel to use for OSD switch indicator 3."
field: osd_switch_indicator3_channel
min: 5
max: MAX_SUPPORTED_RC_CHANNEL_COUNT
default_value: 5
- name: osd_switch_indicators_align_left
description: "Align text to left of switch indicators"
field: osd_switch_indicators_align_left
type: bool
default_value: ON
- name: osd_system_msg_display_time
description: System message display cycle time for multiple messages (milliseconds).
field: system_msg_display_time
default_value: 1000
min: 500
max: 5000
- name: osd_highlight_djis_missing_font_symbols
description: Show question marks where there is no symbol in the DJI font to represent the INAV OSD element's symbol. When off, blank spaces will be used. Only relevent for DJICOMPAT modes.
field: highlight_djis_missing_characters
default_value: ON
type: bool
- name: PG_OSD_COMMON_CONFIG
type: osdCommonConfig_t
headers: ["io/osd_common.h"]

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

@ -48,7 +48,8 @@ typedef enum {
typedef enum {
OUTPUT_MODE_AUTO = 0,
OUTPUT_MODE_MOTORS,
OUTPUT_MODE_SERVOS
OUTPUT_MODE_SERVOS,
OUTPUT_MODE_LED
} outputMode_e;
typedef struct motorAxisCorrectionLimits_s {

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

@ -66,6 +66,14 @@
#include "programming/logic_condition.h"
typedef struct {
float aP;
float aI;
float aD;
float aFF;
timeMs_t targetOverThresholdTimeMs;
} fwPidAttenuation_t;
typedef struct {
uint8_t axis;
float kP; // Proportional gain
@ -107,6 +115,8 @@ typedef struct {
pt3Filter_t rateTargetFilter;
smithPredictor_t smithPredictor;
fwPidAttenuation_t attenuation;
} pidState_t;
STATIC_FASTRAM bool pidFiltersConfigured = false;
@ -155,10 +165,9 @@ static EXTENDED_FASTRAM float iTermAntigravityGain;
#endif
static EXTENDED_FASTRAM uint8_t usedPidControllerType;
typedef void (*pidControllerFnPtr)(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv);
typedef void (*pidControllerFnPtr)(pidState_t *pidState, float dT, float dT_inv);
static EXTENDED_FASTRAM pidControllerFnPtr pidControllerApplyFn;
static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn;
static EXTENDED_FASTRAM bool levelingEnabled = false;
static EXTENDED_FASTRAM bool restartAngleHoldMode = true;
static EXTENDED_FASTRAM bool angleHoldIsLevel = false;
@ -170,7 +179,7 @@ static EXTENDED_FASTRAM bool angleHoldIsLevel = false;
static EXTENDED_FASTRAM float fixedWingLevelTrim;
static EXTENDED_FASTRAM pidController_t fixedWingLevelTrimController;
PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 7);
PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 9);
PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.bank_mc = {
@ -231,9 +240,9 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
},
[PID_HEADING] = { SETTING_NAV_FW_HEADING_P_DEFAULT, 0, 0, 0 },
[PID_POS_Z] = {
.P = SETTING_NAV_FW_POS_Z_P_DEFAULT, // FW_POS_Z_P * 10
.I = SETTING_NAV_FW_POS_Z_I_DEFAULT, // FW_POS_Z_I * 10
.D = SETTING_NAV_FW_POS_Z_D_DEFAULT, // FW_POS_Z_D * 10
.P = SETTING_NAV_FW_POS_Z_P_DEFAULT, // FW_POS_Z_P * 100
.I = SETTING_NAV_FW_POS_Z_I_DEFAULT, // FW_POS_Z_I * 100
.D = SETTING_NAV_FW_POS_Z_D_DEFAULT, // FW_POS_Z_D * 100
.FF = 0,
},
[PID_POS_XY] = {
@ -269,7 +278,6 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.fixedWingReferenceAirspeed = SETTING_FW_REFERENCE_AIRSPEED_DEFAULT,
.fixedWingCoordinatedYawGain = SETTING_FW_TURN_ASSIST_YAW_GAIN_DEFAULT,
.fixedWingCoordinatedPitchGain = SETTING_FW_TURN_ASSIST_PITCH_GAIN_DEFAULT,
.fixedWingItermLimitOnStickPosition = SETTING_FW_ITERM_LIMIT_STICK_POSITION_DEFAULT,
.fixedWingYawItermBankFreeze = SETTING_FW_YAW_ITERM_FREEZE_BANK_ANGLE_DEFAULT,
.navVelXyDTermLpfHz = SETTING_NAV_MC_VEL_XY_DTERM_LPF_HZ_DEFAULT,
@ -299,11 +307,15 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.fixedWingLevelTrim = SETTING_FW_LEVEL_PITCH_TRIM_DEFAULT,
.fixedWingLevelTrimGain = SETTING_FW_LEVEL_PITCH_GAIN_DEFAULT,
.fwAltControlResponseFactor = SETTING_NAV_FW_ALT_CONTROL_RESPONSE_DEFAULT,
#ifdef USE_SMITH_PREDICTOR
.smithPredictorStrength = SETTING_SMITH_PREDICTOR_STRENGTH_DEFAULT,
.smithPredictorDelay = SETTING_SMITH_PREDICTOR_DELAY_DEFAULT,
.smithPredictorFilterHz = SETTING_SMITH_PREDICTOR_LPF_HZ_DEFAULT,
#endif
.fwItermLockTimeMaxMs = SETTING_FW_ITERM_LOCK_TIME_MAX_MS_DEFAULT,
.fwItermLockRateLimit = SETTING_FW_ITERM_LOCK_RATE_THRESHOLD_DEFAULT,
.fwItermLockEngageThreshold = SETTING_FW_ITERM_LOCK_ENGAGE_THRESHOLD_DEFAULT,
);
bool pidInitFilters(void)
@ -670,19 +682,6 @@ static void pidApplySetpointRateLimiting(pidState_t *pidState, flight_dynamics_i
}
}
bool isFixedWingItermLimitActive(float stickPosition)
{
/*
* Iterm anti windup whould be active only when pilot controls the rotation
* velocity directly, not when ANGLE or HORIZON are used
*/
if (levelingEnabled) {
return false;
}
return fabsf(stickPosition) > pidProfile()->fixedWingItermLimitOnStickPosition;
}
static float pTermProcess(pidState_t *pidState, float rateError, float dT) {
float newPTerm = rateError * pidState->kP;
@ -745,41 +744,81 @@ static void applyItermLimiting(pidState_t *pidState) {
}
}
static void nullRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv) {
static void nullRateController(pidState_t *pidState, float dT, float dT_inv) {
UNUSED(pidState);
UNUSED(axis);
UNUSED(dT);
UNUSED(dT_inv);
}
static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv)
static void fwRateAttenuation(pidState_t *pidState, const float rateTarget, const float rateError) {
const float maxRate = currentControlRateProfile->stabilized.rates[pidState->axis] * 10.0f;
const float dampingFactor = attenuation(rateTarget, maxRate * pidProfile()->fwItermLockRateLimit / 100.0f);
/*
* Iterm damping is applied (down to 0) when:
* abs(error) > 10% rate and sticks were moved in the last 500ms (hard stop at this mark)
* itermAttenuation = MIN(curve(setpoint), (abs(error) > 10%) && (sticks were deflected in 500ms) ? 0 : 1)
*/
//If error is greater than 10% or max rate
const bool errorThresholdReached = fabsf(rateError) > maxRate * pidProfile()->fwItermLockEngageThreshold / 100.0f;
//If stick (setpoint) was moved above threshold in the last 500ms
if (fabsf(rateTarget) > maxRate * 0.2f) {
pidState->attenuation.targetOverThresholdTimeMs = millis();
}
//If error is below threshold, we no longer track time for lock mechanism
if (!errorThresholdReached) {
pidState->attenuation.targetOverThresholdTimeMs = 0;
}
pidState->attenuation.aI = MIN(dampingFactor, (errorThresholdReached && (millis() - pidState->attenuation.targetOverThresholdTimeMs) < pidProfile()->fwItermLockTimeMaxMs) ? 0.0f : 1.0f);
//P & D damping factors are always the same and based on current damping factor
pidState->attenuation.aP = dampingFactor;
pidState->attenuation.aD = dampingFactor;
if (pidState->axis == FD_ROLL) {
DEBUG_SET(DEBUG_ALWAYS, 0, pidState->attenuation.aP * 1000);
DEBUG_SET(DEBUG_ALWAYS, 1, pidState->attenuation.aI * 1000);
DEBUG_SET(DEBUG_ALWAYS, 2, pidState->attenuation.aD * 1000);
}
}
static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, float dT, float dT_inv)
{
const float rateTarget = getFlightAxisRateOverride(axis, pidState->rateTarget);
const float rateTarget = getFlightAxisRateOverride(pidState->axis, pidState->rateTarget);
const float rateError = rateTarget - pidState->gyroRate;
const float newPTerm = pTermProcess(pidState, rateError, dT);
const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv);
fwRateAttenuation(pidState, rateTarget, rateError);
const float newPTerm = pTermProcess(pidState, rateError, dT) * pidState->attenuation.aP;
const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv) * pidState->attenuation.aD;
const float newFFTerm = rateTarget * pidState->kFF;
/*
* Integral should be updated only if axis Iterm is not frozen
*/
if (!pidState->itermFreezeActive) {
pidState->errorGyroIf += rateError * pidState->kI * dT;
pidState->errorGyroIf += rateError * pidState->kI * dT * pidState->attenuation.aI;
}
applyItermLimiting(pidState);
const uint16_t limit = getPidSumLimit(axis);
const uint16_t limit = getPidSumLimit(pidState->axis);
if (pidProfile()->pidItermLimitPercent != 0){
float itermLimit = limit * pidProfile()->pidItermLimitPercent * 0.01f;
pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -itermLimit, +itermLimit);
}
axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf + newDTerm, -limit, +limit);
axisPID[pidState->axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf + newDTerm, -limit, +limit);
if (FLIGHT_MODE(SOARING_MODE) && axis == FD_PITCH && calculateRollPitchCenterStatus() == CENTERED) {
if (FLIGHT_MODE(SOARING_MODE) && pidState->axis == FD_PITCH && calculateRollPitchCenterStatus() == CENTERED) {
if (!angleFreefloatDeadband(DEGREES_TO_DECIDEGREES(navConfig()->fw.soaring_pitch_deadband), FD_PITCH)) {
axisPID[FD_PITCH] = 0; // center pitch servo if pitch attitude within soaring mode deadband
}
@ -787,16 +826,16 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
#ifdef USE_AUTOTUNE_FIXED_WING
if (FLIGHT_MODE(AUTO_TUNE) && !FLIGHT_MODE(MANUAL_MODE)) {
autotuneFixedWingUpdate(axis, rateTarget, pidState->gyroRate, constrainf(newPTerm + newFFTerm, -limit, +limit));
autotuneFixedWingUpdate(pidState->axis, rateTarget, pidState->gyroRate, constrainf(newPTerm + newFFTerm, -limit, +limit));
}
#endif
#ifdef USE_BLACKBOX
axisPID_P[axis] = newPTerm;
axisPID_I[axis] = pidState->errorGyroIf;
axisPID_D[axis] = newDTerm;
axisPID_F[axis] = newFFTerm;
axisPID_Setpoint[axis] = rateTarget;
axisPID_P[pidState->axis] = newPTerm;
axisPID_I[pidState->axis] = pidState->errorGyroIf;
axisPID_D[pidState->axis] = newDTerm;
axisPID_F[pidState->axis] = newFFTerm;
axisPID_Setpoint[pidState->axis] = rateTarget;
#endif
pidState->previousRateGyro = pidState->gyroRate;
@ -819,10 +858,10 @@ static float FAST_CODE applyItermRelax(const int axis, float currentPidSetpoint,
return itermErrorRate;
}
static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv)
static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pidState, float dT, float dT_inv)
{
const float rateTarget = getFlightAxisRateOverride(axis, pidState->rateTarget);
const float rateTarget = getFlightAxisRateOverride(pidState->axis, pidState->rateTarget);
const float rateError = rateTarget - pidState->gyroRate;
const float newPTerm = pTermProcess(pidState, rateError, dT);
@ -836,13 +875,13 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid
*/
const float newCDTerm = rateTargetDeltaFiltered * pidState->kCD;
const uint16_t limit = getPidSumLimit(axis);
const uint16_t limit = getPidSumLimit(pidState->axis);
// TODO: Get feedback from mixer on available correction range for each axis
const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf + newCDTerm;
const float newOutputLimited = constrainf(newOutput, -limit, +limit);
float itermErrorRate = applyItermRelax(axis, rateTarget, rateError);
float itermErrorRate = applyItermRelax(pidState->axis, rateTarget, rateError);
#ifdef USE_ANTIGRAVITY
itermErrorRate *= iTermAntigravityGain;
@ -860,14 +899,14 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid
// Don't grow I-term if motors are at their limit
applyItermLimiting(pidState);
axisPID[axis] = newOutputLimited;
axisPID[pidState->axis] = newOutputLimited;
#ifdef USE_BLACKBOX
axisPID_P[axis] = newPTerm;
axisPID_I[axis] = pidState->errorGyroIf;
axisPID_D[axis] = newDTerm;
axisPID_F[axis] = newCDTerm;
axisPID_Setpoint[axis] = rateTarget;
axisPID_P[pidState->axis] = newPTerm;
axisPID_I[pidState->axis] = pidState->errorGyroIf;
axisPID_D[pidState->axis] = newDTerm;
axisPID_F[pidState->axis] = newCDTerm;
axisPID_Setpoint[pidState->axis] = rateTarget;
#endif
pidState->previousRateTarget = rateTarget;
@ -1047,11 +1086,9 @@ static void pidApplyFpvCameraAngleMix(pidState_t *pidState, uint8_t fpvCameraAng
void checkItermLimitingActive(pidState_t *pidState)
{
bool shouldActivate;
if (usedPidControllerType == PID_TYPE_PIFF) {
shouldActivate = isFixedWingItermLimitActive(pidState->stickPosition);
} else
{
bool shouldActivate = false;
if (usedPidControllerType == PID_TYPE_PID) {
shouldActivate = mixerIsOutputSaturated(); //just in case, since it is already managed by itermWindupPointPercent
}
@ -1063,7 +1100,7 @@ void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis
if (usedPidControllerType == PID_TYPE_PIFF && pidProfile()->fixedWingYawItermBankFreeze != 0 && axis == FD_YAW) {
// Do not allow yaw I-term to grow when bank angle is too large
float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll);
if (fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankFreeze && !(FLIGHT_MODE(AUTO_TUNE) || FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance())){
if (fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankFreeze && !(FLIGHT_MODE(AUTO_TUNE) || FLIGHT_MODE(TURN_ASSISTANT))) {
pidState->itermFreezeActive = true;
} else
{
@ -1185,7 +1222,6 @@ void FAST_CODE pidController(float dT)
// Step 3: Run control for ANGLE_MODE, HORIZON_MODE and ANGLEHOLD_MODE
const float horizonRateMagnitude = FLIGHT_MODE(HORIZON_MODE) ? calcHorizonRateMagnitude() : 0.0f;
levelingEnabled = false;
angleHoldIsLevel = false;
for (uint8_t axis = FD_ROLL; axis <= FD_PITCH; axis++) {
@ -1205,14 +1241,13 @@ void FAST_CODE pidController(float dT)
// Apply the Level PID controller
pidLevel(angleTarget, &pidState[axis], axis, horizonRateMagnitude, dT);
canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with ANGLE/HORIZON
levelingEnabled = true;
} else {
restartAngleHoldMode = true;
}
}
// Apply Turn Assistance
if ((FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
if (FLIGHT_MODE(TURN_ASSISTANT) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
float bankAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_ROLL], pidProfile()->max_angle_inclination[FD_ROLL]));
float pitchAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_PITCH], pidProfile()->max_angle_inclination[FD_PITCH]));
pidTurnAssistant(pidState, bankAngleTarget, pitchAngleTarget);
@ -1235,7 +1270,7 @@ void FAST_CODE pidController(float dT)
checkItermLimitingActive(&pidState[axis]);
checkItermFreezingActive(&pidState[axis], axis);
pidControllerApplyFn(&pidState[axis], axis, dT, dT_inv);
pidControllerApplyFn(&pidState[axis], dT, dT_inv);
}
}

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

@ -121,7 +121,6 @@ typedef struct pidProfile_s {
float fixedWingReferenceAirspeed; // Reference tuning airspeed for the airplane - the speed for which PID gains are tuned
float fixedWingCoordinatedYawGain; // This is the gain of the yaw rate required to keep the yaw rate consistent with the turn rate for a coordinated turn.
float fixedWingCoordinatedPitchGain; // This is the gain of the pitch rate to keep the pitch angle constant during coordinated turns.
float fixedWingItermLimitOnStickPosition; //Do not allow Iterm to grow when stick position is above this point
uint16_t fixedWingYawItermBankFreeze; // Freeze yaw Iterm when bank angle is more than this many degrees
float navVelXyDTermLpfHz;
@ -149,11 +148,19 @@ typedef struct pidProfile_s {
float fixedWingLevelTrim;
float fixedWingLevelTrimGain;
uint8_t fwAltControlResponseFactor;
#ifdef USE_SMITH_PREDICTOR
float smithPredictorStrength;
float smithPredictorDelay;
uint16_t smithPredictorFilterHz;
#endif
uint16_t fwItermLockTimeMaxMs;
uint8_t fwItermLockRateLimit;
uint8_t fwItermLockEngageThreshold;
} pidProfile_t;
typedef struct pidAutotuneConfig_s {

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

@ -222,7 +222,7 @@ float calculateRemainingDistanceBeforeRTH(bool takeWindIntoAccount) {
// check requirements
const bool areBatterySettingsOK = feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER) && batteryWasFullWhenPluggedIn();
const bool areRTHEstimatorSettingsOK = batteryMetersConfig()->cruise_power > 0 && currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH &&currentBatteryProfile->capacity.value > 0 && navConfig()->fw.cruise_speed > 0;
const bool areRTHEstimatorSettingsOK = batteryMetersConfig()->cruise_power > 0 && batteryMetersConfig()->capacity_unit == BAT_CAPACITY_UNIT_MWH && currentBatteryProfile->capacity.value > 0 && navConfig()->fw.cruise_speed > 0;
const bool isNavigationOK = navigationPositionEstimateIsHealthy() && isImuHeadingValid();
if (!(areBatterySettingsOK && areRTHEstimatorSettingsOK && isNavigationOK)) {

163
src/main/io/bf_osd_symbols.h
View file

@ -1,163 +0,0 @@
/* @file max7456_symbols.h
* @brief max7456 symbols for the mwosd font set
*
* @author Nathan Tsoi nathan@vertile.com
*
* Copyright (C) 2016 Nathan Tsoi
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>
*/
#pragma once
//Misc
#define BF_SYM_NONE 0x00
#define BF_SYM_END_OF_FONT 0xFF
#define BF_SYM_BLANK 0x20
#define BF_SYM_HYPHEN 0x2D
#define BF_SYM_BBLOG 0x10
#define BF_SYM_HOMEFLAG 0x11
#define BF_SYM_RPM 0x12
#define BF_SYM_ROLL 0x14
#define BF_SYM_PITCH 0x15
#define BF_SYM_TEMPERATURE 0x7A
// GPS and navigation
#define BF_SYM_LAT 0x89
#define BF_SYM_LON 0x98
#define BF_SYM_ALTITUDE 0x7F
#define BF_SYM_TOTAL_DISTANCE 0x71
#define BF_SYM_OVER_HOME 0x05
// RSSI
#define BF_SYM_RSSI 0x01
#define BF_SYM_LINK_QUALITY 0x7B
// Throttle Position (%)
#define BF_SYM_THR 0x04
// Unit Icons (Metric)
#define BF_SYM_M 0x0C
#define BF_SYM_KM 0x7D
#define BF_SYM_C 0x0E
// Unit Icons (Imperial)
#define BF_SYM_FT 0x0F
#define BF_SYM_MILES 0x7E
#define BF_SYM_F 0x0D
// Heading Graphics
#define BF_SYM_HEADING_N 0x18
#define BF_SYM_HEADING_S 0x19
#define BF_SYM_HEADING_E 0x1A
#define BF_SYM_HEADING_W 0x1B
#define BF_SYM_HEADING_DIVIDED_LINE 0x1C
#define BF_SYM_HEADING_LINE 0x1D
// AH Center screen Graphics
#define BF_SYM_AH_CENTER_LINE 0x72
#define BF_SYM_AH_CENTER 0x73
#define BF_SYM_AH_CENTER_LINE_RIGHT 0x74
#define BF_SYM_AH_RIGHT 0x02
#define BF_SYM_AH_LEFT 0x03
#define BF_SYM_AH_DECORATION 0x13
// Satellite Graphics
#define BF_SYM_SAT_L 0x1E
#define BF_SYM_SAT_R 0x1F
// Direction arrows
#define BF_SYM_ARROW_SOUTH 0x60
#define BF_SYM_ARROW_2 0x61
#define BF_SYM_ARROW_3 0x62
#define BF_SYM_ARROW_4 0x63
#define BF_SYM_ARROW_EAST 0x64
#define BF_SYM_ARROW_6 0x65
#define BF_SYM_ARROW_7 0x66
#define BF_SYM_ARROW_8 0x67
#define BF_SYM_ARROW_NORTH 0x68
#define BF_SYM_ARROW_10 0x69
#define BF_SYM_ARROW_11 0x6A
#define BF_SYM_ARROW_12 0x6B
#define BF_SYM_ARROW_WEST 0x6C
#define BF_SYM_ARROW_14 0x6D
#define BF_SYM_ARROW_15 0x6E
#define BF_SYM_ARROW_16 0x6F
#define BF_SYM_ARROW_SMALL_UP 0x75
#define BF_SYM_ARROW_SMALL_DOWN 0x76
// AH Bars
#define BF_SYM_AH_BAR9_0 0x80
#define BF_SYM_AH_BAR9_1 0x81
#define BF_SYM_AH_BAR9_2 0x82
#define BF_SYM_AH_BAR9_3 0x83
#define BF_SYM_AH_BAR9_4 0x84
#define BF_SYM_AH_BAR9_5 0x85
#define BF_SYM_AH_BAR9_6 0x86
#define BF_SYM_AH_BAR9_7 0x87
#define BF_SYM_AH_BAR9_8 0x88
// Progress bar
#define BF_SYM_PB_START 0x8A
#define BF_SYM_PB_FULL 0x8B
#define BF_SYM_PB_HALF 0x8C
#define BF_SYM_PB_EMPTY 0x8D
#define BF_SYM_PB_END 0x8E
#define BF_SYM_PB_CLOSE 0x8F
// Batt evolution
#define BF_SYM_BATT_FULL 0x90
#define BF_SYM_BATT_5 0x91
#define BF_SYM_BATT_4 0x92
#define BF_SYM_BATT_3 0x93
#define BF_SYM_BATT_2 0x94
#define BF_SYM_BATT_1 0x95
#define BF_SYM_BATT_EMPTY 0x96
// Batt Icons
#define BF_SYM_MAIN_BATT 0x97
// Voltage and amperage
#define BF_SYM_VOLT 0x06
#define BF_SYM_AMP 0x9A
#define BF_SYM_MAH 0x07
#define BF_SYM_WATT 0x57 // 0x57 is 'W'
// Time
#define BF_SYM_ON_M 0x9B
#define BF_SYM_FLY_M 0x9C
// Speed
#define BF_SYM_SPEED 0x70
#define BF_SYM_KPH 0x9E
#define BF_SYM_MPH 0x9D
#define BF_SYM_MPS 0x9F
#define BF_SYM_FTPS 0x99
// Menu cursor
#define BF_SYM_CURSOR BF_SYM_AH_LEFT
// Stick overlays
#define BF_SYM_STICK_OVERLAY_SPRITE_HIGH 0x08
#define BF_SYM_STICK_OVERLAY_SPRITE_MID 0x09
#define BF_SYM_STICK_OVERLAY_SPRITE_LOW 0x0A
#define BF_SYM_STICK_OVERLAY_CENTER 0x0B
#define BF_SYM_STICK_OVERLAY_VERTICAL 0x16
#define BF_SYM_STICK_OVERLAY_HORIZONTAL 0x17
// GPS degree/minute/second symbols
#define BF_SYM_GPS_DEGREE BF_SYM_STICK_OVERLAY_SPRITE_HIGH // kind of looks like the degree symbol
#define BF_SYM_GPS_MINUTE 0x27 // '
#define BF_SYM_GPS_SECOND 0x22 // "

748
src/main/io/displayport_msp_bf_compat.c
View file

@ -1,748 +0,0 @@
/*
* This file is part of INAV Project.
*
* 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.
*
* 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 "platform.h"
#if defined(USE_OSD) && defined(USE_MSP_DISPLAYPORT)
#ifndef DISABLE_MSP_BF_COMPAT
#include "io/displayport_msp_bf_compat.h"
#include "io/bf_osd_symbols.h"
#include "drivers/osd_symbols.h"
uint8_t getBfCharacter(uint8_t ch, uint8_t page)
{
uint16_t ech = ch | (page << 8);
if (ech >= 0x20 && ech <= 0x5F) { // ASCII range
return ch;
}
if (ech >= SYM_AH_DECORATION_MIN && ech <= SYM_AH_DECORATION_MAX) {
return BF_SYM_AH_DECORATION;
}
switch (ech) {
case SYM_RSSI:
return BF_SYM_RSSI;
case SYM_LQ:
return BF_SYM_LINK_QUALITY;
case SYM_LAT:
return BF_SYM_LAT;
case SYM_LON:
return BF_SYM_LON;
case SYM_SAT_L:
return BF_SYM_SAT_L;
case SYM_SAT_R:
return BF_SYM_SAT_R;
case SYM_HOME_NEAR:
return BF_SYM_HOMEFLAG;
case SYM_DEGREES:
return BF_SYM_GPS_DEGREE;
/*
case SYM_HEADING:
return BF_SYM_HEADING;
case SYM_SCALE:
return BF_SYM_SCALE;
case SYM_HDP_L:
return BF_SYM_HDP_L;
case SYM_HDP_R:
return BF_SYM_HDP_R;
*/
case SYM_HOME:
return BF_SYM_HOMEFLAG;
case SYM_2RSS:
return BF_SYM_RSSI;
/*
case SYM_DB:
return BF_SYM_DB
case SYM_DBM:
return BF_SYM_DBM;
case SYM_SNR:
return BF_SYM_SNR;
case SYM_AH_DECORATION_UP:
return BF_SYM_AH_DECORATION_UP;
case SYM_AH_DECORATION_DOWN:
return BF_SYM_AH_DECORATION_DOWN;
*/
case SYM_DIRECTION:
return BF_SYM_ARROW_NORTH;
case SYM_DIRECTION + 1: // NE pointing arrow
return BF_SYM_ARROW_7;
case SYM_DIRECTION + 2: // E pointing arrow
return BF_SYM_ARROW_EAST;
case SYM_DIRECTION + 3: // SE pointing arrow
return BF_SYM_ARROW_3;
case SYM_DIRECTION + 4: // S pointing arrow
return BF_SYM_ARROW_SOUTH;
case SYM_DIRECTION + 5: // SW pointing arrow
return BF_SYM_ARROW_15;
case SYM_DIRECTION + 6: // W pointing arrow
return BF_SYM_ARROW_WEST;
case SYM_DIRECTION + 7: // NW pointing arrow
return BF_SYM_ARROW_11;
case SYM_VOLT:
return BF_SYM_VOLT;
case SYM_MAH:
return BF_SYM_MAH;
case SYM_AH_KM:
return BF_SYM_KM;
case SYM_AH_MI:
return BF_SYM_MILES;
/*
case SYM_VTX_POWER:
return BF_SYM_VTX_POWER;
case SYM_AH_NM:
return BF_SYM_AH_NM;
case SYM_MAH_NM_0:
return BF_SYM_MAH_NM_0;
case SYM_MAH_NM_1:
return BF_SYM_MAH_NM_1;
case SYM_MAH_KM_0:
return BF_SYM_MAH_KM_0;
case SYM_MAH_KM_1:
return BF_SYM_MAH_KM_1;
case SYM_MILLIOHM:
return BF_SYM_MILLIOHM;
*/
case SYM_BATT_FULL:
return BF_SYM_BATT_FULL;
case SYM_BATT_5:
return BF_SYM_BATT_5;
case SYM_BATT_4:
return BF_SYM_BATT_4;
case SYM_BATT_3:
return BF_SYM_BATT_3;
case SYM_BATT_2:
return BF_SYM_BATT_2;
case SYM_BATT_1:
return BF_SYM_BATT_1;
case SYM_BATT_EMPTY:
return BF_SYM_BATT_EMPTY;
case SYM_AMP:
return BF_SYM_AMP;
/*
case SYM_WH:
return BF_SYM_WH;
case SYM_WH_KM:
return BF_SYM_WH_KM;
case SYM_WH_MI:
return BF_SYM_WH_MI;
case SYM_WH_NM:
return BF_SYM_WH_NM;
*/
case SYM_WATT:
return BF_SYM_WATT;
/*
case SYM_MW:
return BF_SYM_MW;
case SYM_KILOWATT:
return BF_SYM_KILOWATT;
*/
case SYM_FT:
return BF_SYM_FT;
case SYM_ALT_FT:
return BF_SYM_FT;
case SYM_ALT_M:
return BF_SYM_M;
case SYM_TOTAL:
return BF_SYM_TOTAL_DISTANCE;
/*
case SYM_ALT_KM:
return BF_SYM_ALT_KM;
case SYM_ALT_KFT:
return BF_SYM_ALT_KFT;
case SYM_DIST_M:
return BF_SYM_DIST_M;
case SYM_DIST_KM:
return BF_SYM_DIST_KM;
case SYM_DIST_FT:
return BF_SYM_DIST_FT;
case SYM_DIST_MI:
return BF_SYM_DIST_MI;
case SYM_DIST_NM:
return BF_SYM_DIST_NM;
*/
case SYM_M:
return BF_SYM_M;
case SYM_KM:
return BF_SYM_KM;
case SYM_MI:
return BF_SYM_MILES;
/*
case SYM_NM:
return BF_SYM_NM;
*/
case SYM_WIND_HORIZONTAL:
return 'W'; // W for wind
/*
case SYM_WIND_VERTICAL:
return BF_SYM_WIND_VERTICAL;
case SYM_3D_KT:
return BF_SYM_3D_KT;
*/
case SYM_AIR:
return 'A'; // A for airspeed
case SYM_3D_KMH:
return BF_SYM_KPH;
case SYM_3D_MPH:
return BF_SYM_MPH;
case SYM_RPM:
return BF_SYM_RPM;
case SYM_FTS:
return BF_SYM_FTPS;
/*
case SYM_100FTM:
return BF_SYM_100FTM;
*/
case SYM_MS:
return BF_SYM_MPS;
case SYM_KMH:
return BF_SYM_KPH;
case SYM_MPH:
return BF_SYM_MPH;
/*
case SYM_KT:
return BF_SYM_KT
case SYM_MAH_MI_0:
return BF_SYM_MAH_MI_0;
case SYM_MAH_MI_1:
return BF_SYM_MAH_MI_1;
*/
case SYM_THR:
return BF_SYM_THR;
case SYM_TEMP_F:
return BF_SYM_F;
case SYM_TEMP_C:
return BF_SYM_C;
case SYM_BLANK:
return BF_SYM_BLANK;
/*
case SYM_ON_H:
return BF_SYM_ON_H;
case SYM_FLY_H:
return BF_SYM_FLY_H;
*/
case SYM_ON_M:
return BF_SYM_ON_M;
case SYM_FLY_M:
return BF_SYM_FLY_M;
/*
case SYM_GLIDESLOPE:
return BF_SYM_GLIDESLOPE;
case SYM_WAYPOINT:
return BF_SYM_WAYPOINT;
case SYM_CLOCK:
return BF_SYM_CLOCK;
case SYM_ZERO_HALF_TRAILING_DOT:
return BF_SYM_ZERO_HALF_TRAILING_DOT;
case SYM_ZERO_HALF_LEADING_DOT:
return BF_SYM_ZERO_HALF_LEADING_DOT;
case SYM_AUTO_THR0:
return BF_SYM_AUTO_THR0;
case SYM_AUTO_THR1:
return BF_SYM_AUTO_THR1;
case SYM_ROLL_LEFT:
return BF_SYM_ROLL_LEFT;
case SYM_ROLL_LEVEL:
return BF_SYM_ROLL_LEVEL;
case SYM_ROLL_RIGHT:
return BF_SYM_ROLL_RIGHT;
case SYM_PITCH_UP:
return BF_SYM_PITCH_UP;
case SYM_PITCH_DOWN:
return BF_SYM_PITCH_DOWN;
*/
case SYM_GFORCE:
return 'G';
/*
case SYM_GFORCE_X:
return BF_SYM_GFORCE_X;
case SYM_GFORCE_Y:
return BF_SYM_GFORCE_Y;
case SYM_GFORCE_Z:
return BF_SYM_GFORCE_Z;
case SYM_BARO_TEMP:
return BF_SYM_BARO_TEMP;
case SYM_IMU_TEMP:
return BF_SYM_IMU_TEMP;
case SYM_TEMP:
return BF_SYM_TEMP;
case SYM_TEMP_SENSOR_FIRST:
return BF_SYM_TEMP_SENSOR_FIRST;
case SYM_ESC_TEMP:
return BF_SYM_ESC_TEMP;
case SYM_TEMP_SENSOR_LAST:
return BF_SYM_TEMP_SENSOR_LAST;
case TEMP_SENSOR_SYM_COUNT:
return BF_TEMP_SENSOR_SYM_COUNT;
*/
case SYM_HEADING_N:
return BF_SYM_HEADING_N;
case SYM_HEADING_S:
return BF_SYM_HEADING_S;
case SYM_HEADING_E:
return BF_SYM_HEADING_E;
case SYM_HEADING_W:
return BF_SYM_HEADING_W;
case SYM_HEADING_DIVIDED_LINE:
return BF_SYM_HEADING_DIVIDED_LINE;
case SYM_HEADING_LINE:
return BF_SYM_HEADING_LINE;
/*
case SYM_MAX:
return BF_SYM_MAX;
case SYM_PROFILE:
return BF_SYM_PROFILE;
case SYM_SWITCH_INDICATOR_LOW:
return BF_SYM_SWITCH_INDICATOR_LOW;
case SYM_SWITCH_INDICATOR_MID:
return BF_SYM_SWITCH_INDICATOR_MID;
case SYM_SWITCH_INDICATOR_HIGH:
return BF_SYM_SWITCH_INDICATOR_HIGH;
case SYM_AH:
return BF_SYM_AH;
case SYM_GLIDE_DIST:
return BF_SYM_GLIDE_DIST;
case SYM_GLIDE_MINS:
return BF_SYM_GLIDE_MINS;
case SYM_AH_V_FT_0:
return BF_SYM_AH_V_FT_0;
case SYM_AH_V_FT_1:
return BF_SYM_AH_V_FT_1;
case SYM_AH_V_M_0:
return BF_SYM_AH_V_M_0;
case SYM_AH_V_M_1:
return BF_SYM_AH_V_M_1;
case SYM_FLIGHT_MINS_REMAINING:
return BF_SYM_FLIGHT_MINS_REMAINING;
case SYM_FLIGHT_HOURS_REMAINING:
return BF_SYM_FLIGHT_HOURS_REMAINING;
case SYM_GROUND_COURSE:
return BF_SYM_GROUND_COURSE;
case SYM_CROSS_TRACK_ERROR:
return BF_SYM_CROSS_TRACK_ERROR;
case SYM_LOGO_START:
return BF_SYM_LOGO_START;
case SYM_LOGO_WIDTH:
return BF_SYM_LOGO_WIDTH;
case SYM_LOGO_HEIGHT:
return BF_SYM_LOGO_HEIGHT;
*/
case SYM_AH_LEFT:
return BF_SYM_AH_LEFT;
case SYM_AH_RIGHT:
return BF_SYM_AH_RIGHT;
/*
case SYM_AH_DECORATION_COUNT:
return BF_SYM_AH_DECORATION_COUNT;
*/
case SYM_AH_CH_LEFT:
case SYM_AH_CH_TYPE3:
case SYM_AH_CH_TYPE4:
case SYM_AH_CH_TYPE5:
case SYM_AH_CH_TYPE6:
case SYM_AH_CH_TYPE7:
case SYM_AH_CH_TYPE8:
case SYM_AH_CH_AIRCRAFT1:
return BF_SYM_AH_CENTER_LINE;
case SYM_AH_CH_RIGHT:
case (SYM_AH_CH_TYPE3+2):
case (SYM_AH_CH_TYPE4+2):
case (SYM_AH_CH_TYPE5+2):
case (SYM_AH_CH_TYPE6+2):
case (SYM_AH_CH_TYPE7+2):
case (SYM_AH_CH_TYPE8+2):
case SYM_AH_CH_AIRCRAFT3:
return BF_SYM_AH_CENTER_LINE_RIGHT;
case SYM_AH_CH_AIRCRAFT0:
case SYM_AH_CH_AIRCRAFT4:
return ' ';
case SYM_ARROW_UP:
return BF_SYM_ARROW_NORTH;
case SYM_ARROW_2:
return BF_SYM_ARROW_8;
case SYM_ARROW_3:
return BF_SYM_ARROW_7;
case SYM_ARROW_4:
return BF_SYM_ARROW_6;
case SYM_ARROW_RIGHT:
return BF_SYM_ARROW_EAST;
case SYM_ARROW_6:
return BF_SYM_ARROW_4;
case SYM_ARROW_7:
return BF_SYM_ARROW_3;
case SYM_ARROW_8:
return BF_SYM_ARROW_2;
case SYM_ARROW_DOWN:
return BF_SYM_ARROW_SOUTH;
case SYM_ARROW_10:
return BF_SYM_ARROW_16;
case SYM_ARROW_11:
return BF_SYM_ARROW_15;
case SYM_ARROW_12:
return BF_SYM_ARROW_14;
case SYM_ARROW_LEFT:
return BF_SYM_ARROW_WEST;
case SYM_ARROW_14:
return BF_SYM_ARROW_12;
case SYM_ARROW_15:
return BF_SYM_ARROW_11;
case SYM_ARROW_16:
return BF_SYM_ARROW_10;
case SYM_AH_H_START:
return BF_SYM_AH_BAR9_0;
case (SYM_AH_H_START+1):
return BF_SYM_AH_BAR9_1;
case (SYM_AH_H_START+2):
return BF_SYM_AH_BAR9_2;
case (SYM_AH_H_START+3):
return BF_SYM_AH_BAR9_3;
case (SYM_AH_H_START+4):
return BF_SYM_AH_BAR9_4;
case (SYM_AH_H_START+5):
return BF_SYM_AH_BAR9_5;
case (SYM_AH_H_START+6):
return BF_SYM_AH_BAR9_6;
case (SYM_AH_H_START+7):
return BF_SYM_AH_BAR9_7;
case (SYM_AH_H_START+8):
return BF_SYM_AH_BAR9_8;
// BF does not have vertical artificial horizon. replace with middle horizontal one
case SYM_AH_V_START:
case (SYM_AH_V_START+1):
case (SYM_AH_V_START+2):
case (SYM_AH_V_START+3):
case (SYM_AH_V_START+4):
case (SYM_AH_V_START+5):
return BF_SYM_AH_BAR9_4;
// BF for ESP_RADAR Symbols
case SYM_HUD_CARDINAL:
return BF_SYM_ARROW_SOUTH;
case SYM_HUD_CARDINAL + 1:
return BF_SYM_ARROW_16;
case SYM_HUD_CARDINAL + 2:
return BF_SYM_ARROW_15;
case SYM_HUD_CARDINAL + 3:
return BF_SYM_ARROW_WEST;
case SYM_HUD_CARDINAL + 4:
return BF_SYM_ARROW_12;
case SYM_HUD_CARDINAL + 5:
return BF_SYM_ARROW_11;
case SYM_HUD_CARDINAL + 6:
return BF_SYM_ARROW_NORTH;
case SYM_HUD_CARDINAL + 7:
return BF_SYM_ARROW_7;
case SYM_HUD_CARDINAL + 8:
return BF_SYM_ARROW_6;
case SYM_HUD_CARDINAL + 9:
return BF_SYM_ARROW_EAST;
case SYM_HUD_CARDINAL + 10:
return BF_SYM_ARROW_3;
case SYM_HUD_CARDINAL + 11:
return BF_SYM_ARROW_2;
case SYM_HUD_ARROWS_R3:
return BF_SYM_AH_RIGHT;
case SYM_HUD_ARROWS_L3:
return BF_SYM_AH_LEFT;
case SYM_HUD_SIGNAL_0:
return BF_SYM_AH_BAR9_1;
case SYM_HUD_SIGNAL_1:
return BF_SYM_AH_BAR9_3;
case SYM_HUD_SIGNAL_2:
return BF_SYM_AH_BAR9_4;
case SYM_HUD_SIGNAL_3:
return BF_SYM_AH_BAR9_5;
case SYM_HUD_SIGNAL_4:
return BF_SYM_AH_BAR9_7;
/*
case SYM_VARIO_UP_2A:
return BF_SYM_VARIO_UP_2A;
case SYM_VARIO_UP_1A:
return BF_SYM_VARIO_UP_1A;
case SYM_VARIO_DOWN_1A:
return BF_SYM_VARIO_DOWN_1A;
case SYM_VARIO_DOWN_2A:
return BF_SYM_VARIO_DOWN_2A;
*/
case SYM_ALT:
return BF_SYM_ALTITUDE;
/*
case SYM_HUD_SIGNAL_0:
return BF_SYM_HUD_SIGNAL_0;
case SYM_HUD_SIGNAL_1:
return BF_SYM_HUD_SIGNAL_1;
case SYM_HUD_SIGNAL_2:
return BF_SYM_HUD_SIGNAL_2;
case SYM_HUD_SIGNAL_3:
return BF_SYM_HUD_SIGNAL_3;
case SYM_HUD_SIGNAL_4:
return BF_SYM_HUD_SIGNAL_4;
case SYM_HOME_DIST:
return BF_SYM_HOME_DIST;
*/
case SYM_AH_CH_CENTER:
case (SYM_AH_CH_TYPE3+1):
case (SYM_AH_CH_TYPE4+1):
case (SYM_AH_CH_TYPE5+1):
case (SYM_AH_CH_TYPE6+1):
case (SYM_AH_CH_TYPE7+1):
case (SYM_AH_CH_TYPE8+1):
case SYM_AH_CH_AIRCRAFT2:
return BF_SYM_AH_CENTER;
/*
case SYM_FLIGHT_DIST_REMAINING:
return BF_SYM_FLIGHT_DIST_REMAINING;
case SYM_AH_CH_TYPE3:
return BF_SYM_AH_CH_TYPE3;
case SYM_AH_CH_TYPE4:
return BF_SYM_AH_CH_TYPE4;
case SYM_AH_CH_TYPE5:
return BF_SYM_AH_CH_TYPE5;
case SYM_AH_CH_TYPE6:
return BF_SYM_AH_CH_TYPE6;
case SYM_AH_CH_TYPE7:
return BF_SYM_AH_CH_TYPE7;
case SYM_AH_CH_TYPE8:
return BF_SYM_AH_CH_TYPE8;
case SYM_AH_CH_AIRCRAFT0:
return BF_SYM_AH_CH_AIRCRAFT0;
case SYM_AH_CH_AIRCRAFT1:
return BF_SYM_AH_CH_AIRCRAFT1;
case SYM_AH_CH_AIRCRAFT2:
return BF_SYM_AH_CH_AIRCRAFT2;
case SYM_AH_CH_AIRCRAFT3:
return BF_SYM_AH_CH_AIRCRAFT3;
case SYM_AH_CH_AIRCRAFT4:
return BF_SYM_AH_CH_AIRCRAFT4;
case SYM_HUD_ARROWS_L1:
return BF_SYM_HUD_ARROWS_L1;
case SYM_HUD_ARROWS_L2:
return BF_SYM_HUD_ARROWS_L2;
case SYM_HUD_ARROWS_L3:
return BF_SYM_HUD_ARROWS_L3;
case SYM_HUD_ARROWS_R1:
return BF_SYM_HUD_ARROWS_R1;
case SYM_HUD_ARROWS_R2:
return BF_SYM_HUD_ARROWS_R2;
case SYM_HUD_ARROWS_R3:
return BF_SYM_HUD_ARROWS_R3;
case SYM_HUD_ARROWS_U1:
return BF_SYM_HUD_ARROWS_U1;
case SYM_HUD_ARROWS_U2:
return BF_SYM_HUD_ARROWS_U2;
case SYM_HUD_ARROWS_U3:
return BF_SYM_HUD_ARROWS_U3;
case SYM_HUD_ARROWS_D1:
return BF_SYM_HUD_ARROWS_D1;
case SYM_HUD_ARROWS_D2:
return BF_SYM_HUD_ARROWS_D2;
case SYM_HUD_ARROWS_D3:
return BF_SYM_HUD_ARROWS_D3;
*/
default:
break;
}
return '?'; // Missing/not mapped character
}
#endif
#endif

732
src/main/io/displayport_msp_dji_compat.c Normal file
View file

@ -0,0 +1,732 @@
/*
* This file is part of INAV Project.
*
* 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.
*
* 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 "platform.h"
#if defined(USE_OSD) && defined(USE_MSP_DISPLAYPORT)
#ifndef DISABLE_MSP_DJI_COMPAT
#include "io/displayport_msp_dji_compat.h"
#include "io/dji_osd_symbols.h"
#include "drivers/osd_symbols.h"
uint8_t getDJICharacter(uint8_t ch, uint8_t page)
{
uint16_t ech = ch | ((page & 0x3)<< 8) ;
if (ech >= 0x20 && ech <= 0x5F) { // ASCII range
return ch;
}
if (ech >= SYM_AH_DECORATION_MIN && ech <= SYM_AH_DECORATION_MAX) {
return DJI_SYM_AH_DECORATION;
}
switch (ech) {
case SYM_RSSI:
return DJI_SYM_RSSI;
case SYM_LQ:
return 'Q';
case SYM_LAT:
return DJI_SYM_LAT;
case SYM_LON:
return DJI_SYM_LON;
case SYM_SAT_L:
return DJI_SYM_SAT_L;
case SYM_SAT_R:
return DJI_SYM_SAT_R;
case SYM_HOME_NEAR:
return DJI_SYM_HOMEFLAG;
case SYM_DEGREES:
return DJI_SYM_GPS_DEGREE;
/*
case SYM_HEADING:
return DJI_SYM_HEADING;
case SYM_SCALE:
return DJI_SYM_SCALE;
case SYM_HDP_L:
return DJI_SYM_HDP_L;
case SYM_HDP_R:
return DJI_SYM_HDP_R;
*/
case SYM_HOME:
return DJI_SYM_HOMEFLAG;
case SYM_2RSS:
return DJI_SYM_RSSI;
/*
case SYM_DB:
return DJI_SYM_DB
case SYM_DBM:
return DJI_SYM_DBM;
case SYM_SNR:
return DJI_SYM_SNR;
*/
case SYM_AH_DECORATION_UP:
return DJI_SYM_ARROW_SMALL_UP;
case SYM_AH_DECORATION_DOWN:
return DJI_SYM_ARROW_SMALL_DOWN;
case SYM_DECORATION:
return DJI_SYM_ARROW_SMALL_UP;
case SYM_DECORATION + 1: // NE pointing arrow
return DJI_SYM_ARROW_7;
case SYM_DECORATION + 2: // E pointing arrow
return DJI_SYM_ARROW_EAST;
case SYM_DECORATION + 3: // SE pointing arrow
return DJI_SYM_ARROW_3;
case SYM_DECORATION + 4: // S pointing arrow
return DJI_SYM_ARROW_SOUTH;
case SYM_DECORATION + 5: // SW pointing arrow
return DJI_SYM_ARROW_15;
case SYM_DECORATION + 6: // W pointing arrow
return DJI_SYM_ARROW_WEST;
case SYM_DECORATION + 7: // NW pointing arrow
return DJI_SYM_ARROW_11;
case SYM_VOLT:
return DJI_SYM_VOLT;
case SYM_MAH:
return DJI_SYM_MAH;
case SYM_AH_KM:
return 'K';
case SYM_AH_MI:
return 'M';
/*
case SYM_VTX_POWER:
return DJI_SYM_VTX_POWER;
case SYM_AH_NM:
return DJI_SYM_AH_NM;
case SYM_MAH_NM_0:
return DJI_SYM_MAH_NM_0;
case SYM_MAH_NM_1:
return DJI_SYM_MAH_NM_1;
case SYM_MAH_KM_0:
return DJI_SYM_MAH_KM_0;
case SYM_MAH_KM_1:
return DJI_SYM_MAH_KM_1;
case SYM_MILLIOHM:
return DJI_SYM_MILLIOHM;
*/
case SYM_BATT_FULL:
return DJI_SYM_BATT_FULL;
case SYM_BATT_5:
return DJI_SYM_BATT_5;
case SYM_BATT_4:
return DJI_SYM_BATT_4;
case SYM_BATT_3:
return DJI_SYM_BATT_3;
case SYM_BATT_2:
return DJI_SYM_BATT_2;
case SYM_BATT_1:
return DJI_SYM_BATT_1;
case SYM_BATT_EMPTY:
return DJI_SYM_BATT_EMPTY;
case SYM_AMP:
return DJI_SYM_AMP;
/*
case SYM_WH:
return DJI_SYM_WH;
case SYM_WH_KM:
return DJI_SYM_WH_KM;
case SYM_WH_MI:
return DJI_SYM_WH_MI;
case SYM_WH_NM:
return DJI_SYM_WH_NM;
*/
case SYM_WATT:
return DJI_SYM_WATT;
/*
case SYM_MW:
return DJI_SYM_MW;
case SYM_KILOWATT:
return DJI_SYM_KILOWATT;
*/
case SYM_FT:
return DJI_SYM_FT;
case SYM_ALT_FT:
return DJI_SYM_FT;
case SYM_ALT_M:
return DJI_SYM_M;
case SYM_TOTAL:
return DJI_SYM_FLY_H;
/*
case SYM_ALT_KM:
return DJI_SYM_ALT_KM;
case SYM_ALT_KFT:
return DJI_SYM_ALT_KFT;
case SYM_DIST_M:
return DJI_SYM_DIST_M;
case SYM_DIST_KM:
return DJI_SYM_DIST_KM;
case SYM_DIST_FT:
return DJI_SYM_DIST_FT;
case SYM_DIST_MI:
return DJI_SYM_DIST_MI;
case SYM_DIST_NM:
return DJI_SYM_DIST_NM;
*/
case SYM_M:
return DJI_SYM_M;
case SYM_KM:
return 'K';
case SYM_MI:
return 'M';
/*
case SYM_NM:
return DJI_SYM_NM;
*/
case SYM_WIND_HORIZONTAL:
return 'W'; // W for wind
/*
case SYM_WIND_VERTICAL:
return DJI_SYM_WIND_VERTICAL;
case SYM_3D_KT:
return DJI_SYM_3D_KT;
*/
case SYM_AIR:
return 'A'; // A for airspeed
case SYM_3D_KMH:
return DJI_SYM_KPH;
case SYM_3D_MPH:
return DJI_SYM_MPH;
case SYM_RPM:
return DJI_SYM_RPM;
case SYM_FTS:
return DJI_SYM_FTPS;
/*
case SYM_100FTM:
return DJI_SYM_100FTM;
*/
case SYM_MS:
return DJI_SYM_MPS;
case SYM_KMH:
return DJI_SYM_KPH;
case SYM_MPH:
return DJI_SYM_MPH;
/*
case SYM_KT:
return DJI_SYM_KT
case SYM_MAH_MI_0:
return DJI_SYM_MAH_MI_0;
case SYM_MAH_MI_1:
return DJI_SYM_MAH_MI_1;
*/
case SYM_THR:
return DJI_SYM_THR;
case SYM_TEMP_F:
return DJI_SYM_F;
case SYM_TEMP_C:
return DJI_SYM_C;
case SYM_BLANK:
return DJI_SYM_BLANK;
case SYM_ON_H:
return DJI_SYM_ON_H;
case SYM_FLY_H:
return DJI_SYM_FLY_H;
case SYM_ON_M:
return DJI_SYM_ON_M;
case SYM_FLY_M:
return DJI_SYM_FLY_M;
/*
case SYM_GLIDESLOPE:
return DJI_SYM_GLIDESLOPE;
case SYM_WAYPOINT:
return DJI_SYM_WAYPOINT;
case SYM_CLOCK:
return DJI_SYM_CLOCK;
case SYM_ZERO_HALF_TRAILING_DOT:
return DJI_SYM_ZERO_HALF_TRAILING_DOT;
case SYM_ZERO_HALF_LEADING_DOT:
return DJI_SYM_ZERO_HALF_LEADING_DOT;
*/
case SYM_AUTO_THR0:
return 'A';
case SYM_AUTO_THR1:
return DJI_SYM_THR;
case SYM_ROLL_LEFT:
return DJI_SYM_ROLL;
case SYM_ROLL_LEVEL:
return DJI_SYM_ROLL;
case SYM_ROLL_RIGHT:
return DJI_SYM_ROLL;
case SYM_PITCH_UP:
return DJI_SYM_PITCH;
case SYM_PITCH_DOWN:
return DJI_SYM_PITCH;
case SYM_GFORCE:
return 'G';
/*
case SYM_GFORCE_X:
return DJI_SYM_GFORCE_X;
case SYM_GFORCE_Y:
return DJI_SYM_GFORCE_Y;
case SYM_GFORCE_Z:
return DJI_SYM_GFORCE_Z;
*/
case SYM_BARO_TEMP:
return DJI_SYM_TEMPERATURE;
case SYM_IMU_TEMP:
return DJI_SYM_TEMPERATURE;
case SYM_TEMP:
return DJI_SYM_TEMPERATURE;
case SYM_ESC_TEMP:
return DJI_SYM_TEMPERATURE;
/*
case SYM_TEMP_SENSOR_FIRST:
return DJI_SYM_TEMP_SENSOR_FIRST;
case SYM_TEMP_SENSOR_LAST:
return DJI_SYM_TEMP_SENSOR_LAST;
case TEMP_SENSOR_SYM_COUNT:
return DJI_TEMP_SENSOR_SYM_COUNT;
*/
case SYM_HEADING_N:
return DJI_SYM_HEADING_N;
case SYM_HEADING_S:
return DJI_SYM_HEADING_S;
case SYM_HEADING_E:
return DJI_SYM_HEADING_E;
case SYM_HEADING_W:
return DJI_SYM_HEADING_W;
case SYM_HEADING_DIVIDED_LINE:
return DJI_SYM_HEADING_DIVIDED_LINE;
case SYM_HEADING_LINE:
return DJI_SYM_HEADING_LINE;
case SYM_MAX:
return DJI_SYM_MAX;
/*
case SYM_PROFILE:
return DJI_SYM_PROFILE;
*/
case SYM_SWITCH_INDICATOR_LOW:
return DJI_SYM_STICK_OVERLAY_SPRITE_LOW;
case SYM_SWITCH_INDICATOR_MID:
return DJI_SYM_STICK_OVERLAY_SPRITE_MID;
case SYM_SWITCH_INDICATOR_HIGH:
return DJI_SYM_STICK_OVERLAY_SPRITE_HIGH;
/*
case SYM_AH:
return DJI_SYM_AH;
case SYM_GLIDE_DIST:
return DJI_SYM_GLIDE_DIST;
case SYM_GLIDE_MINS:
return DJI_SYM_GLIDE_MINS;
case SYM_AH_V_FT_0:
return DJI_SYM_AH_V_FT_0;
case SYM_AH_V_FT_1:
return DJI_SYM_AH_V_FT_1;
case SYM_AH_V_M_0:
return DJI_SYM_AH_V_M_0;
case SYM_AH_V_M_1:
return DJI_SYM_AH_V_M_1;
case SYM_FLIGHT_MINS_REMAINING:
return DJI_SYM_FLIGHT_MINS_REMAINING;
case SYM_FLIGHT_HOURS_REMAINING:
return DJI_SYM_FLIGHT_HOURS_REMAINING;
case SYM_GROUND_COURSE:
return DJI_SYM_GROUND_COURSE;
case SYM_CROSS_TRACK_ERROR:
return DJI_SYM_CROSS_TRACK_ERROR;
case SYM_LOGO_START:
return DJI_SYM_LOGO_START;
case SYM_LOGO_WIDTH:
return DJI_SYM_LOGO_WIDTH;
case SYM_LOGO_HEIGHT:
return DJI_SYM_LOGO_HEIGHT;
*/
case SYM_AH_LEFT:
return DJI_SYM_AH_LEFT;
case SYM_AH_RIGHT:
return DJI_SYM_AH_RIGHT;
/*
case SYM_AH_DECORATION_COUNT:
return DJI_SYM_AH_DECORATION_COUNT;
*/
case SYM_AH_CH_LEFT:
case SYM_AH_CH_AIRCRAFT1:
return DJI_SYM_CROSSHAIR_LEFT;
case SYM_AH_CH_CENTER:
case SYM_AH_CH_AIRCRAFT2:
return DJI_SYM_CROSSHAIR_CENTRE;
case SYM_AH_CH_RIGHT:
case SYM_AH_CH_AIRCRAFT3:
return DJI_SYM_CROSSHAIR_RIGHT;
case SYM_AH_CH_AIRCRAFT0:
case SYM_AH_CH_AIRCRAFT4:
return DJI_SYM_BLANK;
case SYM_AH_CH_TYPE3:
return DJI_SYM_NONE;
case (SYM_AH_CH_TYPE3+1):
return DJI_SYM_SMALL_CROSSHAIR;
case (SYM_AH_CH_TYPE3+2):
return DJI_SYM_NONE;
case SYM_AH_CH_TYPE4:
return DJI_SYM_HYPHEN;
case (SYM_AH_CH_TYPE4+1):
return DJI_SYM_SMALL_CROSSHAIR;
case (SYM_AH_CH_TYPE4+2):
return DJI_SYM_HYPHEN;
case SYM_AH_CH_TYPE5:
return DJI_SYM_STICK_OVERLAY_HORIZONTAL;
case (SYM_AH_CH_TYPE5+1):
return DJI_SYM_SMALL_CROSSHAIR;
case (SYM_AH_CH_TYPE5+2):
return DJI_SYM_STICK_OVERLAY_HORIZONTAL;
case SYM_AH_CH_TYPE6:
return DJI_SYM_NONE;
case (SYM_AH_CH_TYPE6+1):
return DJI_SYM_STICK_OVERLAY_SPRITE_MID;
case (SYM_AH_CH_TYPE6+2):
return DJI_SYM_NONE;
case SYM_AH_CH_TYPE7:
return DJI_SYM_ARROW_SMALL_LEFT;
case (SYM_AH_CH_TYPE7+1):
return DJI_SYM_SMALL_CROSSHAIR;
case (SYM_AH_CH_TYPE7+2):
return DJI_SYM_ARROW_SMALL_RIGHT;
case SYM_AH_CH_TYPE8:
return DJI_SYM_AH_LEFT;
case (SYM_AH_CH_TYPE8+1):
return DJI_SYM_SMALL_CROSSHAIR;
case (SYM_AH_CH_TYPE8+2):
return DJI_SYM_AH_RIGHT;
case SYM_ARROW_UP:
return DJI_SYM_ARROW_NORTH;
case SYM_ARROW_2:
return DJI_SYM_ARROW_8;
case SYM_ARROW_3:
return DJI_SYM_ARROW_7;
case SYM_ARROW_4:
return DJI_SYM_ARROW_6;
case SYM_ARROW_RIGHT:
return DJI_SYM_ARROW_EAST;
case SYM_ARROW_6:
return DJI_SYM_ARROW_4;
case SYM_ARROW_7:
return DJI_SYM_ARROW_3;
case SYM_ARROW_8:
return DJI_SYM_ARROW_2;
case SYM_ARROW_DOWN:
return DJI_SYM_ARROW_SOUTH;
case SYM_ARROW_10:
return DJI_SYM_ARROW_16;
case SYM_ARROW_11:
return DJI_SYM_ARROW_15;
case SYM_ARROW_12:
return DJI_SYM_ARROW_14;
case SYM_ARROW_LEFT:
return DJI_SYM_ARROW_WEST;
case SYM_ARROW_14:
return DJI_SYM_ARROW_12;
case SYM_ARROW_15:
return DJI_SYM_ARROW_11;
case SYM_ARROW_16:
return DJI_SYM_ARROW_10;
case SYM_AH_H_START:
return DJI_SYM_AH_BAR9_0;
case (SYM_AH_H_START+1):
return DJI_SYM_AH_BAR9_1;
case (SYM_AH_H_START+2):
return DJI_SYM_AH_BAR9_2;
case (SYM_AH_H_START+3):
return DJI_SYM_AH_BAR9_3;
case (SYM_AH_H_START+4):
return DJI_SYM_AH_BAR9_4;
case (SYM_AH_H_START+5):
return DJI_SYM_AH_BAR9_5;
case (SYM_AH_H_START+6):
return DJI_SYM_AH_BAR9_6;
case (SYM_AH_H_START+7):
return DJI_SYM_AH_BAR9_7;
case (SYM_AH_H_START+8):
return DJI_SYM_AH_BAR9_8;
// DJI does not have vertical artificial horizon. replace with middle horizontal one
case SYM_AH_V_START:
case (SYM_AH_V_START+1):
case (SYM_AH_V_START+2):
case (SYM_AH_V_START+3):
case (SYM_AH_V_START+4):
case (SYM_AH_V_START+5):
return DJI_SYM_AH_BAR9_4;
// DJI for ESP_RADAR Symbols
case SYM_HUD_CARDINAL:
return DJI_SYM_ARROW_SOUTH;
case SYM_HUD_CARDINAL + 1:
return DJI_SYM_ARROW_16;
case SYM_HUD_CARDINAL + 2:
return DJI_SYM_ARROW_15;
case SYM_HUD_CARDINAL + 3:
return DJI_SYM_ARROW_WEST;
case SYM_HUD_CARDINAL + 4:
return DJI_SYM_ARROW_12;
case SYM_HUD_CARDINAL + 5:
return DJI_SYM_ARROW_11;
case SYM_HUD_CARDINAL + 6:
return DJI_SYM_ARROW_NORTH;
case SYM_HUD_CARDINAL + 7:
return DJI_SYM_ARROW_7;
case SYM_HUD_CARDINAL + 8:
return DJI_SYM_ARROW_6;
case SYM_HUD_CARDINAL + 9:
return DJI_SYM_ARROW_EAST;
case SYM_HUD_CARDINAL + 10:
return DJI_SYM_ARROW_3;
case SYM_HUD_CARDINAL + 11:
return DJI_SYM_ARROW_2;
case SYM_HUD_SIGNAL_0:
return DJI_SYM_AH_BAR9_1;
case SYM_HUD_SIGNAL_1:
return DJI_SYM_AH_BAR9_3;
case SYM_HUD_SIGNAL_2:
return DJI_SYM_AH_BAR9_4;
case SYM_HUD_SIGNAL_3:
return DJI_SYM_AH_BAR9_5;
case SYM_HUD_SIGNAL_4:
return DJI_SYM_AH_BAR9_7;
case SYM_VARIO_UP_2A:
return DJI_SYM_ARROW_SMALL_UP;
case SYM_VARIO_UP_1A:
return DJI_SYM_ARROW_SMALL_UP;
case SYM_VARIO_DOWN_1A:
return DJI_SYM_ARROW_SMALL_DOWN;
case SYM_VARIO_DOWN_2A:
return DJI_SYM_ARROW_SMALL_DOWN;
case SYM_ALT:
return DJI_SYM_ALTITUDE;
/*
case SYM_HUD_SIGNAL_0:
return DJI_SYM_HUD_SIGNAL_0;
case SYM_HUD_SIGNAL_1:
return DJI_SYM_HUD_SIGNAL_1;
case SYM_HUD_SIGNAL_2:
return DJI_SYM_HUD_SIGNAL_2;
case SYM_HUD_SIGNAL_3:
return DJI_SYM_HUD_SIGNAL_3;
case SYM_HUD_SIGNAL_4:
return DJI_SYM_HUD_SIGNAL_4;
case SYM_HOME_DIST:
return DJI_SYM_HOME_DIST;
case SYM_FLIGHT_DIST_REMAINING:
return DJI_SYM_FLIGHT_DIST_REMAINING;
*/
case SYM_HUD_ARROWS_L1:
return DJI_SYM_ARROW_SMALL_LEFT;
case SYM_HUD_ARROWS_L2:
return DJI_SYM_ARROW_SMALL_LEFT;
case SYM_HUD_ARROWS_L3:
return DJI_SYM_ARROW_SMALL_LEFT;
case SYM_HUD_ARROWS_R1:
return DJI_SYM_ARROW_SMALL_RIGHT;
case SYM_HUD_ARROWS_R2:
return DJI_SYM_ARROW_SMALL_RIGHT;
case SYM_HUD_ARROWS_R3:
return DJI_SYM_ARROW_SMALL_RIGHT;
case SYM_HUD_ARROWS_U1:
return DJI_SYM_ARROW_SMALL_UP;
case SYM_HUD_ARROWS_U2:
return DJI_SYM_ARROW_SMALL_UP;
case SYM_HUD_ARROWS_U3:
return DJI_SYM_ARROW_SMALL_UP;
case SYM_HUD_ARROWS_D1:
return DJI_SYM_ARROW_SMALL_DOWN;
case SYM_HUD_ARROWS_D2:
return DJI_SYM_ARROW_SMALL_DOWN;
case SYM_HUD_ARROWS_D3:
return DJI_SYM_ARROW_SMALL_DOWN;
default:
break;
}
return (osdConfig()->highlight_djis_missing_characters) ? '?' : SYM_BLANK; // Missing/not mapped character
}
#endif
#endif

12
src/main/io/displayport_msp_bf_compat.h → src/main/io/displayport_msp_dji_compat.h
View file

@ -22,15 +22,15 @@
#include "platform.h"
#if defined(USE_OSD) && defined(USE_MSP_DISPLAYPORT) && !defined(DISABLE_MSP_BF_COMPAT)
#if defined(USE_OSD) && defined(USE_MSP_DISPLAYPORT) && !defined(DISABLE_MSP_DJI_COMPAT)
#include "osd.h"
uint8_t getBfCharacter(uint8_t ch, uint8_t page);
#define isBfCompatibleVideoSystem(osdConfigPtr) (osdConfigPtr->video_system == VIDEO_SYSTEM_BFCOMPAT || osdConfigPtr->video_system == VIDEO_SYSTEM_BFCOMPAT_HD)
uint8_t getDJICharacter(uint8_t ch, uint8_t page);
#define isDJICompatibleVideoSystem(osdConfigPtr) (osdConfigPtr->video_system == VIDEO_SYSTEM_DJICOMPAT || osdConfigPtr->video_system == VIDEO_SYSTEM_DJICOMPAT_HD)
#else
#define getBfCharacter(x, page) (x)
#define getDJICharacter(x, page) (x)
#ifdef OSD_UNIT_TEST
#define isBfCompatibleVideoSystem(osdConfigPtr) (true)
#define isDJICompatibleVideoSystem(osdConfigPtr) (true)
#else
#define isBfCompatibleVideoSystem(osdConfigPtr) (false)
#define isDJICompatibleVideoSystem(osdConfigPtr) (false)
#endif
#endif

60
src/main/io/displayport_msp_osd.c
View file

@ -51,7 +51,7 @@
#include "msp/msp_serial.h"
#include "displayport_msp_osd.h"
#include "displayport_msp_bf_compat.h"
#include "displayport_msp_dji_compat.h"
#define FONT_VERSION 3
@ -100,9 +100,8 @@ static timeMs_t sendSubFrameMs = 0;
static uint8_t currentOsdMode; // HDZero screen mode can change across layouts
static uint8_t screen[SCREENSIZE];
static BITARRAY_DECLARE(fontPage, SCREENSIZE); // font page for each character on the screen
static uint8_t attrs[SCREENSIZE]; // font page, blink and other attributes
static BITARRAY_DECLARE(dirty, SCREENSIZE); // change status for each character on the screen
static BITARRAY_DECLARE(blinkChar, SCREENSIZE); // Does the character blink?
static bool screenCleared;
static uint8_t screenRows, screenCols;
static videoSystem_e osdVideoSystem;
@ -158,6 +157,22 @@ static uint8_t determineHDZeroOsdMode(void)
return HD_3016;
}
uint8_t setAttrPage(uint8_t origAttr, uint8_t page)
{
return (origAttr & ~DISPLAYPORT_MSP_ATTR_FONTPAGE_MASK) | (page & DISPLAYPORT_MSP_ATTR_FONTPAGE_MASK);
}
uint8_t setAttrBlink(uint8_t origAttr, uint8_t blink)
{
return (origAttr & ~DISPLAYPORT_MSP_ATTR_BLINK_MASK) | ((blink << DISPLAYPORT_MSP_ATTR_BLINK) & DISPLAYPORT_MSP_ATTR_BLINK_MASK);
}
uint8_t setAttrVersion(uint8_t origAttr, uint8_t version)
{
return (origAttr & ~DISPLAYPORT_MSP_ATTR_VERSION_MASK) | ((version << DISPLAYPORT_MSP_ATTR_VERSION) & DISPLAYPORT_MSP_ATTR_VERSION_MASK);
}
static int setDisplayMode(displayPort_t *displayPort)
{
if (osdVideoSystem == VIDEO_SYSTEM_HDZERO) {
@ -171,9 +186,8 @@ static int setDisplayMode(displayPort_t *displayPort)
static void init(void)
{
memset(screen, SYM_BLANK, sizeof(screen));
BITARRAY_CLR_ALL(fontPage);
memset(attrs, 0, sizeof(attrs));
BITARRAY_CLR_ALL(dirty);
BITARRAY_CLR_ALL(blinkChar);
}
static int clearScreen(displayPort_t *displayPort)
@ -204,9 +218,8 @@ static bool readChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint1
}
*c = screen[pos];
if (bitArrayGet(fontPage, pos)) {
*c |= 0x100;
}
uint8_t page = getAttrPage(attrs[pos]);
*c |= page << 8;
if (attr) {
*attr = TEXT_ATTRIBUTES_NONE;
@ -219,11 +232,12 @@ static int setChar(const uint16_t pos, const uint16_t c, textAttributes_t attr)
{
if (pos < SCREENSIZE) {
uint8_t ch = c & 0xFF;
bool page = (c >> 8);
if (screen[pos] != ch || bitArrayGet(fontPage, pos) != page) {
uint8_t page = (c >> 8) & DISPLAYPORT_MSP_ATTR_FONTPAGE_MASK;
if (screen[pos] != ch || getAttrPage(attrs[pos]) != page) {
screen[pos] = ch;
(page) ? bitArraySet(fontPage, pos) : bitArrayClr(fontPage, pos);
(TEXT_ATTRIBUTES_HAVE_BLINK(attr)) ? bitArraySet(blinkChar, pos) : bitArrayClr(blinkChar, pos);
attrs[pos] = setAttrPage(attrs[pos], page);
uint8_t blink = (TEXT_ATTRIBUTES_HAVE_BLINK(attr)) ? 1 : 0;
attrs[pos] = setAttrBlink(attrs[pos], blink);
bitArraySet(dirty, pos);
}
}
@ -287,21 +301,21 @@ static int drawScreen(displayPort_t *displayPort) // 250Hz
uint8_t col = pos % COLS;
uint8_t attributes = 0;
int endOfLine = row * COLS + screenCols;
bool page = bitArrayGet(fontPage, pos);
bool blink = bitArrayGet(blinkChar, pos);
uint8_t page = getAttrPage(attrs[pos]);
uint8_t blink = getAttrBlink(attrs[pos]);
uint8_t len = 4;
do {
bitArrayClr(dirty, pos);
subcmd[len] = isBfCompatibleVideoSystem(osdConfig()) ? getBfCharacter(screen[pos++], page): screen[pos++];
subcmd[len] = isDJICompatibleVideoSystem(osdConfig()) ? getDJICharacter(screen[pos++], page): screen[pos++];
len++;
if (bitArrayGet(dirty, pos)) {
next = pos;
}
} while (next == pos && next < endOfLine && bitArrayGet(fontPage, next) == page && bitArrayGet(blinkChar, next) == blink);
} while (next == pos && next < endOfLine && getAttrPage(attrs[next]) == page && getAttrBlink(attrs[next]) == blink);
if (!isBfCompatibleVideoSystem(osdConfig())) {
if (!isDJICompatibleVideoSystem(osdConfig())) {
attributes |= (page << DISPLAYPORT_MSP_ATTR_FONTPAGE);
}
@ -451,7 +465,7 @@ displayPort_t* mspOsdDisplayPortInit(const videoSystem_e videoSystem)
if (mspOsdSerialInit()) {
switch(videoSystem) {
case VIDEO_SYSTEM_AUTO:
case VIDEO_SYSTEM_BFCOMPAT:
case VIDEO_SYSTEM_DJICOMPAT:
case VIDEO_SYSTEM_PAL:
currentOsdMode = SD_3016;
screenRows = PAL_ROWS;
@ -472,7 +486,7 @@ displayPort_t* mspOsdDisplayPortInit(const videoSystem_e videoSystem)
screenRows = DJI_ROWS;
screenCols = DJI_COLS;
break;
case VIDEO_SYSTEM_BFCOMPAT_HD:
case VIDEO_SYSTEM_DJICOMPAT_HD:
case VIDEO_SYSTEM_AVATAR:
currentOsdMode = HD_5320;
screenRows = AVATAR_ROWS;
@ -486,10 +500,10 @@ displayPort_t* mspOsdDisplayPortInit(const videoSystem_e videoSystem)
init();
displayInit(&mspOsdDisplayPort, &mspOsdVTable);
if (osdVideoSystem == VIDEO_SYSTEM_BFCOMPAT) {
mspOsdDisplayPort.displayPortType = "MSP DisplayPort: BetaFlight Compatability mode";
} else if (osdVideoSystem == VIDEO_SYSTEM_BFCOMPAT_HD) {
mspOsdDisplayPort.displayPortType = "MSP DisplayPort: BetaFlight Compatability mode (HD)";
if (osdVideoSystem == VIDEO_SYSTEM_DJICOMPAT) {
mspOsdDisplayPort.displayPortType = "MSP DisplayPort: DJI Compatability mode";
} else if (osdVideoSystem == VIDEO_SYSTEM_DJICOMPAT_HD) {
mspOsdDisplayPort.displayPortType = "MSP DisplayPort: DJI Compatability mode (HD)";
} else {
mspOsdDisplayPort.displayPortType = "MSP DisplayPort";
}

20
src/main/io/displayport_msp_osd.h
View file

@ -27,13 +27,25 @@
#include "drivers/osd.h"
#include "msp/msp_serial.h"
// MSP displayport V2 attribute byte bit functions
#define DISPLAYPORT_MSP_ATTR_FONTPAGE 0 // Select bank of 256 characters as per displayPortSeverity_e
#define DISPLAYPORT_MSP_ATTR_BLINK 6 // Device local blink
#define DISPLAYPORT_MSP_ATTR_VERSION 7 // Format indicator; must be zero for V2 (and V1)
#define DISPLAYPORT_MSP_ATTR_FONTPAGE_MASK 0x3
#define DISPLAYPORT_MSP_ATTR_BLINK_MASK (1 << DISPLAYPORT_MSP_ATTR_BLINK)
#define DISPLAYPORT_MSP_ATTR_VERSION_MASK (1 << DISPLAYPORT_MSP_ATTR_VERSION)
typedef struct displayPort_s displayPort_t;
displayPort_t *mspOsdDisplayPortInit(const videoSystem_e videoSystem);
void mspOsdSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn);
mspPort_t *getMspOsdPort(void);
// MSP displayport V2 attribute byte bit functions
#define DISPLAYPORT_MSP_ATTR_FONTPAGE 0 // Select bank of 256 characters as per displayPortSeverity_e
#define DISPLAYPORT_MSP_ATTR_BLINK 6 // Device local blink
#define DISPLAYPORT_MSP_ATTR_VERSION 7 // Format indicator; must be zero for V2 (and V1)
#define getAttrPage(attr) (attr & DISPLAYPORT_MSP_ATTR_FONTPAGE_MASK)
#define getAttrBlink(attr) ((attr & DISPLAYPORT_MSP_ATTR_BLINK_MASK) >> DISPLAYPORT_MSP_ATTR_BLINK)
#define getAttrVersion(attr) ((attr & DISPLAYPORT_MSP_ATTR_VERSION_MASK) >> DISPLAYPORT_MSP_ATTR_VERSION)
uint8_t setAttrPage(uint8_t origAttr, uint8_t page);
uint8_t setAttrBlink(uint8_t origAttr, uint8_t page);
uint8_t setAttrVersion(uint8_t origAttr, uint8_t page);

161
src/main/io/dji_osd_symbols.h Normal file
View file

@ -0,0 +1,161 @@
/* @file max7456_symbols.h
* @brief max7456 symbols for the mwosd font set
*
* @author Nathan Tsoi nathan@vertile.com
*
* Copyright (C) 2016 Nathan Tsoi
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>
*/
#pragma once
//Misc
#define DJI_SYM_NONE 0x00
#define DJI_SYM_END_OF_FONT 0xFF
#define DJI_SYM_BLANK 0x20
#define DJI_SYM_HYPHEN 0x2D
#define DJI_SYM_BBLOG 0x10
#define DJI_SYM_HOMEFLAG 0x11
#define DJI_SYM_RPM 0x12
#define DJI_SYM_ROLL 0x14
#define DJI_SYM_PITCH 0x15
#define DJI_SYM_TEMPERATURE 0x7A
#define DJI_SYM_MAX 0x24
// GPS and navigation
#define DJI_SYM_LAT 0x89
#define DJI_SYM_LON 0x98
#define DJI_SYM_ALTITUDE 0x7F
#define DJI_SYM_OVER_HOME 0x05
// RSSI
#define DJI_SYM_RSSI 0x01
// Throttle Position (%)
#define DJI_SYM_THR 0x04
// Unit Icons (Metric)
#define DJI_SYM_M 0x0C
#define DJI_SYM_C 0x0E
// Unit Icons (Imperial)
#define DJI_SYM_F 0x0D
#define DJI_SYM_FT 0x0F
// Heading Graphics
#define DJI_SYM_HEADING_N 0x18
#define DJI_SYM_HEADING_S 0x19
#define DJI_SYM_HEADING_E 0x1A
#define DJI_SYM_HEADING_W 0x1B
#define DJI_SYM_HEADING_DIVIDED_LINE 0x1C
#define DJI_SYM_HEADING_LINE 0x1D
// AH Center screen Graphics
#define DJI_SYM_CROSSHAIR_LEFT 0x72
#define DJI_SYM_CROSSHAIR_CENTRE 0x73
#define DJI_SYM_CROSSHAIR_RIGHT 0x74
#define DJI_SYM_AH_RIGHT 0x02
#define DJI_SYM_AH_LEFT 0x03
#define DJI_SYM_AH_DECORATION 0x13
#define DJI_SYM_SMALL_CROSSHAIR 0x7E
// Satellite Graphics
#define DJI_SYM_SAT_L 0x1E
#define DJI_SYM_SAT_R 0x1F
// Direction arrows
#define DJI_SYM_ARROW_SOUTH 0x60
#define DJI_SYM_ARROW_2 0x61
#define DJI_SYM_ARROW_3 0x62
#define DJI_SYM_ARROW_4 0x63
#define DJI_SYM_ARROW_EAST 0x64
#define DJI_SYM_ARROW_6 0x65
#define DJI_SYM_ARROW_7 0x66
#define DJI_SYM_ARROW_8 0x67
#define DJI_SYM_ARROW_NORTH 0x68
#define DJI_SYM_ARROW_10 0x69
#define DJI_SYM_ARROW_11 0x6A
#define DJI_SYM_ARROW_12 0x6B
#define DJI_SYM_ARROW_WEST 0x6C
#define DJI_SYM_ARROW_14 0x6D
#define DJI_SYM_ARROW_15 0x6E
#define DJI_SYM_ARROW_16 0x6F
#define DJI_SYM_ARROW_SMALL_UP 0x75
#define DJI_SYM_ARROW_SMALL_DOWN 0x76
#define DJI_SYM_ARROW_SMALL_RIGHT 0x77
#define DJI_SYM_ARROW_SMALL_LEFT 0x78
// AH Bars
#define DJI_SYM_AH_BAR9_0 0x80
#define DJI_SYM_AH_BAR9_1 0x81
#define DJI_SYM_AH_BAR9_2 0x82
#define DJI_SYM_AH_BAR9_3 0x83
#define DJI_SYM_AH_BAR9_4 0x84
#define DJI_SYM_AH_BAR9_5 0x85
#define DJI_SYM_AH_BAR9_6 0x86
#define DJI_SYM_AH_BAR9_7 0x87
#define DJI_SYM_AH_BAR9_8 0x88
// Progress bar
#define DJI_SYM_PB_START 0x8A
#define DJI_SYM_PB_FULL 0x8B
#define DJI_SYM_PB_HALF 0x8C
#define DJI_SYM_PB_EMPTY 0x8D
#define DJI_SYM_PB_END 0x8E
#define DJI_SYM_PB_CLOSE 0x8F
// Batt evolution
#define DJI_SYM_BATT_FULL 0x90
#define DJI_SYM_BATT_5 0x91
#define DJI_SYM_BATT_4 0x92
#define DJI_SYM_BATT_3 0x93
#define DJI_SYM_BATT_2 0x94
#define DJI_SYM_BATT_1 0x95
#define DJI_SYM_BATT_EMPTY 0x96
// Batt Icons
#define DJI_SYM_MAIN_BATT 0x97
// Voltage and amperage
#define DJI_SYM_VOLT 0x06
#define DJI_SYM_AMP 0x9A
#define DJI_SYM_MAH 0x07
#define DJI_SYM_WATT 0x57 // 0x57 is 'W'
// Time
#define DJI_SYM_ON_H 0x70
#define DJI_SYM_FLY_H 0x71
#define DJI_SYM_ON_M 0x9B
#define DJI_SYM_FLY_M 0x9C
// Speed
#define DJI_SYM_KPH 0x9E
#define DJI_SYM_MPH 0x9D
#define DJI_SYM_MPS 0x9F
#define DJI_SYM_FTPS 0x99
// Stick overlays
#define DJI_SYM_STICK_OVERLAY_SPRITE_HIGH 0x08
#define DJI_SYM_STICK_OVERLAY_SPRITE_MID 0x09
#define DJI_SYM_STICK_OVERLAY_SPRITE_LOW 0x0A
#define DJI_SYM_STICK_OVERLAY_CENTER 0x0B
#define DJI_SYM_STICK_OVERLAY_VERTICAL 0x16
#define DJI_SYM_STICK_OVERLAY_HORIZONTAL 0x17
// GPS degree/minute/second symbols
#define DJI_SYM_GPS_DEGREE DJI_SYM_STICK_OVERLAY_SPRITE_HIGH // kind of looks like the degree symbol
#define DJI_SYM_GPS_MINUTE 0x27 // '
#define DJI_SYM_GPS_SECOND 0x22 // "

7
src/main/io/gps_ublox.c
View file

@ -1048,6 +1048,8 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread)
gpsState.hwVersion = UBX_HW_VERSION_UNKNOWN;
gpsState.autoConfigStep = 0;
// Configure GPS module if enabled
if (gpsState.gpsConfig->autoConfig) {
do {
pollVersion();
gpsState.autoConfigStep++;
@ -1065,8 +1067,7 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread)
ptWaitTimeout((ubx_capabilities.capMaxGnss != 0), GPS_CFG_CMD_TIMEOUT_MS);
} while (gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && ubx_capabilities.capMaxGnss == 0);
}
// Configure GPS module if enabled
if (gpsState.gpsConfig->autoConfig) {
// Configure GPS
ptSpawn(gpsConfigure);
}
@ -1079,7 +1080,7 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread)
ptSemaphoreWait(semNewDataReady);
gpsProcessNewSolutionData(false);
if ((gpsState.gpsConfig->provider == GPS_UBLOX || gpsState.gpsConfig->provider == GPS_UBLOX7PLUS)) {
if ((gpsState.gpsConfig->autoConfig) && (gpsState.gpsConfig->provider == GPS_UBLOX || gpsState.gpsConfig->provider == GPS_UBLOX7PLUS)) {
if ((millis() - gpsState.lastCapaPoolMs) > GPS_CAPA_INTERVAL) {
gpsState.lastCapaPoolMs = millis();

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

@ -70,7 +70,7 @@
#include "io/osd_common.h"
#include "io/osd_hud.h"
#include "io/osd_utils.h"
#include "io/displayport_msp_bf_compat.h"
#include "io/displayport_msp_dji_compat.h"
#include "io/vtx.h"
#include "io/vtx_string.h"
@ -223,7 +223,7 @@ static bool osdDisplayHasCanvas;
#define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees)
PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 10);
PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 11);
PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 1);
void osdStartedSaveProcess(void) {
@ -253,51 +253,6 @@ bool osdIsNotMetric(void) {
return !(osdConfig()->units == OSD_UNIT_METRIC || osdConfig()->units == OSD_UNIT_METRIC_MPH);
}
/*
* Aligns text to the left side. Adds spaces at the end to keep string length unchanged.
*/
/* -- Currently unused --
static void osdLeftAlignString(char *buff)
{
uint8_t sp = 0, ch = 0;
uint8_t len = strlen(buff);
while (buff[sp] == ' ') sp++;
for (ch = 0; ch < (len - sp); ch++) buff[ch] = buff[ch + sp];
for (sp = ch; sp < len; sp++) buff[sp] = ' ';
}*/
/*
* This is a simplified distance conversion code that does not use any scaling
* but is fully compatible with the DJI G2 MSP Displayport OSD implementation.
* (Based on osdSimpleAltitudeSymbol() implementation)
*/
/* void osdSimpleDistanceSymbol(char *buff, int32_t dist) {
int32_t convertedDistance;
char suffix;
switch ((osd_unit_e)osdConfig()->units) {
case OSD_UNIT_UK:
FALLTHROUGH;
case OSD_UNIT_GA:
FALLTHROUGH;
case OSD_UNIT_IMPERIAL:
convertedDistance = CENTIMETERS_TO_FEET(dist);
suffix = SYM_ALT_FT;
break;
case OSD_UNIT_METRIC_MPH:
FALLTHROUGH;
case OSD_UNIT_METRIC:
convertedDistance = CENTIMETERS_TO_METERS(dist);
suffix = SYM_ALT_M; // Intentionally use the altitude symbol, as the distance symbol is not defined in BFCOMPAT mode
break;
}
tfp_sprintf(buff, "%5d", (int) convertedDistance); // 5 digits, allowing up to 99999 meters/feet, which should be plenty for 99.9% of use cases
buff[5] = suffix;
buff[6] = '\0';
} */
/**
* Converts distance into a string based on the current unit system
* prefixed by a a symbol to indicate the unit used.
@ -313,12 +268,12 @@ static void osdFormatDistanceSymbol(char *buff, int32_t dist, uint8_t decimals)
uint8_t symbol_mi = SYM_DIST_MI;
uint8_t symbol_nm = SYM_DIST_NM;
#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values
if (isBfCompatibleVideoSystem(osdConfig())) {
#ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it and change the values
if (isDJICompatibleVideoSystem(osdConfig())) {
// Add one digit so up no switch to scaled decimal occurs above 99
digits = 4U;
sym_index = 4U;
// Use altitude symbols on purpose, as it seems distance symbols are not defined in BFCOMPAT mode
// Use altitude symbols on purpose, as it seems distance symbols are not defined in DJICOMPAT mode
symbol_m = SYM_ALT_M;
symbol_km = SYM_ALT_KM;
symbol_ft = SYM_ALT_FT;
@ -570,8 +525,8 @@ void osdFormatAltitudeSymbol(char *buff, int32_t alt)
buff[0] = ' ';
}
#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values
if (isBfCompatibleVideoSystem(osdConfig())) {
#ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it and change the values
if (isDJICompatibleVideoSystem(osdConfig())) {
totalDigits++;
digits++;
symbolIndex++;
@ -792,21 +747,21 @@ static void osdFormatCoordinate(char *buff, char sym, int32_t val)
int32_t decimalPart = abs(val % (int)GPS_DEGREES_DIVIDER);
STATIC_ASSERT(GPS_DEGREES_DIVIDER == 1e7, adjust_max_decimal_digits);
int decimalDigits;
bool bfcompat = false; // Assume BFCOMPAT mode is no enabled
bool djiCompat = false; // Assume DJICOMPAT mode is no enabled
#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it
if(isBfCompatibleVideoSystem(osdConfig())) {
bfcompat = true;
#ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it
if(isDJICompatibleVideoSystem(osdConfig())) {
djiCompat = true;
}
#endif
if (!bfcompat) {
if (!djiCompat) {
decimalDigits = tfp_sprintf(buff + 1 + integerDigits, "%07d", (int)decimalPart);
// Embbed the decimal separator
buff[1 + integerDigits - 1] += SYM_ZERO_HALF_TRAILING_DOT - '0';
buff[1 + integerDigits] += SYM_ZERO_HALF_LEADING_DOT - '0';
} else {
// BFCOMPAT mode enabled
// DJICOMPAT mode enabled
decimalDigits = tfp_sprintf(buff + 1 + integerDigits, ".%06d", (int)decimalPart);
}
// Fill up to coordinateLength with zeros
@ -1752,8 +1707,8 @@ static bool osdDrawSingleElement(uint8_t item)
case OSD_MAIN_BATT_VOLTAGE: {
uint8_t base_digits = 2U;
#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it
if(isBfCompatibleVideoSystem(osdConfig())) {
#ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it
if(isDJICompatibleVideoSystem(osdConfig())) {
base_digits = 3U; // Add extra digit to account for decimal point taking an extra character space
}
#endif
@ -1763,8 +1718,8 @@ static bool osdDrawSingleElement(uint8_t item)
case OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE: {
uint8_t base_digits = 2U;
#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it
if(isBfCompatibleVideoSystem(osdConfig())) {
#ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it
if(isDJICompatibleVideoSystem(osdConfig())) {
base_digits = 3U; // Add extra digit to account for decimal point taking an extra character space
}
#endif
@ -1787,9 +1742,9 @@ static bool osdDrawSingleElement(uint8_t item)
case OSD_MAH_DRAWN: {
uint8_t mah_digits = osdConfig()->mAh_precision; // Initialize to config value
#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it
if (isBfCompatibleVideoSystem(osdConfig())) {
//BFcompat is unable to work with scaled values and it only has mAh symbol to work with
#ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it
if (isDJICompatibleVideoSystem(osdConfig())) {
//DJIcompat is unable to work with scaled values and it only has mAh symbol to work with
tfp_sprintf(buff, "%5d", (int)getMAhDrawn()); // Use 5 digits to allow packs below 100Ah
buff[5] = SYM_MAH;
buff[6] = '\0';
@ -1825,12 +1780,12 @@ static bool osdDrawSingleElement(uint8_t item)
tfp_sprintf(buff, " NA");
else if (!batteryWasFullWhenPluggedIn())
tfp_sprintf(buff, " NF");
else if (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH) {
else if (batteryMetersConfig()->capacity_unit == BAT_CAPACITY_UNIT_MAH) {
uint8_t mah_digits = osdConfig()->mAh_precision; // Initialize to config value
#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it
if (isBfCompatibleVideoSystem(osdConfig())) {
//BFcompat is unable to work with scaled values and it only has mAh symbol to work with
#ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it
if (isDJICompatibleVideoSystem(osdConfig())) {
//DJIcompat is unable to work with scaled values and it only has mAh symbol to work with
tfp_sprintf(buff, "%5d", (int)getBatteryRemainingCapacity()); // Use 5 digits to allow packs below 100Ah
buff[5] = SYM_MAH;
buff[6] = '\0';
@ -1848,11 +1803,11 @@ static bool osdDrawSingleElement(uint8_t item)
buff[mah_digits + 1] = '\0';
unitsDrawn = true;
}
} else // currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH
} else // batteryMetersConfig()->capacityUnit == BAT_CAPACITY_UNIT_MWH
osdFormatCentiNumber(buff + 1, getBatteryRemainingCapacity() / 10, 0, 2, 0, 3, false);
if (!unitsDrawn) {
buff[4] = currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH ? SYM_MAH : SYM_WH;
buff[4] = batteryMetersConfig()->capacity_unit == BAT_CAPACITY_UNIT_MAH ? SYM_MAH : SYM_WH;
buff[5] = '\0';
}
@ -2139,8 +2094,8 @@ static bool osdDrawSingleElement(uint8_t item)
buff[1] = SYM_HDP_R;
int32_t centiHDOP = 100 * gpsSol.hdop / HDOP_SCALE;
uint8_t digits = 2U;
#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values
if (isBfCompatibleVideoSystem(osdConfig())) {
#ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it and change the values
if (isDJICompatibleVideoSystem(osdConfig())) {
digits = 3U;
}
#endif
@ -2799,8 +2754,8 @@ static bool osdDrawSingleElement(uint8_t item)
// time will be longer than 99 minutes. If it is, it will show 99:^^
if (glideTime > (99 * 60) + 59) {
tfp_sprintf(buff + 1, "%02d:", (int)(glideTime / 60));
buff[4] = SYM_DIRECTION;
buff[5] = SYM_DIRECTION;
buff[4] = SYM_DECORATION;
buff[5] = SYM_DECORATION;
} else {
tfp_sprintf(buff + 1, "%02d:%02d", (int)(glideTime / 60), (int)(glideTime % 60));
}
@ -3147,8 +3102,8 @@ static bool osdDrawSingleElement(uint8_t item)
case OSD_MAIN_BATT_CELL_VOLTAGE:
{
uint8_t base_digits = 3U;
#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it
if(isBfCompatibleVideoSystem(osdConfig())) {
#ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it
if(isDJICompatibleVideoSystem(osdConfig())) {
base_digits = 4U; // Add extra digit to account for decimal point taking an extra character space
}
#endif
@ -3159,8 +3114,8 @@ static bool osdDrawSingleElement(uint8_t item)
case OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE:
{
uint8_t base_digits = 3U;
#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it
if(isBfCompatibleVideoSystem(osdConfig())) {
#ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it
if(isDJICompatibleVideoSystem(osdConfig())) {
base_digits = 4U; // Add extra digit to account for decimal point taking an extra character space
}
#endif
@ -3215,8 +3170,8 @@ static bool osdDrawSingleElement(uint8_t item)
timeUs_t currentTimeUs = micros();
timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated);
uint8_t digits = 3U;
#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values
if (isBfCompatibleVideoSystem(osdConfig())) {
#ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it and change the values
if (isDJICompatibleVideoSystem(osdConfig())) {
// Increase number of digits so values above 99 don't get scaled by osdFormatCentiNumber
digits = 4U;
}
@ -3248,7 +3203,7 @@ static bool osdDrawSingleElement(uint8_t item)
}
if (!efficiencyValid) {
buff[0] = buff[1] = buff[2] = buff[3] = '-';
buff[digits] = SYM_MAH_MI_0; // This will overwrite the "-" at buff[3] if not in BFCOMPAT mode
buff[digits] = SYM_MAH_MI_0; // This will overwrite the "-" at buff[3] if not in DJICOMPAT mode
buff[digits + 1] = SYM_MAH_MI_1;
buff[digits + 2] = '\0';
}
@ -3418,7 +3373,7 @@ static bool osdDrawSingleElement(uint8_t item)
horizontalWindSpeed = getEstimatedHorizontalWindSpeed(&angle);
int16_t windDirection = osdGetHeadingAngle( CENTIDEGREES_TO_DEGREES((int)angle) - DECIDEGREES_TO_DEGREES(attitude.values.yaw) + 22);
buff[0] = SYM_WIND_HORIZONTAL;
buff[1] = SYM_DIRECTION + (windDirection*2 / 90);
buff[1] = SYM_DECORATION + (windDirection*2 / 90);
osdFormatWindSpeedStr(buff + 2, horizontalWindSpeed, valid);
break;
}
@ -3435,10 +3390,10 @@ static bool osdDrawSingleElement(uint8_t item)
float verticalWindSpeed;
verticalWindSpeed = -getEstimatedWindSpeed(Z); //from NED to NEU
if (verticalWindSpeed < 0) {
buff[1] = SYM_AH_DIRECTION_DOWN;
buff[1] = SYM_AH_DECORATION_DOWN;
verticalWindSpeed = -verticalWindSpeed;
} else {
buff[1] = SYM_AH_DIRECTION_UP;
buff[1] = SYM_AH_DECORATION_UP;
}
osdFormatWindSpeedStr(buff + 2, verticalWindSpeed, valid);
break;
@ -3552,7 +3507,7 @@ static bool osdDrawSingleElement(uint8_t item)
} else {
referenceSymbol = '-';
}
displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_DIRECTION);
displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_DECORATION);
displayWriteChar(osdDisplayPort, elemPosX, elemPosY + 1, referenceSymbol);
return true;
}
@ -3919,6 +3874,9 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig,
.airspeed_alarm_min = SETTING_OSD_AIRSPEED_ALARM_MIN_DEFAULT,
.airspeed_alarm_max = SETTING_OSD_AIRSPEED_ALARM_MAX_DEFAULT,
#endif
#ifndef DISABLE_MSP_DJI_COMPAT
.highlight_djis_missing_characters = SETTING_OSD_HIGHLIGHT_DJIS_MISSING_FONT_SYMBOLS_DEFAULT,
#endif
.video_system = SETTING_OSD_VIDEO_SYSTEM_DEFAULT,
.row_shiftdown = SETTING_OSD_ROW_SHIFTDOWN_DEFAULT,
@ -4185,10 +4143,13 @@ uint8_t drawLogos(bool singular, uint8_t row) {
uint8_t logoRow = row;
uint8_t logoColOffset = 0;
bool usePilotLogo = (osdConfig()->use_pilot_logo && osdDisplayIsHD());
bool useINAVLogo = (singular && !usePilotLogo) || !singular;
#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is in use, the pilot logo cannot be used, due to font issues.
if (isBfCompatibleVideoSystem(osdConfig()))
#ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is in use, the pilot logo cannot be used, due to font issues.
if (isDJICompatibleVideoSystem(osdConfig())) {
usePilotLogo = false;
useINAVLogo = false;
}
#endif
uint8_t logoSpacing = osdConfig()->inav_to_pilot_logo_spacing;
@ -4205,7 +4166,7 @@ uint8_t drawLogos(bool singular, uint8_t row) {
}
// Draw INAV logo
if ((singular && !usePilotLogo) || !singular) {
if (useINAVLogo) {
unsigned logo_c = SYM_LOGO_START;
uint8_t logo_x = logoColOffset;
for (uint8_t lRow = 0; lRow < SYM_LOGO_HEIGHT; lRow++) {
@ -4235,6 +4196,10 @@ uint8_t drawLogos(bool singular, uint8_t row) {
}
}
if (!usePilotLogo && !useINAVLogo) {
logoRow += SYM_LOGO_HEIGHT;
}
return logoRow;
}
@ -4727,8 +4692,8 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b
tfp_sprintf(outBuff, ": ");
uint8_t digits = 3U; // Total number of digits (including decimal point)
#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values
if (isBfCompatibleVideoSystem(osdConfig())) {
#ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it and change the values
if (isDJICompatibleVideoSystem(osdConfig())) {
// Add one digit so no switch to scaled decimal occurs above 99
digits = 4U;
}
@ -5404,6 +5369,7 @@ static void osdRefresh(timeUs_t currentTimeUs)
static uint8_t statsCurrentPage = 0;
static bool statsDisplayed = false;
static bool statsAutoPagingEnabled = true;
static bool isThrottleHigh = false;
// Detect arm/disarm
if (armState != ARMING_FLAG(ARMED)) {
@ -5429,6 +5395,7 @@ static void osdRefresh(timeUs_t currentTimeUs)
statsAutoPagingEnabled = osdConfig()->stats_page_auto_swap_time > 0 ? true : false;
osdShowStats(statsSinglePageCompatible, statsCurrentPage);
osdSetNextRefreshIn(STATS_SCREEN_DISPLAY_TIME);
isThrottleHigh = checkStickPosition(THR_HI);
}
armState = ARMING_FLAG(ARMED);
@ -5494,7 +5461,7 @@ static void osdRefresh(timeUs_t currentTimeUs)
}
// Handle events when either "Splash", "Armed" or "Stats" screens are displayed.
if ((currentTimeUs > resumeRefreshAt) || OSD_RESUME_UPDATES_STICK_COMMAND) {
if (currentTimeUs > resumeRefreshAt || (OSD_RESUME_UPDATES_STICK_COMMAND && !isThrottleHigh)) {
// Time elapsed or canceled by stick commands.
// Exit to normal OSD operation.
displayClearScreen(osdDisplayPort);
@ -5503,6 +5470,7 @@ static void osdRefresh(timeUs_t currentTimeUs)
} else {
// Continue "Splash", "Armed" or "Stats" screens.
displayHeartbeat(osdDisplayPort);
isThrottleHigh = checkStickPosition(THR_HI);
}
return;
@ -5721,7 +5689,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
else {
#ifdef USE_FW_AUTOLAND
if (canFwLandCanceld()) {
if (canFwLandingBeCancelled()) {
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_MOVE_STICKS);
} else if (!FLIGHT_MODE(NAV_FW_AUTOLAND)) {
#endif

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

@ -457,6 +457,9 @@ typedef struct osdConfig_s {
bool use_pilot_logo; // If enabled, the pilot logo (last 40 characters of page 2 font) will be used with the INAV logo.
uint8_t inav_to_pilot_logo_spacing; // The space between the INAV and pilot logos, if pilot logo is used. This number may be adjusted so that it fits the odd/even col width.
uint16_t arm_screen_display_time; // Length of time the arm screen is displayed
#ifndef DISABLE_MSP_DJI_COMPAT
bool highlight_djis_missing_characters; // If enabled, show question marks where there is no character in DJI's font to represent an OSD element symbol
#endif
#ifdef USE_ADSB
uint16_t adsb_distance_warning; // in metres
uint16_t adsb_distance_alert; // in metres

8
src/main/io/osd_grid.c
View file

@ -315,16 +315,16 @@ void osdGridDrawSidebars(displayPort_t *display)
// Arrows
if (osdConfig()->sidebar_scroll_arrows) {
displayWriteChar(display, elemPosX - hudwidth, elemPosY - hudheight - 1,
left.arrow == OSD_SIDEBAR_ARROW_UP ? SYM_AH_DIRECTION_UP : SYM_BLANK);
left.arrow == OSD_SIDEBAR_ARROW_UP ? SYM_AH_DECORATION_UP : SYM_BLANK);
displayWriteChar(display, elemPosX + hudwidth, elemPosY - hudheight - 1,
right.arrow == OSD_SIDEBAR_ARROW_UP ? SYM_AH_DIRECTION_UP : SYM_BLANK);
right.arrow == OSD_SIDEBAR_ARROW_UP ? SYM_AH_DECORATION_UP : SYM_BLANK);
displayWriteChar(display, elemPosX - hudwidth, elemPosY + hudheight + 1,
left.arrow == OSD_SIDEBAR_ARROW_DOWN ? SYM_AH_DIRECTION_DOWN : SYM_BLANK);
left.arrow == OSD_SIDEBAR_ARROW_DOWN ? SYM_AH_DECORATION_DOWN : SYM_BLANK);
displayWriteChar(display, elemPosX + hudwidth, elemPosY + hudheight + 1,
right.arrow == OSD_SIDEBAR_ARROW_DOWN ? SYM_AH_DIRECTION_DOWN : SYM_BLANK);
right.arrow == OSD_SIDEBAR_ARROW_DOWN ? SYM_AH_DECORATION_DOWN : SYM_BLANK);
}
// Draw AH sides

4
src/main/io/osd_hud.c
View file

@ -207,7 +207,7 @@ void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitu
if (poiType == 1) { // POI from the ESP radar
error_x = hudWrap360(poiP1 - DECIDEGREES_TO_DEGREES(osdGetHeading()));
osdHudWrite(poi_x - 1, poi_y, SYM_DIRECTION + ((error_x + 22) / 45) % 8, 1);
osdHudWrite(poi_x - 1, poi_y, SYM_DECORATION + ((error_x + 22) / 45) % 8, 1);
osdHudWrite(poi_x + 1, poi_y, SYM_HUD_SIGNAL_0 + poiP2, 1);
}
else if (poiType == 2) { // Waypoint,
@ -248,7 +248,7 @@ void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitu
tfp_sprintf(buff, "%3d", altc);
}
buff[0] = (poiAltitude >= 0) ? SYM_AH_DIRECTION_UP : SYM_AH_DIRECTION_DOWN;
buff[0] = (poiAltitude >= 0) ? SYM_AH_DECORATION_UP : SYM_AH_DECORATION_DOWN;
} else { // Display the distance by default
switch ((osd_unit_e)osdConfig()->units) {
case OSD_UNIT_UK:

4
src/main/io/osd_utils.c
View file

@ -20,7 +20,7 @@
#include "common/maths.h"
#include "common/typeconversion.h"
#include "drivers/osd_symbols.h"
#include "io/displayport_msp_bf_compat.h"
#include "io/displayport_msp_dji_compat.h"
#if defined(USE_OSD) || defined(OSD_UNIT_TEST)
@ -45,7 +45,7 @@ bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int ma
int decimals = maxDecimals;
bool negative = false;
bool scaled = false;
bool explicitDecimal = isBfCompatibleVideoSystem(osdConfig());
bool explicitDecimal = isDJICompatibleVideoSystem(osdConfig());
buff[length] = '\0';

2
src/main/io/rangefinder.h
View file

@ -31,6 +31,8 @@
extern virtualRangefinderVTable_t rangefinderMSPVtable;
extern virtualRangefinderVTable_t rangefinderBenewakeVtable;
extern virtualRangefinderVTable_t rangefinderUSD1Vtable;
extern virtualRangefinderVTable_t rangefinderNanoradarVtable; //NRA15/NRA24
extern virtualRangefinderVTable_t rangefinderFakeVtable;
void mspRangefinderReceiveNewData(uint8_t * bufferPtr);

167
src/main/io/rangefinder_nanoradar.c Normal file
View file

@ -0,0 +1,167 @@
/*
* This file is part of INAV Project.
*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
* You can obtain one at http://mozilla.org/MPL/2.0/.
*
* Alternatively, the contents of this file may be used under the terms
* of the GNU General Public License Version 3, as described below:
*
* This file is free software: you may copy, redistribute and/or modify
* it under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or (at your
* option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
* Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see http://www.gnu.org/licenses/.
*/
#include <stdbool.h>
#include <ctype.h>
#include "platform.h"
#include "io/serial.h"
#include "drivers/time.h"
#if defined(USE_RANGEFINDER_NANORADAR)
#include "drivers/rangefinder/rangefinder_virtual.h"
#define NANORADAR_HDR 0xAA // Header Byte
#define NANORADAR_END 0x55
#define NANORADAR_COMMAND_TARGET_INFO 0x70C
typedef struct __attribute__((packed)) {
uint8_t header0;
uint8_t header1;
uint8_t commandH;
uint8_t commandL;
uint8_t index; // Target ID
uint8_t rcs; // The section of radar reflection
uint8_t rangeH; // Target distance high 8 bi
uint8_t rangeL; // Target distance low 8 bit
uint8_t rsvd1;
uint8_t info; // VrelH Rsvd1 RollCount
uint8_t vrelL;
uint8_t SNR; // Signal-Noise Ratio
uint8_t end0;
uint8_t end1;
} nanoradarPacket_t;
#define NANORADAR_PACKET_SIZE sizeof(nanoradarPacket_t)
#define NANORADAR_TIMEOUT_MS 2000 // 2s
static bool hasNewData = false;
static bool hasEverData = false;
static serialPort_t * serialPort = NULL;
static serialPortConfig_t * portConfig;
static uint8_t buffer[NANORADAR_PACKET_SIZE];
static unsigned bufferPtr;
static int32_t sensorData = RANGEFINDER_NO_NEW_DATA;
static timeMs_t lastProtocolActivityMs;
static bool nanoradarRangefinderDetect(void)
{
portConfig = findSerialPortConfig(FUNCTION_RANGEFINDER);
if (!portConfig) {
return false;
}
return true;
}
static void nanoradarRangefinderInit(void)
{
if (!portConfig) {
return;
}
serialPort = openSerialPort(portConfig->identifier, FUNCTION_RANGEFINDER, NULL, NULL, 115200, MODE_RXTX, SERIAL_NOT_INVERTED);
if (!serialPort) {
return;
}
lastProtocolActivityMs = 0;
}
static void nanoradarRangefinderUpdate(void)
{
nanoradarPacket_t *nanoradarPacket = (nanoradarPacket_t *)buffer;
while (serialRxBytesWaiting(serialPort) > 0) {
uint8_t c = serialRead(serialPort);
if (bufferPtr < NANORADAR_PACKET_SIZE) {
buffer[bufferPtr++] = c;
}
if ((bufferPtr == 1) && (nanoradarPacket->header0 != NANORADAR_HDR)) {
bufferPtr = 0;
continue;
}
if ((bufferPtr == 2) && (nanoradarPacket->header1 != NANORADAR_HDR)) {
bufferPtr = 0;
continue;
}
//only target info packet we are interested
if (bufferPtr == 4) {
uint16_t command = nanoradarPacket->commandH + (nanoradarPacket->commandL * 0x100);
if (command != NANORADAR_COMMAND_TARGET_INFO) {
bufferPtr = 0;
continue;
}
}
// Check for complete packet
if (bufferPtr == NANORADAR_PACKET_SIZE) {
if (nanoradarPacket->end0 == NANORADAR_END && nanoradarPacket->end1 == NANORADAR_END) {
// Valid packet
hasNewData = true;
hasEverData = true;
lastProtocolActivityMs = millis();
sensorData = ((nanoradarPacket->rangeH * 0x100) + nanoradarPacket->rangeL);
}
// Prepare for new packet
bufferPtr = 0;
}
}
}
static int32_t nanoradarRangefinderGetDistance(void)
{
int32_t altitude = (sensorData > 0) ? (sensorData) : RANGEFINDER_OUT_OF_RANGE;
if (hasNewData) {
hasNewData = false;
return altitude;
}
else {
//keep last value for timeout, because radar sends data only if change
if ((millis() - lastProtocolActivityMs) < NANORADAR_TIMEOUT_MS) {
return altitude;
}
return hasEverData ? RANGEFINDER_OUT_OF_RANGE : RANGEFINDER_NO_NEW_DATA;
}
}
virtualRangefinderVTable_t rangefinderNanoradarVtable = {
.detect = nanoradarRangefinderDetect,
.init = nanoradarRangefinderInit,
.update = nanoradarRangefinderUpdate,
.read = nanoradarRangefinderGetDistance
};
#endif

136
src/main/io/rangefinder_usd1_v0.c Normal file
View file

@ -0,0 +1,136 @@
/*
* This file is part of INAV Project.
*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
* You can obtain one at http://mozilla.org/MPL/2.0/.
*
* Alternatively, the contents of this file may be used under the terms
* of the GNU General Public License Version 3, as described below:
*
* This file is free software: you may copy, redistribute and/or modify
* it under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or (at your
* option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
* Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see http://www.gnu.org/licenses/.
*/
#include <stdbool.h>
#include <ctype.h>
#include "platform.h"
#include "io/serial.h"
#include "drivers/time.h"
#if defined(USE_RANGEFINDER_USD1_V0)
#include "drivers/rangefinder/rangefinder_virtual.h"
#define USD1_HDR_V0 72 // Header Byte for beta V0 of USD1_Serial (0x48)
#define USD1_PACKET_SIZE 3
#define USD1_KEEP_DATA_TIMEOUT 2000 // 2s
static serialPort_t * serialPort = NULL;
static serialPortConfig_t * portConfig;
static bool hasNewData = false;
static bool hasEverData = false;
static uint8_t lineBuf[USD1_PACKET_SIZE];
static int32_t sensorData = RANGEFINDER_NO_NEW_DATA;
static timeMs_t lastProtocolActivityMs;
static bool usd1RangefinderDetect(void)
{
portConfig = findSerialPortConfig(FUNCTION_RANGEFINDER);
if (!portConfig) {
return false;
}
return true;
}
static void usd1RangefinderInit(void)
{
if (!portConfig) {
return;
}
serialPort = openSerialPort(portConfig->identifier, FUNCTION_RANGEFINDER, NULL, NULL, 115200, MODE_RXTX, SERIAL_NOT_INVERTED);
if (!serialPort) {
return;
}
lastProtocolActivityMs = 0;
}
static void usd1RangefinderUpdate(void)
{
float sum = 0;
uint16_t count = 0;
uint8_t index = 0;
while (serialRxBytesWaiting(serialPort) > 0) {
uint8_t c = serialRead(serialPort);
if (c == USD1_HDR_V0 && index == 0) {
lineBuf[index] = c;
index = 1;
continue;
}
if (index > 0) {
lineBuf[index] = c;
index++;
if (index == 3) {
index = 0;
sum += (float)((lineBuf[2]&0x7F) * 128 + (lineBuf[1]&0x7F));
count++;
}
}
}
if (count == 0) {
return;
}
hasNewData = true;
hasEverData = true;
lastProtocolActivityMs = millis();
sensorData = (int32_t)(2.5f * sum / (float)count);
}
static int32_t usd1RangefinderGetDistance(void)
{
int32_t altitude = (sensorData > 0) ? (sensorData) : RANGEFINDER_OUT_OF_RANGE;
if (hasNewData) {
hasNewData = false;
return altitude;
}
else {
//keep last value for timeout, because radar sends data only if change
if ((millis() - lastProtocolActivityMs) < USD1_KEEP_DATA_TIMEOUT) {
return altitude;
}
return hasEverData ? RANGEFINDER_OUT_OF_RANGE : RANGEFINDER_NO_NEW_DATA;
}
}
virtualRangefinderVTable_t rangefinderUSD1Vtable = {
.detect = usd1RangefinderDetect,
.init = usd1RangefinderInit,
.update = usd1RangefinderUpdate,
.read = usd1RangefinderGetDistance
};
#endif

3
src/main/msp/msp_protocol_v2_inav.h
View file

@ -31,9 +31,10 @@
#define MSP2_INAV_OUTPUT_MAPPING 0x200A
#define MSP2_INAV_MC_BRAKING 0x200B
#define MSP2_INAV_SET_MC_BRAKING 0x200C
#define MSP2_INAV_OUTPUT_MAPPING_EXT 0x200D
#define MSP2_INAV_OUTPUT_MAPPING_EXT 0x200D // Obsolete, replaced by MSP2_INAV_OUTPUT_MAPPING_EXT2
#define MSP2_INAV_TIMER_OUTPUT_MODE 0x200E
#define MSP2_INAV_SET_TIMER_OUTPUT_MODE 0x200F
#define MSP2_INAV_OUTPUT_MAPPING_EXT2 0x210D
#define MSP2_INAV_MIXER 0x2010
#define MSP2_INAV_SET_MIXER 0x2011

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

@ -205,6 +205,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
// Fixed wing
.fw = {
.max_bank_angle = SETTING_NAV_FW_BANK_ANGLE_DEFAULT, // degrees
.max_auto_climb_rate = SETTING_NAV_FW_AUTO_CLIMB_RATE_DEFAULT, // 5 m/s
.max_manual_climb_rate = SETTING_NAV_FW_MANUAL_CLIMB_RATE_DEFAULT, // 3 m/s
.max_climb_angle = SETTING_NAV_FW_CLIMB_ANGLE_DEFAULT, // degrees
.max_dive_angle = SETTING_NAV_FW_DIVE_ANGLE_DEFAULT, // degrees
@ -300,10 +301,6 @@ bool validateRTHSanityChecker(void);
void updateHomePosition(void);
bool abortLaunchAllowed(void);
// static bool rthAltControlStickOverrideCheck(unsigned axis);
// static void updateRthTrackback(bool forceSaveTrackPoint);
// static fpVector3_t * rthGetTrackbackPos(void);
#ifdef USE_FW_AUTOLAND
static float getLandAltitude(void);
static int32_t calcWindDiff(int32_t heading, int32_t windHeading);
@ -327,8 +324,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_TRACKBACK(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LOITER_ABOVE_HOME(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHING(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigationFSMState_t previousState);
@ -355,6 +352,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_LOITER(navig
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_GLIDE(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FLARE(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FINISHED(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_ABORT(navigationFSMState_t previousState);
#endif
@ -422,7 +420,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
.persistentId = NAV_PERSISTENT_ID_POSHOLD_3D_INITIALIZE,
.onEntry = navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE,
.timeoutMs = 0,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT,
.mapToFlightModes = NAV_ALTHOLD_MODE | NAV_POSHOLD_MODE,
.mwState = MW_NAV_STATE_HOLD_INFINIT,
.mwError = MW_NAV_ERROR_NONE,
@ -437,7 +435,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
.persistentId = NAV_PERSISTENT_ID_POSHOLD_3D_IN_PROGRESS,
.onEntry = navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS,
.timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT | NAV_RC_ALT | NAV_RC_POS | NAV_RC_YAW,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT | NAV_RC_ALT | NAV_RC_POS | NAV_RC_YAW,
.mapToFlightModes = NAV_ALTHOLD_MODE | NAV_POSHOLD_MODE,
.mwState = MW_NAV_STATE_HOLD_INFINIT,
.mwError = MW_NAV_ERROR_NONE,
@ -584,7 +582,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_CLIMB_TO_SAFE_ALT,
[NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_TRACKBACK] = NAV_STATE_RTH_TRACKBACK,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
[NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING] = NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
}
},
@ -593,7 +591,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
.persistentId = NAV_PERSISTENT_ID_RTH_CLIMB_TO_SAFE_ALT,
.onEntry = navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT,
.timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW, // allow pos adjustment while climbind to safe alt
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW, // allow pos adjustment while climbing to safe alt
.mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_RTH_CLIMB,
.mwError = MW_NAV_ERROR_WAIT_FOR_RTH_ALT,
@ -639,7 +637,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HEAD_HOME, // re-process the state
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
@ -650,18 +648,18 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
}
},
[NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING] = {
.persistentId = NAV_PERSISTENT_ID_RTH_HOVER_PRIOR_TO_LANDING,
.onEntry = navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
[NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING] = {
.persistentId = NAV_PERSISTENT_ID_RTH_LOITER_PRIOR_TO_LANDING,
.onEntry = navOnEnteringState_NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING,
.timeoutMs = 500,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
.mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_LAND_SETTLE,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING,
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_LANDING,
[NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME] = NAV_STATE_RTH_HOVER_ABOVE_HOME,
[NAV_FSM_EVENT_SWITCH_TO_RTH_LOITER_ABOVE_HOME] = NAV_STATE_RTH_LOITER_ABOVE_HOME,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
@ -671,16 +669,16 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
}
},
[NAV_STATE_RTH_HOVER_ABOVE_HOME] = {
.persistentId = NAV_PERSISTENT_ID_RTH_HOVER_ABOVE_HOME,
.onEntry = navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME,
[NAV_STATE_RTH_LOITER_ABOVE_HOME] = {
.persistentId = NAV_PERSISTENT_ID_RTH_LOITER_ABOVE_HOME,
.onEntry = navOnEnteringState_NAV_STATE_RTH_LOITER_ABOVE_HOME,
.timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT,
.mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_HOVER_ABOVE_HOME,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HOVER_ABOVE_HOME,
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_LOITER_ABOVE_HOME,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
@ -694,7 +692,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
.persistentId = NAV_PERSISTENT_ID_RTH_LANDING,
.onEntry = navOnEnteringState_NAV_STATE_RTH_LANDING,
.timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
.mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
.mwError = MW_NAV_ERROR_LANDING,
@ -707,7 +705,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING] = NAV_STATE_FW_LANDING_CLIMB_TO_LOITER,
[NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME] = NAV_STATE_RTH_HOVER_ABOVE_HOME,
[NAV_FSM_EVENT_SWITCH_TO_RTH_LOITER_ABOVE_HOME] = NAV_STATE_RTH_LOITER_ABOVE_HOME,
}
},
@ -715,7 +713,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
.persistentId = NAV_PERSISTENT_ID_RTH_FINISHING,
.onEntry = navOnEnteringState_NAV_STATE_RTH_FINISHING,
.timeoutMs = 0,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH,
.mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
.mwError = MW_NAV_ERROR_LANDING,
@ -826,7 +824,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
.persistentId = NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME, // There is no state for timed hold?
.onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME,
.timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
.mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_HOLD_TIMED,
.mwError = MW_NAV_ERROR_NONE,
@ -847,7 +845,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
.persistentId = NAV_PERSISTENT_ID_WAYPOINT_RTH_LAND,
.onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND,
.timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
.mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
.mwError = MW_NAV_ERROR_LANDING,
@ -885,7 +883,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
.persistentId = NAV_PERSISTENT_ID_WAYPOINT_FINISHED,
.onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED,
.timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP | NAV_AUTO_WP_DONE,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP | NAV_AUTO_WP_DONE,
.mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_WP_ENROUTE,
.mwError = MW_NAV_ERROR_FINISH,
@ -924,7 +922,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
.persistentId = NAV_PERSISTENT_ID_EMERGENCY_LANDING_IN_PROGRESS,
.onEntry = navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS,
.timeoutMs = 10,
.stateFlags = NAV_CTL_EMERG | NAV_REQUIRE_ANGLE,
.stateFlags = NAV_CTL_HOLD | NAV_CTL_EMERG | NAV_REQUIRE_ANGLE,
.mapToFlightModes = 0,
.mwState = MW_NAV_STATE_EMERGENCY_LANDING,
.mwError = MW_NAV_ERROR_LANDING,
@ -942,7 +940,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
.persistentId = NAV_PERSISTENT_ID_EMERGENCY_LANDING_FINISHED,
.onEntry = navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINISHED,
.timeoutMs = 10,
.stateFlags = NAV_CTL_EMERG | NAV_REQUIRE_ANGLE,
.stateFlags = NAV_CTL_HOLD | NAV_CTL_EMERG | NAV_REQUIRE_ANGLE,
.mapToFlightModes = 0,
.mwState = MW_NAV_STATE_LANDED,
.mwError = MW_NAV_ERROR_LANDING,
@ -1051,7 +1049,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
.persistentId = NAV_PERSISTENT_ID_FW_LANDING_CLIMB_TO_LOITER,
.onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_CLIMB_TO_LOITER,
.timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH,
.mapToFlightModes = NAV_FW_AUTOLAND,
.mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
.mwError = MW_NAV_ERROR_NONE,
@ -1072,7 +1070,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
.persistentId = NAV_PERSISTENT_ID_FW_LANDING_LOITER,
.onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_LOITER,
.timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH,
.mapToFlightModes = NAV_FW_AUTOLAND,
.mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
.mwError = MW_NAV_ERROR_NONE,
@ -1106,6 +1104,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_FINISHED] = NAV_STATE_FW_LANDING_FINISHED,
[NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT] = NAV_STATE_FW_LANDING_ABORT,
}
},
@ -1127,6 +1126,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_FINISHED] = NAV_STATE_FW_LANDING_FINISHED,
[NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT] = NAV_STATE_FW_LANDING_ABORT,
}
},
@ -1141,10 +1141,25 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_FW_LANDING_FLARE, // re-process the state
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_FINISHED] = NAV_STATE_FW_LANDING_FINISHED,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
}
},
[NAV_STATE_FW_LANDING_FINISHED] = {
.persistentId = NAV_PERSISTENT_ID_FW_LANDING_FINISHED,
.onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_FINISHED,
.timeoutMs = 10,
.stateFlags = NAV_REQUIRE_ANGLE,
.mapToFlightModes = NAV_FW_AUTOLAND,
.mwState = MW_NAV_STATE_LANDED,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_FW_LANDING_FINISHED, // re-process the state
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
}
},
[NAV_STATE_FW_LANDING_ABORT] = {
.persistentId = NAV_PERSISTENT_ID_FW_LANDING_ABORT,
.onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_ABORT,
@ -1245,7 +1260,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(n
}
// Prepare position controller if idle or current Mode NOT active in position hold state
if (previousState != NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING && previousState != NAV_STATE_RTH_HOVER_ABOVE_HOME &&
if (previousState != NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING && previousState != NAV_STATE_RTH_LOITER_ABOVE_HOME &&
previousState != NAV_STATE_RTH_LANDING && previousState != NAV_STATE_WAYPOINT_RTH_LAND &&
previousState != NAV_STATE_WAYPOINT_FINISHED && previousState != NAV_STATE_WAYPOINT_HOLD_TIME)
{
@ -1426,9 +1441,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
setHomePosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING, NAV_HOME_VALID_ALL);
setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
return NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING
} else {
if (rthTrackBackIsActive() && rth_trackback.activePointIndex >= 0 && !isWaypointMissionRTHActive()) {
return NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING; // NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING
}
else {
// Switch to RTH trackback
if (rthTrackBackCanBeActivated() && rth_trackback.activePointIndex >= 0 && !isWaypointMissionRTHActive()) {
rthTrackBackUpdate(true); // save final trackpoint for altitude and max trackback distance reference
posControl.flags.rthTrackbackActive = true;
calculateAndSetActiveWaypointToLocalPosition(getRthTrackBackPosition());
@ -1495,8 +1512,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
initializeRTHSanityChecker();
}
// Save initial home distance for future use
// Save initial home distance and direction for future use
posControl.rthState.rthInitialDistance = posControl.homeDistance;
posControl.activeWaypoint.bearing = posControl.homeDirection;
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL);
if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING_LEGACY)) {
@ -1546,7 +1564,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_TRACKBACK(navigatio
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
if (rthTrackBackSetNewPosition()) {
if (!rthTrackBackSetNewPosition()) {
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE;
}
@ -1582,7 +1600,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
if ((posControl.flags.estPosStatus >= EST_USABLE)) {
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL);
if (isWaypointReached(tmpHomePos, 0)) {
if (isWaypointReached(tmpHomePos, &posControl.activeWaypoint.bearing)) {
// Successfully reached position target - update XYZ-position
setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
@ -1591,7 +1609,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
if (navConfig()->general.flags.rth_use_linear_descent && posControl.rthState.rthLinearDescentActive)
posControl.rthState.rthLinearDescentActive = false;
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING
} else {
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_XY);
return NAV_FSM_EVENT_NONE;
@ -1605,7 +1623,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
return NAV_FSM_EVENT_NONE;
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING(navigationFSMState_t previousState)
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING(navigationFSMState_t previousState)
{
UNUSED(previousState);
@ -1638,8 +1656,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
// Wait until target heading is reached for MR (with 15 deg margin for error), or continue for Fixed Wing
if (!pauseLanding && ((ABS(wrap_18000(posControl.rthState.homePosition.heading - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) || STATE(FIXED_WING_LEGACY))) {
resetLandingDetector(); // force reset landing detector just in case
updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET);
return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; // success = land
updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_CURRENT);
return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_LOITER_ABOVE_HOME; // success = land
} else {
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_FINAL);
setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
@ -1647,7 +1665,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
}
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(navigationFSMState_t previousState)
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LOITER_ABOVE_HOME(navigationFSMState_t previousState)
{
UNUSED(previousState);
@ -1656,7 +1674,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(na
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_HOVER);
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_LOITER);
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
return NAV_FSM_EVENT_NONE;
@ -1664,9 +1682,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(na
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationFSMState_t previousState)
{
#ifndef USE_FW_AUTOLAND
UNUSED(previousState);
#endif
//On ROVER and BOAT we immediately switch to the next event
if (!STATE(ALTITUDE_CONTROL)) {
@ -1679,7 +1695,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
/* If position sensors unavailable - land immediately (wait for timeout on GPS)
* Continue to check for RTH sanity during landing */
if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout() || (previousState != NAV_STATE_WAYPOINT_REACHED && !validateRTHSanityChecker())) {
if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout() || (FLIGHT_MODE(NAV_RTH_MODE) && !validateRTHSanityChecker())) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
@ -1698,7 +1714,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
shIdx = posControl.safehomeState.index;
missionFwLandConfigStartIdx = MAX_SAFE_HOMES;
#endif
if (previousState == NAV_STATE_WAYPOINT_REACHED && missionIdx >= 0) {
if (FLIGHT_MODE(NAV_WP_MODE) && missionIdx >= 0) {
approachSettingIdx = missionFwLandConfigStartIdx + missionIdx;
} else if (shIdx >= 0) {
approachSettingIdx = shIdx;
@ -1706,7 +1722,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
if (!posControl.fwLandState.landAborted && approachSettingIdx >= 0 && (fwAutolandApproachConfig(approachSettingIdx)->landApproachHeading1 != 0 || fwAutolandApproachConfig(approachSettingIdx)->landApproachHeading2 != 0)) {
if (previousState == NAV_STATE_WAYPOINT_REACHED) {
if (FLIGHT_MODE(NAV_WP_MODE)) {
posControl.fwLandState.landPos = posControl.activeWaypoint.pos;
posControl.fwLandState.landWp = true;
} else {
@ -1831,7 +1847,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
case NAV_WP_ACTION_LAND:
calculateAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]);
posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos);
posControl.wpInitialAltitude = posControl.actualState.abs.pos.z;
posControl.wpAltitudeReached = false;
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS
@ -1895,10 +1910,17 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
fpVector3_t tmpWaypoint;
tmpWaypoint.x = posControl.activeWaypoint.pos.x;
tmpWaypoint.y = posControl.activeWaypoint.pos.y;
tmpWaypoint.z = scaleRangef(constrainf(posControl.wpDistance, posControl.wpInitialDistance / 10.0f, posControl.wpInitialDistance),
posControl.wpInitialDistance, posControl.wpInitialDistance / 10.0f,
posControl.wpInitialAltitude, posControl.activeWaypoint.pos.z);
setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING);
// Use linear climb between WPs arriving at WP altitude when within 10% of total distance to WP
// Update climb rate until within 100cm of total climb xy distance to WP
float climbRate = 0.0f;
if (posControl.wpDistance - 0.1f * posControl.wpInitialDistance > 100.0f) {
climbRate = posControl.actualState.velXY * (posControl.activeWaypoint.pos.z - posControl.actualState.abs.pos.z) /
(posControl.wpDistance - 0.1f * posControl.wpInitialDistance);
}
updateClimbRateToAltitudeController(climbRate, posControl.activeWaypoint.pos.z, ROC_TO_ALT_TARGET);
if(STATE(MULTIROTOR)) {
switch (wpHeadingControl.mode) {
case NAV_WP_HEAD_MODE_NONE:
@ -1999,8 +2021,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navi
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navigationFSMState_t previousState)
{
UNUSED(previousState);
#ifdef USE_FW_AUTOLAND
if (posControl.fwLandState.landAborted) {
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED;
@ -2009,21 +2029,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navig
const navigationFSMEvent_t landEvent = navOnEnteringState_NAV_STATE_RTH_LANDING(previousState);
#ifdef USE_FW_AUTOLAND
if (landEvent == NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING) {
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING;
} else
#endif
if (landEvent == NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME) {
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED;
} else if (landEvent == NAV_FSM_EVENT_SUCCESS) {
// Landing controller returned success - invoke RTH finishing state and finish the waypoint
if (landEvent == NAV_FSM_EVENT_SUCCESS) {
// Landing controller returned success - invoke RTH finish states and finish the waypoint
navOnEnteringState_NAV_STATE_RTH_FINISHING(previousState);
return NAV_FSM_EVENT_SUCCESS;
}
else {
return NAV_FSM_EVENT_NONE;
navOnEnteringState_NAV_STATE_RTH_FINISHED(previousState);
}
return landEvent;
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_NEXT(navigationFSMState_t previousState)
@ -2224,7 +2236,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_CLIMB_TO_LOI
}
if (ABS(getEstimatedActualPosition(Z) - posControl.fwLandState.landAproachAltAgl) < (navConfig()->general.waypoint_enforce_altitude > 0 ? navConfig()->general.waypoint_enforce_altitude : FW_LAND_LOITER_ALT_TOLERANCE)) {
updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET);
updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_CURRENT);
posControl.fwLandState.landState = FW_AUTOLAND_STATE_LOITER;
return NAV_FSM_EVENT_SUCCESS;
}
@ -2334,6 +2346,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH(nav
{
UNUSED(previousState);
if (STATE(LANDING_DETECTED)) {
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_FINISHED;
}
if ((posControl.flags.estHeadingStatus == EST_NONE) || checkForPositionSensorTimeout()) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
@ -2342,12 +2358,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH(nav
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT;
}
if (isLandingDetected()) {
posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
//disarm(DISARM_LANDING);
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
}
if (getLandAltitude() <= fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landAlt + navFwAutolandConfig()->glideAltitude - (fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->isSeaLevelRef ? GPS_home.alt : 0)) {
resetPositionController();
posControl.cruise.course = posControl.fwLandState.landingDirection;
@ -2384,6 +2394,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_GLIDE(naviga
{
UNUSED(previousState);
if (STATE(LANDING_DETECTED)) {
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_FINISHED;
}
if (isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband)) {
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT;
}
@ -2393,12 +2407,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_GLIDE(naviga
return NAV_FSM_EVENT_SUCCESS;
}
if (isLandingDetected()) {
posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
//disarm(DISARM_LANDING);
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
}
setDesiredPosition(NULL, posControl.cruise.course, NAV_POS_UPDATE_HEADING);
return NAV_FSM_EVENT_NONE;
}
@ -2407,16 +2415,24 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FLARE(naviga
{
UNUSED(previousState);
if (isLandingDetected()) {
posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
//disarm(DISARM_LANDING);
return NAV_FSM_EVENT_SUCCESS;
if (STATE(LANDING_DETECTED)) {
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_FINISHED;
}
setDesiredPosition(NULL, posControl.cruise.course, NAV_POS_UPDATE_HEADING);
return NAV_FSM_EVENT_NONE;
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FINISHED(navigationFSMState_t previousState)
{
UNUSED(previousState);
posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
return NAV_FSM_EVENT_NONE;
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_ABORT(navigationFSMState_t previousState)
{
UNUSED(previousState);
@ -2533,7 +2549,7 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode)
posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthFinalAltitude;
break;
case RTH_HOME_FINAL_HOVER:
case RTH_HOME_FINAL_LOITER:
if (navConfig()->general.rth_home_altitude) {
posControl.rthState.homeTmpWaypoint.z = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_home_altitude;
}
@ -2818,30 +2834,28 @@ static bool getLocalPosNextWaypoint(fpVector3_t * nextWpPos)
/*-----------------------------------------------------------
* Check if waypoint is/was reached.
* waypointBearing stores initial bearing to waypoint
* 'waypointBearing' stores initial bearing to waypoint.
*-----------------------------------------------------------*/
bool isWaypointReached(const fpVector3_t *waypointPos, const int32_t *waypointBearing)
{
posControl.wpDistance = calculateDistanceToDestination(waypointPos);
// Airplane will do a circular loiter at hold waypoints and might never approach them closer than waypoint_radius
// Check within 10% margin of circular loiter radius
if (STATE(AIRPLANE) && isNavHoldPositionActive() && posControl.wpDistance <= (navConfig()->fw.loiter_radius * 1.10f)) {
return true;
}
// Check if waypoint was missed based on bearing to waypoint exceeding given angular limit relative to initial waypoint bearing.
// Default angular limit = 100 degs with a reduced limit of 60 degs used if fixed wing waypoint turn smoothing option active
uint16_t relativeBearingTargetAngle = 10000;
if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP || posControl.flags.rthTrackbackActive) {
// If WP turn smoothing CUT option used WP is reached when start of turn is initiated
if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_CUT && posControl.flags.wpTurnSmoothingActive) {
if (STATE(AIRPLANE) && posControl.flags.wpTurnSmoothingActive) {
// If WP mode turn smoothing CUT option used waypoint is reached when start of turn is initiated
if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_CUT) {
posControl.flags.wpTurnSmoothingActive = false;
return true;
}
// Check if waypoint was missed based on bearing to WP exceeding 100 degrees relative to waypoint Yaw
// Same method for turn smoothing option but relative bearing set at 60 degrees
uint16_t relativeBearing = posControl.flags.wpTurnSmoothingActive ? 6000 : 10000;
if (ABS(wrap_18000(calculateBearingToDestination(waypointPos) - *waypointBearing)) > relativeBearing) {
return true;
relativeBearingTargetAngle = 6000;
}
if (ABS(wrap_18000(calculateBearingToDestination(waypointPos) - *waypointBearing)) > relativeBearingTargetAngle) {
return true;
}
return posControl.wpDistance <= (navConfig()->general.waypoint_radius);
@ -3274,8 +3288,7 @@ void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlag
// Z-position
if ((useMask & NAV_POS_UPDATE_Z) != 0) {
updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); // Reset RoC/RoD -> altitude controller
posControl.desiredState.pos.z = pos->z;
updateClimbRateToAltitudeController(0, pos->z, ROC_TO_ALT_TARGET);
}
// Heading
@ -3327,9 +3340,6 @@ void updateLandingStatus(timeMs_t currentTimeMs)
}
resetLandingDetector();
if (!IS_RC_MODE_ACTIVE(BOXARM)) {
DISABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED);
}
return;
}
@ -3388,66 +3398,77 @@ bool isProbablyStillFlying(void)
/*-----------------------------------------------------------
* Z-position controller
*-----------------------------------------------------------*/
float getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros)
{
const bool emergLandingIsActive = navigationIsExecutingAnEmergencyLanding();
float maxClimbRate = STATE(MULTIROTOR) ? navConfig()->mc.max_auto_climb_rate : navConfig()->fw.max_auto_climb_rate;
if (posControl.flags.rocToAltMode == ROC_TO_ALT_CONSTANT) {
if (posControl.flags.isAdjustingAltitude) {
maxClimbRate = STATE(MULTIROTOR) ? navConfig()->mc.max_manual_climb_rate : navConfig()->fw.max_manual_climb_rate;
}
return constrainf(posControl.desiredState.climbRateDemand, -maxClimbRate, maxClimbRate);
}
if (posControl.desiredState.climbRateDemand) {
maxClimbRate = constrainf(ABS(posControl.desiredState.climbRateDemand), 0.0f, maxClimbRate);
} else if (emergLandingIsActive) {
maxClimbRate = navConfig()->general.emerg_descent_rate;
}
const float targetAltitudeError = targetAltitude - navGetCurrentActualPositionAndVelocity()->pos.z;
float targetVel = 0.0f;
if (STATE(MULTIROTOR)) {
targetVel = getSqrtControllerVelocity(targetAltitude, deltaMicros);
} else {
targetVel = pidProfile()->fwAltControlResponseFactor * targetAltitudeError / 100.0f;
}
if (emergLandingIsActive && targetAltitudeError > -50.0f) {
return -100.0f; // maintain 1 m/s descent during emerg landing when within 50cm of min speed landing altitude target
} else {
return constrainf(targetVel, -maxClimbRate, maxClimbRate);
}
}
void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAltitude, climbRateToAltitudeControllerMode_e mode)
{
#define MIN_TARGET_CLIMB_RATE 100.0f // cm/s
/* ROC_TO_ALT_TARGET - constant climb rate until close to target altitude then reducing down as altitude is reached.
* Any non zero climb rate sets the max allowed climb rate. Default max climb rate limits are used when set to 0.
*
* ROC_TO_ALT_CURRENT - similar to ROC_TO_ALT_TARGET except target altitude set to current altitude.
* No climb rate or altitude target required.
*
* ROC_TO_ALT_CONSTANT - constant climb rate. Climb rate and direction required. Target alt not required. */
static timeUs_t lastUpdateTimeUs;
timeUs_t currentTimeUs = micros();
// Terrain following uses different altitude measurement
const float altitudeToUse = navGetCurrentActualPositionAndVelocity()->pos.z;
if (mode != ROC_TO_ALT_RESET && desiredClimbRate) {
/* ROC_TO_ALT_CONSTANT - constant climb rate
* ROC_TO_ALT_TARGET - constant climb rate until close to target altitude reducing to min rate when altitude reached
* Rate reduction starts at distance from target altitude of 5 x climb rate */
if (mode == ROC_TO_ALT_TARGET && fabsf(desiredClimbRate) > MIN_TARGET_CLIMB_RATE) {
const int8_t direction = desiredClimbRate > 0 ? 1 : -1;
const float absClimbRate = fabsf(desiredClimbRate);
const uint16_t maxRateCutoffAlt = absClimbRate * 5;
const float verticalVelScaled = scaleRangef(navGetCurrentActualPositionAndVelocity()->pos.z - targetAltitude,
0.0f, -maxRateCutoffAlt * direction, MIN_TARGET_CLIMB_RATE, absClimbRate);
desiredClimbRate = direction * constrainf(verticalVelScaled, MIN_TARGET_CLIMB_RATE, absClimbRate);
if (mode == ROC_TO_ALT_CURRENT) {
posControl.desiredState.pos.z = navGetCurrentActualPositionAndVelocity()->pos.z;
desiredClimbRate = 0.0f;
} else if (mode == ROC_TO_ALT_TARGET) {
posControl.desiredState.pos.z = targetAltitude;
}
posControl.desiredState.climbRateDemand = desiredClimbRate;
posControl.flags.rocToAltMode = mode;
/*
* If max altitude is set, reset climb rate if altitude is reached and climb rate is > 0
* In other words, when altitude is reached, allow it only to shrink
* If max altitude is set limit desired altitude and impose altitude limit for constant climbs unless climb rate is -ve.
* Inhibit during RTH mode and also WP mode with altitude enforce active to avoid climbs getting stuck at max alt limit.
*/
if (navConfig()->general.max_altitude > 0 && altitudeToUse >= navConfig()->general.max_altitude && desiredClimbRate > 0) {
desiredClimbRate = 0;
if (navConfig()->general.max_altitude && !FLIGHT_MODE(NAV_RTH_MODE) && !(FLIGHT_MODE(NAV_WP_MODE) && navConfig()->general.waypoint_enforce_altitude)) {
posControl.desiredState.pos.z = MIN(posControl.desiredState.pos.z, navConfig()->general.max_altitude);
if (mode != ROC_TO_ALT_CONSTANT || (mode == ROC_TO_ALT_CONSTANT && desiredClimbRate < 0.0f)) {
return;
}
if (STATE(FIXED_WING_LEGACY)) {
// Fixed wing climb rate controller is open-loop. We simply move the known altitude target
float timeDelta = US2S(currentTimeUs - lastUpdateTimeUs);
static bool targetHoldActive = false;
if (timeDelta <= HZ2S(MIN_POSITION_UPDATE_RATE_HZ) && desiredClimbRate) {
// Update target altitude only if actual altitude moving in same direction and lagging by < 5 m, otherwise hold target
if (navGetCurrentActualPositionAndVelocity()->vel.z * desiredClimbRate >= 0 && fabsf(posControl.desiredState.pos.z - altitudeToUse) < 500) {
posControl.desiredState.pos.z += desiredClimbRate * timeDelta;
targetHoldActive = false;
} else if (!targetHoldActive) { // Reset and hold target to actual + climb rate boost until actual catches up
posControl.desiredState.pos.z = altitudeToUse + desiredClimbRate;
targetHoldActive = true;
}
} else {
targetHoldActive = false;
if (posControl.flags.isAdjustingAltitude) {
posControl.desiredState.pos.z = navConfig()->general.max_altitude;
posControl.flags.rocToAltMode = ROC_TO_ALT_TARGET;
}
}
else {
// Multicopter climb-rate control is closed-loop, it's possible to directly calculate desired altitude setpoint to yield the required RoC/RoD
posControl.desiredState.pos.z = altitudeToUse + (desiredClimbRate / posControl.pids.pos[Z].param.kP);
}
} else { // ROC_TO_ALT_RESET or zero desired climbrate
posControl.desiredState.pos.z = altitudeToUse;
}
lastUpdateTimeUs = currentTimeUs;
}
static void resetAltitudeController(bool useTerrainFollowing)
@ -3949,16 +3970,20 @@ bool isLastMissionWaypoint(void)
/* Checks if Nav hold position is active */
bool isNavHoldPositionActive(void)
{
// WP mode last WP hold and Timed hold positions
if (FLIGHT_MODE(NAV_WP_MODE)) {
return isLastMissionWaypoint() || NAV_Status.state == MW_NAV_STATE_HOLD_TIMED;
/* If the current Nav state isn't flagged as a hold point (NAV_CTL_HOLD) then
* waypoints are assumed to be hold points by default unless excluded as defined here */
if (navGetCurrentStateFlags() & NAV_CTL_HOLD) {
return true;
}
// RTH mode (spiral climb and Home positions but excluding RTH Trackback point positions) and POSHOLD mode
// Also hold position during emergency landing if position valid
return (FLIGHT_MODE(NAV_RTH_MODE) && !posControl.flags.rthTrackbackActive) ||
FLIGHT_MODE(NAV_POSHOLD_MODE) ||
(posControl.navState == NAV_STATE_FW_LANDING_CLIMB_TO_LOITER || posControl.navState == NAV_STATE_FW_LANDING_LOITER) ||
navigationIsExecutingAnEmergencyLanding();
// No hold required for basic WP type unless it's the last mission waypoint
if (FLIGHT_MODE(NAV_WP_MODE)) {
return posControl.waypointList[posControl.activeWaypointIndex].action != NAV_WP_ACTION_WAYPOINT || isLastMissionWaypoint();
}
// No hold required for Trackback WPs or for fixed wing autoland WPs not flagged as hold points (returned above if they are)
return !FLIGHT_MODE(NAV_FW_AUTOLAND) && !posControl.flags.rthTrackbackActive;
}
float getActiveSpeed(void)
@ -4686,9 +4711,9 @@ void navigationUsePIDs(void)
0.0f
);
navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 10.0f,
(float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 10.0f,
(float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 10.0f,
navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 100.0f,
(float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 100.0f,
(float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 100.0f,
0.0f,
NAV_DTERM_CUT_HZ,
0.0f
@ -4797,9 +4822,9 @@ void abortForcedRTH(void)
rthState_e getStateOfForcedRTH(void)
{
/* If forced RTH activated and in AUTO_RTH or EMERG state */
/* If forced RTH activated and in AUTO_RTH, EMERG state or FW Auto Landing */
if (posControl.flags.forcedRTHActivated && ((navGetStateFlags(posControl.navState) & (NAV_AUTO_RTH | NAV_CTL_EMERG | NAV_MIXERAT)) || FLIGHT_MODE(NAV_FW_AUTOLAND))) {
if (posControl.navState == NAV_STATE_RTH_FINISHED || posControl.navState == NAV_STATE_EMERGENCY_LANDING_FINISHED) {
if (posControl.navState == NAV_STATE_RTH_FINISHED || posControl.navState == NAV_STATE_EMERGENCY_LANDING_FINISHED || posControl.navState == NAV_STATE_FW_LANDING_FINISHED) {
return RTH_HAS_LANDED;
}
else {
@ -5043,12 +5068,9 @@ void resetFwAutolandApproach(int8_t idx)
}
}
bool canFwLandCanceld(void)
bool canFwLandingBeCancelled(void)
{
return posControl.navState == NAV_STATE_FW_LANDING_CLIMB_TO_LOITER
|| posControl.navState == NAV_STATE_FW_LANDING_LOITER
|| posControl.navState == NAV_STATE_FW_LANDING_APPROACH
|| posControl.navState == NAV_STATE_FW_LANDING_GLIDE;
return FLIGHT_MODE(NAV_FW_AUTOLAND) && posControl.navState != NAV_STATE_FW_LANDING_FLARE;
}
#endif

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

@ -260,8 +260,6 @@ typedef struct positionEstimationConfig_s {
float max_eph_epv; // Max estimated position error acceptable for estimation (cm)
float baro_epv; // Baro position error
uint8_t use_gps_no_baro;
#ifdef USE_GPS_FIX_ESTIMATION
uint8_t allow_gps_fix_estimation;
#endif
@ -328,7 +326,7 @@ typedef struct navConfig_s {
struct {
uint8_t max_bank_angle; // multicopter max banking angle (deg)
uint16_t max_auto_climb_rate; // max vertical speed limitation cm/sec
uint16_t max_auto_climb_rate; // max vertical speed limitation nav modes cm/sec
uint16_t max_manual_climb_rate; // manual velocity control max vertical speed
#ifdef USE_MR_BRAKING_MODE
@ -350,6 +348,7 @@ typedef struct navConfig_s {
struct {
uint8_t max_bank_angle; // Fixed wing max banking angle (deg)
uint16_t max_auto_climb_rate; // max vertical speed limitation nav modes cm/sec
uint16_t max_manual_climb_rate; // manual velocity control max vertical speed
uint8_t max_climb_angle; // Fixed wing max banking angle (deg)
uint8_t max_dive_angle; // Fixed wing max banking angle (deg)
@ -702,7 +701,7 @@ uint8_t getActiveWpNumber(void);
int32_t navigationGetHomeHeading(void);
#ifdef USE_FW_AUTOLAND
bool canFwLandCanceld(void);
bool canFwLandingBeCancelled(void);
#endif
/* Compatibility data */

120
src/main/navigation/navigation_declination_gen.c
View file

@ -1,63 +1,77 @@
/* this file is automatically generated by src/utils/declination.py - DO NOT EDIT! */
/* Updated on 2024-04-19 12:54:15.842704 */
/* Updated on 2024-05-13 21:53:14.731603 */
#include <stdint.h>
const float SAMPLING_RES = 10;
const float SAMPLING_MIN_LAT = -90;
const float SAMPLING_MAX_LAT = 90;
const float SAMPLING_MIN_LON = -180;
const float SAMPLING_MAX_LON = 180;
#if defined(NAV_AUTO_MAG_DECLINATION_PRECISE)
#define SAMPLING_RES 10.00000f
#define SAMPLING_MIN_LON -180.00000f
#define SAMPLING_MAX_LON 180.00000f
#define SAMPLING_MIN_LAT -90.00000f
#define SAMPLING_MAX_LAT 90.00000f
static const float declination_table[19][37] = {
{148.63040f,138.63040f,128.63040f,118.63040f,108.63040f,98.63040f,88.63040f,78.63040f,68.63040f,58.63040f,48.63040f,38.63040f,28.63040f,18.63040f,8.63040f,-1.36960f,-11.36960f,-21.36960f,-31.36960f,-41.36960f,-51.36960f,-61.36960f,-71.36960f,-81.36960f,-91.36960f,-101.36960f,-111.36960f,-121.36960f,-131.36960f,-141.36960f,-151.36960f,-161.36960f,-171.36960f,178.63040f,168.63040f,158.63040f,148.63040f},
{128.87816f,116.70769f,105.62443f,95.48127f,86.09683f,77.29367f,68.91718f,60.84302f,52.97822f,45.25811f,37.64003f,30.09478f,22.59702f,15.11676f,7.61418f,0.03947f,-7.66207f,-15.54157f,-23.63819f,-31.97410f,-40.55489f,-49.37538f,-58.42917f,-67.71936f,-77.26833f,-87.12519f,-97.37007f,-108.11458f,-119.49579f,-131.65830f,-144.71588f,-158.68662f,-173.41452f,171.47213f,156.49935f,142.18469f,128.87816f},
{85.86755f,77.88182f,71.42849f,65.89217f,60.86962f,56.04972f,51.17842f,46.06489f,40.60132f,34.77619f,28.67056f,22.43282f,16.23087f,10.18755f,4.31917f,-1.49067f,-7.45797f,-13.81467f,-20.71641f,-28.18762f,-36.12471f,-44.34676f,-52.66473f,-60.93937f,-69.11381f,-77.22546f,-85.41334f,-93.94372f,-103.29064f,-114.35005f,-128.97727f,-151.06023f,174.83520f,137.67818f,112.23020f,96.52063f,85.86756f},
{48.43188f,46.98353f,45.36982f,43.80186f,42.32760f,40.82382f,38.99678f,36.46379f,32.90329f,28.18877f,22.45599f,16.10387f,9.71432f,3.86202f,-1.14650f,-5.45997f,-9.64078f,-14.38658f,-20.18766f,-27.11174f,-34.80967f,-42.71859f,-50.31107f,-57.23008f,-63.28782f,-68.39719f,-72.47622f,-75.31221f,-76.30788f,-73.74072f,-61.55512f,-18.96160f,29.20071f,44.46666f,48.67974f,49.28540f,48.43188f},
{31.58850f,31.74835f,31.39924f,30.87678f,30.43574f,30.19800f,29.98360f,29.24010f,27.23113f,23.37602f,17.54538f,10.24099f,2.57680f,-4.11819f,-8.98330f,-12.08874f,-14.32383f,-16.91831f,-20.97106f,-26.92020f,-34.17714f,-41.56631f,-48.09571f,-53.19469f,-56.53278f,-57.82558f,-56.66031f,-52.27310f,-43.55594f,-29.95085f,-13.24765f,2.54212f,14.60798f,22.72224f,27.72553f,30.44935f,31.58850f},
{22.79850f,23.32533f,23.34746f,23.05694f,22.66226f,22.42885f,22.46289f,22.41289f,21.37705f,18.23370f,12.30982f,4.02530f,-4.93197f,-12.42012f,-17.23389f,-19.62803f,-20.56810f,-20.93312f,-21.98947f,-25.41090f,-31.19026f,-37.44166f,-42.49725f,-45.51856f,-46.02918f,-43.74555f,-38.55838f,-30.56411f,-20.73773f,-11.07131f,-2.79308f,4.21937f,10.21441f,15.15242f,18.92948f,21.44642f,22.79850f},
{17.16987f,17.68582f,17.85385f,17.73377f,17.34906f,16.86130f,16.52747f,16.34574f,15.56790f,12.74839f,6.74610f,-2.00418f,-11.22826f,-18.37193f,-22.46233f,-24.19496f,-24.47888f,-23.22908f,-20.72783f,-19.60150f,-21.98233f,-26.43395f,-30.43203f,-32.35125f,-31.47499f,-27.95264f,-22.40690f,-15.51422f,-8.63620f,-3.35146f,0.42144f,3.87188f,7.48128f,10.95260f,13.91406f,16.01563f,17.16987f},
{13.46149f,13.75829f,13.85281f,13.81813f,13.52893f,12.94281f,12.32698f,11.88976f,11.00215f,8.11359f,1.99478f,-6.62228f,-15.08744f,-21.05798f,-23.92224f,-24.34577f,-23.04831f,-19.84546f,-14.88685f,-10.38631f,-9.06427f,-11.31395f,-15.06340f,-17.65505f,-17.71488f,-15.54256f,-11.89991f,-7.27581f,-2.87289f,-0.16517f,1.24684f,2.97092f,5.51371f,8.29954f,10.80542f,12.58786f,13.46149f},
{11.19370f,11.20122f,11.07257f,11.02175f,10.82482f,10.26986f,9.62418f,9.09716f,7.97068f,4.77978f,-1.35469f,-9.31946f,-16.55524f,-21.15274f,-22.50861f,-21.01857f,-17.63827f,-13.15823f,-8.29683f,-4.01764f,-1.51484f,-1.87967f,-4.62279f,-7.47003f,-8.63091f,-8.05888f,-6.24442f,-3.42551f,-0.62936f,0.67783f,0.89029f,1.80510f,3.93778f,6.48671f,8.83523f,10.50781f,11.19370f},
{9.96310f,9.79689f,9.46606f,9.40362f,9.31492f,8.84240f,8.22130f,7.55735f,6.02850f,2.43377f,-3.55150f,-10.60044f,-16.54723f,-19.75861f,-19.59018f,-16.58982f,-12.16100f,-7.67553f,-3.93189f,-0.91522f,1.34973f,1.87637f,0.22653f,-2.15228f,-3.61566f,-3.90720f,-3.29932f,-1.83286f,-0.23997f,0.20367f,-0.19418f,0.32122f,2.29252f,4.85536f,7.33778f,9.21090f,9.96310f},
{9.18888f,9.24336f,8.98253f,9.05281f,9.17191f,8.83598f,8.10836f,7.00345f,4.80464f,0.69064f,-5.15618f,-11.30079f,-15.93916f,-17.74937f,-16.42349f,-12.86230f,-8.44857f,-4.38855f,-1.42704f,0.70613f,2.51072f,3.31152f,2.34785f,0.48172f,-0.89199f,-1.49634f,-1.59901f,-1.18437f,-0.66243f,-0.94842f,-1.78088f,-1.62463f,0.11657f,2.73547f,5.55820f,7.95812f,9.18888f},
{8.09034f,8.95675f,9.30400f,9.80188f,10.27100f,10.13647f,9.19791f,7.38846f,4.25004f,-0.60600f,-6.53915f,-11.93135f,-15.26054f,-15.79066f,-13.79060f,-10.27561f,-6.25103f,-2.56551f,0.07346f,1.80766f,3.24155f,4.05015f,3.51428f,2.10305f,0.93136f,0.25053f,-0.27213f,-0.70545f,-1.24346f,-2.37500f,-3.75122f,-4.07408f,-2.71911f,-0.15132f,2.99380f,6.02278f,8.09034f},
{6.27792f,8.37723f,9.88296f,11.16334f,12.08487f,12.15767f,11.03142f,8.51947f,4.34248f,-1.47518f,-7.78911f,-12.71269f,-15.01118f,-14.61457f,-12.25578f,-8.87216f,-5.13675f,-1.63793f,0.99774f,2.71417f,3.98293f,4.78698f,4.67468f,3.82712f,2.93970f,2.18792f,1.25958f,-0.01231f,-1.71794f,-3.92282f,-6.03566f,-6.90283f,-5.95491f,-3.51614f,-0.20102f,3.30598f,6.27792f},
{4.15672f,7.47883f,10.31556f,12.59798f,14.06987f,14.34572f,13.08042f,9.97537f,4.77447f,-2.16465f,-9.14687f,-14.00316f,-15.77819f,-14.88455f,-12.28640f,-8.84845f,-5.11869f,-1.55612f,1.34605f,3.41756f,4.91977f,6.01696f,6.56254f,6.51350f,6.02774f,5.09138f,3.48456f,1.07344f,-2.08240f,-5.60383f,-8.56486f,-9.89888f,-9.20525f,-6.83312f,-3.41374f,0.42499f,4.15672f},
{2.38746f,6.60961f,10.51138f,13.76537f,15.94256f,16.59043f,15.27093f,11.52705f,5.06990f,-3.36003f,-11.36932f,-16.51244f,-18.15167f,-17.03678f,-14.21651f,-10.51524f,-6.47146f,-2.49805f,1.03216f,3.92789f,6.27640f,8.24229f,9.81368f,10.78355f,10.85070f,9.68870f,7.04509f,2.91449f,-2.25894f,-7.44786f,-11.30963f,-12.92043f,-12.20858f,-9.70263f,-6.08438f,-1.92562f,2.38746f},
{1.14371f,5.95768f,10.55039f,14.56595f,17.51248f,18.77049f,17.56825f,12.99646f,4.54568f,-6.33792f,-15.91121f,-21.43028f,-22.88293f,-21.44177f,-18.22110f,-13.99911f,-9.28961f,-4.46489f,0.18470f,4.47851f,8.37200f,11.87759f,14.90388f,17.13814f,18.04159f,16.93503f,13.18748f,6.66358f,-1.56678f,-9.20492f,-14.20053f,-15.96059f,-14.99797f,-12.15657f,-8.19328f,-3.64820f,1.14371f},
{-0.35955f,4.92766f,9.99739f,14.51863f,17.99503f,19.64821f,18.23668f,11.99980f,-0.09657f,-14.74873f,-25.55246f,-30.33910f,-30.58104f,-28.00627f,-23.75441f,-18.51083f,-12.70165f,-6.61593f,-0.46628f,5.58665f,11.41580f,16.88997f,21.81133f,25.83916f,28.40134f,28.57592f,24.99531f,16.28607f,3.28542f,-9.10495f,-16.59733f,-19.05958f,-18.04316f,-14.91590f,-10.58486f,-5.61478f,-0.35955f},
{-5.35521f,-0.10619f,4.69597f,8.54683f,10.65268f,9.65875f,3.42678f,-9.71040f,-26.04578f,-37.63969f,-42.28407f,-42.02380f,-38.82980f,-33.88684f,-27.86664f,-21.16414f,-14.02720f,-6.62426f,0.91924f,8.49972f,16.02040f,23.37681f,30.43782f,37.01500f,42.80483f,47.26542f,49.32663f,46.69337f,34.75696f,10.85970f,-10.89085f,-20.36338f,-21.94791f,-19.71753f,-15.63466f,-10.66363f,-5.35521f},
{-166.99999f,-156.99999f,-146.99999f,-136.99999f,-126.99999f,-116.99999f,-106.99999f,-96.99999f,-86.99999f,-76.99999f,-66.99999f,-56.99999f,-46.99999f,-36.99999f,-26.99999f,-16.99999f,-6.99999f,3.00001f,13.00001f,23.00001f,33.00000f,43.00000f,53.00000f,63.00000f,73.00000f,83.00000f,93.00000f,103.00000f,113.00000f,123.00000f,133.00000f,143.00000f,153.00000f,163.00000f,173.00000f,-177.00000f,-167.00000f}
const float declination_table[19][37] = {
{148.83402f,138.83401f,128.83401f,118.83402f,108.83402f,98.83402f,88.83402f,78.83402f,68.83402f,58.83402f,48.83402f,38.83402f,28.83402f,18.83402f,8.83402f,-1.16598f,-11.16598f,-21.16598f,-31.16598f,-41.16598f,-51.16598f,-61.16598f,-71.16598f,-81.16598f,-91.16598f,-101.16598f,-111.16598f,-121.16598f,-131.16598f,-141.16598f,-151.16598f,-161.16598f,-171.16598f,178.83402f,168.83402f,158.83402f,148.83402f},
{129.09306f,116.89412f,105.78898f,95.63017f,86.23504f,77.42476f,69.04348f,60.96591f,53.09841f,45.37592f,37.75568f,30.20867f,22.70998f,15.23027f,7.73037f,0.16098f,-7.53238f,-15.40109f,-23.48496f,-31.80703f,-40.37375f,-49.18062f,-58.22152f,-67.49946f,-77.03640f,-86.88079f,-97.11212f,-107.84156f,-119.20626f,-131.35191f,-144.39481f,-158.35719f,-173.08769f,171.78274f,156.78187f,142.43310f,129.09306f},
{85.81367f,77.83602f,71.40003f,65.88433f,60.88263f,56.08204f,51.22755f,46.12774f,40.67419f,34.85457f,28.74909f,22.50583f,16.29363f,10.23812f,4.36029f,-1.45095f,-7.40778f,-13.74178f,-20.61213f,-28.04922f,-35.95530f,-44.15322f,-52.45486f,-60.71951f,-68.88732f,-76.99183f,-85.16746f,-93.67473f,-102.97950f,-113.96429f,-128.46293f,-150.36073f,175.55488f,138.00554f,112.28051f,96.48319f,85.81367f},
{48.22805f,46.81921f,45.24300f,43.71189f,42.27245f,40.80022f,39.00177f,36.49549f,32.95972f,28.26545f,22.54379f,16.18846f,9.77818f,3.88971f,-1.16066f,-5.50500f,-9.68832f,-14.40052f,-20.13990f,-26.99385f,-34.63319f,-42.50584f,-50.08478f,-57.00760f,-63.07978f,-68.20773f,-72.30336f,-75.14845f,-76.14220f,-73.56875f,-61.46573f,-19.66028f,28.39616f,43.98783f,48.35027f,49.03265f,48.22805f},
{31.42931f,31.60530f,31.28145f,30.79047f,30.38024f,30.16777f,29.97487f,29.25597f,27.27948f,23.46202f,17.66378f,10.37176f,2.68673f,-4.06559f,-9.01317f,-12.19989f,-14.47861f,-17.04830f,-21.00672f,-26.83158f,-33.98769f,-41.32654f,-47.85214f,-52.97945f,-56.36644f,-57.72068f,-56.62201f,-52.29959f,-43.64045f,-30.08348f,-13.41611f,2.35315f,14.41455f,22.53506f,27.54777f,30.28028f,31.42931f},
{22.67985f,23.21169f,23.25494f,22.99742f,22.63545f,22.42369f,22.46877f,22.43187f,21.42571f,18.33186f,12.46440f,4.21692f,-4.75068f,-12.30560f,-17.22721f,-19.74039f,-20.77499f,-21.16946f,-22.14607f,-25.39336f,-31.01102f,-37.19196f,-42.26227f,-45.34969f,-45.95366f,-43.77299f,-38.67388f,-30.72261f,-20.87969f,-11.16390f,-2.84711f,4.17466f,10.15622f,15.07307f,18.83144f,21.33469f,22.67985f},
{17.07334f,17.59062f,17.77934f,17.69577f,17.34929f,16.88674f,16.56217f,16.38656f,15.63204f,12.86517f,6.93562f,-1.75751f,-10.98436f,-18.20308f,-22.41588f,-24.27993f,-24.67590f,-23.50719f,-21.00605f,-19.72369f,-21.87960f,-26.21214f,-30.23195f,-32.25069f,-31.49417f,-28.07385f,-22.58093f,-15.67611f,-8.73630f,-3.38117f,0.43868f,3.90117f,7.49078f,10.92679f,13.85484f,15.93237f,17.07334f},
{13.37426f,13.67217f,13.78744f,13.78998f,13.54222f,12.98780f,12.38852f,11.96065f,11.09372f,8.25499f,2.21162f,-6.34723f,-14.82596f,-20.89032f,-23.89434f,-24.45752f,-23.26867f,-20.14461f,-15.21492f,-10.62227f,-9.09952f,-11.18829f,-14.91638f,-17.58833f,-17.75073f,-15.65321f,-12.03643f,-7.38772f,-2.92537f,-0.15156f,1.30857f,3.04660f,5.56513f,8.30463f,10.76538f,12.51642f,13.37426f},
{11.10969f,11.11944f,11.01236f,10.99728f,10.84097f,10.32286f,9.70494f,9.19687f,8.09157f,4.94277f,-1.12864f,-9.05484f,-16.32980f,-21.04376f,-22.55109f,-21.19496f,-17.89373f,-13.44411f,-8.58084f,-4.24867f,-1.62827f,-1.86509f,-4.55045f,-7.42469f,-8.64775f,-8.12536f,-6.32716f,-3.48850f,-0.64489f,0.72027f,0.97976f,1.90952f,4.01623f,6.51266f,8.80749f,10.44185f,11.10969f},
{9.88349f,9.72040f,9.41289f,9.38439f,9.33291f,8.89921f,8.31537f,7.68011f,6.17272f,2.60695f,-3.33977f,-10.37899f,-16.38823f,-19.72660f,-19.70115f,-16.80477f,-12.41185f,-7.91607f,-4.15177f,-1.10381f,1.22204f,1.82879f,0.23517f,-2.13784f,-3.62813f,-3.94630f,-3.34494f,-1.85961f,-0.22644f,0.26762f,-0.08803f,0.44077f,2.38763f,4.89878f,7.32401f,9.15291f,9.88349f},
{9.12464f,9.18177f,8.94457f,9.04553f,9.19618f,8.89744f,8.21223f,7.14260f,4.96349f,0.86055f,-4.97931f,-11.14357f,-15.85640f,-17.78532f,-16.57645f,-13.08478f,-8.67898f,-4.59243f,-1.60427f,0.55144f,2.39028f,3.23916f,2.31654f,0.46434f,-0.91732f,-1.53003f,-1.62589f,-1.18564f,-0.62403f,-0.86624f,-1.66562f,-1.50145f,0.21812f,2.79294f,5.56329f,7.91823f,9.12464f},
{8.06290f,8.93172f,9.29767f,9.81684f,10.30693f,10.20311f,9.30721f,7.53634f,4.41412f,-0.45099f,-6.41113f,-11.85029f,-15.25565f,-15.88209f,-13.96742f,-10.49677f,-6.46933f,-2.75504f,-0.08743f,1.66921f,3.12874f,3.96903f,3.45981f,2.05876f,0.88578f,0.20951f,-0.29117f,-0.68669f,-1.18101f,-2.27487f,-3.63054f,-3.95562f,-2.62221f,-0.08793f,3.01927f,6.01454f,8.06290f},
{6.31041f,8.41617f,9.93058f,11.21483f,12.13996f,12.23034f,11.14011f,8.66524f,4.49928f,-1.34733f,-7.71989f,-12.71272f,-15.08305f,-14.75641f,-12.45343f,-9.09705f,-5.35725f,-1.83470f,0.82823f,2.57001f,3.86519f,4.69588f,4.60243f,3.76104f,2.87601f,2.13967f,1.24833f,0.02788f,-1.62859f,-3.80231f,-5.90990f,-6.79346f,-5.87079f,-3.45535f,-0.15720f,3.34006f,6.31041f},
{4.26294f,7.59975f,10.43666f,12.70491f,14.16020f,14.43398f,13.18770f,10.10768f,4.90586f,-2.08386f,-9.15208f,-14.09196f,-15.93051f,-15.08268f,-12.51517f,-9.09089f,-5.35759f,-1.77919f,1.14514f,3.24292f,4.77489f,5.90082f,6.46683f,6.42854f,5.95440f,5.04613f,3.49019f,1.14215f,-1.96091f,-5.46021f,-8.43365f,-9.79880f,-9.13330f,-6.77311f,-3.34752f,0.50970f,4.26294f},
{2.57464f,6.81988f,10.72127f,13.95370f,16.10049f,16.72460f,15.39548f,11.64478f,5.15172f,-3.36781f,-11.49173f,-16.72206f,-18.40831f,-17.31430f,-14.50140f,-10.79977f,-6.74992f,-2.76545f,0.78143f,3.70100f,6.08013f,8.07892f,9.67963f,10.67456f,10.77058f,9.65347f,7.07600f,3.02043f,-2.09809f,-7.27683f,-11.16806f,-12.81861f,-12.13043f,-9.62141f,-5.97603f,-1.77704f,2.57464f},
{1.43503f,6.27921f,10.88019f,14.88261f,17.80063f,19.02158f,17.77297f,13.12636f,4.53718f,-6.54055f,-16.27050f,-21.85552f,-23.31365f,-21.85455f,-18.61101f,-14.36797f,-9.64057f,-4.79964f,-0.13239f,4.18385f,8.10605f,11.64519f,14.70734f,16.97990f,17.92977f,16.88820f,13.23164f,6.81146f,-1.34665f,-8.97876f,-14.01257f,-15.81005f,-14.85926f,-12.00033f,-7.99733f,-3.40265f,1.43502f},
{0.13094f,5.45479f,10.54962f,15.08235f,18.55436f,20.17754f,18.67574f,12.20150f,-0.36226f,-15.49225f,-26.46004f,-31.19380f,-31.33251f,-28.66356f,-24.33699f,-19.03627f,-13.18344f,-7.06335f,-0.88475f,5.19490f,11.05071f,16.55252f,21.50321f,25.56424f,28.16988f,28.41286f,24.95220f,16.43585f,3.63861f,-8.67079f,-16.18909f,-18.69045f,-17.68753f,-14.54612f,-10.18137f,-5.16795f,0.13094f},
{-4.14830f,1.12345f,5.96040f,9.84415f,11.94596f,10.80871f,4.02869f,-10.36405f,-27.94912f,-39.79617f,-44.16477f,-43.57950f,-40.12657f,-34.99189f,-28.83083f,-22.02403f,-14.80922f,-7.34799f,0.23881f,7.85050f,15.39197f,22.75942f,29.82156f,36.38924f,42.15868f,46.59144f,48.63821f,46.09967f,34.72041f,12.13936f,-8.98865f,-18.65098f,-20.47841f,-18.40133f,-14.39834f,-9.45818f,-4.14830f},
{-169.79948f,-159.79948f,-149.79948f,-139.79948f,-129.79948f,-119.79948f,-109.79948f,-99.79948f,-89.79948f,-79.79948f,-69.79948f,-59.79948f,-49.79948f,-39.79948f,-29.79948f,-19.79948f,-9.79948f,0.20052f,10.20052f,20.20052f,30.20052f,40.20052f,50.20052f,60.20052f,70.20052f,80.20052f,90.20052f,100.20052f,110.20052f,120.20052f,130.20052f,140.20052f,150.20052f,160.20052f,170.20052f,-179.79948f,-169.79948f}
};
#else /* !NAV_AUTO_MAG_DECLINATION_PRECISE */
#define SAMPLING_RES 10.00000f
#define SAMPLING_MIN_LON -180.00000f
#define SAMPLING_MAX_LON 180.00000f
#define SAMPLING_MIN_LAT -60.00000f
#define SAMPLING_MAX_LAT 60.00000f
static const int8_t declination_table[13][37] = {
{48,47,45,44,42,41,39,36,33,28,22,16,10,4,-1,-5,-10,-14,-20,-27,-35,-43,-50,-57,-63,-68,-72,-75,-76,-74,-62,-19,29,44,49,49,48},
{32,32,31,31,30,30,30,29,27,23,18,10,3,-4,-9,-12,-14,-17,-21,-27,-34,-42,-48,-53,-57,-58,-57,-52,-44,-30,-13,3,15,23,28,30,32},
{23,23,23,23,23,22,22,22,21,18,12,4,-5,-12,-17,-20,-21,-21,-22,-25,-31,-37,-42,-46,-46,-44,-39,-31,-21,-11,-3,4,10,15,19,21,23},
{17,18,18,18,17,17,17,16,16,13,7,-2,-11,-18,-22,-24,-24,-23,-21,-20,-22,-26,-30,-32,-31,-28,-22,-16,-9,-3,0,4,7,11,14,16,17},
{13,14,14,14,14,13,12,12,11,8,2,-7,-15,-21,-24,-24,-23,-20,-15,-10,-9,-11,-15,-18,-18,-16,-12,-7,-3,0,1,3,6,8,11,13,13},
{11,11,11,11,11,10,10,9,8,5,-1,-9,-17,-21,-23,-21,-18,-13,-8,-4,-2,-2,-5,-7,-9,-8,-6,-3,-1,1,1,2,4,6,9,11,11},
{10,10,9,9,9,9,8,8,6,2,-4,-11,-17,-20,-20,-17,-12,-8,-4,-1,1,2,0,-2,-4,-4,-3,-2,0,0,0,0,2,5,7,9,10},
{9,9,9,9,9,9,8,7,5,1,-5,-11,-16,-18,-16,-13,-8,-4,-1,1,3,3,2,0,-1,-1,-2,-1,-1,-1,-2,-2,0,3,6,8,9},
{8,9,9,10,10,10,9,7,4,-1,-7,-12,-15,-16,-14,-10,-6,-3,0,2,3,4,4,2,1,0,0,-1,-1,-2,-4,-4,-3,0,3,6,8},
{6,8,10,11,12,12,11,9,4,-1,-8,-13,-15,-15,-12,-9,-5,-2,1,3,4,5,5,4,3,2,1,0,-2,-4,-6,-7,-6,-4,0,3,6},
{4,7,10,13,14,14,13,10,5,-2,-9,-14,-16,-15,-12,-9,-5,-2,1,3,5,6,7,7,6,5,3,1,-2,-6,-9,-10,-9,-7,-3,0,4},
{2,7,11,14,16,17,15,12,5,-3,-11,-17,-18,-17,-14,-11,-6,-2,1,4,6,8,10,11,11,10,7,3,-2,-7,-11,-13,-12,-10,-6,-2,2},
{1,6,11,15,18,19,18,13,5,-6,-16,-21,-23,-21,-18,-14,-9,-4,0,4,8,12,15,17,18,17,13,7,-2,-9,-14,-16,-15,-12,-8,-4,1}
const float inclination_table[19][37] = {
{-72.02070f,-72.02071f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02071f,-72.02070f,-72.02071f},
{-78.23208f,-77.46612f,-76.54604f,-75.51344f,-74.40408f,-73.25043f,-72.08420f,-70.93779f,-69.84384f,-68.83332f,-67.93241f,-67.15960f,-66.52407f,-66.02648f,-65.66205f,-65.42519f,-65.31392f,-65.33255f,-65.49161f,-65.80511f,-66.28626f,-66.94289f,-67.77395f,-68.76776f,-69.90201f,-71.14489f,-72.45664f,-73.79090f,-75.09549f,-76.31306f,-77.38240f,-78.24183f,-78.83644f,-79.12876f,-79.10879f,-78.79618f,-78.23208f},
{-80.80007f,-78.97830f,-77.14756f,-75.29021f,-73.38193f,-71.40769f,-69.37743f,-67.33857f,-65.37896f,-63.61481f,-62.16302f,-61.10460f,-60.45316f,-60.14557f,-60.06470f,-60.08698f,-60.13217f,-60.19219f,-60.32958f,-60.65151f,-61.27183f,-62.27481f,-63.69257f,-65.50302f,-67.64474f,-70.03800f,-72.60073f,-75.25455f,-77.92173f,-80.51496f,-82.91263f,-84.88736f,-85.95658f,-85.63251f,-84.30522f,-82.60297f,-80.80007f},
{-77.45516f,-75.43606f,-73.49604f,-71.57699f,-69.59653f,-67.45749f,-65.08505f,-62.48902f,-59.81600f,-57.35291f,-55.46368f,-54.46054f,-54.44855f,-55.23685f,-56.40404f,-57.49090f,-58.18917f,-58.42457f,-58.34278f,-58.24952f,-58.51231f,-59.42525f,-61.10217f,-63.46936f,-66.34503f,-69.53469f,-72.88656f,-76.29320f,-79.66218f,-82.88130f,-85.73520f,-87.38955f,-86.36194f,-84.15583f,-81.83294f,-79.58616f,-77.45517f},
{-71.58214f,-69.61565f,-67.71488f,-65.86479f,-64.01505f,-62.04510f,-59.77482f,-57.07400f,-54.02405f,-51.03212f,-48.80491f,-48.09823f,-49.26412f,-51.92460f,-55.16562f,-58.07793f,-60.10267f,-61.01513f,-60.84749f,-59.97822f,-59.15799f,-59.18575f,-60.45940f,-62.86069f,-65.97595f,-69.36736f,-72.71868f,-75.80268f,-78.35851f,-80.06262f,-80.67983f,-80.25871f,-79.08901f,-77.46953f,-75.59576f,-73.59787f,-71.58215f},
{-64.39807f,-62.40221f,-60.40960f,-58.42549f,-56.48151f,-54.55795f,-52.49863f,-50.03954f,-47.03044f,-43.80319f,-41.39118f,-41.16169f,-43.78688f,-48.51954f,-53.83971f,-58.60413f,-62.35170f,-64.81766f,-65.60605f,-64.62579f,-62.64650f,-61.08659f,-61.05364f,-62.68316f,-65.34905f,-68.24163f,-70.78236f,-72.62117f,-73.51797f,-73.52965f,-73.02048f,-72.24528f,-71.21594f,-69.88980f,-68.25886f,-66.38361f,-64.39807f},
{-55.02826f,-52.87120f,-50.69073f,-48.44731f,-46.19513f,-44.04849f,-42.00465f,-39.74123f,-36.79801f,-33.33110f,-30.77621f,-31.26765f,-35.68330f,-42.58861f,-49.78570f,-56.05256f,-61.22645f,-65.24992f,-67.52021f,-67.43115f,-65.26549f,-62.35447f,-60.45735f,-60.49007f,-61.96843f,-63.81210f,-65.26064f,-65.90429f,-65.53604f,-64.51130f,-63.52424f,-62.77292f,-61.94296f,-60.76846f,-59.16172f,-57.17787f,-55.02826f},
{-42.23934f,-39.72248f,-37.30606f,-34.84704f,-32.29950f,-29.84715f,-27.62882f,-25.25073f,-22.02346f,-18.13398f,-15.56621f,-17.09131f,-23.52381f,-32.78684f,-42.11183f,-49.91489f,-55.93881f,-60.34177f,-62.85919f,-63.04596f,-60.93631f,-57.41244f,-54.21948f,-52.82417f,-53.10885f,-54.02972f,-54.81946f,-54.92826f,-53.94911f,-52.38029f,-51.27875f,-50.79557f,-50.20937f,-49.05107f,-47.23568f,-44.84941f,-42.23934f},
{-25.31616f,-22.26353f,-19.64432f,-17.13765f,-14.48794f,-11.90452f,-9.56551f,-6.93532f,-3.35246f,0.62534f,2.69744f,0.20162f,-7.49683f,-18.49333f,-29.80260f,-38.99555f,-45.22087f,-48.81124f,-50.28975f,-49.88122f,-47.49942f,-43.57896f,-39.80201f,-37.81589f,-37.58213f,-38.12733f,-38.79073f,-38.91636f,-37.84560f,-36.15384f,-35.28930f,-35.35164f,-35.11620f,-33.90633f,-31.71407f,-28.67497f,-25.31616f},
{-5.22365f,-1.68002f,0.98130f,3.28459f,5.73141f,8.13291f,10.33797f,12.92226f,16.28019f,19.52644f,20.64919f,17.75076f,10.26968f,-0.68527f,-12.39654f,-21.81573f,-27.55757f,-29.98547f,-30.24089f,-29.18412f,-26.62224f,-22.51726f,-18.50023f,-16.36665f,-16.02599f,-16.47452f,-17.18522f,-17.56335f,-16.82114f,-15.49276f,-15.21405f,-16.04947f,-16.38744f,-15.33409f,-12.92178f,-9.31548f,-5.22365f},
{14.65727f,18.24457f,20.73560f,22.68750f,24.74090f,26.83478f,28.82717f,31.06453f,33.64927f,35.73650f,35.91607f,33.06931f,26.82267f,17.82545f,8.14938f,0.35249f,-4.22224f,-5.65846f,-5.08244f,-3.67265f,-1.30171f,2.36894f,6.00997f,7.96495f,8.31670f,8.01335f,7.45026f,6.96852f,7.19717f,7.76654f,7.31456f,5.77666f,4.68883f,5.07867f,7.05547f,10.50432f,14.65727f},
{31.00203f,34.03206f,36.22486f,37.93041f,39.71929f,41.67672f,43.64451f,45.63057f,47.51149f,48.64179f,48.13476f,45.46014f,40.64785f,34.34474f,27.90343f,22.77471f,19.78522f,19.08118f,19.96093f,21.39191f,23.28877f,25.96005f,28.61507f,30.09860f,30.42073f,30.29938f,30.04062f,29.74069f,29.65970f,29.53545f,28.53162f,26.62276f,24.91817f,24.37385f,25.31100f,27.72542f,31.00203f},
{43.33608f,45.45851f,47.28808f,48.92364f,50.69187f,52.68920f,54.75852f,56.70372f,58.24993f,58.87748f,58.03709f,55.61656f,52.01961f,47.93440f,44.13769f,41.24459f,39.61639f,39.39065f,40.22833f,41.47069f,42.89022f,44.59331f,46.23881f,47.24171f,47.57154f,47.63935f,47.66249f,47.62919f,47.50096f,47.00810f,45.71114f,43.68014f,41.65440f,40.38784f,40.27252f,41.37670f,43.33608f},
{53.10131f,54.36161f,55.82606f,57.45873f,59.32146f,61.39014f,63.50628f,65.43734f,66.85905f,67.31408f,66.44780f,64.38450f,61.68268f,58.96721f,56.68844f,55.08044f,54.25566f,54.24982f,54.89128f,55.82780f,56.83438f,57.87845f,58.85920f,59.59271f,60.06042f,60.40887f,60.71534f,60.90716f,60.81057f,60.15911f,58.74748f,56.73559f,54.64618f,53.02448f,52.20550f,52.28132f,53.10131f},
{61.90363f,62.59082f,63.73000f,65.25974f,67.10373f,69.13404f,71.16923f,72.97878f,74.24745f,74.58969f,73.79370f,72.08739f,69.99806f,68.01780f,66.44167f,65.38391f,64.86230f,64.84287f,65.21557f,65.80372f,66.46017f,67.12970f,67.80720f,68.49179f,69.18909f,69.89166f,70.52514f,70.92481f,70.86509f,70.14456f,68.73300f,66.86155f,64.92449f,63.29260f,62.20331f,61.74779f,61.90363f},
{70.57319f,70.97549f,71.81891f,73.05700f,74.60513f,76.33503f,78.07584f,79.60158f,80.60606f,80.76720f,79.99878f,78.58008f,76.92858f,75.37566f,74.11340f,73.22087f,72.70518f,72.52979f,72.62677f,72.91213f,73.31657f,73.81370f,74.42101f,75.17239f,76.07666f,77.07532f,78.01176f,78.63384f,78.66938f,77.98598f,76.70086f,75.10445f,73.50638f,72.14451f,71.16426f,70.63412f,70.57319f},
{78.82351f,79.06440f,79.60559f,80.41612f,81.44382f,82.60991f,83.79747f,84.82323f,85.41014f,85.29919f,84.52961f,83.39228f,82.15604f,80.99185f,79.99787f,79.22370f,78.68527f,78.37555f,78.27394f,78.35635f,78.60546f,79.01788f,79.60326f,80.37283f,81.31875f,82.38760f,83.44917f,84.26774f,84.54142f,84.12292f,83.18704f,82.03917f,80.91872f,79.97342f,79.28584f,78.89722f,78.82351f},
{85.92404f,85.99807f,86.21370f,86.55320f,86.98729f,87.46743f,87.90712f,88.15645f,88.05609f,87.61718f,86.98684f,86.28678f,85.58976f,84.94098f,84.37090f,83.90045f,83.54357f,83.30879f,83.20077f,83.22179f,83.37274f,83.65354f,84.06234f,84.59369f,85.23581f,85.96691f,86.74942f,87.51716f,88.14109f,88.39226f,88.15089f,87.63861f,87.08412f,86.59723f,86.22810f,86.00055f,85.92404f},
{88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f}
};
#endif
const float intensity_table[19][37] = {
{0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f},
{0.60562f,0.59923f,0.59133f,0.58213f,0.57184f,0.56070f,0.54892f,0.53679f,0.52458f,0.51262f,0.50121f,0.49067f,0.48128f,0.47330f,0.46697f,0.46249f,0.46006f,0.45986f,0.46203f,0.46665f,0.47372f,0.48312f,0.49460f,0.50778f,0.52217f,0.53717f,0.55217f,0.56652f,0.57964f,0.59102f,0.60027f,0.60715f,0.61152f,0.61342f,0.61294f,0.61026f,0.60562f},
{0.62993f,0.61663f,0.60163f,0.58513f,0.56721f,0.54792f,0.52741f,0.50597f,0.48417f,0.46274f,0.44252f,0.42424f,0.40840f,0.39524f,0.38483f,0.37722f,0.37262f,0.37149f,0.37444f,0.38211f,0.39496f,0.41303f,0.43591f,0.46271f,0.49220f,0.52288f,0.55321f,0.58160f,0.60660f,0.62705f,0.64217f,0.65166f,0.65569f,0.65477f,0.64966f,0.64113f,0.62993f},
{0.61859f,0.59954f,0.57951f,0.55859f,0.53652f,0.51281f,0.48703f,0.45921f,0.43010f,0.40125f,0.37460f,0.35184f,0.33383f,0.32034f,0.31043f,0.30313f,0.29816f,0.29624f,0.29887f,0.30789f,0.32478f,0.35003f,0.38290f,0.42162f,0.46385f,0.50716f,0.54918f,0.58755f,0.62009f,0.64502f,0.66138f,0.66918f,0.66921f,0.66276f,0.65127f,0.63614f,0.61859f},
{0.58434f,0.56134f,0.53820f,0.51511f,0.49174f,0.46715f,0.44010f,0.40984f,0.37699f,0.34382f,0.31364f,0.28967f,0.27359f,0.26453f,0.25966f,0.25610f,0.25262f,0.24997f,0.25053f,0.25779f,0.27518f,0.30435f,0.34417f,0.39141f,0.44206f,0.49253f,0.54000f,0.58194f,0.61593f,0.64015f,0.65395f,0.65794f,0.65353f,0.64245f,0.62626f,0.60641f,0.58434f},
{0.53921f,0.51448f,0.48995f,0.46595f,0.44250f,0.41888f,0.39359f,0.36521f,0.33366f,0.30097f,0.27115f,0.24895f,0.23726f,0.23477f,0.23677f,0.23885f,0.23920f,0.23803f,0.23691f,0.23986f,0.25296f,0.28060f,0.32245f,0.37391f,0.42855f,0.48085f,0.52753f,0.56643f,0.59562f,0.61429f,0.62316f,0.62341f,0.61626f,0.60311f,0.58508f,0.56327f,0.53921f},
{0.48785f,0.46379f,0.43988f,0.41636f,0.39367f,0.37182f,0.34998f,0.32674f,0.30099f,0.27346f,0.24780f,0.22959f,0.22251f,0.22507f,0.23198f,0.23916f,0.24558f,0.25057f,0.25278f,0.25389f,0.26060f,0.28058f,0.31692f,0.36595f,0.41928f,0.46891f,0.51065f,0.54231f,0.56238f,0.57214f,0.57493f,0.57236f,0.56426f,0.55095f,0.53309f,0.51149f,0.48785f},
{0.43209f,0.41089f,0.38995f,0.36941f,0.34979f,0.33155f,0.31470f,0.29826f,0.28044f,0.26067f,0.24164f,0.22828f,0.22418f,0.22862f,0.23770f,0.24867f,0.26122f,0.27397f,0.28296f,0.28650f,0.28895f,0.29863f,0.32271f,0.36133f,0.40653f,0.44908f,0.48374f,0.50731f,0.51774f,0.51826f,0.51530f,0.51075f,0.50252f,0.48985f,0.47321f,0.45336f,0.43209f},
{0.37890f,0.36283f,0.34739f,0.33271f,0.31922f,0.30724f,0.29696f,0.28780f,0.27809f,0.26658f,0.25432f,0.24439f,0.23994f,0.24247f,0.25117f,0.26398f,0.27930f,0.29517f,0.30771f,0.31403f,0.31546f,0.31823f,0.33068f,0.35590f,0.38846f,0.42051f,0.44683f,0.46322f,0.46680f,0.46142f,0.45465f,0.44832f,0.43950f,0.42721f,0.41228f,0.39568f,0.37890f},
{0.34114f,0.33192f,0.32348f,0.31616f,0.31061f,0.30672f,0.30410f,0.30225f,0.29980f,0.29492f,0.28700f,0.27755f,0.26956f,0.26653f,0.27083f,0.28123f,0.29446f,0.30791f,0.31935f,0.32678f,0.32987f,0.33216f,0.33985f,0.35562f,0.37648f,0.39779f,0.41572f,0.42632f,0.42691f,0.42012f,0.41134f,0.40209f,0.39087f,0.37775f,0.36429f,0.35181f,0.34114f},
{0.32818f,0.32516f,0.32319f,0.32283f,0.32517f,0.32980f,0.33535f,0.34062f,0.34397f,0.34296f,0.33615f,0.32475f,0.31211f,0.30265f,0.30010f,0.30443f,0.31265f,0.32241f,0.33228f,0.34067f,0.34700f,0.35327f,0.36224f,0.37416f,0.38773f,0.40158f,0.41356f,0.42060f,0.42076f,0.41455f,0.40362f,0.38928f,0.37303f,0.35700f,0.34334f,0.33369f,0.32818f},
{0.33981f,0.34000f,0.34266f,0.34809f,0.35721f,0.36937f,0.38240f,0.39400f,0.40180f,0.40293f,0.39568f,0.38157f,0.36502f,0.35110f,0.34343f,0.34233f,0.34605f,0.35337f,0.36304f,0.37282f,0.38187f,0.39163f,0.40273f,0.41394f,0.42487f,0.43601f,0.44625f,0.45294f,0.45399f,0.44803f,0.43420f,0.41393f,0.39124f,0.37028f,0.35389f,0.34377f,0.33981f},
{0.37236f,0.37289f,0.37826f,0.38816f,0.40237f,0.41956f,0.43724f,0.45274f,0.46333f,0.46592f,0.45864f,0.44316f,0.42447f,0.40808f,0.39741f,0.39283f,0.39339f,0.39860f,0.40735f,0.41725f,0.42701f,0.43745f,0.44902f,0.46095f,0.47308f,0.48570f,0.49764f,0.50631f,0.50895f,0.50305f,0.48719f,0.46316f,0.43591f,0.41060f,0.39068f,0.37789f,0.37236f},
{0.42245f,0.42215f,0.42846f,0.44064f,0.45724f,0.47601f,0.49435f,0.50993f,0.52038f,0.52304f,0.51631f,0.50148f,0.48288f,0.46550f,0.45264f,0.44515f,0.44269f,0.44488f,0.45083f,0.45875f,0.46742f,0.47714f,0.48867f,0.50225f,0.51759f,0.53373f,0.54863f,0.55948f,0.56339f,0.55795f,0.54232f,0.51859f,0.49131f,0.46537f,0.44426f,0.42977f,0.42245f},
{0.48319f,0.48226f,0.48758f,0.49842f,0.51308f,0.52913f,0.54416f,0.55622f,0.56358f,0.56459f,0.55836f,0.54575f,0.52954f,0.51323f,0.49956f,0.48985f,0.48445f,0.48324f,0.48569f,0.49085f,0.49796f,0.50710f,0.51894f,0.53390f,0.55144f,0.56981f,0.58634f,0.59810f,0.60259f,0.59827f,0.58527f,0.56571f,0.54318f,0.52144f,0.50331f,0.49032f,0.48319f},
{0.53929f,0.53799f,0.54070f,0.54691f,0.55552f,0.56497f,0.57363f,0.58010f,0.58328f,0.58236f,0.57702f,0.56775f,0.55586f,0.54317f,0.53137f,0.52173f,0.51505f,0.51167f,0.51159f,0.51458f,0.52039f,0.52900f,0.54059f,0.55506f,0.57159f,0.58845f,0.60334f,0.61392f,0.61846f,0.61626f,0.60790f,0.59512f,0.58032f,0.56589f,0.55363f,0.54459f,0.53929f},
{0.57278f,0.57090f,0.57068f,0.57187f,0.57403f,0.57656f,0.57878f,0.58007f,0.57991f,0.57795f,0.57409f,0.56849f,0.56162f,0.55414f,0.54681f,0.54040f,0.53554f,0.53271f,0.53223f,0.53423f,0.53873f,0.54564f,0.55475f,0.56559f,0.57735f,0.58891f,0.59900f,0.60643f,0.61040f,0.61066f,0.60758f,0.60203f,0.59514f,0.58801f,0.58156f,0.57637f,0.57278f},
{0.57900f,0.57728f,0.57584f,0.57463f,0.57360f,0.57263f,0.57160f,0.57039f,0.56890f,0.56707f,0.56489f,0.56241f,0.55976f,0.55710f,0.55465f,0.55263f,0.55127f,0.55074f,0.55121f,0.55273f,0.55530f,0.55883f,0.56314f,0.56798f,0.57300f,0.57785f,0.58216f,0.58562f,0.58802f,0.58928f,0.58943f,0.58865f,0.58716f,0.58523f,0.58309f,0.58097f,0.57900f},
{0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f}
};

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

@ -122,7 +122,7 @@ bool adjustFixedWingAltitudeFromRCInput(void)
else {
// Adjusting finished - reset desired position to stay exactly where pilot released the stick
if (posControl.flags.isAdjustingAltitude) {
updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET);
updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_CURRENT);
}
return false;
}
@ -133,39 +133,20 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
{
static pt1Filter_t velzFilterState;
// On a fixed wing we might not have a reliable climb rate source (if no BARO available), so we can't apply PID controller to
// velocity error. We use PID controller on altitude error and calculate desired pitch angle
float desiredClimbRate = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros);
// Update energies
const float demSPE = (posControl.desiredState.pos.z * 0.01f) * GRAVITY_MSS;
const float demSKE = 0.0f;
const float estSPE = (navGetCurrentActualPositionAndVelocity()->pos.z * 0.01f) * GRAVITY_MSS;
const float estSKE = 0.0f;
// speedWeight controls balance between potential and kinetic energy used for pitch controller
// speedWeight = 1.0 : pitch will only control airspeed and won't control altitude
// speedWeight = 0.5 : pitch will be used to control both airspeed and altitude
// speedWeight = 0.0 : pitch will only control altitude
const float speedWeight = 0.0f; // no speed sensing for now
const float demSEB = demSPE * (1.0f - speedWeight) - demSKE * speedWeight;
const float estSEB = estSPE * (1.0f - speedWeight) - estSKE * speedWeight;
// SEB to pitch angle gain to account for airspeed (with respect to specified reference (tuning) speed
const float pitchGainInv = 1.0f / 1.0f;
// Here we use negative values for dive for better clarity
float maxClimbDeciDeg = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle);
const float minDiveDeciDeg = -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle);
// Reduce max allowed climb pitch if performing loiter (stall prevention)
if (needToCalculateCircularLoiter) {
maxClimbDeciDeg = maxClimbDeciDeg * 0.67f;
// Reduce max allowed climb rate by 2/3 if performing loiter (stall prevention)
if (needToCalculateCircularLoiter && desiredClimbRate > 0.67f * navConfig()->fw.max_auto_climb_rate) {
desiredClimbRate = 0.67f * navConfig()->fw.max_auto_climb_rate;
}
// PID controller to translate energy balance error [J] into pitch angle [decideg]
float targetPitchAngle = navPidApply3(&posControl.pids.fw_alt, demSEB, estSEB, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, 0, pitchGainInv, 1.0f);
// Here we use negative values for dive for better clarity
const float maxClimbDeciDeg = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle);
const float minDiveDeciDeg = -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle);
// PID controller to translate desired climb rate error into pitch angle [decideg]
float currentClimbRate = navGetCurrentActualPositionAndVelocity()->vel.z;
float targetPitchAngle = navPidApply2(&posControl.pids.fw_alt, desiredClimbRate, currentClimbRate, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, PID_DTERM_FROM_ERROR);
// Apply low-pass filter to prevent rapid correction
targetPitchAngle = pt1FilterApply4(&velzFilterState, targetPitchAngle, getSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicros));
@ -173,6 +154,9 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
// Reconstrain pitch angle (> 0 - climb, < 0 - dive)
targetPitchAngle = constrainf(targetPitchAngle, minDiveDeciDeg, maxClimbDeciDeg);
posControl.rcAdjustment[PITCH] = targetPitchAngle;
posControl.desiredState.vel.z = desiredClimbRate;
navDesiredVelocity[Z] = constrain(lrintf(posControl.desiredState.vel.z), -32678, 32767);
}
void applyFixedWingAltitudeAndThrottleController(timeUs_t currentTimeUs)
@ -290,9 +274,12 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
// Detemine if a circular loiter is required.
// For waypoints only use circular loiter when angular visibility is > 30 degs, otherwise head straight toward target
#define TAN_15DEG 0.26795f
needToCalculateCircularLoiter = isNavHoldPositionActive() &&
(distanceToActualTarget <= (navLoiterRadius / TAN_15DEG)) &&
(distanceToActualTarget > 50.0f);
bool loiterApproachActive = isNavHoldPositionActive() &&
distanceToActualTarget <= (navLoiterRadius / TAN_15DEG) &&
distanceToActualTarget > 50.0f;
needToCalculateCircularLoiter = loiterApproachActive || (navGetCurrentStateFlags() & NAV_CTL_HOLD);
//if vtol landing is required, fly straight to homepoint
if ((posControl.navState == NAV_STATE_RTH_HEAD_HOME) && navigationRTHAllowsLanding() && checkMixerATRequired(MIXERAT_REQUEST_LAND)){
needToCalculateCircularLoiter = false;
@ -782,8 +769,8 @@ void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs)
rcCommand[THROTTLE] = setDesiredThrottle(currentBatteryProfile->failsafe_throttle, true);
if (posControl.flags.estAltStatus >= EST_USABLE) {
// target min descent rate 10m above takeoff altitude
updateClimbRateToAltitudeController(-navConfig()->general.emerg_descent_rate, 1000.0f, ROC_TO_ALT_TARGET);
// target min descent rate at distance 2 x emerg descent rate above takeoff altitude
updateClimbRateToAltitudeController(0, 2.0f * navConfig()->general.emerg_descent_rate, ROC_TO_ALT_TARGET);
applyFixedWingAltitudeAndThrottleController(currentTimeUs);
int16_t pitchCorrection = constrain(posControl.rcAdjustment[PITCH], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle));

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

@ -63,28 +63,22 @@ static pt1Filter_t altholdThrottleFilterState;
static bool prepareForTakeoffOnReset = false;
static sqrt_controller_t alt_hold_sqrt_controller;
// Position to velocity controller for Z axis
static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
float getSqrtControllerVelocity(float targetAltitude, timeDelta_t deltaMicros)
{
float targetVel = sqrtControllerApply(
return sqrtControllerApply(
&alt_hold_sqrt_controller,
posControl.desiredState.pos.z,
targetAltitude,
navGetCurrentActualPositionAndVelocity()->pos.z,
US2S(deltaMicros)
);
// hard limit desired target velocity to max_climb_rate
float vel_max_z = 0.0f;
if (posControl.flags.isAdjustingAltitude) {
vel_max_z = navConfig()->mc.max_manual_climb_rate;
} else {
vel_max_z = navConfig()->mc.max_auto_climb_rate;
}
targetVel = constrainf(targetVel, -vel_max_z, vel_max_z);
// Position to velocity controller for Z axis
static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
{
float targetVel = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros);
posControl.pids.pos[Z].output_constrained = targetVel;
posControl.pids.pos[Z].output_constrained = targetVel; // only used for Blackbox and OSD info
// Limit max up/down acceleration target
const float smallVelChange = US2S(deltaMicros) * (GRAVITY_CMSS * 0.1f);
@ -132,8 +126,7 @@ bool adjustMulticopterAltitudeFromRCInput(void)
// In terrain follow mode we apply different logic for terrain control
if (posControl.flags.estAglStatus == EST_TRUSTED && altTarget > 10.0f) {
// We have solid terrain sensor signal - directly map throttle to altitude
updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET);
posControl.desiredState.pos.z = altTarget;
updateClimbRateToAltitudeController(0, altTarget, ROC_TO_ALT_TARGET);
}
else {
updateClimbRateToAltitudeController(-50.0f, 0, ROC_TO_ALT_CONSTANT);
@ -144,6 +137,7 @@ bool adjustMulticopterAltitudeFromRCInput(void)
}
else {
const int16_t rcThrottleAdjustment = applyDeadbandRescaled(rcCommand[THROTTLE] - altHoldThrottleRCZero, rcControlsConfig()->alt_hold_deadband, -500, 500);
if (rcThrottleAdjustment) {
// set velocity proportional to stick movement
float rcClimbRate;
@ -165,7 +159,7 @@ bool adjustMulticopterAltitudeFromRCInput(void)
else {
// Adjusting finished - reset desired position to stay exactly where pilot released the stick
if (posControl.flags.isAdjustingAltitude) {
updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET);
updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_CURRENT);
}
return false;
@ -931,8 +925,8 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
// Check if last correction was not too long ago
if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
// target min descent rate 5m above takeoff altitude
updateClimbRateToAltitudeController(-navConfig()->general.emerg_descent_rate, 500.0f, ROC_TO_ALT_TARGET);
// target min descent rate at distance 2 x emerg descent rate above takeoff altitude
updateClimbRateToAltitudeController(0, 2.0f * navConfig()->general.emerg_descent_rate, ROC_TO_ALT_TARGET);
updateAltitudeVelocityController_MC(deltaMicrosPositionUpdate);
updateAltitudeThrottleController_MC(deltaMicrosPositionUpdate);
}

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

@ -56,7 +56,7 @@
navigationPosEstimator_t posEstimator;
static float initialBaroAltitudeOffset = 0.0f;
PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 6);
PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 7);
PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
// Inertial position estimator parameters
@ -64,7 +64,6 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
.reset_altitude_type = SETTING_INAV_RESET_ALTITUDE_DEFAULT,
.reset_home_type = SETTING_INAV_RESET_HOME_DEFAULT,
.gravity_calibration_tolerance = SETTING_INAV_GRAVITY_CAL_TOLERANCE_DEFAULT, // 5 cm/s/s calibration error accepted (0.5% of gravity)
.use_gps_no_baro = SETTING_INAV_USE_GPS_NO_BARO_DEFAULT, // Use GPS altitude if no baro is available on all aircrafts
.allow_dead_reckoning = SETTING_INAV_ALLOW_DEAD_RECKONING_DEFAULT,
.max_surface_altitude = SETTING_INAV_MAX_SURFACE_ALTITUDE_DEFAULT,
@ -554,14 +553,19 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
bool correctOK = false;
//ignore baro if difference is too big, baro is probably wrong
float wBaro = 0.0f;
if (ctx->newFlags & EST_BARO_VALID) {
// Ignore baro if difference is too big, baro is probably wrong
const float gpsBaroResidual = ctx->newFlags & EST_GPS_Z_VALID ? fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt) : 0.0f;
//fade out the baro to prevent sudden jump
// Fade out the baro to prevent sudden jump
const float start_epv = positionEstimationConfig()->max_eph_epv;
const float end_epv = positionEstimationConfig()->max_eph_epv * 2.0f;
const float wBaro = scaleRangef(constrainf(gpsBaroResidual, start_epv, end_epv), start_epv, end_epv, 1.0f, 0.0f);
//use both baro and gps
if ((ctx->newFlags & EST_BARO_VALID) && (!positionEstimationConfig()->use_gps_no_baro) && (wBaro > 0.01f)) {
wBaro = scaleRangef(constrainf(gpsBaroResidual, start_epv, end_epv), start_epv, end_epv, 1.0f, 0.0f);
}
// Always use Baro if no GPS otherwise only use if accuracy OK compared to GPS
if (wBaro > 0.01f) {
timeUs_t currentTimeUs = micros();
if (!ARMING_FLAG(ARMED)) {
@ -597,6 +601,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
correctOK = true;
}
if (ctx->newFlags & EST_GPS_Z_VALID) {
// Reset current estimate to GPS altitude if estimate not valid
if (!(ctx->newFlags & EST_Z_VALID)) {

42
src/main/navigation/navigation_private.h
View file

@ -58,7 +58,7 @@ typedef enum {
} navSetWaypointFlags_t;
typedef enum {
ROC_TO_ALT_RESET,
ROC_TO_ALT_CURRENT,
ROC_TO_ALT_CONSTANT,
ROC_TO_ALT_TARGET
} climbRateToAltitudeControllerMode_e;
@ -91,6 +91,8 @@ typedef struct navigationFlags_s {
navigationEstimateStatus_e estHeadingStatus; // Indicate valid heading - wither mag or GPS at certain speed on airplane
bool gpsCfEstimatedAltitudeMismatch; // Indicates a mismatch between GPS altitude and estimated altitude
climbRateToAltitudeControllerMode_e rocToAltMode;
bool isAdjustingPosition;
bool isAdjustingAltitude;
bool isAdjustingHeading;
@ -136,6 +138,7 @@ typedef struct {
fpVector3_t pos;
fpVector3_t vel;
int32_t yaw;
int16_t climbRateDemand;
} navigationDesiredState_t;
typedef enum {
@ -163,16 +166,18 @@ typedef enum {
NAV_FSM_EVENT_STATE_SPECIFIC_3, // State-specific event
NAV_FSM_EVENT_STATE_SPECIFIC_4, // State-specific event
NAV_FSM_EVENT_STATE_SPECIFIC_5, // State-specific event
NAV_FSM_EVENT_STATE_SPECIFIC_6, // State-specific event
NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT = NAV_FSM_EVENT_STATE_SPECIFIC_1,
NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_FINISHED = NAV_FSM_EVENT_STATE_SPECIFIC_2,
NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME = NAV_FSM_EVENT_STATE_SPECIFIC_1,
NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND = NAV_FSM_EVENT_STATE_SPECIFIC_2,
NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED = NAV_FSM_EVENT_STATE_SPECIFIC_3,
NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE = NAV_FSM_EVENT_STATE_SPECIFIC_1,
NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_TRACKBACK = NAV_FSM_EVENT_STATE_SPECIFIC_2,
NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME = NAV_FSM_EVENT_STATE_SPECIFIC_3,
NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING = NAV_FSM_EVENT_STATE_SPECIFIC_1,
NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND = NAV_FSM_EVENT_STATE_SPECIFIC_1,
NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED = NAV_FSM_EVENT_STATE_SPECIFIC_2,
NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME = NAV_FSM_EVENT_STATE_SPECIFIC_3,
NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME = NAV_FSM_EVENT_STATE_SPECIFIC_4,
NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_TRACKBACK = NAV_FSM_EVENT_STATE_SPECIFIC_5,
NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE = NAV_FSM_EVENT_STATE_SPECIFIC_6,
NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT = NAV_FSM_EVENT_STATE_SPECIFIC_6,
NAV_FSM_EVENT_SWITCH_TO_RTH_LOITER_ABOVE_HOME = NAV_FSM_EVENT_STATE_SPECIFIC_4,
NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING = NAV_FSM_EVENT_STATE_SPECIFIC_5,
NAV_FSM_EVENT_COUNT,
} navigationFSMEvent_t;
@ -195,7 +200,7 @@ typedef enum {
NAV_PERSISTENT_ID_RTH_INITIALIZE = 8,
NAV_PERSISTENT_ID_RTH_CLIMB_TO_SAFE_ALT = 9,
NAV_PERSISTENT_ID_RTH_HEAD_HOME = 10,
NAV_PERSISTENT_ID_RTH_HOVER_PRIOR_TO_LANDING = 11,
NAV_PERSISTENT_ID_RTH_LOITER_PRIOR_TO_LANDING = 11,
NAV_PERSISTENT_ID_RTH_LANDING = 12,
NAV_PERSISTENT_ID_RTH_FINISHING = 13,
NAV_PERSISTENT_ID_RTH_FINISHED = 14,
@ -226,7 +231,7 @@ typedef enum {
NAV_PERSISTENT_ID_CRUISE_ADJUSTING = 34,
NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME = 35,
NAV_PERSISTENT_ID_RTH_HOVER_ABOVE_HOME = 36,
NAV_PERSISTENT_ID_RTH_LOITER_ABOVE_HOME = 36,
NAV_PERSISTENT_ID_UNUSED_4 = 37, // was NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME
NAV_PERSISTENT_ID_RTH_TRACKBACK = 38,
@ -239,6 +244,7 @@ typedef enum {
NAV_PERSISTENT_ID_FW_LANDING_GLIDE = 45,
NAV_PERSISTENT_ID_FW_LANDING_FLARE = 46,
NAV_PERSISTENT_ID_FW_LANDING_ABORT = 47,
NAV_PERSISTENT_ID_FW_LANDING_FINISHED = 48,
} navigationPersistentId_e;
typedef enum {
@ -256,8 +262,8 @@ typedef enum {
NAV_STATE_RTH_CLIMB_TO_SAFE_ALT,
NAV_STATE_RTH_TRACKBACK,
NAV_STATE_RTH_HEAD_HOME,
NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
NAV_STATE_RTH_HOVER_ABOVE_HOME,
NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING,
NAV_STATE_RTH_LOITER_ABOVE_HOME,
NAV_STATE_RTH_LANDING,
NAV_STATE_RTH_FINISHING,
NAV_STATE_RTH_FINISHED,
@ -291,6 +297,7 @@ typedef enum {
NAV_STATE_FW_LANDING_APPROACH,
NAV_STATE_FW_LANDING_GLIDE,
NAV_STATE_FW_LANDING_FLARE,
NAV_STATE_FW_LANDING_FINISHED,
NAV_STATE_FW_LANDING_ABORT,
NAV_STATE_MIXERAT_INITIALIZE,
@ -328,6 +335,7 @@ typedef enum {
NAV_AUTO_WP_DONE = (1 << 15), // Waypoint mission reached the last waypoint and is idling
NAV_MIXERAT = (1 << 16), // MIXERAT in progress
NAV_CTL_HOLD = (1 << 17), // Nav loiter active at position
} navigationFSMStateFlags_t;
typedef struct {
@ -394,7 +402,7 @@ typedef enum {
RTH_HOME_ENROUTE_INITIAL, // Initial position for RTH approach
RTH_HOME_ENROUTE_PROPORTIONAL, // Prorpotional position for RTH approach
RTH_HOME_ENROUTE_FINAL, // Final position for RTH approach
RTH_HOME_FINAL_HOVER, // Final hover altitude (if rth_home_altitude is set)
RTH_HOME_FINAL_LOITER, // Final loiter altitude (if rth_home_altitude is set)
RTH_HOME_FINAL_LAND, // Home position and altitude
} rthTargetMode_e;
@ -519,6 +527,7 @@ bool isWaypointNavTrackingActive(void);
void updateActualHeading(bool headingValid, int32_t newHeading, int32_t newGroundCourse);
void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelValid, float newX, float newY, float newVelX, float newVelY);
void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus, float gpsCfEstimatedAltitudeError);
float getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros);
bool checkForPositionSensorTimeout(void);
@ -537,10 +546,9 @@ bool adjustMulticopterHeadingFromRCInput(void);
bool adjustMulticopterPositionFromRCInput(int16_t rcPitchAdjustment, int16_t rcRollAdjustment);
void applyMulticopterNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs);
bool isMulticopterLandingDetected(void);
void calculateMulticopterInitialHoldPosition(fpVector3_t * pos);
float getSqrtControllerVelocity(float targetAltitude, timeDelta_t deltaMicros);
/* Fixed-wing specific functions */
void setupFixedWingAltitudeController(void);

11
src/main/navigation/rth_trackback.c
View file

@ -37,9 +37,10 @@
rth_trackback_t rth_trackback;
bool rthTrackBackIsActive(void)
bool rthTrackBackCanBeActivated(void)
{
return navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_ON || (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_FS && posControl.flags.forcedRTHActivated);
return posControl.flags.estPosStatus >= EST_USABLE &&
(navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_ON || (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_FS && posControl.flags.forcedRTHActivated));
}
void rthTrackBackUpdate(bool forceSaveTrackPoint)
@ -127,7 +128,7 @@ void rthTrackBackUpdate(bool forceSaveTrackPoint)
bool rthTrackBackSetNewPosition(void)
{
if (posControl.flags.estPosStatus == EST_NONE) {
return false;
return false; // will fall back to RTH initialize allowing full RTH to handle position loss correctly
}
const int32_t distFromStartTrackback = CENTIMETERS_TO_METERS(calculateDistanceToDestination(&rth_trackback.pointsList[rth_trackback.lastSavedIndex]));
@ -142,7 +143,7 @@ bool rthTrackBackSetNewPosition(void)
if (rth_trackback.activePointIndex < 0 || cancelTrackback) {
rth_trackback.WrapAroundCounter = rth_trackback.activePointIndex = -1;
posControl.flags.rthTrackbackActive = false;
return true; // Procede to home after final trackback point
return false; // No more trackback points to set, procede to home
}
if (isWaypointReached(&posControl.activeWaypoint.pos, &posControl.activeWaypoint.bearing)) {
@ -161,7 +162,7 @@ bool rthTrackBackSetNewPosition(void)
setDesiredPosition(getRthTrackBackPosition(), 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
}
return false;
return true;
}
fpVector3_t *getRthTrackBackPosition(void)

2
src/main/navigation/rth_trackback.h
View file

@ -35,7 +35,7 @@ typedef struct
extern rth_trackback_t rth_trackback;
bool rthTrackBackIsActive(void);
bool rthTrackBackCanBeActivated(void);
bool rthTrackBackSetNewPosition(void);
void rthTrackBackUpdate(bool forceSaveTrackPoint);
fpVector3_t *getRthTrackBackPosition(void);

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

@ -816,6 +816,10 @@ static int logicConditionGetFlightOperandValue(int operand) {
return getConfigProfile() + 1;
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_BATT_PROFILE: //int
return getConfigBatteryProfile() + 1;
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_MIXER_PROFILE: // int
return currentMixerProfileIndex + 1;
break;

1
src/main/programming/logic_condition.h
View file

@ -142,6 +142,7 @@ typedef enum {
LOGIC_CONDITION_OPERAND_FLIGHT_MIXER_TRANSITION_ACTIVE, //0,1 // 39
LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_YAW, // deg // 40
LOGIC_CONDITION_OPERAND_FLIGHT_FW_LAND_STATE, // 41
LOGIC_CONDITION_OPERAND_FLIGHT_BATT_PROFILE, // int // 42
} logicFlightOperands_e;
typedef enum {

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

@ -97,7 +97,7 @@ static int32_t mWhDrawn = 0; // energy (milliWatt hours) draw
batteryState_e batteryState;
const batteryProfile_t *currentBatteryProfile;
PG_REGISTER_ARRAY_WITH_RESET_FN(batteryProfile_t, MAX_BATTERY_PROFILE_COUNT, batteryProfiles, PG_BATTERY_PROFILES, 2);
PG_REGISTER_ARRAY_WITH_RESET_FN(batteryProfile_t, MAX_BATTERY_PROFILE_COUNT, batteryProfiles, PG_BATTERY_PROFILES, 3);
void pgResetFn_batteryProfiles(batteryProfile_t *instance)
{
@ -118,7 +118,6 @@ void pgResetFn_batteryProfiles(batteryProfile_t *instance)
.value = SETTING_BATTERY_CAPACITY_DEFAULT,
.warning = SETTING_BATTERY_CAPACITY_WARNING_DEFAULT,
.critical = SETTING_BATTERY_CAPACITY_CRITICAL_DEFAULT,
.unit = SETTING_BATTERY_CAPACITY_UNIT_DEFAULT,
},
.controlRateProfile = 0,
@ -167,7 +166,7 @@ void pgResetFn_batteryProfiles(batteryProfile_t *instance)
}
}
PG_REGISTER_WITH_RESET_TEMPLATE(batteryMetersConfig_t, batteryMetersConfig, PG_BATTERY_METERS_CONFIG, 1);
PG_REGISTER_WITH_RESET_TEMPLATE(batteryMetersConfig_t, batteryMetersConfig, PG_BATTERY_METERS_CONFIG, 2);
PG_RESET_TEMPLATE(batteryMetersConfig_t, batteryMetersConfig,
@ -186,6 +185,8 @@ PG_RESET_TEMPLATE(batteryMetersConfig_t, batteryMetersConfig,
.voltageSource = SETTING_BAT_VOLTAGE_SRC_DEFAULT,
.capacity_unit = SETTING_BATTERY_CAPACITY_UNIT_DEFAULT,
.cruise_power = SETTING_CRUISE_POWER_DEFAULT,
.idle_power = SETTING_IDLE_POWER_DEFAULT,
.rth_energy_margin = SETTING_RTH_ENERGY_MARGIN_DEFAULT,
@ -405,7 +406,7 @@ void batteryUpdate(timeUs_t timeDelta)
if ((currentBatteryProfile->capacity.value > 0) && batteryFullWhenPluggedIn) {
uint32_t capacityDiffBetweenFullAndEmpty = currentBatteryProfile->capacity.value - currentBatteryProfile->capacity.critical;
int32_t drawn = (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH ? mWhDrawn : mAhDrawn);
int32_t drawn = (batteryMetersConfig()->capacity_unit == BAT_CAPACITY_UNIT_MWH ? mWhDrawn : mAhDrawn);
batteryRemainingCapacity = (drawn > (int32_t)capacityDiffBetweenFullAndEmpty ? 0 : capacityDiffBetweenFullAndEmpty - drawn);
}

9
src/main/sensors/battery_config_structs.h
View file

@ -68,6 +68,8 @@ typedef struct batteryMetersConfig_s {
batVoltageSource_e voltageSource;
batCapacityUnit_e capacity_unit; // Describes unit of capacity.value, capacity.warning and capacity.critical
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
@ -90,10 +92,9 @@ typedef struct batteryProfile_s {
#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
uint32_t value; // mAh or mWh (see batteryMetersConfig()->capacity_unit)
uint32_t warning; // mAh or mWh (see batteryMetersConfig()->capacity_unit)
uint32_t critical; // mAh or mWh (see batteryMetersConfig()->capacity_unit)
} capacity;
uint8_t controlRateProfile;

16
src/main/sensors/rangefinder.c
View file

@ -130,6 +130,22 @@ static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwar
rangefinderHardware = RANGEFINDER_BENEWAKE;
rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_VIRTUAL_TASK_PERIOD_MS));
}
#endif
break;
case RANGEFINDER_USD1_V0:
#if defined(USE_RANGEFINDER_USD1_V0)
if (virtualRangefinderDetect(dev, &rangefinderUSD1Vtable)) {
rangefinderHardware = RANGEFINDER_USD1_V0;
rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_VIRTUAL_TASK_PERIOD_MS));
}
#endif
break;
case RANGEFINDER_NANORADAR:
#if defined(USE_RANGEFINDER_NANORADAR)
if (virtualRangefinderDetect(dev, &rangefinderNanoradarVtable)) {
rangefinderHardware = RANGEFINDER_NANORADAR;
rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_VIRTUAL_TASK_PERIOD_MS));
}
#endif
break;

2
src/main/sensors/rangefinder.h
View file

@ -32,6 +32,8 @@ typedef enum {
RANGEFINDER_TOF10102I2C = 7,
RANGEFINDER_FAKE = 8,
RANGEFINDER_TERARANGER_EVO = 9,
RANGEFINDER_USD1_V0 = 10,
RANGEFINDER_NANORADAR = 11,
} rangefinderType_e;
typedef struct rangefinderConfig_s {

6
src/main/target/AIRBOTF7/target.c
View file

@ -42,13 +42,13 @@ BUSDEV_REGISTER_SPI_TAG(busdev_imu1_mpu6500, DEVHW_MPU6500, GYRO_1_SPI_BUS,
#endif
timerHardware_t timerHardware[] = {
DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), // LED
DEF_TIM(TIM1, CH1, PA8, TIM_USE_ANY, 0, 0), // Cam control, SS, UNUSED
DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), //S1
DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), //S2
DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), //S3
DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), //S4
//
DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), // LED
DEF_TIM(TIM1, CH1, PA8, TIM_USE_ANY, 0, 0), // Cam control, SS, UNUSED
};
const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

15
src/main/target/ANYFCM7/target.c
View file

@ -26,12 +26,6 @@
#define TIM_EN_N TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_N_CHANNEL
timerHardware_t timerHardware[] = {
// DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, 0, 0 ), // S1_IN
// DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0 ), // S2_IN
// DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM, 0, 0 ), // S3_IN DMA2_ST2 DMA2_ST2
// DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM, 0, 0 ), // S4_IN DMA2_ST3 DMA2_ST2
// DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0, 0 ), // S5_IN DMA2_ST4 DMA2_ST2
// DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM, 0, 0 ), // S6_IN DMA2_ST7
DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S10_OUT 1 DMA1_ST7
DEF_TIM(TIM2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S6_OUT 2 DMA1_ST1
@ -39,10 +33,17 @@ timerHardware_t timerHardware[] = {
DEF_TIM(TIM2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 1 ), // S1_OUT 4 DMA1_ST7 DMA1_ST6
DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S2_OUT DMA1_ST4
DEF_TIM(TIM1, CH2N, PB0, TIM_USE_OUTPUT_AUTO, 0, 1 ), // S3_OUT DMA2_ST6 DMA2_ST2
DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO | TIM_USE_LED, 0, 0 ), // S4_OUT DMA1_ST5
DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S4_OUT DMA1_ST5 -- Used to have TIM_USE_LED
DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S7_OUT DMA1_ST2
DEF_TIM(TIM1, CH3N, PB1, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S8_OUT DMA2_ST6
DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S9_OUT DMA1_ST4
// DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, 0, 0 ), // S1_IN
// DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0 ), // S2_IN
// DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM, 0, 0 ), // S3_IN DMA2_ST2 DMA2_ST2
// DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM, 0, 0 ), // S4_IN DMA2_ST3 DMA2_ST2
// DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0, 0 ), // S5_IN DMA2_ST4 DMA2_ST2
// DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM, 0, 0 ), // S6_IN DMA2_ST7
};
const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

2
src/main/target/BLUEJAYF4/target.c
View file

@ -30,7 +30,7 @@ timerHardware_t timerHardware[] = {
DEF_TIM(TIM2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S3_OUT - DMA1_ST1
DEF_TIM(TIM9, CH2, PA3, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S4_OUT - no DMA
//DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 1 ), // S4_OUT - DMA1_ST3 (Could be DMA1_ST1 with dmaopt=0)
DEF_TIM(TIM3, CH4, PB0, TIM_USE_OUTPUT_AUTO | TIM_USE_LED, 0, 0 ), // S5_OUT - DMA1_ST2
DEF_TIM(TIM3, CH4, PB0, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S5_OUT - DMA1_ST2 -- used to be TIM_USE_LED
DEF_TIM(TIM3, CH3, PB1, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S6_OUT - DMA1_ST7
};

2
src/main/target/DALRCF405/target.c
View file

@ -26,7 +26,7 @@ timerHardware_t timerHardware[] = {
DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 (1,7)
DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 (2,2)
DEF_TIM(TIM1, CH3, PA10, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 (2,6)
DEF_TIM(TIM1, CH3, PA10, TIM_USE_OUTPUT_AUTO, 0, 1), // S3 (2,6)
DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 1), // S4 (2,1) (2.3 2.6)
DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 1), // S5 (2,4) (2.2)
DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 (1,2)

18
src/main/target/F4BY/target.c
View file

@ -6,14 +6,6 @@
#include "drivers/timer.h"
timerHardware_t timerHardware[] = {
// DEF_TIM(TIM3, CH4, PC9, TIM_USE_PPM | TIM_USE_PWM, 0, 0), // S1_IN
// DEF_TIM(TIM3, CH3, PC8, TIM_USE_PWM, 0, 0), // S2_IN
// DEF_TIM(TIM3, CH1, PC6, TIM_USE_PWM, 0, 0), // S3_IN
// DEF_TIM(TIM3, CH2, PC7, TIM_USE_PWM, 0, 0), // S4_IN
// DEF_TIM(TIM4, CH4, PD15, TIM_USE_PWM, 0, 0), // S5_IN
// DEF_TIM(TIM4, CH3, PD14, TIM_USE_PWM, 0, 0), // S6_IN
// DEF_TIM(TIM4, CH2, PD13, TIM_USE_PWM, 0, 0), // S7_IN
// DEF_TIM(TIM4, CH1, PD12, TIM_USE_PWM, 0, 0), // S8_IN
DEF_TIM(TIM2, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1_OUT
DEF_TIM(TIM2, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0), // S2_OUT
@ -25,6 +17,16 @@ timerHardware_t timerHardware[] = {
DEF_TIM(TIM1, CH4, PE14, TIM_USE_OUTPUT_AUTO, 0, 0), // S8_OUT
DEF_TIM(TIM9, CH2, PE6, TIM_USE_ANY, 0, 0), // sonar echo if needed
// DEF_TIM(TIM3, CH4, PC9, TIM_USE_PPM | TIM_USE_PWM, 0, 0), // S1_IN
// DEF_TIM(TIM3, CH3, PC8, TIM_USE_PWM, 0, 0), // S2_IN
// DEF_TIM(TIM3, CH1, PC6, TIM_USE_PWM, 0, 0), // S3_IN
// DEF_TIM(TIM3, CH2, PC7, TIM_USE_PWM, 0, 0), // S4_IN
// DEF_TIM(TIM4, CH4, PD15, TIM_USE_PWM, 0, 0), // S5_IN
// DEF_TIM(TIM4, CH3, PD14, TIM_USE_PWM, 0, 0), // S6_IN
// DEF_TIM(TIM4, CH2, PD13, TIM_USE_PWM, 0, 0), // S7_IN
// DEF_TIM(TIM4, CH1, PD12, TIM_USE_PWM, 0, 0), // S8_IN
};
const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

1
src/main/target/FLYWOOF405PRO/CMakeLists.txt
View file

@ -1 +1,2 @@
target_stm32f405xg(FLYWOOF405PRO)
target_stm32f405xg(FLYWOOF405HD)

34
src/main/target/FLYWOOF405PRO/config.c Normal file
View file

@ -0,0 +1,34 @@
/*
* 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 "fc/fc_msp_box.h"
#include "io/piniobox.h"
#include "drivers/pwm_output.h"
#include "drivers/pwm_mapping.h"
void targetConfiguration(void)
{
// Make sure S1-S4 default to Motors
timerOverridesMutable(timer2id(TIM3))->outputMode = OUTPUT_MODE_MOTORS;
timerOverridesMutable(timer2id(TIM2))->outputMode = OUTPUT_MODE_MOTORS;
}

3
src/main/target/FLYWOOF405PRO/target.c
View file

@ -24,15 +24,12 @@
timerHardware_t timerHardware[] = {
DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 D(1,3,2)
DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 D(1,0,2)
DEF_TIM(TIM2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 1), // S3 D(1,7,5)
DEF_TIM(TIM2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 D(1,2,5)
DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 D(2,4,7)
DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S6
DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S7
DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S8
DEF_TIM(TIM1, CH2, PA9, TIM_USE_LED, 0, 0), //2812LED D(1,5,3)
// DEF_TIM(TIM10, CH1, PB8, TIM_USE_PPM, 0, 0), // PPM
};

19
src/main/target/FLYWOOF405PRO/target.h
View file

@ -17,9 +17,13 @@
#pragma once
#ifdef FLYWOOF405PRO
#define TARGET_BOARD_IDENTIFIER "F4PR"
#define USBD_PRODUCT_STRING "FLYWOOF405PRO"
#else
#define TARGET_BOARD_IDENTIFIER "F4HD"
#define USBD_PRODUCT_STRING "FLYWOOF405HD"
#endif
#define LED0 PC14 //Green
#define BEEPER PC13
@ -84,11 +88,12 @@
#define SPI3_MISO_PIN PC11
#define SPI3_MOSI_PIN PC12
#ifdef FLYWOOF405PRO
#define USE_OSD
#define USE_MAX7456
#define MAX7456_SPI_BUS BUS_SPI3
#define MAX7456_CS_PIN PB14//
#define MAX7456_CS_PIN PB14
#endif
// *************** Onboard flash ********************
#define USE_FLASHFS
@ -109,6 +114,10 @@
#define UART1_TX_PIN PB6
#define UART1_RX_PIN PA10
#define USE_UART2
#define UART2_TX_PIN PD5
#define UART2_RX_PIN PD6
#define USE_UART3
#define UART3_TX_PIN PB10
#define UART3_RX_PIN PB11
@ -125,7 +134,7 @@
#define UART6_TX_PIN PC6
#define UART6_RX_PIN PC7
#define SERIAL_PORT_COUNT 6
#define SERIAL_PORT_COUNT 7
#define DEFAULT_RX_TYPE RX_TYPE_SERIAL
#define SERIALRX_PROVIDER SERIALRX_SBUS
@ -159,6 +168,6 @@
#define TARGET_IO_PORTA 0xffff
#define TARGET_IO_PORTB 0xffff
#define TARGET_IO_PORTC 0xffff
#define TARGET_IO_PORTD (BIT(2))
#define TARGET_IO_PORTD 0xffff
#define MAX_PWM_OUTPUT_PORTS 8

2
src/main/target/FLYWOOF411/target.c
View file

@ -39,7 +39,7 @@ timerHardware_t timerHardware[] = {
#else
DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 1), // S1_OUT 2,1
DEF_TIM(TIM1, CH2, PA9, TIM_USE_OUTPUT_AUTO, 0, 1), // S2_OUT 2,2
DEF_TIM(TIM1, CH3, PA10, TIM_USE_OUTPUT_AUTO, 0, 0), // S3_OUT 2,6
DEF_TIM(TIM1, CH3, PA10, TIM_USE_OUTPUT_AUTO, 0, 1), // S3_OUT 2,6
DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR , 0, 0), // S4_OUT 1,7
DEF_TIM(TIM3, CH4, PB1, TIM_USE_ANY, 0, 0), // RSSI 1,2

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

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

29
src/main/target/FLYWOOF722PRO/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 <stdint.h>
#include "platform.h"
#include "drivers/pwm_mapping.h"
#include "fc/fc_msp_box.h"
#include "io/piniobox.h"
void targetConfiguration(void)
{
pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1;
timerOverridesMutable(timer2id(TIM3))->outputMode = OUTPUT_MODE_MOTORS;
timerOverridesMutable(timer2id(TIM2))->outputMode = OUTPUT_MODE_MOTORS;
}

48
src/main/target/FLYWOOF722PRO/target.c Normal file
View file

@ -0,0 +1,48 @@
/*
* 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 <stdint.h>
#include "platform.h"
#include "drivers/bus.h"
#include "drivers/io.h"
#include "drivers/pwm_mapping.h"
#include "drivers/timer.h"
#include "drivers/pinio.h"
#include "drivers/sensor.h"
BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU6000_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_MPU6000_ALIGN);
BUSDEV_REGISTER_SPI_TAG(busdev_ICM42605, DEVHW_ICM42605, ICM42605_SPI_BUS, ICM42605_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_ICM42605_ALIGN);
BUSDEV_REGISTER_SPI_TAG(busdev_BMI270, DEVHW_BMI270, BMI270_SPI_BUS, BMI270_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_BMI270_ALIGN);
timerHardware_t timerHardware[] = {
//DEF_TIM(TIM8, CH1, PC6, TIM_USE_PPM,0, 0), // PPM&SBUS
DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 - D(1,2)
DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 - D(1,4)
DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 - D(1,6)
DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 - D(1,5)
DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 - D(2,4)
DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 - D(2,1)
DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 - D(1,0)
DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 - D(1,3)
DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED STRIP(1,5)
};
const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

159
src/main/target/FLYWOOF722PRO/target.h Normal file
View file

@ -0,0 +1,159 @@
/*
* 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/>.
*/
#define TARGET_BOARD_IDENTIFIER "FWF7"
#define USBD_PRODUCT_STRING "FLYWOOF722PRO"
/*** Indicators ***/
#define LED0 PC15
#define USE_BEEPER
#define BEEPER PC14
#define BEEPER_INVERTED
/*** SPI/I2C bus ***/
#define USE_SPI
#define USE_SPI_DEVICE_1 // Gyro 1/2
#define SPI1_SCK_PIN PA5
#define SPI1_MISO_PIN PA6
#define SPI1_MOSI_PIN PA7
#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS
#define USE_IMU_MPU6000
#define IMU_MPU6000_ALIGN CW0_DEG
#define MPU6000_SPI_BUS BUS_SPI1
#define MPU6000_CS_PIN PA4
#define USE_IMU_ICM42605
#define IMU_ICM42605_ALIGN CW270_DEG
#define ICM42605_SPI_BUS BUS_SPI1
#define ICM42605_CS_PIN PA4
#define USE_IMU_BMI270
#define IMU_BMI270_ALIGN CW180_DEG
#define BMI270_SPI_BUS BUS_SPI1
#define BMI270_CS_PIN PA4
#define USE_SPI_DEVICE_2 // MAX7456
#define SPI2_SCK_PIN PB13
#define SPI2_MISO_PIN PB14
#define SPI2_MOSI_PIN PB15
#define USE_SPI_DEVICE_3 // FLASH
#define SPI3_SCK_PIN PC10
#define SPI3_MISO_PIN PC11
#define SPI3_MOSI_PIN PB5
#define USE_I2C
#define USE_I2C_DEVICE_1 // I2C pads
#define I2C1_SCL PB8
#define I2C1_SDA PB9
/*** Onboard flash ***/
#define USE_FLASHFS
#define USE_FLASH_M25P16
#define M25P16_CS_PIN PC13
#define M25P16_SPI_BUS BUS_SPI3
/*** OSD ***/
#define USE_OSD
#define USE_MAX7456
#define MAX7456_CS_PIN PB12
#define MAX7456_SPI_BUS BUS_SPI2
/*** Serial ports ***/
#define USB_IO
#define USE_VCP
#define USE_UART1
#define UART1_TX_PIN PA9
#define UART1_RX_PIN PA10
#define USE_UART2
#define UART2_TX_PIN PA2
#define UART2_RX_PIN PA3
#define USE_UART3
#define UART3_TX_PIN PB10
#define UART3_RX_PIN PB11
#define USE_UART4
#define UART4_TX_PIN PA0
#define UART4_RX_PIN PA1
#define USE_UART5
#define UART5_TX_PIN PC12
#define UART5_RX_PIN PD2
#define USE_UART6
#define UART6_TX_PIN PC6
#define UART6_RX_PIN PC7
#define SERIAL_PORT_COUNT 7
/*** BARO & MAG ***/
#define USE_BARO
#define BARO_I2C_BUS BUS_I2C1
#define USE_BARO_BMP280
#define USE_BARO_MS5611
#define USE_BARO_DPS310
#define USE_BARO_SPL06
#define USE_MAG
#define MAG_I2C_BUS BUS_I2C1
#define USE_MAG_ALL
/*** ADC ***/
#define USE_ADC
#define ADC_CHANNEL_1_PIN PC0
#define ADC_CHANNEL_2_PIN PC1
#define ADC_CHANNEL_3_PIN PC2
#define CURRENT_METER_ADC_CHANNEL ADC_CHN_1
#define VBAT_ADC_CHANNEL ADC_CHN_2
#define RSSI_ADC_CHANNEL ADC_CHN_3
/*** PINIO ***/
#define USE_PINIO
#define USE_PINIOBOX
#define PINIO1_PIN PB0
/*** LED STRIP ***/
#define USE_LED_STRIP
#define WS2811_PIN PA8
/*** Default settings ***/
#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX)
#define CURRENT_METER_SCALE_DEFAULT 170
#define SERIALRX_UART SERIAL_PORT_USART1
#define DEFAULT_RX_TYPE RX_TYPE_SERIAL
#define SERIALRX_PROVIDER SERIALRX_SBUS
/*** Timer/PWM output ***/
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
#define MAX_PWM_OUTPUT_PORTS 8
#define USE_DSHOT
#define USE_ESC_SENSOR
/*** Used pins ***/
#define TARGET_IO_PORTA 0xffff
#define TARGET_IO_PORTB 0xffff
#define TARGET_IO_PORTC 0xffff
#define TARGET_IO_PORTD (BIT(2))

12
src/main/target/FLYWOOF745/target.c
View file

@ -30,13 +30,13 @@
#include "drivers/pinio.h"
timerHardware_t timerHardware[] = {
// DEF_TIM(TIM1, CH3, PE13, TIM_USE_PPM, 0, 1), // PPM, DMA2_ST6
DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // M1 , DMA1_ST2
DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // M2 , DMA2_ST7
DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // M3 , DMA1_ST7
// DEF_TIM(TIM1, CH3, PE13, TIM_USE_PPM, 0, 1), // PPM,
// DMA2_ST6
DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // M1 , DMA1_ST7
DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // M2 , DMA1_ST2
DEF_TIM(TIM1, CH1, PE9, TIM_USE_OUTPUT_AUTO, 0, 2), // M3 , DMA2_ST2
DEF_TIM(TIM1, CH2, PE11, TIM_USE_OUTPUT_AUTO, 0, 1), // M4 , DMA2_ST4
DEF_TIM(TIM1, CH1, PE9, TIM_USE_OUTPUT_AUTO, 0, 2), // M5 , DMA2_ST2
DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // M5 , DMA2_ST7
DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 0), // M6 , DMA1_ST1
DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // M7 , DMA1_ST4
DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // M8 , DMA1_ST5

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

@ -41,6 +41,16 @@
#define MPU6000_CS_PIN SPI4_NSS_PIN
#define MPU6000_SPI_BUS BUS_SPI4
#define USE_IMU_BMI270
#define IMU_BMI270_ALIGN CW270_DEG
#define BMI270_CS_PIN SPI4_NSS_PIN
#define BMI270_SPI_BUS BUS_SPI4
#define USE_IMU_ICM42605
#define IMU_ICM42605_ALIGN CW270_DEG
#define ICM42605_CS_PIN SPI4_NSS_PIN
#define ICM42605_SPI_BUS BUS_SPI4
#define USB_IO
#define USE_VCP
#define VBUS_SENSING_ENABLED

4
src/main/target/FOXEERF745AIO/target.c
View file

@ -25,12 +25,12 @@
timerHardware_t timerHardware[] = {
DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED STRIP - D(2, 6, 0)
DEF_TIM(TIM3, CH1, PB4, TIM_USE_MOTOR, 0, 0), // M1 - D(1, 4, 5)
DEF_TIM(TIM3, CH2, PB5, TIM_USE_MOTOR, 0, 0), // M2 - D(1, 5, 5)
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0), // M3 - D(1, 2, 5)
DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0), // M4 - D(1, 7, 5)
//
DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED STRIP - D(2, 6, 0)
};

4
src/main/target/FRSKYPILOT/target.c
View file

@ -39,13 +39,9 @@ timerHardware_t timerHardware[] = {
DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S7
DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0), // S8
DEF_TIM(TIM4, CH4, PB9, TIM_USE_OUTPUT_AUTO, 0, 0), // S9
#ifdef FRSKYPILOT_LED
DEF_TIM(TIM2, CH2, PA1, TIM_USE_LED | TIM_USE_LED, 0, 0), // S10 now LED, S11 & S12 UART 3 only
#else
DEF_TIM(TIM2, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0), // S10
DEF_TIM(TIM2, CH3, PB10, TIM_USE_OUTPUT_AUTO, 0, 0), // S11
DEF_TIM(TIM2, CH4, PB11, TIM_USE_OUTPUT_AUTO, 0, 0), // S12
#endif
DEF_TIM(TIM5, CH1, PA0, TIM_USE_BEEPER, 0, 0), // beeper
};

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

@ -0,0 +1 @@
target_stm32f745xg(GEPRCF745_BT_HD)

63
src/main/target/GEPRCF745_BT_HD/config.c Normal file
View file

@ -0,0 +1,63 @@
/*
* 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 "common/axis.h"
#include "config/config_master.h"
#include "config/feature.h"
#include "drivers/sensor.h"
#include "drivers/pwm_esc_detect.h"
#include "drivers/pwm_output.h"
#include "drivers/serial.h"
#include "fc/rc_controls.h"
#include "flight/failsafe.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "rx/rx.h"
#include "io/serial.h"
#include "sensors/battery.h"
#include "sensors/sensors.h"
#include "telemetry/telemetry.h"
#include "fc/fc_msp_box.h"
#include "io/piniobox.h"
#define BLUETOOTH_MSP_BAUDRATE BAUD_115200
void targetConfiguration(void)
{
pinioBoxConfigMutable()->permanentId[0] = BOXARM;
pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER1;
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART3)].functionMask = FUNCTION_MSP;
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART3)].msp_baudrateIndex = BLUETOOTH_MSP_BAUDRATE;
}

52
src/main/target/GEPRCF745_BT_HD/target.c Normal file
View file

@ -0,0 +1,52 @@
/*
* This file is part of INAV Project.
*
* 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 <stdint.h>
#include "platform.h"
#include "drivers/bus.h"
#include "drivers/io.h"
#include "drivers/pwm_mapping.h"
#include "drivers/timer.h"
#include "drivers/sensor.h"
BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, IMU1_SPI_BUS, IMU1_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU1_ALIGN);
BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000_2, DEVHW_MPU6000, IMU2_SPI_BUS, IMU2_CS_PIN, NONE, 1, DEVFLAGS_NONE, IMU2_ALIGN);
BUSDEV_REGISTER_SPI_TAG(busdev_icm42688, DEVHW_MPU6000, IMU1_SPI_BUS, IMU1_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU1_ALIGN);
BUSDEV_REGISTER_SPI_TAG(busdev_icm42688_2, DEVHW_ICM42605, IMU2_SPI_BUS, IMU2_CS_PIN, NONE, 1, DEVFLAGS_NONE, IMU2_ALIGN);
timerHardware_t timerHardware[] = {
DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0),
DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0),
DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0),
DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0),
DEF_TIM(TIM4, CH1, PD12, TIM_USE_OUTPUT_AUTO, 0, 0),
DEF_TIM(TIM4, CH2, PD13, TIM_USE_OUTPUT_AUTO, 0, 0),
DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0),
DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0),
DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0),
};
const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

198
src/main/target/GEPRCF745_BT_HD/target.h Normal file
View file

@ -0,0 +1,198 @@
/*
* This file is part of INAV Project.
*
* 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 "GEPR"
#define USBD_PRODUCT_STRING "GEPRCF745_BT_HD"
#define USE_TARGET_CONFIG
#define LED0 PC13
#define BEEPER PD2
#define BEEPER_INVERTED
//**************** SPI BUS *************************
#define USE_SPI
#define USE_SPI_DEVICE_1
#define SPI1_SCK_PIN PA5
#define SPI1_MISO_PIN PA6
#define SPI1_MOSI_PIN PA7
#define USE_SPI_DEVICE_2
#define SPI2_SCK_PIN PB13
#define SPI2_MISO_PIN PB14
#define SPI2_MOSI_PIN PB15
#define USE_SPI_DEVICE_3
#define SPI3_SCK_PIN PC10
#define SPI3_MISO_PIN PC11
#define SPI3_MOSI_PIN PC12
#define USE_SPI_DEVICE_4
#define SPI4_SCK_PIN PE2
#define SPI4_MISO_PIN PE5
#define SPI4_MOSI_PIN PE6
// *************** Gyro & ACC **********************
#define USE_DUAL_GYRO
#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS
#define USE_IMU_MPU6000
#define USE_IMU_ICM42605
// *****IMU1 MPU6000 & ICM42688 ON SPI1 **************
#define IMU1_ALIGN CW180_DEG
#define IMU1_SPI_BUS BUS_SPI1
#define IMU1_CS_PIN PA4
// *****IMU2 MPU6000 & ICM42688 ON SPI2 **************
#define IMU2_ALIGN CW0_DEG
#define IMU2_SPI_BUS BUS_SPI2
#define IMU2_CS_PIN PB12
// *************** I2C/Baro/Mag *********************
#define USE_I2C
#define USE_I2C_DEVICE_1
#define I2C1_SCL PB8
#define I2C1_SDA PB9
#define USE_BARO
#define BARO_I2C_BUS BUS_I2C1
#define USE_BARO_BMP280
#define USE_BARO_DPS310
#define USE_BARO_MS5611
#define USE_MAG
#define MAG_I2C_BUS BUS_I2C1
#define USE_MAG_HMC5883
#define USE_MAG_QMC5883
#define USE_MAG_IST8310
#define USE_MAG_IST8308
#define USE_MAG_MAG3110
#define USE_MAG_LIS3MDL
#define TEMPERATURE_I2C_BUS BUS_I2C1
#define PITOT_I2C_BUS BUS_I2C1
#define USE_RANGEFINDER
#define RANGEFINDER_I2C_BUS BUS_I2C1
#define BNO055_I2C_BUS BUS_I2C1
// *************** FLASH **************************
#define USE_SDCARD
#define USE_SDCARD_SPI
#define SDCARD_SPI_BUS BUS_SPI3
#define SDCARD_CS_PIN PA15
#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
// ************PINIO to disable BT*****************
#define USE_PINIO
#define USE_PINIOBOX
#define PINIO1_PIN PE13
#define PINIO1_FLAGS PINIO_FLAGS_INVERTED
// ************PINIO to other
#define PINIO2_PIN PC14 //Enable vsw
// *************** OSD *****************************
#define USE_MAX7456
#define MAX7456_SPI_BUS BUS_SPI4
#define MAX7456_CS_PIN PE4
// *************** 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_UART6
#define UART6_RX_PIN PC7
#define UART6_TX_PIN PC6
#define USE_UART7
#define UART7_RX_PIN PE7
#define UART7_TX_PIN PE8
#define USE_UART8
#define UART8_RX_PIN PE0
#define UART8_TX_PIN PE1
#define SERIAL_PORT_COUNT 8
#define DEFAULT_RX_TYPE RX_TYPE_SERIAL
#define SERIALRX_PROVIDER SERIALRX_SBUS
#define SERIALRX_UART SERIAL_PORT_USART2
// *************** ADC *****************************
#define USE_ADC
#define ADC_INSTANCE ADC1
#define ADC1_DMA_STREAM DMA2_Stream0
#define ADC_CHANNEL_1_PIN PC3
#define ADC_CHANNEL_2_PIN PC5
#define ADC_CHANNEL_3_PIN PC2
#define VBAT_ADC_CHANNEL ADC_CHN_1
#define RSSI_ADC_CHANNEL ADC_CHN_2
#define CURRENT_METER_ADC_CHANNEL ADC_CHN_3
#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX)
#define USE_LED_STRIP
#define WS2811_PIN PA8
#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 TARGET_IO_PORTE 0xffff
#define MAX_PWM_OUTPUT_PORTS 8
#define USE_DSHOT
#define USE_ESC_SENSOR

2
src/main/target/HAKRCF405V2/target.c
View file

@ -26,7 +26,7 @@ timerHardware_t timerHardware[] = {
DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S2_OUT D2_ST7
DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 1), // S3_OUT D2_ST1
DEF_TIM(TIM1, CH2, PA9, TIM_USE_OUTPUT_AUTO, 0, 1), // S4_OUT D2_ST2
DEF_TIM(TIM1, CH3, PA10, TIM_USE_OUTPUT_AUTO, 0, 0), // S5_OUT D2_ST6
DEF_TIM(TIM1, CH3, PA10, TIM_USE_OUTPUT_AUTO, 0, 1), // S5_OUT D2_ST6
DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S6_OUT D1_ST4
DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S7_OUT D1_ST7
DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S8_OUT D1_ST2

2
src/main/target/HAKRCF722V2/target.c
View file

@ -39,7 +39,7 @@ timerHardware_t timerHardware[] = {
DEF_TIM(TIM1, CH2, PA9, TIM_USE_OUTPUT_AUTO, 0, 1), // S4
DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S5
DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S6
DEF_TIM(TIM1, CH3, PA10, TIM_USE_OUTPUT_AUTO, 0, 0), // S7
DEF_TIM(TIM1, CH3, PA10, TIM_USE_OUTPUT_AUTO, 0, 1), // S7
DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S8
DEF_TIM(TIM2, CH2, PB3, TIM_USE_LED, 0, 0), // 2812LED

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

@ -0,0 +1 @@
target_stm32f405xg(JHEF405PRO SKIP_RELEASES)

34
src/main/target/JHEF405PRO/target.c Normal file
View file

@ -0,0 +1,34 @@
/*
* 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 <stdbool.h>
#include <platform.h>
#include "drivers/io.h"
#include "drivers/pwm_mapping.h"
#include "drivers/timer.h"
timerHardware_t timerHardware[] = {
DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0), // S1
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0), // S2
DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, 0, 1), // S3
DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, 0, 0), // S4
DEF_TIM(TIM4, CH3, PB8, TIM_USE_PPM, 0, 0), // PPM
DEF_TIM(TIM1, CH2, PA9, TIM_USE_LED, 0, 1 ), //LED
};
const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

154
src/main/target/JHEF405PRO/target.h Normal file
View file

@ -0,0 +1,154 @@
/*
* This is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This software is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#define TARGET_BOARD_IDENTIFIER "JHEF405PRO"
#define USBD_PRODUCT_STRING "JHEF405PRO"
#define LED0 PC14
#define BEEPER PC13
#define BEEPER_INVERTED
// *************** SPI1 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 USE_IMU_ICM42605
#define IMU_ICM42605_ALIGN CW90_DEG
#define ICM42605_CS_PIN PB12
#define ICM42605_SPI_BUS BUS_SPI1
#define USE_EXTI
#define GYRO_INT_EXTI PB13
#define USE_MPU_DATA_READY_SIGNAL
// *************** SPI3 OSD & Flash ***************************
#define USE_SPI_DEVICE_3
#define SPI3_SCK_PIN PC10
#define SPI3_MISO_PIN PC11
#define SPI3_MOSI_PIN PC12
// OSD
#define USE_OSD
#define USE_MAX7456
#define MAX7456_SPI_BUS BUS_SPI3
#define MAX7456_CS_PIN PB14
// Flash
#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
#define USE_FLASHFS
#define USE_FLASH_M25P16
#define M25P16_CS_PIN PB3
#define M25P16_SPI_BUS BUS_SPI3
// *************** I2C /Baro/Mag *********************
#define USE_I2C
#define USE_I2C_DEVICE_1
#define I2C1_SCL PB8
#define I2C1_SDA PB9
#define DEFAULT_I2C_BUS BUS_I2C1
// Baro
#define USE_BARO
#define BARO_I2C_BUS DEFAULT_I2C_BUS
#define USE_BARO_DPS310
// Mag
#define USE_MAG
#define USE_MAG_ALL
#define MAG_I2C_BUS DEFAULT_I2C_BUS
// Temperature
#define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS
// Range finder
#define USE_RANGEFINDER
#define USE_RANGEFINDER_HCSR04_I2C
#define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS
// PITOT
#define PITOT_I2C_BUS DEFAULT_I2C_BUS
// *************** UART *****************************
#define USB_IO
#define USE_VCP
#define VBUS_SENSING_PIN PA8 // TODO validate
#define VBUS_SENSING_ENABLED
#define USE_UART1
#define UART1_RX_PIN PA10
#define UART1_TX_PIN PB6
#define USE_UART2
#define UART2_RX_PIN PD6
#define UART2_TX_PIN PD5
#define USE_UART3
#define UART3_RX_PIN PB11
#define UART3_TX_PIN PB10
#define USE_UART4
#define UART4_RX_PIN PA1
#define UART4_TX_PIN PA0
#define USE_UART6
#define UART6_RX_PIN PC7
#define UART6_TX_PIN PC6
#define SERIAL_PORT_COUNT 6
// *************** ADC ***************************
#define USE_ADC
#define ADC_INSTANCE ADC1
#define ADC1_DMA_STREAM DMA2_Stream0
#define ADC_CHANNEL_1_PIN PC3
#define ADC_CHANNEL_2_PIN PC2
#define ADC_CHANNEL_3_PIN PC0
#define VBAT_ADC_CHANNEL ADC_CHN_1
#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2
#define RSSI_ADC_CHANNEL ADC_CHN_3
#define CURRENT_METER_SCALE 250
// *************** OTHERS *************************
#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_CURRENT_METER | FEATURE_OSD | FEATURE_TELEMETRY)
// Rx defaults
#define DEFAULT_RX_TYPE RX_TYPE_SERIAL
#define SERIALRX_PROVIDER SERIALRX_CRSF
#define SERIALRX_UART SERIAL_PORT_USART2
// *************** LED2812 ************************
#define USE_LED_STRIP
#define WS2811_PIN PA9
#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
//TIMER
#define MAX_PWM_OUTPUT_PORTS 4
#define USE_DSHOT

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

@ -57,7 +57,4 @@ void targetConfiguration(void)
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].functionMask = FUNCTION_MSP;
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].msp_baudrateIndex = BAUD_115200;
// To improve backwards compatibility with INAV versions 6.x and older
timerOverridesMutable(timer2id(TIM3))->outputMode = OUTPUT_MODE_MOTORS;
}

2
src/main/target/MATEKF405/target.c
View file

@ -29,7 +29,7 @@ timerHardware_t timerHardware[] = {
DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 1), // S3 UP(2,1)
DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 UP(2,1)
DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO | TIM_USE_LED , 0, 0), // S5 UP(1,7)
DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 UP(1,7) -- LED
DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 UP(2,5)
DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 D(1,7)!S5 UP(2,6)

6
src/main/target/NEUTRONRCF435MINI/target.c
View file

@ -26,13 +26,13 @@
timerHardware_t timerHardware[] = {
DEF_TIM(TMR1, CH1, PA8, TIM_USE_ANY |TIM_USE_LED, 0,6), // PWM1 - LED MCO1 DMA1 CH2
DEF_TIM(TMR4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0,0), // motor1 DMA2 CH7
DEF_TIM(TMR4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0,2), // motor2 DMA2 CH6
DEF_TIM(TMR2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0,1), // motor3 DMA2 CH5
DEF_TIM(TMR3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0,3), // motor4 DMA2 CH4
//
DEF_TIM(TMR1, CH1, PA8, TIM_USE_ANY |TIM_USE_LED, 0,6), // PWM1 - LED MCO1 DMA1 CH2
//
};
const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

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