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

Merge remote-tracking branch 'origin/development' into dzikuvx-bno055-secondary-imu

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2019-12-09 17:30:12 +01:00
commit ce8f5c7327
140 changed files with 5848 additions and 1163 deletions
Download patch file Download diff file Expand all files Collapse all files

1
.travis.yml
View file

@ -8,6 +8,7 @@ env:
- GOAL=targets-group-5 - GOAL=targets-group-5
- GOAL=targets-group-6 - GOAL=targets-group-6
- GOAL=targets-group-7 - GOAL=targets-group-7
- GOAL=targets-group-8
- GOAL=targets-group-rest - GOAL=targets-group-rest
# use new docker environment # use new docker environment

15
Makefile
View file

@ -126,14 +126,15 @@ else
$(error Unknown target MCU specified.) $(error Unknown target MCU specified.)
endif endif
GROUP_1_TARGETS := ALIENFLIGHTF3 ALIENFLIGHTF4 AIRHEROF3 AIRHEROF3_QUAD COLIBRI_RACE LUX_RACE SPARKY REVO SPARKY2 COLIBRI KISSFC FALCORE FF_F35_LIGHTNING FF_FORTINIF4 FF_PIKOF4 FF_PIKOF4OSD GROUP_1_TARGETS := AIRHEROF3 AIRHEROF3_QUAD LUX_RACE SPARKY REVO SPARKY2 COLIBRI FALCORE FF_F35_LIGHTNING FF_FORTINIF4 FF_PIKOF4 FF_PIKOF4OSD
GROUP_2_TARGETS := SPRACINGF3 SPRACINGF3EVO SPRACINGF3EVO_1SS SPRACINGF3MINI SPRACINGF4EVO CLRACINGF4AIR CLRACINGF4AIRV2 BEEROTORF4 BETAFLIGHTF3 BETAFLIGHTF4 PIKOBLX GROUP_2_TARGETS := SPRACINGF3 SPRACINGF3EVO SPRACINGF3EVO_1SS SPRACINGF3MINI SPRACINGF4EVO CLRACINGF4AIR CLRACINGF4AIRV2 BEEROTORF4 BETAFLIGHTF3 BETAFLIGHTF4 PIKOBLX
GROUP_3_TARGETS := OMNIBUS AIRBOTF4 BLUEJAYF4 OMNIBUSF4 OMNIBUSF4PRO FIREWORKSV2 SPARKY2 MATEKF405 OMNIBUSF7 DYSF4PRO OMNIBUSF4PRO_LEDSTRIPM5 OMNIBUSF7NXT OMNIBUSF7V2 ASGARD32F4 GROUP_3_TARGETS := OMNIBUS AIRBOTF4 BLUEJAYF4 OMNIBUSF4 OMNIBUSF4PRO FIREWORKSV2 SPARKY2 MATEKF405 OMNIBUSF7 DYSF4PRO OMNIBUSF4PRO_LEDSTRIPM5 OMNIBUSF7NXT OMNIBUSF7V2 ASGARD32F4
GROUP_4_TARGETS := ANYFC ANYFCF7 ANYFCF7_EXTERNAL_BARO ANYFCM7 ALIENFLIGHTNGF7 PIXRACER YUPIF4 YUPIF4MINI YUPIF4R2 YUPIF7 MATEKF405SE MATEKF411 MATEKF722 MATEKF405OSD MATEKF405_SERVOS6 NOX GROUP_4_TARGETS := ANYFC ANYFCF7 ANYFCF7_EXTERNAL_BARO ALIENFLIGHTNGF7 PIXRACER YUPIF7 MATEKF405SE MATEKF411 MATEKF722 MATEKF405OSD MATEKF405_SERVOS6 NOX
GROUP_5_TARGETS := ASGARD32F7 CHEBUZZF3 CLRACINGF4AIRV3 DALRCF405 DALRCF722DUAL DYSF4PROV2 F4BY FISHDRONEF4 FOXEERF405 FOXEERF722DUAL FRSKYF3 FRSKYF4 FURYF3 FURYF3_SPIFLASH FURYF4OSD GROUP_5_TARGETS := ASGARD32F7 CLRACINGF4AIRV3 DALRCF405 DALRCF722DUAL DYSF4PROV2 F4BY FISHDRONEF4 FOXEERF405 FOXEERF722DUAL FRSKYF3 FRSKYF4 FURYF3 FURYF3_SPIFLASH FURYF4OSD
GROUP_6_TARGETS := MAMBAF405 OMNIBUSF4V3 OMNIBUSF4V3_S6_SS OMNIBUSF4V3_S5S6_SS OMNIBUSF4V3_S5_S6_2SS AIKONF4 GROUP_6_TARGETS := MAMBAF405 OMNIBUSF4V3 OMNIBUSF4V3_S6_SS OMNIBUSF4V3_S5S6_SS OMNIBUSF4V3_S5_S6_2SS AIKONF4
GROUP_7_TARGETS := KAKUTEF4 KAKUTEF4V2 KAKUTEF7 KAKUTEF7MINI KFC32F3_INAV KROOZX MATEKF411_RSSI MATEKF411_SFTSRL2 MATEKF722MINI MATEKF722SE MATEKF722_HEXSERVO GROUP_7_TARGETS := KAKUTEF4 KAKUTEF4V2 KAKUTEF7 KAKUTEF7MINI KFC32F3_INAV MATEKF411_RSSI MATEKF411_SFTSRL2 MATEKF722MINI MATEKF722SE MATEKF722_HEXSERVO
GROUP_OTHER_TARGETS := $(filter-out $(GROUP_1_TARGETS) $(GROUP_2_TARGETS) $(GROUP_3_TARGETS) $(GROUP_4_TARGETS) $(GROUP_5_TARGETS) $(GROUP_6_TARGETS) $(GROUP_7_TARGETS), $(VALID_TARGETS)) GROUP_8_TARGETS := MATEKF765
GROUP_OTHER_TARGETS := $(filter-out $(GROUP_1_TARGETS) $(GROUP_2_TARGETS) $(GROUP_3_TARGETS) $(GROUP_4_TARGETS) $(GROUP_5_TARGETS) $(GROUP_6_TARGETS) $(GROUP_7_TARGETS) $(GROUP_8_TARGETS), $(VALID_TARGETS))
REVISION = $(shell git rev-parse --short HEAD) REVISION = $(shell git rev-parse --short HEAD)
@ -415,6 +416,9 @@ targets-group-6: $(GROUP_6_TARGETS)
## targets-group-7 : build some targets ## targets-group-7 : build some targets
targets-group-7: $(GROUP_7_TARGETS) targets-group-7: $(GROUP_7_TARGETS)
## targets-group-8 : build some targets
targets-group-8: $(GROUP_8_TARGETS)
## targets-group-rest: build the rest of the targets (not listed in group 1, 2 or 3) ## targets-group-rest: build the rest of the targets (not listed in group 1, 2 or 3)
targets-group-rest: $(GROUP_OTHER_TARGETS) targets-group-rest: $(GROUP_OTHER_TARGETS)
@ -510,6 +514,7 @@ targets:
$(V0) @echo "targets-group-5: $(GROUP_5_TARGETS)" $(V0) @echo "targets-group-5: $(GROUP_5_TARGETS)"
$(V0) @echo "targets-group-6: $(GROUP_6_TARGETS)" $(V0) @echo "targets-group-6: $(GROUP_6_TARGETS)"
$(V0) @echo "targets-group-7: $(GROUP_7_TARGETS)" $(V0) @echo "targets-group-7: $(GROUP_7_TARGETS)"
$(V0) @echo "targets-group-7: $(GROUP_8_TARGETS)"
$(V0) @echo "targets-group-rest: $(GROUP_OTHER_TARGETS)" $(V0) @echo "targets-group-rest: $(GROUP_OTHER_TARGETS)"
$(V0) @echo "Release targets: $(RELEASE_TARGETS)" $(V0) @echo "Release targets: $(RELEASE_TARGETS)"

1
build_docs.sh
View file

@ -29,7 +29,6 @@ doc_files=(
'Migrating from baseflight.md' 'Migrating from baseflight.md'
'Boards.md' 'Boards.md'
'Board - AlienFlight.md' 'Board - AlienFlight.md'
'Board - ChebuzzF3.md'
'Board - ColibriRace.md' 'Board - ColibriRace.md'
'Board - Motolab.md' 'Board - Motolab.md'
'Board - Paris Air Hero 32.md' 'Board - Paris Air Hero 32.md'

2
docs/1wire.md
View file

@ -58,7 +58,7 @@ The following parameters can be used to enable and configure this in the related
``` ```
// Define your esc hardware // Define your esc hardware
#if defined(STM32F3DISCOVERY) && !(defined(CHEBUZZF3)) #if defined(STM32F3DISCOVERY)
const escHardware_t escHardware[ESC_COUNT] = { const escHardware_t escHardware[ESC_COUNT] = {
{ GPIOD, 12 }, { GPIOD, 12 },
{ GPIOD, 13 }, { GPIOD, 13 },

7
docs/Board - ChebuzzF3.md
View file

@ -1,10 +1,3 @@
# Board - ChebuzzF3
The ChebuzzF3 is a daugter board which connects to the bottom of an STM32F3Discovery board and provides pin headers and ports for various FC connections.
All connections were traced using a multimeter and then verified against the TauLabs source code using the revision linked.
https://github.com/TauLabs/TauLabs/blob/816760dec2a20db7fb9ec1a505add240e696c31f/flight/targets/flyingf3/board-info/board_hw_defs.c
## Connections ## Connections

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

@ -23,7 +23,7 @@
* `MATEKF411` if you want to control LEDs and have SS1 TX on ST1 pad. * `MATEKF411` if you want to control LEDs and have SS1 TX on ST1 pad.
* `MATEKF411_FD_SFTSRL` if you need the softserial to be full-duplex (TX = ST1 pad, RX = LED pad), at the cost of losing the LED output. * `MATEKF411_FD_SFTSRL` if you need the softserial to be full-duplex (TX = ST1 pad, RX = LED pad), at the cost of losing the LED output.
* `MATEKF411_RSSI` if you want to have analog RSSI input on ST1 pad. SS1 TX will be available on the LED pad. * `MATEKF411_RSSI` if you want to have analog RSSI input on ST1 pad. SS1 TX will be available on the LED pad.
* `MATEKF411_SFTSRL2`if you want to use two softserials (TX only) at the same time. Eg. Smart Audio + S. Port, Smart Audio + LTM * `MATEKF411_SFTSRL2`if you want to use two softserials (TX only) at the same time. Eg. Smart Audio + S. Port, Smart Audio + LTM. (SS1 TX will be on ST1 pad and SS2 TX will be on LED pad).
## Where to buy: ## Where to buy:

18
docs/Board - MatekF405.md
View file

@ -75,8 +75,22 @@ I2C requires that the WS2812 led strip is moved to S5, thus WS2812 is not usable
Soft serial is available as an alternative to a hardware UART on RX4/TX4 and TX2. By default this is NOT inverted. In order to use this feature: Soft serial is available as an alternative to a hardware UART on RX4/TX4 and TX2. By default this is NOT inverted. In order to use this feature:
* Enable soft serial * Enable soft serial
* Do not assign any function to hardware UART4 or UART2-TX * Do not assign any function to hardware UART4
* Assign the desired function to the soft-serial port * Assign the desired function to the soft-serial ports
* UART4 TX/RX pads will be used as SoftSerial 1 TX/RX pads
* UART2 TX pad will be used as SoftSerial 2 TX pad
RX2 and SBUS pads can be used as normal for receiver-only UART. If you need a full duplex UART (IE: TBS Crossfire) and SoftSerial, then use UART 1, 3 or 5 for Full Duplex.
#### Common scenarios for SoftSerial on this boards:
You need to wire a FrSky receiver (SBUS and SmartPort) to the Flight controller
* Connect SBUS from Receiver to SBUS pin on the Flight Controller
* Connect SmartPort from Receiver to TX2 pad on the Flight Controller
* Enable SoftSerial and set UART2 for Serial RX and SoftSerial 2 for SmartPort Telemetry
You need to wire a SmartAudio or Trump VTX to the Flight controller
* Connect SmartAudio/Tramp from VTX to the TX4 pad on the Flight Controller
* Enable SoftSerial and set SoftSerial 1 for SmartAudio or Tramp
### USB ### USB

28
docs/Board - MatekF765-Wing.md Normal file
View file

@ -0,0 +1,28 @@
# Board - [MATEKSYS F765-Wing](https://inavflight.com/shop/s/bg/1557661)
![Matek F765 Wing](http://www.mateksys.com/wp-content/uploads/2019/08/F765-WING_2.jpg)
## Specification:
* STM32F765 216MHz CPU
* 2-8S LiPo support
* 132A current sensor
* OSD
* BMP280 barometer
* SD slot
* 7 UART serial ports
* 2 I2C
* PDB for 2 motors
* 12 outputs (10 DSHOT)
* 8A BEC for the servos
* 9V supply for FPV gear
* Dual camera switcher
## Details
* For Matek F765-WING use `MATEKF765` firmware.
* No need for SBUS inversion
## Where to buy:
* [Banggood](https://inavflight.com/shop/s/bg/1557661)

51
docs/Boards.md
View file

@ -1,20 +1,17 @@
# Flight controller hardware # Flight controller hardware
The current focus is geared towards flight controller hardware that use the STM32F3, STM32F4, STM32F7 series processors. The core logic is separated from the hardware drivers, porting to other processors is possible. ### Sponsored and recommended boards
Current list of supported boards can be obtained using [INAV release page](https://github.com/iNavFlight/inav/releases) These boards come from companies that support INAV development. Buying one of these boards you make your small contribution for improving INAV as well.
### Boards based on F1 CPUs Also these boards are tested by INAV development team and usually flown on daily basis.
These boards are no longer supported including Naze32, CC3D and all derivatives. | Board name | CPU Family | Target name(s) | GPS | Compass | Barometer | Telemetry | RX | Blackbox |
|---------------------------|:----------:|:-------------------------:|:----:|:-------:|:--------------:|:---------:|:------------------------------:|:--------------------:|
| [Airbot OMNIBUS F4 PRO](https://inavflight.com/shop/p/OMNIBUSF4PRO)| F4 | OMNIBUSF4PRO | All | All | All | All | All | SERIAL, SD |
| [Airbot OMNIBUS F4](https://inavflight.com/shop/s/bg/1319176)| F4 | OMNIBUSF4 | All | All | All | All | All | SERIAL, SD |
### Boards based on F3 CPUs Note: In the above and following tables, the sensor columns indicates firmware support for the sensor category; it does not necessarily mean there is an on-board sensor.
These boards are supported, but not recommeneded for modern setups.
### Boards based on F4/F7 CPUs
These boards are powerfull and in general support everything INAV is capable of. Limitations are quite rare and are usually caused by hardware design issues.
### Recommended boards ### Recommended boards
@ -22,7 +19,31 @@ These boards are well tested with INAV and are known to be of good quality and r
| Board name | CPU Family | Target name(s) | GPS | Compass | Barometer | Telemetry | RX | Blackbox | | Board name | CPU Family | Target name(s) | GPS | Compass | Barometer | Telemetry | RX | Blackbox |
|---------------------------|:----------:|:-------------------------:|:----:|:-------:|:--------------:|:---------:|:------------------------------:|:--------------------:| |---------------------------|:----------:|:-------------------------:|:----:|:-------:|:--------------:|:---------:|:------------------------------:|:--------------------:|
| [PARIS Sirius™ AIR3](http://www.multiwiicopter.com/products/inav-air3-fixed-wing) | F3 | AIRHEROF3, AIRHEROF3_QUAD | All | All | All | All | All | SERIAL | | [Matek F405-CTR](https://inavflight.com/shop/p/MATEKF405CTR) | F4 | MATEKF405 | All | All | All | All | All | SERIAL, SD |
| [Airbot OMNIBUS AIO F3](http://shop.myairbot.com/index.php/flight-control/cleanflight-baseflight/omnibusv11.html) | F3 | OMNIBUS | All | All | All | All | All | SERIAL, SD | | [Matek F405-STD](https://inavflight.com/shop/p/MATEKF405STD) | F4 | MATEKF405 | All | All | All | All | All | SERIAL, SD |
| [Airbot OMNIBUS AIO F4](http://shop.myairbot.com/index.php/flight-control/cleanflight-baseflight/omnibusf4v2.html)| F4 | OMNIBUSF4, OMNIBUSF4PRO | All | All | All | All | All | SERIAL, SD, SPIFLASH | | [Matek F405-WING](https://inavflight.com/shop/p/MATEKF405WING) | F4 | MATEKF405SE | All | All | All | All | All | SERIAL, SD |
| [Airbot F4 / Flip F4](http://shop.myairbot.com/index.php/flight-control/apm/airbotf4v1.html) | F4 | AIRBOTF4 | All | All | All | All | All | SERIAL, SPIFLASH | | [Matek F722 WING](https://inavflight.com/shop/p/MATEKF722WING) | F7 | MATEKF722SE | All | All | All | All | All | SERIAL, SD |
| [Matek F722-SE](https://inavflight.com/shop/p/MATEKF722SE) | F7 | MATEKF722SE | All | All | All | All | All | SERIAL, SD |
| [Matek F722-STD](https://inavflight.com/shop/p/MATEKF722STD) | F7 | MATEKF722 | All | All | All | All | All | SERIAL, SD |
| [Matek F722-MINI](https://inavflight.com/shop/p/MATEKF722MINI) | F7 | MATEKF722SE | All | All | All | All | All | SPIFLASH |
It's possible to find more supported and tested boards [here](https://github.com/iNavFlight/inav/wiki/Welcome-to-INAV,-useful-links-and-products)
### Boards documentation
See the [docs](https://github.com/iNavFlight/inav/tree/master/docs) folder for additional information regards to many targets in INAV, to example help in finding pinout and features. _Feel free to help improve the docs._
### Boards based on F4/F7 CPUs
These boards are powerful and in general support everything INAV is capable of. Limitations are quite rare and are usually caused by hardware design issues.
### Boards based on F3 CPUs
Boards based on F3 boards will be supported for as long as practical, sometimes with reduced features due to lack of resources. No new features will be added so F3 boards are not recommended for new builds.
### Boards based on F1 CPUs
Boards based on STM32F1 CPUs are no longer supported by latest INAV version. Last release is 1.7.3
### Not recommended for new setups
F1 and F3 boards are no longer recommended. Users should choose a board from the supported F4 or F7 devices available in the latest release.

11
docs/Cli.md
View file

@ -418,6 +418,10 @@ A shorter form is also supported to enable and disable functions using `serial <
| acc_lpf_type | BIQUAD | Specifies the type of the software LPF of the acc signals. BIQUAD gives better filtering and more delay, PT1 less filtering and less delay, so use only on clean builds. | | acc_lpf_type | BIQUAD | Specifies the type of the software LPF of the acc signals. BIQUAD gives better filtering and more delay, PT1 less filtering and less delay, so use only on clean builds. |
| dterm_lpf_hz | 40 | Dterm low pass filter cutoff frequency. Default setting is very conservative and small multirotors should use higher value between 80 and 100Hz. 80 seems like a gold spot for 7-inch builds while 100 should work best with 5-inch machines. If motors are getting too hot, lower the value | | dterm_lpf_hz | 40 | Dterm low pass filter cutoff frequency. Default setting is very conservative and small multirotors should use higher value between 80 and 100Hz. 80 seems like a gold spot for 7-inch builds while 100 should work best with 5-inch machines. If motors are getting too hot, lower the value |
| yaw_lpf_hz | 30 | Yaw low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below) | | yaw_lpf_hz | 30 | Yaw low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below) |
| dyn_notch_width_percent | 8 | Distance in % of the attenuated frequency for double dynamic filter notched. When set to `0` single dynamic notch filter is used |
| dyn_notch_range | MEDIUM | Dynamic gyro filter range. Possible values `LOW` `MEDIUM` `HIGH`. `MEDIUM` should work best for 5-6" multirotors. `LOW` should work best with 7" and bigger. `HIGH` should work with everything below 4" |
| dyn_notch_q | 120 | Q factor for dynamic notches |
| dyn_notch_min_hz | 150 | Minimum frequency for dynamic notches. Default value of `150` works best with 5" multirors. Should be lowered with increased size of propellers. Values around `100` work fine on 7" drones. 10" can go down to `60` - `70` |
| gyro_stage2_lowpass_hz | 0 | Software based second stage lowpass filter for gyro. Value is cutoff frequency (Hz). Currently experimental | | gyro_stage2_lowpass_hz | 0 | Software based second stage lowpass filter for gyro. Value is cutoff frequency (Hz). Currently experimental |
| pidsum_limit | 500 | A limitation to overall amount of correction Flight PID can request on each axis (Roll/Pitch/Yaw). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help | | pidsum_limit | 500 | A limitation to overall amount of correction Flight PID can request on each axis (Roll/Pitch/Yaw). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help |
| yaw_p_limit | 300 | | | yaw_p_limit | 300 | |
@ -489,3 +493,10 @@ A shorter form is also supported to enable and disable functions using `serial <
| acc_event_threshold_low | 0 | Acceleration threshold [cm/s/s] for low-g / freefall detection text messages sent by SIM module. A setting of less than 100 is suggested. Valid values: [0-900], 0 = detection off. | | acc_event_threshold_low | 0 | Acceleration threshold [cm/s/s] for low-g / freefall detection text messages sent by SIM module. A setting of less than 100 is suggested. Valid values: [0-900], 0 = detection off. |
| acc_event_threshold_neg_x | 0 | Acceleration threshold [cm/s/s] for backwards acceleration / fixed wing landing detection text messages sent by SIM module. Suggested value for fixed wing: 1100. 0 = detection off. | | acc_event_threshold_neg_x | 0 | Acceleration threshold [cm/s/s] for backwards acceleration / fixed wing landing detection text messages sent by SIM module. Suggested value for fixed wing: 1100. 0 = detection off. |
| sim_low_altitude | 0 | Threshold for low altitude warning messages sent by SIM module when the 'L' transmit flag is set in `sim_transmit_flags`.| | sim_low_altitude | 0 | Threshold for low altitude warning messages sent by SIM module when the 'L' transmit flag is set in `sim_transmit_flags`.|
| rssi_source | `AUTO` | Source of RSSI input. Possible values: `NONE`, `AUTO`, `ADC`, `CHANNEL`, `PROTOCOL`, `MSP` |
| throttle_scale | 1.000 | Throttle scaling factor. `1` means no throttle scaling. `0.5` means throttle scaled down by 50% |
| vbat_meter_type | `ADC` | Vbat voltage source. Possible values: `NONE`, `ADC`, `ESC`. `ESC` required ESC telemetry enebled and running |
| antigravity_gain | 1 | Max Antigravity gain. `1` means Antigravity is disabled, `2` means Iterm is allowed to double during rapid throttle movements |
| antigravity_accelerator | 1 | |
| antigravity_cutoff_lpf_hz | 15 | Antigravity cutoff frequenct for Throtte filter. Antigravity is based on the difference between actual and filtered throttle input. The bigger is the difference, the bigger Antigravity gain |
| sim_pin | | PIN for GSM card module |

76
docs/Global Functions.md Normal file
View file

@ -0,0 +1,76 @@
# Global Functions
_Global Functions_ (abbr. GF) are a mechanism allowing to override certain flight parameters (during flight). Global Functions are activated by [Logic Conditions](Logic%20Conditions.md)
## CLI
`gf <rule> <enabled> <logic condition> <action> <operand type> <operand value> <flags>`
* `<rule>` - GF ID, indexed from `0`
* `<enabled>` - `0` evaluates as disabled, `1` evaluates as enabled. Only enabled GFs are executed
* `<logic condition>` - the ID of _LogicCondition_ used to trigger GF On/Off. Then LC evaluates `true`, GlobalFunction will be come active
* `<action>` - action to execute when GF is active
* `<operand type>` - allows to pass arguments into Global Function action. Syntax is the same as in case of Logic Conditions operands. Used only when `action` requires it. Should be kept at `0` in other case. See [Logic Conditions](Logic%20Conditions.md)
* `<operand value>` - allows to pass arguments into Global Function action. Syntax is the same as in case of Logic Conditions operands. Used only when `action` requires it. Should be kept at `0` in other case. See [Logic Conditions](Logic%20Conditions.md)
* `<flags>` - allows to pass arguments into Global Function action. Syntax is the same as in case of Logic Conditions operands
## Actions
| Action ID | Name | Notes |
|---- |---- |---- |
| 0 | OVERRIDE_ARMING_SAFETY | Allows to arm on any angle even without GPS fix |
| 1 | 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%. |
| 2 | 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 |
| 3 | SET_VTX_POWER_LEVEL | Sets VTX power level. Accepted values are `0-3` for SmartAudio and `0-4` for Tramp protocol |
| 4 | INVERT_ROLL | Inverts ROLL axis input for PID/PIFF controller |
| 5 | INVERT_PITCH | Inverts PITCH axis input for PID/PIFF controller |
| 6 | INVERT_YAW | Inverts YAW axis input for PID/PIFF controller |
| 7 | 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 |
## Flags
Current no flags are implemented
## Example
### Dynamic THROTTLE scale
`gf 0 1 0 1 0 50 0`
Limits the THROTTLE output to 50% when Logic Condition `0` evaluates as `true`
### Set VTX power level via Smart Audio
`gf 0 1 0 3 0 3 0`
Sets VTX power level to `3` when Logic Condition `0` evaluates as `true`
### Invert ROLL and PITCH when rear facing camera FPV is used
Solves the problem from [https://github.com/iNavFlight/inav/issues/4439](https://github.com/iNavFlight/inav/issues/4439)
```
gf 0 1 0 4 0 0 0
gf 1 1 0 5 0 0 0
```
Inverts ROLL and PITCH input when Logic Condition `0` evaluates as `true`. Moving Pitch stick up will cause pitch down (up for rear facing camera). Moving Roll stick right will cause roll left of a quad (right in rear facing camera)
### Cut motors but keep other throttle bindings active
`gf 0 1 0 7 0 1000 0`
Sets Thhrottle output to `0%` when Logic Condition `0` evaluates as `true`
### Set throttle to 50% and keep other throttle bindings active
`gf 0 1 0 7 0 1500 0`
Sets Thhrottle output to about `50%` when Logic Condition `0` evaluates as `true`
### Set throttle control to different RC channel
`gf 0 1 0 7 1 7 0`
If Logic Condition `0` evaluates as `true`, motor throttle control is bound to RC channel 7 instead of throttle channel

2
docs/LedStrip.md
View file

@ -50,7 +50,7 @@ from another BEC. Just ensure that the GROUND is the same for all BEC outputs a
| Target | Pin | LED Strip | Signal | | Target | Pin | LED Strip | Signal |
| --------------------- | ---- | --------- | -------| | --------------------- | ---- | --------- | -------|
| ChebuzzF3/F3Discovery | PB8 | Data In | PB8 | | F3Discovery | PB8 | Data In | PB8 |
| Sparky | PWM5 | Data In | PA6 | | Sparky | PWM5 | Data In | PA6 |
If you have LEDs that are intermittent, flicker or show the wrong colors then drop the VIN to less than 4.7v, e.g. by using an inline If you have LEDs that are intermittent, flicker or show the wrong colors then drop the VIN to less than 4.7v, e.g. by using an inline

22
docs/Logic Conditions.md
View file

@ -1,18 +1,24 @@
# Logic Conditions # Logic Conditions
Logic Conditions (abbr. LC) is a mechanism that allows to evaluate cenrtain flight parameters (RC channels, switches, altitude, distance, timers, other logic conditions) and use the value of evaluated expression in different places of INAV. Currently, the result of LCs can be used in:
* [Servo mixer](Mixer.md) to activate/deactivate certain servo mix rulers
* [Global functions](Global%20Functions.md) to activate/deactivate system overrides
Logic conditions can be edited using INAV Configurator user interface, of via CLI
## CLI ## CLI
`logic <rule> <enabled> <operation> <operand A type> <operand A value> <operand B type> <operand B value> <flags>` `logic <rule> <enabled> <operation> <operand A type> <operand A value> <operand B type> <operand B value> <flags>`
* `<rule>` * `<rule>` - ID of Logic Condition rule
* `<enabled>` * `<enabled>` - `0` evaluates as disabled, `1` evaluates as enabled
* `<operation>` * `<operation>` - See `Operations` paragraph
* `<operand A type>` * `<operand A type>` - See `Operands` paragraph
* `<operand A value>` * `<operand A value>` - See `Operands` paragraph
* `<operand B type>` * `<operand B type>` - See `Operands` paragraph
* `<operand B value>` * `<operand B value>` - See `Operands` paragraph
* `<flags>` * `<flags>` - See `Flags` paragraph
### Operations ### Operations

13
docs/Mixer.md
View file

@ -91,3 +91,16 @@ The `smix` command is used to list, create or modify rules. To list the currentl
To create or modify rules use the `smix` command with the following syntax: `smix <n> <servo_index> <input_id> <weight> <speed> <logic_condition_id>`. `<n>` is representing the index of the servo mixing rule to create or modify (integer). To disable a mixing rule set the weight to 0. To create or modify rules use the `smix` command with the following syntax: `smix <n> <servo_index> <input_id> <weight> <speed> <logic_condition_id>`. `<n>` is representing the index of the servo mixing rule to create or modify (integer). To disable a mixing rule set the weight to 0.
`logic_condition_id` default value is `-1` for rules that should be always executed. `logic_condition_id` default value is `-1` for rules that should be always executed.
### Logic Conditions
[Logic Conditions](Logic%20Conditions.md) allows to activate/deactivate `smix` rules based on user input and flight parameters. If Logic Condition evaluates as `false`, smix rule connected with with LC will not be active and used inside the Mixer.
This mechanism allows to move servos when desired conditions are met. For example, if an airplane is equipped with a pitot tube and flaps, flaps can be automatically deployed when airspeed goes below a threshold.
Other usages can be:
* automatic parachute deployment
* VTOL and especially tail-sitters that require change in mixings during flight mode transition
* crowbar airbrakes
* any kind of servo mixings that should be changed during flight

103
docs/development/Building in Windows.md
View file

@ -1,103 +0,0 @@
# Building in windows
##Setup Cygwin
download the Setup*.exe from https://www.cygwin.com/
![Cygwin Installation](assets/001.cygwin_dl.png)
Execute the download Setup and step through the installation wizard (no need to customize the settings here). Stop at the "Select Packages" Screen and select the following Packages
for Installation:
- Devel/binutils
- Devel/git
- Devel/git-completion (Optional)
- Devel/make
- Editors/vim
- Editors/vim-common (Optional)
- Shells/mintty (should be already selected)
![Cygwin Installation](assets/004.cygwin_setup.png)
![Cygwin Installation](assets/002.cygwin_setup.png)
![Cygwin Installation](assets/003.cygwin_setup.png)
![Cygwin Installation](assets/005.cygwin_setup.png)
![Cygwin Installation](assets/006.cygwin_setup.png)
Continue with the Installation and accept all autodetected dependencies.
![Cygwin Installation](assets/007.cygwin_setup.png)
##Setup GNU ARM Toolchain
----------
use the latest version available. Download this version from https://gcc.gnu.org/mirrors.html - preferably as a ZIP-File.
Extract the contents of this archive to any folder of your choice, for instance ```C:\dev\gcc-arm-none-eabi-*version number*```.
![GNU ARM Toolchain Setup](assets/008.toolchain.png)
add the "bin" subdirectory to the PATH Windows environment variable: ```%PATH%;C:\dev\gcc-arm-none-eabi-*version number*\bin```
![GNU ARM Toolchain Setup](assets/009.toolchain_path.png)
![GNU ARM Toolchain Setup](assets/010.toolchain_path.png)
##Setup Ruby
Install the latest Ruby version using [Ruby Installer](https://rubyinstaller.org).
## Checkout and compile INAV
Head over to the INAV Github page and grab the URL of the GIT Repository: "https://github.com/iNavFlight/inav.git"
Open the Cygwin-Terminal, navigate to your development folder and use the git commandline to checkout the repository:
```bash
cd /cygdrive/c/dev
git clone https://github.com/iNavFlight/inav.git
```
![GIT Checkout](assets/011.git_checkout.png)
![GIT Checkout](assets/012.git_checkout.png)
To compile your INAV binaries, enter the inav directory and build the project using the make command. You can append TARGET=[HARDWARE] if you want to build anything other than the default SPRACINGF3 target:
```bash
cd inav
make TARGET=SPRACINGF3
```
![GIT Checkout](assets/013.compile.png)
within few moments you should have your binary ready:
```bash
(...)
arm-none-eabi-size ./obj/main/inav_SPRACINGF3.elf
text data bss dec hex filename
95388 308 10980 106676 1a0b4 ./obj/main/inav_SPRACINGF3.elf
arm-none-eabi-objcopy -O ihex --set-start 0x8000000 obj/main/inav_SPRACINGF3.elf obj/inav_SPRACINGF3.hex
```
You can use the INAV-Configurator to flash the ```obj/inav_SPRACINGF3.hex``` file.
## Updating and rebuilding
Navigate to the local INAV repository and use the following steps to pull the latest changes and rebuild your version of INAV:
```bash
cd /cygdrive/c/dev/inav
git reset --hard
git pull
make clean
make TARGET=SPRACINGF3
```

3
fake_travis_build.sh
View file

@ -2,8 +2,6 @@
targets=("PUBLISHMETA=True" \ targets=("PUBLISHMETA=True" \
"RUNTESTS=True" \ "RUNTESTS=True" \
"TARGET=CHEBUZZF3" \
"TARGET=COLIBRI_RACE" \
"TARGET=SPRACINGF3" \ "TARGET=SPRACINGF3" \
"TARGET=SPRACINGF3EVO" \ "TARGET=SPRACINGF3EVO" \
"TARGET=LUX_RACE" \ "TARGET=LUX_RACE" \
@ -11,7 +9,6 @@ targets=("PUBLISHMETA=True" \
"TARGET=RMDO" \ "TARGET=RMDO" \
"TARGET=SPARKY" \ "TARGET=SPARKY" \
"TARGET=STM32F3DISCOVERY" \ "TARGET=STM32F3DISCOVERY" \
"TARGET=ALIENFLIGHTF3"\
"TARGET=RCEXPLORERF3" ) "TARGET=RCEXPLORERF3" )
#fake a travis build environment #fake a travis build environment
export TRAVIS_BUILD_NUMBER=$(date +%s) export TRAVIS_BUILD_NUMBER=$(date +%s)

10
make/release.mk
View file

@ -1,7 +1,7 @@
RELEASE_TARGETS = AIRHEROF3 AIRHEROF3_QUAD RELEASE_TARGETS = AIRHEROF3 AIRHEROF3_QUAD
RELEASE_TARGETS += COLIBRI_RACE LUX_RACE FURYF3 FURYF3_SPIFLASH RCEXPLORERF3 RMDO SPARKY KFC32F3_INAV FALCORE MOTOLAB ANYFC BLUEJAYF4 COLIBRI F4BY DALRCF405 RELEASE_TARGETS += LUX_RACE FURYF3 FURYF3_SPIFLASH RCEXPLORERF3 RMDO SPARKY KFC32F3_INAV FALCORE MOTOLAB ANYFC BLUEJAYF4 COLIBRI F4BY DALRCF405
RELEASE_TARGETS += QUANTON REVO SPARKY2 YUPIF4 YUPIF4R2 YUPIF4MINI KROOZX PIKOBLX CLRACINGF4AIR CLRACINGF4AIRV2 PIXRACER BEEROTORF4 ANYFCF7 ANYFCF7_EXTERNAL_BARO ANYFCM7 RELEASE_TARGETS += QUANTON REVO SPARKY2 PIKOBLX CLRACINGF4AIR CLRACINGF4AIRV2 PIXRACER BEEROTORF4 ANYFCF7 ANYFCF7_EXTERNAL_BARO
RELEASE_TARGETS += ALIENFLIGHTNGF7 RELEASE_TARGETS += ALIENFLIGHTNGF7
RELEASE_TARGETS += BETAFLIGHTF3 BETAFLIGHTF4 RELEASE_TARGETS += BETAFLIGHTF3 BETAFLIGHTF4
@ -15,7 +15,7 @@ RELEASE_TARGETS += OMNIBUSF4 OMNIBUSF4PRO OMNIBUSF4PRO_LEDSTRIPM5 DYSF4PRO DYSF4
RELEASE_TARGETS += OMNIBUSF4V3 OMNIBUSF4V3_S6_SS OMNIBUSF4V3_S5S6_SS OMNIBUSF4V3_S5_S6_2SS RELEASE_TARGETS += OMNIBUSF4V3 OMNIBUSF4V3_S6_SS OMNIBUSF4V3_S5S6_SS OMNIBUSF4V3_S5_S6_2SS
RELEASE_TARGETS += OMNIBUSF7 OMNIBUSF7V2 OMNIBUSF7NXT YUPIF7 RELEASE_TARGETS += OMNIBUSF7 OMNIBUSF7V2 OMNIBUSF7NXT YUPIF7
RELEASE_TARGETS += MATEKF405 MATEKF405_SERVOS6 MATEKF405OSD MATEKF722 MATEKF722_HEXSERVO MATEKF722SE MATEKF722MINI MATEKF405SE MATEKF411 MATEKF411_SFTSRL2 MATEKF411_FD_SFTSRL MATEKF411_RSSI RELEASE_TARGETS += MATEKF405 MATEKF405_SERVOS6 MATEKF405OSD MATEKF722 MATEKF722_HEXSERVO MATEKF722SE MATEKF722MINI MATEKF405SE MATEKF411 MATEKF411_SFTSRL2 MATEKF411_FD_SFTSRL MATEKF411_RSSI MATEKF411SE MATEKF765
RELEASE_TARGETS += FOXEERF405 FOXEERF722DUAL RELEASE_TARGETS += FOXEERF405 FOXEERF722DUAL
@ -29,4 +29,6 @@ RELEASE_TARGETS += MAMBAF405
RELEASE_TARGETS += IFLIGHTF7_TWING IFLIGHTF4_TWING RELEASE_TARGETS += IFLIGHTF7_TWING IFLIGHTF4_TWING
RELEASE_TARGETS += AIKONF4 RELEASE_TARGETS += AIKONF4
RELEASE_TARGETS += MATEKF765

18
make/source.mk
View file

@ -21,9 +21,10 @@ COMMON_SRC = \
common/olc.c \ common/olc.c \
common/printf.c \ common/printf.c \
common/streambuf.c \ common/streambuf.c \
common/string_light.c \
common/time.c \ common/time.c \
common/typeconversion.c \ common/typeconversion.c \
common/string_light.c \ common/uvarint.c \
config/config_eeprom.c \ config/config_eeprom.c \
config/config_streamer.c \ config/config_streamer.c \
config/feature.c \ config/feature.c \
@ -37,10 +38,13 @@ COMMON_SRC = \
drivers/bus_i2c_soft.c \ drivers/bus_i2c_soft.c \
drivers/bus_spi.c \ drivers/bus_spi.c \
drivers/display.c \ drivers/display.c \
drivers/display_canvas.c \
drivers/display_font_metadata.c \
drivers/exti.c \ drivers/exti.c \
drivers/io.c \ drivers/io.c \
drivers/io_pca9685.c \ drivers/io_pca9685.c \
drivers/light_led.c \ drivers/light_led.c \
drivers/osd.c \
drivers/resource.c \ drivers/resource.c \
drivers/rx_nrf24l01.c \ drivers/rx_nrf24l01.c \
drivers/rx_spi.c \ drivers/rx_spi.c \
@ -96,11 +100,13 @@ COMMON_SRC = \
flight/wind_estimator.c \ flight/wind_estimator.c \
flight/gyroanalyse.c \ flight/gyroanalyse.c \
flight/secondary_imu.c \ flight/secondary_imu.c \
flight/rpm_filter.c \
io/beeper.c \ io/beeper.c \
io/lights.c \
io/pwmdriver_i2c.c \
io/esc_serialshot.c \ io/esc_serialshot.c \
io/frsky_osd.c \
io/lights.c \
io/piniobox.c \ io/piniobox.c \
io/pwmdriver_i2c.c \
io/serial.c \ io/serial.c \
io/serial_4way.c \ io/serial_4way.c \
io/serial_4way_avrootloader.c \ io/serial_4way_avrootloader.c \
@ -174,6 +180,7 @@ COMMON_SRC = \
io/opflow_cxof.c \ io/opflow_cxof.c \
io/opflow_msp.c \ io/opflow_msp.c \
io/dashboard.c \ io/dashboard.c \
io/displayport_frsky_osd.c \
io/displayport_max7456.c \ io/displayport_max7456.c \
io/displayport_msp.c \ io/displayport_msp.c \
io/displayport_oled.c \ io/displayport_oled.c \
@ -183,8 +190,11 @@ COMMON_SRC = \
io/gps_nmea.c \ io/gps_nmea.c \
io/gps_naza.c \ io/gps_naza.c \
io/ledstrip.c \ io/ledstrip.c \
io/osd_hud.c \
io/osd.c \ io/osd.c \
io/osd_canvas.c \
io/osd_common.c \
io/osd_grid.c \
io/osd_hud.c \
navigation/navigation.c \ navigation/navigation.c \
navigation/navigation_fixedwing.c \ navigation/navigation_fixedwing.c \
navigation/navigation_fw_launch.c \ navigation/navigation_fw_launch.c \

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

@ -58,6 +58,7 @@
#include "flight/mixer.h" #include "flight/mixer.h"
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/servos.h" #include "flight/servos.h"
#include "flight/rpm_filter.h"
#include "io/beeper.h" #include "io/beeper.h"
#include "io/gps.h" #include "io/gps.h"
@ -1711,6 +1712,16 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("pidSumLimit", "%d", pidProfile()->pidSumLimit); BLACKBOX_PRINT_HEADER_LINE("pidSumLimit", "%d", pidProfile()->pidSumLimit);
BLACKBOX_PRINT_HEADER_LINE("axisAccelerationLimitYaw", "%d", pidProfile()->axisAccelerationLimitYaw); BLACKBOX_PRINT_HEADER_LINE("axisAccelerationLimitYaw", "%d", pidProfile()->axisAccelerationLimitYaw);
BLACKBOX_PRINT_HEADER_LINE("axisAccelerationLimitRollPitch", "%d", pidProfile()->axisAccelerationLimitRollPitch); BLACKBOX_PRINT_HEADER_LINE("axisAccelerationLimitRollPitch", "%d", pidProfile()->axisAccelerationLimitRollPitch);
#ifdef USE_RPM_FILTER
BLACKBOX_PRINT_HEADER_LINE("rpm_gyro_filter_enabled", "%d", rpmFilterConfig()->gyro_filter_enabled);
BLACKBOX_PRINT_HEADER_LINE("rpm_gyro_harmonics", "%d", rpmFilterConfig()->gyro_harmonics);
BLACKBOX_PRINT_HEADER_LINE("rpm_gyro_min_hz", "%d", rpmFilterConfig()->gyro_min_hz);
BLACKBOX_PRINT_HEADER_LINE("rpm_gyro_q", "%d", rpmFilterConfig()->gyro_q);
BLACKBOX_PRINT_HEADER_LINE("rpm_dterm_filter_enabled", "%d", rpmFilterConfig()->dterm_filter_enabled);
BLACKBOX_PRINT_HEADER_LINE("rpm_dterm_harmonics", "%d", rpmFilterConfig()->dterm_harmonics);
BLACKBOX_PRINT_HEADER_LINE("rpm_dterm_min_hz", "%d", rpmFilterConfig()->dterm_min_hz);
BLACKBOX_PRINT_HEADER_LINE("rpm_dterm_q", "%d", rpmFilterConfig()->dterm_q);
#endif
default: default:
return true; return true;
} }

13
src/main/build/debug.h
View file

@ -51,29 +51,22 @@ extern timeUs_t sectionTimes[2][4];
typedef enum { typedef enum {
DEBUG_NONE, DEBUG_NONE,
DEBUG_GYRO, DEBUG_GYRO,
DEBUG_NOTCH,
DEBUG_NAV_LANDING_DETECTOR,
DEBUG_FW_CLIMB_RATE_TO_ALTITUDE,
DEBUG_AGL, DEBUG_AGL,
DEBUG_FLOW_RAW, DEBUG_FLOW_RAW,
DEBUG_FLOW, DEBUG_FLOW,
DEBUG_SBUS, DEBUG_SBUS,
DEBUG_FPORT, DEBUG_FPORT,
DEBUG_ALWAYS, DEBUG_ALWAYS,
DEBUG_STAGE2,
DEBUG_SAG_COMP_VOLTAGE, DEBUG_SAG_COMP_VOLTAGE,
DEBUG_VIBE, DEBUG_VIBE,
DEBUG_CRUISE, DEBUG_CRUISE,
DEBUG_REM_FLIGHT_TIME, DEBUG_REM_FLIGHT_TIME,
DEBUG_SMARTAUDIO, DEBUG_SMARTAUDIO,
DEBUG_ACC, DEBUG_ACC,
DEBUG_GENERIC,
DEBUG_ITERM_RELAX, DEBUG_ITERM_RELAX,
DEBUG_D_BOOST, DEBUG_ERPM,
DEBUG_ANTIGRAVITY, DEBUG_RPM_FILTER,
DEBUG_FFT, DEBUG_RPM_FREQ,
DEBUG_FFT_TIME,
DEBUG_FFT_FREQ,
DEBUG_IMU2, DEBUG_IMU2,
DEBUG_COUNT DEBUG_COUNT
} debugType_e; } debugType_e;

2
src/main/build/version.h
View file

@ -16,7 +16,7 @@
*/ */
#define FC_VERSION_MAJOR 2 // increment when a major release is made (big new feature, etc) #define FC_VERSION_MAJOR 2 // increment when a major release is made (big new feature, etc)
#define FC_VERSION_MINOR 3 // increment when a minor release is made (small new feature, change etc) #define FC_VERSION_MINOR 4 // increment when a minor release is made (small new feature, change etc)
#define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed #define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed
#define STR_HELPER(x) #x #define STR_HELPER(x) #x

6
src/main/common/global_functions.c
View file

@ -113,6 +113,12 @@ void globalFunctionsProcess(int8_t functionId) {
GLOBAL_FUNCTION_FLAG_ENABLE(GLOBAL_FUNCTION_FLAG_OVERRIDE_INVERT_YAW); GLOBAL_FUNCTION_FLAG_ENABLE(GLOBAL_FUNCTION_FLAG_OVERRIDE_INVERT_YAW);
} }
break; break;
case GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE:
if (conditionValue) {
globalFunctionValues[GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE] = globalFunctionsStates[functionId].value;
GLOBAL_FUNCTION_FLAG_ENABLE(GLOBAL_FUNCTION_FLAG_OVERRIDE_THROTTLE);
}
break;
} }
} }
} }

3
src/main/common/global_functions.h
View file

@ -36,6 +36,7 @@ typedef enum {
GLOBAL_FUNCTION_ACTION_INVERT_ROLL, GLOBAL_FUNCTION_ACTION_INVERT_ROLL,
GLOBAL_FUNCTION_ACTION_INVERT_PITCH, GLOBAL_FUNCTION_ACTION_INVERT_PITCH,
GLOBAL_FUNCTION_ACTION_INVERT_YAW, GLOBAL_FUNCTION_ACTION_INVERT_YAW,
GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE,
GLOBAL_FUNCTION_ACTION_LAST GLOBAL_FUNCTION_ACTION_LAST
} globalFunctionActions_e; } globalFunctionActions_e;
@ -46,6 +47,7 @@ typedef enum {
GLOBAL_FUNCTION_FLAG_OVERRIDE_INVERT_ROLL = (1 << 3), GLOBAL_FUNCTION_FLAG_OVERRIDE_INVERT_ROLL = (1 << 3),
GLOBAL_FUNCTION_FLAG_OVERRIDE_INVERT_PITCH = (1 << 4), GLOBAL_FUNCTION_FLAG_OVERRIDE_INVERT_PITCH = (1 << 4),
GLOBAL_FUNCTION_FLAG_OVERRIDE_INVERT_YAW = (1 << 5), GLOBAL_FUNCTION_FLAG_OVERRIDE_INVERT_YAW = (1 << 5),
GLOBAL_FUNCTION_FLAG_OVERRIDE_THROTTLE = (1 << 6),
} globalFunctionFlags_t; } globalFunctionFlags_t;
typedef struct globalFunction_s { typedef struct globalFunction_s {
@ -69,6 +71,7 @@ extern uint64_t globalFunctionsFlags;
#define GLOBAL_FUNCTION_FLAG(mask) (globalFunctionsFlags & (mask)) #define GLOBAL_FUNCTION_FLAG(mask) (globalFunctionsFlags & (mask))
PG_DECLARE_ARRAY(globalFunction_t, MAX_GLOBAL_FUNCTIONS, globalFunctions); PG_DECLARE_ARRAY(globalFunction_t, MAX_GLOBAL_FUNCTIONS, globalFunctions);
extern int globalFunctionValues[GLOBAL_FUNCTION_ACTION_LAST];
void globalFunctionsUpdateTask(timeUs_t currentTimeUs); void globalFunctionsUpdateTask(timeUs_t currentTimeUs);
float getThrottleScale(float globalThrottleScale); float getThrottleScale(float globalThrottleScale);

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

@ -29,6 +29,7 @@ typedef enum {
LOG_TOPIC_TEMPERATURE, // 7, mask = 128 LOG_TOPIC_TEMPERATURE, // 7, mask = 128
LOG_TOPIC_POS_ESTIMATOR, // 8, mask = 256 LOG_TOPIC_POS_ESTIMATOR, // 8, mask = 256
LOG_TOPIC_VTX, // 9, mask = 512 LOG_TOPIC_VTX, // 9, mask = 512
LOG_TOPIC_OSD, // 10, mask = 1024
LOG_TOPIC_COUNT, LOG_TOPIC_COUNT,
} logTopic_e; } logTopic_e;

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

@ -23,6 +23,7 @@
#include "maths.h" #include "maths.h"
#include "vector.h" #include "vector.h"
#include "quaternion.h" #include "quaternion.h"
#include "platform.h"
// http://lolengine.net/blog/2011/12/21/better-function-approximations // http://lolengine.net/blog/2011/12/21/better-function-approximations
// Chebyshev http://stackoverflow.com/questions/345085/how-do-trigonometric-functions-work/345117#345117 // Chebyshev http://stackoverflow.com/questions/345085/how-do-trigonometric-functions-work/345117#345117
@ -158,7 +159,7 @@ int constrain(int amt, int low, int high)
return amt; return amt;
} }
float constrainf(float amt, float low, float high) float FAST_CODE NOINLINE constrainf(float amt, float low, float high)
{ {
if (amt < low) if (amt < low)
return low; return low;

5
src/main/common/utils.h
View file

@ -28,6 +28,9 @@
#define CONCAT_HELPER(x,y) x ## y #define CONCAT_HELPER(x,y) x ## y
#define CONCAT(x,y) CONCAT_HELPER(x, y) #define CONCAT(x,y) CONCAT_HELPER(x, y)
#define CONCAT3_HELPER(x, y, z) x ## y ## z
#define CONCAT3(x, y, z) CONCAT3_HELPER(x, y, z)
#define CONCAT4_HELPER(x, y, z, w) x ## y ## z ## w #define CONCAT4_HELPER(x, y, z, w) x ## y ## z ## w
#define CONCAT4(x, y, z, w) CONCAT4_HELPER(x, y, z, w) #define CONCAT4(x, y, z, w) CONCAT4_HELPER(x, y, z, w)
@ -106,4 +109,4 @@ void * memcpy_fn ( void * destination, const void * source, size_t num ) asm("me
#define FALLTHROUGH do {} while(0) #define FALLTHROUGH do {} while(0)
#endif #endif
#define ALIGNED(x) __attribute__ ((aligned(x))) #define ALIGNED(x) __attribute__ ((aligned(x)))

70
src/main/common/uvarint.c Normal file
View file

@ -0,0 +1,70 @@
/*
* This file is part of INAV.
*
* 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/.
*
* @author Alberto Garcia Hierro <alberto@garciahierro.com>
*/
#include "common/uvarint.h"
int uvarintEncode(uint32_t val, uint8_t *ptr, size_t size)
{
unsigned ii = 0;
while (val > 0x80)
{
if (ii >= size) {
return -1;
}
ptr[ii] = (val & 0xFF) | 0x80;
val >>= 7;
ii++;
}
if (ii >= size) {
return -1;
}
ptr[ii] = val & 0xFF;
return ii + 1;
}
int uvarintDecode(uint32_t *val, const uint8_t *ptr, size_t size)
{
unsigned s = 0;
*val = 0;
for (size_t ii = 0; ii < size; ii++)
{
uint8_t b = ptr[ii];
if (b < 0x80)
{
if (ii > 5 || (ii == 5 && b > 1))
{
// uint32_t overflow
return -2;
}
*val |= ((uint32_t)b) << s;
return ii + 1;
}
*val |= ((uint32_t)(b & 0x7f)) << s;
s += 7;
}
// no value could be decoded and we have no data left
return -1;
}

33
src/main/common/uvarint.h Normal file
View file

@ -0,0 +1,33 @@
/*
* This file is part of INAV.
*
* 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/.
*
* @author Alberto Garcia Hierro <alberto@garciahierro.com>
*/
#pragma once
#include <stddef.h>
#include <stdint.h>
int uvarintEncode(uint32_t val, uint8_t *ptr, size_t size);
int uvarintDecode(uint32_t *val, const uint8_t *ptr, size_t size);

5
src/main/config/parameter_group_ids.h
View file

@ -109,8 +109,9 @@
#define PG_GENERAL_SETTINGS 1019 #define PG_GENERAL_SETTINGS 1019
#define PG_GLOBAL_FUNCTIONS 1020 #define PG_GLOBAL_FUNCTIONS 1020
#define PG_ESC_SENSOR_CONFIG 1021 #define PG_ESC_SENSOR_CONFIG 1021
#define PG_SECONDARY_IMU 1022 #define PG_RPM_FILTER_CONFIG 1022
#define PG_INAV_END 1021 #define PG_SECONDARY_IMU 1023
#define PG_INAV_END 1023
// OSD configuration (subject to change) // OSD configuration (subject to change)
//#define PG_OSD_FONT_CONFIG 2047 //#define PG_OSD_FONT_CONFIG 2047

348
src/main/drivers/barometer/barometer_bmp388.c Normal file
View file

@ -0,0 +1,348 @@
/*
* This file is part of iNav.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* Cleanflight and Betaflight are distributed in the hope that they
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*
* BMP388 Driver author: Dominic Clifton
* iNav port: Michel Pastor
*/
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <platform.h>
#include "build/build_config.h"
#include "build/debug.h"
#include "common/utils.h"
#include "common/log.h" // XXX
#include "drivers/time.h"
#include "drivers/io.h"
#include "drivers/bus.h"
#include "drivers/barometer/barometer.h"
#include "drivers/barometer/barometer_bmp388.h"
#if defined(USE_BARO) && (defined(USE_BARO_BMP388) || defined(USE_BARO_SPI_BMP388))
#define BMP388_I2C_ADDR (0x76) // same as BMP280/BMP180
#define BMP388_DEFAULT_CHIP_ID (0x50) // from https://github.com/BoschSensortec/BMP3-Sensor-API/blob/master/bmp3_defs.h#L130
#define BMP388_CMD_REG (0x7E)
#define BMP388_RESERVED_UPPER_REG (0x7D)
// everything between BMP388_RESERVED_UPPER_REG and BMP388_RESERVED_LOWER_REG is reserved.
#define BMP388_RESERVED_LOWER_REG (0x20)
#define BMP388_CONFIG_REG (0x1F)
#define BMP388_RESERVED_0x1E_REG (0x1E)
#define BMP388_ODR_REG (0x1D)
#define BMP388_OSR_REG (0x1C)
#define BMP388_PWR_CTRL_REG (0x1B)
#define BMP388_IF_CONFIG_REG (0x1A)
#define BMP388_INT_CTRL_REG (0x19)
#define BMP388_FIFO_CONFIG_2_REG (0x18)
#define BMP388_FIFO_CONFIG_1_REG (0x17)
#define BMP388_FIFO_WTM_1_REG (0x16)
#define BMP388_FIFO_WTM_0_REG (0x15)
#define BMP388_FIFO_DATA_REG (0x14)
#define BMP388_FIFO_LENGTH_1_REG (0x13)
#define BMP388_FIFO_LENGTH_0_REG (0x12)
#define BMP388_INT_STATUS_REG (0x11)
#define BMP388_EVENT_REG (0x10)
#define BMP388_SENSORTIME_3_REG (0x0F) // BME780 only
#define BMP388_SENSORTIME_2_REG (0x0E)
#define BMP388_SENSORTIME_1_REG (0x0D)
#define BMP388_SENSORTIME_0_REG (0x0C)
#define BMP388_RESERVED_0x0B_REG (0x0B)
#define BMP388_RESERVED_0x0A_REG (0x0A)
// see friendly register names below
#define BMP388_DATA_5_REG (0x09)
#define BMP388_DATA_4_REG (0x08)
#define BMP388_DATA_3_REG (0x07)
#define BMP388_DATA_2_REG (0x06)
#define BMP388_DATA_1_REG (0x05)
#define BMP388_DATA_0_REG (0x04)
#define BMP388_STATUS_REG (0x03)
#define BMP388_ERR_REG (0x02)
#define BMP388_RESERVED_0x01_REG (0x01)
#define BMP388_CHIP_ID_REG (0x00)
// friendly register names, from datasheet 4.3.4
#define BMP388_PRESS_MSB_23_16_REG BMP388_DATA_2_REG
#define BMP388_PRESS_LSB_15_8_REG BMP388_DATA_1_REG
#define BMP388_PRESS_XLSB_7_0_REG BMP388_DATA_0_REG
// friendly register names, from datasheet 4.3.5
#define BMP388_TEMP_MSB_23_16_REG BMP388_DATA_5_REG
#define BMP388_TEMP_LSB_15_8_REG BMP388_DATA_4_REG
#define BMP388_TEMP_XLSB_7_0_REG BMP388_DATA_3_REG
#define BMP388_DATA_FRAME_SIZE ((BMP388_DATA_5_REG - BMP388_DATA_0_REG) + 1) // +1 for inclusive
// from Datasheet 3.3
#define BMP388_MODE_SLEEP (0x00)
#define BMP388_MODE_FORCED (0x01)
#define BMP388_MODE_NORMAL (0x02)
#define BMP388_CALIRATION_LOWER_REG (0x30) // See datasheet 4.3.19, "calibration data"
#define BMP388_TRIMMING_NVM_PAR_T1_LSB_REG (0x31) // See datasheet 3.11.1 "Memory map trimming coefficients"
#define BMP388_TRIMMING_NVM_PAR_P11_REG (0x45) // See datasheet 3.11.1 "Memory map trimming coefficients"
#define BMP388_CALIRATION_UPPER_REG (0x57)
#define BMP388_TRIMMING_DATA_LENGTH ((BMP388_TRIMMING_NVM_PAR_P11_REG - BMP388_TRIMMING_NVM_PAR_T1_LSB_REG) + 1) // +1 for inclusive
#define BMP388_OVERSAMP_1X (0x00)
#define BMP388_OVERSAMP_2X (0x01)
#define BMP388_OVERSAMP_4X (0x02)
#define BMP388_OVERSAMP_8X (0x03)
#define BMP388_OVERSAMP_16X (0x04)
#define BMP388_OVERSAMP_32X (0x05)
// INT_CTRL register
#define BMP388_INT_OD_BIT 0
#define BMP388_INT_LEVEL_BIT 1
#define BMP388_INT_LATCH_BIT 2
#define BMP388_INT_FWTM_EN_BIT 3
#define BMP388_INT_FFULL_EN_BIT 4
#define BMP388_INT_RESERVED_5_BIT 5
#define BMP388_INT_DRDY_EN_BIT 6
#define BMP388_INT_RESERVED_7_BIT 7
// OSR register
#define BMP388_OSR_P_BIT 0 // to 2
#define BMP388_OSR4_T_BIT 3 // to 5
#define BMP388_OSR_P_MASK (0x03) // -----111
#define BMP388_OSR4_T_MASK (0x38) // --111---
// configure pressure and temperature oversampling, forced sampling mode
#define BMP388_PRESSURE_OSR (BMP388_OVERSAMP_8X)
#define BMP388_TEMPERATURE_OSR (BMP388_OVERSAMP_1X)
// see Datasheet 3.11.1 Memory Map Trimming Coefficients
typedef struct bmp388_calib_param_s {
uint16_t T1;
uint16_t T2;
int8_t T3;
int16_t P1;
int16_t P2;
int8_t P3;
int8_t P4;
uint16_t P5;
uint16_t P6;
int8_t P7;
int8_t P8;
int16_t P9;
int8_t P10;
int8_t P11;
} __attribute__((packed)) bmp388_calib_param_t;
STATIC_ASSERT(sizeof(bmp388_calib_param_t) == BMP388_TRIMMING_DATA_LENGTH, bmp388_calibration_structure_incorrectly_packed);
static bmp388_calib_param_t bmp388_cal;
// uncompensated pressure and temperature
static uint32_t bmp388_up = 0;
static uint32_t bmp388_ut = 0;
static uint8_t sensor_data[BMP388_DATA_FRAME_SIZE+1];
static int64_t t_lin = 0;
static bool bmp388StartUT(baroDev_t *baro);
static bool bmp388GetUT(baroDev_t *baro);
static bool bmp388StartUP(baroDev_t *baro);
static bool bmp388GetUP(baroDev_t *baro);
static bool bmp388Calculate(baroDev_t *baro, int32_t *pressure, int32_t *temperature);
static bool bmp388BeginForcedMeasurement(busDevice_t *busdev)
{
// enable pressure measurement, temperature measurement, set power mode and start sampling
uint8_t mode = BMP388_MODE_FORCED << 4 | 1 << 1 | 1 << 0;
return busWrite(busdev, BMP388_PWR_CTRL_REG, mode);
}
static bool bmp388StartUT(baroDev_t *baro)
{
UNUSED(baro);
// dummy
return true;
}
static bool bmp388GetUT(baroDev_t *baro)
{
UNUSED(baro);
// dummy
return true;
}
static bool bmp388StartUP(baroDev_t *baro)
{
// start measurement
return bmp388BeginForcedMeasurement(baro->busDev);
}
static bool bmp388GetUP(baroDev_t *baro)
{
busReadBuf(baro->busDev, BMP388_DATA_0_REG, sensor_data, BMP388_DATA_FRAME_SIZE + 1);
bmp388_up = sensor_data[1] << 0 | sensor_data[2] << 8 | sensor_data[3] << 16;
bmp388_ut = sensor_data[4] << 0 | sensor_data[5] << 8 | sensor_data[6] << 16;
return true;
}
// Returns temperature in DegC, resolution is 0.01 DegC. Output value of "5123" equals 51.23 DegC
static int64_t bmp388CompensateTemperature(uint32_t uncomp_temperature)
{
uint64_t partial_data1;
uint64_t partial_data2;
uint64_t partial_data3;
int64_t partial_data4;
int64_t partial_data5;
int64_t partial_data6;
int64_t comp_temp;
partial_data1 = uncomp_temperature - (256 * bmp388_cal.T1);
partial_data2 = bmp388_cal.T2 * partial_data1;
partial_data3 = partial_data1 * partial_data1;
partial_data4 = (int64_t)partial_data3 * bmp388_cal.T3;
partial_data5 = ((int64_t)(partial_data2 * 262144) + partial_data4);
partial_data6 = partial_data5 / 4294967296;
/* Update t_lin, needed for pressure calculation */
t_lin = partial_data6;
comp_temp = (int64_t)((partial_data6 * 25) / 16384);
return comp_temp;
}
static uint64_t bmp388CompensatePressure(uint32_t uncomp_pressure)
{
int64_t partial_data1;
int64_t partial_data2;
int64_t partial_data3;
int64_t partial_data4;
int64_t partial_data5;
int64_t partial_data6;
int64_t offset;
int64_t sensitivity;
uint64_t comp_press;
partial_data1 = t_lin * t_lin;
partial_data2 = partial_data1 / 64;
partial_data3 = (partial_data2 * t_lin) / 256;
partial_data4 = (bmp388_cal.P8 * partial_data3) / 32;
partial_data5 = (bmp388_cal.P7 * partial_data1) * 16;
partial_data6 = (bmp388_cal.P6 * t_lin) * 4194304;
offset = (bmp388_cal.P5 * 140737488355328) + partial_data4 + partial_data5 + partial_data6;
partial_data2 = (bmp388_cal.P4 * partial_data3) / 32;
partial_data4 = (bmp388_cal.P3 * partial_data1) * 4;
partial_data5 = (bmp388_cal.P2 - 16384) * t_lin * 2097152;
sensitivity = ((bmp388_cal.P1 - 16384) * 70368744177664) + partial_data2 + partial_data4 + partial_data5;
partial_data1 = (sensitivity / 16777216) * uncomp_pressure;
partial_data2 = bmp388_cal.P10 * t_lin;
partial_data3 = partial_data2 + (65536 * bmp388_cal.P9);
partial_data4 = (partial_data3 * uncomp_pressure) / 8192;
partial_data5 = (partial_data4 * uncomp_pressure) / 512;
partial_data6 = (int64_t)((uint64_t)uncomp_pressure * (uint64_t)uncomp_pressure);
partial_data2 = (bmp388_cal.P11 * partial_data6) / 65536;
partial_data3 = (partial_data2 * uncomp_pressure) / 128;
partial_data4 = (offset / 4) + partial_data1 + partial_data5 + partial_data3;
comp_press = (((uint64_t)partial_data4 * 25) / (uint64_t)1099511627776);
return comp_press;
}
STATIC_UNIT_TESTED bool bmp388Calculate(baroDev_t *baro, int32_t *pressure, int32_t *temperature)
{
UNUSED(baro);
// calculate
int64_t t;
int64_t p;
t = bmp388CompensateTemperature(bmp388_ut);
p = bmp388CompensatePressure(bmp388_up);
if (pressure)
*pressure = (int32_t)(p / 256);
if (temperature)
*temperature = t;
return true;
}
#define DETECTION_MAX_RETRY_COUNT 5
static bool deviceDetect(busDevice_t * busDev)
{
for (int retry = 0; retry < DETECTION_MAX_RETRY_COUNT; retry++) {
uint8_t chipId[2];
delay(100);
bool ack = busReadBuf(busDev, BMP388_CHIP_ID_REG, chipId, 2);
if (ack && chipId[1] == BMP388_DEFAULT_CHIP_ID) {
return true;
}
};
return false;
}
bool bmp388Detect(baroDev_t *baro)
{
baro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_BMP388, 0, OWNER_BARO);
if (baro->busDev == NULL) {
return false;
}
busSetSpeed(baro->busDev, BUS_SPEED_STANDARD);
if (!deviceDetect(baro->busDev)) {
busDeviceDeInit(baro->busDev);
return false;
}
uint8_t calibration_buf[sizeof(bmp388_calib_param_t) + 1];
// read calibration
busReadBuf(baro->busDev, BMP388_TRIMMING_NVM_PAR_T1_LSB_REG, calibration_buf, sizeof(bmp388_calib_param_t) + 1);
memcpy(&bmp388_cal, calibration_buf + 1, sizeof(bmp388_calib_param_t));
// set oversampling + power mode (forced), and start sampling
busWrite(baro->busDev, BMP388_OSR_REG,
((BMP388_PRESSURE_OSR << BMP388_OSR_P_BIT) & BMP388_OSR_P_MASK) |
((BMP388_TEMPERATURE_OSR << BMP388_OSR4_T_BIT) & BMP388_OSR4_T_MASK)
);
bmp388BeginForcedMeasurement(baro->busDev);
baro->ut_delay = 0;
baro->get_ut = bmp388GetUT;
baro->start_ut = bmp388StartUT;
baro->up_delay = 234 + (392 + ((1 << (BMP388_PRESSURE_OSR + 1)) * 2000)) + (313 + ((1 << (BMP388_TEMPERATURE_OSR + 1)) * 2000));
baro->start_up = bmp388StartUP;
baro->get_up = bmp388GetUP;
baro->calculate = bmp388Calculate;
return true;
}
#endif

30
src/main/drivers/barometer/barometer_bmp388.h Normal file
View file

@ -0,0 +1,30 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* Cleanflight and Betaflight are distributed in the hope that they
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*
* BMP388 Driver author: Dominic Clifton
*
* References:
* BMP388 datasheet - https://ae-bst.resource.bosch.com/media/_tech/media/datasheets/BST-BMP388-DS001.pdf
* BMP3-Sensor-API - https://github.com/BoschSensortec/BMP3-Sensor-API
* BMP280 Cleanflight driver
*/
#pragma once
bool bmp388Detect(baroDev_t *baro);

255
src/main/drivers/barometer/barometer_spl006.c
View file

@ -1,255 +0,0 @@
/*
* This file is part of iNav.
*
* 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 <math.h>
#include <stdbool.h>
#include <stdint.h>
#include <platform.h>
#include "build/build_config.h"
#include "common/utils.h"
#include "drivers/time.h"
#include "drivers/io.h"
#include "drivers/bus.h"
#include "drivers/barometer/barometer.h"
#include "drivers/barometer/barometer_spl006.h"
#if defined(USE_BARO_SPL006)
// SPL006, address 0x76
typedef struct {
int16_t c0;
int16_t c1;
int32_t c00;
int32_t c10;
int16_t c01;
int16_t c11;
int16_t c20;
int16_t c21;
int16_t c30;
} spl006_coeffs_t;
spl006_coeffs_t spl006_cal;
// uncompensated pressure and temperature
static int32_t spl006_pressure_raw = 0;
static int32_t spl006_temperature_raw = 0;
static int8_t spl006_samples_to_cfg_reg_value(uint8_t sample_rate)
{
switch(sample_rate)
{
case 1: return 0;
case 2: return 1;
case 4: return 2;
case 8: return 3;
case 16: return 4;
case 32: return 5;
case 64: return 6;
case 128: return 7;
default: return -1; // invalid
}
}
static int32_t spl006_raw_value_scale_factor(uint8_t oversampling_rate)
{
switch(oversampling_rate)
{
case 1: return 524288;
case 2: return 1572864;
case 4: return 3670016;
case 8: return 7864320;
case 16: return 253952;
case 32: return 516096;
case 64: return 1040384;
case 128: return 2088960;
default: return -1; // invalid
}
}
static bool spl006_start_temperature_measurement(baroDev_t * baro)
{
return busWrite(baro->busDev, SPL006_MODE_AND_STATUS_REG, SPL006_MEAS_TEMPERATURE);
}
static bool spl006_read_temperature(baroDev_t * baro)
{
uint8_t data[SPL006_TEMPERATURE_LEN];
int32_t spl006_temperature;
bool ack = busReadBuf(baro->busDev, SPL006_TEMPERATURE_START_REG, data, SPL006_TEMPERATURE_LEN);
if (ack) {
spl006_temperature = (int32_t)((data[0] & 0x80 ? 0xFF000000 : 0) | (((uint32_t)(data[0])) << 16) | (((uint32_t)(data[1])) << 8) | ((uint32_t)data[2]));
spl006_temperature_raw = spl006_temperature;
}
return ack;
}
static bool spl006_start_pressure_measurement(baroDev_t * baro)
{
return busWrite(baro->busDev, SPL006_MODE_AND_STATUS_REG, SPL006_MEAS_PRESSURE);
}
static bool spl006_read_pressure(baroDev_t * baro)
{
uint8_t data[SPL006_PRESSURE_LEN];
int32_t spl006_pressure;
bool ack = busReadBuf(baro->busDev, SPL006_PRESSURE_START_REG, data, SPL006_PRESSURE_LEN);
if (ack) {
spl006_pressure = (int32_t)((data[0] & 0x80 ? 0xFF000000 : 0) | (((uint32_t)(data[0])) << 16) | (((uint32_t)(data[1])) << 8) | ((uint32_t)data[2]));
spl006_pressure_raw = spl006_pressure;
}
return ack;
}
// Returns temperature in degrees centigrade
static float spl006_compensate_temperature(int32_t temperature_raw)
{
const float t_raw_sc = (float)temperature_raw / spl006_raw_value_scale_factor(SPL006_TEMPERATURE_OVERSAMPLING);
const float temp_comp = (float)spl006_cal.c0 / 2 + t_raw_sc * spl006_cal.c1;
return temp_comp;
}
// Returns pressure in Pascal
static float spl006_compensate_pressure(int32_t pressure_raw, int32_t temperature_raw)
{
const float p_raw_sc = (float)pressure_raw / spl006_raw_value_scale_factor(SPL006_PRESSURE_OVERSAMPLING);
const float t_raw_sc = (float)temperature_raw / spl006_raw_value_scale_factor(SPL006_TEMPERATURE_OVERSAMPLING);
const float pressure_cal = (float)spl006_cal.c00 + p_raw_sc * ((float)spl006_cal.c10 + p_raw_sc * ((float)spl006_cal.c20 + p_raw_sc * spl006_cal.c30));
const float p_temp_comp = t_raw_sc * ((float)spl006_cal.c01 + p_raw_sc * ((float)spl006_cal.c11 + p_raw_sc * spl006_cal.c21));
return pressure_cal + p_temp_comp;
}
bool spl006_calculate(baroDev_t * baro, int32_t * pressure, int32_t * temperature)
{
UNUSED(baro);
if (pressure) {
*pressure = lrintf(spl006_compensate_pressure(spl006_pressure_raw, spl006_temperature_raw));
}
if (temperature) {
*temperature = lrintf(spl006_compensate_temperature(spl006_temperature_raw) * 100);
}
return true;
}
#define DETECTION_MAX_RETRY_COUNT 5
static bool deviceDetect(busDevice_t * busDev)
{
uint8_t chipId;
for (int retry = 0; retry < DETECTION_MAX_RETRY_COUNT; retry++) {
delay(100);
bool ack = busRead(busDev, SPL006_CHIP_ID_REG, &chipId);
if (ack && chipId == SPL006_DEFAULT_CHIP_ID) {
return true;
}
};
return false;
}
static bool read_calibration_coefficients(baroDev_t *baro) {
uint8_t sstatus;
if (!(busRead(baro->busDev, SPL006_MODE_AND_STATUS_REG, &sstatus) && (sstatus & SPL006_MEAS_CFG_COEFFS_RDY)))
return false; // error reading status or coefficients not ready
uint8_t caldata[SPL006_CALIB_COEFFS_LEN];
if (!busReadBuf(baro->busDev, SPL006_CALIB_COEFFS_START, (uint8_t *)&caldata, SPL006_CALIB_COEFFS_LEN)) {
return false;
}
spl006_cal.c0 = (caldata[0] & 0x80 ? 0xF000 : 0) | ((uint16_t)caldata[0] << 4) | (((uint16_t)caldata[1] & 0xF0) >> 4);
spl006_cal.c1 = ((caldata[1] & 0x8 ? 0xF000 : 0) | ((uint16_t)caldata[1] & 0x0F) << 8) | (uint16_t)caldata[2];
spl006_cal.c00 = (caldata[3] & 0x80 ? 0xFFF00000 : 0) | ((uint32_t)caldata[3] << 12) | ((uint32_t)caldata[4] << 4) | (((uint32_t)caldata[5] & 0xF0) >> 4);
spl006_cal.c10 = (caldata[5] & 0x8 ? 0xFFF00000 : 0) | (((uint32_t)caldata[5] & 0x0F) << 16) | ((uint32_t)caldata[6] << 8) | (uint32_t)caldata[7];
spl006_cal.c01 = ((uint16_t)caldata[8] << 8) | ((uint16_t)caldata[9]);
spl006_cal.c11 = ((uint16_t)caldata[10] << 8) | (uint16_t)caldata[11];
spl006_cal.c20 = ((uint16_t)caldata[12] << 8) | (uint16_t)caldata[13];
spl006_cal.c21 = ((uint16_t)caldata[14] << 8) | (uint16_t)caldata[15];
spl006_cal.c30 = ((uint16_t)caldata[16] << 8) | (uint16_t)caldata[17];
return true;
}
static bool spl006_configure_measurements(baroDev_t *baro)
{
uint8_t reg_value;
reg_value = SPL006_TEMP_USE_EXT_SENSOR | spl006_samples_to_cfg_reg_value(SPL006_TEMPERATURE_OVERSAMPLING);
if (!busWrite(baro->busDev, SPL006_TEMPERATURE_CFG_REG, reg_value)) {
return false;
}
reg_value = spl006_samples_to_cfg_reg_value(SPL006_PRESSURE_OVERSAMPLING);
if (!busWrite(baro->busDev, SPL006_PRESSURE_CFG_REG, reg_value)) {
return false;
}
reg_value = 0;
if (SPL006_TEMPERATURE_OVERSAMPLING > 8) {
reg_value |= SPL006_TEMPERATURE_RESULT_BIT_SHIFT;
}
if (SPL006_PRESSURE_OVERSAMPLING > 8) {
reg_value |= SPL006_PRESSURE_RESULT_BIT_SHIFT;
}
if (!busWrite(baro->busDev, SPL006_INT_AND_FIFO_CFG_REG, reg_value)) {
return false;
}
return true;
}
bool spl006Detect(baroDev_t *baro)
{
baro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_SPL006, 0, OWNER_BARO);
if (baro->busDev == NULL) {
return false;
}
busSetSpeed(baro->busDev, BUS_SPEED_STANDARD);
if (!(deviceDetect(baro->busDev) && read_calibration_coefficients(baro) && spl006_configure_measurements(baro))) {
busDeviceDeInit(baro->busDev);
return false;
}
baro->ut_delay = SPL006_MEASUREMENT_TIME(SPL006_TEMPERATURE_OVERSAMPLING) * 1000;
baro->get_ut = spl006_read_temperature;
baro->start_ut = spl006_start_temperature_measurement;
baro->up_delay = SPL006_MEASUREMENT_TIME(SPL006_PRESSURE_OVERSAMPLING) * 1000;
baro->start_up = spl006_start_pressure_measurement;
baro->get_up = spl006_read_pressure;
baro->calculate = spl006_calculate;
return true;
}
#endif

69
src/main/drivers/barometer/barometer_spl006.h
View file

@ -1,69 +0,0 @@
/*
* This file is part of iNav.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#define SPL006_I2C_ADDR 0x76
#define SPL006_DEFAULT_CHIP_ID 0x10
#define SPL006_PRESSURE_START_REG 0x00
#define SPL006_PRESSURE_LEN 3 // 24 bits, 3 bytes
#define SPL006_PRESSURE_B2_REG 0x00 // Pressure MSB Register
#define SPL006_PRESSURE_B1_REG 0x01 // Pressure middle byte Register
#define SPL006_PRESSURE_B0_REG 0x02 // Pressure LSB Register
#define SPL006_TEMPERATURE_START_REG 0x03
#define SPL006_TEMPERATURE_LEN 3 // 24 bits, 3 bytes
#define SPL006_TEMPERATURE_B2_REG 0x03 // Temperature MSB Register
#define SPL006_TEMPERATURE_B1_REG 0x04 // Temperature middle byte Register
#define SPL006_TEMPERATURE_B0_REG 0x05 // Temperature LSB Register
#define SPL006_PRESSURE_CFG_REG 0x06 // Pressure config
#define SPL006_TEMPERATURE_CFG_REG 0x07 // Temperature config
#define SPL006_MODE_AND_STATUS_REG 0x08 // Mode and status
#define SPL006_INT_AND_FIFO_CFG_REG 0x09 // Interrupt and FIFO config
#define SPL006_INT_STATUS_REG 0x0A // Interrupt and FIFO config
#define SPL006_FIFO_STATUS_REG 0x0B // Interrupt and FIFO config
#define SPL006_RST_REG 0x0C // Softreset Register
#define SPL006_CHIP_ID_REG 0x0D // Chip ID Register
#define SPL006_CALIB_COEFFS_START 0x10
#define SPL006_CALIB_COEFFS_END 0x21
#define SPL006_CALIB_COEFFS_LEN (SPL006_CALIB_COEFFS_END - SPL006_CALIB_COEFFS_START + 1)
// TEMPERATURE_CFG_REG
#define SPL006_TEMP_USE_EXT_SENSOR (1<<7)
// MODE_AND_STATUS_REG
#define SPL006_MEAS_PRESSURE (1<<0) // measure pressure
#define SPL006_MEAS_TEMPERATURE (1<<1) // measure temperature
#define SPL006_MEAS_CFG_CONTINUOUS (1<<2)
#define SPL006_MEAS_CFG_PRESSURE_RDY (1<<4)
#define SPL006_MEAS_CFG_TEMPERATURE_RDY (1<<5)
#define SPL006_MEAS_CFG_SENSOR_RDY (1<<6)
#define SPL006_MEAS_CFG_COEFFS_RDY (1<<7)
// INT_AND_FIFO_CFG_REG
#define SPL006_PRESSURE_RESULT_BIT_SHIFT (1<<2) // necessary for pressure oversampling > 8
#define SPL006_TEMPERATURE_RESULT_BIT_SHIFT (1<<3) // necessary for temperature oversampling > 8
#define SPL006_PRESSURE_OVERSAMPLING 8 // oversampling 8
#define SPL006_TEMPERATURE_OVERSAMPLING 8 // oversampling 8
#define SPL006_MEASUREMENT_TIME(oversampling) ((2 + lrintf(oversampling * 1.6)) + 1) // ms
bool spl006Detect(baroDev_t *baro);

255
src/main/drivers/barometer/barometer_spl06.c Normal file
View file

@ -0,0 +1,255 @@
/*
* This file is part of iNav.
*
* 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 <math.h>
#include <stdbool.h>
#include <stdint.h>
#include <platform.h>
#include "build/build_config.h"
#include "common/utils.h"
#include "drivers/time.h"
#include "drivers/io.h"
#include "drivers/bus.h"
#include "drivers/barometer/barometer.h"
#include "drivers/barometer/barometer_spl06.h"
#if defined(USE_BARO_SPL06)
// SPL06, address 0x76
typedef struct {
int16_t c0;
int16_t c1;
int32_t c00;
int32_t c10;
int16_t c01;
int16_t c11;
int16_t c20;
int16_t c21;
int16_t c30;
} spl06_coeffs_t;
spl06_coeffs_t spl06_cal;
// uncompensated pressure and temperature
static int32_t spl06_pressure_raw = 0;
static int32_t spl06_temperature_raw = 0;
static int8_t spl06_samples_to_cfg_reg_value(uint8_t sample_rate)
{
switch(sample_rate)
{
case 1: return 0;
case 2: return 1;
case 4: return 2;
case 8: return 3;
case 16: return 4;
case 32: return 5;
case 64: return 6;
case 128: return 7;
default: return -1; // invalid
}
}
static int32_t spl06_raw_value_scale_factor(uint8_t oversampling_rate)
{
switch(oversampling_rate)
{
case 1: return 524288;
case 2: return 1572864;
case 4: return 3670016;
case 8: return 7864320;
case 16: return 253952;
case 32: return 516096;
case 64: return 1040384;
case 128: return 2088960;
default: return -1; // invalid
}
}
static bool spl06_start_temperature_measurement(baroDev_t * baro)
{
return busWrite(baro->busDev, SPL06_MODE_AND_STATUS_REG, SPL06_MEAS_TEMPERATURE);
}
static bool spl06_read_temperature(baroDev_t * baro)
{
uint8_t data[SPL06_TEMPERATURE_LEN];
int32_t spl06_temperature;
bool ack = busReadBuf(baro->busDev, SPL06_TEMPERATURE_START_REG, data, SPL06_TEMPERATURE_LEN);
if (ack) {
spl06_temperature = (int32_t)((data[0] & 0x80 ? 0xFF000000 : 0) | (((uint32_t)(data[0])) << 16) | (((uint32_t)(data[1])) << 8) | ((uint32_t)data[2]));
spl06_temperature_raw = spl06_temperature;
}
return ack;
}
static bool spl06_start_pressure_measurement(baroDev_t * baro)
{
return busWrite(baro->busDev, SPL06_MODE_AND_STATUS_REG, SPL06_MEAS_PRESSURE);
}
static bool spl06_read_pressure(baroDev_t * baro)
{
uint8_t data[SPL06_PRESSURE_LEN];
int32_t spl06_pressure;
bool ack = busReadBuf(baro->busDev, SPL06_PRESSURE_START_REG, data, SPL06_PRESSURE_LEN);
if (ack) {
spl06_pressure = (int32_t)((data[0] & 0x80 ? 0xFF000000 : 0) | (((uint32_t)(data[0])) << 16) | (((uint32_t)(data[1])) << 8) | ((uint32_t)data[2]));
spl06_pressure_raw = spl06_pressure;
}
return ack;
}
// Returns temperature in degrees centigrade
static float spl06_compensate_temperature(int32_t temperature_raw)
{
const float t_raw_sc = (float)temperature_raw / spl06_raw_value_scale_factor(SPL06_TEMPERATURE_OVERSAMPLING);
const float temp_comp = (float)spl06_cal.c0 / 2 + t_raw_sc * spl06_cal.c1;
return temp_comp;
}
// Returns pressure in Pascal
static float spl06_compensate_pressure(int32_t pressure_raw, int32_t temperature_raw)
{
const float p_raw_sc = (float)pressure_raw / spl06_raw_value_scale_factor(SPL06_PRESSURE_OVERSAMPLING);
const float t_raw_sc = (float)temperature_raw / spl06_raw_value_scale_factor(SPL06_TEMPERATURE_OVERSAMPLING);
const float pressure_cal = (float)spl06_cal.c00 + p_raw_sc * ((float)spl06_cal.c10 + p_raw_sc * ((float)spl06_cal.c20 + p_raw_sc * spl06_cal.c30));
const float p_temp_comp = t_raw_sc * ((float)spl06_cal.c01 + p_raw_sc * ((float)spl06_cal.c11 + p_raw_sc * spl06_cal.c21));
return pressure_cal + p_temp_comp;
}
bool spl06_calculate(baroDev_t * baro, int32_t * pressure, int32_t * temperature)
{
UNUSED(baro);
if (pressure) {
*pressure = lrintf(spl06_compensate_pressure(spl06_pressure_raw, spl06_temperature_raw));
}
if (temperature) {
*temperature = lrintf(spl06_compensate_temperature(spl06_temperature_raw) * 100);
}
return true;
}
#define DETECTION_MAX_RETRY_COUNT 5
static bool deviceDetect(busDevice_t * busDev)
{
uint8_t chipId;
for (int retry = 0; retry < DETECTION_MAX_RETRY_COUNT; retry++) {
delay(100);
bool ack = busRead(busDev, SPL06_CHIP_ID_REG, &chipId);
if (ack && chipId == SPL06_DEFAULT_CHIP_ID) {
return true;
}
};
return false;
}
static bool read_calibration_coefficients(baroDev_t *baro) {
uint8_t sstatus;
if (!(busRead(baro->busDev, SPL06_MODE_AND_STATUS_REG, &sstatus) && (sstatus & SPL06_MEAS_CFG_COEFFS_RDY)))
return false; // error reading status or coefficients not ready
uint8_t caldata[SPL06_CALIB_COEFFS_LEN];
if (!busReadBuf(baro->busDev, SPL06_CALIB_COEFFS_START, (uint8_t *)&caldata, SPL06_CALIB_COEFFS_LEN)) {
return false;
}
spl06_cal.c0 = (caldata[0] & 0x80 ? 0xF000 : 0) | ((uint16_t)caldata[0] << 4) | (((uint16_t)caldata[1] & 0xF0) >> 4);
spl06_cal.c1 = ((caldata[1] & 0x8 ? 0xF000 : 0) | ((uint16_t)caldata[1] & 0x0F) << 8) | (uint16_t)caldata[2];
spl06_cal.c00 = (caldata[3] & 0x80 ? 0xFFF00000 : 0) | ((uint32_t)caldata[3] << 12) | ((uint32_t)caldata[4] << 4) | (((uint32_t)caldata[5] & 0xF0) >> 4);
spl06_cal.c10 = (caldata[5] & 0x8 ? 0xFFF00000 : 0) | (((uint32_t)caldata[5] & 0x0F) << 16) | ((uint32_t)caldata[6] << 8) | (uint32_t)caldata[7];
spl06_cal.c01 = ((uint16_t)caldata[8] << 8) | ((uint16_t)caldata[9]);
spl06_cal.c11 = ((uint16_t)caldata[10] << 8) | (uint16_t)caldata[11];
spl06_cal.c20 = ((uint16_t)caldata[12] << 8) | (uint16_t)caldata[13];
spl06_cal.c21 = ((uint16_t)caldata[14] << 8) | (uint16_t)caldata[15];
spl06_cal.c30 = ((uint16_t)caldata[16] << 8) | (uint16_t)caldata[17];
return true;
}
static bool spl06_configure_measurements(baroDev_t *baro)
{
uint8_t reg_value;
reg_value = SPL06_TEMP_USE_EXT_SENSOR | spl06_samples_to_cfg_reg_value(SPL06_TEMPERATURE_OVERSAMPLING);
if (!busWrite(baro->busDev, SPL06_TEMPERATURE_CFG_REG, reg_value)) {
return false;
}
reg_value = spl06_samples_to_cfg_reg_value(SPL06_PRESSURE_OVERSAMPLING);
if (!busWrite(baro->busDev, SPL06_PRESSURE_CFG_REG, reg_value)) {
return false;
}
reg_value = 0;
if (SPL06_TEMPERATURE_OVERSAMPLING > 8) {
reg_value |= SPL06_TEMPERATURE_RESULT_BIT_SHIFT;
}
if (SPL06_PRESSURE_OVERSAMPLING > 8) {
reg_value |= SPL06_PRESSURE_RESULT_BIT_SHIFT;
}
if (!busWrite(baro->busDev, SPL06_INT_AND_FIFO_CFG_REG, reg_value)) {
return false;
}
return true;
}
bool spl06Detect(baroDev_t *baro)
{
baro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_SPL06, 0, OWNER_BARO);
if (baro->busDev == NULL) {
return false;
}
busSetSpeed(baro->busDev, BUS_SPEED_STANDARD);
if (!(deviceDetect(baro->busDev) && read_calibration_coefficients(baro) && spl06_configure_measurements(baro))) {
busDeviceDeInit(baro->busDev);
return false;
}
baro->ut_delay = SPL06_MEASUREMENT_TIME(SPL06_TEMPERATURE_OVERSAMPLING) * 1000;
baro->get_ut = spl06_read_temperature;
baro->start_ut = spl06_start_temperature_measurement;
baro->up_delay = SPL06_MEASUREMENT_TIME(SPL06_PRESSURE_OVERSAMPLING) * 1000;
baro->start_up = spl06_start_pressure_measurement;
baro->get_up = spl06_read_pressure;
baro->calculate = spl06_calculate;
return true;
}
#endif

69
src/main/drivers/barometer/barometer_spl06.h Normal file
View file

@ -0,0 +1,69 @@
/*
* This file is part of iNav.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#define SPL06_I2C_ADDR 0x76
#define SPL06_DEFAULT_CHIP_ID 0x10
#define SPL06_PRESSURE_START_REG 0x00
#define SPL06_PRESSURE_LEN 3 // 24 bits, 3 bytes
#define SPL06_PRESSURE_B2_REG 0x00 // Pressure MSB Register
#define SPL06_PRESSURE_B1_REG 0x01 // Pressure middle byte Register
#define SPL06_PRESSURE_B0_REG 0x02 // Pressure LSB Register
#define SPL06_TEMPERATURE_START_REG 0x03
#define SPL06_TEMPERATURE_LEN 3 // 24 bits, 3 bytes
#define SPL06_TEMPERATURE_B2_REG 0x03 // Temperature MSB Register
#define SPL06_TEMPERATURE_B1_REG 0x04 // Temperature middle byte Register
#define SPL06_TEMPERATURE_B0_REG 0x05 // Temperature LSB Register
#define SPL06_PRESSURE_CFG_REG 0x06 // Pressure config
#define SPL06_TEMPERATURE_CFG_REG 0x07 // Temperature config
#define SPL06_MODE_AND_STATUS_REG 0x08 // Mode and status
#define SPL06_INT_AND_FIFO_CFG_REG 0x09 // Interrupt and FIFO config
#define SPL06_INT_STATUS_REG 0x0A // Interrupt and FIFO config
#define SPL06_FIFO_STATUS_REG 0x0B // Interrupt and FIFO config
#define SPL06_RST_REG 0x0C // Softreset Register
#define SPL06_CHIP_ID_REG 0x0D // Chip ID Register
#define SPL06_CALIB_COEFFS_START 0x10
#define SPL06_CALIB_COEFFS_END 0x21
#define SPL06_CALIB_COEFFS_LEN (SPL06_CALIB_COEFFS_END - SPL06_CALIB_COEFFS_START + 1)
// TEMPERATURE_CFG_REG
#define SPL06_TEMP_USE_EXT_SENSOR (1<<7)
// MODE_AND_STATUS_REG
#define SPL06_MEAS_PRESSURE (1<<0) // measure pressure
#define SPL06_MEAS_TEMPERATURE (1<<1) // measure temperature
#define SPL06_MEAS_CFG_CONTINUOUS (1<<2)
#define SPL06_MEAS_CFG_PRESSURE_RDY (1<<4)
#define SPL06_MEAS_CFG_TEMPERATURE_RDY (1<<5)
#define SPL06_MEAS_CFG_SENSOR_RDY (1<<6)
#define SPL06_MEAS_CFG_COEFFS_RDY (1<<7)
// INT_AND_FIFO_CFG_REG
#define SPL06_PRESSURE_RESULT_BIT_SHIFT (1<<2) // necessary for pressure oversampling > 8
#define SPL06_TEMPERATURE_RESULT_BIT_SHIFT (1<<3) // necessary for temperature oversampling > 8
#define SPL06_PRESSURE_OVERSAMPLING 8 // oversampling 8
#define SPL06_TEMPERATURE_OVERSAMPLING 8 // oversampling 8
#define SPL06_MEASUREMENT_TIME(oversampling) ((2 + lrintf(oversampling * 1.6)) + 1) // ms
bool spl06Detect(baroDev_t *baro);

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

@ -102,7 +102,8 @@ typedef enum {
DEVHW_MS5611, DEVHW_MS5611,
DEVHW_MS5607, DEVHW_MS5607,
DEVHW_LPS25H, DEVHW_LPS25H,
DEVHW_SPL006, DEVHW_SPL06,
DEVHW_BMP388,
/* Compass chips */ /* Compass chips */
DEVHW_HMC5883, DEVHW_HMC5883,

72
src/main/drivers/display.c
View file

@ -25,9 +25,11 @@
#include "config/parameter_group_ids.h" #include "config/parameter_group_ids.h"
#include "drivers/display.h"
#include "drivers/display_canvas.h"
#include "drivers/display_font_metadata.h"
#include "drivers/time.h" #include "drivers/time.h"
#include "display.h"
#define SW_BLINK_CYCLE_MS 200 // 200ms on / 200ms off #define SW_BLINK_CYCLE_MS 200 // 200ms on / 200ms off
@ -72,6 +74,19 @@ static bool displayEmulateTextAttributes(displayPort_t *instance,
return false; return false;
} }
static void displayUpdateMaxChar(displayPort_t *instance)
{
if (displayIsReady(instance)) {
displayFontMetadata_t metadata;
if (displayGetFontMetadata(&metadata, instance)) {
instance->maxChar = metadata.charCount - 1;
} else {
// Assume 8-bit character implementation
instance->maxChar = 255;
}
}
}
void displayClearScreen(displayPort_t *instance) void displayClearScreen(displayPort_t *instance)
{ {
instance->vTable->clearScreen(instance); instance->vTable->clearScreen(instance);
@ -157,6 +172,9 @@ int displayWriteWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, const ch
int displayWriteChar(displayPort_t *instance, uint8_t x, uint8_t y, uint16_t c) int displayWriteChar(displayPort_t *instance, uint8_t x, uint8_t y, uint16_t c)
{ {
if (instance->maxChar == 0) {
displayUpdateMaxChar(instance);
}
if (c > instance->maxChar) { if (c > instance->maxChar) {
return -1; return -1;
} }
@ -167,6 +185,9 @@ int displayWriteChar(displayPort_t *instance, uint8_t x, uint8_t y, uint16_t c)
int displayWriteCharWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, uint16_t c, textAttributes_t attr) int displayWriteCharWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, uint16_t c, textAttributes_t attr)
{ {
if (instance->maxChar == 0) {
displayUpdateMaxChar(instance);
}
if (c > instance->maxChar) { if (c > instance->maxChar) {
return -1; return -1;
} }
@ -237,6 +258,46 @@ int displayWriteFontCharacter(displayPort_t *instance, uint16_t addr, const osdC
return -1; return -1;
} }
bool displayIsReady(displayPort_t *instance)
{
if (instance->vTable->isReady) {
return instance->vTable->isReady(instance);
}
// Drivers that don't provide an isReady method are
// assumed to be immediately ready (either by actually
// begin ready very quickly or by blocking)
return true;
}
void displayBeginTransaction(displayPort_t *instance, displayTransactionOption_e opts)
{
if (instance->vTable->beginTransaction) {
instance->vTable->beginTransaction(instance, opts);
}
}
void displayCommitTransaction(displayPort_t *instance)
{
if (instance->vTable->commitTransaction) {
instance->vTable->commitTransaction(instance);
}
}
bool displayGetCanvas(displayCanvas_t *canvas, const displayPort_t *instance)
{
#if defined(USE_CANVAS)
if (canvas && instance->vTable->getCanvas && instance->vTable->getCanvas(canvas, instance)) {
canvas->gridElementWidth = canvas->width / instance->cols;
canvas->gridElementHeight = canvas->height / instance->rows;
return true;
}
#else
UNUSED(canvas);
UNUSED(instance);
#endif
return false;
}
void displayInit(displayPort_t *instance, const displayPortVTable_t *vTable) void displayInit(displayPort_t *instance, const displayPortVTable_t *vTable)
{ {
instance->vTable = vTable; instance->vTable = vTable;
@ -253,12 +314,7 @@ void displayInit(displayPort_t *instance, const displayPortVTable_t *vTable)
TEXT_ATTRIBUTES_REMOVE_BLINK(instance->cachedSupportedTextAttributes); TEXT_ATTRIBUTES_REMOVE_BLINK(instance->cachedSupportedTextAttributes);
} }
displayFontMetadata_t metadata; instance->maxChar = 0;
if (displayGetFontMetadata(&metadata, instance)) { displayUpdateMaxChar(instance);
instance->maxChar = metadata.charCount - 1;
} else {
// Assume 8-bit character implementation
instance->maxChar = 255;
}
} }

24
src/main/drivers/display.h
View file

@ -30,6 +30,12 @@ typedef struct displayConfig_s {
PG_DECLARE(displayConfig_t, displayConfig); PG_DECLARE(displayConfig_t, displayConfig);
typedef enum {
DISPLAY_TRANSACTION_OPT_NONE = 0,
DISPLAY_TRANSACTION_OPT_PROFILED = 1 << 0,
DISPLAY_TRANSACTION_OPT_RESET_DRAWING = 1 << 1,
} displayTransactionOption_e;
// Represents the attributes for a given piece of text // Represents the attributes for a given piece of text
// either a single character or a string. For forward // either a single character or a string. For forward
// compatibility, always use the TEXT_ATTRIBUTE... // compatibility, always use the TEXT_ATTRIBUTE...
@ -56,14 +62,12 @@ typedef uint8_t textAttributes_t;
static inline void TEXT_ATTRIBUTES_COPY(textAttributes_t *dst, textAttributes_t *src) { *dst = *src; } static inline void TEXT_ATTRIBUTES_COPY(textAttributes_t *dst, textAttributes_t *src) { *dst = *src; }
typedef struct displayFontMetadata_s { typedef struct displayCanvas_s displayCanvas_t;
uint8_t version; typedef struct displayFontMetadata_s displayFontMetadata_t;
uint16_t charCount; typedef struct displayPortVTable_s displayPortVTable_t;
} displayFontMetadata_t;
struct displayPortVTable_s;
typedef struct displayPort_s { typedef struct displayPort_s {
const struct displayPortVTable_s *vTable; const displayPortVTable_t *vTable;
void *device; void *device;
uint8_t rows; uint8_t rows;
uint8_t cols; uint8_t cols;
@ -95,6 +99,10 @@ typedef struct displayPortVTable_s {
textAttributes_t (*supportedTextAttributes)(const displayPort_t *displayPort); textAttributes_t (*supportedTextAttributes)(const displayPort_t *displayPort);
bool (*getFontMetadata)(displayFontMetadata_t *metadata, const displayPort_t *displayPort); bool (*getFontMetadata)(displayFontMetadata_t *metadata, const displayPort_t *displayPort);
int (*writeFontCharacter)(displayPort_t *instance, uint16_t addr, const osdCharacter_t *chr); int (*writeFontCharacter)(displayPort_t *instance, uint16_t addr, const osdCharacter_t *chr);
bool (*isReady)(displayPort_t *displayPort);
void (*beginTransaction)(displayPort_t *displayPort, displayTransactionOption_e opts);
void (*commitTransaction)(displayPort_t *displayPort);
bool (*getCanvas)(displayCanvas_t *canvas, const displayPort_t *displayPort);
} displayPortVTable_t; } displayPortVTable_t;
typedef struct displayPortProfile_s { typedef struct displayPortProfile_s {
@ -124,4 +132,8 @@ void displayResync(displayPort_t *instance);
uint16_t displayTxBytesFree(const displayPort_t *instance); uint16_t displayTxBytesFree(const displayPort_t *instance);
bool displayGetFontMetadata(displayFontMetadata_t *metadata, const displayPort_t *instance); bool displayGetFontMetadata(displayFontMetadata_t *metadata, const displayPort_t *instance);
int displayWriteFontCharacter(displayPort_t *instance, uint16_t addr, const osdCharacter_t *chr); int displayWriteFontCharacter(displayPort_t *instance, uint16_t addr, const osdCharacter_t *chr);
bool displayIsReady(displayPort_t *instance);
void displayBeginTransaction(displayPort_t *instance, displayTransactionOption_e opts);
void displayCommitTransaction(displayPort_t *instance);
bool displayGetCanvas(displayCanvas_t *canvas, const displayPort_t *instance);
void displayInit(displayPort_t *instance, const displayPortVTable_t *vTable); void displayInit(displayPort_t *instance, const displayPortVTable_t *vTable);

275
src/main/drivers/display_canvas.c Normal file
View file

@ -0,0 +1,275 @@
/*
* This file is part of INAV.
*
* 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/.
*
* @author Alberto Garcia Hierro <alberto@garciahierro.com>
*/
#include "drivers/display_canvas.h"
void displayCanvasSetStrokeColor(displayCanvas_t *displayCanvas, displayCanvasColor_e color)
{
if (displayCanvas->vTable->setStrokeColor) {
displayCanvas->vTable->setStrokeColor(displayCanvas, color);
}
}
void displayCanvasSetFillColor(displayCanvas_t *displayCanvas, displayCanvasColor_e color)
{
if (displayCanvas->vTable->setFillColor) {
displayCanvas->vTable->setFillColor(displayCanvas, color);
}
}
void displayCanvasSetStrokeAndFillColor(displayCanvas_t *displayCanvas, displayCanvasColor_e color)
{
if (displayCanvas->vTable->setStrokeAndFillColor) {
displayCanvas->vTable->setStrokeAndFillColor(displayCanvas, color);
} else {
displayCanvasSetStrokeColor(displayCanvas, color);
displayCanvasSetFillColor(displayCanvas, color);
}
}
void displayCanvasSetColorInversion(displayCanvas_t *displayCanvas, bool inverted)
{
if (displayCanvas->vTable->setColorInversion) {
displayCanvas->vTable->setColorInversion(displayCanvas, inverted);
}
}
void displayCanvasSetPixel(displayCanvas_t *displayCanvas, int x, int y, displayCanvasColor_e color)
{
if (displayCanvas->vTable->setPixel) {
displayCanvas->vTable->setPixel(displayCanvas, x, y, color);
}
}
void displayCanvasSetPixelToStrokeColor(displayCanvas_t *displayCanvas, int x, int y)
{
if (displayCanvas->vTable->setPixelToStrokeColor) {
displayCanvas->vTable->setPixelToStrokeColor(displayCanvas, x, y);
}
}
void displayCanvasSetPixelToFillColor(displayCanvas_t *displayCanvas, int x, int y)
{
if (displayCanvas->vTable->setPixelToFillColor) {
displayCanvas->vTable->setPixelToFillColor(displayCanvas, x, y);
}
}
void displayCanvasSetStrokeWidth(displayCanvas_t *displayCanvas, unsigned w)
{
if (displayCanvas->vTable->setStrokeWidth) {
displayCanvas->vTable->setStrokeWidth(displayCanvas, w);
}
}
void displayCanvasSetLineOutlineType(displayCanvas_t *displayCanvas, displayCanvasOutlineType_e outlineType)
{
if (displayCanvas->vTable->setLineOutlineType) {
displayCanvas->vTable->setLineOutlineType(displayCanvas, outlineType);
}
}
void displayCanvasSetLineOutlineColor(displayCanvas_t *displayCanvas, displayCanvasColor_e outlineColor)
{
if (displayCanvas->vTable->setLineOutlineColor) {
displayCanvas->vTable->setLineOutlineColor(displayCanvas, outlineColor);
}
}
void displayCanvasClipToRect(displayCanvas_t *displayCanvas, int x, int y, int w, int h)
{
if (displayCanvas->vTable->clipToRect) {
displayCanvas->vTable->clipToRect(displayCanvas, x, y, w, h);
}
}
void displayCanvasClearRect(displayCanvas_t *displayCanvas, int x, int y, int w, int h)
{
if (displayCanvas->vTable->clearRect) {
displayCanvas->vTable->clearRect(displayCanvas, x, y, w, h);
}
}
void displayCanvasResetDrawingState(displayCanvas_t *displayCanvas)
{
if (displayCanvas->vTable->resetDrawingState) {
displayCanvas->vTable->resetDrawingState(displayCanvas);
}
}
void displayCanvasDrawCharacter(displayCanvas_t *displayCanvas, int x, int y, uint16_t chr, displayCanvasBitmapOption_t opts)
{
if (displayCanvas->vTable->drawCharacter) {
displayCanvas->vTable->drawCharacter(displayCanvas, x, y, chr, opts);
}
}
void displayCanvasDrawCharacterMask(displayCanvas_t *displayCanvas, int x, int y, uint16_t chr, displayCanvasColor_e color, displayCanvasBitmapOption_t opts)
{
if (displayCanvas->vTable->drawCharacterMask) {
displayCanvas->vTable->drawCharacterMask(displayCanvas, x, y, chr, color, opts);
}
}
void displayCanvasDrawString(displayCanvas_t *displayCanvas, int x, int y, const char *s, displayCanvasBitmapOption_t opts)
{
if (displayCanvas->vTable->drawString) {
displayCanvas->vTable->drawString(displayCanvas, x, y, s, opts);
}
}
void displayCanvasDrawStringMask(displayCanvas_t *displayCanvas, int x, int y, const char *s, displayCanvasColor_e color, displayCanvasBitmapOption_t opts)
{
if (displayCanvas->vTable->drawStringMask) {
displayCanvas->vTable->drawStringMask(displayCanvas, x, y, s, color, opts);
}
}
void displayCanvasMoveToPoint(displayCanvas_t *displayCanvas, int x, int y)
{
if (displayCanvas->vTable->moveToPoint) {
displayCanvas->vTable->moveToPoint(displayCanvas, x, y);
}
}
void displayCanvasStrokeLineToPoint(displayCanvas_t *displayCanvas, int x, int y)
{
if (displayCanvas->vTable->strokeLineToPoint) {
displayCanvas->vTable->strokeLineToPoint(displayCanvas, x, y);
}
}
void displayCanvasStrokeTriangle(displayCanvas_t *displayCanvas, int x1, int y1, int x2, int y2, int x3, int y3)
{
if (displayCanvas->vTable->strokeTriangle) {
displayCanvas->vTable->strokeTriangle(displayCanvas, x1, y1, x2, y2, x3, y3);
}
}
void displayCanvasFillTriangle(displayCanvas_t *displayCanvas, int x1, int y1, int x2, int y2, int x3, int y3)
{
if (displayCanvas->vTable->fillTriangle) {
displayCanvas->vTable->fillTriangle(displayCanvas, x1, y1, x2, y2, x3, y3);
}
}
void displayCanvasFillStrokeTriangle(displayCanvas_t *displayCanvas, int x1, int y1, int x2, int y2, int x3, int y3)
{
if (displayCanvas->vTable->fillStrokeTriangle) {
displayCanvas->vTable->fillStrokeTriangle(displayCanvas, x1, y1, x2, y2, x3, y3);
}
}
void displayCanvasStrokeRect(displayCanvas_t *displayCanvas, int x, int y, int w, int h)
{
if (displayCanvas->vTable->strokeRect) {
displayCanvas->vTable->strokeRect(displayCanvas, x, y, w, h);
}
}
void displayCanvasFillRect(displayCanvas_t *displayCanvas, int x, int y, int w, int h)
{
if (displayCanvas->vTable->fillRect) {
displayCanvas->vTable->fillRect(displayCanvas, x, y, w, h);
}
}
void displayCanvasFillStrokeRect(displayCanvas_t *displayCanvas, int x, int y, int w, int h)
{
if (displayCanvas->vTable->fillStrokeRect) {
displayCanvas->vTable->fillStrokeRect(displayCanvas, x, y, w, h);
}
}
void displayCanvasStrokeEllipseInRect(displayCanvas_t *displayCanvas, int x, int y, int w, int h)
{
if (displayCanvas->vTable->strokeEllipseInRect) {
displayCanvas->vTable->strokeEllipseInRect(displayCanvas, x, y, w, h);
}
}
void displayCanvasFillEllipseInRect(displayCanvas_t *displayCanvas, int x, int y, int w, int h)
{
if (displayCanvas->vTable->fillEllipseInRect) {
displayCanvas->vTable->fillEllipseInRect(displayCanvas, x, y, w, h);
}
}
void displayCanvasFillStrokeEllipseInRect(displayCanvas_t *displayCanvas, int x, int y, int w, int h)
{
if (displayCanvas->vTable->fillStrokeEllipseInRect) {
displayCanvas->vTable->fillStrokeEllipseInRect(displayCanvas, x, y, w, h);
}
}
void displayCanvasCtmReset(displayCanvas_t *displayCanvas)
{
if (displayCanvas->vTable->ctmReset) {
displayCanvas->vTable->ctmReset(displayCanvas);
}
}
void displayCanvasCtmSet(displayCanvas_t *displayCanvas, float m11, float m12, float m21, float m22, float m31, float m32)
{
if (displayCanvas->vTable->ctmSet) {
displayCanvas->vTable->ctmSet(displayCanvas, m11, m12, m21, m22, m31, m32);
}
}
void displayCanvasCtmTranslate(displayCanvas_t *displayCanvas, float tx, float ty)
{
if (displayCanvas->vTable->ctmTranslate) {
displayCanvas->vTable->ctmTranslate(displayCanvas, tx, ty);
}
}
void displayCanvasCtmScale(displayCanvas_t *displayCanvas, float sx, float sy)
{
if (displayCanvas->vTable->ctmScale) {
displayCanvas->vTable->ctmScale(displayCanvas, sx, sy);
}
}
void displayCanvasCtmRotate(displayCanvas_t *displayCanvas, float r)
{
if (displayCanvas->vTable->ctmRotate) {
displayCanvas->vTable->ctmRotate(displayCanvas, r);
}
}
void displayCanvasContextPush(displayCanvas_t *displayCanvas)
{
if (displayCanvas->vTable->contextPush) {
displayCanvas->vTable->contextPush(displayCanvas);
}
}
void displayCanvasContextPop(displayCanvas_t *displayCanvas)
{
if (displayCanvas->vTable->contextPop) {
displayCanvas->vTable->contextPop(displayCanvas);
}
}

143
src/main/drivers/display_canvas.h Normal file
View file

@ -0,0 +1,143 @@
/*
* This file is part of INAV.
*
* 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/.
*
* @author Alberto Garcia Hierro <alberto@garciahierro.com>
*/
#pragma once
#include <stdbool.h>
#include <stdint.h>
typedef enum {
DISPLAY_CANVAS_BITMAP_OPT_INVERT_COLORS = 1 << 0,
DISPLAY_CANVAS_BITMAP_OPT_SOLID_BACKGROUND = 1 << 1,
DISPLAY_CANVAS_BITMAP_OPT_ERASE_TRANSPARENT = 1 << 2,
} displayCanvasBitmapOption_t;
typedef enum {
DISPLAY_CANVAS_COLOR_BLACK = 0,
DISPLAY_CANVAS_COLOR_TRANSPARENT = 1,
DISPLAY_CANVAS_COLOR_WHITE = 2,
DISPLAY_CANVAS_COLOR_GRAY = 3,
} displayCanvasColor_e;
typedef enum {
DISPLAY_CANVAS_OUTLINE_TYPE_NONE = 0,
DISPLAY_CANVAS_OUTLINE_TYPE_TOP = 1 << 0,
DISPLAY_CANVAS_OUTLINE_TYPE_RIGHT = 1 << 1,
DISPLAY_CANVAS_OUTLINE_TYPE_BOTTOM = 1 << 2,
DISPLAY_CANVAS_OUTLINE_TYPE_LEFT = 1 << 3,
} displayCanvasOutlineType_e;
typedef struct displayCanvasVTable_s displayCanvasVTable_t;
typedef struct displayCanvas_s {
const displayCanvasVTable_t *vTable;
void *device;
uint16_t width;
uint16_t height;
uint8_t gridElementWidth;
uint8_t gridElementHeight;
} displayCanvas_t;
typedef struct displayCanvasVTable_s {
void (*setStrokeColor)(displayCanvas_t *displayCanvas, displayCanvasColor_e color);
void (*setFillColor)(displayCanvas_t *displayCanvas, displayCanvasColor_e color);
void (*setStrokeAndFillColor)(displayCanvas_t *displayCanvas, displayCanvasColor_e color);
void (*setColorInversion)(displayCanvas_t *displayCanvas, bool inverted);
void (*setPixel)(displayCanvas_t *displayCanvas, int x, int y, displayCanvasColor_e color);
void (*setPixelToStrokeColor)(displayCanvas_t *displayCanvas, int x, int y);
void (*setPixelToFillColor)(displayCanvas_t *displayCanvas, int x, int y);
void (*setStrokeWidth)(displayCanvas_t *displayCanvas, unsigned w);
void (*setLineOutlineType)(displayCanvas_t *displayCanvas, displayCanvasOutlineType_e outlineType);
void (*setLineOutlineColor)(displayCanvas_t *displayCanvas, displayCanvasColor_e outlineColor);
void (*clipToRect)(displayCanvas_t *displayCanvas, int x, int y, int w, int h);
void (*clearRect)(displayCanvas_t *displayCanvas, int x, int y, int w, int h);
void (*resetDrawingState)(displayCanvas_t *displayCanvas);
void (*drawCharacter)(displayCanvas_t *displayCanvas, int x, int y, uint16_t chr, displayCanvasBitmapOption_t opts);
void (*drawCharacterMask)(displayCanvas_t *displayCanvas, int x, int y, uint16_t chr, displayCanvasColor_e color, displayCanvasBitmapOption_t opts);
void (*drawString)(displayCanvas_t *displayCanvas, int x, int y, const char *s, displayCanvasBitmapOption_t opts);
void (*drawStringMask)(displayCanvas_t *displayCanvas, int x, int y, const char *s, displayCanvasColor_e color, displayCanvasBitmapOption_t opts);
void (*moveToPoint)(displayCanvas_t *displayCanvas, int x, int y);
void (*strokeLineToPoint)(displayCanvas_t *displayCanvas, int x, int y);
void (*strokeTriangle)(displayCanvas_t *displayCanvas, int x1, int y1, int x2, int y2, int x3, int y3);
void (*fillTriangle)(displayCanvas_t *displayCanvas, int x1, int y1, int x2, int y2, int x3, int y3);
void (*fillStrokeTriangle)(displayCanvas_t *displayCanvas, int x1, int y1, int x2, int y2, int x3, int y3);
void (*strokeRect)(displayCanvas_t *displayCanvas, int x, int y, int w, int h);
void (*fillRect)(displayCanvas_t *displayCanvas, int x, int y, int w, int h);
void (*fillStrokeRect)(displayCanvas_t *displayCanvas, int x, int y, int w, int h);
void (*strokeEllipseInRect)(displayCanvas_t *displayCanvas, int x, int y, int w, int h);
void (*fillEllipseInRect)(displayCanvas_t *displayCanvas, int x, int y, int w, int h);
void (*fillStrokeEllipseInRect)(displayCanvas_t *displayCanvas, int x, int y, int w, int h);
void (*ctmReset)(displayCanvas_t *displayCanvas);
void (*ctmSet)(displayCanvas_t *displayCanvas, float m11, float m12, float m21, float m22, float m31, float m32);
void (*ctmTranslate)(displayCanvas_t *displayCanvas, float tx, float ty);
void (*ctmScale)(displayCanvas_t *displayCanvas, float sx, float sy);
void (*ctmRotate)(displayCanvas_t *displayCanvas, float r);
void (*contextPush)(displayCanvas_t *displayCanvas);
void (*contextPop)(displayCanvas_t *displayCanvas);
} displayCanvasVTable_t;
void displayCanvasSetStrokeColor(displayCanvas_t *displayCanvas, displayCanvasColor_e color);
void displayCanvasSetFillColor(displayCanvas_t *displayCanvas, displayCanvasColor_e color);
void displayCanvasSetStrokeAndFillColor(displayCanvas_t *displayCanvas, displayCanvasColor_e color);
void displayCanvasSetColorInversion(displayCanvas_t *displayCanvas, bool inverted);
void displayCanvasSetPixel(displayCanvas_t *displayCanvas, int x, int y, displayCanvasColor_e);
void displayCanvasSetPixelToStrokeColor(displayCanvas_t *displayCanvas, int x, int y);
void displayCanvasSetPixelToFillColor(displayCanvas_t *displayCanvas, int x, int y);
void displayCanvasSetStrokeWidth(displayCanvas_t *displayCanvas, unsigned w);
void displayCanvasSetLineOutlineType(displayCanvas_t *displayCanvas, displayCanvasOutlineType_e outlineType);
void displayCanvasSetLineOutlineColor(displayCanvas_t *displayCanvas, displayCanvasColor_e outlineColor);
void displayCanvasClipToRect(displayCanvas_t *displayCanvas, int x, int y, int w, int h);
void displayCanvasClearRect(displayCanvas_t *displayCanvas, int x, int y, int w, int h);
void displayCanvasResetDrawingState(displayCanvas_t *displayCanvas);
void displayCanvasDrawCharacter(displayCanvas_t *displayCanvas, int x, int y, uint16_t chr, displayCanvasBitmapOption_t opts);
void displayCanvasDrawCharacterMask(displayCanvas_t *displayCanvas, int x, int y, uint16_t chr, displayCanvasColor_e color, displayCanvasBitmapOption_t opts);
void displayCanvasDrawString(displayCanvas_t *displayCanvas, int x, int y, const char *s, displayCanvasBitmapOption_t opts);
void displayCanvasDrawStringMask(displayCanvas_t *displayCanvas, int x, int y, const char *s, displayCanvasColor_e color, displayCanvasBitmapOption_t opts);
void displayCanvasMoveToPoint(displayCanvas_t *displayCanvas, int x, int y);
void displayCanvasStrokeLineToPoint(displayCanvas_t *displayCanvas, int x, int y);
void displayCanvasStrokeTriangle(displayCanvas_t *displayCanvas, int x1, int y1, int x2, int y2, int x3, int y3);
void displayCanvasFillTriangle(displayCanvas_t *displayCanvas, int x1, int y1, int x2, int y2, int x3, int y3);
void displayCanvasFillStrokeTriangle(displayCanvas_t *displayCanvas, int x1, int y1, int x2, int y2, int x3, int y3);
void displayCanvasStrokeRect(displayCanvas_t *displayCanvas, int x, int y, int w, int h);
void displayCanvasFillRect(displayCanvas_t *displayCanvas, int x, int y, int w, int h);
void displayCanvasFillStrokeRect(displayCanvas_t *displayCanvas, int x, int y, int w, int h);
void displayCanvasStrokeEllipseInRect(displayCanvas_t *displayCanvas, int x, int y, int w, int h);
void displayCanvasFillEllipseInRect(displayCanvas_t *displayCanvas, int x, int y, int w, int h);
void displayCanvasFillStrokeEllipseInRect(displayCanvas_t *displayCanvas, int x, int y, int w, int h);
void displayCanvasCtmReset(displayCanvas_t *displayCanvas);
void displayCanvasCtmSet(displayCanvas_t *displayCanvas, float m11, float m12, float m21, float m22, float m31, float m32);
void displayCanvasCtmTranslate(displayCanvas_t *displayCanvas, float tx, float ty);
void displayCanvasCtmScale(displayCanvas_t *displayCanvas, float sx, float sy);
void displayCanvasCtmRotate(displayCanvas_t *displayCanvas, float r);
void displayCanvasContextPush(displayCanvas_t *displayCanvas);
void displayCanvasContextPop(displayCanvas_t *displayCanvas);

11
src/main/drivers/display_font_metadata.c Normal file
View file

@ -0,0 +1,11 @@
#include "drivers/display_font_metadata.h"
#include "drivers/osd.h"
bool displayFontMetadataUpdateFromCharacter(displayFontMetadata_t *metadata, const osdCharacter_t *chr)
{
if (chr && FONT_CHR_IS_METADATA(chr)) {
metadata->version = chr->data[5];
return true;
}
return false;
}

25
src/main/drivers/display_font_metadata.h Normal file
View file

@ -0,0 +1,25 @@
#pragma once
#include <stdbool.h>
#include <stdint.h>
typedef struct osdCharacter_s osdCharacter_t;
typedef struct displayFontMetadata_s {
uint8_t version;
uint16_t charCount;
} displayFontMetadata_t;
// 'I', 'N', 'A', 'V', 1
#define FONT_CHR_IS_METADATA(chr) ((chr)->data[0] == 'I' && \
(chr)->data[1] == 'N' && \
(chr)->data[2] == 'A' && \
(chr)->data[3] == 'V' && \
(chr)->data[4] == 1)
#define FONT_METADATA_CHR_INDEX 255
// Used for runtime detection of display drivers that might
// support 256 or 512 characters.
#define FONT_METADATA_CHR_INDEX_2ND_PAGE 256
bool displayFontMetadataUpdateFromCharacter(displayFontMetadata_t *metadata, const osdCharacter_t *chr);

66
src/main/drivers/max7456.c
View file

@ -167,7 +167,6 @@
// Under normal conditions, it should take 20us // Under normal conditions, it should take 20us
#define MAX_RESET_TIMEOUT_MS 50 #define MAX_RESET_TIMEOUT_MS 50
#define CHARS_PER_LINE 30 // XXX Should be related to VIDEO_BUFFER_CHARS_*?
#define MAKE_CHAR_MODE_U8(c, m) ((((uint16_t)c) << 8) | m) #define MAKE_CHAR_MODE_U8(c, m) ((((uint16_t)c) << 8) | m)
#define MAKE_CHAR_MODE(c, m) (MAKE_CHAR_MODE_U8(c, m) | (c > 255 ? CHAR_MODE_EXT : 0)) #define MAKE_CHAR_MODE(c, m) (MAKE_CHAR_MODE_U8(c, m) | (c > 255 ? CHAR_MODE_EXT : 0))
#define CHAR_BLANK MAKE_CHAR_MODE(0x20, 0) #define CHAR_BLANK MAKE_CHAR_MODE(0x20, 0)
@ -177,11 +176,10 @@
#define CHAR_MODE_EXT (1 << 2) #define CHAR_MODE_EXT (1 << 2)
#define CHAR_MODE_IS_EXT(m) ((m) & CHAR_MODE_EXT) #define CHAR_MODE_IS_EXT(m) ((m) & CHAR_MODE_EXT)
// we write everything in screenBuffer and set a dirty bit // we write everything in osdCharacterGridBuffer and set a dirty bit
// in screenIsDirty to upgrade only changed chars this solution // in screenIsDirty to update only changed chars. This solution
// is faster than redrawing the whole screen on each frame // is faster than redrawing the whole screen on each frame.
static uint16_t screenBuffer[VIDEO_BUFFER_CHARS_PAL] ALIGNED(4); static BITARRAY_DECLARE(screenIsDirty, MAX7456_BUFFER_CHARS_PAL);
static BITARRAY_DECLARE(screenIsDirty, VIDEO_BUFFER_CHARS_PAL);
//max chars to update in one idle //max chars to update in one idle
#define MAX_CHARS2UPDATE 10 #define MAX_CHARS2UPDATE 10
@ -289,9 +287,9 @@ uint16_t max7456GetScreenSize(void)
// TODO: Inspect all callers, make sure they can handle zero and // TODO: Inspect all callers, make sure they can handle zero and
// change this function to return zero before initialization. // change this function to return zero before initialization.
if (state.isInitialized && ((state.registers.vm0 & VIDEO_MODE_PAL) == 0)) { if (state.isInitialized && ((state.registers.vm0 & VIDEO_MODE_PAL) == 0)) {
return VIDEO_BUFFER_CHARS_NTSC; return MAX7456_BUFFER_CHARS_NTSC;
} }
return VIDEO_BUFFER_CHARS_PAL; return MAX7456_BUFFER_CHARS_PAL;
} }
uint8_t max7456GetRowsCount(void) uint8_t max7456GetRowsCount(void)
@ -301,14 +299,14 @@ uint8_t max7456GetRowsCount(void)
return 0; return 0;
} }
if (state.registers.vm0 & VIDEO_MODE_PAL) { if (state.registers.vm0 & VIDEO_MODE_PAL) {
return VIDEO_LINES_PAL; return MAX7456_LINES_PAL;
} }
return VIDEO_LINES_NTSC; return MAX7456_LINES_NTSC;
} }
//because MAX7456 need some time to detect video system etc. we need to wait for a while to initialize it at startup //because MAX7456 need some time to detect video system etc. we need to wait for a while to initialize it at startup
//and in case of restart we need to reinitialize chip. Note that we can't touch screenBuffer here, since //and in case of restart we need to reinitialize chip. Note that we can't touch osdCharacterGridBuffer here, since
//it might already have some data by the first time this function is called. //it might already have some data by the first time this function is called.
static void max7456ReInit(void) static void max7456ReInit(void)
{ {
@ -366,7 +364,7 @@ static void max7456ReInit(void)
//here we init only CS and try to init MAX for first time //here we init only CS and try to init MAX for first time
void max7456Init(const videoSystem_e videoSystem) void max7456Init(const videoSystem_e videoSystem)
{ {
uint8_t buf[(VIDEO_LINES_PAL + 1) * 2]; uint8_t buf[(MAX7456_LINES_PAL + 1) * 2];
int bufPtr; int bufPtr;
state.dev = busDeviceInit(BUSTYPE_SPI, DEVHW_MAX7456, 0, OWNER_OSD); state.dev = busDeviceInit(BUSTYPE_SPI, DEVHW_MAX7456, 0, OWNER_OSD);
@ -383,9 +381,9 @@ void max7456Init(const videoSystem_e videoSystem)
state.registers.dmm = 0; state.registers.dmm = 0;
state.videoSystem = videoSystem; state.videoSystem = videoSystem;
// Set screenbuffer to all blanks // Set screen buffer to all blanks
for (uint_fast16_t ii = 0; ii < ARRAYLEN(screenBuffer); ii++) { for (uint_fast16_t ii = 0; ii < ARRAYLEN(osdCharacterGridBuffer); ii++) {
screenBuffer[ii] = CHAR_BLANK; osdCharacterGridBuffer[ii] = CHAR_BLANK;
} }
// Wait for software reset to finish // Wait for software reset to finish
@ -399,8 +397,8 @@ void max7456Init(const videoSystem_e videoSystem)
// NTSC because all the brightness registers can be written // NTSC because all the brightness registers can be written
// regardless of the video mode. // regardless of the video mode.
bufPtr = 0; bufPtr = 0;
for (int ii = 0; ii < VIDEO_LINES_PAL; ii++) { for (int ii = 0; ii < MAX7456_LINES_PAL; ii++) {
bufPtr = max7456PrepareBuffer(buf, bufPtr, MAX7456ADD_RB0 + ii, BWBRIGHTNESS); bufPtr = max7456PrepareBuffer(buf, bufPtr, MAX7456ADD_RB0 + ii, MAX7456_BWBRIGHTNESS);
} }
// Set the blink duty cycle // Set the blink duty cycle
@ -410,9 +408,9 @@ void max7456Init(const videoSystem_e videoSystem)
void max7456ClearScreen(void) void max7456ClearScreen(void)
{ {
for (uint_fast16_t ii = 0; ii < ARRAYLEN(screenBuffer); ii++) { for (uint_fast16_t ii = 0; ii < ARRAYLEN(osdCharacterGridBuffer); ii++) {
if (screenBuffer[ii] != CHAR_BLANK) { if (osdCharacterGridBuffer[ii] != CHAR_BLANK) {
screenBuffer[ii] = CHAR_BLANK; osdCharacterGridBuffer[ii] = CHAR_BLANK;
bitArraySet(screenIsDirty, ii); bitArraySet(screenIsDirty, ii);
} }
} }
@ -420,19 +418,19 @@ void max7456ClearScreen(void)
void max7456WriteChar(uint8_t x, uint8_t y, uint16_t c, uint8_t mode) void max7456WriteChar(uint8_t x, uint8_t y, uint16_t c, uint8_t mode)
{ {
unsigned pos = y * CHARS_PER_LINE + x; unsigned pos = y * MAX7456_CHARS_PER_LINE + x;
uint16_t val = MAKE_CHAR_MODE(c, mode); uint16_t val = MAKE_CHAR_MODE(c, mode);
if (screenBuffer[pos] != val) { if (osdCharacterGridBuffer[pos] != val) {
screenBuffer[pos] = val; osdCharacterGridBuffer[pos] = val;
bitArraySet(screenIsDirty, pos); bitArraySet(screenIsDirty, pos);
} }
} }
bool max7456ReadChar(uint8_t x, uint8_t y, uint16_t *c, uint8_t *mode) bool max7456ReadChar(uint8_t x, uint8_t y, uint16_t *c, uint8_t *mode)
{ {
unsigned pos = y * CHARS_PER_LINE + x; unsigned pos = y * MAX7456_CHARS_PER_LINE + x;
if (pos < ARRAYLEN(screenBuffer)) { if (pos < ARRAYLEN(osdCharacterGridBuffer)) {
uint16_t val = screenBuffer[pos]; uint16_t val = osdCharacterGridBuffer[pos];
*c = CHAR_BYTE(val); *c = CHAR_BYTE(val);
*mode = MODE_BYTE(val); *mode = MODE_BYTE(val);
if (CHAR_MODE_IS_EXT(*mode)) { if (CHAR_MODE_IS_EXT(*mode)) {
@ -448,15 +446,15 @@ void max7456Write(uint8_t x, uint8_t y, const char *buff, uint8_t mode)
{ {
uint8_t i = 0; uint8_t i = 0;
uint16_t c; uint16_t c;
unsigned pos = y * CHARS_PER_LINE + x; unsigned pos = y * MAX7456_CHARS_PER_LINE + x;
for (i = 0; *buff; i++, buff++, pos++) { for (i = 0; *buff; i++, buff++, pos++) {
//do not write past screen's end of line //do not write past screen's end of line
if (x + i >= CHARS_PER_LINE) { if (x + i >= MAX7456_CHARS_PER_LINE) {
break; break;
} }
c = MAKE_CHAR_MODE_U8(*buff, mode); c = MAKE_CHAR_MODE_U8(*buff, mode);
if (screenBuffer[pos] != c) { if (osdCharacterGridBuffer[pos] != c) {
screenBuffer[pos] = c; osdCharacterGridBuffer[pos] = c;
bitArraySet(screenIsDirty, pos); bitArraySet(screenIsDirty, pos);
} }
} }
@ -483,8 +481,8 @@ static bool max7456DrawScreenPartial(void)
uint8_t ph = pos >> 8; uint8_t ph = pos >> 8;
uint8_t pl = pos & 0xff; uint8_t pl = pos & 0xff;
charMode = MODE_BYTE(screenBuffer[pos]); charMode = MODE_BYTE(osdCharacterGridBuffer[pos]);
uint8_t chr = CHAR_BYTE(screenBuffer[pos]); uint8_t chr = CHAR_BYTE(osdCharacterGridBuffer[pos]);
if (CHAR_MODE_IS_EXT(charMode)) { if (CHAR_MODE_IS_EXT(charMode)) {
if (!DMM_IS_8BIT_MODE(state.registers.dmm)) { if (!DMM_IS_8BIT_MODE(state.registers.dmm)) {
state.registers.dmm |= DMM_8BIT_MODE; state.registers.dmm |= DMM_8BIT_MODE;
@ -622,8 +620,8 @@ void max7456RefreshAll(void)
// Mark non-blank characters as dirty // Mark non-blank characters as dirty
BITARRAY_CLR_ALL(screenIsDirty); BITARRAY_CLR_ALL(screenIsDirty);
for (unsigned ii = 0; ii < ARRAYLEN(screenBuffer); ii++) { for (unsigned ii = 0; ii < ARRAYLEN(osdCharacterGridBuffer); ii++) {
if (!CHAR_IS_BLANK(screenBuffer[ii])) { if (!CHAR_IS_BLANK(osdCharacterGridBuffer[ii])) {
bitArraySet(screenIsDirty, ii); bitArraySet(screenIsDirty, ii);
} }
} }

20
src/main/drivers/max7456.h
View file

@ -21,20 +21,22 @@
#include "drivers/osd.h" #include "drivers/osd.h"
#ifndef WHITEBRIGHTNESS #ifndef MAX7456_WHITEBRIGHTNESS
#define WHITEBRIGHTNESS 0x01 #define MAX7456_WHITEBRIGHTNESS 0x01
#endif #endif
#ifndef BLACKBRIGHTNESS #ifndef MAX7456_BLACKBRIGHTNESS
#define BLACKBRIGHTNESS 0x00 #define MAX7456_BLACKBRIGHTNESS 0x00
#endif #endif
#define BWBRIGHTNESS ((BLACKBRIGHTNESS << 2) | WHITEBRIGHTNESS) #define MAX7456_BWBRIGHTNESS ((MAX7456_BLACKBRIGHTNESS << 2) | MAX7456_WHITEBRIGHTNESS)
#define MAX7456_CHARS_PER_LINE 30
/** PAL or NTSC, value is number of chars total */ /** PAL or NTSC, value is number of chars total */
#define VIDEO_BUFFER_CHARS_NTSC 390 #define MAX7456_LINES_NTSC 13
#define VIDEO_BUFFER_CHARS_PAL 480 #define MAX7456_LINES_PAL 16
#define VIDEO_LINES_NTSC 13 #define MAX7456_BUFFER_CHARS_NTSC (MAX7456_LINES_NTSC * MAX7456_CHARS_PER_LINE)
#define VIDEO_LINES_PAL 16 #define MAX7456_BUFFER_CHARS_PAL (MAX7456_LINES_PAL * MAX7456_CHARS_PER_LINE)
enum VIDEO_TYPES { AUTO = 0, PAL, NTSC }; enum VIDEO_TYPES { AUTO = 0, PAL, NTSC };

104
src/main/drivers/osd.c Normal file
View file

@ -0,0 +1,104 @@
/*
* This file is part of INAV.
*
* 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 "drivers/display_canvas.h"
#include "drivers/osd.h"
uint16_t osdCharacterGridBuffer[OSD_CHARACTER_GRID_BUFFER_SIZE] ALIGNED(4);
void osdCharacterGridBufferClear(void)
{
uint32_t *ptr = (uint32_t *)osdCharacterGridBuffer;
uint32_t *end = (uint32_t *)(ARRAYEND(osdCharacterGridBuffer));
for (; ptr < end; ptr++) {
*ptr = 0;
}
}
static void osdGridBufferConstrainRect(int *x, int *y, int *w, int *h, int totalWidth, int totalHeight)
{
if (*w < 0) {
*x += *w;
*w = -*w;
}
if (*h < 0) {
*y += *h;
*h = -*h;
}
if (*x < 0) {
*w -= *x;
*x = 0;
}
if (*y < 0) {
*h += *y;
*y = 0;
}
int maxX = *x + *w;
int extraWidth = maxX - totalWidth;
if (extraWidth > 0) {
*w -= extraWidth;
}
int maxY = *y + *h;
int extraHeight = maxY - totalHeight;
if (extraHeight > 0) {
*h -= extraHeight;
}
}
void osdGridBufferClearGridRect(int x, int y, int w, int h)
{
osdGridBufferConstrainRect(&x, &y, &w, &h, OSD_CHARACTER_GRID_MAX_WIDTH, OSD_CHARACTER_GRID_MAX_HEIGHT);
int maxX = x + w;
int maxY = y + h;
for (int ii = x; ii < maxX + w; ii++) {
for (int jj = y; jj < maxY; jj++) {
*osdCharacterGridBufferGetEntryPtr(ii, jj) = 0;
}
}
}
void osdGridBufferClearPixelRect(displayCanvas_t *canvas, int x, int y, int w, int h)
{
// Ensure we clear all grid slots that overlap with this rect
if (w < 0) {
x += w;
w = -w;
}
if (h < 0) {
y += h;
h = -h;
}
int gx = x / canvas->gridElementWidth;
int gy = y / canvas->gridElementHeight;
int gw = (w + canvas->gridElementWidth - 1) / canvas->gridElementWidth;
int gh = (h + canvas->gridElementHeight - 1) / canvas->gridElementHeight;
osdGridBufferClearGridRect(gx, gy, gw, gh);
}
uint16_t *osdCharacterGridBufferGetEntryPtr(unsigned x, unsigned y)
{
unsigned pos = y * OSD_CHARACTER_GRID_MAX_WIDTH + x;
return &osdCharacterGridBuffer[pos];
}

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

@ -22,6 +22,8 @@
#include <stdint.h> #include <stdint.h>
#include "common/utils.h"
#define OSD_CHAR_WIDTH 12 #define OSD_CHAR_WIDTH 12
#define OSD_CHAR_HEIGHT 18 #define OSD_CHAR_HEIGHT 18
#define OSD_CHAR_BITS_PER_PIXEL 2 #define OSD_CHAR_BITS_PER_PIXEL 2
@ -36,6 +38,7 @@
// 3 is unused but it's interpreted as transparent by all drivers // 3 is unused but it's interpreted as transparent by all drivers
typedef struct displayCanvas_s displayCanvas_t;
// Video Character Display parameters // Video Character Display parameters
@ -57,3 +60,15 @@ typedef enum {
typedef struct osdCharacter_s { typedef struct osdCharacter_s {
uint8_t data[OSD_CHAR_BYTES]; uint8_t data[OSD_CHAR_BYTES];
} osdCharacter_t; } osdCharacter_t;
#define OSD_CHARACTER_GRID_MAX_WIDTH 30
#define OSD_CHARACTER_GRID_MAX_HEIGHT 16
#define OSD_CHARACTER_GRID_BUFFER_SIZE (OSD_CHARACTER_GRID_MAX_WIDTH * OSD_CHARACTER_GRID_MAX_HEIGHT)
extern uint16_t osdCharacterGridBuffer[OSD_CHARACTER_GRID_BUFFER_SIZE] ALIGNED(4);
// Sets all buffer entries to 0
void osdCharacterGridBufferClear(void);
void osdGridBufferClearGridRect(int x, int y, int w, int h);
void osdGridBufferClearPixelRect(displayCanvas_t *canvas, int x, int y, int w, int h);
uint16_t *osdCharacterGridBufferGetEntryPtr(unsigned x, unsigned y);

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

@ -116,7 +116,7 @@
#define SYM_3D_KMH 0x89 // 137 KM/H 3D #define SYM_3D_KMH 0x89 // 137 KM/H 3D
#define SYM_3D_MPH 0x8A // 138 MPH 3D #define SYM_3D_MPH 0x8A // 138 MPH 3D
// 0x8B // 139 - #define SYM_RPM 0x8B // 139 RPM
// 0x8C // 140 - // 0x8C // 140 -
// 0x8D // 141 - // 0x8D // 141 -
// 0x8E // 142 - // 0x8E // 142 -

2
src/main/drivers/serial_uart.h
View file

@ -17,6 +17,8 @@
#pragma once #pragma once
#define UART_AF(uart, af) CONCAT3(GPIO_AF, af, _ ## uart)
// Since serial ports can be used for any function these buffer sizes should be equal // Since serial ports can be used for any function these buffer sizes should be equal
// The two largest things that need to be sent are: 1, MSP responses, 2, UBLOX SVINFO packet. // The two largest things that need to be sent are: 1, MSP responses, 2, UBLOX SVINFO packet.

32
src/main/drivers/serial_uart_stm32f7xx.c
View file

@ -54,7 +54,11 @@ static uartDevice_t uart1 =
.dev = USART1, .dev = USART1,
.rx = IO_TAG(UART1_RX_PIN), .rx = IO_TAG(UART1_RX_PIN),
.tx = IO_TAG(UART1_TX_PIN), .tx = IO_TAG(UART1_TX_PIN),
#ifdef UART1_AF
.af = UART_AF(USART1, UART1_AF),
#else
.af = GPIO_AF7_USART1, .af = GPIO_AF7_USART1,
#endif
#ifdef UART1_AHB1_PERIPHERALS #ifdef UART1_AHB1_PERIPHERALS
.rcc_ahb1 = UART1_AHB1_PERIPHERALS, .rcc_ahb1 = UART1_AHB1_PERIPHERALS,
#endif #endif
@ -70,7 +74,11 @@ static uartDevice_t uart2 =
.dev = USART2, .dev = USART2,
.rx = IO_TAG(UART2_RX_PIN), .rx = IO_TAG(UART2_RX_PIN),
.tx = IO_TAG(UART2_TX_PIN), .tx = IO_TAG(UART2_TX_PIN),
#ifdef UART2_AF
.af = UART_AF(USART2, UART2_AF),
#else
.af = GPIO_AF7_USART2, .af = GPIO_AF7_USART2,
#endif
#ifdef UART2_AHB1_PERIPHERALS #ifdef UART2_AHB1_PERIPHERALS
.rcc_ahb1 = UART2_AHB1_PERIPHERALS, .rcc_ahb1 = UART2_AHB1_PERIPHERALS,
#endif #endif
@ -86,7 +94,11 @@ static uartDevice_t uart3 =
.dev = USART3, .dev = USART3,
.rx = IO_TAG(UART3_RX_PIN), .rx = IO_TAG(UART3_RX_PIN),
.tx = IO_TAG(UART3_TX_PIN), .tx = IO_TAG(UART3_TX_PIN),
#ifdef UART3_AF
.af = UART_AF(USART3, UART3_AF),
#else
.af = GPIO_AF7_USART3, .af = GPIO_AF7_USART3,
#endif
#ifdef UART3_AHB1_PERIPHERALS #ifdef UART3_AHB1_PERIPHERALS
.rcc_ahb1 = UART3_AHB1_PERIPHERALS, .rcc_ahb1 = UART3_AHB1_PERIPHERALS,
#endif #endif
@ -102,7 +114,11 @@ static uartDevice_t uart4 =
.dev = UART4, .dev = UART4,
.rx = IO_TAG(UART4_RX_PIN), .rx = IO_TAG(UART4_RX_PIN),
.tx = IO_TAG(UART4_TX_PIN), .tx = IO_TAG(UART4_TX_PIN),
#ifdef UART4_AF
.af = UART_AF(UART4, UART4_AF),
#else
.af = GPIO_AF8_UART4, .af = GPIO_AF8_UART4,
#endif
#ifdef UART4_AHB1_PERIPHERALS #ifdef UART4_AHB1_PERIPHERALS
.rcc_ahb1 = UART4_AHB1_PERIPHERALS, .rcc_ahb1 = UART4_AHB1_PERIPHERALS,
#endif #endif
@ -118,7 +134,11 @@ static uartDevice_t uart5 =
.dev = UART5, .dev = UART5,
.rx = IO_TAG(UART5_RX_PIN), .rx = IO_TAG(UART5_RX_PIN),
.tx = IO_TAG(UART5_TX_PIN), .tx = IO_TAG(UART5_TX_PIN),
#ifdef UART5_AF
.af = UART_AF(UART5, UART5_AF),
#else
.af = GPIO_AF8_UART5, .af = GPIO_AF8_UART5,
#endif
#ifdef UART5_AHB1_PERIPHERALS #ifdef UART5_AHB1_PERIPHERALS
.rcc_ahb1 = UART5_AHB1_PERIPHERALS, .rcc_ahb1 = UART5_AHB1_PERIPHERALS,
#endif #endif
@ -134,7 +154,11 @@ static uartDevice_t uart6 =
.dev = USART6, .dev = USART6,
.rx = IO_TAG(UART6_RX_PIN), .rx = IO_TAG(UART6_RX_PIN),
.tx = IO_TAG(UART6_TX_PIN), .tx = IO_TAG(UART6_TX_PIN),
#ifdef UART6_AF
.af = UART_AF(USART6, UART6_AF),
#else
.af = GPIO_AF8_USART6, .af = GPIO_AF8_USART6,
#endif
#ifdef UART6_AHB1_PERIPHERALS #ifdef UART6_AHB1_PERIPHERALS
.rcc_ahb1 = UART6_AHB1_PERIPHERALS, .rcc_ahb1 = UART6_AHB1_PERIPHERALS,
#endif #endif
@ -150,7 +174,11 @@ static uartDevice_t uart7 =
.dev = UART7, .dev = UART7,
.rx = IO_TAG(UART7_RX_PIN), .rx = IO_TAG(UART7_RX_PIN),
.tx = IO_TAG(UART7_TX_PIN), .tx = IO_TAG(UART7_TX_PIN),
#ifdef UART7_AF
.af = UART_AF(UART7, UART7_AF),
#else
.af = GPIO_AF8_UART7, .af = GPIO_AF8_UART7,
#endif
#ifdef UART7_AHB1_PERIPHERALS #ifdef UART7_AHB1_PERIPHERALS
.rcc_ahb1 = UART7_AHB1_PERIPHERALS, .rcc_ahb1 = UART7_AHB1_PERIPHERALS,
#endif #endif
@ -165,7 +193,11 @@ static uartDevice_t uart8 =
.dev = UART8, .dev = UART8,
.rx = IO_TAG(UART8_RX_PIN), .rx = IO_TAG(UART8_RX_PIN),
.tx = IO_TAG(UART8_TX_PIN), .tx = IO_TAG(UART8_TX_PIN),
#ifdef UART8_AF
.af = UART_AF(UART8, UART8_AF),
#else
.af = GPIO_AF8_UART8, .af = GPIO_AF8_UART8,
#endif
#ifdef UART8_AHB1_PERIPHERALS #ifdef UART8_AHB1_PERIPHERALS
.rcc_ahb1 = UART8_AHB1_PERIPHERALS, .rcc_ahb1 = UART8_AHB1_PERIPHERALS,
#endif #endif

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

@ -144,7 +144,7 @@ static bool commandBatchError = false;
// sync this with features_e // sync this with features_e
static const char * const featureNames[] = { static const char * const featureNames[] = {
"THR_VBAT_COMP", "VBAT", "TX_PROF_SEL", "BAT_PROF_AUTOSWITCH", "MOTOR_STOP", "THR_VBAT_COMP", "VBAT", "TX_PROF_SEL", "BAT_PROF_AUTOSWITCH", "MOTOR_STOP",
"DYNAMIC_FILTERS", "SOFTSERIAL", "GPS", "", "DYNAMIC_FILTERS", "SOFTSERIAL", "GPS", "RPM_FILTERS",
"", "TELEMETRY", "CURRENT_METER", "3D", "", "", "TELEMETRY", "CURRENT_METER", "3D", "",
"", "RSSI_ADC", "LED_STRIP", "DASHBOARD", "", "", "RSSI_ADC", "LED_STRIP", "DASHBOARD", "",
"BLACKBOX", "", "TRANSPONDER", "AIRMODE", "BLACKBOX", "", "TRANSPONDER", "AIRMODE",
@ -1612,8 +1612,8 @@ static void cliServo(char *cmdline)
servo = servoParamsMutable(i); servo = servoParamsMutable(i);
if ( if (
arguments[MIN] < PWM_PULSE_MIN || arguments[MIN] > PWM_PULSE_MAX || arguments[MIN] < SERVO_OUTPUT_MIN || arguments[MIN] > SERVO_OUTPUT_MAX ||
arguments[MAX] < PWM_PULSE_MIN || arguments[MAX] > PWM_PULSE_MAX || arguments[MAX] < SERVO_OUTPUT_MIN || arguments[MAX] > SERVO_OUTPUT_MAX ||
arguments[MIDDLE] < arguments[MIN] || arguments[MIDDLE] > arguments[MAX] || arguments[MIDDLE] < arguments[MIN] || arguments[MIDDLE] > arguments[MAX] ||
arguments[MIN] > arguments[MAX] || arguments[MAX] < arguments[MIN] || arguments[MIN] > arguments[MAX] || arguments[MAX] < arguments[MIN] ||
arguments[RATE] < -125 || arguments[RATE] > 125 arguments[RATE] < -125 || arguments[RATE] > 125

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

@ -167,27 +167,18 @@ uint32_t getLooptime(void) {
void validateAndFixConfig(void) void validateAndFixConfig(void)
{ {
#ifdef USE_GYRO_NOTCH_1
if (gyroConfig()->gyro_soft_notch_cutoff_1 >= gyroConfig()->gyro_soft_notch_hz_1) { if (gyroConfig()->gyro_soft_notch_cutoff_1 >= gyroConfig()->gyro_soft_notch_hz_1) {
gyroConfigMutable()->gyro_soft_notch_hz_1 = 0; gyroConfigMutable()->gyro_soft_notch_hz_1 = 0;
} }
#endif
#ifdef USE_GYRO_NOTCH_2
if (gyroConfig()->gyro_soft_notch_cutoff_2 >= gyroConfig()->gyro_soft_notch_hz_2) { if (gyroConfig()->gyro_soft_notch_cutoff_2 >= gyroConfig()->gyro_soft_notch_hz_2) {
gyroConfigMutable()->gyro_soft_notch_hz_2 = 0; gyroConfigMutable()->gyro_soft_notch_hz_2 = 0;
} }
#endif
#ifdef USE_DTERM_NOTCH
if (pidProfile()->dterm_soft_notch_cutoff >= pidProfile()->dterm_soft_notch_hz) { if (pidProfile()->dterm_soft_notch_cutoff >= pidProfile()->dterm_soft_notch_hz) {
pidProfileMutable()->dterm_soft_notch_hz = 0; pidProfileMutable()->dterm_soft_notch_hz = 0;
} }
#endif
#ifdef USE_ACC_NOTCH
if (accelerometerConfig()->acc_notch_cutoff >= accelerometerConfig()->acc_notch_hz) { if (accelerometerConfig()->acc_notch_cutoff >= accelerometerConfig()->acc_notch_hz) {
accelerometerConfigMutable()->acc_notch_hz = 0; accelerometerConfigMutable()->acc_notch_hz = 0;
} }
#endif
// Disable unused features // Disable unused features
featureClear(FEATURE_UNUSED_3 | FEATURE_UNUSED_4 | FEATURE_UNUSED_5 | FEATURE_UNUSED_6 | FEATURE_UNUSED_7 | FEATURE_UNUSED_8 | FEATURE_UNUSED_9 | FEATURE_UNUSED_10); featureClear(FEATURE_UNUSED_3 | FEATURE_UNUSED_4 | FEATURE_UNUSED_5 | FEATURE_UNUSED_6 | FEATURE_UNUSED_7 | FEATURE_UNUSED_8 | FEATURE_UNUSED_9 | FEATURE_UNUSED_10);

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

@ -44,7 +44,7 @@ typedef enum {
FEATURE_DYNAMIC_FILTERS = 1 << 5, // was FEATURE_SERVO_TILT FEATURE_DYNAMIC_FILTERS = 1 << 5, // was FEATURE_SERVO_TILT
FEATURE_SOFTSERIAL = 1 << 6, FEATURE_SOFTSERIAL = 1 << 6,
FEATURE_GPS = 1 << 7, FEATURE_GPS = 1 << 7,
FEATURE_UNUSED_3 = 1 << 8, // was FEATURE_FAILSAFE FEATURE_UNUSED_3 = 1 << 8, // was FEATURE_FAILSAFE
FEATURE_UNUSED_4 = 1 << 9, // was FEATURE_SONAR FEATURE_UNUSED_4 = 1 << 9, // was FEATURE_SONAR
FEATURE_TELEMETRY = 1 << 10, FEATURE_TELEMETRY = 1 << 10,
FEATURE_CURRENT_METER = 1 << 11, FEATURE_CURRENT_METER = 1 << 11,

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

@ -90,11 +90,13 @@
#include "flight/mixer.h" #include "flight/mixer.h"
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/servos.h" #include "flight/servos.h"
#include "flight/rpm_filter.h"
#include "io/asyncfatfs/asyncfatfs.h" #include "io/asyncfatfs/asyncfatfs.h"
#include "io/beeper.h" #include "io/beeper.h"
#include "io/lights.h" #include "io/lights.h"
#include "io/dashboard.h" #include "io/dashboard.h"
#include "io/displayport_frsky_osd.h"
#include "io/displayport_msp.h" #include "io/displayport_msp.h"
#include "io/displayport_max7456.h" #include "io/displayport_max7456.h"
#include "io/flashfs.h" #include "io/flashfs.h"
@ -130,6 +132,8 @@
#include "sensors/rangefinder.h" #include "sensors/rangefinder.h"
#include "sensors/sensors.h" #include "sensors/sensors.h"
#include "scheduler/scheduler.h"
#include "telemetry/telemetry.h" #include "telemetry/telemetry.h"
#include "uav_interconnect/uav_interconnect.h" #include "uav_interconnect/uav_interconnect.h"
@ -214,12 +218,8 @@ void init(void)
// Latch active features to be used for feature() in the remainder of init(). // Latch active features to be used for feature() in the remainder of init().
latchActiveFeatures(); latchActiveFeatures();
#ifdef ALIENFLIGHTF3
ledInit(hardwareRevision == AFF3_REV_1 ? false : true);
#else
ledInit(false); ledInit(false);
#endif
#ifdef USE_EXTI #ifdef USE_EXTI
EXTIInit(); EXTIInit();
@ -507,11 +507,21 @@ void init(void)
#ifdef USE_OSD #ifdef USE_OSD
if (feature(FEATURE_OSD)) { if (feature(FEATURE_OSD)) {
#if defined(USE_FRSKYOSD)
if (!osdDisplayPort) {
osdDisplayPort = frskyOSDDisplayPortInit(osdConfig()->video_system);
}
#endif
#if defined(USE_MAX7456) #if defined(USE_MAX7456)
// If there is a max7456 chip for the OSD then use it // If there is a max7456 chip for the OSD and we have no
osdDisplayPort = max7456DisplayPortInit(osdConfig()->video_system); // external OSD initialized, use it.
if (!osdDisplayPort) {
osdDisplayPort = max7456DisplayPortInit(osdConfig()->video_system);
}
#elif defined(USE_OSD_OVER_MSP_DISPLAYPORT) // OSD over MSP; not supported (yet) #elif defined(USE_OSD_OVER_MSP_DISPLAYPORT) // OSD over MSP; not supported (yet)
osdDisplayPort = displayPortMspInit(); if (!osdDisplayPort) {
osdDisplayPort = displayPortMspInit();
}
#endif #endif
// osdInit will register with CMS by itself. // osdInit will register with CMS by itself.
osdInit(osdDisplayPort); osdInit(osdDisplayPort);
@ -629,5 +639,19 @@ void init(void)
motorControlEnable = true; motorControlEnable = true;
fcTasksInit(); fcTasksInit();
#ifdef USE_OSD
if (feature(FEATURE_OSD) && (osdDisplayPort != NULL)) {
setTaskEnabled(TASK_OSD, feature(FEATURE_OSD));
}
#endif
#ifdef USE_RPM_FILTER
disableRpmFilters();
if (STATE(ESC_SENSOR_ENABLED) && (rpmFilterConfig()->gyro_filter_enabled || rpmFilterConfig()->dterm_filter_enabled)) {
rpmFiltersInit();
setTaskEnabled(TASK_RPM_FILTER, true);
}
#endif
systemState |= SYSTEM_STATE_READY; systemState |= SYSTEM_STATE_READY;
} }

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

@ -1101,43 +1101,19 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU8(dst, gyroConfig()->gyro_soft_lpf_hz); sbufWriteU8(dst, gyroConfig()->gyro_soft_lpf_hz);
sbufWriteU16(dst, pidProfile()->dterm_lpf_hz); sbufWriteU16(dst, pidProfile()->dterm_lpf_hz);
sbufWriteU16(dst, pidProfile()->yaw_lpf_hz); sbufWriteU16(dst, pidProfile()->yaw_lpf_hz);
#ifdef USE_GYRO_NOTCH_1
sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_1); //masterConfig.gyro_soft_notch_hz_1 sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_1); //masterConfig.gyro_soft_notch_hz_1
sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_1); //BF: masterConfig.gyro_soft_notch_cutoff_1 sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_1); //BF: masterConfig.gyro_soft_notch_cutoff_1
#else
sbufWriteU16(dst, 0); //masterConfig.gyro_soft_notch_hz_1
sbufWriteU16(dst, 1); //BF: masterConfig.gyro_soft_notch_cutoff_1
#endif
#ifdef USE_DTERM_NOTCH
sbufWriteU16(dst, pidProfile()->dterm_soft_notch_hz); //BF: pidProfile()->dterm_notch_hz sbufWriteU16(dst, pidProfile()->dterm_soft_notch_hz); //BF: pidProfile()->dterm_notch_hz
sbufWriteU16(dst, pidProfile()->dterm_soft_notch_cutoff); //pidProfile()->dterm_notch_cutoff sbufWriteU16(dst, pidProfile()->dterm_soft_notch_cutoff); //pidProfile()->dterm_notch_cutoff
#else
sbufWriteU16(dst, 0); //BF: pidProfile()->dterm_notch_hz
sbufWriteU16(dst, 1); //pidProfile()->dterm_notch_cutoff
#endif
#ifdef USE_GYRO_NOTCH_2
sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_2); //BF: masterConfig.gyro_soft_notch_hz_2 sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_2); //BF: masterConfig.gyro_soft_notch_hz_2
sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_2); //BF: masterConfig.gyro_soft_notch_cutoff_2 sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_2); //BF: masterConfig.gyro_soft_notch_cutoff_2
#else
sbufWriteU16(dst, 0); //BF: masterConfig.gyro_soft_notch_hz_2
sbufWriteU16(dst, 1); //BF: masterConfig.gyro_soft_notch_cutoff_2
#endif
#ifdef USE_ACC_NOTCH
sbufWriteU16(dst, accelerometerConfig()->acc_notch_hz); sbufWriteU16(dst, accelerometerConfig()->acc_notch_hz);
sbufWriteU16(dst, accelerometerConfig()->acc_notch_cutoff); sbufWriteU16(dst, accelerometerConfig()->acc_notch_cutoff);
#else
sbufWriteU16(dst, 0);
sbufWriteU16(dst, 1);
#endif
#ifdef USE_GYRO_BIQUAD_RC_FIR2
sbufWriteU16(dst, gyroConfig()->gyro_stage2_lowpass_hz); sbufWriteU16(dst, gyroConfig()->gyro_stage2_lowpass_hz);
#else
sbufWriteU16(dst, 0);
#endif
break; break;
case MSP_PID_ADVANCED: case MSP_PID_ADVANCED:
@ -2017,43 +1993,38 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
gyroConfigMutable()->gyro_soft_lpf_hz = sbufReadU8(src); gyroConfigMutable()->gyro_soft_lpf_hz = sbufReadU8(src);
pidProfileMutable()->dterm_lpf_hz = constrain(sbufReadU16(src), 0, 255); pidProfileMutable()->dterm_lpf_hz = constrain(sbufReadU16(src), 0, 255);
pidProfileMutable()->yaw_lpf_hz = constrain(sbufReadU16(src), 0, 255); pidProfileMutable()->yaw_lpf_hz = constrain(sbufReadU16(src), 0, 255);
#ifdef USE_GYRO_NOTCH_1
if (dataSize >= 9) { if (dataSize >= 9) {
gyroConfigMutable()->gyro_soft_notch_hz_1 = constrain(sbufReadU16(src), 0, 500); gyroConfigMutable()->gyro_soft_notch_hz_1 = constrain(sbufReadU16(src), 0, 500);
gyroConfigMutable()->gyro_soft_notch_cutoff_1 = constrain(sbufReadU16(src), 1, 500); gyroConfigMutable()->gyro_soft_notch_cutoff_1 = constrain(sbufReadU16(src), 1, 500);
} else } else {
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
#endif }
#ifdef USE_DTERM_NOTCH
if (dataSize >= 13) { if (dataSize >= 13) {
pidProfileMutable()->dterm_soft_notch_hz = constrain(sbufReadU16(src), 0, 500); pidProfileMutable()->dterm_soft_notch_hz = constrain(sbufReadU16(src), 0, 500);
pidProfileMutable()->dterm_soft_notch_cutoff = constrain(sbufReadU16(src), 1, 500); pidProfileMutable()->dterm_soft_notch_cutoff = constrain(sbufReadU16(src), 1, 500);
pidInitFilters(); pidInitFilters();
} else } else {
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
#endif }
#ifdef USE_GYRO_NOTCH_2
if (dataSize >= 17) { if (dataSize >= 17) {
gyroConfigMutable()->gyro_soft_notch_hz_2 = constrain(sbufReadU16(src), 0, 500); gyroConfigMutable()->gyro_soft_notch_hz_2 = constrain(sbufReadU16(src), 0, 500);
gyroConfigMutable()->gyro_soft_notch_cutoff_2 = constrain(sbufReadU16(src), 1, 500); gyroConfigMutable()->gyro_soft_notch_cutoff_2 = constrain(sbufReadU16(src), 1, 500);
} else } else {
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
#endif }
#ifdef USE_ACC_NOTCH
if (dataSize >= 21) { if (dataSize >= 21) {
accelerometerConfigMutable()->acc_notch_hz = constrain(sbufReadU16(src), 0, 255); accelerometerConfigMutable()->acc_notch_hz = constrain(sbufReadU16(src), 0, 255);
accelerometerConfigMutable()->acc_notch_cutoff = constrain(sbufReadU16(src), 1, 255); accelerometerConfigMutable()->acc_notch_cutoff = constrain(sbufReadU16(src), 1, 255);
} else } else {
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
#endif }
#ifdef USE_GYRO_BIQUAD_RC_FIR2
if (dataSize >= 22) { if (dataSize >= 22) {
gyroConfigMutable()->gyro_stage2_lowpass_hz = constrain(sbufReadU16(src), 0, 500); gyroConfigMutable()->gyro_stage2_lowpass_hz = constrain(sbufReadU16(src), 0, 500);
} else } else {
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
#endif }
} else } else
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
break; break;
@ -3012,6 +2983,10 @@ static bool mspSettingInfoCommand(sbuf_t *dst, sbuf_t *src)
return false; return false;
} }
char name_buf[SETTING_MAX_WORD_LENGTH+1];
settingGetName(setting, name_buf);
sbufWriteDataSafe(dst, name_buf, strlen(name_buf) + 1);
// Parameter Group ID // Parameter Group ID
sbufWriteU16(dst, settingGetPgn(setting)); sbufWriteU16(dst, settingGetPgn(setting));

12
src/main/fc/fc_tasks.c
View file

@ -48,6 +48,7 @@
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/wind_estimator.h" #include "flight/wind_estimator.h"
#include "flight/secondary_imu.h" #include "flight/secondary_imu.h"
#include "flight/rpm_filter.h"
#include "navigation/navigation.h" #include "navigation/navigation.h"
@ -314,9 +315,6 @@ void fcTasksInit(void)
#ifdef USE_PWM_SERVO_DRIVER #ifdef USE_PWM_SERVO_DRIVER
setTaskEnabled(TASK_PWMDRIVER, feature(FEATURE_PWM_SERVO_DRIVER)); setTaskEnabled(TASK_PWMDRIVER, feature(FEATURE_PWM_SERVO_DRIVER));
#endif #endif
#ifdef USE_OSD
setTaskEnabled(TASK_OSD, feature(FEATURE_OSD));
#endif
#ifdef USE_CMS #ifdef USE_CMS
#ifdef USE_MSP_DISPLAYPORT #ifdef USE_MSP_DISPLAYPORT
setTaskEnabled(TASK_CMS, true); setTaskEnabled(TASK_CMS, true);
@ -576,4 +574,12 @@ cfTask_t cfTasks[TASK_COUNT] = {
.staticPriority = TASK_PRIORITY_IDLE, .staticPriority = TASK_PRIORITY_IDLE,
}, },
#endif #endif
#ifdef USE_RPM_FILTER
[TASK_RPM_FILTER] = {
.taskName = "RPM",
.taskFunc = rpmFilterUpdateTask,
.desiredPeriod = TASK_PERIOD_HZ(RPM_FILTER_UPDATE_RATE_HZ), // 300Hz @3,33ms
.staticPriority = TASK_PRIORITY_LOW,
},
#endif
}; };

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

@ -173,37 +173,48 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
rcDelayCommand++; rcDelayCommand++;
} }
} }
} else } else {
rcDelayCommand = 0; rcDelayCommand = 0;
}
rcSticks = stTmp; rcSticks = stTmp;
// perform actions // perform actions
bool armingSwitchIsActive = IS_RC_MODE_ACTIVE(BOXARM); bool armingSwitchIsActive = IS_RC_MODE_ACTIVE(BOXARM);
emergencyArmingUpdate(armingSwitchIsActive); emergencyArmingUpdate(armingSwitchIsActive);
if (armingSwitchIsActive) {
rcDisarmTimeMs = currentTimeMs; if (STATE(FIXED_WING) && feature(FEATURE_MOTOR_STOP) && armingConfig()->fixed_wing_auto_arm) {
tryArm(); // Auto arm on throttle when using fixedwing and motorstop
} else { if (throttleStatus != THROTTLE_LOW) {
// Disarming via ARM BOX tryArm();
// Don't disarm via switch if failsafe is active or receiver doesn't receive data - we can't trust receiver return;
// and can't afford to risk disarming in the air
if (ARMING_FLAG(ARMED) && !IS_RC_MODE_ACTIVE(BOXFAILSAFE) && rxIsReceivingSignal() && !failsafeIsActive()) {
const timeMs_t disarmDelay = currentTimeMs - rcDisarmTimeMs;
if (disarmDelay > armingConfig()->switchDisarmDelayMs) {
if (armingConfig()->disarm_kill_switch || (throttleStatus == THROTTLE_LOW)) {
disarm(DISARM_SWITCH);
}
}
}
else {
rcDisarmTimeMs = currentTimeMs;
} }
} }
else {
if (armingSwitchIsActive) {
rcDisarmTimeMs = currentTimeMs;
tryArm();
} else {
// Disarming via ARM BOX
// Don't disarm via switch if failsafe is active or receiver doesn't receive data - we can't trust receiver
// and can't afford to risk disarming in the air
if (ARMING_FLAG(ARMED) && !IS_RC_MODE_ACTIVE(BOXFAILSAFE) && rxIsReceivingSignal() && !failsafeIsActive()) {
const timeMs_t disarmDelay = currentTimeMs - rcDisarmTimeMs;
if (disarmDelay > armingConfig()->switchDisarmDelayMs) {
if (armingConfig()->disarm_kill_switch || (throttleStatus == THROTTLE_LOW)) {
disarm(DISARM_SWITCH);
}
}
}
else {
rcDisarmTimeMs = currentTimeMs;
}
}
// KILLSWITCH disarms instantly // KILLSWITCH disarms instantly
if (IS_RC_MODE_ACTIVE(BOXKILLSWITCH)) { if (IS_RC_MODE_ACTIVE(BOXKILLSWITCH)) {
disarm(DISARM_KILLSWITCH); disarm(DISARM_KILLSWITCH);
}
} }
if (rcDelayCommand != 20) { if (rcDelayCommand != 20) {

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

@ -122,6 +122,7 @@ typedef enum {
NAV_CRUISE_BRAKING_LOCKED = (1 << 13), NAV_CRUISE_BRAKING_LOCKED = (1 << 13),
NAV_EXTRA_ARMING_SAFETY_BYPASSED = (1 << 14), // nav_extra_arming_safey was bypassed. Keep it until power cycle. NAV_EXTRA_ARMING_SAFETY_BYPASSED = (1 << 14), // nav_extra_arming_safey was bypassed. Keep it until power cycle.
AIRMODE_ACTIVE = (1 << 15), AIRMODE_ACTIVE = (1 << 15),
ESC_SENSOR_ENABLED = (1 << 16),
} stateFlags_t; } stateFlags_t;
#define DISABLE_STATE(mask) (stateFlags &= ~(mask)) #define DISABLE_STATE(mask) (stateFlags &= ~(mask))

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

@ -9,6 +9,7 @@
#include "fc/settings.h" #include "fc/settings.h"
#include "config/general_settings.h" #include "config/general_settings.h"
#include "flight/rpm_filter.h"
#include "settings_generated.c" #include "settings_generated.c"
static bool settingGetWord(char *buf, int idx) static bool settingGetWord(char *buf, int idx)

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

@ -16,7 +16,7 @@ tables:
values: ["NONE", "PMW3901", "CXOF", "MSP", "FAKE"] values: ["NONE", "PMW3901", "CXOF", "MSP", "FAKE"]
enum: opticalFlowSensor_e enum: opticalFlowSensor_e
- name: baro_hardware - name: baro_hardware
values: ["NONE", "AUTO", "BMP085", "MS5611", "BMP280", "MS5607", "LPS25H", "SPL006", "FAKE"] values: ["NONE", "AUTO", "BMP085", "MS5611", "BMP280", "MS5607", "LPS25H", "SPL06", "BMP388", "FAKE"]
enum: baroSensor_e enum: baroSensor_e
- name: pitot_hardware - name: pitot_hardware
values: ["NONE", "AUTO", "MS4525", "ADC", "VIRTUAL", "FAKE"] values: ["NONE", "AUTO", "MS4525", "ADC", "VIRTUAL", "FAKE"]
@ -78,10 +78,10 @@ tables:
- name: i2c_speed - name: i2c_speed
values: ["400KHZ", "800KHZ", "100KHZ", "200KHZ"] values: ["400KHZ", "800KHZ", "100KHZ", "200KHZ"]
- name: debug_modes - name: debug_modes
values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW", values: ["NONE", "GYRO", "AGL", "FLOW_RAW",
"FLOW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "SAG_COMP_VOLTAGE", "FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE",
"VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "GENERIC", "ITERM_RELAX", "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "ITERM_RELAX",
"D_BOOST", "ANTIGRAVITY", "FFT", "FFT_TIME", "FFT_FREQ", "DEBUG_IMU2"] "ERPM", "RPM_FILTER", "RPM_FREQ", "DEBUG_IMU2"]
- name: async_mode - name: async_mode
values: ["NONE", "GYRO", "ALL"] values: ["NONE", "GYRO", "ALL"]
- name: aux_operator - name: aux_operator
@ -135,6 +135,9 @@ tables:
- name: dynamicFilterRangeTable - name: dynamicFilterRangeTable
values: ["HIGH", "MEDIUM", "LOW"] values: ["HIGH", "MEDIUM", "LOW"]
enum: dynamicFilterRange_e enum: dynamicFilterRange_e
- name: pidTypeTable
values: ["NONE", "PID", "PIFF", "AUTO"]
enum: pidType_e
groups: groups:
- name: PG_GYRO_CONFIG - name: PG_GYRO_CONFIG
@ -164,25 +167,20 @@ groups:
max: 128 max: 128
- name: gyro_notch1_hz - name: gyro_notch1_hz
field: gyro_soft_notch_hz_1 field: gyro_soft_notch_hz_1
condition: USE_GYRO_NOTCH_1
max: 500 max: 500
- name: gyro_notch1_cutoff - name: gyro_notch1_cutoff
field: gyro_soft_notch_cutoff_1 field: gyro_soft_notch_cutoff_1
condition: USE_GYRO_NOTCH_1
min: 1 min: 1
max: 500 max: 500
- name: gyro_notch2_hz - name: gyro_notch2_hz
field: gyro_soft_notch_hz_2 field: gyro_soft_notch_hz_2
condition: USE_GYRO_NOTCH_2
max: 500 max: 500
- name: gyro_notch2_cutoff - name: gyro_notch2_cutoff
field: gyro_soft_notch_cutoff_2 field: gyro_soft_notch_cutoff_2
condition: USE_GYRO_NOTCH_2
min: 1 min: 1
max: 500 max: 500
- name: gyro_stage2_lowpass_hz - name: gyro_stage2_lowpass_hz
field: gyro_stage2_lowpass_hz field: gyro_stage2_lowpass_hz
condition: USE_GYRO_BIQUAD_RC_FIR2
min: 0 min: 0
max: 500 max: 500
- name: dyn_notch_width_percent - name: dyn_notch_width_percent
@ -236,11 +234,9 @@ groups:
headers: ["sensors/acceleration.h"] headers: ["sensors/acceleration.h"]
members: members:
- name: acc_notch_hz - name: acc_notch_hz
condition: USE_ACC_NOTCH
min: 0 min: 0
max: 255 max: 255
- name: acc_notch_cutoff - name: acc_notch_cutoff
condition: USE_ACC_NOTCH
min: 1 min: 1
max: 255 max: 255
- name: align_acc - name: align_acc
@ -309,7 +305,7 @@ groups:
- name: PG_SECONDARY_IMU - name: PG_SECONDARY_IMU
type: secondaryImuConfig_t type: secondaryImuConfig_t
headers: ["flight/secondary_imu.h"] headers: ["flight/secondary_imu.h"]
condition: USE_OPFLOW condition: USE_SECONDARY_IMU
members: members:
- name: imu2_enabled - name: imu2_enabled
field: enabled field: enabled
@ -537,6 +533,10 @@ groups:
field: throttleScale field: throttleScale
min: 0 min: 0
max: 1 max: 1
- name: motor_poles
field: motorPoleCount
min: 4
max: 255
- name: PG_FAILSAFE_CONFIG - name: PG_FAILSAFE_CONFIG
type: failsafeConfig_t type: failsafeConfig_t
@ -898,6 +898,46 @@ groups:
min: 0 min: 0
max: 3 max: 3
- name: PG_RPM_FILTER_CONFIG
condition: USE_RPM_FILTER
type: rpmFilterConfig_t
members:
- name: rpm_gyro_filter_enabled
field: gyro_filter_enabled
type: bool
- name: rpm_dterm_filter_enabled
field: dterm_filter_enabled
type: bool
- name: rpm_gyro_harmonics
field: gyro_harmonics
type: uint8_t
min: 1
max: 3
- name: rpm_gyro_min_hz
field: gyro_min_hz
type: uint8_t
min: 50
max: 200
- name: rpm_gyro_q
field: gyro_q
type: uint16_t
min: 1
max: 3000
- name: dterm_gyro_harmonics
field: dterm_harmonics
type: uint8_t
min: 1
max: 3
- name: rpm_dterm_min_hz
field: dterm_min_hz
type: uint8_t
min: 50
max: 200
- name: rpm_dterm_q
field: dterm_q
type: uint16_t
min: 1
max: 3000
- name: PG_GPS_CONFIG - name: PG_GPS_CONFIG
type: gpsConfig_t type: gpsConfig_t
condition: USE_GPS condition: USE_GPS
@ -1099,12 +1139,10 @@ groups:
max: 1 max: 1
- name: dterm_notch_hz - name: dterm_notch_hz
field: dterm_soft_notch_hz field: dterm_soft_notch_hz
condition: USE_DTERM_NOTCH
min: 0 min: 0
max: 500 max: 500
- name: dterm_notch_cutoff - name: dterm_notch_cutoff
field: dterm_soft_notch_cutoff field: dterm_soft_notch_cutoff
condition: USE_DTERM_NOTCH
min: 1 min: 1
max: 500 max: 500
- name: pidsum_limit - name: pidsum_limit
@ -1257,6 +1295,9 @@ groups:
condition: USE_ANTIGRAVITY condition: USE_ANTIGRAVITY
min: 1 min: 1
max: 30 max: 30
- name: pid_type
field: pidControllerType
table: pidTypeTable
- name: PG_PID_AUTOTUNE_CONFIG - name: PG_PID_AUTOTUNE_CONFIG
type: pidAutotuneConfig_t type: pidAutotuneConfig_t

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

@ -157,9 +157,6 @@ void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn,
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
float sample = state->oversampledGyroAccumulator[axis] * state->maxSampleCountRcp; float sample = state->oversampledGyroAccumulator[axis] * state->maxSampleCountRcp;
state->downsampledGyroData[axis][state->circularBufferIdx] = sample; state->downsampledGyroData[axis][state->circularBufferIdx] = sample;
if (axis == 0) {
DEBUG_SET(DEBUG_FFT, 2, lrintf(sample));
}
state->oversampledGyroAccumulator[axis] = 0; state->oversampledGyroAccumulator[axis] = 0;
} }
@ -201,12 +198,6 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilt
arm_cfft_instance_f32 *Sint = &(state->fftInstance.Sint); arm_cfft_instance_f32 *Sint = &(state->fftInstance.Sint);
uint32_t startTime = 0;
if (debugMode == (DEBUG_FFT_TIME)) {
startTime = micros();
}
DEBUG_SET(DEBUG_FFT_TIME, 0, state->updateStep);
switch (state->updateStep) { switch (state->updateStep) {
case STEP_ARM_CFFT_F32: case STEP_ARM_CFFT_F32:
{ {
@ -224,14 +215,12 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilt
arm_radix8_butterfly_f32(state->fftData, FFT_BIN_COUNT, Sint->pTwiddle, 1); arm_radix8_butterfly_f32(state->fftData, FFT_BIN_COUNT, Sint->pTwiddle, 1);
break; break;
} }
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
break; break;
} }
case STEP_BITREVERSAL: case STEP_BITREVERSAL:
{ {
// 6us // 6us
arm_bitreversal_32((uint32_t*) state->fftData, Sint->bitRevLength, Sint->pBitRevTable); arm_bitreversal_32((uint32_t*) state->fftData, Sint->bitRevLength, Sint->pBitRevTable);
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
state->updateStep++; state->updateStep++;
FALLTHROUGH; FALLTHROUGH;
} }
@ -240,14 +229,12 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilt
// 14us // 14us
// this does not work in place => fftData AND rfftData needed // this does not work in place => fftData AND rfftData needed
stage_rfft_f32(&state->fftInstance, state->fftData, state->rfftData); stage_rfft_f32(&state->fftInstance, state->fftData, state->rfftData);
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
break; break;
} }
case STEP_ARM_CMPLX_MAG_F32: case STEP_ARM_CMPLX_MAG_F32:
{ {
// 8us // 8us
arm_cmplx_mag_f32(state->rfftData, state->fftData, FFT_BIN_COUNT); arm_cmplx_mag_f32(state->rfftData, state->fftData, FFT_BIN_COUNT);
DEBUG_SET(DEBUG_FFT_TIME, 2, micros() - startTime);
state->updateStep++; state->updateStep++;
FALLTHROUGH; FALLTHROUGH;
} }
@ -312,15 +299,7 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilt
dynNotchMaxFFT = MAX(dynNotchMaxFFT, state->centerFreq[state->updateAxis]); dynNotchMaxFFT = MAX(dynNotchMaxFFT, state->centerFreq[state->updateAxis]);
if (state->updateAxis == 0) {
DEBUG_SET(DEBUG_FFT, 3, lrintf(fftMeanIndex * 100));
DEBUG_SET(DEBUG_FFT_FREQ, 0, state->centerFreq[state->updateAxis]);
}
if (state->updateAxis == 1) {
DEBUG_SET(DEBUG_FFT_FREQ, 1, state->centerFreq[state->updateAxis]);
}
// Debug FFT_Freq carries raw gyro, gyro after first filter set, FFT centre for roll and for pitch // Debug FFT_Freq carries raw gyro, gyro after first filter set, FFT centre for roll and for pitch
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
break; break;
} }
case STEP_UPDATE_FILTERS: case STEP_UPDATE_FILTERS:
@ -329,8 +308,6 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilt
// calculate cutoffFreq and notch Q, update notch filter =1.8+((A2-150)*0.004) // calculate cutoffFreq and notch Q, update notch filter =1.8+((A2-150)*0.004)
if (state->prevCenterFreq[state->updateAxis] != state->centerFreq[state->updateAxis]) { if (state->prevCenterFreq[state->updateAxis] != state->centerFreq[state->updateAxis]) {
DEBUG_SET(DEBUG_FFT_FREQ, state->updateAxis + 5, state->centerFreq[state->updateAxis]);
if (dualNotch) { if (dualNotch) {
biquadFilterUpdate(&notchFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis] * dynNotch1Ctr, getLooptime(), dynNotchQ, FILTER_NOTCH); biquadFilterUpdate(&notchFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis] * dynNotch1Ctr, getLooptime(), dynNotchQ, FILTER_NOTCH);
biquadFilterUpdate(&notchFilterDyn2[state->updateAxis], state->centerFreq[state->updateAxis] * dynNotch2Ctr, getLooptime(), dynNotchQ, FILTER_NOTCH); biquadFilterUpdate(&notchFilterDyn2[state->updateAxis], state->centerFreq[state->updateAxis] * dynNotch2Ctr, getLooptime(), dynNotchQ, FILTER_NOTCH);
@ -338,7 +315,6 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilt
biquadFilterUpdate(&notchFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis], getLooptime(), dynNotchQ, FILTER_NOTCH); biquadFilterUpdate(&notchFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis], getLooptime(), dynNotchQ, FILTER_NOTCH);
} }
} }
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
state->updateAxis = (state->updateAxis + 1) % XYZ_AXIS_COUNT; state->updateAxis = (state->updateAxis + 1) % XYZ_AXIS_COUNT;
state->updateStep++; state->updateStep++;
@ -354,8 +330,6 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilt
if (state->circularBufferIdx > 0) { if (state->circularBufferIdx > 0) {
arm_mult_f32(&state->downsampledGyroData[state->updateAxis][0], &hanningWindow[ringBufIdx], &state->fftData[ringBufIdx], state->circularBufferIdx); arm_mult_f32(&state->downsampledGyroData[state->updateAxis][0], &hanningWindow[ringBufIdx], &state->fftData[ringBufIdx], state->circularBufferIdx);
} }
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
} }
} }

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

@ -74,7 +74,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 1);
PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
.yaw_motor_direction = 1, .yaw_motor_direction = 1,
.yaw_jump_prevention_limit = 200, .yaw_jump_prevention_limit = 350,
.platformType = PLATFORM_MULTIROTOR, .platformType = PLATFORM_MULTIROTOR,
.hasFlaps = false, .hasFlaps = false,
.appliedMixerPreset = -1, //This flag is not available in CLI and used by Configurator only .appliedMixerPreset = -1, //This flag is not available in CLI and used by Configurator only
@ -93,7 +93,7 @@ PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
#define DEFAULT_MAX_THROTTLE 1850 #define DEFAULT_MAX_THROTTLE 1850
PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 3); PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 4);
PG_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
.minthrottle = DEFAULT_MIN_THROTTLE, .minthrottle = DEFAULT_MIN_THROTTLE,
@ -103,12 +103,16 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
.mincommand = 1000, .mincommand = 1000,
.motorAccelTimeMs = 0, .motorAccelTimeMs = 0,
.motorDecelTimeMs = 0, .motorDecelTimeMs = 0,
.digitalIdleOffsetValue = 450, // Same scale as in Betaflight .digitalIdleOffsetValue = 450, // Same scale as in Betaflight
.throttleScale = 1.0f .throttleScale = 1.0f,
.motorPoleCount = 14 // Most brushless motors that we use are 14 poles
); );
PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTOR_MIXER, 0); PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTOR_MIXER, 0);
typedef void (*motorRateLimitingApplyFnPtr)(const float dT);
static EXTENDED_FASTRAM motorRateLimitingApplyFnPtr motorRateLimitingApplyFn;
static void computeMotorCount(void) static void computeMotorCount(void)
{ {
motorCount = 0; motorCount = 0;
@ -153,6 +157,49 @@ void mixerUpdateStateFlags(void)
} }
} }
void nullMotorRateLimiting(const float dT)
{
UNUSED(dT);
}
void applyMotorRateLimiting(const float dT)
{
static float motorPrevious[MAX_SUPPORTED_MOTORS] = { 0 };
if (feature(FEATURE_3D)) {
// FIXME: Don't apply rate limiting in 3D mode
for (int i = 0; i < motorCount; i++) {
motorPrevious[i] = motor[i];
}
}
else {
// Calculate max motor step
const uint16_t motorRange = motorConfig()->maxthrottle - motorConfig()->minthrottle;
const float motorMaxInc = (motorConfig()->motorAccelTimeMs == 0) ? 2000 : motorRange * dT / (motorConfig()->motorAccelTimeMs * 1e-3f);
const float motorMaxDec = (motorConfig()->motorDecelTimeMs == 0) ? 2000 : motorRange * dT / (motorConfig()->motorDecelTimeMs * 1e-3f);
for (int i = 0; i < motorCount; i++) {
// Apply motor rate limiting
motorPrevious[i] = constrainf(motor[i], motorPrevious[i] - motorMaxDec, motorPrevious[i] + motorMaxInc);
// Handle throttle below min_throttle (motor start/stop)
if (motorPrevious[i] < motorConfig()->minthrottle) {
if (motor[i] < motorConfig()->minthrottle) {
motorPrevious[i] = motor[i];
}
else {
motorPrevious[i] = motorConfig()->minthrottle;
}
}
}
}
// Update motor values
for (int i = 0; i < motorCount; i++) {
motor[i] = motorPrevious[i];
}
}
void mixerInit(void) void mixerInit(void)
{ {
computeMotorCount(); computeMotorCount();
@ -163,6 +210,12 @@ void mixerInit(void)
} }
mixerResetDisarmedMotors(); mixerResetDisarmedMotors();
if (motorConfig()->motorAccelTimeMs || motorConfig()->motorDecelTimeMs) {
motorRateLimitingApplyFn = applyMotorRateLimiting;
} else {
motorRateLimitingApplyFn = nullMotorRateLimiting;
}
} }
void mixerResetDisarmedMotors(void) void mixerResetDisarmedMotors(void)
@ -239,44 +292,6 @@ void stopPwmAllMotors(void)
pwmShutdownPulsesForAllMotors(motorCount); pwmShutdownPulsesForAllMotors(motorCount);
} }
static void applyMotorRateLimiting(const float dT)
{
static float motorPrevious[MAX_SUPPORTED_MOTORS] = { 0 };
if (feature(FEATURE_3D)) {
// FIXME: Don't apply rate limiting in 3D mode
for (int i = 0; i < motorCount; i++) {
motorPrevious[i] = motor[i];
}
}
else {
// Calculate max motor step
const uint16_t motorRange = motorConfig()->maxthrottle - motorConfig()->minthrottle;
const float motorMaxInc = (motorConfig()->motorAccelTimeMs == 0) ? 2000 : motorRange * dT / (motorConfig()->motorAccelTimeMs * 1e-3f);
const float motorMaxDec = (motorConfig()->motorDecelTimeMs == 0) ? 2000 : motorRange * dT / (motorConfig()->motorDecelTimeMs * 1e-3f);
for (int i = 0; i < motorCount; i++) {
// Apply motor rate limiting
motorPrevious[i] = constrainf(motor[i], motorPrevious[i] - motorMaxDec, motorPrevious[i] + motorMaxInc);
// Handle throttle below min_throttle (motor start/stop)
if (motorPrevious[i] < motorConfig()->minthrottle) {
if (motor[i] < motorConfig()->minthrottle) {
motorPrevious[i] = motor[i];
}
else {
motorPrevious[i] = motorConfig()->minthrottle;
}
}
}
}
// Update motor values
for (int i = 0; i < motorCount; i++) {
motor[i] = motorPrevious[i];
}
}
void FAST_CODE NOINLINE mixTable(const float dT) void FAST_CODE NOINLINE mixTable(const float dT)
{ {
int16_t input[3]; // RPY, range [-500:+500] int16_t input[3]; // RPY, range [-500:+500]
@ -320,6 +335,13 @@ void FAST_CODE NOINLINE mixTable(const float dT)
static int16_t throttlePrevious = 0; // Store the last throttle direction for deadband transitions static int16_t throttlePrevious = 0; // Store the last throttle direction for deadband transitions
// Find min and max throttle based on condition. // Find min and max throttle based on condition.
#ifdef USE_GLOBAL_FUNCTIONS
if (GLOBAL_FUNCTION_FLAG(GLOBAL_FUNCTION_FLAG_OVERRIDE_THROTTLE)) {
throttleMin = motorConfig()->minthrottle;
throttleMax = motorConfig()->maxthrottle;
mixerThrottleCommand = constrain(globalFunctionValues[GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE], throttleMin, throttleMax);
} else
#endif
if (feature(FEATURE_3D)) { if (feature(FEATURE_3D)) {
if (!ARMING_FLAG(ARMED)) throttlePrevious = PWM_RANGE_MIDDLE; // When disarmed set to mid_rc. It always results in positive direction after arming. if (!ARMING_FLAG(ARMED)) throttlePrevious = PWM_RANGE_MIDDLE; // When disarmed set to mid_rc. It always results in positive direction after arming.
@ -406,7 +428,7 @@ void FAST_CODE NOINLINE mixTable(const float dT)
} }
/* Apply motor acceleration/deceleration limit */ /* Apply motor acceleration/deceleration limit */
applyMotorRateLimiting(dT); motorRateLimitingApplyFn(dT);
} }
motorStatus_e getMotorStatus(void) motorStatus_e getMotorStatus(void)

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

@ -91,7 +91,8 @@ typedef struct motorConfig_s {
uint16_t motorAccelTimeMs; // Time limit for motor to accelerate from 0 to 100% throttle [ms] uint16_t motorAccelTimeMs; // Time limit for motor to accelerate from 0 to 100% throttle [ms]
uint16_t motorDecelTimeMs; // Time limit for motor to decelerate from 0 to 100% throttle [ms] uint16_t motorDecelTimeMs; // Time limit for motor to decelerate from 0 to 100% throttle [ms]
uint16_t digitalIdleOffsetValue; uint16_t digitalIdleOffsetValue;
float throttleScale; // Scaling factor for throttle. float throttleScale; // Scaling factor for throttle.
uint8_t motorPoleCount; // Magnetic poles in the motors for calculating actual RPM from eRPM provided by ESC telemetry
} motorConfig_t; } motorConfig_t;
PG_DECLARE(motorConfig_t, motorConfig); PG_DECLARE(motorConfig_t, motorConfig);

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

@ -41,6 +41,7 @@
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/mixer.h" #include "flight/mixer.h"
#include "flight/rpm_filter.h"
#include "io/gps.h" #include "io/gps.h"
@ -81,11 +82,8 @@ typedef struct {
rateLimitFilter_t axisAccelFilter; rateLimitFilter_t axisAccelFilter;
pt1Filter_t ptermLpfState; pt1Filter_t ptermLpfState;
biquadFilter_t deltaLpfState; biquadFilter_t deltaLpfState;
// Dterm notch filtering // Dterm notch filtering
#ifdef USE_DTERM_NOTCH
biquadFilter_t deltaNotchFilter; biquadFilter_t deltaNotchFilter;
#endif
float stickPosition; float stickPosition;
#ifdef USE_D_BOOST #ifdef USE_D_BOOST
@ -94,17 +92,15 @@ typedef struct {
pt1Filter_t dBoostLpf; pt1Filter_t dBoostLpf;
biquadFilter_t dBoostGyroLpf; biquadFilter_t dBoostGyroLpf;
#endif #endif
bool itermLimitActive;
} pidState_t; } pidState_t;
#ifdef USE_DTERM_NOTCH
STATIC_FASTRAM filterApplyFnPtr notchFilterApplyFn; STATIC_FASTRAM filterApplyFnPtr notchFilterApplyFn;
#endif
STATIC_FASTRAM bool pidFiltersConfigured = false; STATIC_FASTRAM bool pidFiltersConfigured = false;
FASTRAM float headingHoldCosZLimit; static EXTENDED_FASTRAM float headingHoldCosZLimit;
FASTRAM int16_t headingHoldTarget; static EXTENDED_FASTRAM int16_t headingHoldTarget;
STATIC_FASTRAM pt1Filter_t headingHoldRateFilter; static EXTENDED_FASTRAM pt1Filter_t headingHoldRateFilter;
STATIC_FASTRAM pt1Filter_t fixedWingTpaFilter; static EXTENDED_FASTRAM pt1Filter_t fixedWingTpaFilter;
// Thrust PID Attenuation factor. 0.0f means fully attenuated, 1.0f no attenuation is applied // Thrust PID Attenuation factor. 0.0f means fully attenuated, 1.0f no attenuation is applied
STATIC_FASTRAM bool pidGainsUpdateRequired; STATIC_FASTRAM bool pidGainsUpdateRequired;
@ -138,8 +134,19 @@ static EXTENDED_FASTRAM float dBoostMaxAtAlleceleration;
static EXTENDED_FASTRAM uint16_t yawPLimit; static EXTENDED_FASTRAM uint16_t yawPLimit;
static EXTENDED_FASTRAM uint8_t yawLpfHz; static EXTENDED_FASTRAM uint8_t yawLpfHz;
static EXTENDED_FASTRAM uint16_t pidSumLimit;
static EXTENDED_FASTRAM float motorItermWindupPoint;
static EXTENDED_FASTRAM float antiWindupScaler;
#ifdef USE_ANTIGRAVITY
static EXTENDED_FASTRAM float iTermAntigravityGain;
#endif
static EXTENDED_FASTRAM uint8_t usedPidControllerType;
PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 10); typedef void (*pidControllerFnPtr)(pidState_t *pidState, flight_dynamics_index_t axis, float dT);
static EXTENDED_FASTRAM pidControllerFnPtr pidControllerApplyFn;
static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn;
PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 11);
PG_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.bank_mc = { .bank_mc = {
@ -211,7 +218,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.dterm_soft_notch_hz = 0, .dterm_soft_notch_hz = 0,
.dterm_soft_notch_cutoff = 1, .dterm_soft_notch_cutoff = 1,
.dterm_lpf_hz = 40, .dterm_lpf_hz = 40,
.yaw_lpf_hz = 30, .yaw_lpf_hz = 0,
.dterm_setpoint_weight = 1.0f, .dterm_setpoint_weight = 1.0f,
.use_dterm_fir_filter = 1, .use_dterm_fir_filter = 1,
@ -237,47 +244,16 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.navVelXyDTermLpfHz = NAV_ACCEL_CUTOFF_FREQUENCY_HZ, .navVelXyDTermLpfHz = NAV_ACCEL_CUTOFF_FREQUENCY_HZ,
.iterm_relax_type = ITERM_RELAX_SETPOINT, .iterm_relax_type = ITERM_RELAX_SETPOINT,
.iterm_relax_cutoff = MC_ITERM_RELAX_CUTOFF_DEFAULT, .iterm_relax_cutoff = MC_ITERM_RELAX_CUTOFF_DEFAULT,
.iterm_relax = ITERM_RELAX_OFF, .iterm_relax = ITERM_RELAX_RP,
.dBoostFactor = 1.0f, .dBoostFactor = 1.25f,
.dBoostMaxAtAlleceleration = 7500.0f, .dBoostMaxAtAlleceleration = 7500.0f,
.dBoostGyroDeltaLpfHz = D_BOOST_GYRO_LPF_HZ, .dBoostGyroDeltaLpfHz = D_BOOST_GYRO_LPF_HZ,
.antigravityGain = 1.0f, .antigravityGain = 1.0f,
.antigravityAccelerator = 1.0f, .antigravityAccelerator = 1.0f,
.antigravityCutoff = ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, .antigravityCutoff = ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF,
.pidControllerType = PID_TYPE_AUTO,
); );
void pidInit(void)
{
pidResetTPAFilter();
// Calculate max overall tilt (max pitch + max roll combined) as a limit to heading hold
headingHoldCosZLimit = cos_approx(DECIDEGREES_TO_RADIANS(pidProfile()->max_angle_inclination[FD_ROLL])) *
cos_approx(DECIDEGREES_TO_RADIANS(pidProfile()->max_angle_inclination[FD_PITCH]));
pidGainsUpdateRequired = false;
itermRelax = pidProfile()->iterm_relax;
itermRelaxType = pidProfile()->iterm_relax_type;
itermRelaxSetpointThreshold = MC_ITERM_RELAX_SETPOINT_THRESHOLD * MC_ITERM_RELAX_CUTOFF_DEFAULT / pidProfile()->iterm_relax_cutoff;
if (mixerConfig()->platformType == PLATFORM_MULTIROTOR) {
yawPLimit = pidProfile()->yaw_p_limit;
} else {
yawPLimit = 0;
}
yawLpfHz = pidProfile()->yaw_lpf_hz;
#ifdef USE_D_BOOST
dBoostFactor = pidProfile()->dBoostFactor;
dBoostMaxAtAlleceleration = pidProfile()->dBoostMaxAtAlleceleration;
#endif
#ifdef USE_ANTIGRAVITY
antigravityGain = pidProfile()->antigravityGain;
antigravityAccelerator = pidProfile()->antigravityAccelerator;
#endif
}
bool pidInitFilters(void) bool pidInitFilters(void)
{ {
const uint32_t refreshRate = getLooptime(); const uint32_t refreshRate = getLooptime();
@ -310,7 +286,6 @@ bool pidInitFilters(void)
firFilterInit(&pidState[axis].gyroRateFilter, pidState[axis].gyroRateBuf, PID_GYRO_RATE_BUF_LENGTH, dtermCoeffs); firFilterInit(&pidState[axis].gyroRateFilter, pidState[axis].gyroRateBuf, PID_GYRO_RATE_BUF_LENGTH, dtermCoeffs);
} }
#ifdef USE_DTERM_NOTCH
notchFilterApplyFn = nullFilterApply; notchFilterApplyFn = nullFilterApply;
if (pidProfile()->dterm_soft_notch_hz != 0) { if (pidProfile()->dterm_soft_notch_hz != 0) {
notchFilterApplyFn = (filterApplyFnPtr)biquadFilterApply; notchFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
@ -318,7 +293,6 @@ bool pidInitFilters(void)
biquadFilterInitNotch(&pidState[axis].deltaNotchFilter, refreshRate, pidProfile()->dterm_soft_notch_hz, pidProfile()->dterm_soft_notch_cutoff); biquadFilterInitNotch(&pidState[axis].deltaNotchFilter, refreshRate, pidProfile()->dterm_soft_notch_hz, pidProfile()->dterm_soft_notch_cutoff);
} }
} }
#endif
// Init other filters // Init other filters
for (int axis = 0; axis < 3; ++ axis) { for (int axis = 0; axis < 3; ++ axis) {
@ -346,7 +320,7 @@ bool pidInitFilters(void)
void pidResetTPAFilter(void) void pidResetTPAFilter(void)
{ {
if (STATE(FIXED_WING) && currentControlRateProfile->throttle.fixedWingTauMs > 0) { if (usedPidControllerType == PID_TYPE_PIFF && currentControlRateProfile->throttle.fixedWingTauMs > 0) {
pt1FilterInitRC(&fixedWingTpaFilter, currentControlRateProfile->throttle.fixedWingTauMs * 1e-3f, getLooptime() * 1e-6f); pt1FilterInitRC(&fixedWingTpaFilter, currentControlRateProfile->throttle.fixedWingTauMs * 1e-3f, getLooptime() * 1e-6f);
pt1FilterReset(&fixedWingTpaFilter, motorConfig()->minthrottle); pt1FilterReset(&fixedWingTpaFilter, motorConfig()->minthrottle);
} }
@ -444,7 +418,7 @@ void FAST_CODE NOINLINE updatePIDCoefficients(float dT)
STATIC_FASTRAM uint16_t prevThrottle = 0; STATIC_FASTRAM uint16_t prevThrottle = 0;
// Check if throttle changed. Different logic for fixed wing vs multirotor // Check if throttle changed. Different logic for fixed wing vs multirotor
if (STATE(FIXED_WING) && (currentControlRateProfile->throttle.fixedWingTauMs > 0)) { if (usedPidControllerType == PID_TYPE_PIFF && (currentControlRateProfile->throttle.fixedWingTauMs > 0)) {
uint16_t filteredThrottle = pt1FilterApply3(&fixedWingTpaFilter, rcCommand[THROTTLE], dT); uint16_t filteredThrottle = pt1FilterApply3(&fixedWingTpaFilter, rcCommand[THROTTLE], dT);
if (filteredThrottle != prevThrottle) { if (filteredThrottle != prevThrottle) {
prevThrottle = filteredThrottle; prevThrottle = filteredThrottle;
@ -459,7 +433,7 @@ void FAST_CODE NOINLINE updatePIDCoefficients(float dT)
} }
#ifdef USE_ANTIGRAVITY #ifdef USE_ANTIGRAVITY
if (!STATE(FIXED_WING)) { if (usedPidControllerType == PID_TYPE_PID) {
antigravityThrottleHpf = rcCommand[THROTTLE] - pt1FilterApply(&antigravityThrottleLpf, rcCommand[THROTTLE]); antigravityThrottleHpf = rcCommand[THROTTLE] - pt1FilterApply(&antigravityThrottleLpf, rcCommand[THROTTLE]);
} }
#endif #endif
@ -476,12 +450,12 @@ void FAST_CODE NOINLINE updatePIDCoefficients(float dT)
return; return;
} }
const float tpaFactor = STATE(FIXED_WING) ? calculateFixedWingTPAFactor(prevThrottle) : calculateMultirotorTPAFactor(); const float tpaFactor = usedPidControllerType == PID_TYPE_PIFF ? calculateFixedWingTPAFactor(prevThrottle) : calculateMultirotorTPAFactor();
// PID coefficients can be update only with THROTTLE and TPA or inflight PID adjustments // PID coefficients can be update only with THROTTLE and TPA or inflight PID adjustments
//TODO: Next step would be to update those only at THROTTLE or inflight adjustments change //TODO: Next step would be to update those only at THROTTLE or inflight adjustments change
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
if (STATE(FIXED_WING)) { if (usedPidControllerType == PID_TYPE_PIFF) {
// Airplanes - scale all PIDs according to TPA // Airplanes - scale all PIDs according to TPA
pidState[axis].kP = pidBank()->pid[axis].P / FP_PID_RATE_P_MULTIPLIER * tpaFactor; pidState[axis].kP = pidBank()->pid[axis].P / FP_PID_RATE_P_MULTIPLIER * tpaFactor;
pidState[axis].kI = pidBank()->pid[axis].I / FP_PID_RATE_I_MULTIPLIER * tpaFactor; pidState[axis].kI = pidBank()->pid[axis].I / FP_PID_RATE_I_MULTIPLIER * tpaFactor;
@ -606,6 +580,20 @@ static FAST_CODE NOINLINE float pTermProcess(pidState_t *pidState, flight_dynami
return newPTerm; return newPTerm;
} }
static void applyItermLimiting(pidState_t *pidState) {
if (pidState->itermLimitActive) {
pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit);
} else {
pidState->errorGyroIfLimit = fabsf(pidState->errorGyroIf);
}
}
static void nullRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT) {
UNUSED(pidState);
UNUSED(axis);
UNUSED(dT);
}
static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT) static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
{ {
const float rateError = pidState->rateTarget - pidState->gyroRate; const float rateError = pidState->rateTarget - pidState->gyroRate;
@ -615,11 +603,7 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
// Calculate integral // Calculate integral
pidState->errorGyroIf += rateError * pidState->kI * dT; pidState->errorGyroIf += rateError * pidState->kI * dT;
if (STATE(ANTI_WINDUP) || isFixedWingItermLimitActive(pidState->stickPosition)) { applyItermLimiting(pidState);
pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit);
} else {
pidState->errorGyroIfLimit = fabsf(pidState->errorGyroIf);
}
if (pidProfile()->fixedWingItermThrowLimit != 0) { if (pidProfile()->fixedWingItermThrowLimit != 0) {
pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidProfile()->fixedWingItermThrowLimit, pidProfile()->fixedWingItermThrowLimit); pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidProfile()->fixedWingItermThrowLimit, pidProfile()->fixedWingItermThrowLimit);
@ -631,7 +615,7 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
} }
#endif #endif
axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf, -pidProfile()->pidSumLimit, +pidProfile()->pidSumLimit); axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf, -pidSumLimit, +pidSumLimit);
#ifdef USE_BLACKBOX #ifdef USE_BLACKBOX
axisPID_P[axis] = newPTerm; axisPID_P[axis] = newPTerm;
@ -668,7 +652,7 @@ static void FAST_CODE applyItermRelax(const int axis, const float gyroRate, floa
} }
} }
#ifdef USE_D_BOOST #ifdef USE_D_BOOST
static float FAST_CODE applyDBoost(pidState_t *pidState, flight_dynamics_index_t axis, float dT) { static float FAST_CODE applyDBoost(pidState_t *pidState, float dT) {
float dBoost = 1.0f; float dBoost = 1.0f;
@ -682,14 +666,6 @@ static float FAST_CODE applyDBoost(pidState_t *pidState, flight_dynamics_index_t
dBoost = pt1FilterApply4(&pidState->dBoostLpf, dBoost, D_BOOST_LPF_HZ, dT); dBoost = pt1FilterApply4(&pidState->dBoostLpf, dBoost, D_BOOST_LPF_HZ, dT);
dBoost = constrainf(dBoost, 1.0f, dBoostFactor); dBoost = constrainf(dBoost, 1.0f, dBoostFactor);
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_D_BOOST, 0, dBoostGyroAcceleration);
DEBUG_SET(DEBUG_D_BOOST, 1, dBoostRateAcceleration);
DEBUG_SET(DEBUG_D_BOOST, 2, dBoost * 100);
} else if (axis == FD_PITCH) {
DEBUG_SET(DEBUG_D_BOOST, 3, dBoost * 100);
}
pidState->previousRateTarget = pidState->rateTarget; pidState->previousRateTarget = pidState->rateTarget;
pidState->previousRateGyro = pidState->gyroRate; pidState->previousRateGyro = pidState->gyroRate;
} }
@ -697,43 +673,42 @@ static float FAST_CODE applyDBoost(pidState_t *pidState, flight_dynamics_index_t
return dBoost; return dBoost;
} }
#else #else
static float applyDBoost(pidState_t *pidState, flight_dynamics_index_t axis, float dT) { static float applyDBoost(pidState_t *pidState, float dT) {
UNUSED(pidState); UNUSED(pidState);
UNUSED(axis);
UNUSED(dT); UNUSED(dT);
return 1.0f; return 1.0f;
} }
#endif #endif
static void FAST_CODE pidApplyMulticopterRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT) static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
{ {
const float rateError = pidState->rateTarget - pidState->gyroRate; const float rateError = pidState->rateTarget - pidState->gyroRate;
const float newPTerm = pTermProcess(pidState, axis, rateError, dT); const float newPTerm = pTermProcess(pidState, axis, rateError, dT);
// Calculate new D-term // Calculate new D-term
float newDTerm; float newDTerm;
if (pidBank()->pid[axis].D == 0) { if (pidState->kD == 0) {
// optimisation for when D8 is zero, often used by YAW axis // optimisation for when D8 is zero, often used by YAW axis
newDTerm = 0; newDTerm = 0;
} else { } else {
// Calculate delta for Dterm calculation. Apply filters before derivative to minimize effects of dterm kick // Calculate delta for Dterm calculation. Apply filters before derivative to minimize effects of dterm kick
float deltaFiltered = pidProfile()->dterm_setpoint_weight * pidState->rateTarget - pidState->gyroRate; float deltaFiltered = pidProfile()->dterm_setpoint_weight * pidState->rateTarget - pidState->gyroRate;
#ifdef USE_DTERM_NOTCH
// Apply D-term notch // Apply D-term notch
deltaFiltered = notchFilterApplyFn(&pidState->deltaNotchFilter, deltaFiltered); deltaFiltered = notchFilterApplyFn(&pidState->deltaNotchFilter, deltaFiltered);
#ifdef USE_RPM_FILTER
deltaFiltered = rpmFilterDtermApply((uint8_t)axis, deltaFiltered);
#endif #endif
// Apply additional lowpass // Apply additional lowpass
if (pidProfile()->dterm_lpf_hz) { deltaFiltered = dTermLpfFilterApplyFn(&pidState->deltaLpfState, deltaFiltered);
deltaFiltered = biquadFilterApply(&pidState->deltaLpfState, deltaFiltered);
}
firFilterUpdate(&pidState->gyroRateFilter, deltaFiltered); firFilterUpdate(&pidState->gyroRateFilter, deltaFiltered);
newDTerm = firFilterApply(&pidState->gyroRateFilter); newDTerm = firFilterApply(&pidState->gyroRateFilter);
// Calculate derivative // Calculate derivative
newDTerm = newDTerm * (pidState->kD / dT) * applyDBoost(pidState, axis, dT); newDTerm = newDTerm * (pidState->kD / dT) * applyDBoost(pidState, dT);
// Additionally constrain D // Additionally constrain D
newDTerm = constrainf(newDTerm, -300.0f, 300.0f); newDTerm = constrainf(newDTerm, -300.0f, 300.0f);
@ -741,19 +716,12 @@ static void FAST_CODE pidApplyMulticopterRateController(pidState_t *pidState, fl
// TODO: Get feedback from mixer on available correction range for each axis // TODO: Get feedback from mixer on available correction range for each axis
const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf; const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf;
const float newOutputLimited = constrainf(newOutput, -pidProfile()->pidSumLimit, +pidProfile()->pidSumLimit); const float newOutputLimited = constrainf(newOutput, -pidSumLimit, +pidSumLimit);
// Prevent strong Iterm accumulation during stick inputs
const float motorItermWindupPoint = 1.0f - (pidProfile()->itermWindupPointPercent / 100.0f);
const float antiWindupScaler = constrainf((1.0f - getMotorMixRange()) / motorItermWindupPoint, 0.0f, 1.0f);
float itermErrorRate = rateError; float itermErrorRate = rateError;
applyItermRelax(axis, pidState->gyroRate, pidState->rateTarget, &itermErrorRate); applyItermRelax(axis, pidState->gyroRate, pidState->rateTarget, &itermErrorRate);
#ifdef USE_ANTIGRAVITY #ifdef USE_ANTIGRAVITY
const float iTermAntigravityGain = scaleRangef(fabsf(antigravityThrottleHpf) * antigravityAccelerator, 0.0f, 1000.0f, 1.0f, antigravityGain);
DEBUG_SET(DEBUG_ANTIGRAVITY, 0, iTermAntigravityGain * 100);
DEBUG_SET(DEBUG_ANTIGRAVITY, 1, antigravityThrottleHpf);
itermErrorRate *= iTermAntigravityGain; itermErrorRate *= iTermAntigravityGain;
#endif #endif
@ -761,11 +729,7 @@ static void FAST_CODE pidApplyMulticopterRateController(pidState_t *pidState, fl
+ ((newOutputLimited - newOutput) * pidState->kT * antiWindupScaler * dT); + ((newOutputLimited - newOutput) * pidState->kT * antiWindupScaler * dT);
// Don't grow I-term if motors are at their limit // Don't grow I-term if motors are at their limit
if (STATE(ANTI_WINDUP) || mixerIsOutputSaturated()) { applyItermLimiting(pidState);
pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit);
} else {
pidState->errorGyroIfLimit = fabsf(pidState->errorGyroIf);
}
axisPID[axis] = newOutputLimited; axisPID[axis] = newOutputLimited;
@ -957,7 +921,20 @@ static void pidApplyFpvCameraAngleMix(pidState_t *pidState, uint8_t fpvCameraAng
pidState[YAW].rateTarget = constrainf(yawRate * cosCameraAngle + rollRate * sinCameraAngle, -GYRO_SATURATION_LIMIT, GYRO_SATURATION_LIMIT); pidState[YAW].rateTarget = constrainf(yawRate * cosCameraAngle + rollRate * sinCameraAngle, -GYRO_SATURATION_LIMIT, GYRO_SATURATION_LIMIT);
} }
void FAST_CODE pidController(float dT) void FAST_CODE checkItermLimitingActive(pidState_t *pidState)
{
bool shouldActivate;
if (usedPidControllerType == PID_TYPE_PIFF) {
shouldActivate = isFixedWingItermLimitActive(pidState->stickPosition);
} else
{
shouldActivate = mixerIsOutputSaturated();
}
pidState->itermLimitActive = STATE(ANTI_WINDUP) || shouldActivate;
}
void FAST_CODE NOINLINE pidController(float dT)
{ {
if (!pidFiltersConfigured) { if (!pidFiltersConfigured) {
return; return;
@ -1008,30 +985,29 @@ void FAST_CODE pidController(float dT)
pidApplyFpvCameraAngleMix(pidState, currentControlRateProfile->misc.fpvCamAngleDegrees); pidApplyFpvCameraAngleMix(pidState, currentControlRateProfile->misc.fpvCamAngleDegrees);
} }
// Apply setpoint rate of change limits // Prevent strong Iterm accumulation during stick inputs
for (int axis = 0; axis < 3; axis++) { antiWindupScaler = constrainf((1.0f - getMotorMixRange()) / motorItermWindupPoint, 0.0f, 1.0f);
pidApplySetpointRateLimiting(&pidState[axis], axis, dT);
} #ifdef USE_ANTIGRAVITY
iTermAntigravityGain = scaleRangef(fabsf(antigravityThrottleHpf) * antigravityAccelerator, 0.0f, 1000.0f, 1.0f, antigravityGain);
#endif
// Step 4: Run gyro-driven control
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
// Apply PID setpoint controller // Apply setpoint rate of change limits
if (STATE(FIXED_WING)) { pidApplySetpointRateLimiting(&pidState[axis], axis, dT);
pidApplyFixedWingRateController(&pidState[axis], axis, dT);
} // Step 4: Run gyro-driven control
else { checkItermLimitingActive(&pidState[axis]);
pidApplyMulticopterRateController(&pidState[axis], axis, dT); pidControllerApplyFn(&pidState[axis], axis, dT);
}
} }
} }
pidType_e pidIndexGetType(pidIndex_e pidIndex) pidType_e pidIndexGetType(pidIndex_e pidIndex)
{ {
if (pidIndex == PID_ROLL || pidIndex == PID_PITCH || pidIndex == PID_YAW) {
return usedPidControllerType;
}
if (STATE(FIXED_WING)) { if (STATE(FIXED_WING)) {
// FW specific
if (pidIndex == PID_ROLL || pidIndex == PID_PITCH || pidIndex == PID_YAW) {
return PID_TYPE_PIFF;
}
if (pidIndex == PID_VEL_XY || pidIndex == PID_VEL_Z) { if (pidIndex == PID_VEL_XY || pidIndex == PID_VEL_Z) {
return PID_TYPE_NONE; return PID_TYPE_NONE;
} }
@ -1042,3 +1018,68 @@ pidType_e pidIndexGetType(pidIndex_e pidIndex)
} }
return PID_TYPE_PID; return PID_TYPE_PID;
} }
void pidInit(void)
{
pidResetTPAFilter();
// Calculate max overall tilt (max pitch + max roll combined) as a limit to heading hold
headingHoldCosZLimit = cos_approx(DECIDEGREES_TO_RADIANS(pidProfile()->max_angle_inclination[FD_ROLL])) *
cos_approx(DECIDEGREES_TO_RADIANS(pidProfile()->max_angle_inclination[FD_PITCH]));
pidGainsUpdateRequired = false;
itermRelax = pidProfile()->iterm_relax;
itermRelaxType = pidProfile()->iterm_relax_type;
itermRelaxSetpointThreshold = MC_ITERM_RELAX_SETPOINT_THRESHOLD * MC_ITERM_RELAX_CUTOFF_DEFAULT / pidProfile()->iterm_relax_cutoff;
if (mixerConfig()->platformType == PLATFORM_MULTIROTOR) {
yawPLimit = pidProfile()->yaw_p_limit;
} else {
yawPLimit = 0;
}
yawLpfHz = pidProfile()->yaw_lpf_hz;
pidSumLimit = pidProfile()->pidSumLimit;
motorItermWindupPoint = 1.0f - (pidProfile()->itermWindupPointPercent / 100.0f);
#ifdef USE_D_BOOST
dBoostFactor = pidProfile()->dBoostFactor;
dBoostMaxAtAlleceleration = pidProfile()->dBoostMaxAtAlleceleration;
#endif
#ifdef USE_ANTIGRAVITY
antigravityGain = pidProfile()->antigravityGain;
antigravityAccelerator = pidProfile()->antigravityAccelerator;
#endif
if (pidProfile()->pidControllerType == PID_TYPE_AUTO) {
if (mixerConfig()->platformType == PLATFORM_AIRPLANE) {
usedPidControllerType = PID_TYPE_PIFF;
} else {
usedPidControllerType = PID_TYPE_PID;
}
} else {
usedPidControllerType = pidProfile()->pidControllerType;
}
if (pidProfile()->dterm_lpf_hz) {
dTermLpfFilterApplyFn = (filterApplyFnPtr) biquadFilterApply;
} else {
dTermLpfFilterApplyFn = nullFilterApply;
}
if (usedPidControllerType == PID_TYPE_PIFF) {
pidControllerApplyFn = pidApplyFixedWingRateController;
} else if (usedPidControllerType == PID_TYPE_PID) {
pidControllerApplyFn = pidApplyMulticopterRateController;
} else {
pidControllerApplyFn = nullRateController;
}
}
const pidBank_t FAST_CODE NOINLINE * pidBank(void) {
return usedPidControllerType == PID_TYPE_PIFF ? &pidProfile()->bank_fw : &pidProfile()->bank_mc;
}
pidBank_t FAST_CODE NOINLINE * pidBankMutable(void) {
return usedPidControllerType == PID_TYPE_PIFF ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc;
}

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

@ -26,7 +26,7 @@
#define PID_SUM_LIMIT_DEFAULT 500 #define PID_SUM_LIMIT_DEFAULT 500
#define YAW_P_LIMIT_MIN 100 // Maximum value for yaw P limiter #define YAW_P_LIMIT_MIN 100 // Maximum value for yaw P limiter
#define YAW_P_LIMIT_MAX 500 // Maximum value for yaw P limiter #define YAW_P_LIMIT_MAX 500 // Maximum value for yaw P limiter
#define YAW_P_LIMIT_DEFAULT 300 // Default value for yaw P limiter #define YAW_P_LIMIT_DEFAULT 500 // Default value for yaw P limiter
#define HEADING_HOLD_RATE_LIMIT_MIN 10 #define HEADING_HOLD_RATE_LIMIT_MIN 10
#define HEADING_HOLD_RATE_LIMIT_MAX 250 #define HEADING_HOLD_RATE_LIMIT_MAX 250
@ -51,7 +51,7 @@ FP-PID has been rescaled to match LuxFloat (and MWRewrite) from Cleanflight 1.13
#define FP_PID_YAWHOLD_P_MULTIPLIER 80.0f #define FP_PID_YAWHOLD_P_MULTIPLIER 80.0f
#define MC_ITERM_RELAX_SETPOINT_THRESHOLD 40.0f #define MC_ITERM_RELAX_SETPOINT_THRESHOLD 40.0f
#define MC_ITERM_RELAX_CUTOFF_DEFAULT 20 #define MC_ITERM_RELAX_CUTOFF_DEFAULT 15
#define ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF 15 // The anti gravity throttle highpass filter cutoff #define ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF 15 // The anti gravity throttle highpass filter cutoff
@ -72,9 +72,10 @@ typedef enum {
// TODO(agh): PIDFF // TODO(agh): PIDFF
typedef enum { typedef enum {
PID_TYPE_NONE, // Not used in the current platform/mixer/configuration PID_TYPE_NONE = 0, // Not used in the current platform/mixer/configuration
PID_TYPE_PID, // Uses P, I and D terms PID_TYPE_PID, // Uses P, I and D terms
PID_TYPE_PIFF, // Uses P, I and FF, ignoring D PID_TYPE_PIFF, // Uses P, I and FF, ignoring D
PID_TYPE_AUTO, // Autodetect
} pidType_e; } pidType_e;
typedef struct pid8_s { typedef struct pid8_s {
@ -100,6 +101,7 @@ typedef enum {
} itermRelaxType_e; } itermRelaxType_e;
typedef struct pidProfile_s { typedef struct pidProfile_s {
uint8_t pidControllerType;
pidBank_t bank_fw; pidBank_t bank_fw;
pidBank_t bank_mc; pidBank_t bank_mc;
@ -155,18 +157,14 @@ typedef struct pidAutotuneConfig_s {
PG_DECLARE_PROFILE(pidProfile_t, pidProfile); PG_DECLARE_PROFILE(pidProfile_t, pidProfile);
PG_DECLARE(pidAutotuneConfig_t, pidAutotuneConfig); PG_DECLARE(pidAutotuneConfig_t, pidAutotuneConfig);
static inline const pidBank_t * pidBank(void) { return STATE(FIXED_WING) ? &pidProfile()->bank_fw : &pidProfile()->bank_mc; } const pidBank_t * pidBank(void);
static inline pidBank_t * pidBankMutable(void) { return STATE(FIXED_WING) ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc; } pidBank_t * pidBankMutable(void);
extern int16_t axisPID[]; extern int16_t axisPID[];
extern int32_t axisPID_P[], axisPID_I[], axisPID_D[], axisPID_Setpoint[]; extern int32_t axisPID_P[], axisPID_I[], axisPID_D[], axisPID_Setpoint[];
void pidInit(void); void pidInit(void);
#ifdef USE_DTERM_NOTCH
bool pidInitFilters(void); bool pidInitFilters(void);
#endif
void pidResetErrorAccumulators(void); void pidResetErrorAccumulators(void);
void pidResetTPAFilter(void); void pidResetTPAFilter(void);

232
src/main/flight/rpm_filter.c Normal file
View file

@ -0,0 +1,232 @@
/*
* 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 "flight/rpm_filter.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "build/debug.h"
#include "common/axis.h"
#include "common/utils.h"
#include "common/maths.h"
#include "common/filter.h"
#include "flight/mixer.h"
#include "sensors/esc_sensor.h"
#include "fc/config.h"
#ifdef USE_RPM_FILTER
#define RPM_TO_HZ 60.0f
#define RPM_FILTER_RPM_LPF_HZ 150
#define RPM_FILTER_HARMONICS 3
PG_REGISTER_WITH_RESET_TEMPLATE(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONFIG, 0);
PG_RESET_TEMPLATE(rpmFilterConfig_t, rpmFilterConfig,
.gyro_filter_enabled = 0,
.dterm_filter_enabled = 0,
.gyro_harmonics = 1,
.gyro_min_hz = 100,
.gyro_q = 500,
.dterm_harmonics = 1,
.dterm_min_hz = 100,
.dterm_q = 500, );
typedef struct
{
float q;
float minHz;
float maxHz;
uint8_t harmonics;
biquadFilter_t filters[XYZ_AXIS_COUNT][MAX_SUPPORTED_MOTORS][RPM_FILTER_HARMONICS];
} rpmFilterBank_t;
typedef float (*rpmFilterApplyFnPtr)(rpmFilterBank_t *filter, uint8_t axis, float input);
typedef void (*rpmFilterUpdateFnPtr)(rpmFilterBank_t *filterBank, uint8_t motor, float baseFrequency);
static EXTENDED_FASTRAM pt1Filter_t motorFrequencyFilter[MAX_SUPPORTED_MOTORS];
static EXTENDED_FASTRAM float erpmToHz;
static EXTENDED_FASTRAM rpmFilterBank_t gyroRpmFilters;
static EXTENDED_FASTRAM rpmFilterBank_t dtermRpmFilters;
static EXTENDED_FASTRAM rpmFilterApplyFnPtr rpmGyroApplyFn;
static EXTENDED_FASTRAM rpmFilterApplyFnPtr rpmDtermApplyFn;
static EXTENDED_FASTRAM rpmFilterUpdateFnPtr rpmGyroUpdateFn;
static EXTENDED_FASTRAM rpmFilterUpdateFnPtr rpmDtermUpdateFn;
float nullRpmFilterApply(rpmFilterBank_t *filter, uint8_t axis, float input)
{
UNUSED(filter);
UNUSED(axis);
return input;
}
void nullRpmFilterUpdate(rpmFilterBank_t *filterBank, uint8_t motor, float baseFrequency) {
UNUSED(filterBank);
UNUSED(motor);
UNUSED(baseFrequency);
}
float FAST_CODE rpmFilterApply(rpmFilterBank_t *filterBank, uint8_t axis, float input)
{
float output = input;
for (uint8_t motor = 0; motor < getMotorCount(); motor++)
{
for (int harmonicIndex = 0; harmonicIndex < filterBank->harmonics; harmonicIndex++)
{
output = biquadFilterApplyDF1(
&filterBank->filters[axis][motor][harmonicIndex],
output
);
}
}
return output;
}
static void rpmFilterInit(rpmFilterBank_t *filter, uint16_t q, uint8_t minHz, uint8_t harmonics)
{
filter->q = q / 100.0f;
filter->minHz = minHz;
filter->harmonics = harmonics;
/*
* Max frequency has to be lower than Nyquist frequency for looptime
*/
filter->maxHz = 0.48f * 1000000.0f / getLooptime();
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++)
{
for (int motor = 0; motor < getMotorCount(); motor++)
{
/*
* Harmonics are indexed from 1 where 1 means base frequency
* C indexes arrays from 0, so we need to shift
*/
for (int harmonicIndex = 0; harmonicIndex < harmonics; harmonicIndex++)
{
biquadFilterInit(
&filter->filters[axis][motor][harmonicIndex],
filter->minHz * (harmonicIndex + 1),
getLooptime(),
filter->q,
FILTER_NOTCH);
}
}
}
}
void disableRpmFilters(void) {
rpmGyroApplyFn = (rpmFilterApplyFnPtr)nullRpmFilterApply;
rpmDtermApplyFn = (rpmFilterApplyFnPtr)nullRpmFilterApply;
}
void FAST_CODE NOINLINE rpmFilterUpdate(rpmFilterBank_t *filterBank, uint8_t motor, float baseFrequency)
{
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++)
{
for (int harmonicIndex = 0; harmonicIndex < filterBank->harmonics; harmonicIndex++)
{
float harmonicFrequency = baseFrequency * (harmonicIndex + 1);
harmonicFrequency = constrainf(harmonicFrequency, filterBank->minHz, filterBank->maxHz);
biquadFilterUpdate(
&filterBank->filters[axis][motor][harmonicIndex],
harmonicFrequency,
getLooptime(),
filterBank->q,
FILTER_NOTCH);
}
}
}
void rpmFiltersInit(void)
{
for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++)
{
pt1FilterInit(&motorFrequencyFilter[i], RPM_FILTER_RPM_LPF_HZ, RPM_FILTER_UPDATE_RATE_US * 1e-6f);
}
erpmToHz = ERPM_PER_LSB / (motorConfig()->motorPoleCount / 2) / RPM_TO_HZ;
rpmGyroUpdateFn = (rpmFilterUpdateFnPtr)nullRpmFilterUpdate;
rpmDtermUpdateFn = (rpmFilterUpdateFnPtr)nullRpmFilterUpdate;
if (rpmFilterConfig()->gyro_filter_enabled)
{
rpmFilterInit(
&gyroRpmFilters,
rpmFilterConfig()->gyro_q,
rpmFilterConfig()->gyro_min_hz,
rpmFilterConfig()->gyro_harmonics);
rpmGyroApplyFn = (rpmFilterApplyFnPtr)rpmFilterApply;
rpmGyroUpdateFn = (rpmFilterUpdateFnPtr)rpmFilterUpdate;
}
if (rpmFilterConfig()->dterm_filter_enabled)
{
rpmFilterInit(
&dtermRpmFilters,
rpmFilterConfig()->dterm_q,
rpmFilterConfig()->dterm_min_hz,
rpmFilterConfig()->dterm_harmonics);
rpmDtermApplyFn = (rpmFilterApplyFnPtr)rpmFilterApply;
rpmDtermUpdateFn = (rpmFilterUpdateFnPtr)rpmFilterUpdate;
}
}
void FAST_CODE NOINLINE rpmFilterUpdateTask(timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
uint8_t motorCount = getMotorCount();
/*
* For each motor, read ERPM, filter it and update motor frequency
*/
for (uint8_t i = 0; i < motorCount; i++)
{
const escSensorData_t *escState = getEscTelemetry(i); //Get ESC telemetry
const float baseFrequency = pt1FilterApply(&motorFrequencyFilter[i], escState->rpm * erpmToHz); //Filter motor frequency
if (i < 4) {
DEBUG_SET(DEBUG_RPM_FREQ, i, (int)baseFrequency);
}
rpmGyroUpdateFn(&gyroRpmFilters, i, baseFrequency);
rpmDtermUpdateFn(&dtermRpmFilters, i, baseFrequency);
}
}
float FAST_CODE rpmFilterGyroApply(uint8_t axis, float input)
{
return rpmGyroApplyFn(&gyroRpmFilters, axis, input);
}
float FAST_CODE rpmFilterDtermApply(uint8_t axis, float input)
{
return rpmDtermApplyFn(&dtermRpmFilters, axis, input);
}
#endif

53
src/main/flight/rpm_filter.h Normal file
View file

@ -0,0 +1,53 @@
/*
* This file is part of INAV Project.
*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
* You can obtain one at http://mozilla.org/MPL/2.0/.
*
* Alternatively, the contents of this file may be used under the terms
* of the GNU General Public License Version 3, as described below:
*
* This file is free software: you may copy, redistribute and/or modify
* it under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or (at your
* option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
* Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see http://www.gnu.org/licenses/.
*/
#pragma once
#include "config/parameter_group.h"
#include "common/time.h"
typedef struct rpmFilterConfig_s {
uint8_t gyro_filter_enabled;
uint8_t dterm_filter_enabled;
uint8_t gyro_harmonics;
uint8_t gyro_min_hz;
uint16_t gyro_q;
uint8_t dterm_harmonics;
uint8_t dterm_min_hz;
uint16_t dterm_q;
} rpmFilterConfig_t;
PG_DECLARE(rpmFilterConfig_t, rpmFilterConfig);
#define RPM_FILTER_UPDATE_RATE_HZ 500
#define RPM_FILTER_UPDATE_RATE_US (1000000.0f / RPM_FILTER_UPDATE_RATE_HZ)
void disableRpmFilters(void);
void rpmFiltersInit(void);
void rpmFilterUpdateTask(timeUs_t currentTimeUs);
float rpmFilterGyroApply(uint8_t axis, float input);
float rpmFilterDtermApply(uint8_t axis, float input);

5
src/main/flight/servos.h
View file

@ -108,9 +108,8 @@ typedef struct servoMixer_s {
#define MAX_SERVO_RULES (2 * MAX_SUPPORTED_SERVOS) #define MAX_SERVO_RULES (2 * MAX_SUPPORTED_SERVOS)
#define MAX_SERVO_SPEED UINT8_MAX #define MAX_SERVO_SPEED UINT8_MAX
#define MAX_SERVO_BOXES 3 #define SERVO_OUTPUT_MAX 2500
#define SERVO_OUTPUT_MIN 500
#define SERVO_MIXER_INPUT_WIDTH 1000
PG_DECLARE_ARRAY(servoMixer_t, MAX_SERVO_RULES, customServoMixers); PG_DECLARE_ARRAY(servoMixer_t, MAX_SERVO_RULES, customServoMixers);

531
src/main/io/displayport_frsky_osd.c Normal file
View file

@ -0,0 +1,531 @@
/*
* 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 <stdint.h>
#include "platform.h"
#if defined(USE_FRSKYOSD)
#include "common/utils.h"
#include "drivers/display.h"
#include "drivers/display_canvas.h"
#include "drivers/display_font_metadata.h"
#include "io/displayport_frsky_osd.h"
#include "io/frsky_osd.h"
static displayPort_t frskyOSDDisplayPort;
static int grab(displayPort_t *displayPort)
{
UNUSED(displayPort);
return 0;
}
static int release(displayPort_t *displayPort)
{
UNUSED(displayPort);
return 0;
}
static int clearScreen(displayPort_t *displayPort)
{
UNUSED(displayPort);
frskyOSDClearScreen();
return 0;
}
static int drawScreen(displayPort_t *displayPort)
{
UNUSED(displayPort);
frskyOSDUpdate();
return 0;
}
static int screenSize(const displayPort_t *displayPort)
{
UNUSED(displayPort);
return frskyOSDGetGridRows() * frskyOSDGetGridCols();
}
static int writeString(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *s, textAttributes_t attr)
{
UNUSED(displayPort);
frskyOSDDrawStringInGrid(x, y, s, attr);
return 0;
}
static int writeChar(displayPort_t *displayPort, uint8_t x, uint8_t y, uint16_t c, textAttributes_t attr)
{
UNUSED(displayPort);
frskyOSDDrawCharInGrid(x, y, c, attr);
return 0;
}
static bool readChar(displayPort_t *displayPort, uint8_t x, uint8_t y, uint16_t *c, textAttributes_t *attr)
{
UNUSED(displayPort);
return frskyOSDReadCharInGrid(x, y, c, attr);
}
static bool isTransferInProgress(const displayPort_t *displayPort)
{
UNUSED(displayPort);
return false;
}
static void updateGridSize(displayPort_t *displayPort)
{
displayPort->rows = frskyOSDGetGridRows();
displayPort->cols = frskyOSDGetGridCols();
}
static void resync(displayPort_t *displayPort)
{
UNUSED(displayPort);
// TODO(agh): Do we need to flush the screen here?
// MAX7456's driver does a full redraw in resync(),
// so some callers might be expecting that.
updateGridSize(displayPort);
}
static int heartbeat(displayPort_t *displayPort)
{
UNUSED(displayPort);
return 0;
}
static uint32_t txBytesFree(const displayPort_t *displayPort)
{
UNUSED(displayPort);
return UINT32_MAX;
}
static textAttributes_t supportedTextAttributes(const displayPort_t *displayPort)
{
UNUSED(displayPort);
textAttributes_t attr = TEXT_ATTRIBUTES_NONE;
TEXT_ATTRIBUTES_ADD_INVERTED(attr);
TEXT_ATTRIBUTES_ADD_SOLID_BG(attr);
return attr;
}
static bool getFontMetadata(displayFontMetadata_t *metadata, const displayPort_t *displayPort)
{
UNUSED(displayPort);
osdCharacter_t chr;
metadata->charCount = 512;
return frskyOSDReadFontCharacter(FONT_METADATA_CHR_INDEX, &chr) &&
displayFontMetadataUpdateFromCharacter(metadata, &chr);
}
static int writeFontCharacter(displayPort_t *instance, uint16_t addr, const osdCharacter_t *chr)
{
UNUSED(instance);
frskyOSDWriteFontCharacter(addr, chr);
return 0;
}
static bool isReady(displayPort_t *instance)
{
if (frskyOSDIsReady()) {
updateGridSize(instance);
return true;
}
return false;
}
static void beginTransaction(displayPort_t *instance, displayTransactionOption_e opts)
{
UNUSED(instance);
frskyOSDTransactionOptions_e frskyOpts = 0;
if (opts & DISPLAY_TRANSACTION_OPT_PROFILED) {
frskyOpts |= FRSKY_OSD_TRANSACTION_OPT_PROFILED;
}
if (opts & DISPLAY_TRANSACTION_OPT_RESET_DRAWING) {
frskyOpts |= FRSKY_OSD_TRANSACTION_OPT_RESET_DRAWING;
}
frskyOSDBeginTransaction(frskyOpts);
}
static void commitTransaction(displayPort_t *instance)
{
UNUSED(instance);
frskyOSDCommitTransaction();
}
static frskyOSDColor_e frskyOSDGetColor(displayCanvasColor_e color)
{
switch (color)
{
case DISPLAY_CANVAS_COLOR_BLACK:
return FRSKY_OSD_COLOR_BLACK;
case DISPLAY_CANVAS_COLOR_TRANSPARENT:
return FRSKY_OSD_COLOR_TRANSPARENT;
case DISPLAY_CANVAS_COLOR_WHITE:
return FRSKY_OSD_COLOR_WHITE;
case DISPLAY_CANVAS_COLOR_GRAY:
return FRSKY_OSD_COLOR_GRAY;
}
return FRSKY_OSD_COLOR_BLACK;
}
static void setStrokeColor(displayCanvas_t *displayCanvas, displayCanvasColor_e color)
{
UNUSED(displayCanvas);
frskyOSDSetStrokeColor(frskyOSDGetColor(color));
}
static void setFillColor(displayCanvas_t *displayCanvas, displayCanvasColor_e color)
{
UNUSED(displayCanvas);
frskyOSDSetFillColor(frskyOSDGetColor(color));
}
static void setStrokeAndFillColor(displayCanvas_t *displayCanvas, displayCanvasColor_e color)
{
UNUSED(displayCanvas);
frskyOSDSetStrokeAndFillColor(frskyOSDGetColor(color));
}
static void setColorInversion(displayCanvas_t *displayCanvas, bool inverted)
{
UNUSED(displayCanvas);
frskyOSDSetColorInversion(inverted);
}
static void setPixel(displayCanvas_t *displayCanvas, int x, int y, displayCanvasColor_e color)
{
UNUSED(displayCanvas);
frskyOSDSetPixel(x, y, frskyOSDGetColor(color));
}
static void setPixelToStrokeColor(displayCanvas_t *displayCanvas, int x, int y)
{
UNUSED(displayCanvas);
frskyOSDSetPixelToStrokeColor(x, y);
}
static void setPixelToFillColor(displayCanvas_t *displayCanvas, int x, int y)
{
UNUSED(displayCanvas);
frskyOSDSetPixelToFillColor(x, y);
}
static void setStrokeWidth(displayCanvas_t *displayCanvas, unsigned w)
{
UNUSED(displayCanvas);
frskyOSDSetStrokeWidth(w);
}
static void setLineOutlineType(displayCanvas_t *displayCanvas, displayCanvasOutlineType_e outlineType)
{
UNUSED(displayCanvas);
frskyOSDSetLineOutlineType(outlineType);
}
static void setLineOutlineColor(displayCanvas_t *displayCanvas, displayCanvasColor_e outlineColor)
{
UNUSED(displayCanvas);
frskyOSDSetLineOutlineColor(outlineColor);
}
static void clipToRect(displayCanvas_t *displayCanvas, int x, int y, int w, int h)
{
UNUSED(displayCanvas);
frskyOSDClipToRect(x, y, w, h);
}
static void clearRect(displayCanvas_t *displayCanvas, int x, int y, int w, int h)
{
UNUSED(displayCanvas);
frskyOSDClearRect(x, y, w, h);
}
static void resetDrawingState(displayCanvas_t *displayCanvas)
{
UNUSED(displayCanvas);
frskyOSDResetDrawingState();
}
static void drawCharacter(displayCanvas_t *displayCanvas, int x, int y, uint16_t chr, displayCanvasBitmapOption_t opts)
{
UNUSED(displayCanvas);
frskyOSDDrawCharacter(x, y, chr, opts);
}
static void drawCharacterMask(displayCanvas_t *displayCanvas, int x, int y, uint16_t chr, displayCanvasColor_e color, displayCanvasBitmapOption_t opts)
{
UNUSED(displayCanvas);
frskyOSDDrawCharacterMask(x, y, chr, frskyOSDGetColor(color), opts);
}
static void drawString(displayCanvas_t *displayCanvas, int x, int y, const char *s, displayCanvasBitmapOption_t opts)
{
UNUSED(displayCanvas);
frskyOSDDrawString(x, y, s, opts);
}
static void drawStringMask(displayCanvas_t *displayCanvas, int x, int y, const char *s, displayCanvasColor_e color, displayCanvasBitmapOption_t opts)
{
UNUSED(displayCanvas);
frskyOSDDrawStringMask(x, y, s, frskyOSDGetColor(color), opts);
}
static void moveToPoint(displayCanvas_t *displayCanvas, int x, int y)
{
UNUSED(displayCanvas);
frskyOSDMoveToPoint(x, y);
}
static void strokeLineToPoint(displayCanvas_t *displayCanvas, int x, int y)
{
UNUSED(displayCanvas);
frskyOSDStrokeLineToPoint(x, y);
}
static void strokeTriangle(displayCanvas_t *displayCanvas, int x1, int y1, int x2, int y2, int x3, int y3)
{
UNUSED(displayCanvas);
frskyOSDStrokeTriangle(x1, y1, x2, y2, x3, y3);
}
static void fillTriangle(displayCanvas_t *displayCanvas, int x1, int y1, int x2, int y2, int x3, int y3)
{
UNUSED(displayCanvas);
frskyOSDFillTriangle(x1, y1, x2, y2, x3, y3);
}
static void fillStrokeTriangle(displayCanvas_t *displayCanvas, int x1, int y1, int x2, int y2, int x3, int y3)
{
UNUSED(displayCanvas);
frskyOSDFillStrokeTriangle(x1, y1, x2, y2, x3, y3);
}
static void strokeRect(displayCanvas_t *displayCanvas, int x, int y, int w, int h)
{
UNUSED(displayCanvas);
frskyOSDStrokeRect(x, y, w, h);
}
static void fillRect(displayCanvas_t *displayCanvas, int x, int y, int w, int h)
{
UNUSED(displayCanvas);
frskyOSDFillRect(x, y, w, h);
}
static void fillStrokeRect(displayCanvas_t *displayCanvas, int x, int y, int w, int h)
{
UNUSED(displayCanvas);
frskyOSDFillStrokeRect(x, y, w, h);
}
static void strokeEllipseInRect(displayCanvas_t *displayCanvas, int x, int y, int w, int h)
{
UNUSED(displayCanvas);
frskyOSDStrokeEllipseInRect(x, y, w, h);
}
static void fillEllipseInRect(displayCanvas_t *displayCanvas, int x, int y, int w, int h)
{
UNUSED(displayCanvas);
frskyOSDFillEllipseInRect(x, y, w, h);
}
static void fillStrokeEllipseInRect(displayCanvas_t *displayCanvas, int x, int y, int w, int h)
{
UNUSED(displayCanvas);
frskyOSDFillStrokeEllipseInRect(x, y, w, h);
}
static void ctmReset(displayCanvas_t *displayCanvas)
{
UNUSED(displayCanvas);
frskyOSDCtmReset();
}
static void ctmSet(displayCanvas_t *displayCanvas, float m11, float m12, float m21, float m22, float m31, float m32)
{
UNUSED(displayCanvas);
frskyOSDCtmSet(m11, m12, m21, m22, m31, m32);
}
static void ctmTranslate(displayCanvas_t *displayCanvas, float tx, float ty)
{
UNUSED(displayCanvas);
frskyOSDCtmTranslate(tx, ty);
}
static void ctmScale(displayCanvas_t *displayCanvas, float sx, float sy)
{
UNUSED(displayCanvas);
frskyOSDCtmScale(sx, sy);
}
static void ctmRotate(displayCanvas_t *displayCanvas, float r)
{
UNUSED(displayCanvas);
frskyOSDCtmRotate(r);
}
static void contextPush(displayCanvas_t *displayCanvas)
{
UNUSED(displayCanvas);
frskyOSDContextPush();
}
static void contextPop(displayCanvas_t *displayCanvas)
{
UNUSED(displayCanvas);
frskyOSDContextPop();
}
static const displayCanvasVTable_t frskyOSDCanvasVTable = {
.setStrokeColor = setStrokeColor,
.setFillColor = setFillColor,
.setStrokeAndFillColor = setStrokeAndFillColor,
.setColorInversion = setColorInversion,
.setPixel = setPixel,
.setPixelToStrokeColor = setPixelToStrokeColor,
.setPixelToFillColor = setPixelToFillColor,
.setStrokeWidth = setStrokeWidth,
.setLineOutlineType = setLineOutlineType,
.setLineOutlineColor = setLineOutlineColor,
.clipToRect = clipToRect,
.clearRect = clearRect,
.resetDrawingState = resetDrawingState,
.drawCharacter = drawCharacter,
.drawCharacterMask = drawCharacterMask,
.drawString = drawString,
.drawStringMask = drawStringMask,
.moveToPoint = moveToPoint,
.strokeLineToPoint = strokeLineToPoint,
.strokeTriangle = strokeTriangle,
.fillTriangle = fillTriangle,
.fillStrokeTriangle = fillStrokeTriangle,
.strokeRect = strokeRect,
.fillRect = fillRect,
.fillStrokeRect = fillStrokeRect,
.strokeEllipseInRect = strokeEllipseInRect,
.fillEllipseInRect = fillEllipseInRect,
.fillStrokeEllipseInRect = fillStrokeEllipseInRect,
.ctmReset = ctmReset,
.ctmSet = ctmSet,
.ctmTranslate = ctmTranslate,
.ctmScale = ctmScale,
.ctmRotate = ctmRotate,
.contextPush = contextPush,
.contextPop = contextPop,
};
static bool getCanvas(displayCanvas_t *canvas, const displayPort_t *instance)
{
UNUSED(instance);
canvas->vTable = &frskyOSDCanvasVTable;
canvas->width = frskyOSDGetPixelWidth();
canvas->height = frskyOSDGetPixelHeight();
return true;
}
static const displayPortVTable_t frskyOSDVTable = {
.grab = grab,
.release = release,
.clearScreen = clearScreen,
.drawScreen = drawScreen,
.screenSize = screenSize,
.writeString = writeString,
.writeChar = writeChar,
.readChar = readChar,
.isTransferInProgress = isTransferInProgress,
.heartbeat = heartbeat,
.resync = resync,
.txBytesFree = txBytesFree,
.supportedTextAttributes = supportedTextAttributes,
.getFontMetadata = getFontMetadata,
.writeFontCharacter = writeFontCharacter,
.isReady = isReady,
.beginTransaction = beginTransaction,
.commitTransaction = commitTransaction,
.getCanvas = getCanvas,
};
displayPort_t *frskyOSDDisplayPortInit(const videoSystem_e videoSystem)
{
if (frskyOSDInit(videoSystem)) {
displayInit(&frskyOSDDisplayPort, &frskyOSDVTable);
resync(&frskyOSDDisplayPort);
return &frskyOSDDisplayPort;
}
return NULL;
}
#endif // USE_FRSKYOSD

7
src/main/io/displayport_frsky_osd.h Normal file
View file

@ -0,0 +1,7 @@
#pragma once
#include "drivers/osd.h"
typedef struct displayPort_s displayPort_t;
displayPort_t *frskyOSDDisplayPortInit(const videoSystem_e videoSystem);

21
src/main/io/displayport_max7456.c
View file

@ -28,17 +28,11 @@
#include "config/parameter_group_ids.h" #include "config/parameter_group_ids.h"
#include "drivers/display.h" #include "drivers/display.h"
#include "drivers/display_font_metadata.h"
#include "drivers/max7456.h" #include "drivers/max7456.h"
#include "io/displayport_max7456.h" #include "io/displayport_max7456.h"
// 'I', 'N', 'A', 'V', 1
#define CHR_IS_METADATA(chr) ((chr)->data[0] == 'I' && \
(chr)->data[1] == 'N' && \
(chr)->data[2] == 'A' && \
(chr)->data[3] == 'V' && \
(chr)->data[4] == 1)
displayPort_t max7456DisplayPort; displayPort_t max7456DisplayPort;
static uint8_t max7456Mode(textAttributes_t attr) static uint8_t max7456Mode(textAttributes_t attr)
@ -154,19 +148,14 @@ static bool getFontMetadata(displayFontMetadata_t *metadata, const displayPort_t
osdCharacter_t chr; osdCharacter_t chr;
max7456ReadNvm(255, &chr); max7456ReadNvm(FONT_METADATA_CHR_INDEX, &chr);
if (CHR_IS_METADATA(&chr)) { if (displayFontMetadataUpdateFromCharacter(metadata, &chr)) {
metadata->version = chr.data[5];
// Not all MAX7456 chips support 512 characters. To detect this, // Not all MAX7456 chips support 512 characters. To detect this,
// we place metadata in both characters 255 and 256. This way we // we place metadata in both characters 255 and 256. This way we
// can find out how many characters the font in NVM has. // can find out how many characters the font in NVM has.
max7456ReadNvm(256, &chr); max7456ReadNvm(FONT_METADATA_CHR_INDEX_2ND_PAGE, &chr);
if (CHR_IS_METADATA(&chr)) { metadata->charCount = FONT_CHR_IS_METADATA(&chr) ? 512 : 256;
metadata->charCount = 512;
} else {
metadata->charCount = 256;
}
return true; return true;
} }

992
src/main/io/frsky_osd.c Normal file
View file

@ -0,0 +1,992 @@
#include <stddef.h>
#include <stdint.h>
#include <string.h>
#include "platform.h"
#if defined(USE_FRSKYOSD)
#include "common/crc.h"
#include "common/log.h"
#include "common/maths.h"
#include "common/time.h"
#include "common/utils.h"
#include "common/uvarint.h"
#include "drivers/time.h"
#include "io/frsky_osd.h"
#include "io/serial.h"
#define FRSKY_OSD_BAUDRATE 115200
#define FRSKY_OSD_SUPPORTED_API_VERSION 1
#define FRSKY_OSD_PREAMBLE_BYTE_0 '$'
#define FRSKY_OSD_PREAMBLE_BYTE_1 'A'
#define FRSKY_OSD_GRID_BUFFER_CHAR_BITS 9
#define FRSKY_OSD_GRID_BUFFER_CHAR_MASK ((1 << FRSKY_OSD_GRID_BUFFER_CHAR_BITS) - 1)
#define FRSKY_OSD_GRID_BUFFER_ENCODE(chr, attr) ((chr & FRSKY_OSD_GRID_BUFFER_CHAR_MASK) | (attr << FRSKY_OSD_GRID_BUFFER_CHAR_BITS))
#define FRSKY_OSD_CHAR_ATTRIBUTE_COLOR_INVERSE (1 << 0)
#define FRSKY_OSD_CHAR_ATTRIBUTE_SOLID_BACKGROUND (1 << 1)
#define FRSKY_OSD_CHAR_DATA_BYTES 54
#define FRSKY_OSD_CHAR_METADATA_BYTES 10
#define FRSKY_OSD_CHAR_TOTAL_BYTES (FRSKY_OSD_CHAR_DATA_BYTES + FRSKY_OSD_CHAR_METADATA_BYTES)
#define FRSKY_OSD_SEND_BUFFER_SIZE 192
#define FRSKY_OSD_RECV_BUFFER_SIZE 128
#define FRSKY_OSD_CMD_RESPONSE_ERROR 0
#define FRSKY_OSD_INFO_INTERVAL_MS 1000
#define FRSKY_OSD_TRACE(fmt, ...)
#define FRSKY_OSD_DEBUG(fmt, ...) LOG_D(OSD, fmt, ##__VA_ARGS__)
#define FRSKY_OSD_ERROR(fmt, ...) LOG_E(OSD, fmt, ##__VA_ARGS__)
#define FRSKY_OSD_ASSERT(x)
typedef enum
{
OSD_CMD_RESPONSE_ERROR = 0,
OSD_CMD_INFO = 1,
OSD_CMD_READ_FONT = 2,
OSD_CMD_WRITE_FONT = 3,
OSD_CMD_GET_CAMERA = 4,
OSD_CMD_SET_CAMERA = 5,
OSD_CMD_GET_ACTIVE_CAMERA = 6,
OSD_CMD_GET_OSD_ENABLED = 7,
OSD_CMD_SET_OSD_ENABLED = 8,
OSD_CMD_TRANSACTION_BEGIN = 16,
OSD_CMD_TRANSACTION_COMMIT = 17,
OSD_CMD_TRANSACTION_BEGIN_PROFILED = 18,
OSD_CMD_TRANSACTION_BEGIN_RESET_DRAWING = 19,
OSD_CMD_DRAWING_SET_STROKE_COLOR = 22,
OSD_CMD_DRAWING_SET_FILL_COLOR = 23,
OSD_CMD_DRAWING_SET_STROKE_AND_FILL_COLOR = 24,
OSD_CMD_DRAWING_SET_COLOR_INVERSION = 25,
OSD_CMD_DRAWING_SET_PIXEL = 26,
OSD_CMD_DRAWING_SET_PIXEL_TO_STROKE_COLOR = 27,
OSD_CMD_DRAWING_SET_PIXEL_TO_FILL_COLOR = 28,
OSD_CMD_DRAWING_SET_STROKE_WIDTH = 29,
OSD_CMD_DRAWING_SET_LINE_OUTLINE_TYPE = 30,
OSD_CMD_DRAWING_SET_LINE_OUTLINE_COLOR = 31,
OSD_CMD_DRAWING_CLIP_TO_RECT = 40,
OSD_CMD_DRAWING_CLEAR_SCREEN = 41,
OSD_CMD_DRAWING_CLEAR_RECT = 42,
OSD_CMD_DRAWING_RESET = 43,
OSD_CMD_DRAWING_DRAW_BITMAP = 44,
OSD_CMD_DRAWING_DRAW_BITMAP_MASK = 45,
OSD_CMD_DRAWING_DRAW_CHAR = 46,
OSD_CMD_DRAWING_DRAW_CHAR_MASK = 47,
OSD_CMD_DRAWING_DRAW_STRING = 48,
OSD_CMD_DRAWING_DRAW_STRING_MASK = 49,
OSD_CMD_DRAWING_MOVE_TO_POINT = 50,
OSD_CMD_DRAWING_STROKE_LINE_TO_POINT = 51,
OSD_CMD_DRAWING_STROKE_TRIANGLE = 52,
OSD_CMD_DRAWING_FILL_TRIANGLE = 53,
OSD_CMD_DRAWING_FILL_STROKE_TRIANGLE = 54,
OSD_CMD_DRAWING_STROKE_RECT = 55,
OSD_CMD_DRAWING_FILL_RECT = 56,
OSD_CMD_DRAWING_FILL_STROKE_RECT = 57,
OSD_CMD_DRAWING_STROKE_ELLIPSE_IN_RECT = 58,
OSD_CMD_DRAWING_FILL_ELLIPSE_IN_RECT = 59,
OSD_CMD_DRAWING_FILL_STROKE_ELLIPSE_IN_RECT = 60,
OSD_CMD_CTM_RESET = 80,
OSD_CMD_CTM_SET = 81,
OSD_CMD_CTM_TRANSLATE = 82,
OSD_CMD_CTM_SCALE = 83,
OSD_CMD_CTM_ROTATE = 84,
OSD_CMD_CTM_ROTATE_ABOUT = 85,
OSD_CMD_CTM_SHEAR = 86,
OSD_CMD_CTM_SHEAR_ABOUT = 87,
OSD_CMD_CTM_MULTIPLY = 88,
OSD_CMD_CONTEXT_PUSH = 100,
OSD_CMD_CONTEXT_POP = 101,
// MAX7456 emulation commands
OSD_CMD_DRAW_GRID_CHR = 110,
OSD_CMD_DRAW_GRID_STR = 111,
} osdCommand_e;
typedef enum {
RECV_STATE_NONE,
RECV_STATE_SYNC,
RECV_STATE_LENGTH,
RECV_STATE_DATA,
RECV_STATE_CHECKSUM,
RECV_STATE_DONE,
} frskyOSDRecvState_e;
typedef struct frskyOSDInfoResponse_s {
uint8_t magic[3];
uint8_t versionMajor;
uint8_t versionMinor;
uint8_t versionPatch;
uint8_t gridRows;
uint8_t gridColumns;
uint16_t pixelWidth;
uint16_t pixelHeight;
uint8_t tvStandard;
uint8_t hasDetectedCamera;
uint16_t maxFrameSize;
uint8_t contextStackSize;
} __attribute__((packed)) frskyOSDInfoResponse_t;
typedef struct frskyOSDFontCharacter_s {
uint16_t addr;
struct {
uint8_t bitmap[FRSKY_OSD_CHAR_DATA_BYTES]; // 12x18 2bpp
uint8_t metadata[FRSKY_OSD_CHAR_METADATA_BYTES];
} data;
} __attribute__((packed)) frskyOSDCharacter_t;
typedef struct frskyOSDDrawGridCharCmd_s {
uint8_t gx;
uint8_t gy;
uint16_t chr;
uint8_t opts;
} __attribute__((packed)) frskyOSDDrawGridCharCmd_t;
typedef struct frskyOSDDrawGridStrHeaderCmd_s {
uint8_t gx;
uint8_t gy;
uint8_t opts;
// uvarint with size and blob folow
} __attribute__((packed)) frskyOSDDrawGridStrHeaderCmd_t;
typedef struct frskyOSDPoint_s {
int x : 12;
int y : 12;
} __attribute__((packed)) frskyOSDPoint_t;
typedef struct frskyOSDSize_s {
int w : 12;
int h : 12;
} __attribute__((packed)) frskyOSDSize_t;
typedef struct frskyOSDRect_s {
frskyOSDPoint_t origin;
frskyOSDSize_t size;
} __attribute__((packed)) frskyOSDRect_t;
typedef struct frskyOSDTriangle_s {
frskyOSDPoint_t p1;
frskyOSDPoint_t p2;
frskyOSDPoint_t p3;
} __attribute__((packed)) frskyOSDTriangle_t;
typedef struct frskyOSDSetPixel_s {
frskyOSDPoint_t p;
uint8_t color;
} __attribute__((packed)) frskyOSDSetPixel_t;
typedef struct frskyOSDDrawCharacterCmd_s {
frskyOSDPoint_t p;
uint16_t chr;
uint8_t opts;
} __attribute__((packed)) frskyOSDDrawCharacterCmd_t;
typedef struct frskyOSDDrawCharacterMaskCmd_s {
frskyOSDDrawCharacterCmd_t dc;
uint8_t maskColor;
} __attribute__((packed)) frskyOSDDrawCharacterMaskCmd_t;
typedef struct frskyOSDDrawStrCommandHeaderCmd_s {
frskyOSDPoint_t p;
uint8_t opts;
// uvarint with size and blob follow
} __attribute__((packed)) frskyOSDDrawStrCommandHeaderCmd_t;
typedef struct frskyOSDDrawStrMaskCommandHeaderCmd_s {
frskyOSDPoint_t p;
uint8_t opts;
uint8_t maskColor;
// uvarint with size and blob follow
} __attribute__((packed)) frskyOSDDrawStrMaskCommandHeaderCmd_t;
typedef struct frskyOSDState_s {
struct {
uint8_t data[FRSKY_OSD_SEND_BUFFER_SIZE];
uint8_t pos;
} sendBuffer;
struct {
uint8_t state;
uint8_t crc;
uint16_t expected;
uint8_t expectedShift;
uint8_t data[FRSKY_OSD_RECV_BUFFER_SIZE];
uint8_t pos;
} recvBuffer;
struct {
uint8_t major;
uint8_t minor;
timeMs_t nextRequest;
struct {
uint8_t rows;
uint8_t columns;
} grid;
struct {
uint16_t width;
uint16_t height;
} viewport;
} info;
struct {
uint16_t addr;
osdCharacter_t *chr;
} recvOsdCharacter;
serialPort_t *port;
bool initialized;
timeMs_t nextInfoRequest;
} frskyOSDState_t;
static frskyOSDState_t state;
static uint8_t frskyOSDChecksum(uint8_t crc, uint8_t c)
{
return crc8_dvb_s2(crc, c);
}
static void frskyOSDResetReceiveBuffer(void)
{
state.recvBuffer.state = RECV_STATE_NONE;
state.recvBuffer.crc = 0;
state.recvBuffer.expected = 0;
state.recvBuffer.expectedShift = 0;
state.recvBuffer.pos = 0;
}
static void frskyOSDResetSendBuffer(void)
{
state.sendBuffer.pos = 0;
}
static void frskyOSDProcessCommandU8(uint8_t *crc, uint8_t c)
{
while (serialTxBytesFree(state.port) == 0) {
};
serialWrite(state.port, c);
if (crc) {
*crc = crc8_dvb_s2(*crc, c);
}
}
static void frskyOSDSendCommand(uint8_t cmd, const void *payload, size_t size)
{
int required = size + 1;
FRSKY_OSD_ASSERT(required <= sizeof(state.sendBuffer.data));
int rem = sizeof(state.sendBuffer.data) - state.sendBuffer.pos;
if (rem < required) {
frskyOSDFlushSendBuffer();
}
state.sendBuffer.data[state.sendBuffer.pos++] = cmd;
const uint8_t *ptr = payload;
for (size_t ii = 0; ii < size; ii++, ptr++) {
state.sendBuffer.data[state.sendBuffer.pos++] = *ptr;
}
}
static void frskyOSDStateReset(serialPort_t *port)
{
frskyOSDResetReceiveBuffer();
frskyOSDResetSendBuffer();
state.info.grid.rows = 0;
state.info.grid.columns = 0;
state.info.viewport.width = 0;
state.info.viewport.height = 0;
state.port = port;
state.initialized = false;
}
static void frskyOSDUpdateReceiveBuffer(void)
{
while (serialRxBytesWaiting(state.port) > 0) {
uint8_t c = serialRead(state.port);
switch ((frskyOSDRecvState_e)state.recvBuffer.state) {
case RECV_STATE_NONE:
if (c != FRSKY_OSD_PREAMBLE_BYTE_0) {
break;
}
state.recvBuffer.state = RECV_STATE_SYNC;
break;
case RECV_STATE_SYNC:
if (c != FRSKY_OSD_PREAMBLE_BYTE_1) {
frskyOSDResetReceiveBuffer();
break;
}
state.recvBuffer.state = RECV_STATE_LENGTH;
break;
case RECV_STATE_LENGTH:
state.recvBuffer.crc = frskyOSDChecksum(state.recvBuffer.crc, c);
state.recvBuffer.expected |= (c & 0x7F) << state.recvBuffer.expectedShift;
state.recvBuffer.expectedShift += 7;
if (c < 0x80) {
// Full uvarint decoded. Check against buffer size.
if (state.recvBuffer.expected > sizeof(state.recvBuffer.data)) {
FRSKY_OSD_ERROR("Can't handle payload of size %u with a buffer of size %u",
state.recvBuffer.expected, sizeof(state.recvBuffer.data));
frskyOSDResetReceiveBuffer();
break;
}
FRSKY_OSD_TRACE("Payload of size %u", state.recvBuffer.expected);
state.recvBuffer.state = state.recvBuffer.expected > 0 ? RECV_STATE_DATA : RECV_STATE_CHECKSUM;
}
break;
case RECV_STATE_DATA:
state.recvBuffer.data[state.recvBuffer.pos++] = c;
state.recvBuffer.crc = frskyOSDChecksum(state.recvBuffer.crc, c);
if (state.recvBuffer.pos == state.recvBuffer.expected) {
state.recvBuffer.state = RECV_STATE_CHECKSUM;
}
break;
case RECV_STATE_CHECKSUM:
if (c != state.recvBuffer.crc) {
FRSKY_OSD_DEBUG("Checksum error %u != %u. Discarding %u bytes",
c, state.recvBuffer.crc, state.recvBuffer.pos);
frskyOSDResetReceiveBuffer();
break;
}
state.recvBuffer.state = RECV_STATE_DONE;
break;
case RECV_STATE_DONE:
FRSKY_OSD_DEBUG("Received unexpected byte %u after data", c);
break;
}
}
}
static bool frskyOSDIsResponseAvailable(void)
{
return state.recvBuffer.state == RECV_STATE_DONE;
}
static bool frskyOSDHandleCommand(osdCommand_e cmd, const void *payload, size_t size)
{
const uint8_t *ptr = payload;
switch (cmd) {
case OSD_CMD_RESPONSE_ERROR:
{
if (size >= 2) {
FRSKY_OSD_ERROR("Received an error %02x in response to command %u", *(ptr + 1), *ptr);
return true;
}
break;
}
case OSD_CMD_INFO:
{
if (size < sizeof(frskyOSDInfoResponse_t)) {
break;
}
const frskyOSDInfoResponse_t *resp = payload;
if (resp->magic[0] != 'A' || resp->magic[1] != 'G' || resp->magic[2] != 'H') {
FRSKY_OSD_ERROR("Invalid magic number %x %x %x, expecting AGH",
resp->magic[0], resp->magic[1], resp->magic[2]);
return false;
}
state.info.major = resp->versionMajor;
state.info.minor = resp->versionMinor;
state.info.grid.rows = resp->gridRows;
state.info.grid.columns = resp->gridColumns;
state.info.viewport.width = resp->pixelWidth;
state.info.viewport.height = resp->pixelHeight;
if (!state.initialized) {
FRSKY_OSD_DEBUG("FrSky OSD initialized. Version %u.%u.%u, pixels=%ux%u, grid=%ux%u",
resp->versionMajor, resp->versionMinor, resp->versionPatch,
resp->pixelWidth, resp->pixelHeight, resp->gridColumns, resp->gridRows);
state.initialized = true;
frskyOSDClearScreen();
frskyOSDResetDrawingState();
}
return true;
}
case OSD_CMD_READ_FONT:
{
if (!state.recvOsdCharacter.chr) {
FRSKY_OSD_DEBUG("Got unexpected font character");
break;
}
if (size < sizeof(uint16_t) + FRSKY_OSD_CHAR_TOTAL_BYTES) {
FRSKY_OSD_TRACE("Received buffer too small for a character: %u bytes", size);
break;
}
const frskyOSDCharacter_t *chr = payload;
state.recvOsdCharacter.addr = chr->addr;
FRSKY_OSD_TRACE("Received character %u", chr->addr);
// Skip character address
memcpy(state.recvOsdCharacter.chr->data, &chr->data, MIN(sizeof(state.recvOsdCharacter.chr->data), (size_t)FRSKY_OSD_CHAR_TOTAL_BYTES));
return true;
}
case OSD_CMD_WRITE_FONT:
{
// We only wait for the confirmation, we're not interested in the data
return true;
}
default:
break;
}
return false;
}
static bool frskyOSDDispatchResponse(void)
{
const uint8_t *payload = state.recvBuffer.data;
int remaining = (int)state.recvBuffer.pos;
bool ok = false;
if (remaining > 0) {
// OSD sends commands one by one, so we don't need to handle
// a frame with multiple ones.
uint8_t cmd = *payload;
payload++;
remaining--;
if (frskyOSDHandleCommand(cmd, payload, remaining)) {
ok = true;
} else {
FRSKY_OSD_DEBUG("Discarding buffer due to unhandled command %u (%d bytes remaining)", cmd, remaining);
}
}
frskyOSDResetReceiveBuffer();
return ok;
}
static void frskyOSDClearReceiveBuffer(void)
{
frskyOSDUpdateReceiveBuffer();
if (frskyOSDIsResponseAvailable()) {
frskyOSDDispatchResponse();
} else if (state.recvBuffer.pos > 0) {
FRSKY_OSD_DEBUG("Discarding receive buffer with %u bytes", state.recvBuffer.pos);
frskyOSDResetReceiveBuffer();
}
}
static void frskyOSDSendAsyncCommand(uint8_t cmd, const void *data, size_t size)
{
FRSKY_OSD_TRACE("Send async cmd %u", cmd);
frskyOSDSendCommand(cmd, data, size);
}
static bool frskyOSDSendSyncCommand(uint8_t cmd, const void *data, size_t size, timeMs_t timeout)
{
FRSKY_OSD_TRACE("Send sync cmd %u", cmd);
frskyOSDClearReceiveBuffer();
frskyOSDSendCommand(cmd, data, size);
frskyOSDFlushSendBuffer();
timeMs_t end = millis() + timeout;
while (millis() < end) {
frskyOSDUpdateReceiveBuffer();
if (frskyOSDIsResponseAvailable() && frskyOSDDispatchResponse()) {
FRSKY_OSD_DEBUG("Got sync response");
return true;
}
}
FRSKY_OSD_DEBUG("Sync response failed");
return false;
}
static bool frskyOSDShouldRequestInfo(void)
{
return !frskyOSDIsReady() || millis() > state.nextInfoRequest;
}
static void frskyOSDRequestInfo(void)
{
timeMs_t now = millis();
if (state.info.nextRequest < now) {
uint8_t version = FRSKY_OSD_SUPPORTED_API_VERSION;
frskyOSDSendAsyncCommand(OSD_CMD_INFO, &version, sizeof(version));
frskyOSDFlushSendBuffer();
state.info.nextRequest = now + FRSKY_OSD_INFO_INTERVAL_MS;
}
}
static uint8_t frskyOSDEncodeAttr(textAttributes_t attr)
{
uint8_t frskyOSDAttr = 0;
if (TEXT_ATTRIBUTES_HAVE_INVERTED(attr)) {
frskyOSDAttr |= FRSKY_OSD_CHAR_ATTRIBUTE_COLOR_INVERSE;
}
if (TEXT_ATTRIBUTES_HAVE_SOLID_BG(attr)) {
frskyOSDAttr |= FRSKY_OSD_CHAR_ATTRIBUTE_SOLID_BACKGROUND;
}
return frskyOSDAttr;
}
bool frskyOSDInit(videoSystem_e videoSystem)
{
UNUSED(videoSystem);
FRSKY_OSD_TRACE("frskyOSDInit()");
// TODO: Use videoSystem to set the signal standard when
// no input is detected.
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_FRSKY_OSD);
if (portConfig) {
FRSKY_OSD_TRACE("FrSky OSD configured, trying to connect...");
portOptions_t portOptions = 0;
serialPort_t *port = openSerialPort(portConfig->identifier,
FUNCTION_FRSKY_OSD, NULL, NULL, FRSKY_OSD_BAUDRATE,
MODE_RXTX, portOptions);
if (port) {
frskyOSDStateReset(port);
frskyOSDRequestInfo();
return true;
}
}
return false;
}
bool frskyOSDIsReady(void)
{
return state.info.minor > 0 || state.info.major > 0;
}
void frskyOSDUpdate(void)
{
if (!state.port) {
return;
}
frskyOSDUpdateReceiveBuffer();
if (frskyOSDIsResponseAvailable()) {
frskyOSDDispatchResponse();
}
if (frskyOSDShouldRequestInfo()) {
frskyOSDRequestInfo();
}
}
void frskyOSDBeginTransaction(frskyOSDTransactionOptions_e opts)
{
if (opts & FRSKY_OSD_TRANSACTION_OPT_PROFILED) {
frskyOSDPoint_t p = { .x = 0, .y = 10};
frskyOSDSendAsyncCommand(OSD_CMD_TRANSACTION_BEGIN_PROFILED, &p, sizeof(p));
if (opts & FRSKY_OSD_TRANSACTION_OPT_RESET_DRAWING) {
frskyOSDResetDrawingState();
}
} else if (opts & FRSKY_OSD_TRANSACTION_OPT_RESET_DRAWING) {
frskyOSDSendAsyncCommand(OSD_CMD_TRANSACTION_BEGIN_RESET_DRAWING, NULL, 0);
} else {
frskyOSDSendAsyncCommand(OSD_CMD_TRANSACTION_BEGIN, NULL, 0);
}
}
void frskyOSDCommitTransaction(void)
{
// Check wether the only command in the queue is a transaction begin.
// In that, case, discard the send buffer since it will make generate
// an empty transaction.
if (state.sendBuffer.pos == 1) {
if (state.sendBuffer.data[0] == OSD_CMD_TRANSACTION_BEGIN ||
state.sendBuffer.data[0] == OSD_CMD_TRANSACTION_BEGIN_RESET_DRAWING) {
state.sendBuffer.pos = 0;
return;
}
}
frskyOSDSendAsyncCommand(OSD_CMD_TRANSACTION_COMMIT, NULL, 0);
frskyOSDFlushSendBuffer();
}
void frskyOSDFlushSendBuffer(void)
{
if (state.sendBuffer.pos > 0) {
frskyOSDProcessCommandU8(NULL, FRSKY_OSD_PREAMBLE_BYTE_0);
frskyOSDProcessCommandU8(NULL, FRSKY_OSD_PREAMBLE_BYTE_1);
uint8_t crc = 0;
uint8_t buffer[4];
int lengthSize = uvarintEncode(state.sendBuffer.pos, buffer, sizeof(buffer));
for (int ii = 0; ii < lengthSize; ii++) {
frskyOSDProcessCommandU8(&crc, buffer[ii]);
}
for (unsigned ii = 0; ii < state.sendBuffer.pos; ii++) {
frskyOSDProcessCommandU8(&crc, state.sendBuffer.data[ii]);
}
frskyOSDProcessCommandU8(NULL, crc);
state.sendBuffer.pos = 0;
}
}
bool frskyOSDReadFontCharacter(unsigned char_address, osdCharacter_t *chr)
{
if (!frskyOSDIsReady()) {
return false;
}
uint16_t addr = char_address;
state.recvOsdCharacter.addr = UINT16_MAX;
state.recvOsdCharacter.chr = chr;
// 500ms should be more than enough to receive ~70 bytes @ 115200 bps
bool ok = frskyOSDSendSyncCommand(OSD_CMD_READ_FONT, &addr, sizeof(addr), 500);
state.recvOsdCharacter.chr = NULL;
if (ok && state.recvOsdCharacter.addr == addr) {
return true;
}
return false;
}
bool frskyOSDWriteFontCharacter(unsigned char_address, const osdCharacter_t *chr)
{
if (!frskyOSDIsReady()) {
return false;
}
frskyOSDCharacter_t c;
STATIC_ASSERT(sizeof(*chr) == sizeof(c.data), invalid_character_size);
memcpy(&c.data, chr, sizeof(c.data));
c.addr = char_address;
FRSKY_OSD_TRACE("Writing font character %u", char_address);
frskyOSDSendSyncCommand(OSD_CMD_WRITE_FONT, &c, sizeof(c), 1000);
return true;
}
unsigned frskyOSDGetGridRows(void)
{
return state.info.grid.rows;
}
unsigned frskyOSDGetGridCols(void)
{
return state.info.grid.columns;
}
unsigned frskyOSDGetPixelWidth(void)
{
return state.info.viewport.width;
}
unsigned frskyOSDGetPixelHeight(void)
{
return state.info.viewport.height;
}
static void frskyOSDSendCharInGrid(unsigned x, unsigned y, uint16_t chr, textAttributes_t attr)
{
uint8_t payload[] = {
x,
y,
chr & 0xFF,
chr >> 8,
frskyOSDEncodeAttr(attr),
};
frskyOSDSendAsyncCommand(OSD_CMD_DRAW_GRID_CHR, payload, sizeof(payload));
}
static void frskyOSDSendAsyncBlobCommand(uint8_t cmd, const void *header, size_t headerSize, const void *blob, size_t blobSize)
{
uint8_t payload[128];
memcpy(payload, header, headerSize);
int uvarintSize = uvarintEncode(blobSize, &payload[headerSize], sizeof(payload) - headerSize);
memcpy(&payload[headerSize + uvarintSize], blob, blobSize);
frskyOSDSendAsyncCommand(cmd, payload, headerSize + uvarintSize + blobSize);
}
void frskyOSDDrawStringInGrid(unsigned x, unsigned y, const char *buff, textAttributes_t attr)
{
unsigned charsUpdated = 0;
const char *updatedCharAt = NULL;
uint16_t *entry = osdCharacterGridBufferGetEntryPtr(x, y);
const char *p;
unsigned xx;
for (p = buff, xx = x; *p && xx < state.info.grid.columns; p++, entry++, xx++) {
unsigned val = FRSKY_OSD_GRID_BUFFER_ENCODE(*p, attr);
if (*entry != val) {
if (++charsUpdated == 1) {
// First character that needs to be updated, save it
// in case we can issue a single update.
updatedCharAt = p;
}
*entry = val;
}
}
if (charsUpdated == 0) {
return;
}
if (charsUpdated == 1) {
frskyOSDSendCharInGrid(x + (updatedCharAt - buff), y, *updatedCharAt, attr);
return;
}
frskyOSDDrawGridStrHeaderCmd_t cmd;
cmd.gx = x;
cmd.gy = y;
cmd.opts = frskyOSDEncodeAttr(attr);
frskyOSDSendAsyncBlobCommand(OSD_CMD_DRAW_GRID_STR, &cmd, sizeof(cmd), buff, strlen(buff) + 1);
}
void frskyOSDDrawCharInGrid(unsigned x, unsigned y, uint16_t chr, textAttributes_t attr)
{
uint16_t *entry = osdCharacterGridBufferGetEntryPtr(x, y);
unsigned val = FRSKY_OSD_GRID_BUFFER_ENCODE(chr, attr);
if (*entry == val) {
return;
}
frskyOSDSendCharInGrid(x, y, chr, attr);
*entry = val;
}
bool frskyOSDReadCharInGrid(unsigned x, unsigned y, uint16_t *c, textAttributes_t *attr)
{
uint16_t val = *osdCharacterGridBufferGetEntryPtr(x, y);
// We use the lower 10 bits for characters
*c = val & FRSKY_OSD_GRID_BUFFER_CHAR_MASK;
*attr = val >> FRSKY_OSD_GRID_BUFFER_CHAR_BITS;
return true;
}
void frskyOSDClearScreen(void)
{
osdCharacterGridBufferClear();
frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_CLEAR_SCREEN, NULL, 0);
}
void frskyOSDSetStrokeColor(frskyOSDColor_e color)
{
uint8_t c = color;
frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_SET_STROKE_COLOR, &c, sizeof(c));
}
void frskyOSDSetFillColor(frskyOSDColor_e color)
{
uint8_t c = color;
frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_SET_FILL_COLOR, &c, sizeof(c));
}
void frskyOSDSetStrokeAndFillColor(frskyOSDColor_e color)
{
uint8_t c = color;
frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_SET_STROKE_AND_FILL_COLOR, &c, sizeof(c));
}
void frskyOSDSetColorInversion(bool inverted)
{
uint8_t c = inverted ? 1 : 0;
frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_SET_COLOR_INVERSION, &c, sizeof(c));
}
void frskyOSDSetPixel(int x, int y, frskyOSDColor_e color)
{
frskyOSDSetPixel_t sp = {.p = {.x = x, .y = y}, .color = color};
frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_SET_PIXEL, &sp, sizeof(sp));
}
void frskyOSDSetPixelToStrokeColor(int x, int y)
{
frskyOSDPoint_t p = { .x = x, .y = y};
frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_SET_PIXEL_TO_STROKE_COLOR, &p, sizeof(p));
}
void frskyOSDSetPixelToFillColor(int x, int y)
{
frskyOSDPoint_t p = { .x = x, .y = y};
frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_SET_PIXEL_TO_FILL_COLOR, &p, sizeof(p));
}
void frskyOSDSetStrokeWidth(unsigned width)
{
uint8_t w = width;
frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_SET_STROKE_WIDTH, &w, sizeof(w));
}
void frskyOSDSetLineOutlineType(frskyOSDLineOutlineType_e outlineType)
{
uint8_t type = outlineType;
frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_SET_LINE_OUTLINE_TYPE, &type, sizeof(type));
}
void frskyOSDSetLineOutlineColor(frskyOSDColor_e outlineColor)
{
uint8_t color = outlineColor;
frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_SET_LINE_OUTLINE_COLOR, &color, sizeof(color));
}
void frskyOSDClipToRect(int x, int y, int w, int h)
{
frskyOSDRect_t r = { .origin = { .x = x, .y = y}, .size = {.w = w, .h = h}};
frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_CLIP_TO_RECT, &r, sizeof(r));
}
void frskyOSDClearRect(int x, int y, int w, int h)
{
frskyOSDRect_t r = { .origin = { .x = x, .y = y}, .size = {.w = w, .h = h}};
frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_CLEAR_RECT, &r, sizeof(r));
}
void frskyOSDResetDrawingState(void)
{
frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_RESET, NULL, 0);
}
void frskyOSDDrawCharacter(int x, int y, uint16_t chr, uint8_t opts)
{
frskyOSDDrawCharacterCmd_t dc = { .p = {.x = x, .y = y}, .chr = chr, .opts = opts};
frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_DRAW_CHAR, &dc, sizeof(dc));
}
void frskyOSDDrawCharacterMask(int x, int y, uint16_t chr, frskyOSDColor_e color, uint8_t opts)
{
frskyOSDDrawCharacterMaskCmd_t dc = { .dc = { .p = {.x = x, .y = y}, .chr = chr, .opts = opts}, .maskColor = color};
frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_DRAW_CHAR_MASK, &dc, sizeof(dc));
}
void frskyOSDDrawString(int x, int y, const char *s, uint8_t opts)
{
frskyOSDDrawStrCommandHeaderCmd_t cmd;
cmd.p.x = x;
cmd.p.y = y;
cmd.opts = opts;
frskyOSDSendAsyncBlobCommand(OSD_CMD_DRAWING_DRAW_STRING, &cmd, sizeof(cmd), s, strlen(s) + 1);
}
void frskyOSDDrawStringMask(int x, int y, const char *s, frskyOSDColor_e color, uint8_t opts)
{
frskyOSDDrawStrMaskCommandHeaderCmd_t cmd;
cmd.p.x = x;
cmd.p.y = y;
cmd.opts = opts;
cmd.maskColor = color;
frskyOSDSendAsyncBlobCommand(OSD_CMD_DRAWING_DRAW_STRING_MASK, &cmd, sizeof(cmd), s, strlen(s) + 1);
}
void frskyOSDMoveToPoint(int x, int y)
{
frskyOSDPoint_t p = { .x = x, .y = y};
frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_MOVE_TO_POINT, &p, sizeof(p));
}
void frskyOSDStrokeLineToPoint(int x, int y)
{
frskyOSDPoint_t p = { .x = x, .y = y};
frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_STROKE_LINE_TO_POINT, &p, sizeof(p));
}
void frskyOSDStrokeTriangle(int x1, int y1, int x2, int y2, int x3, int y3)
{
frskyOSDTriangle_t t = {.p1 = {.x = x1, .y = y1}, .p2 = {.x = x2, .y = y2}, .p3 = { .x = x3, .y = y3}};
frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_STROKE_TRIANGLE, &t, sizeof(t));
}
void frskyOSDFillTriangle(int x1, int y1, int x2, int y2, int x3, int y3)
{
frskyOSDTriangle_t t = {.p1 = {.x = x1, .y = y1}, .p2 = {.x = x2, .y = y2}, .p3 = { .x = x3, .y = y3}};
frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_FILL_TRIANGLE, &t, sizeof(t));
}
void frskyOSDFillStrokeTriangle(int x1, int y1, int x2, int y2, int x3, int y3)
{
frskyOSDTriangle_t t = {.p1 = {.x = x1, .y = y1}, .p2 = {.x = x2, .y = y2}, .p3 = { .x = x3, .y = y3}};
frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_FILL_STROKE_TRIANGLE, &t, sizeof(t));
}
void frskyOSDStrokeRect(int x, int y, int w, int h)
{
frskyOSDRect_t r = { .origin = { .x = x, .y = y}, .size = {.w = w, .h = h}};
frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_STROKE_RECT, &r, sizeof(r));
}
void frskyOSDFillRect(int x, int y, int w, int h)
{
frskyOSDRect_t r = { .origin = { .x = x, .y = y}, .size = {.w = w, .h = h}};
frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_FILL_RECT, &r, sizeof(r));
}
void frskyOSDFillStrokeRect(int x, int y, int w, int h)
{
frskyOSDRect_t r = { .origin = { .x = x, .y = y}, .size = {.w = w, .h = h}};
frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_FILL_STROKE_RECT, &r, sizeof(r));
}
void frskyOSDStrokeEllipseInRect(int x, int y, int w, int h)
{
frskyOSDRect_t r = { .origin = { .x = x, .y = y}, .size = {.w = w, .h = h}};
frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_STROKE_ELLIPSE_IN_RECT, &r, sizeof(r));
}
void frskyOSDFillEllipseInRect(int x, int y, int w, int h)
{
frskyOSDRect_t r = { .origin = { .x = x, .y = y}, .size = {.w = w, .h = h}};
frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_FILL_ELLIPSE_IN_RECT, &r, sizeof(r));
}
void frskyOSDFillStrokeEllipseInRect(int x, int y, int w, int h)
{
frskyOSDRect_t r = { .origin = { .x = x, .y = y}, .size = {.w = w, .h = h}};
frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_FILL_STROKE_ELLIPSE_IN_RECT, &r, sizeof(r));
}
void frskyOSDCtmReset(void)
{
frskyOSDSendAsyncCommand(OSD_CMD_CTM_RESET, NULL, 0);
}
void frskyOSDCtmSet(float m11, float m12, float m21, float m22, float m31, float m32)
{
float values[] = {
m11, m12,
m21, m22,
m31, m32,
};
frskyOSDSendAsyncCommand(OSD_CMD_CTM_SET, values, sizeof(values));
}
void frskyOSDCtmTranslate(float tx, float ty)
{
float values[] = {
tx,
ty,
};
frskyOSDSendAsyncCommand(OSD_CMD_CTM_TRANSLATE, values, sizeof(values));
}
void frskyOSDCtmScale(float sx, float sy)
{
float values[] = {
sx,
sy,
};
frskyOSDSendAsyncCommand(OSD_CMD_CTM_SCALE, values, sizeof(values));
}
void frskyOSDCtmRotate(float r)
{
frskyOSDSendAsyncCommand(OSD_CMD_CTM_ROTATE, &r, sizeof(r));
}
void frskyOSDContextPush(void)
{
frskyOSDSendAsyncCommand(OSD_CMD_CONTEXT_PUSH, NULL, 0);
}
void frskyOSDContextPop(void)
{
frskyOSDSendAsyncCommand(OSD_CMD_CONTEXT_POP, NULL, 0);
}
#endif

86
src/main/io/frsky_osd.h Normal file
View file

@ -0,0 +1,86 @@
#pragma once
#include <stdbool.h>
#include <stdint.h>
#include "drivers/display.h"
#include "drivers/osd.h"
typedef enum {
FRSKY_OSD_TRANSACTION_OPT_PROFILED = 1 << 0,
FRSKY_OSD_TRANSACTION_OPT_RESET_DRAWING = 1 << 1,
} frskyOSDTransactionOptions_e;
typedef enum {
FRSKY_OSD_COLOR_BLACK = 0,
FRSKY_OSD_COLOR_TRANSPARENT = 1,
FRSKY_OSD_COLOR_WHITE = 2,
FRSKY_OSD_COLOR_GRAY = 3,
} frskyOSDColor_e;
typedef enum {
FRSKY_OSD_OUTLINE_TYPE_NONE = 0,
FRSKY_OSD_OUTLINE_TYPE_TOP = 1 << 0,
FRSKY_OSD_OUTLINE_TYPE_RIGHT = 1 << 1,
FRSKY_OSD_OUTLINE_TYPE_BOTTOM = 1 << 2,
FRSKY_OSD_OUTLINE_TYPE_LEFT = 1 << 3,
} frskyOSDLineOutlineType_e;
bool frskyOSDInit(videoSystem_e videoSystem);
bool frskyOSDIsReady(void);
void frskyOSDUpdate(void);
void frskyOSDBeginTransaction(frskyOSDTransactionOptions_e opts);
void frskyOSDCommitTransaction(void);
void frskyOSDFlushSendBuffer(void);
bool frskyOSDReadFontCharacter(unsigned char_address, osdCharacter_t *chr);
bool frskyOSDWriteFontCharacter(unsigned char_address, const osdCharacter_t *chr);
unsigned frskyOSDGetGridRows(void);
unsigned frskyOSDGetGridCols(void);
unsigned frskyOSDGetPixelWidth(void);
unsigned frskyOSDGetPixelHeight(void);
void frskyOSDDrawStringInGrid(unsigned x, unsigned y, const char *buff, textAttributes_t attr);
void frskyOSDDrawCharInGrid(unsigned x, unsigned y, uint16_t chr, textAttributes_t attr);
bool frskyOSDReadCharInGrid(unsigned x, unsigned y, uint16_t *c, textAttributes_t *attr);
void frskyOSDClearScreen(void);
void frskyOSDSetStrokeColor(frskyOSDColor_e color);
void frskyOSDSetFillColor(frskyOSDColor_e color);
void frskyOSDSetStrokeAndFillColor(frskyOSDColor_e color);
void frskyOSDSetColorInversion(bool inverted);
void frskyOSDSetPixel(int x, int y, frskyOSDColor_e color);
void frskyOSDSetPixelToStrokeColor(int x, int y);
void frskyOSDSetPixelToFillColor(int x, int y);
void frskyOSDSetStrokeWidth(unsigned width);
void frskyOSDSetLineOutlineType(frskyOSDLineOutlineType_e outlineType);
void frskyOSDSetLineOutlineColor(frskyOSDColor_e outlineColor);
void frskyOSDClipToRect(int x, int y, int w, int h);
void frskyOSDClearRect(int x, int y, int w, int h);
void frskyOSDResetDrawingState(void);
void frskyOSDDrawCharacter(int x, int y, uint16_t chr, uint8_t opts);
void frskyOSDDrawCharacterMask(int x, int y, uint16_t chr, frskyOSDColor_e color, uint8_t opts);
void frskyOSDDrawString(int x, int y, const char *s, uint8_t opts);
void frskyOSDDrawStringMask(int x, int y, const char *s, frskyOSDColor_e color, uint8_t opts);
void frskyOSDMoveToPoint(int x, int y);
void frskyOSDStrokeLineToPoint(int x, int y);
void frskyOSDStrokeTriangle(int x1, int y1, int x2, int y2, int x3, int y3);
void frskyOSDFillTriangle(int x1, int y1, int x2, int y2, int x3, int y3);
void frskyOSDFillStrokeTriangle(int x1, int y1, int x2, int y2, int x3, int y3);
void frskyOSDStrokeRect(int x, int y, int w, int h);
void frskyOSDFillRect(int x, int y, int w, int h);
void frskyOSDFillStrokeRect(int x, int y, int w, int h);
void frskyOSDStrokeEllipseInRect(int x, int y, int w, int h);
void frskyOSDFillEllipseInRect(int x, int y, int w, int h);
void frskyOSDFillStrokeEllipseInRect(int x, int y, int w, int h);
void frskyOSDCtmReset(void);
void frskyOSDCtmSet(float m11, float m12, float m21, float m22, float m31, float m32);
void frskyOSDCtmTranslate(float tx, float ty);
void frskyOSDCtmScale(float sx, float sy);
void frskyOSDCtmRotate(float r);
void frskyOSDContextPush(void);
void frskyOSDContextPop(void);

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

@ -43,6 +43,7 @@
#include "common/axis.h" #include "common/axis.h"
#include "common/constants.h" #include "common/constants.h"
#include "common/filter.h" #include "common/filter.h"
#include "common/log.h"
#include "common/olc.h" #include "common/olc.h"
#include "common/printf.h" #include "common/printf.h"
#include "common/string_light.h" #include "common/string_light.h"
@ -55,6 +56,8 @@
#include "config/parameter_group_ids.h" #include "config/parameter_group_ids.h"
#include "drivers/display.h" #include "drivers/display.h"
#include "drivers/display_canvas.h"
#include "drivers/display_font_metadata.h"
#include "drivers/osd_symbols.h" #include "drivers/osd_symbols.h"
#include "drivers/time.h" #include "drivers/time.h"
#include "drivers/vtx_common.h" #include "drivers/vtx_common.h"
@ -62,6 +65,7 @@
#include "io/flashfs.h" #include "io/flashfs.h"
#include "io/gps.h" #include "io/gps.h"
#include "io/osd.h" #include "io/osd.h"
#include "io/osd_common.h"
#include "io/osd_hud.h" #include "io/osd_hud.h"
#include "io/vtx.h" #include "io/vtx.h"
#include "io/vtx_string.h" #include "io/vtx_string.h"
@ -96,6 +100,7 @@
#include "sensors/sensors.h" #include "sensors/sensors.h"
#include "sensors/pitotmeter.h" #include "sensors/pitotmeter.h"
#include "sensors/temperature.h" #include "sensors/temperature.h"
#include "sensors/esc_sensor.h"
#ifdef USE_HARDWARE_REVISION_DETECTION #ifdef USE_HARDWARE_REVISION_DETECTION
#include "hardware_revision.h" #include "hardware_revision.h"
@ -180,13 +185,15 @@ typedef struct osdMapData_s {
static osdMapData_t osdMapData; static osdMapData_t osdMapData;
static displayPort_t *osdDisplayPort; static displayPort_t *osdDisplayPort;
static bool osdDisplayIsReady = false;
#if defined(USE_CANVAS)
static displayCanvas_t osdCanvas;
static bool osdDisplayHasCanvas;
#else
#define osdDisplayHasCanvas false
#endif
#define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees) #define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees)
#define AH_HEIGHT 9
#define AH_WIDTH 11
#define AH_PREV_SIZE (AH_WIDTH > AH_HEIGHT ? AH_WIDTH : AH_HEIGHT)
#define AH_H_SYM_COUNT 9
#define AH_V_SYM_COUNT 6
#define AH_SIDEBAR_WIDTH_POS 7 #define AH_SIDEBAR_WIDTH_POS 7
#define AH_SIDEBAR_HEIGHT_POS 3 #define AH_SIDEBAR_HEIGHT_POS 3
@ -879,6 +886,26 @@ static void osdFormatThrottlePosition(char *buff, bool autoThr, textAttributes_t
tfp_sprintf(buff + 2, "%3d", (constrain(thr, PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN)); tfp_sprintf(buff + 2, "%3d", (constrain(thr, PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN));
} }
#if defined(USE_ESC_SENSOR)
static void osdFormatRpm(char *buff, uint32_t rpm)
{
buff[0] = SYM_RPM;
if (rpm) {
if (rpm >= 1000) {
osdFormatCentiNumber(buff + 1, rpm / 10, 0, 1, 1, 2);
buff[3] = 'K';
buff[4] = '\0';
}
else {
tfp_sprintf(buff + 1, "%3lu", rpm);
}
}
else {
strcpy(buff + 1, "---");
}
}
#endif
int32_t osdGetAltitude(void) int32_t osdGetAltitude(void)
{ {
#if defined(USE_NAV) #if defined(USE_NAV)
@ -992,7 +1019,7 @@ int16_t osdGetHeading(void)
} }
// Returns a heading angle in degrees normalized to [0, 360). // Returns a heading angle in degrees normalized to [0, 360).
static int osdGetHeadingAngle(int angle) int osdGetHeadingAngle(int angle)
{ {
while (angle < 0) { while (angle < 0) {
angle += 360; angle += 360;
@ -1375,29 +1402,22 @@ static bool osdDrawSingleElement(uint8_t item)
{ {
if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) { if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) {
if (GPS_distanceToHome < (navConfig()->general.min_rth_distance / 100) ) { if (GPS_distanceToHome < (navConfig()->general.min_rth_distance / 100) ) {
buff[0] = SYM_HOME_NEAR; displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_HOME_NEAR);
} }
else else
{ {
// There are 16 orientations for the home direction arrow.
// so we use 22.5deg per image, where the 1st image is used
// for [349, 11], the 2nd for [12, 33], etc...
int homeDirection = GPS_directionToHome - DECIDEGREES_TO_DEGREES(osdGetHeading()); int homeDirection = GPS_directionToHome - DECIDEGREES_TO_DEGREES(osdGetHeading());
// Add 11 to the angle, so first character maps to [349, 11] osdDrawDirArrow(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), homeDirection, true);
int homeArrowDir = osdGetHeadingAngle(homeDirection + 11);
unsigned arrowOffset = homeArrowDir * 2 / 45;
buff[0] = SYM_ARROW_UP + arrowOffset;
} }
} else { } else {
// No home or no fix or unknown heading, blink. // No home or no fix or unknown heading, blink.
// If we're unarmed, show the arrow pointing up so users can see the arrow // If we're unarmed, show the arrow pointing up so users can see the arrow
// while configuring the OSD. If we're armed, show a '-' indicating that // while configuring the OSD. If we're armed, show a '-' indicating that
// we don't know the direction to home. // we don't know the direction to home.
buff[0] = ARMING_FLAG(ARMED) ? '-' : SYM_ARROW_UP;
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
displayWriteCharWithAttr(osdDisplayPort, elemPosX, elemPosY, ARMING_FLAG(ARMED) ? '-' : SYM_ARROW_UP, elemAttr);
} }
buff[1] = '\0'; return true;
break;
} }
case OSD_HOME_HEADING_ERROR: case OSD_HOME_HEADING_ERROR:
@ -1776,16 +1796,9 @@ static bool osdDrawSingleElement(uint8_t item)
case OSD_ARTIFICIAL_HORIZON: case OSD_ARTIFICIAL_HORIZON:
{ {
osdCrosshairPosition(&elemPosX, &elemPosY);
// Store the positions we draw over to erase only these at the next iteration
static int8_t previous_written[AH_PREV_SIZE];
static int8_t previous_orient = -1;
float pitch_rad_to_char = (float)(AH_HEIGHT / 2 + 0.5) / DEGREES_TO_RADIANS(osdConfig()->ahi_max_pitch);
float rollAngle; float rollAngle;
float pitchAngle; float pitchAngle;
#ifdef USE_SECONDARY_IMU #ifdef USE_SECONDARY_IMU
if (secondaryImuConfig()->useForOsdAHI) { if (secondaryImuConfig()->useForOsdAHI) {
rollAngle = DECIDEGREES_TO_RADIANS(secondaryImuState.eulerAngles.values.roll); rollAngle = DECIDEGREES_TO_RADIANS(secondaryImuState.eulerAngles.values.roll);
@ -1802,57 +1815,8 @@ static bool osdDrawSingleElement(uint8_t item)
if (osdConfig()->ahi_reverse_roll) { if (osdConfig()->ahi_reverse_roll) {
rollAngle = -rollAngle; rollAngle = -rollAngle;
} }
osdDrawArtificialHorizon(osdDisplayPort, osdGetDisplayPortCanvas(),
float ky = sin_approx(rollAngle); OSD_DRAW_POINT_GRID(elemPosX, elemPosY), rollAngle, pitchAngle);
float kx = cos_approx(rollAngle);
if (previous_orient != -1) {
for (int i = 0; i < AH_PREV_SIZE; ++i) {
if (previous_written[i] > -1) {
int8_t dx = (previous_orient ? previous_written[i] : i) - AH_PREV_SIZE / 2;
int8_t dy = (previous_orient ? i : previous_written[i]) - AH_PREV_SIZE / 2;
displayWriteChar(osdDisplayPort, elemPosX + dx, elemPosY - dy, SYM_BLANK);
previous_written[i] = -1;
}
}
}
if (fabsf(ky) < fabsf(kx)) {
previous_orient = 0;
for (int8_t dx = -AH_WIDTH / 2; dx <= AH_WIDTH / 2; dx++) {
float fy = dx * (ky / kx) + pitchAngle * pitch_rad_to_char + 0.49f;
int8_t dy = floorf(fy);
const uint8_t chX = elemPosX + dx, chY = elemPosY - dy;
uint16_t c;
if ((dy >= -AH_HEIGHT / 2) && (dy <= AH_HEIGHT / 2) && displayReadCharWithAttr(osdDisplayPort, chX, chY, &c, NULL) && (c == SYM_BLANK)) {
c = SYM_AH_H_START + ((AH_H_SYM_COUNT - 1) - (uint8_t)((fy - dy) * AH_H_SYM_COUNT));
displayWriteChar(osdDisplayPort, elemPosX + dx, elemPosY - dy, c);
previous_written[dx + AH_PREV_SIZE / 2] = dy + AH_PREV_SIZE / 2;
}
}
} else {
previous_orient = 1;
for (int8_t dy = -AH_HEIGHT / 2; dy <= AH_HEIGHT / 2; dy++) {
const float fx = (dy - pitchAngle * pitch_rad_to_char) * (kx / ky) + 0.5f;
const int8_t dx = floorf(fx);
const uint8_t chX = elemPosX + dx, chY = elemPosY - dy;
uint16_t c;
if ((dx >= -AH_WIDTH / 2) && (dx <= AH_WIDTH / 2) && displayReadCharWithAttr(osdDisplayPort, chX, chY, &c, NULL) && (c == SYM_BLANK)) {
c = SYM_AH_V_START + (fx - dx) * AH_V_SYM_COUNT;
displayWriteChar(osdDisplayPort, chX, chY, c);
previous_written[dy + AH_PREV_SIZE / 2] = dx + AH_PREV_SIZE / 2;
}
}
}
osdDrawSingleElement(OSD_HORIZON_SIDEBARS); osdDrawSingleElement(OSD_HORIZON_SIDEBARS);
osdDrawSingleElement(OSD_CROSSHAIRS); osdDrawSingleElement(OSD_CROSSHAIRS);
@ -1904,39 +1868,8 @@ static bool osdDrawSingleElement(uint8_t item)
#if defined(USE_BARO) || defined(USE_GPS) #if defined(USE_BARO) || defined(USE_GPS)
case OSD_VARIO: case OSD_VARIO:
{ {
int16_t v = getEstimatedActualVelocity(Z) / 50; //50cm = 1 arrow float zvel = getEstimatedActualVelocity(Z);
uint8_t vchars[] = {SYM_BLANK, SYM_BLANK, SYM_BLANK, SYM_BLANK, SYM_BLANK}; osdDrawVario(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), zvel);
if (v >= 6)
vchars[0] = SYM_VARIO_UP_2A;
else if (v == 5)
vchars[0] = SYM_VARIO_UP_1A;
if (v >=4)
vchars[1] = SYM_VARIO_UP_2A;
else if (v == 3)
vchars[1] = SYM_VARIO_UP_1A;
if (v >=2)
vchars[2] = SYM_VARIO_UP_2A;
else if (v == 1)
vchars[2] = SYM_VARIO_UP_1A;
if (v <= -2)
vchars[2] = SYM_VARIO_DOWN_2A;
else if (v == -1)
vchars[2] = SYM_VARIO_DOWN_1A;
if (v <= -4)
vchars[3] = SYM_VARIO_DOWN_2A;
else if (v == -3)
vchars[3] = SYM_VARIO_DOWN_1A;
if (v <= -6)
vchars[4] = SYM_VARIO_DOWN_2A;
else if (v == -5)
vchars[4] = SYM_VARIO_DOWN_1A;
displayWriteChar(osdDisplayPort, elemPosX, elemPosY, vchars[0]);
displayWriteChar(osdDisplayPort, elemPosX, elemPosY+1, vchars[1]);
displayWriteChar(osdDisplayPort, elemPosX, elemPosY+2, vchars[2]);
displayWriteChar(osdDisplayPort, elemPosX, elemPosY+3, vchars[3]);
displayWriteChar(osdDisplayPort, elemPosX, elemPosY+4, vchars[4]);
return true; return true;
} }
@ -2285,49 +2218,14 @@ static bool osdDrawSingleElement(uint8_t item)
case OSD_HEADING_GRAPH: case OSD_HEADING_GRAPH:
{ {
static const uint8_t graph[] = {
SYM_HEADING_LINE,
SYM_HEADING_E,
SYM_HEADING_LINE,
SYM_HEADING_DIVIDED_LINE,
SYM_HEADING_LINE,
SYM_HEADING_S,
SYM_HEADING_LINE,
SYM_HEADING_DIVIDED_LINE,
SYM_HEADING_LINE,
SYM_HEADING_W,
SYM_HEADING_LINE,
SYM_HEADING_DIVIDED_LINE,
SYM_HEADING_LINE,
SYM_HEADING_N,
SYM_HEADING_LINE,
SYM_HEADING_DIVIDED_LINE,
SYM_HEADING_LINE,
SYM_HEADING_E,
SYM_HEADING_LINE,
SYM_HEADING_DIVIDED_LINE,
SYM_HEADING_LINE,
SYM_HEADING_S,
SYM_HEADING_LINE,
SYM_HEADING_DIVIDED_LINE,
SYM_HEADING_LINE,
SYM_HEADING_W,
SYM_HEADING_LINE,
};
if (osdIsHeadingValid()) { if (osdIsHeadingValid()) {
int16_t h = DECIDEGREES_TO_DEGREES(osdGetHeading()); osdDrawHeadingGraph(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), osdGetHeading());
if (h >= 180) { return true;
h -= 360;
}
int hh = h * 4;
hh = hh + 720 + 45;
hh = hh / 90;
memcpy_fn(buff, graph + hh + 1, 9);
} else { } else {
buff[0] = buff[2] = buff[4] = buff[6] = buff[8] = SYM_HEADING_LINE; buff[0] = buff[2] = buff[4] = buff[6] = buff[8] = SYM_HEADING_LINE;
buff[1] = buff[3] = buff[5] = buff[7] = SYM_HEADING_DIVIDED_LINE; buff[1] = buff[3] = buff[5] = buff[7] = SYM_HEADING_DIVIDED_LINE;
buff[OSD_HEADING_GRAPH_WIDTH] = '\0';
} }
buff[9] = '\0';
break; break;
} }
@ -2583,6 +2481,20 @@ static bool osdDrawSingleElement(uint8_t item)
} }
#endif #endif
#if defined(USE_ESC_SENSOR)
case OSD_ESC_RPM:
{
escSensorData_t * escSensor = escSensorGetData();
if (escSensor && escSensor->dataAge <= ESC_DATA_MAX_AGE) {
osdFormatRpm(buff, escSensor->rpm);
}
else {
osdFormatRpm(buff, 0);
}
break;
}
#endif
default: default:
return false; return false;
} }
@ -2651,6 +2563,12 @@ static uint8_t osdIncElementIndex(uint8_t elementIndex)
} }
} }
if (!STATE(ESC_SENSOR_ENABLED)) {
if (elementIndex == OSD_ESC_RPM) {
elementIndex++;
}
}
if (elementIndex == OSD_ITEM_COUNT) { if (elementIndex == OSD_ITEM_COUNT) {
elementIndex = 0; elementIndex = 0;
} }
@ -2788,6 +2706,10 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
osdConfig->item_pos[0][OSD_VTX_POWER] = OSD_POS(3, 5); osdConfig->item_pos[0][OSD_VTX_POWER] = OSD_POS(3, 5);
#if defined(USE_ESC_SENSOR)
osdConfig->item_pos[0][OSD_ESC_RPM] = OSD_POS(1, 2);
#endif
#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
osdConfig->item_pos[0][OSD_RC_SOURCE] = OSD_POS(3, 4); osdConfig->item_pos[0][OSD_RC_SOURCE] = OSD_POS(3, 4);
#endif #endif
@ -2858,26 +2780,30 @@ static void osdSetNextRefreshIn(uint32_t timeMs) {
refreshWaitForResumeCmdRelease = true; refreshWaitForResumeCmdRelease = true;
} }
void osdInit(displayPort_t *osdDisplayPortToUse) static void osdCompleteAsyncInitialization(void)
{ {
if (!osdDisplayPortToUse) if (!displayIsReady(osdDisplayPort)) {
// Update the display.
// XXX: Rename displayDrawScreen() and associated functions
// to displayUpdate()
displayDrawScreen(osdDisplayPort);
return; return;
}
BUILD_BUG_ON(OSD_POS_MAX != OSD_POS(31,31)); osdDisplayIsReady = true;
osdDisplayPort = osdDisplayPortToUse; #if defined(USE_CANVAS)
osdDisplayHasCanvas = displayGetCanvas(&osdCanvas, osdDisplayPort);
#ifdef USE_CMS
cmsDisplayPortRegister(osdDisplayPort);
#endif #endif
armState = ARMING_FLAG(ARMED); displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING);
displayClearScreen(osdDisplayPort); displayClearScreen(osdDisplayPort);
uint8_t y = 1; uint8_t y = 1;
displayFontMetadata_t metadata; displayFontMetadata_t metadata;
bool fontHasMetadata = displayGetFontMetadata(&metadata, osdDisplayPort); bool fontHasMetadata = displayGetFontMetadata(&metadata, osdDisplayPort);
LOG_D(OSD, "Font metadata version %s: %u (%u chars)",
fontHasMetadata ? "Y" : "N", metadata.version, metadata.charCount);
if (fontHasMetadata && metadata.charCount > 256) { if (fontHasMetadata && metadata.charCount > 256) {
hasExtendedFont = true; hasExtendedFont = true;
@ -2949,10 +2875,28 @@ void osdInit(displayPort_t *osdDisplayPortToUse)
} }
#endif #endif
displayCommitTransaction(osdDisplayPort);
displayResync(osdDisplayPort); displayResync(osdDisplayPort);
osdSetNextRefreshIn(SPLASH_SCREEN_DISPLAY_TIME); osdSetNextRefreshIn(SPLASH_SCREEN_DISPLAY_TIME);
} }
void osdInit(displayPort_t *osdDisplayPortToUse)
{
if (!osdDisplayPortToUse)
return;
BUILD_BUG_ON(OSD_POS_MAX != OSD_POS(31,31));
osdDisplayPort = osdDisplayPortToUse;
#ifdef USE_CMS
cmsDisplayPortRegister(osdDisplayPort);
#endif
armState = ARMING_FLAG(ARMED);
osdCompleteAsyncInitialization();
}
static void osdResetStats(void) static void osdResetStats(void)
{ {
stats.max_current = 0; stats.max_current = 0;
@ -3214,12 +3158,14 @@ static void osdRefresh(timeUs_t currentTimeUs)
#ifdef USE_CMS #ifdef USE_CMS
if (!displayIsGrabbed(osdDisplayPort)) { if (!displayIsGrabbed(osdDisplayPort)) {
displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING);
if (fullRedraw) { if (fullRedraw) {
displayClearScreen(osdDisplayPort); displayClearScreen(osdDisplayPort);
fullRedraw = false; fullRedraw = false;
} }
osdDrawNextElement(); osdDrawNextElement();
displayHeartbeat(osdDisplayPort); displayHeartbeat(osdDisplayPort);
displayCommitTransaction(osdDisplayPort);
#ifdef OSD_CALLS_CMS #ifdef OSD_CALLS_CMS
} else { } else {
cmsUpdate(currentTimeUs); cmsUpdate(currentTimeUs);
@ -3240,6 +3186,11 @@ void osdUpdate(timeUs_t currentTimeUs)
return; return;
} }
if (!osdDisplayIsReady) {
osdCompleteAsyncInitialization();
return;
}
#if defined(OSD_ALTERNATE_LAYOUT_COUNT) && OSD_ALTERNATE_LAYOUT_COUNT > 0 #if defined(OSD_ALTERNATE_LAYOUT_COUNT) && OSD_ALTERNATE_LAYOUT_COUNT > 0
// Check if the layout has changed. Higher numbered // Check if the layout has changed. Higher numbered
// boxes take priority. // boxes take priority.
@ -3337,4 +3288,14 @@ displayPort_t *osdGetDisplayPort(void)
return osdDisplayPort; return osdDisplayPort;
} }
displayCanvas_t *osdGetDisplayPortCanvas(void)
{
#if defined(USE_CANVAS)
if (osdDisplayHasCanvas) {
return &osdCanvas;
}
#endif
return NULL;
}
#endif // OSD #endif // OSD

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

@ -150,6 +150,7 @@ typedef enum {
OSD_GFORCE_Z, OSD_GFORCE_Z,
OSD_RC_SOURCE, OSD_RC_SOURCE,
OSD_VTX_POWER, OSD_VTX_POWER,
OSD_ESC_RPM,
OSD_ITEM_COUNT // MUST BE LAST OSD_ITEM_COUNT // MUST BE LAST
} osd_items_e; } osd_items_e;
@ -248,6 +249,7 @@ typedef struct osdConfig_s {
PG_DECLARE(osdConfig_t, osdConfig); PG_DECLARE(osdConfig_t, osdConfig);
typedef struct displayPort_s displayPort_t; typedef struct displayPort_s displayPort_t;
typedef struct displayCanvas_s displayCanvas_t;
void osdInit(displayPort_t *osdDisplayPort); void osdInit(displayPort_t *osdDisplayPort);
void osdUpdate(timeUs_t currentTimeUs); void osdUpdate(timeUs_t currentTimeUs);
@ -264,10 +266,14 @@ int osdGetActiveLayout(bool *overridden);
bool osdItemIsFixed(osd_items_e item); bool osdItemIsFixed(osd_items_e item);
displayPort_t *osdGetDisplayPort(void); displayPort_t *osdGetDisplayPort(void);
displayCanvas_t *osdGetDisplayPortCanvas(void);
int16_t osdGetHeading(void); int16_t osdGetHeading(void);
int32_t osdGetAltitude(void); int32_t osdGetAltitude(void);
void osdCrosshairPosition(uint8_t *x, uint8_t *y); void osdCrosshairPosition(uint8_t *x, uint8_t *y);
bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int maxDecimals, int maxScaledDecimals, int length); bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int maxDecimals, int maxScaledDecimals, int length);
void osdFormatAltitudeSymbol(char *buff, int32_t alt); void osdFormatAltitudeSymbol(char *buff, int32_t alt);
void osdFormatVelocityStr(char* buff, int32_t vel, bool _3D); void osdFormatVelocityStr(char* buff, int32_t vel, bool _3D);
// Returns a heading angle in degrees normalized to [0, 360).
int osdGetHeadingAngle(int angle);

349
src/main/io/osd_canvas.c Normal file
View file

@ -0,0 +1,349 @@
/*
* This file is part of INAV.
*
* 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/.
*
* @author Alberto Garcia Hierro <alberto@garciahierro.com>
*/
#include <math.h>
#include "platform.h"
#if defined(USE_CANVAS)
#define AHI_MAX_DRAW_INTERVAL_MS 1000
#include "common/log.h"
#include "common/maths.h"
#include "common/printf.h"
#include "common/utils.h"
#include "drivers/display.h"
#include "drivers/display_canvas.h"
#include "drivers/osd.h"
#include "drivers/osd_symbols.h"
#include "drivers/time.h"
#include "io/osd_common.h"
void osdCanvasDrawVarioShape(displayCanvas_t *canvas, unsigned ex, unsigned ey, float zvel, bool erase)
{
char sym;
float ratio = zvel / (OSD_VARIO_CM_S_PER_ARROW * 2);
int height = -ratio * canvas->gridElementHeight;
int step;
int x = ex * canvas->gridElementWidth;
int start;
int dstart;
if (zvel > 0) {
sym = SYM_VARIO_UP_2A;
start = ceilf(OSD_VARIO_HEIGHT_ROWS / 2.0f);
dstart = start - 1;
step = -canvas->gridElementHeight;
} else {
sym = SYM_VARIO_DOWN_2A;
start = floorf(OSD_VARIO_HEIGHT_ROWS / 2.0f);
dstart = start;
step = canvas->gridElementHeight;
}
int y = (start + ey) * canvas->gridElementHeight;
displayCanvasClipToRect(canvas, x, y, canvas->gridElementWidth, height);
int dy = (dstart + ey) * canvas->gridElementHeight;
for (int ii = 0, yy = dy; ii < (OSD_VARIO_HEIGHT_ROWS + 1) / 2; ii++, yy += step) {
if (erase) {
displayCanvasDrawCharacterMask(canvas, x, yy, sym, DISPLAY_CANVAS_COLOR_TRANSPARENT, 0);
} else {
displayCanvasDrawCharacter(canvas, x, yy, sym, 0);
}
}
}
void osdCanvasDrawVario(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, float zvel)
{
UNUSED(display);
static float prev = 0;
if (fabsf(prev - zvel) < (OSD_VARIO_CM_S_PER_ARROW / 20.0f)) {
return;
}
uint8_t ex;
uint8_t ey;
osdDrawPointGetGrid(&ex, &ey, display, canvas, p);
osdCanvasDrawVarioShape(canvas, ex, ey, prev, true);
osdCanvasDrawVarioShape(canvas, ex, ey, zvel, false);
prev = zvel;
}
void osdCanvasDrawDirArrow(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, float degrees, bool eraseBefore)
{
UNUSED(display);
int px;
int py;
osdDrawPointGetPixels(&px, &py, display, canvas, p);
displayCanvasClipToRect(canvas, px, py, canvas->gridElementWidth, canvas->gridElementHeight);
if (eraseBefore) {
displayCanvasSetFillColor(canvas, DISPLAY_CANVAS_COLOR_TRANSPARENT);
displayCanvasFillRect(canvas, px, py, canvas->gridElementWidth, canvas->gridElementHeight);
}
displayCanvasSetFillColor(canvas, DISPLAY_CANVAS_COLOR_WHITE);
displayCanvasSetStrokeColor(canvas, DISPLAY_CANVAS_COLOR_BLACK);
displayCanvasCtmRotate(canvas, -DEGREES_TO_RADIANS(degrees));
displayCanvasCtmTranslate(canvas, px + canvas->gridElementWidth / 2, py + canvas->gridElementHeight / 2);
displayCanvasFillStrokeTriangle(canvas, 0, 6, 5, -6, -5, -6);
displayCanvasSetFillColor(canvas, DISPLAY_CANVAS_COLOR_TRANSPARENT);
displayCanvasFillStrokeTriangle(canvas, 0, -2, 6, -7, -6, -7);
displayCanvasMoveToPoint(canvas, 6, -7);
displayCanvasSetStrokeColor(canvas, DISPLAY_CANVAS_COLOR_TRANSPARENT);
displayCanvasStrokeLineToPoint(canvas, -6, -7);
}
static void osdDrawArtificialHorizonLevelLine(displayCanvas_t *canvas, int width, int pos, int margin, bool erase)
{
displayCanvasSetLineOutlineType(canvas, DISPLAY_CANVAS_OUTLINE_TYPE_BOTTOM);
if (erase) {
displayCanvasSetStrokeColor(canvas, DISPLAY_CANVAS_COLOR_TRANSPARENT);
displayCanvasSetLineOutlineColor(canvas, DISPLAY_CANVAS_COLOR_TRANSPARENT);
} else {
displayCanvasSetStrokeColor(canvas, DISPLAY_CANVAS_COLOR_WHITE);
displayCanvasSetLineOutlineColor(canvas, DISPLAY_CANVAS_COLOR_BLACK);
}
int yoff = pos >= 0 ? 10 : -10;
int yc = -pos - 1;
int sz = width / 2;
// Horizontal strokes
displayCanvasMoveToPoint(canvas, -sz, yc);
displayCanvasStrokeLineToPoint(canvas, -margin, yc);
displayCanvasMoveToPoint(canvas, sz, yc);
displayCanvasStrokeLineToPoint(canvas, margin, yc);
// Vertical strokes
displayCanvasSetLineOutlineType(canvas, DISPLAY_CANVAS_OUTLINE_TYPE_LEFT);
displayCanvasMoveToPoint(canvas, -sz, yc);
displayCanvasStrokeLineToPoint(canvas, -sz, yc + yoff);
displayCanvasSetLineOutlineType(canvas, DISPLAY_CANVAS_OUTLINE_TYPE_RIGHT);
displayCanvasMoveToPoint(canvas, sz, yc);
displayCanvasStrokeLineToPoint(canvas, sz, yc + yoff);
}
static void osdDrawArtificialHorizonShapes(displayCanvas_t *canvas, float pitchAngle, float rollAngle, bool erase)
{
int barWidth = (OSD_AHI_WIDTH - 1) * canvas->gridElementWidth;
int levelBarWidth = barWidth * (3.0/4);
int crosshairMargin = 6;
float pixelsPerDegreeLevel = 3.5f;
int maxWidth = (OSD_AHI_WIDTH + 1) * canvas->gridElementWidth;
int maxHeight = OSD_AHI_HEIGHT * canvas->gridElementHeight;
int borderSize = 3;
char buf[12];
displayCanvasContextPush(canvas);
int lx = (canvas->width - maxWidth) / 2;
int ty = (canvas->height - maxHeight) / 2;
if (!erase) {
int rx = lx + maxWidth;
int by = ty + maxHeight;
displayCanvasSetStrokeColor(canvas, DISPLAY_CANVAS_COLOR_BLACK);
displayCanvasMoveToPoint(canvas, lx, ty + borderSize);
displayCanvasStrokeLineToPoint(canvas, lx, ty);
displayCanvasStrokeLineToPoint(canvas, lx + borderSize, ty);
displayCanvasMoveToPoint(canvas, rx, ty + borderSize);
displayCanvasStrokeLineToPoint(canvas, rx, ty);
displayCanvasStrokeLineToPoint(canvas, rx - borderSize, ty);
displayCanvasMoveToPoint(canvas,lx, by - borderSize);
displayCanvasStrokeLineToPoint(canvas, lx, by);
displayCanvasStrokeLineToPoint(canvas, lx + borderSize, by);
displayCanvasMoveToPoint(canvas, rx, by - borderSize);
displayCanvasStrokeLineToPoint(canvas, rx, by);
displayCanvasStrokeLineToPoint(canvas, rx - borderSize, by);
}
displayCanvasClipToRect(canvas, lx + 1, ty + 1, maxWidth - 2, maxHeight - 2);
osdGridBufferClearPixelRect(canvas, lx, ty, maxWidth, maxHeight);
if (erase) {
displayCanvasSetStrokeAndFillColor(canvas, DISPLAY_CANVAS_COLOR_TRANSPARENT);
displayCanvasSetLineOutlineColor(canvas, DISPLAY_CANVAS_COLOR_TRANSPARENT);
} else {
displayCanvasSetStrokeColor(canvas, DISPLAY_CANVAS_COLOR_WHITE);
displayCanvasSetLineOutlineColor(canvas, DISPLAY_CANVAS_COLOR_BLACK);
}
// The draw just the 5 bars closest to the current pitch level
float pitchDegrees = RADIANS_TO_DEGREES(pitchAngle);
float pitchCenter = roundf(pitchDegrees / 10.0f);
float pitchOffset = -pitchDegrees * pixelsPerDegreeLevel;
float translateX = canvas->width / 2;
float translateY = canvas->height / 2;
displayCanvasCtmTranslate(canvas, 0, pitchOffset);
displayCanvasContextPush(canvas);
displayCanvasCtmRotate(canvas, -rollAngle);
displayCanvasCtmTranslate(canvas, translateX, translateY);
for (int ii = pitchCenter - 2; ii <= pitchCenter + 2; ii++) {
if (ii == 0) {
displayCanvasSetLineOutlineType(canvas, DISPLAY_CANVAS_OUTLINE_TYPE_BOTTOM);
displayCanvasMoveToPoint(canvas, -barWidth / 2, 0);
displayCanvasStrokeLineToPoint(canvas, -crosshairMargin, 0);
displayCanvasMoveToPoint(canvas, barWidth / 2, 0);
displayCanvasStrokeLineToPoint(canvas, crosshairMargin, 0);
continue;
}
int pos = ii * 10 * pixelsPerDegreeLevel;
int margin = (ii > 9 || ii < -9) ? 9 : 6;
osdDrawArtificialHorizonLevelLine(canvas, levelBarWidth, -pos, margin, erase);
}
displayCanvasContextPop(canvas);
displayCanvasCtmTranslate(canvas, translateX, translateY);
displayCanvasCtmScale(canvas, 0.5f, 0.5f);
// Draw line labels
float sx = sin_approx(-rollAngle);
float sy = cos_approx(rollAngle);
for (int ii = pitchCenter - 2; ii <= pitchCenter + 2; ii++) {
if (ii == 0) {
continue;
}
int level = ii * 10;
int absLevel = ABS(level);
tfp_snprintf(buf, sizeof(buf), "%d", absLevel);
int pos = level * pixelsPerDegreeLevel;
int charY = 9 - pos * 2;
int cx = (absLevel >= 100 ? -1.5f : -1.0) * canvas->gridElementWidth;
int px = cx + (pitchOffset + pos) * sx * 2;
int py = -charY - (pitchOffset + pos) * (1 - sy) * 2;
if (erase) {
displayCanvasDrawStringMask(canvas, px, py, buf, DISPLAY_CANVAS_COLOR_TRANSPARENT, 0);
} else {
displayCanvasDrawString(canvas, px, py, buf, 0);
}
}
displayCanvasContextPop(canvas);
}
void osdCanvasDrawArtificialHorizon(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, float pitchAngle, float rollAngle)
{
UNUSED(display);
UNUSED(p);
static float prevPitchAngle = 9999;
static float prevRollAngle = 9999;
static timeMs_t nextDrawMs = 0;
timeMs_t now = millis();
if (fabsf(prevPitchAngle - pitchAngle) > 0.01f ||
fabsf(prevRollAngle - rollAngle) > 0.01f ||
now > nextDrawMs) {
osdDrawArtificialHorizonShapes(canvas, prevPitchAngle, prevRollAngle, true);
osdDrawArtificialHorizonShapes(canvas, pitchAngle, rollAngle, false);
prevPitchAngle = pitchAngle;
prevRollAngle = rollAngle;
nextDrawMs = now + AHI_MAX_DRAW_INTERVAL_MS;
}
}
void osdCanvasDrawHeadingGraph(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, int heading)
{
static const uint8_t graph[] = {
SYM_HEADING_W,
SYM_HEADING_LINE,
SYM_HEADING_DIVIDED_LINE,
SYM_HEADING_LINE,
SYM_HEADING_N,
SYM_HEADING_LINE,
SYM_HEADING_DIVIDED_LINE,
SYM_HEADING_LINE,
SYM_HEADING_E,
SYM_HEADING_LINE,
SYM_HEADING_DIVIDED_LINE,
SYM_HEADING_LINE,
SYM_HEADING_S,
SYM_HEADING_LINE,
SYM_HEADING_DIVIDED_LINE,
SYM_HEADING_LINE,
SYM_HEADING_W,
SYM_HEADING_LINE,
SYM_HEADING_DIVIDED_LINE,
SYM_HEADING_LINE,
SYM_HEADING_N,
SYM_HEADING_LINE,
SYM_HEADING_DIVIDED_LINE,
SYM_HEADING_LINE,
SYM_HEADING_E,
SYM_HEADING_LINE,
};
STATIC_ASSERT(sizeof(graph) > (3599 / OSD_HEADING_GRAPH_DECIDEGREES_PER_CHAR) + OSD_HEADING_GRAPH_WIDTH + 1, graph_is_too_short);
char buf[OSD_HEADING_GRAPH_WIDTH + 1];
int px;
int py;
osdDrawPointGetPixels(&px, &py, display, canvas, p);
int rw = OSD_HEADING_GRAPH_WIDTH * canvas->gridElementWidth;
int rh = canvas->gridElementHeight;
displayCanvasClipToRect(canvas, px, py, rw, rh);
int idx = heading / OSD_HEADING_GRAPH_DECIDEGREES_PER_CHAR;
int offset = ((heading % OSD_HEADING_GRAPH_DECIDEGREES_PER_CHAR) * canvas->gridElementWidth) / OSD_HEADING_GRAPH_DECIDEGREES_PER_CHAR;
memcpy_fn(buf, graph + idx, sizeof(buf) - 1);
buf[sizeof(buf) - 1] = '\0';
// We need a +1 because characters are 12px wide, so
// they can't have a 1px arrow centered. All existing fonts
// place the arrow at 5px, hence there's a 1px offset.
// TODO: Put this in font metadata and read it back.
displayCanvasDrawString(canvas, px - offset + 1, py, buf, DISPLAY_CANVAS_BITMAP_OPT_ERASE_TRANSPARENT);
displayCanvasSetStrokeColor(canvas, DISPLAY_CANVAS_COLOR_BLACK);
displayCanvasSetFillColor(canvas, DISPLAY_CANVAS_COLOR_WHITE);
int rmx = px + rw / 2;
displayCanvasFillStrokeTriangle(canvas, rmx - 2, py - 1, rmx + 2, py - 1, rmx, py + 1);
}
#endif

38
src/main/io/osd_canvas.h Normal file
View file

@ -0,0 +1,38 @@
/*
* This file is part of INAV.
*
* 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/.
*
* @author Alberto Garcia Hierro <alberto@garciahierro.com>
*/
#pragma once
#include <stdbool.h>
typedef struct displayPort_s displayPort_t;
typedef struct displayCanvas_s displayCanvas_t;
typedef struct osdDrawPoint_s osdDrawPoint_t;
void osdCanvasDrawVario(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, float zvel);
void osdCanvasDrawDirArrow(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, float degrees, bool eraseBefore);
void osdCanvasDrawArtificialHorizon(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, float pitchAngle, float rollAngle);
void osdCanvasDrawHeadingGraph(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, int heading);

141
src/main/io/osd_common.c Normal file
View file

@ -0,0 +1,141 @@
/*
* This file is part of INAV.
*
* 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/.
*
* @author Alberto Garcia Hierro <alberto@garciahierro.com>
*/
#include "platform.h"
#include "common/utils.h"
#include "drivers/display.h"
#include "drivers/display_canvas.h"
#include "drivers/osd.h"
#include "io/osd_canvas.h"
#include "io/osd_common.h"
#include "io/osd_grid.h"
#define CANVAS_DEFAULT_GRID_ELEMENT_WIDTH OSD_CHAR_WIDTH
#define CANVAS_DEFAULT_GRID_ELEMENT_HEIGHT OSD_CHAR_HEIGHT
void osdDrawPointGetGrid(uint8_t *gx, uint8_t *gy, const displayPort_t *display, const displayCanvas_t *canvas, const osdDrawPoint_t *p)
{
UNUSED(display);
switch (p->type) {
case OSD_DRAW_POINT_TYPE_GRID:
*gx = p->grid.gx;
*gy = p->grid.gy;
break;
case OSD_DRAW_POINT_TYPE_PIXEL:
*gx = p->pixel.px / (canvas ? canvas->gridElementWidth : CANVAS_DEFAULT_GRID_ELEMENT_WIDTH);
*gy = p->pixel.py / (canvas ? canvas->gridElementHeight : OSD_CHAR_HEIGHT);
break;
}
}
void osdDrawPointGetPixels(int *px, int *py, const displayPort_t *display, const displayCanvas_t *canvas, const osdDrawPoint_t *p)
{
UNUSED(display);
switch (p->type) {
case OSD_DRAW_POINT_TYPE_GRID:
*px = p->grid.gx * (canvas ? canvas->gridElementWidth : CANVAS_DEFAULT_GRID_ELEMENT_WIDTH);
*py = p->grid.gy * (canvas ? canvas->gridElementHeight : CANVAS_DEFAULT_GRID_ELEMENT_HEIGHT);
break;
case OSD_DRAW_POINT_TYPE_PIXEL:
*px = p->pixel.px;
*py = p->pixel.py;
break;
}
}
void osdDrawVario(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, float zvel)
{
uint8_t gx;
uint8_t gy;
#if defined(USE_CANVAS)
if (canvas) {
osdCanvasDrawVario(display, canvas, p, zvel);
} else {
#endif
osdDrawPointGetGrid(&gx, &gy, display, canvas, p);
osdGridDrawVario(display, gx, gy, zvel);
#if defined(USE_CANVAS)
}
#endif
}
void osdDrawDirArrow(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, float degrees, bool eraseBefore)
{
#if !defined(USE_CANVAS)
UNUSED(eraseBefore);
#endif
uint8_t gx;
uint8_t gy;
#if defined(USE_CANVAS)
if (canvas) {
osdCanvasDrawDirArrow(display, canvas, p, degrees, eraseBefore);
} else {
#endif
osdDrawPointGetGrid(&gx, &gy, display, canvas, p);
osdGridDrawDirArrow(display, gx, gy, degrees);
#if defined(USE_CANVAS)
}
#endif
}
void osdDrawArtificialHorizon(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, float rollAngle, float pitchAngle)
{
uint8_t gx;
uint8_t gy;
#if defined(USE_CANVAS)
if (canvas) {
osdCanvasDrawArtificialHorizon(display, canvas, p, pitchAngle, rollAngle);
} else {
#endif
osdDrawPointGetGrid(&gx, &gy, display, canvas, p);
osdGridDrawArtificialHorizon(display, gx, gy, pitchAngle, rollAngle);
#if defined(USE_CANVAS)
}
#endif
}
void osdDrawHeadingGraph(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, int heading)
{
uint8_t gx;
uint8_t gy;
#if defined(USE_CANVAS)
if (canvas) {
osdCanvasDrawHeadingGraph(display, canvas, p, heading);
} else {
#endif
osdDrawPointGetGrid(&gx, &gy, display, canvas, p);
osdGridDrawHeadingGraph(display, gx, gy, heading);
#if defined(USE_CANVAS)
}
#endif
}

80
src/main/io/osd_common.h Normal file
View file

@ -0,0 +1,80 @@
/*
* This file is part of INAV.
*
* 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/.
*
* @author Alberto Garcia Hierro <alberto@garciahierro.com>
*/
#pragma once
#include <stdbool.h>
#include <stdint.h>
#define OSD_VARIO_CM_S_PER_ARROW 50 // 1 arrow = 50cm/s
#define OSD_VARIO_HEIGHT_ROWS 5
#define OSD_AHI_HEIGHT 9
#define OSD_AHI_WIDTH 11
#define OSD_AHI_PREV_SIZE (OSD_AHI_WIDTH > OSD_AHI_HEIGHT ? OSD_AHI_WIDTH : OSD_AHI_HEIGHT)
#define OSD_AHI_H_SYM_COUNT 9
#define OSD_AHI_V_SYM_COUNT 6
#define OSD_HEADING_GRAPH_WIDTH 9
#define OSD_HEADING_GRAPH_DECIDEGREES_PER_CHAR 225
typedef struct displayPort_s displayPort_t;
typedef struct displayCanvas_s displayCanvas_t;
typedef enum {
OSD_DRAW_POINT_TYPE_GRID,
OSD_DRAW_POINT_TYPE_PIXEL,
} osdDrawPointType_e;
typedef struct osdDrawPoint_s {
osdDrawPointType_e type;
union {
struct {
uint8_t gx;
uint8_t gy;
} grid;
struct {
int16_t px;
int16_t py;
} pixel;
};
} osdDrawPoint_t;
#define OSD_DRAW_POINT_GRID(_x, _y) (&(osdDrawPoint_t){ .type = OSD_DRAW_POINT_TYPE_GRID, .grid = {.gx = (_x), .gy = (_y)}})
#define OSD_DRAW_POINT_PIXEL(_x, _y) (&(osdDrawPoint_t){ .type = OSD_DRAW_POINT_TYPE_PIXEL, .pixel = {.px = (_x), .py = (_y)}})
void osdDrawPointGetGrid(uint8_t *gx, uint8_t *gy, const displayPort_t *display, const displayCanvas_t *canvas, const osdDrawPoint_t *p);
void osdDrawPointGetPixels(int *px, int *py, const displayPort_t *display, const displayCanvas_t *canvas, const osdDrawPoint_t *p);
void osdDrawVario(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, float zvel);
// Draws an arrow at the given point, where 0 degrees points to the top of the viewport and
// positive angles result in clockwise rotation. If eraseBefore is true, the rect surrouing
// the arrow will be erased first (need for e.g. the home arrow, since it uses multiple symbols)
void osdDrawDirArrow(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, float degrees, bool eraseBefore);
void osdDrawArtificialHorizon(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, float rollAngle, float pitchAngle);
// Draws a heading graph with heading given as 0.1 degree steps i.e. [0, 3600). It uses 9 horizontal
// grid slots.
void osdDrawHeadingGraph(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, int heading);

200
src/main/io/osd_grid.c Normal file
View file

@ -0,0 +1,200 @@
/*
* This file is part of INAV.
*
* 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/.
*
* @author Alberto Garcia Hierro <alberto@garciahierro.com>
*/
#include <math.h>
#include "platform.h"
#if defined(USE_OSD)
#include "common/maths.h"
#include "common/utils.h"
#include "drivers/display.h"
#include "drivers/osd_symbols.h"
#include "io/osd.h"
#include "io/osd_common.h"
void osdGridDrawVario(displayPort_t *display, unsigned gx, unsigned gy, float zvel)
{
int v = zvel / OSD_VARIO_CM_S_PER_ARROW;
uint8_t vchars[] = {SYM_BLANK, SYM_BLANK, SYM_BLANK, SYM_BLANK, SYM_BLANK};
if (v >= 6)
vchars[0] = SYM_VARIO_UP_2A;
else if (v == 5)
vchars[0] = SYM_VARIO_UP_1A;
if (v >=4)
vchars[1] = SYM_VARIO_UP_2A;
else if (v == 3)
vchars[1] = SYM_VARIO_UP_1A;
if (v >=2)
vchars[2] = SYM_VARIO_UP_2A;
else if (v == 1)
vchars[2] = SYM_VARIO_UP_1A;
if (v <= -2)
vchars[2] = SYM_VARIO_DOWN_2A;
else if (v == -1)
vchars[2] = SYM_VARIO_DOWN_1A;
if (v <= -4)
vchars[3] = SYM_VARIO_DOWN_2A;
else if (v == -3)
vchars[3] = SYM_VARIO_DOWN_1A;
if (v <= -6)
vchars[4] = SYM_VARIO_DOWN_2A;
else if (v == -5)
vchars[4] = SYM_VARIO_DOWN_1A;
displayWriteChar(display, gx, gy, vchars[0]);
displayWriteChar(display, gx, gy + 1, vchars[1]);
displayWriteChar(display, gx, gy + 2, vchars[2]);
displayWriteChar(display, gx, gy + 3, vchars[3]);
displayWriteChar(display, gx, gy + 4, vchars[4]);
}
void osdGridDrawDirArrow(displayPort_t *display, unsigned gx, unsigned gy, float degrees)
{
// There are 16 orientations for the direction arrow.
// so we use 22.5deg per image, where the 1st image is used
// for [349, 11], the 2nd for [12, 33], etc...
// Add 11 to the angle, so first character maps to [349, 11]
int dir = osdGetHeadingAngle(degrees + 11);
unsigned arrowOffset = dir * 2 / 45;
displayWriteChar(display, gx, gy, SYM_ARROW_UP + arrowOffset);
}
void osdGridDrawArtificialHorizon(displayPort_t *display, unsigned gx, unsigned gy, float pitchAngle, float rollAngle)
{
UNUSED(gx);
UNUSED(gy);
uint8_t elemPosX;
uint8_t elemPosY;
osdCrosshairPosition(&elemPosX, &elemPosY);
// Store the positions we draw over to erase only these at the next iteration
static int8_t previous_written[OSD_AHI_PREV_SIZE];
static int8_t previous_orient = -1;
float pitch_rad_to_char = (float)(OSD_AHI_HEIGHT / 2 + 0.5) / DEGREES_TO_RADIANS(osdConfig()->ahi_max_pitch);
float ky = sin_approx(rollAngle);
float kx = cos_approx(rollAngle);
if (previous_orient != -1) {
for (int i = 0; i < OSD_AHI_PREV_SIZE; ++i) {
if (previous_written[i] > -1) {
int8_t dx = (previous_orient ? previous_written[i] : i) - OSD_AHI_PREV_SIZE / 2;
int8_t dy = (previous_orient ? i : previous_written[i]) - OSD_AHI_PREV_SIZE / 2;
displayWriteChar(display, elemPosX + dx, elemPosY - dy, SYM_BLANK);
previous_written[i] = -1;
}
}
}
if (fabsf(ky) < fabsf(kx)) {
previous_orient = 0;
for (int8_t dx = -OSD_AHI_WIDTH / 2; dx <= OSD_AHI_WIDTH / 2; dx++) {
float fy = dx * (ky / kx) + pitchAngle * pitch_rad_to_char + 0.49f;
int8_t dy = floorf(fy);
const uint8_t chX = elemPosX + dx, chY = elemPosY - dy;
uint16_t c;
if ((dy >= -OSD_AHI_HEIGHT / 2) && (dy <= OSD_AHI_HEIGHT / 2) && displayReadCharWithAttr(display, chX, chY, &c, NULL) && (c == SYM_BLANK)) {
c = SYM_AH_H_START + ((OSD_AHI_H_SYM_COUNT - 1) - (uint8_t)((fy - dy) * OSD_AHI_H_SYM_COUNT));
displayWriteChar(display, elemPosX + dx, elemPosY - dy, c);
previous_written[dx + OSD_AHI_PREV_SIZE / 2] = dy + OSD_AHI_PREV_SIZE / 2;
}
}
} else {
previous_orient = 1;
for (int8_t dy = -OSD_AHI_HEIGHT / 2; dy <= OSD_AHI_HEIGHT / 2; dy++) {
const float fx = (dy - pitchAngle * pitch_rad_to_char) * (kx / ky) + 0.5f;
const int8_t dx = floorf(fx);
const uint8_t chX = elemPosX + dx, chY = elemPosY - dy;
uint16_t c;
if ((dx >= -OSD_AHI_WIDTH / 2) && (dx <= OSD_AHI_WIDTH / 2) && displayReadCharWithAttr(display, chX, chY, &c, NULL) && (c == SYM_BLANK)) {
c = SYM_AH_V_START + (fx - dx) * OSD_AHI_V_SYM_COUNT;
displayWriteChar(display, chX, chY, c);
previous_written[dy + OSD_AHI_PREV_SIZE / 2] = dx + OSD_AHI_PREV_SIZE / 2;
}
}
}
}
void osdGridDrawHeadingGraph(displayPort_t *display, unsigned gx, unsigned gy, int heading)
{
static const uint8_t graph[] = {
SYM_HEADING_LINE,
SYM_HEADING_E,
SYM_HEADING_LINE,
SYM_HEADING_DIVIDED_LINE,
SYM_HEADING_LINE,
SYM_HEADING_S,
SYM_HEADING_LINE,
SYM_HEADING_DIVIDED_LINE,
SYM_HEADING_LINE,
SYM_HEADING_W,
SYM_HEADING_LINE,
SYM_HEADING_DIVIDED_LINE,
SYM_HEADING_LINE,
SYM_HEADING_N,
SYM_HEADING_LINE,
SYM_HEADING_DIVIDED_LINE,
SYM_HEADING_LINE,
SYM_HEADING_E,
SYM_HEADING_LINE,
SYM_HEADING_DIVIDED_LINE,
SYM_HEADING_LINE,
SYM_HEADING_S,
SYM_HEADING_LINE,
SYM_HEADING_DIVIDED_LINE,
SYM_HEADING_LINE,
SYM_HEADING_W,
SYM_HEADING_LINE,
};
char buf[OSD_HEADING_GRAPH_WIDTH + 1];
int16_t h = DECIDEGREES_TO_DEGREES(heading);
if (h >= 180) {
h -= 360;
}
int hh = h * 4;
hh = hh + 720 + 45;
hh = hh / 90;
memcpy_fn(buf, graph + hh + 1, sizeof(buf) - 1);
buf[sizeof(buf) - 1] = '\0';
displayWrite(display, gx, gy, buf);
}
#endif

34
src/main/io/osd_grid.h Normal file
View file

@ -0,0 +1,34 @@
/*
* This file is part of INAV.
*
* 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/.
*
* @author Alberto Garcia Hierro <alberto@garciahierro.com>
*/
#pragma once
typedef struct displayPort_s displayPort_t;
void osdGridDrawVario(displayPort_t *display, unsigned gx, unsigned gy, float zvel);
void osdGridDrawDirArrow(displayPort_t *display, unsigned gx, unsigned gy, float degrees);
void osdGridDrawArtificialHorizon(displayPort_t *display, unsigned gx, unsigned gy, float pitchAngle, float rollAngle);
void osdGridDrawHeadingGraph(displayPort_t *display, unsigned gx, unsigned gy, int heading);

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

@ -50,7 +50,8 @@ typedef enum {
FUNCTION_RANGEFINDER = (1 << 16), // 65536 FUNCTION_RANGEFINDER = (1 << 16), // 65536
FUNCTION_VTX_FFPV = (1 << 17), // 131072 FUNCTION_VTX_FFPV = (1 << 17), // 131072
FUNCTION_ESCSERIAL = (1 << 18), // 262144: this is used for both SERIALSHOT and ESC_SENSOR telemetry FUNCTION_ESCSERIAL = (1 << 18), // 262144: this is used for both SERIALSHOT and ESC_SENSOR telemetry
FUNCTION_TELEMETRY_SIM = (1 << 19) // 524288 FUNCTION_TELEMETRY_SIM = (1 << 19), // 524288
FUNCTION_FRSKY_OSD = (1 << 20), // 1048576
} serialPortFunction_e; } serialPortFunction_e;
typedef enum { typedef enum {

2
src/main/msp/msp_protocol.h
View file

@ -60,7 +60,7 @@
#define MSP_PROTOCOL_VERSION 0 // Same version over MSPv1 & MSPv2 - message format didn't change and it backward compatible #define MSP_PROTOCOL_VERSION 0 // Same version over MSPv1 & MSPv2 - message format didn't change and it backward compatible
#define API_VERSION_MAJOR 2 // increment when major changes are made #define API_VERSION_MAJOR 2 // increment when major changes are made
#define API_VERSION_MINOR 3 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR #define API_VERSION_MINOR 4 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR
#define API_VERSION_LENGTH 2 #define API_VERSION_LENGTH 2

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

@ -1368,6 +1368,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
switch (posControl.waypointList[posControl.activeWaypointIndex].action) { switch (posControl.waypointList[posControl.activeWaypointIndex].action) {
case NAV_WP_ACTION_WAYPOINT: case NAV_WP_ACTION_WAYPOINT:
calculateAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]); calculateAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]);
posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos);
posControl.wpInitialAltitude = posControl.actualState.abs.pos.z;
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS
case NAV_WP_ACTION_RTH: case NAV_WP_ACTION_RTH:
@ -1392,7 +1394,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED
} }
else { else {
setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING); fpVector3_t tmpWaypoint;
tmpWaypoint.x = posControl.activeWaypoint.pos.x;
tmpWaypoint.y = posControl.activeWaypoint.pos.y;
tmpWaypoint.z = scaleRange(constrainf(posControl.wpDistance, posControl.wpInitialDistance / 10, posControl.wpInitialDistance),
posControl.wpInitialDistance, posControl.wpInitialDistance / 10,
posControl.wpInitialAltitude, posControl.activeWaypoint.pos.z);
setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
return NAV_FSM_EVENT_NONE; // will re-process state in >10ms return NAV_FSM_EVENT_NONE; // will re-process state in >10ms
} }
break; break;
@ -2040,15 +2048,15 @@ bool isWaypointMissed(const navWaypointPosition_t * waypoint)
static bool isWaypointPositionReached(const fpVector3_t * pos, const bool isWaypointHome) static bool isWaypointPositionReached(const fpVector3_t * pos, const bool isWaypointHome)
{ {
// We consider waypoint reached if within specified radius // We consider waypoint reached if within specified radius
const uint32_t wpDistance = calculateDistanceToDestination(pos); posControl.wpDistance = calculateDistanceToDestination(pos);
if (STATE(FIXED_WING) && isWaypointHome) { if (STATE(FIXED_WING) && isWaypointHome) {
// Airplane will do a circular loiter over home and might never approach it closer than waypoint_radius - need extra check // Airplane will do a circular loiter over home and might never approach it closer than waypoint_radius - need extra check
return (wpDistance <= navConfig()->general.waypoint_radius) return (posControl.wpDistance <= navConfig()->general.waypoint_radius)
|| (wpDistance <= (navConfig()->fw.loiter_radius * 1.10f)); // 10% margin of desired circular loiter radius || (posControl.wpDistance <= (navConfig()->fw.loiter_radius * 1.10f)); // 10% margin of desired circular loiter radius
} }
else { else {
return (wpDistance <= navConfig()->general.waypoint_radius); return (posControl.wpDistance <= navConfig()->general.waypoint_radius);
} }
} }
@ -2076,7 +2084,8 @@ float getFinalRTHAltitude(void)
static void updateDesiredRTHAltitude(void) static void updateDesiredRTHAltitude(void)
{ {
if (ARMING_FLAG(ARMED)) { if (ARMING_FLAG(ARMED)) {
if (!(navGetStateFlags(posControl.navState) & NAV_AUTO_RTH)) { if (!((navGetStateFlags(posControl.navState) & NAV_AUTO_RTH)
|| ((navGetStateFlags(posControl.navState) & NAV_AUTO_WP) && posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_RTH))) {
switch (navConfig()->general.flags.rth_alt_control_mode) { switch (navConfig()->general.flags.rth_alt_control_mode) {
case NAV_RTH_NO_ALT: case NAV_RTH_NO_ALT:
posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z; posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z;
@ -2391,9 +2400,6 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
// Fixed wing climb rate controller is open-loop. We simply move the known altitude target // Fixed wing climb rate controller is open-loop. We simply move the known altitude target
float timeDelta = US2S(currentTimeUs - lastUpdateTimeUs); float timeDelta = US2S(currentTimeUs - lastUpdateTimeUs);
DEBUG_SET(DEBUG_FW_CLIMB_RATE_TO_ALTITUDE, 0, desiredClimbRate);
DEBUG_SET(DEBUG_FW_CLIMB_RATE_TO_ALTITUDE, 1, timeDelta * 1000);
if (timeDelta <= HZ2S(MIN_POSITION_UPDATE_RATE_HZ)) { if (timeDelta <= HZ2S(MIN_POSITION_UPDATE_RATE_HZ)) {
posControl.desiredState.pos.z += desiredClimbRate * timeDelta; posControl.desiredState.pos.z += desiredClimbRate * timeDelta;
posControl.desiredState.pos.z = constrainf(posControl.desiredState.pos.z, altitudeToUse - 500, altitudeToUse + 500); // FIXME: calculate sanity limits in a smarter way posControl.desiredState.pos.z = constrainf(posControl.desiredState.pos.z, altitudeToUse - 500, altitudeToUse + 500); // FIXME: calculate sanity limits in a smarter way

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

@ -660,10 +660,6 @@ bool isMulticopterLandingDetected(void)
bool possibleLandingDetected = isAtMinimalThrust && !verticalMovement && !horizontalMovement; bool possibleLandingDetected = isAtMinimalThrust && !verticalMovement && !horizontalMovement;
DEBUG_SET(DEBUG_NAV_LANDING_DETECTOR, 0, isAtMinimalThrust * 100 + !verticalMovement * 10 + !horizontalMovement * 1);
DEBUG_SET(DEBUG_NAV_LANDING_DETECTOR, 1, (landingThrSamples == 0) ? 0 : (rcCommandAdjustedThrottle - (landingThrSum / landingThrSamples)));
DEBUG_SET(DEBUG_NAV_LANDING_DETECTOR, 2, (currentTimeUs - landingTimer) / 1000);
// If we have surface sensor (for example sonar) - use it to detect touchdown // If we have surface sensor (for example sonar) - use it to detect touchdown
if ((posControl.flags.estAglStatus == EST_TRUSTED) && (posControl.actualState.agl.pos.z >= 0)) { if ((posControl.flags.estAglStatus == EST_TRUSTED) && (posControl.actualState.agl.pos.z >= 0)) {
// TODO: Come up with a clever way to let sonar increase detection performance, not just add extra safety. // TODO: Come up with a clever way to let sonar increase detection performance, not just add extra safety.

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

@ -354,6 +354,9 @@ typedef struct {
navWaypointPosition_t activeWaypoint; // Local position and initial bearing, filled on waypoint activation navWaypointPosition_t activeWaypoint; // Local position and initial bearing, filled on waypoint activation
int8_t activeWaypointIndex; int8_t activeWaypointIndex;
float wpInitialAltitude; // Altitude at start of WP
float wpInitialDistance; // Distance when starting flight to WP
float wpDistance; // Distance to active WP
/* Internals & statistics */ /* Internals & statistics */
int16_t rcAdjustment[4]; int16_t rcAdjustment[4];

8
src/main/rx/eleres.c
View file

@ -375,6 +375,10 @@ rx_spi_received_e eleresDataReceived(uint8_t *payload, uint16_t *linkQuality)
statusRegisters[0] = 0; statusRegisters[0] = 0;
statusRegisters[1] = 0; statusRegisters[1] = 0;
if (linkQuality) {
*linkQuality = eleresRssi();
}
if (rxSpiCheckIrq()) if (rxSpiCheckIrq())
{ {
statusRegisters[0] = rfmSpiRead(0x03); statusRegisters[0] = rfmSpiRead(0x03);
@ -385,10 +389,6 @@ rx_spi_received_e eleresDataReceived(uint8_t *payload, uint16_t *linkQuality)
} }
} }
if (linkQuality) {
*linkQuality = eleresRssi();
}
eleresSetRcDataFromPayload(NULL,NULL); eleresSetRcDataFromPayload(NULL,NULL);
return RX_SPI_RECEIVED_NONE; return RX_SPI_RECEIVED_NONE;

16
src/main/rx/ibus.c
View file

@ -46,7 +46,9 @@
#include "telemetry/ibus.h" #include "telemetry/ibus.h"
#include "telemetry/ibus_shared.h" #include "telemetry/ibus_shared.h"
#define IBUS_MAX_CHANNEL 14 #define IBUS_MAX_CHANNEL 18
//In AFHDS there is 18 channels encoded in 14 slots (each slot is 2 byte long)
#define IBUS_MAX_SLOTS 14
#define IBUS_BUFFSIZE 32 #define IBUS_BUFFSIZE 32
#define IBUS_MODEL_IA6B 0 #define IBUS_MODEL_IA6B 0
#define IBUS_MODEL_IA6 1 #define IBUS_MODEL_IA6 1
@ -139,7 +141,7 @@ static bool isChecksumOkIa6(void)
uint16_t chksum, rxsum; uint16_t chksum, rxsum;
chksum = ibusChecksum; chksum = ibusChecksum;
rxsum = ibus[ibusFrameSize - 2] + (ibus[ibusFrameSize - 1] << 8); rxsum = ibus[ibusFrameSize - 2] + (ibus[ibusFrameSize - 1] << 8);
for (i = 0, offset = ibusChannelOffset; i < IBUS_MAX_CHANNEL; i++, offset += 2) { for (i = 0, offset = ibusChannelOffset; i < IBUS_MAX_SLOTS; i++, offset += 2) {
chksum += ibus[offset] + (ibus[offset + 1] << 8); chksum += ibus[offset] + (ibus[offset + 1] << 8);
} }
return chksum == rxsum; return chksum == rxsum;
@ -165,9 +167,13 @@ static bool checksumIsOk(void) {
static void updateChannelData(void) { static void updateChannelData(void) {
uint8_t i; uint8_t i;
uint8_t offset; uint8_t offset;
for (i = 0, offset = ibusChannelOffset; i < IBUS_MAX_CHANNEL; i++, offset += 2) { for (i = 0, offset = ibusChannelOffset; i < IBUS_MAX_SLOTS; i++, offset += 2) {
ibusChannelData[i] = ibus[offset] + (ibus[offset + 1] << 8); ibusChannelData[i] = ibus[offset] + ((ibus[offset + 1] & 0x0F) << 8);
}
//latest IBUS recievers are using prviously not used 4 bits on every channel to incresse total channel count
for (i = IBUS_MAX_SLOTS, offset = ibusChannelOffset + 1; i < IBUS_MAX_CHANNEL; i++, offset += 6) {
ibusChannelData[i] = ((ibus[offset] & 0xF0) >> 4) | (ibus[offset + 2] & 0xF0) | ((ibus[offset + 4] & 0xF0) << 4);
} }
} }

3
src/main/scheduler/scheduler.h
View file

@ -116,6 +116,9 @@ typedef enum {
#ifdef USE_GLOBAL_FUNCTIONS #ifdef USE_GLOBAL_FUNCTIONS
TASK_GLOBAL_FUNCTIONS, TASK_GLOBAL_FUNCTIONS,
#endif #endif
#ifdef USE_RPM_FILTER
TASK_RPM_FILTER,
#endif
#ifdef USE_SECONDARY_IMU #ifdef USE_SECONDARY_IMU
TASK_SECONDARY_IMU, TASK_SECONDARY_IMU,
#endif #endif

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

@ -81,10 +81,8 @@ STATIC_FASTRAM void *accSoftLpfFilter[XYZ_AXIS_COUNT];
STATIC_FASTRAM pt1Filter_t accVibeFloorFilter[XYZ_AXIS_COUNT]; STATIC_FASTRAM pt1Filter_t accVibeFloorFilter[XYZ_AXIS_COUNT];
STATIC_FASTRAM pt1Filter_t accVibeFilter[XYZ_AXIS_COUNT]; STATIC_FASTRAM pt1Filter_t accVibeFilter[XYZ_AXIS_COUNT];
#ifdef USE_ACC_NOTCH
STATIC_FASTRAM filterApplyFnPtr accNotchFilterApplyFn; STATIC_FASTRAM filterApplyFnPtr accNotchFilterApplyFn;
STATIC_FASTRAM void *accNotchFilter[XYZ_AXIS_COUNT]; STATIC_FASTRAM void *accNotchFilter[XYZ_AXIS_COUNT];
#endif
PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 3); PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 3);
@ -578,13 +576,11 @@ void accUpdate(void)
acc.accADCf[axis] = accSoftLpfFilterApplyFn(accSoftLpfFilter[axis], acc.accADCf[axis]); acc.accADCf[axis] = accSoftLpfFilterApplyFn(accSoftLpfFilter[axis], acc.accADCf[axis]);
} }
#ifdef USE_ACC_NOTCH
if (accelerometerConfig()->acc_notch_hz) { if (accelerometerConfig()->acc_notch_hz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
acc.accADCf[axis] = accNotchFilterApplyFn(accNotchFilter[axis], acc.accADCf[axis]); acc.accADCf[axis] = accNotchFilterApplyFn(accNotchFilter[axis], acc.accADCf[axis]);
} }
} }
#endif
} }
@ -665,7 +661,6 @@ void accInitFilters(void)
pt1FilterInit(&accVibeFilter[axis], ACC_VIBE_FILT_HZ, accDt); pt1FilterInit(&accVibeFilter[axis], ACC_VIBE_FILT_HZ, accDt);
} }
#ifdef USE_ACC_NOTCH
STATIC_FASTRAM biquadFilter_t accFilterNotch[XYZ_AXIS_COUNT]; STATIC_FASTRAM biquadFilter_t accFilterNotch[XYZ_AXIS_COUNT];
accNotchFilterApplyFn = nullFilterApply; accNotchFilterApplyFn = nullFilterApply;
@ -676,7 +671,6 @@ void accInitFilters(void)
biquadFilterInitNotch(accNotchFilter[axis], acc.accTargetLooptime, accelerometerConfig()->acc_notch_hz, accelerometerConfig()->acc_notch_cutoff); biquadFilterInitNotch(accNotchFilter[axis], acc.accTargetLooptime, accelerometerConfig()->acc_notch_hz, accelerometerConfig()->acc_notch_cutoff);
} }
} }
#endif
} }

24
src/main/sensors/barometer.c
View file

@ -33,10 +33,11 @@
#include "drivers/barometer/barometer.h" #include "drivers/barometer/barometer.h"
#include "drivers/barometer/barometer_bmp085.h" #include "drivers/barometer/barometer_bmp085.h"
#include "drivers/barometer/barometer_bmp280.h" #include "drivers/barometer/barometer_bmp280.h"
#include "drivers/barometer/barometer_bmp388.h"
#include "drivers/barometer/barometer_lps25h.h" #include "drivers/barometer/barometer_lps25h.h"
#include "drivers/barometer/barometer_fake.h" #include "drivers/barometer/barometer_fake.h"
#include "drivers/barometer/barometer_ms56xx.h" #include "drivers/barometer/barometer_ms56xx.h"
#include "drivers/barometer/barometer_spl006.h" #include "drivers/barometer/barometer_spl06.h"
#include "drivers/time.h" #include "drivers/time.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
@ -132,10 +133,23 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse)
} }
FALLTHROUGH; FALLTHROUGH;
case BARO_SPL006: case BARO_BMP388:
#if defined(USE_BARO_SPL006) || defined(USE_BARO_SPI_SPL006) #if defined(USE_BARO_BMP388) || defined(USE_BARO_SPI_BMP388)
if (spl006Detect(dev)) { if (bmp388Detect(dev)) {
baroHardware = BARO_SPL006; baroHardware = BARO_BMP388;
break;
}
#endif
/* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
if (baroHardwareToUse != BARO_AUTODETECT) {
break;
}
FALLTHROUGH;
case BARO_SPL06:
#if defined(USE_BARO_SPL06) || defined(USE_BARO_SPI_SPL06)
if (spl06Detect(dev)) {
baroHardware = BARO_SPL06;
break; break;
} }
#endif #endif

5
src/main/sensors/barometer.h
View file

@ -29,8 +29,9 @@ typedef enum {
BARO_BMP280 = 4, BARO_BMP280 = 4,
BARO_MS5607 = 5, BARO_MS5607 = 5,
BARO_LPS25H = 6, BARO_LPS25H = 6,
BARO_SPL006 = 7, BARO_SPL06 = 7,
BARO_FAKE = 8, BARO_BMP388 = 8,
BARO_FAKE = 9,
BARO_MAX = BARO_FAKE BARO_MAX = BARO_FAKE
} baroSensor_e; } baroSensor_e;

26
src/main/sensors/esc_sensor.c
View file

@ -43,6 +43,8 @@
#include "drivers/pwm_output.h" #include "drivers/pwm_output.h"
#include "sensors/esc_sensor.h" #include "sensors/esc_sensor.h"
#include "io/serial.h" #include "io/serial.h"
#include "fc/runtime_config.h"
#if defined(USE_ESC_SENSOR) #if defined(USE_ESC_SENSOR)
@ -110,8 +112,13 @@ static bool escSensorDecodeFrame(void)
escSensorData[escSensorMotor].temperature = telemetryBuffer[0]; escSensorData[escSensorMotor].temperature = telemetryBuffer[0];
escSensorData[escSensorMotor].voltage = ((uint16_t)telemetryBuffer[1]) << 8 | telemetryBuffer[2]; escSensorData[escSensorMotor].voltage = ((uint16_t)telemetryBuffer[1]) << 8 | telemetryBuffer[2];
escSensorData[escSensorMotor].current = ((uint16_t)telemetryBuffer[3]) << 8 | telemetryBuffer[4]; escSensorData[escSensorMotor].current = ((uint16_t)telemetryBuffer[3]) << 8 | telemetryBuffer[4];
escSensorData[escSensorMotor].erpm = ((uint16_t)telemetryBuffer[7]) << 8 | telemetryBuffer[8]; escSensorData[escSensorMotor].rpm = ((uint16_t)telemetryBuffer[7]) << 8 | telemetryBuffer[8];
escSensorDataNeedsUpdate = true; escSensorDataNeedsUpdate = true;
if (escSensorMotor < 4) {
DEBUG_SET(DEBUG_ERPM, escSensorMotor, escSensorData[escSensorMotor].rpm);
}
return ESC_SENSOR_FRAME_COMPLETE; return ESC_SENSOR_FRAME_COMPLETE;
} }
else { else {
@ -123,6 +130,15 @@ static bool escSensorDecodeFrame(void)
return ESC_SENSOR_FRAME_PENDING; return ESC_SENSOR_FRAME_PENDING;
} }
uint32_t FAST_CODE computeRpm(int16_t erpm) {
return lrintf((float)erpm * ERPM_PER_LSB / (motorConfig()->motorPoleCount / 2));
}
escSensorData_t NOINLINE * getEscTelemetry(uint8_t esc)
{
return &escSensorData[esc];
}
escSensorData_t * escSensorGetData(void) escSensorData_t * escSensorGetData(void)
{ {
if (!escSensorPort) { if (!escSensorPort) {
@ -134,7 +150,7 @@ escSensorData_t * escSensorGetData(void)
escSensorDataCombined.temperature = 0; escSensorDataCombined.temperature = 0;
escSensorDataCombined.voltage = 0; escSensorDataCombined.voltage = 0;
escSensorDataCombined.current = 0; escSensorDataCombined.current = 0;
escSensorDataCombined.erpm = 0; escSensorDataCombined.rpm = 0;
// Combine data only from active sensors, ignore stale sensors // Combine data only from active sensors, ignore stale sensors
int usedEscSensorCount = 0; int usedEscSensorCount = 0;
@ -145,7 +161,7 @@ escSensorData_t * escSensorGetData(void)
escSensorDataCombined.temperature = MAX(escSensorDataCombined.temperature, escSensorData[i].temperature); escSensorDataCombined.temperature = MAX(escSensorDataCombined.temperature, escSensorData[i].temperature);
escSensorDataCombined.voltage += escSensorData[i].voltage; escSensorDataCombined.voltage += escSensorData[i].voltage;
escSensorDataCombined.current += escSensorData[i].current; escSensorDataCombined.current += escSensorData[i].current;
escSensorDataCombined.erpm += escSensorData[i].erpm; escSensorDataCombined.rpm += escSensorData[i].rpm;
} }
} }
@ -153,7 +169,7 @@ escSensorData_t * escSensorGetData(void)
if (usedEscSensorCount) { if (usedEscSensorCount) {
escSensorDataCombined.current = (uint32_t)escSensorDataCombined.current * getMotorCount() / usedEscSensorCount + escSensorConfig()->currentOffset; escSensorDataCombined.current = (uint32_t)escSensorDataCombined.current * getMotorCount() / usedEscSensorCount + escSensorConfig()->currentOffset;
escSensorDataCombined.voltage = (uint32_t)escSensorDataCombined.voltage / usedEscSensorCount; escSensorDataCombined.voltage = (uint32_t)escSensorDataCombined.voltage / usedEscSensorCount;
escSensorDataCombined.erpm = (uint32_t)escSensorDataCombined.erpm / usedEscSensorCount; escSensorDataCombined.rpm = computeRpm((float)escSensorDataCombined.rpm / usedEscSensorCount);
} }
else { else {
escSensorDataCombined.dataAge = ESC_DATA_INVALID; escSensorDataCombined.dataAge = ESC_DATA_INVALID;
@ -191,6 +207,8 @@ bool escSensorInitialize(void)
escSensorData[i].dataAge = ESC_DATA_INVALID; escSensorData[i].dataAge = ESC_DATA_INVALID;
} }
ENABLE_STATE(ESC_SENSOR_ENABLED);
return true; return true;
} }

5
src/main/sensors/esc_sensor.h
View file

@ -29,7 +29,7 @@ typedef struct {
int8_t temperature; int8_t temperature;
int16_t voltage; int16_t voltage;
int32_t current; int32_t current;
int16_t erpm; int16_t rpm;
} escSensorData_t; } escSensorData_t;
typedef struct escSensorConfig_s { typedef struct escSensorConfig_s {
@ -40,7 +40,10 @@ PG_DECLARE(escSensorConfig_t, escSensorConfig);
#define ESC_DATA_MAX_AGE 10 #define ESC_DATA_MAX_AGE 10
#define ESC_DATA_INVALID 255 #define ESC_DATA_INVALID 255
#define ERPM_PER_LSB 100.0f
bool escSensorInitialize(void); bool escSensorInitialize(void);
void escSensorUpdate(timeUs_t currentTimeUs); void escSensorUpdate(timeUs_t currentTimeUs);
escSensorData_t * escSensorGetData(void); escSensorData_t * escSensorGetData(void);
escSensorData_t * getEscTelemetry(uint8_t esc);
uint32_t computeRpm(int16_t erpm);

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

@ -68,6 +68,7 @@
#include "sensors/sensors.h" #include "sensors/sensors.h"
#include "flight/gyroanalyse.h" #include "flight/gyroanalyse.h"
#include "flight/rpm_filter.h"
#ifdef USE_HARDWARE_REVISION_DETECTION #ifdef USE_HARDWARE_REVISION_DETECTION
#include "hardware_revision.h" #include "hardware_revision.h"
@ -84,20 +85,13 @@ STATIC_FASTRAM int32_t gyroADC[XYZ_AXIS_COUNT];
STATIC_FASTRAM filterApplyFnPtr softLpfFilterApplyFn; STATIC_FASTRAM filterApplyFnPtr softLpfFilterApplyFn;
STATIC_FASTRAM void *softLpfFilter[XYZ_AXIS_COUNT]; STATIC_FASTRAM void *softLpfFilter[XYZ_AXIS_COUNT];
#ifdef USE_GYRO_NOTCH_1
STATIC_FASTRAM filterApplyFnPtr notchFilter1ApplyFn; STATIC_FASTRAM filterApplyFnPtr notchFilter1ApplyFn;
STATIC_FASTRAM void *notchFilter1[XYZ_AXIS_COUNT]; STATIC_FASTRAM void *notchFilter1[XYZ_AXIS_COUNT];
#endif
#ifdef USE_GYRO_NOTCH_2
STATIC_FASTRAM filterApplyFnPtr notchFilter2ApplyFn; STATIC_FASTRAM filterApplyFnPtr notchFilter2ApplyFn;
STATIC_FASTRAM void *notchFilter2[XYZ_AXIS_COUNT]; STATIC_FASTRAM void *notchFilter2[XYZ_AXIS_COUNT];
#endif
#if defined(USE_GYRO_BIQUAD_RC_FIR2)
// gyro biquad RC FIR2 filter
STATIC_FASTRAM filterApplyFnPtr gyroFilterStage2ApplyFn; STATIC_FASTRAM filterApplyFnPtr gyroFilterStage2ApplyFn;
STATIC_FASTRAM void *stage2Filter[XYZ_AXIS_COUNT]; STATIC_FASTRAM void *stage2Filter[XYZ_AXIS_COUNT];
#endif
#ifdef USE_DYNAMIC_FILTERS #ifdef USE_DYNAMIC_FILTERS
@ -337,16 +331,13 @@ void gyroInitFilters(void)
{ {
STATIC_FASTRAM filter_t gyroFilterLPF[XYZ_AXIS_COUNT]; STATIC_FASTRAM filter_t gyroFilterLPF[XYZ_AXIS_COUNT];
softLpfFilterApplyFn = nullFilterApply; softLpfFilterApplyFn = nullFilterApply;
#ifdef USE_GYRO_NOTCH_1
STATIC_FASTRAM biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT]; STATIC_FASTRAM biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT];
notchFilter1ApplyFn = nullFilterApply; notchFilter1ApplyFn = nullFilterApply;
#endif
#ifdef USE_GYRO_NOTCH_2
STATIC_FASTRAM biquadFilter_t gyroFilterNotch_2[XYZ_AXIS_COUNT]; STATIC_FASTRAM biquadFilter_t gyroFilterNotch_2[XYZ_AXIS_COUNT];
notchFilter2ApplyFn = nullFilterApply; notchFilter2ApplyFn = nullFilterApply;
#endif
#ifdef USE_GYRO_BIQUAD_RC_FIR2
STATIC_FASTRAM biquadFilter_t gyroFilterStage2[XYZ_AXIS_COUNT]; STATIC_FASTRAM biquadFilter_t gyroFilterStage2[XYZ_AXIS_COUNT];
gyroFilterStage2ApplyFn = nullFilterApply; gyroFilterStage2ApplyFn = nullFilterApply;
@ -357,7 +348,6 @@ void gyroInitFilters(void)
biquadRCFIR2FilterInit(stage2Filter[axis], gyroConfig()->gyro_stage2_lowpass_hz, getLooptime()); biquadRCFIR2FilterInit(stage2Filter[axis], gyroConfig()->gyro_stage2_lowpass_hz, getLooptime());
} }
} }
#endif
if (gyroConfig()->gyro_soft_lpf_hz) { if (gyroConfig()->gyro_soft_lpf_hz) {
@ -380,7 +370,6 @@ void gyroInitFilters(void)
} }
} }
#ifdef USE_GYRO_NOTCH_1
if (gyroConfig()->gyro_soft_notch_hz_1) { if (gyroConfig()->gyro_soft_notch_hz_1) {
notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply; notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply;
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
@ -388,9 +377,7 @@ void gyroInitFilters(void)
biquadFilterInitNotch(notchFilter1[axis], getLooptime(), gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1); biquadFilterInitNotch(notchFilter1[axis], getLooptime(), gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1);
} }
} }
#endif
#ifdef USE_GYRO_NOTCH_2
if (gyroConfig()->gyro_soft_notch_hz_2) { if (gyroConfig()->gyro_soft_notch_hz_2) {
notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply; notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
@ -398,7 +385,6 @@ void gyroInitFilters(void)
biquadFilterInitNotch(notchFilter2[axis], getLooptime(), gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2); biquadFilterInitNotch(notchFilter2[axis], getLooptime(), gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2);
} }
} }
#endif
} }
void gyroStartCalibration(void) void gyroStartCalibration(void)
@ -479,50 +465,25 @@ void FAST_CODE NOINLINE gyroUpdate()
DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf)); DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf));
#ifdef USE_DYNAMIC_FILTERS #ifdef USE_RPM_FILTER
if (isDynamicFilterActive()) { DEBUG_SET(DEBUG_RPM_FILTER, axis, gyroADCf);
if (axis == FD_ROLL) { gyroADCf = rpmFilterGyroApply(axis, gyroADCf);
DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf)); DEBUG_SET(DEBUG_RPM_FILTER, axis + 3, gyroADCf);
DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf));
}
}
#endif #endif
if (axis < 2) {
DEBUG_SET(DEBUG_STAGE2, axis, lrintf(gyroADCf));
}
#ifdef USE_GYRO_BIQUAD_RC_FIR2
gyroADCf = gyroFilterStage2ApplyFn(stage2Filter[axis], gyroADCf); gyroADCf = gyroFilterStage2ApplyFn(stage2Filter[axis], gyroADCf);
#endif
if (axis < 2) {
DEBUG_SET(DEBUG_STAGE2, axis + 2, lrintf(gyroADCf));
}
gyroADCf = softLpfFilterApplyFn(softLpfFilter[axis], gyroADCf); gyroADCf = softLpfFilterApplyFn(softLpfFilter[axis], gyroADCf);
DEBUG_SET(DEBUG_NOTCH, axis, lrintf(gyroADCf));
#ifdef USE_GYRO_NOTCH_1
gyroADCf = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf); gyroADCf = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf);
#endif
#ifdef USE_GYRO_NOTCH_2
gyroADCf = notchFilter2ApplyFn(notchFilter2[axis], gyroADCf); gyroADCf = notchFilter2ApplyFn(notchFilter2[axis], gyroADCf);
#endif
gyro.gyroADCf[axis] = gyroADCf;
#ifdef USE_DYNAMIC_FILTERS #ifdef USE_DYNAMIC_FILTERS
if (isDynamicFilterActive()) { if (isDynamicFilterActive()) {
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf));
DEBUG_SET(DEBUG_FFT_FREQ, 2, lrintf(gyroADCf));
}
gyroDataAnalysePush(&gyroAnalyseState, axis, gyroADCf); gyroDataAnalysePush(&gyroAnalyseState, axis, gyroADCf);
gyroADCf = notchFilterDynApplyFn((filter_t *)&notchFilterDyn[axis], gyroADCf); gyroADCf = notchFilterDynApplyFn((filter_t *)&notchFilterDyn[axis], gyroADCf);
gyroADCf = notchFilterDynApplyFn2((filter_t *)&notchFilterDyn2[axis], gyroADCf); gyroADCf = notchFilterDynApplyFn2((filter_t *)&notchFilterDyn2[axis], gyroADCf);
} }
#endif #endif
gyro.gyroADCf[axis] = gyroADCf;
} }
#ifdef USE_DYNAMIC_FILTERS #ifdef USE_DYNAMIC_FILTERS
@ -530,6 +491,7 @@ void FAST_CODE NOINLINE gyroUpdate()
gyroDataAnalyse(&gyroAnalyseState, notchFilterDyn, notchFilterDyn2); gyroDataAnalyse(&gyroAnalyseState, notchFilterDyn, notchFilterDyn2);
} }
#endif #endif
} }
bool gyroReadTemperature(void) bool gyroReadTemperature(void)

3
src/main/target/AIRBOTF4/target.h
View file

@ -158,4 +158,5 @@
#define PCA9685_I2C_BUS BUS_I2C2 #define PCA9685_I2C_BUS BUS_I2C2
#define USE_DSHOT #define USE_DSHOT
#define USE_ESC_SENSOR

0
src/main/target/ALIENFLIGHTF3/target.mk → src/main/target/ALIENFLIGHTF3/target.mk_
View file

0
src/main/target/ALIENFLIGHTF4/target.mk → src/main/target/ALIENFLIGHTF4/target.mk_
View file

0
src/main/target/ANYFCM7/target.mk → src/main/target/ANYFCM7/target.mk_
View file

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

@ -161,6 +161,7 @@
// Number of available PWM outputs // Number of available PWM outputs
#define USE_DSHOT #define USE_DSHOT
#define USE_ESC_SENSOR
#define MAX_PWM_OUTPUT_PORTS 4 #define MAX_PWM_OUTPUT_PORTS 4
#define TARGET_MOTOR_COUNT 4 #define TARGET_MOTOR_COUNT 4

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

@ -166,6 +166,7 @@
// Number of available PWM outputs // Number of available PWM outputs
#define USE_DSHOT #define USE_DSHOT
#define USE_ESC_SENSOR
#define MAX_PWM_OUTPUT_PORTS 4 #define MAX_PWM_OUTPUT_PORTS 4
#define TARGET_MOTOR_COUNT 4 #define TARGET_MOTOR_COUNT 4

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