Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-20 14:55:18 +03:00
Code Issues Wiki Activity

Merge remote-tracking branch 'upstream/master' into abo_fixed_wing_soaring_mode

Browse source
This commit is contained in:
breadoven 2021-10-28 20:31:47 +01:00
commit 9ce4e2758f
261 changed files with 4827 additions and 4478 deletions
Download patch file Download diff file Expand all files Collapse all files

2
CMakeLists.txt
View file

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

17
Notes.md
View file

@ -1,17 +0,0 @@
#Notes
## Expected acc/mag/gyro behavior
Observations of a flight-tested flip32 board in the configurator's 'raw sensor data' tab.
lift front of board (i.e. pitch back)
gyro y increases (green)
acc x increases (blue)
acc z decreases (red)
mag x decreases (blue)
lift left of boar (i.e. roll right)
gyro x increases (blue)
acc y increases (green)
mag y decreases (green)
mag z increases (red)

10
README.md
View file

@ -2,6 +2,13 @@
![INAV](http://static.rcgroups.net/forums/attachments/6/1/0/3/7/6/a9088858-102-inav.png)
# INAV Community
* [INAV Discord Server](https://discord.gg/peg2hhbYwN)
* [INAV Official on Facebook](https://www.facebook.com/groups/INAVOfficial)
* [RC Groups Support](https://www.rcgroups.com/forums/showthread.php?2495732-Cleanflight-iNav-(navigation-rewrite)-project)
* [INAV Official on Telegram](https://t.me/INAVFlight)
## Features
* Runs on the most popular F4 and F7 flight controllers
@ -47,9 +54,6 @@ See: https://github.com/iNavFlight/inav/blob/master/docs/Installation.md
* [Wing Tuning Masterclass](docs/INAV_Wing_Tuning_Masterclass.pdf)
* [Official documentation](https://github.com/iNavFlight/inav/tree/master/docs)
* [Official Wiki](https://github.com/iNavFlight/inav/wiki)
* [INAV Official on Telegram](https://t.me/INAVFlight)
* [INAV Official on Facebook](https://www.facebook.com/groups/INAVOfficial)
* [RC Groups Support](https://www.rcgroups.com/forums/showthread.php?2495732-Cleanflight-iNav-(navigation-rewrite)-project)
* [Video series by Painless360](https://www.youtube.com/playlist?list=PLYsWjANuAm4qdXEGFSeUhOZ10-H8YTSnH)
* [Video series by Paweł Spychalski](https://www.youtube.com/playlist?list=PLOUQ8o2_nCLloACrA6f1_daCjhqY2x0fB)

12
cmake/arm-none-eabi-checks.cmake
View file

@ -2,13 +2,13 @@ include(gcc)
set(arm_none_eabi_triplet "arm-none-eabi")
# Keep version in sync with the distribution files below
set(arm_none_eabi_gcc_version "9.3.1")
set(arm_none_eabi_base_url "https://developer.arm.com/-/media/Files/downloads/gnu-rm/9-2020q2/gcc-arm-none-eabi-9-2020-q2-update")
set(arm_none_eabi_gcc_version "10.2.1")
set(arm_none_eabi_base_url "https://developer.arm.com/-/media/Files/downloads/gnu-rm/10-2020q4/gcc-arm-none-eabi-10-2020-q4-major")
# suffix and checksum
set(arm_none_eabi_win32 "win32.zip" 184b3397414485f224e7ba950989aab6)
set(arm_none_eabi_linux_amd64 "x86_64-linux.tar.bz2" 2b9eeccc33470f9d3cda26983b9d2dc6)
set(arm_none_eabi_linux_aarch64 "aarch64-linux.tar.bz2" 000b0888cbe7b171e2225b29be1c327c)
set(arm_none_eabi_gcc_macos "mac.tar.bz2" 75a171beac35453fd2f0f48b3cb239c3)
set(arm_none_eabi_win32 "win32.zip" 5ee6542a2af847934177bc8fa1294c0d)
set(arm_none_eabi_linux_amd64 "x86_64-linux.tar.bz2" 8312c4c91799885f222f663fc81f9a31)
set(arm_none_eabi_linux_aarch64 "aarch64-linux.tar.bz2" 1c3b8944c026d50362eef1f01f329a8e)
set(arm_none_eabi_gcc_macos "mac.tar.bz2" e588d21be5a0cc9caa60938d2422b058)
function(arm_none_eabi_gcc_distname var)
string(REPLACE "/" ";" url_parts ${arm_none_eabi_base_url})

2
cmake/settings.cmake
View file

@ -33,7 +33,7 @@ function(enable_settings exe name)
add_custom_command(
OUTPUT ${output}
COMMAND
${CMAKE_COMMAND} -E env CFLAGS="${cflags}" TARGET=${name} PATH=$ENV{PATH} SETTINGS_CXX=${args_SETTINGS_CXX}
${CMAKE_COMMAND} -E env CFLAGS="${cflags}" TARGET=${name} PATH="$ENV{PATH}" SETTINGS_CXX=${args_SETTINGS_CXX}
${RUBY_EXECUTABLE} ${SETTINGS_GENERATOR} ${MAIN_DIR} ${SETTINGS_FILE} -o "${dir}"
DEPENDS ${SETTINGS_GENERATOR} ${SETTINGS_FILE}
)

4
cmake/stm32.cmake
View file

@ -189,7 +189,7 @@ endfunction()
function(add_hex_target name exe hex)
add_custom_target(${name} ALL
cmake -E env PATH=$ENV{PATH}
cmake -E env PATH="$ENV{PATH}"
# TODO: Overriding the start address with --set-start 0x08000000
# seems to be required due to some incorrect assumptions about .hex
# files in the configurator. Verify wether that's the case and fix
@ -201,7 +201,7 @@ endfunction()
function(add_bin_target name exe bin)
add_custom_target(${name}
cmake -E env PATH=$ENV{PATH}
cmake -E env PATH="$ENV{PATH}"
${CMAKE_OBJCOPY} -Obinary $<TARGET_FILE:${exe}> ${bin}
BYPRODUCTS ${bin}
)

2
cmake/stm32f3.cmake
View file

@ -19,6 +19,8 @@ main_sources(STM32F3_SRC
target/system_stm32f30x.c
config/config_streamer_stm32f3.c
config/config_streamer_ram.c
config/config_streamer_extflash.c
drivers/adc_stm32f30x.c
drivers/bus_i2c_stm32f30x.c

7
cmake/stm32f4.cmake
View file

@ -36,6 +36,8 @@ main_sources(STM32F4_SRC
target/system_stm32f4xx.c
config/config_streamer_stm32f4.c
config/config_streamer_ram.c
config/config_streamer_extflash.c
drivers/adc_stm32f4xx.c
drivers/adc_stm32f4xx.c
@ -124,6 +126,7 @@ exclude_basenames(STM32F411_OR_F427_STDPERIPH_SRC ${STM32F411_OR_F427_STDPERIPH_
set(STM32F411_COMPILE_DEFINITIONS
STM32F411xE
MCU_FLASH_SIZE=512
OPTIMIZATION -Os
)
function(target_stm32f411xe name)
@ -132,9 +135,9 @@ function(target_stm32f411xe name)
STARTUP startup_stm32f411xe.s
SOURCES ${STM32F411_OR_F427_STDPERIPH_SRC}
COMPILE_DEFINITIONS ${STM32F411_COMPILE_DEFINITIONS}
LINKER_SCRIPT stm32_flash_f411xe
LINKER_SCRIPT stm32_flash_f411xe
SVD STM32F411
${ARGN}
${ARGN}
)
endfunction()

2
cmake/stm32f7.cmake
View file

@ -61,6 +61,8 @@ main_sources(STM32F7_SRC
target/system_stm32f7xx.c
config/config_streamer_stm32f7.c
config/config_streamer_ram.c
config/config_streamer_extflash.c
drivers/adc_stm32f7xx.c
drivers/bus_i2c_hal.c

2
cmake/stm32h7.cmake
View file

@ -142,6 +142,8 @@ main_sources(STM32H7_SRC
target/system_stm32h7xx.c
config/config_streamer_stm32h7.c
config/config_streamer_ram.c
config/config_streamer_extflash.c
drivers/adc_stm32h7xx.c
drivers/bus_i2c_hal.c

4
docs/Boards.md
View file

@ -30,7 +30,7 @@ These boards are well tested with INAV and are known to be of good quality and r
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._
See the [docs/boards](https://github.com/iNavFlight/inav/tree/master/docs/boards) 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
@ -38,7 +38,7 @@ These boards are powerful and in general support everything INAV is capable of.
### 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 STM32F3 MCUs are no longer supported by latest INAV version. Last release is 2.6.1.
### Boards based on F1 CPUs

1
docs/Controls.md
View file

@ -39,6 +39,7 @@ The stick positions are combined to activate different functions:
| Save setting | LOW | LOW | LOW | HIGH |
| Enter OSD Menu (CMS) | CENTER | LOW | HIGH | CENTER |
For graphical stick position in all transmitter modes, check out [this page](https://www.mrd-rc.com/tutorials-tools-and-testing/inav-flight/inav-stick-commands-for-all-transmitter-modes/).
![Stick Positions](assets/images/StickPositions.png)
## Yaw control

25
docs/Failsafe.md
View file

@ -9,12 +9,13 @@ Failsafe is a state the flight controller is meant to enter when the radio recei
If the failsafe happens while the flight controller is disarmed, it only prevent arming. If it happens while armed, the failsafe policy configured in `failsafe_procedure` is engaged. The available procedures are:
* __DROP:__ Just kill the motors and disarm (crash the craft).
* __SET-THR:__ Enable auto-level mode (for multirotor) or enter preconfigured roll/pitch/yaw spiral down (for airplanes) and set the throttle to a predefined value (`failsafe_throttle`) for a predefined time (`failsafe_off_delay`). This is meant to get the craft to a safe-ish landing (or more realistically, a controlled crash), it doesn't require any extra sensor other than gyros and accelerometers.
* __RTH:__ (Return To Home) One of the key features of inav, it automatically navigates the craft back to the home position and (optionally) lands it. Similarly to all other automated navigation methods, it requires GPS for any type of craft, plus compass and barometer for multicopters.
* __LAND:__ (replaces **SET-THR** from INAV V3.1.0) Performs an Emergency Landing. Enables auto-level mode (for multirotor) or enters a preconfigured roll/pitch/yaw spiral down (for airplanes). If altitude sensors are working an actively controlled descent is performed using the Emergency Landing descent speed (`nav_emerg_landing_speed`). If altitude sensors are unavailable descent is performed with the throttle set to a predefined value (`failsafe_throttle`). The descent can be limited to a predefined time (`failsafe_off_delay`) after which the craft disarms. This is meant to get the craft to a safe-ish landing (or more realistically, a controlled crash), Other than using altitude sensors for an actively controlled descent it doesn't require any extra sensors other than gyros and accelerometers.
* __SET-THR:__ (Pre INAV V3.1.0) Same as **LAND** except it doesn't use an Emergency Landing but is limited instead to just setting the throttle to a predefined value (`failsafe_throttle`) to perform a descent. It doesn't require any extra sensors other than gyros and accelerometers.
* __RTH:__ (Return To Home) One of the key features of INAV, it automatically navigates the craft back to the home position and (optionally) lands it. Similarly to all other automated navigation methods, it requires GPS for any type of craft, plus compass and barometer for multicopters.
* __NONE:__ Do nothing. This is meant to let the craft perform a fully automated flight (eg. waypoint flight) outside of radio range. Highly unsafe when used with manual flight.
Note that:
* Should the failsafe disarm the flight controller (**DROP**, **SET-THR** after `failsafe_off_delay` or **RTH** with `nav_disarm_on_landing` ON), the flight controller will be disarmed and re-arming will be locked until the signal from the receiver is restored for 30 seconds AND the arming switch is in the OFF position (when an arm switch is in use).
* Should the failsafe disarm the flight controller (**DROP**, **LAND/SET-THR** after `failsafe_off_delay` or **RTH** with `nav_disarm_on_landing` ON), the flight controller will be disarmed and re-arming will be locked until the signal from the receiver is restored for 30 seconds AND the arming switch is in the OFF position (when an arm switch is in use).
* Prior to starting failsafe it is checked if the throttle position has been below `min_throttle` for the last `failsafe_throttle_low_delay` seconds. If it was, the craft is assumed to be on the ground and is simply disarmed. This feature can be disabled completely by setting `failsafe_throttle_low_delay` to zero, which may be necessary to do if the craft may fly long with zero throttle (eg. gliders).
@ -22,9 +23,9 @@ Note that:
* If the craft is landed but armed, the failsafe may make the motors and props spin again and even make the craft take off (in case of **RTH** failsafe). Take expecially care of this when using `MOTOR_STOP` feature. **Props will spin up without warning**. Have a look at the `failsafe_throttle_low_delay` setting explained above to learn when this could happen.
* If any required navigation sensor becomes unavailable during a Return to Home (eg. loss of GPS fix), an emergency landing similar to **SET-THR** but with barometer support will be performed after a short timeout. An emergency landing would also be performed right when the failsafe is triggered if any required sensor is reported as unavailable.
* If any required navigation sensor becomes unavailable during a Return to Home (eg. loss of GPS fix), an emergency landing, as used by the **LAND** procedure, will be performed after a short timeout. An emergency landing would also be performed right when the failsafe is triggered if any required sensor is reported as unavailable.
* **SET-THR** doesn't control the descend in any other way than setting a fixed throttle. Thus, appropriate testing must be performed to find the right throttle value. Consider that a constant throttle setting will yield different thrusts depending on battery voltage, so when you evaluate the throttle value do it with a loaded battery. Failure to do so may cause a flyaway.
* The **SET-THR** procedure doesn't control descent in any way other than setting a fixed throttle. This is also the case for the **LAND** procedure when altitude sensors are unavailable. Thus, appropriate testing must be performed to find the right throttle value. Consider that a constant throttle setting will yield different thrusts depending on battery voltage, so when you evaluate the throttle value do it with a loaded battery. Failure to do so may cause a flyaway.
* When the failsafe mode is aborted (RC signal restored/failsafe switch set to OFF), the current stick positions will be enforced immediately. Be ready to react quickly.
@ -48,7 +49,7 @@ Failsafe delays are configured in 0.1 second units. Distances are in centimeters
#### `failsafe_procedure`
Selects the failsafe procedure. Valid procedures are **DROP**, **SET-THR**, **RTH** and **NONE**. See above for a description of each one.
Selects the failsafe procedure. Valid procedures are **DROP**, **LAND/SET-THR**, **RTH** and **NONE**. See above for a description of each one.
#### `failsafe_delay`
@ -122,25 +123,29 @@ Enables landing when home position is reached. If OFF the craft will hover indef
Instructs the flight controller to disarm the craft when landing is detected
### Parameters relevant to **SET-THR** failsafe procedure
### Parameters relevant to **LAND/SET-THR** failsafe procedure
#### `failsafe_off_delay`
Delay after failsafe activates before motors finally turn off. This is the amount of time 'failsafe_throttle' is active. If you fly at higher altitudes you may need more time to descend safely. Set to zero to keep `failsafe_throttle` active indefinitely.
#### `nav_emerg_landing_speed`
(**LAND** only) Actively controlled descent speed when altitude sensors are available. If altitude sensors aren't available landing descent falls back to using the fixed thottle setting `failsafe_throttle` so ensure this is also set correctly.
#### `failsafe_throttle`
Throttle level used for landing. Specify a value that causes the aircraft to descend at about 1M/sec. Default is set to 1000 which should correspond to throttle off.
#### `failsafe_fw_roll_angle`
This parameter defines amount of roll angle (in 1/10 deg units) to execute on failsafe. Negative = LEFT
This parameter defines the amount of roll angle (in 1/10 deg units) to execute on failsafe. Negative = LEFT
#### `failsafe_fw_pitch_angle`
This parameter defines amount of pitch angle (in 1/10 deg units) to execute on `SET-THR` failsafe for an airplane. Negative = CLIMB
This parameter defines the amount of pitch angle (in 1/10 deg units) to execute on failsafe for an airplane. Negative = CLIMB
#### `failsafe_fw_yaw_rate`
This parameter defines amount of yaw rate (in deg per second units) to execute on `SET-THR` failsafe for an airplane. Negative = LEFT
This parameter defines the amount of yaw rate (in deg per second units) to execute on failsafe for an airplane. Negative = LEFT

6
docs/Programming Framework.md
View file

@ -74,6 +74,7 @@ IPF can be edited using INAV Configurator user interface, of via CLI
| 38 | RC_CHANNEL_OVERRIDE | Overrides channel set by `Operand A` to value of `Operand B` |
| 39 | SET_HEADING_TARGET | Sets heading-hold target to `Operand A`, in degrees. Value wraps-around. |
| 40 | MOD | Divide `Operand A` by `Operand B` and returns the remainder |
| 41 | LOITER_RADIUS_OVERRIDE | Sets the loiter radius to `Operand A` [`0` : `100000`] in cm. If the value is lower than the loiter radius set in the **Advanced Tuning**, that will be used. |
### Operands
@ -125,8 +126,9 @@ IPF can be edited using INAV Configurator user interface, of via CLI
| 30 | ACTIVE_WAYPOINT_ACTION | See ACTIVE_WAYPOINT_ACTION paragraph |
| 31 | 3D HOME_DISTANCE | in `meters`, calculated from HOME_DISTANCE and ALTITUDE using Pythagorean theorem |
| 32 | CROSSFIRE LQ | Crossfire Link quality as returned by the CRSF protocol |
| 33 | CROSSFIRE SNR | Crossfire SNR as returned by the CRSF protocol |
| 34 | GPS_VALID | boolean `0`/`1`. True when the GPS has a valid 3D Fix |
| 33 | CROSSFIRE SNR | Crossfire SNR as returned by the CRSF protocol |
| 34 | GPS_VALID | boolean `0`/`1`. True when the GPS has a valid 3D Fix |
| 35 | LOITER_RADIUS | The current loiter radius in cm. |
#### ACTIVE_WAYPOINT_ACTION

6
docs/Rangefinder.md
View file

@ -14,19 +14,17 @@ Current support of rangefinders in INAV is very limited. They are used only to:
Following rangefinders are supported:
* HC-SR04 - ***DO NOT USE*** HC-SR04 while most popular is not suited to use in noise reach environments (like multirotors). It is proven that this sonar rangefinder start to report random values already at 1.5m above solid concrete surface. Reported altitude is valid up to only 75cm above concrete. ***DO NOT USE***
* US-100 in trigger-echo mode - Can be used as direct replacement of _HC-SR04_ when `rangefinder_hardware` is set to _HCSR04_. Useful up to 2m over concrete and correctly reporting _out of range_ when out of range
* SRF10 - experimental
* INAV_I2C - is a simple [DIY rangefinder interface with Arduino Pro Mini 3.3V](https://github.com/iNavFlight/inav-rangefinder). Can be used to connect when flight controller has no Trigger-Echo ports.
* VL53L0X - simple laser rangefinder usable up to 75cm
* UIB - experimental
* MSP - experimental
* TOF10120 - small & lightweight laser range sensor, usable up to 200cm
## Connections
Target dependent in case of Trigger/Echo solutions like `HC-SR04` and `US-100`.
I2C solutions like `VL53L0X` or `INAV_I2C` can be connected to I2C port and used as any other I2C device.
#### Constraints
Target dependent in case of Trigger/Echo solutions like `HC-SR04` and `US-100`. No constrains for I2C like `VL53L0X` or `INAV_I2C`.
iNav does not support `HC-SR04` and `US-100`. No constrains for I2C like `VL53L0X` or `INAV_I2C`.

206
docs/Settings.md
View file

@ -572,16 +572,6 @@ ADC , VIRTUAL, NONE. The virtual current sensor, once calibrated, estimates the
---
### d_boost_factor
_// TODO_
| Default | Min | Max |
| --- | --- | --- |
| 1.25 | 1 | 3 |
---
### d_boost_gyro_delta_lpf_hz
_// TODO_
@ -592,6 +582,16 @@ _// TODO_
---
### d_boost_max
_// TODO_
| Default | Min | Max |
| --- | --- | --- |
| 1.25 | 1 | 3 |
---
### d_boost_max_at_acceleration
_// TODO_
@ -602,6 +602,16 @@ _// TODO_
---
### d_boost_min
_// TODO_
| Default | Min | Max |
| --- | --- | --- |
| 0.5 | 0 | 1 |
---
### deadband
These are values (in us) by how much RC input can be different before it's considered valid. For transmitters with jitter on outputs, this value can be increased. Defaults are zero, but can be increased up to 10 or so if rc inputs twitch while idle.
@ -744,11 +754,11 @@ Cutoff frequency for stage 2 D-term low pass filter
### dterm_lpf2_type
Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation.
Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`, `PT2`, `PT3`.
| Default | Min | Max |
| --- | --- | --- |
| BIQUAD | | |
| PT1 | | |
---
@ -758,17 +768,17 @@ Dterm low pass filter cutoff frequency. Default setting is very conservative and
| Default | Min | Max |
| --- | --- | --- |
| 40 | 0 | 500 |
| 110 | 0 | 500 |
---
### dterm_lpf_type
Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation.
Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`, `PT2`, `PT3`.
| Default | Min | Max |
| --- | --- | --- |
| BIQUAD | | |
| PT2 | | |
---
@ -778,17 +788,17 @@ Enable/disable dynamic gyro notch also known as Matrix Filter
| Default | Min | Max |
| --- | --- | --- |
| OFF | | |
| ON | | |
---
### dynamic_gyro_notch_min_hz
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`
Minimum frequency for dynamic notches. Default value of `150` works best with 5" multirotors. Should be lowered with increased size of propellers. Values around `100` work fine on 7" drones. 10" can go down to `60` - `70`
| Default | Min | Max |
| --- | --- | --- |
| 150 | 30 | 1000 |
| 50 | 30 | 1000 |
---
@ -802,16 +812,6 @@ Q factor for dynamic notches
---
### dynamic_gyro_notch_range
Range for dynamic gyro notches. `MEDIUM` for 5", `HIGH` for 3" and `MEDIUM`/`LOW` for 7" and bigger propellers
| Default | Min | Max |
| --- | --- | --- |
| MEDIUM | | |
---
### eleres_freq
_// TODO_
@ -904,7 +904,7 @@ Time in deciseconds to wait before activating failsafe when signal is lost. See
### failsafe_fw_pitch_angle
Amount of dive/climb when `SET-THR` failsafe is active on a fixed-wing machine. In 1/10 deg (deci-degrees). Negative values = climb
Amount of dive/climb when `LAND` (or old `SET-THR`) failsafe is active on a fixed-wing machine. In 1/10 deg (deci-degrees). Negative values = climb
| Default | Min | Max |
| --- | --- | --- |
@ -914,7 +914,7 @@ Amount of dive/climb when `SET-THR` failsafe is active on a fixed-wing machine.
### failsafe_fw_roll_angle
Amount of banking when `SET-THR` failsafe is active on a fixed-wing machine. In 1/10 deg (deci-degrees). Negative values = left roll
Amount of banking when `LAND` (or old `SET-THR`) failsafe is active on a fixed-wing machine. In 1/10 deg (deci-degrees). Negative values = left roll
| Default | Min | Max |
| --- | --- | --- |
@ -924,7 +924,7 @@ Amount of banking when `SET-THR` failsafe is active on a fixed-wing machine. In
### failsafe_fw_yaw_rate
Requested yaw rate to execute when `SET-THR` failsafe is active on a fixed-wing machine. In deg/s. Negative values = left turn
Requested yaw rate to execute when `LAND` (or old `SET-THR`) failsafe is active on a fixed-wing machine. In deg/s. Negative values = left turn
| Default | Min | Max |
| --- | --- | --- |
@ -1008,7 +1008,7 @@ What failsafe procedure to initiate in Stage 2. See [Failsafe documentation](Fai
| Default | Min | Max |
| --- | --- | --- |
| SET-THR | | |
| LAND | | |
---
@ -1532,36 +1532,6 @@ Enable use of Galileo satellites. This is at the expense of other regional const
---
### gyro_abg_alpha
Alpha factor for Gyro Alpha-Beta-Gamma filter
| Default | Min | Max |
| --- | --- | --- |
| 0 | 0 | 1 |
---
### gyro_abg_boost
Boost factor for Gyro Alpha-Beta-Gamma filter
| Default | Min | Max |
| --- | --- | --- |
| 0.35 | 0 | 2 |
---
### gyro_abg_half_life
Sample half-life for Gyro Alpha-Beta-Gamma filter
| Default | Min | Max |
| --- | --- | --- |
| 0.5 | 0 | 10 |
---
### gyro_anti_aliasing_lpf_hz
Gyro processing anti-aliasing filter cutoff frequency. In normal operation this filter setting should never be changed. In Hz
@ -1668,7 +1638,7 @@ _// TODO_
| Default | Min | Max |
| --- | --- | --- |
| 0 | 0 | 1 |
| 0 | 0 | 2 |
---
@ -1938,7 +1908,7 @@ Inertial Measurement Unit KP Gain for accelerometer measurements
| Default | Min | Max |
| --- | --- | --- |
| 2500 | | 65535 |
| 1000 | | 65535 |
---
@ -1948,7 +1918,7 @@ Inertial Measurement Unit KP Gain for compass measurements
| Default | Min | Max |
| --- | --- | --- |
| 10000 | | 65535 |
| 5000 | | 65535 |
---
@ -2468,7 +2438,7 @@ Exposition value used for the PITCH/ROLL axes by the `MANUAL` flight mode [0-100
| Default | Min | Max |
| --- | --- | --- |
| 70 | 0 | 100 |
| 35 | 0 | 100 |
---
@ -2842,26 +2812,6 @@ When powering up, gyro bias is calculated. If the model is shaking/moving during
---
### motor_accel_time
Minimum time for the motor(s) to accelerate from 0 to 100% throttle (ms) [0-1000]
| Default | Min | Max |
| --- | --- | --- |
| 0 | 0 | 1000 |
---
### motor_decel_time
Minimum time for the motor(s) to deccelerate from 100 to 0% throttle (ms) [0-1000]
| Default | Min | Max |
| --- | --- | --- |
| 0 | 0 | 1000 |
---
### motor_direction_inverted
Use if you need to inverse yaw motor direction.
@ -4152,6 +4102,16 @@ Value above which to make the OSD distance from home indicator blink (meters)
---
### osd_esc_rpm_precision
Number of characters used to display the RPM value.
| Default | Min | Max |
| --- | --- | --- |
| 3 | 3 | 6 |
---
### osd_esc_temp_alarm_max
Temperature above which the IMU temperature OSD element will start blinking (decidegrees centigrade)
@ -4572,6 +4532,16 @@ Display minimum voltage of the `BATTERY` or the average per `CELL` in the OSD st
---
### osd_stats_page_auto_swap_time
Auto swap display time interval between disarm stats pages (seconds). Reverts to manual control when Roll stick used to change pages. Disabled when set to 0.
| Default | Min | Max |
| --- | --- | --- |
| 3 | 0 | 10 |
---
### osd_telemetry
To enable OSD telemetry for antenna tracker. Possible values are `OFF`, `ON` and `TEST`
@ -4802,6 +4772,66 @@ Limits acceleration of YAW rotation speed that can be requested by stick input.
---
### rate_dynamics_center_correction
The center stick correction for Rate Dynamics
| Default | Min | Max |
| --- | --- | --- |
| 10 | 10 | 95 |
---
### rate_dynamics_center_sensitivity
The center stick sensitivity for Rate Dynamics
| Default | Min | Max |
| --- | --- | --- |
| 100 | 25 | 175 |
---
### rate_dynamics_center_weight
The center stick weight for Rate Dynamics
| Default | Min | Max |
| --- | --- | --- |
| 0 | 0 | 95 |
---
### rate_dynamics_end_correction
The end stick correction for Rate Dynamics
| Default | Min | Max |
| --- | --- | --- |
| 10 | 10 | 95 |
---
### rate_dynamics_end_sensitivity
The end stick sensitivity for Rate Dynamics
| Default | Min | Max |
| --- | --- | --- |
| 100 | 25 | 175 |
---
### rate_dynamics_end_weight
The end stick weight for Rate Dynamics
| Default | Min | Max |
| --- | --- | --- |
| 0 | 0 | 95 |
---
### rc_expo
Exposition value used for the PITCH/ROLL axes by all the stabilized flights modes (all but `MANUAL`)
@ -5148,7 +5178,7 @@ Enable Kalman filter on the gyro data
| Default | Min | Max |
| --- | --- | --- |
| OFF | | |
| ON | | |
---
@ -5674,7 +5704,7 @@ These are values (in us) by how much RC input can be different before it's consi
### yaw_lpf_hz
Yaw low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below)
Yaw P term low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below)
| Default | Min | Max |
| --- | --- | --- |

1
docs/Telemetry.md
View file

@ -121,6 +121,7 @@ The following sensors are transmitted
* **0430** : if `frsky_pitch_roll = ON` set this will be pitch degrees*10
* **0440** : if `frsky_pitch_roll = ON` set this will be roll degrees*10
* **0450** : 'Flight Path Vector' or 'Course over ground' in degrees*10
* **0460** : Azimuth in degrees*10
### Compatible SmartPort/INAV telemetry flight status
To quickly and easily monitor these SmartPort sensors and flight modes, install [OpenTX Telemetry Widget](https://github.com/iNavFlight/OpenTX-Telemetry-Widget) to your Taranis Q X7, X9D, X9D+ or X9E transmitter.

0
docs/Board - Airbot F4 and Flip32 F4.md → docs/boards/Airbot F4 and Flip32 F4.md
View file

0
docs/Board - AlienFlight.md → docs/boards/AlienFlight.md
View file

2
docs/Board - AnyFC F7.md → docs/boards/AnyFC F7.md
View file

@ -71,7 +71,7 @@ Airplanes
Buzzer is supported with additional switching MOSFET transistor when connected to PWM Output #9.
![Buzzer](assets/images/anyfcf7_buzzer_pwm9.png)
![Buzzer](../assets/images/anyfcf7_buzzer_pwm9.png)
## SD Card Detection

0
docs/Board - BeeRotor F4.md → docs/boards/BeeRotor F4.md
View file

2
docs/Board - BetFPVF722.md → docs/boards/BetFPVF722.md
View file

@ -9,4 +9,4 @@ BetFPVF722 does not have I2C pads broken out. I2C is shared with UART3
> I2C and UART3 can not be used at the same time! Connect the serial receiver to a different serial port when I2C device like MAG is used.
![BetaFPVF722 I2C](assets/betafpvf722.png)
![BetaFPVF722 I2C](../assets/betafpvf722.png)

0
docs/Board - DALRCF405.md → docs/boards/DALRCF405.md
View file

0
docs/Board - DALRCF722DUAL.md → docs/boards/DALRCF722DUAL.md
View file

0
docs/Board - FLYWOOF411.md → docs/boards/FLYWOOF411.md Executable file → Normal file
View file

0
docs/Board - FOXEERF405.md → docs/boards/FOXEERF405.md
View file

0
docs/Board - FOXEERF722DUAL.md → docs/boards/FOXEERF722DUAL.md
View file

0
docs/Board - FrSky Pilot.md → docs/boards/FrSky Pilot.md
View file

0
docs/Board - IFLIGHTF7_TWING.md → docs/boards/IFLIGHTF7_TWING.md
View file

0
docs/Board - KroozX.md → docs/boards/KroozX.md Executable file → Normal file
View file

0
docs/Board - Matek F411 Wing.md → docs/boards/Matek F411 Wing.md
View file

0
docs/Board - Matek F411-WSE.md → docs/boards/Matek F411-WSE.md
View file

0
docs/Board - Matek F722-STD.md → docs/boards/Matek F722-STD.md
View file

2
docs/Board - MatekF405 Wing.md → docs/boards/MatekF405 Wing.md
View file

@ -1,7 +1,5 @@
# Board - [MATEKSYS F405-Wing](https://inavflight.com/shop/p/MATEKF405WING)
![Matek F405 Wing](https://quadmeup.com/wp-content/uploads/2018/12/DSC_0007.jpg)
## Specification:
* STM32F405 CPU

0
docs/Board - MatekF405.md → docs/boards/MatekF405.md
View file

0
docs/Board - MatekF722-SE.md → docs/boards/MatekF722-SE.md
View file

0
docs/Board - MatekF722.md → docs/boards/MatekF722.md
View file

0
docs/Board - MatekF765-Wing.md → docs/boards/MatekF765-Wing.md
View file

0
docs/Board - NOX.md → docs/boards/NOX.md Executable file → Normal file
View file

14
docs/Board - Omnibus F4.md → docs/boards/Omnibus F4.md
View file

@ -155,14 +155,14 @@ This board allows for single **SoftwareSerial** port on small soldering pads loc
Please note that this is *not* the motor PWM5/PWM6 pins, but small surface mount pads CH5/CH6.
### Omnibus F4 Pro SoftwareSerial Connections
![Omnibus F4 Pro SoftwareSerial Connections](assets/images/omnibusf4pro_ss.jpg)
![Omnibus F4 Pro SoftwareSerial Connections](../assets/images/omnibusf4pro_ss.jpg)
| Pad | SoftwareSerial Role |
| ---- | ---- |
| CH5 | RX |
| CH6 | TX |
![Omnibus F4 Pro SmartPort using SoftwareSerial](assets/images/omnibusf4pro_ss.png)
![Omnibus F4 Pro SmartPort using SoftwareSerial](../assets/images/omnibusf4pro_ss.png)
### Omnibus F4 v3/v4/v5 SoftwareSerial Connections
@ -181,22 +181,22 @@ Following diagrams applies to _Pro_ version with integrated current meter and JS
## Board layout
![Omnibus F4 Pro Board Layout](assets/images/omnibusf4pro.png)
![Omnibus F4 Pro Board Layout](../assets/images/omnibusf4pro.png)
## Flying wing motor and servos
![Omnibus F4 Pro Flying Wing Setup](assets/images/omnibusf4pro_flyingwing_setup.png)
![Omnibus F4 Pro Flying Wing Setup](../assets/images/omnibusf4pro_flyingwing_setup.png)
## RX setup
![Omnibus F4 Pro RX Setup](assets/images/omnibusf4pro_rx.png)
![Omnibus F4 Pro RX Setup](../assets/images/omnibusf4pro_rx.png)
## FPV setup
![Omnibus F4 Pro FPV Setup](assets/images/omnibusf4pro_fpv_setup.png)
![Omnibus F4 Pro FPV Setup](../assets/images/omnibusf4pro_fpv_setup.png)
## GPS setup
![Omnibus F4 Pro GPS Setup](assets/images/omnibusf4pro_gps_setup.png)
![Omnibus F4 Pro GPS Setup](../assets/images/omnibusf4pro_gps_setup.png)
_Diagrams created by Albert Kravcov (skaman82)_

0
docs/Board - Omnibus F7.md → docs/boards/Omnibus F7.md
View file

0
docs/Board - PixRacer R14.md → docs/boards/PixRacer R14.md
View file

0
docs/Board - Revolution.md → docs/boards/Revolution.md
View file

0
docs/Board - WingFC.md → docs/boards/WingFC.md
View file

0
docs/Board - YuPiF4.md → docs/boards/YuPiF4.md
View file

51
docs/development/Atomic Barrier.md
View file

@ -1,51 +0,0 @@
# Atomic Barrier Warning
If GCC is upgraded and a warning appears when compiling then the generated asm source must be verified.
e.g.
```
%% serial_softserial.c
warning "Please verify that ATOMIC_BARRIER works as intended"
```
To perform the verification, proceed as per discusson on issue #167 which reads:
I hope it's enough to check that optimized-away variable still has cleanup code at end of scope.
```
static int markme=0;
markme++;
ATOMIC_BLOCK_NB(0xff) {
ATOMIC_BARRIER(markme);
markme++;
};
markme++;
```
pass `-save-temps=obj` (or `-save-temps=cwd`, but lots of files will end up in same directory as makefile) to gcc link step (LTO is in use), find resulting `*.ltrans*.ltrans.s` (grep for `markme`, on linux it ends up in `/tmp`) and check that generated assembly sequence is:
```
MSR basepri_max, r3
# (possibly markme address load)
# barier (markme) start
# (increment markme, load and store to memory)
ldr r2, [r3]
adds r0, r2, #1
str r0, [r3]
# barier(markme) end
MSR basepri, r3
# (markme value should be cached in register on next increment)
```
The # barrier(markme) must surround access code and must be inside MSR basepri instructions ..
Similar approach is used for ATOMIC_BLOCK in avr libraries, so gcc should not break this behavior.
IMO attribute(cleanup) and asm volatile is defined in a way that should guarantee this.
attribute(cleanup) is probably safer way to implement atomic sections - another possibility is to explicitly place barriers in code, but that can (and will eventually) lead to missed barrier/basepri restore on same path creating very hard to find bug.
The MEMORY_BARRIER() code can be omitted and use ATOMIC_BLOCK with full memory barriers, but IMO it is better to explicitly state what memory is protected by barrier and gcc can use this knowledge to greatly improve generated code in future.

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

@ -138,3 +138,28 @@ appendWindowsPath=false
8. `cd build`
9. `cmake ..`
9. `make {TARGET}` should be working again
### Building targets is very slow
I was pretty shocked when my new i7 -10750 laptop took 25 minutes to build a single target. My old i3-4030 could do the same job in about 2.5 minutes. If you're also suffering from slow builds. Open an elevated PowerShell window and type
```
wsl -l -v
```
If you see your Linux distribution is using WSL 2, this is the problem. WSL 2 is quicker than WSL 1 for a lot of things. However, if your files are on a windows mounted drive in Linux, it is extremely slow. There are two options:
1. Put your files on the Linux file system
2. Change to WSL 1
#### Using the Linux file system (recommended)
To use the Linux file system, make sure the distro is running. Open File Explorer and navigate to `\\wsl$`. In that path you will find your distros listed. At this point, map a network drive to your distro. Inside the distro, you can find your home directory at `/home/~username~/`. Create your GitHub folders here.
If after this you have problems with writing to the directories from within VSCode. Open the application for your distro and type
```
sudo chown -R ~username~ GitHub
```
`~Username~` is your root distro user that you created and `GitHub` should be the root folder for your GitHub repositories.
#### To switch back to WSL 1
To do this, in the elevated PowerShell window, you can see the name of your distro. Mine is **Ubuntu-20.04**, so I'll use that in this example. Simply type
```
wsl --set-version Ubuntu-20.04 1
```
and your distro will be converted to WSL 1. Once finished, reboot your system. Next time you compile a build, it will be faster.

33
src/main/CMakeLists.txt
View file

@ -71,26 +71,23 @@ main_sources(COMMON_SRC
drivers/accgyro/accgyro.c
drivers/accgyro/accgyro.h
drivers/accgyro/accgyro_adxl345.c
drivers/accgyro/accgyro_adxl345.h
drivers/accgyro/accgyro_bma280.c
drivers/accgyro/accgyro_bma280.h
drivers/accgyro/accgyro_bmi088.c
drivers/accgyro/accgyro_bmi088.h
drivers/accgyro/accgyro_bmi160.c
drivers/accgyro/accgyro_bmi160.h
drivers/accgyro/accgyro_bmi270.c
drivers/accgyro/accgyro_bmi270.h
drivers/accgyro/accgyro_bmi270_maximum_fifo.c
drivers/accgyro/accgyro_fake.c
drivers/accgyro/accgyro_fake.h
drivers/accgyro/accgyro_icm20689.c
drivers/accgyro/accgyro_icm20689.h
drivers/accgyro/accgyro_l3g4200d.c
drivers/accgyro/accgyro_l3g4200d.h
drivers/accgyro/accgyro_icm42605.c
drivers/accgyro/accgyro_icm42605.h
drivers/accgyro/accgyro_l3gd20.c
drivers/accgyro/accgyro_l3gd20.h
drivers/accgyro/accgyro_lsm303dlhc.c
drivers/accgyro/accgyro_lsm303dlhc.h
drivers/accgyro/accgyro_mma845x.c
drivers/accgyro/accgyro_mma845x.h
drivers/accgyro/accgyro_mpu.c
drivers/accgyro/accgyro_mpu.h
drivers/accgyro/accgyro_mpu3050.c
@ -162,6 +159,10 @@ main_sources(COMMON_SRC
drivers/compass/compass_qmc5883l.h
drivers/compass/compass_rm3100.c
drivers/compass/compass_rm3100.h
drivers/compass/compass_vcm5883.c
drivers/compass/compass_vcm5883.h
drivers/compass/compass_mlx90393.c
drivers/compass/compass_mlx90393.h
drivers/compass/compass_msp.c
drivers/compass/compass_msp.h
@ -228,10 +229,6 @@ main_sources(COMMON_SRC
drivers/pinio.c
drivers/pinio.h
drivers/rangefinder/rangefinder_hcsr04.c
drivers/rangefinder/rangefinder_hcsr04.h
drivers/rangefinder/rangefinder_hcsr04_i2c.c
drivers/rangefinder/rangefinder_hcsr04_i2c.h
drivers/rangefinder/rangefinder_srf10.c
drivers/rangefinder/rangefinder_srf10.h
drivers/rangefinder/rangefinder_vl53l0x.c
@ -242,13 +239,13 @@ main_sources(COMMON_SRC
drivers/rangefinder/rangefinder_virtual.h
drivers/rangefinder/rangefinder_us42.c
drivers/rangefinder/rangefinder_us42.h
drivers/rangefinder/rangefinder_tof10120_i2c.c
drivers/rangefinder/rangefinder_tof10120_i2c.h
drivers/resource.c
drivers/resource.h
drivers/rcc.c
drivers/rcc.h
drivers/rx_pwm.c
drivers/rx_pwm.h
drivers/rx_spi.c
drivers/rx_spi.h
drivers/serial.c
@ -323,6 +320,8 @@ main_sources(COMMON_SRC
flight/kalman.h
flight/smith_predictor.c
flight/smith_predictor.h
flight/rate_dynamics.c
flight/rate_dynamics.h
flight/mixer.c
flight/mixer.h
flight/pid.c
@ -349,8 +348,6 @@ main_sources(COMMON_SRC
io/beeper.c
io/beeper.h
io/esc_serialshot.c
io/esc_serialshot.h
io/servo_sbus.c
io/servo_sbus.h
io/frsky_osd.c
@ -411,8 +408,6 @@ main_sources(COMMON_SRC
rx/msp.h
rx/msp_override.c
rx/msp_override.h
rx/pwm.c
rx/pwm.h
rx/frsky_crc.c
rx/frsky_crc.h
rx/rx.c
@ -581,6 +576,8 @@ main_sources(COMMON_SRC
telemetry/ghst.h
telemetry/hott.c
telemetry/hott.h
telemetry/jetiexbus.c
telemetry/jetiexbus.h
telemetry/ibus_shared.c
telemetry/ibus_shared.h
telemetry/ibus.c

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

@ -97,15 +97,32 @@
#define BLACKBOX_INVERTED_CARD_DETECTION 0
#endif
PG_REGISTER_WITH_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 1);
PG_REGISTER_WITH_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 2);
PG_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig,
.device = DEFAULT_BLACKBOX_DEVICE,
.rate_num = SETTING_BLACKBOX_RATE_NUM_DEFAULT,
.rate_denom = SETTING_BLACKBOX_RATE_DENOM_DEFAULT,
.invertedCardDetection = BLACKBOX_INVERTED_CARD_DETECTION,
.includeFlags = BLACKBOX_FEATURE_NAV_PID | BLACKBOX_FEATURE_NAV_POS |
BLACKBOX_FEATURE_MAG | BLACKBOX_FEATURE_ACC | BLACKBOX_FEATURE_ATTITUDE |
BLACKBOX_FEATURE_RC_DATA | BLACKBOX_FEATURE_RC_COMMAND | BLACKBOX_FEATURE_MOTORS,
);
void blackboxIncludeFlagSet(uint32_t mask)
{
blackboxConfigMutable()->includeFlags |= mask;
}
void blackboxIncludeFlagClear(uint32_t mask)
{
blackboxConfigMutable()->includeFlags &= ~(mask);
}
bool blackboxIncludeFlag(uint32_t mask) {
return (blackboxConfig()->includeFlags & mask) == mask;
}
#define BLACKBOX_SHUTDOWN_TIMEOUT_MILLIS 200
static const int32_t blackboxSInterval = 4096;
@ -199,6 +216,9 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
{"axisD", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_0)},
{"axisD", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_1)},
{"axisD", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_2)},
{"axisF", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ALWAYS},
{"axisF", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ALWAYS},
{"axisF", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ALWAYS},
{"fwAltP", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)},
{"fwAltI", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)},
@ -233,16 +253,16 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
{"mcSurfaceOut",-1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
/* rcData are encoded together as a group: */
{"rcData", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
{"rcData", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
{"rcData", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
{"rcData", 3, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
{"rcData", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), FLIGHT_LOG_FIELD_CONDITION_RC_DATA},
{"rcData", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), FLIGHT_LOG_FIELD_CONDITION_RC_DATA},
{"rcData", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), FLIGHT_LOG_FIELD_CONDITION_RC_DATA},
{"rcData", 3, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), FLIGHT_LOG_FIELD_CONDITION_RC_DATA},
/* rcCommands are encoded together as a group in P-frames: */
{"rcCommand", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
{"rcCommand", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
{"rcCommand", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
{"rcCommand", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), FLIGHT_LOG_FIELD_CONDITION_RC_COMMAND},
{"rcCommand", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), FLIGHT_LOG_FIELD_CONDITION_RC_COMMAND},
{"rcCommand", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), FLIGHT_LOG_FIELD_CONDITION_RC_COMMAND},
/* Throttle is always in the range [minthrottle..maxthrottle]: */
{"rcCommand", 3, UNSIGNED, .Ipredict = PREDICT(MINTHROTTLE), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
{"rcCommand", 3, UNSIGNED, .Ipredict = PREDICT(MINTHROTTLE), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), FLIGHT_LOG_FIELD_CONDITION_RC_COMMAND},
{"vbat", -1, UNSIGNED, .Ipredict = PREDICT(VBATREF), .Iencode = ENCODING(NEG_14BIT), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_VBAT},
{"amperage", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_AMPERAGE},
@ -267,12 +287,12 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
{"gyroADC", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"gyroADC", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"gyroADC", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"accSmooth", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"accSmooth", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"accSmooth", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"attitude", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"attitude", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"attitude", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"accSmooth", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC},
{"accSmooth", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC},
{"accSmooth", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC},
{"attitude", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ATTITUDE},
{"attitude", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ATTITUDE},
{"attitude", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ATTITUDE},
{"debug", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_DEBUG},
{"debug", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_DEBUG},
{"debug", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_DEBUG},
@ -310,28 +330,26 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
{"servo", 14, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
{"servo", 15, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
#ifdef NAV_BLACKBOX
{"navState", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"navFlags", -1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"navEPH", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"navEPV", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"navPos", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"navPos", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"navPos", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"navVel", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"navVel", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"navVel", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"navAcc", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"navAcc", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"navAcc", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"navTgtVel", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"navTgtVel", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"navTgtVel", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"navTgtPos", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"navTgtPos", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"navTgtPos", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"navSurf", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
#endif
{"navEPH", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
{"navEPV", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
{"navPos", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
{"navPos", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
{"navPos", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
{"navVel", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
{"navVel", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
{"navVel", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
{"navTgtVel", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
{"navTgtVel", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
{"navTgtVel", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
{"navTgtPos", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
{"navTgtPos", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
{"navTgtPos", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
{"navSurf", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
{"navAcc", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_ACC},
{"navAcc", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_ACC},
{"navAcc", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_ACC},
};
#ifdef USE_GPS
@ -422,6 +440,7 @@ typedef struct blackboxMainState_s {
int32_t axisPID_P[XYZ_AXIS_COUNT];
int32_t axisPID_I[XYZ_AXIS_COUNT];
int32_t axisPID_D[XYZ_AXIS_COUNT];
int32_t axisPID_F[XYZ_AXIS_COUNT];
int32_t axisPID_Setpoint[XYZ_AXIS_COUNT];
int32_t mcPosAxisP[XYZ_AXIS_COUNT];
@ -461,7 +480,6 @@ typedef struct blackboxMainState_s {
int32_t surfaceRaw;
#endif
uint16_t rssi;
#ifdef NAV_BLACKBOX
int16_t navState;
uint16_t navFlags;
uint16_t navEPH;
@ -474,7 +492,6 @@ typedef struct blackboxMainState_s {
int16_t navHeading;
int16_t navTargetHeading;
int16_t navSurface;
#endif
} blackboxMainState_t;
typedef struct blackboxGpsState_s {
@ -579,6 +596,9 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
case FLIGHT_LOG_FIELD_CONDITION_ALWAYS:
return true;
case FLIGHT_LOG_FIELD_CONDITION_MOTORS:
return blackboxIncludeFlag(BLACKBOX_FEATURE_MOTORS);
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1:
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_2:
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_3:
@ -587,7 +607,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_6:
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_7:
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_8:
return getMotorCount() >= condition - FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1 + 1;
return (getMotorCount() >= condition - FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1 + 1) && blackboxIncludeFlag(BLACKBOX_FEATURE_MOTORS);
case FLIGHT_LOG_FIELD_CONDITION_SERVOS:
return isMixerUsingServos();
@ -596,11 +616,11 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1:
case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_2:
// D output can be set by either the D or the FF term
return pidBank()->pid[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0].D != 0 || pidBank()->pid[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0].FF != 0;
return pidBank()->pid[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0].D != 0;
case FLIGHT_LOG_FIELD_CONDITION_MAG:
#ifdef USE_MAG
return sensors(SENSOR_MAG);
return sensors(SENSOR_MAG) && blackboxIncludeFlag(BLACKBOX_FEATURE_MAG);
#else
return false;
#endif
@ -634,14 +654,14 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
case FLIGHT_LOG_FIELD_CONDITION_FIXED_WING_NAV:
#ifdef USE_NAV
return STATE(FIXED_WING_LEGACY);
return STATE(FIXED_WING_LEGACY) && blackboxIncludeFlag(BLACKBOX_FEATURE_NAV_PID);
#else
return false;
#endif
case FLIGHT_LOG_FIELD_CONDITION_MC_NAV:
#ifdef USE_NAV
return !STATE(FIXED_WING_LEGACY);
return !STATE(FIXED_WING_LEGACY) && blackboxIncludeFlag(BLACKBOX_FEATURE_NAV_PID);
#else
return false;
#endif
@ -657,9 +677,28 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
case FLIGHT_LOG_FIELD_CONDITION_DEBUG:
return debugMode != DEBUG_NONE;
case FLIGHT_LOG_FIELD_CONDITION_NAV_ACC:
return blackboxIncludeFlag(BLACKBOX_FEATURE_NAV_ACC);
case FLIGHT_LOG_FIELD_CONDITION_NAV_POS:
return blackboxIncludeFlag(BLACKBOX_FEATURE_NAV_POS);
case FLIGHT_LOG_FIELD_CONDITION_ACC:
return blackboxIncludeFlag(BLACKBOX_FEATURE_ACC);
case FLIGHT_LOG_FIELD_CONDITION_ATTITUDE:
return blackboxIncludeFlag(BLACKBOX_FEATURE_ATTITUDE);
case FLIGHT_LOG_FIELD_CONDITION_RC_COMMAND:
return blackboxIncludeFlag(BLACKBOX_FEATURE_RC_COMMAND);
case FLIGHT_LOG_FIELD_CONDITION_RC_DATA:
return blackboxIncludeFlag(BLACKBOX_FEATURE_RC_DATA);
case FLIGHT_LOG_FIELD_CONDITION_NEVER:
return false;
default:
return false;
}
@ -733,6 +772,7 @@ static void writeIntraframe(void)
blackboxWriteSignedVB(blackboxCurrent->axisPID_D[x]);
}
}
blackboxWriteSignedVBArray(blackboxCurrent->axisPID_F, XYZ_AXIS_COUNT);
if (testBlackboxCondition(CONDITION(FIXED_WING_NAV))) {
blackboxWriteSignedVBArray(blackboxCurrent->fwAltPID, 3);
@ -756,16 +796,20 @@ static void writeIntraframe(void)
}
// Write raw stick positions
blackboxWriteSigned16VBArray(blackboxCurrent->rcData, 4);
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_RC_DATA)) {
blackboxWriteSigned16VBArray(blackboxCurrent->rcData, 4);
}
// Write roll, pitch and yaw first:
blackboxWriteSigned16VBArray(blackboxCurrent->rcCommand, 3);
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_RC_COMMAND)) {
blackboxWriteSigned16VBArray(blackboxCurrent->rcCommand, 3);
/*
* Write the throttle separately from the rest of the RC data so we can apply a predictor to it.
* Throttle lies in range [minthrottle..maxthrottle]:
*/
blackboxWriteUnsignedVB(blackboxCurrent->rcCommand[THROTTLE] - getThrottleIdleValue());
/*
* Write the throttle separately from the rest of the RC data so we can apply a predictor to it.
* Throttle lies in range [minthrottle..maxthrottle]:
*/
blackboxWriteUnsignedVB(blackboxCurrent->rcCommand[THROTTLE] - getThrottleIdleValue());
}
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {
/*
@ -811,20 +855,28 @@ static void writeIntraframe(void)
}
blackboxWriteSigned16VBArray(blackboxCurrent->gyroADC, XYZ_AXIS_COUNT);
blackboxWriteSigned16VBArray(blackboxCurrent->accADC, XYZ_AXIS_COUNT);
blackboxWriteSigned16VBArray(blackboxCurrent->attitude, XYZ_AXIS_COUNT);
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ACC)) {
blackboxWriteSigned16VBArray(blackboxCurrent->accADC, XYZ_AXIS_COUNT);
}
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ATTITUDE)) {
blackboxWriteSigned16VBArray(blackboxCurrent->attitude, XYZ_AXIS_COUNT);
}
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_DEBUG)) {
blackboxWriteSignedVBArray(blackboxCurrent->debug, DEBUG32_VALUE_COUNT);
}
//Motors can be below minthrottle when disarmed, but that doesn't happen much
blackboxWriteUnsignedVB(blackboxCurrent->motor[0] - getThrottleIdleValue());
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_MOTORS)) {
//Motors can be below minthrottle when disarmed, but that doesn't happen much
blackboxWriteUnsignedVB(blackboxCurrent->motor[0] - getThrottleIdleValue());
//Motors tend to be similar to each other so use the first motor's value as a predictor of the others
const int motorCount = getMotorCount();
for (int x = 1; x < motorCount; x++) {
blackboxWriteSignedVB(blackboxCurrent->motor[x] - blackboxCurrent->motor[0]);
//Motors tend to be similar to each other so use the first motor's value as a predictor of the others
const int motorCount = getMotorCount();
for (int x = 1; x < motorCount; x++) {
blackboxWriteSignedVB(blackboxCurrent->motor[x] - blackboxCurrent->motor[0]);
}
}
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_SERVOS)) {
@ -834,36 +886,41 @@ static void writeIntraframe(void)
}
}
#ifdef NAV_BLACKBOX
blackboxWriteSignedVB(blackboxCurrent->navState);
blackboxWriteSignedVB(blackboxCurrent->navFlags);
blackboxWriteSignedVB(blackboxCurrent->navEPH);
blackboxWriteSignedVB(blackboxCurrent->navEPV);
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
blackboxWriteSignedVB(blackboxCurrent->navPos[x]);
/*
* NAV_POS fields
*/
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NAV_POS)) {
blackboxWriteSignedVB(blackboxCurrent->navEPH);
blackboxWriteSignedVB(blackboxCurrent->navEPV);
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
blackboxWriteSignedVB(blackboxCurrent->navPos[x]);
}
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
blackboxWriteSignedVB(blackboxCurrent->navRealVel[x]);
}
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
blackboxWriteSignedVB(blackboxCurrent->navTargetVel[x]);
}
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
blackboxWriteSignedVB(blackboxCurrent->navTargetPos[x]);
}
blackboxWriteSignedVB(blackboxCurrent->navSurface);
}
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
blackboxWriteSignedVB(blackboxCurrent->navRealVel[x]);
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NAV_ACC)) {
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
blackboxWriteSignedVB(blackboxCurrent->navAccNEU[x]);
}
}
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
blackboxWriteSignedVB(blackboxCurrent->navAccNEU[x]);
}
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
blackboxWriteSignedVB(blackboxCurrent->navTargetVel[x]);
}
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
blackboxWriteSignedVB(blackboxCurrent->navTargetPos[x]);
}
blackboxWriteSignedVB(blackboxCurrent->navSurface);
#endif
//Rotate our history buffers:
//The current state becomes the new "before" state
@ -943,6 +1000,9 @@ static void writeInterframe(void)
}
}
arraySubInt32(deltas, blackboxCurrent->axisPID_F, blackboxLast->axisPID_F, XYZ_AXIS_COUNT);
blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT);
if (testBlackboxCondition(CONDITION(FIXED_WING_NAV))) {
arraySubInt32(deltas, blackboxCurrent->fwAltPID, blackboxLast->fwAltPID, 3);
@ -981,18 +1041,22 @@ static void writeInterframe(void)
*/
// rcData
for (int x = 0; x < 4; x++) {
deltas[x] = blackboxCurrent->rcData[x] - blackboxLast->rcData[x];
}
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_RC_DATA)) {
for (int x = 0; x < 4; x++) {
deltas[x] = blackboxCurrent->rcData[x] - blackboxLast->rcData[x];
}
blackboxWriteTag8_4S16(deltas);
blackboxWriteTag8_4S16(deltas);
}
// rcCommand
for (int x = 0; x < 4; x++) {
deltas[x] = blackboxCurrent->rcCommand[x] - blackboxLast->rcCommand[x];
}
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_RC_COMMAND)) {
for (int x = 0; x < 4; x++) {
deltas[x] = blackboxCurrent->rcCommand[x] - blackboxLast->rcCommand[x];
}
blackboxWriteTag8_4S16(deltas);
blackboxWriteTag8_4S16(deltas);
}
//Check for sensors that are updated periodically (so deltas are normally zero)
int optionalFieldCount = 0;
@ -1039,47 +1103,64 @@ static void writeInterframe(void)
//Since gyros, accs and motors are noisy, base their predictions on the average of the history:
blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, gyroADC), XYZ_AXIS_COUNT);
blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, accADC), XYZ_AXIS_COUNT);
blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, attitude), XYZ_AXIS_COUNT);
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ACC)) {
blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, accADC), XYZ_AXIS_COUNT);
}
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ATTITUDE)) {
blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, attitude), XYZ_AXIS_COUNT);
}
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_DEBUG)) {
blackboxWriteArrayUsingAveragePredictor32(offsetof(blackboxMainState_t, debug), DEBUG32_VALUE_COUNT);
}
blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, motor), getMotorCount());
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_MOTORS)) {
blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, motor), getMotorCount());
}
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_SERVOS)) {
blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, servo), MAX_SUPPORTED_SERVOS);
}
#ifdef NAV_BLACKBOX
blackboxWriteSignedVB(blackboxCurrent->navState - blackboxLast->navState);
blackboxWriteSignedVB(blackboxCurrent->navFlags - blackboxLast->navFlags);
blackboxWriteSignedVB(blackboxCurrent->navEPH - blackboxLast->navEPH);
blackboxWriteSignedVB(blackboxCurrent->navEPV - blackboxLast->navEPV);
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
blackboxWriteSignedVB(blackboxCurrent->navPos[x] - blackboxLast->navPos[x]);
/*
* NAV_POS fields
*/
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NAV_POS)) {
blackboxWriteSignedVB(blackboxCurrent->navEPH - blackboxLast->navEPH);
blackboxWriteSignedVB(blackboxCurrent->navEPV - blackboxLast->navEPV);
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
blackboxWriteSignedVB(blackboxCurrent->navPos[x] - blackboxLast->navPos[x]);
}
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
blackboxWriteSignedVB(blackboxHistory[0]->navRealVel[x] - (blackboxHistory[1]->navRealVel[x] + blackboxHistory[2]->navRealVel[x]) / 2);
}
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
blackboxWriteSignedVB(blackboxHistory[0]->navTargetVel[x] - (blackboxHistory[1]->navTargetVel[x] + blackboxHistory[2]->navTargetVel[x]) / 2);
}
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
blackboxWriteSignedVB(blackboxHistory[0]->navTargetPos[x] - blackboxLast->navTargetPos[x]);
}
blackboxWriteSignedVB(blackboxCurrent->navSurface - blackboxLast->navSurface);
}
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
blackboxWriteSignedVB(blackboxHistory[0]->navRealVel[x] - (blackboxHistory[1]->navRealVel[x] + blackboxHistory[2]->navRealVel[x]) / 2);
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NAV_ACC)) {
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
blackboxWriteSignedVB(blackboxHistory[0]->navAccNEU[x] - (blackboxHistory[1]->navAccNEU[x] + blackboxHistory[2]->navAccNEU[x]) / 2);
}
}
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
blackboxWriteSignedVB(blackboxHistory[0]->navAccNEU[x] - (blackboxHistory[1]->navAccNEU[x] + blackboxHistory[2]->navAccNEU[x]) / 2);
}
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
blackboxWriteSignedVB(blackboxHistory[0]->navTargetVel[x] - (blackboxHistory[1]->navTargetVel[x] + blackboxHistory[2]->navTargetVel[x]) / 2);
}
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
blackboxWriteSignedVB(blackboxHistory[0]->navTargetPos[x] - blackboxLast->navTargetPos[x]);
}
blackboxWriteSignedVB(blackboxCurrent->navSurface - blackboxLast->navSurface);
#endif
//Rotate our history buffers
blackboxHistory[2] = blackboxHistory[1];
blackboxHistory[1] = blackboxHistory[0];
@ -1396,6 +1477,7 @@ static void loadMainState(timeUs_t currentTimeUs)
blackboxCurrent->axisPID_P[i] = axisPID_P[i];
blackboxCurrent->axisPID_I[i] = axisPID_I[i];
blackboxCurrent->axisPID_D[i] = axisPID_D[i];
blackboxCurrent->axisPID_F[i] = axisPID_F[i];
blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i]);
blackboxCurrent->accADC[i] = lrintf(acc.accADCf[i] * acc.dev.acc_1G);
#ifdef USE_MAG
@ -1479,7 +1561,6 @@ static void loadMainState(timeUs_t currentTimeUs)
blackboxCurrent->servo[i] = servo[i];
}
#ifdef NAV_BLACKBOX
blackboxCurrent->navState = navCurrentState;
blackboxCurrent->navFlags = navFlags;
blackboxCurrent->navEPH = navEPH;
@ -1492,7 +1573,6 @@ static void loadMainState(timeUs_t currentTimeUs)
blackboxCurrent->navTargetPos[i] = navTargetPosition[i];
}
blackboxCurrent->navSurface = navActualSurface;
#endif
}
/**
@ -1690,15 +1770,18 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("rates", "%d,%d,%d", currentControlRateProfile->stabilized.rates[ROLL],
currentControlRateProfile->stabilized.rates[PITCH],
currentControlRateProfile->stabilized.rates[YAW]);
BLACKBOX_PRINT_HEADER_LINE("rollPID", "%d,%d,%d", pidBank()->pid[PID_ROLL].P,
BLACKBOX_PRINT_HEADER_LINE("rollPID", "%d,%d,%d,%d", pidBank()->pid[PID_ROLL].P,
pidBank()->pid[PID_ROLL].I,
pidBank()->pid[PID_ROLL].D);
BLACKBOX_PRINT_HEADER_LINE("pitchPID", "%d,%d,%d", pidBank()->pid[PID_PITCH].P,
pidBank()->pid[PID_ROLL].D,
pidBank()->pid[PID_ROLL].FF);
BLACKBOX_PRINT_HEADER_LINE("pitchPID", "%d,%d,%d,%d", pidBank()->pid[PID_PITCH].P,
pidBank()->pid[PID_PITCH].I,
pidBank()->pid[PID_PITCH].D);
BLACKBOX_PRINT_HEADER_LINE("yawPID", "%d,%d,%d", pidBank()->pid[PID_YAW].P,
pidBank()->pid[PID_PITCH].D,
pidBank()->pid[PID_PITCH].FF);
BLACKBOX_PRINT_HEADER_LINE("yawPID", "%d,%d,%d,%d", pidBank()->pid[PID_YAW].P,
pidBank()->pid[PID_YAW].I,
pidBank()->pid[PID_YAW].D);
pidBank()->pid[PID_YAW].D,
pidBank()->pid[PID_YAW].FF);
BLACKBOX_PRINT_HEADER_LINE("altPID", "%d,%d,%d", pidBank()->pid[PID_POS_Z].P,
pidBank()->pid[PID_POS_Z].I,
pidBank()->pid[PID_POS_Z].D);
@ -1726,7 +1809,6 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf_hz", "%d", gyroConfig()->gyro_main_lpf_hz);
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf_type", "%d", gyroConfig()->gyro_main_lpf_type);
#ifdef USE_DYNAMIC_FILTERS
BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchRange", "%d", gyroConfig()->dynamicGyroNotchRange);
BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchQ", "%d", gyroConfig()->dynamicGyroNotchQ);
BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchMinHz", "%d", gyroConfig()->dynamicGyroNotchMinHz);
#endif
@ -1749,9 +1831,7 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("motor_pwm_rate", "%d", motorConfig()->motorPwmRate);
BLACKBOX_PRINT_HEADER_LINE("debug_mode", "%d", systemConfig()->debug_mode);
BLACKBOX_PRINT_HEADER_LINE("features", "%d", featureConfig()->enabledFeatures);
#ifdef NAV_BLACKBOX
BLACKBOX_PRINT_HEADER_LINE("waypoints", "%d,%d", getWaypointCount(),isWaypointListValid());
#endif
BLACKBOX_PRINT_HEADER_LINE("acc_notch_hz", "%d", accelerometerConfig()->acc_notch_hz);
BLACKBOX_PRINT_HEADER_LINE("acc_notch_cutoff", "%d", accelerometerConfig()->acc_notch_cutoff);
BLACKBOX_PRINT_HEADER_LINE("pidSumLimit", "%d", pidProfile()->pidSumLimit);

17
src/main/blackbox/blackbox.h
View file

@ -21,11 +21,25 @@
#include "config/parameter_group.h"
typedef enum {
BLACKBOX_FEATURE_NAV_ACC = 1 << 0,
BLACKBOX_FEATURE_NAV_POS = 1 << 1,
BLACKBOX_FEATURE_NAV_PID = 1 << 2,
BLACKBOX_FEATURE_MAG = 1 << 3,
BLACKBOX_FEATURE_ACC = 1 << 4,
BLACKBOX_FEATURE_ATTITUDE = 1 << 5,
BLACKBOX_FEATURE_RC_DATA = 1 << 6,
BLACKBOX_FEATURE_RC_COMMAND = 1 << 7,
BLACKBOX_FEATURE_MOTORS = 1 << 8,
} blackboxFeatureMask_e;
typedef struct blackboxConfig_s {
uint16_t rate_num;
uint16_t rate_denom;
uint8_t device;
uint8_t invertedCardDetection;
uint32_t includeFlags;
} blackboxConfig_t;
PG_DECLARE(blackboxConfig_t, blackboxConfig);
@ -37,3 +51,6 @@ void blackboxUpdate(timeUs_t currentTimeUs);
void blackboxStart(void);
void blackboxFinish(void);
bool blackboxMayEditConfig(void);
void blackboxIncludeFlagSet(uint32_t mask);
void blackboxIncludeFlagClear(uint32_t mask);
bool blackboxIncludeFlag(uint32_t mask);

9
src/main/blackbox/blackbox_fielddefs.h
View file

@ -24,6 +24,7 @@
typedef enum FlightLogFieldCondition {
FLIGHT_LOG_FIELD_CONDITION_ALWAYS = 0,
FLIGHT_LOG_FIELD_CONDITION_MOTORS,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_2,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_3,
@ -52,6 +53,14 @@ typedef enum FlightLogFieldCondition {
FLIGHT_LOG_FIELD_CONDITION_DEBUG,
FLIGHT_LOG_FIELD_CONDITION_NAV_ACC,
FLIGHT_LOG_FIELD_CONDITION_NAV_POS,
FLIGHT_LOG_FIELD_CONDITION_NAV_PID,
FLIGHT_LOG_FIELD_CONDITION_ACC,
FLIGHT_LOG_FIELD_CONDITION_ATTITUDE,
FLIGHT_LOG_FIELD_CONDITION_RC_DATA,
FLIGHT_LOG_FIELD_CONDITION_RC_COMMAND,
FLIGHT_LOG_FIELD_CONDITION_NEVER,
FLIGHT_LOG_FIELD_CONDITION_FIRST = FLIGHT_LOG_FIELD_CONDITION_ALWAYS,

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

@ -53,8 +53,10 @@
#ifdef STM32H7
#define DMA_RAM __attribute__ ((section(".DMA_RAM")))
#define SLOW_RAM __attribute__ ((section(".SLOW_RAM")))
#else
#define DMA_RAM
#define SLOW_RAM
#endif
#define STATIC_FASTRAM static FASTRAM

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

@ -72,7 +72,6 @@ typedef enum {
DEBUG_DYNAMIC_FILTER,
DEBUG_DYNAMIC_FILTER_FREQUENCY,
DEBUG_IRLOCK,
DEBUG_CD,
DEBUG_KALMAN_GAIN,
DEBUG_PID_MEASUREMENT,
DEBUG_SPM_CELLS, // Smartport master FLVSS
@ -81,12 +80,11 @@ typedef enum {
DEBUG_PCF8574,
DEBUG_DYNAMIC_GYRO_LPF,
DEBUG_AUTOLEVEL,
DEBUG_FW_D,
DEBUG_IMU2,
DEBUG_ALTITUDE,
DEBUG_GYRO_ALPHA_BETA_GAMMA,
DEBUG_SMITH_PREDICTOR,
DEBUG_AUTOTRIM,
DEBUG_AUTOTUNE,
DEBUG_RATE_DYNAMICS,
DEBUG_COUNT
} debugType_e;

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

@ -432,7 +432,8 @@ static const OSD_Entry cmsx_menuMechanicsEntries[] =
{
OSD_LABEL_DATA_ENTRY("-- MECHANICS --", profileIndexString),
#ifdef USE_D_BOOST
OSD_SETTING_ENTRY("DBOOST_FACTOR", SETTING_D_BOOST_FACTOR),
OSD_SETTING_ENTRY("DBOOST_MIN", SETTING_D_BOOST_MIN),
OSD_SETTING_ENTRY("DBOOST_MAX", SETTING_D_BOOST_MAX),
#endif
#ifdef USE_ANTIGRAVITY
OSD_SETTING_ENTRY("ANTIGRAV. GAIN", SETTING_ANTIGRAVITY_GAIN),

8
src/main/cms/cms_menu_misc.c
View file

@ -43,12 +43,16 @@ static const OSD_Entry menuMiscEntries[]=
OSD_LABEL_ENTRY("-- MISC --"),
OSD_SETTING_ENTRY("THR IDLE", SETTING_THROTTLE_IDLE),
#if defined(USE_OSD) && defined(USE_ADC)
#ifdef USE_OSD
#ifdef USE_ADC
OSD_SETTING_ENTRY("OSD VOLT DECIMALS", SETTING_OSD_MAIN_VOLTAGE_DECIMALS),
OSD_SETTING_ENTRY("STATS ENERGY UNIT", SETTING_OSD_STATS_ENERGY_UNIT),
#endif
#endif // ADC
OSD_SETTING_ENTRY("STATS PAGE SWAP TIME", SETTING_OSD_STATS_PAGE_AUTO_SWAP_TIME),
#endif // OSD
OSD_SETTING_ENTRY("FS PROCEDURE", SETTING_FAILSAFE_PROCEDURE),
OSD_BACK_AND_END_ENTRY,
};

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

@ -18,7 +18,8 @@
#pragma once
#define FEET_PER_MILE 5280
#define FEET_PER_NAUTICALMILE 6076.118
#define FEET_PER_KILOFEET 1000 // Used for altitude
#define METERS_PER_KILOMETER 1000
#define METERS_PER_MILE 1609
#define METERS_PER_MILE 1609.344
#define METERS_PER_NAUTICALMILE 1852.001

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

@ -317,68 +317,35 @@ FAST_CODE void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint
filter->y2 = y2;
}
#ifdef USE_ALPHA_BETA_GAMMA_FILTER
void alphaBetaGammaFilterInit(alphaBetaGammaFilter_t *filter, float alpha, float boostGain, float halfLife, float dT) {
// beta, gamma, and eta gains all derived from
// http://yadda.icm.edu.pl/yadda/element/bwmeta1.element.baztech-922ff6cb-e991-417f-93f0-77448f1ef4ec/c/A_Study_Jeong_1_2017.pdf
FUNCTION_COMPILE_FOR_SIZE
void initFilter(const uint8_t filterType, filter_t *filter, const float cutoffFrequency, const uint32_t refreshRate) {
const float dT = refreshRate * 1e-6f;
const float xi = powf(-alpha + 1.0f, 0.25); // fourth rool of -a + 1
filter->xk = 0.0f;
filter->vk = 0.0f;
filter->ak = 0.0f;
filter->jk = 0.0f;
filter->a = alpha;
filter->b = (1.0f / 6.0f) * powf(1.0f - xi, 2) * (11.0f + 14.0f * xi + 11 * xi * xi);
filter->g = 2 * powf(1.0f - xi, 3) * (1 + xi);
filter->e = (1.0f / 6.0f) * powf(1 - xi, 4);
filter->dT = dT;
filter->dT2 = dT * dT;
filter->dT3 = dT * dT * dT;
pt1FilterInit(&filter->boostFilter, 100, dT);
const float boost = boostGain * 100;
filter->boost = (boost * boost / 10000) * 0.003;
filter->halfLife = halfLife != 0 ? powf(0.5f, dT / halfLife): 1.0f;
}
FAST_CODE float alphaBetaGammaFilterApply(alphaBetaGammaFilter_t *filter, float input) {
//xk - current system state (ie: position)
//vk - derivative of system state (ie: velocity)
//ak - derivative of system velociy (ie: acceleration)
//jk - derivative of system acceleration (ie: jerk)
float rk; // residual error
// give the filter limited history
filter->xk *= filter->halfLife;
filter->vk *= filter->halfLife;
filter->ak *= filter->halfLife;
filter->jk *= filter->halfLife;
// update our (estimated) state 'x' from the system (ie pos = pos + vel (last).dT)
filter->xk += filter->dT * filter->vk + (1.0f / 2.0f) * filter->dT2 * filter->ak + (1.0f / 6.0f) * filter->dT3 * filter->jk;
// update (estimated) velocity (also estimated dterm from measurement)
filter->vk += filter->dT * filter->ak + 0.5f * filter->dT2 * filter->jk;
filter->ak += filter->dT * filter->jk;
// what is our residual error (measured - estimated)
rk = input - filter->xk;
// artificially boost the error to increase the response of the filter
rk += pt1FilterApply(&filter->boostFilter, fabsf(rk) * rk * filter->boost);
if ((fabsf(rk * filter->a) > fabsf(input - filter->xk))) {
rk = (input - filter->xk) / filter->a;
if (cutoffFrequency) {
if (filterType == FILTER_PT1) {
pt1FilterInit(&filter->pt1, cutoffFrequency, dT);
} if (filterType == FILTER_PT2) {
pt2FilterInit(&filter->pt2, pt2FilterGain(cutoffFrequency, dT));
} if (filterType == FILTER_PT3) {
pt3FilterInit(&filter->pt3, pt3FilterGain(cutoffFrequency, dT));
} else {
biquadFilterInitLPF(&filter->biquad, cutoffFrequency, refreshRate);
}
}
filter->rk = rk; // for logging
// update our estimates given the residual error.
filter->xk += filter->a * rk;
filter->vk += filter->b / filter->dT * rk;
filter->ak += filter->g / (2.0f * filter->dT2) * rk;
filter->jk += filter->e / (6.0f * filter->dT3) * rk;
return filter->xk;
}
#endif
FUNCTION_COMPILE_FOR_SIZE
void assignFilterApplyFn(uint8_t filterType, float cutoffFrequency, filterApplyFnPtr *applyFn) {
*applyFn = nullFilterApply;
if (cutoffFrequency) {
if (filterType == FILTER_PT1) {
*applyFn = (filterApplyFnPtr) pt1FilterApply;
} if (filterType == FILTER_PT2) {
*applyFn = (filterApplyFnPtr) pt2FilterApply;
} if (filterType == FILTER_PT3) {
*applyFn = (filterApplyFnPtr) pt3FilterApply;
} else {
*applyFn = (filterApplyFnPtr) biquadFilterApply;
}
}
}

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

@ -48,11 +48,15 @@ typedef struct biquadFilter_s {
typedef union {
biquadFilter_t biquad;
pt1Filter_t pt1;
pt2Filter_t pt2;
pt3Filter_t pt3;
} filter_t;
typedef enum {
FILTER_PT1 = 0,
FILTER_BIQUAD
FILTER_BIQUAD,
FILTER_PT2,
FILTER_PT3
} filterType_e;
typedef enum {
@ -128,3 +132,6 @@ void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refre
void alphaBetaGammaFilterInit(alphaBetaGammaFilter_t *filter, float alpha, float boostGain, float halfLife, float dT);
float alphaBetaGammaFilterApply(alphaBetaGammaFilter_t *filter, float input);
void initFilter(uint8_t filterType, filter_t *filter, float cutoffFrequency, uint32_t refreshRate);
void assignFilterApplyFn(uint8_t filterType, float cutoffFrequency, filterApplyFnPtr *applyFn);

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

@ -524,11 +524,15 @@ float bellCurve(const float x, const float curveWidth)
}
float fast_fsqrtf(const double value) {
float ret = 0.0f;
#ifdef USE_ARM_MATH
float squirt;
arm_sqrt_f32(value, &squirt);
return squirt;
arm_sqrt_f32(value, &ret);
#else
return sqrtf(value);
ret = sqrtf(value);
#endif
if (isnan(ret))
{
return 0;
}
return ret;
}

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

@ -56,12 +56,16 @@
#define RADIANS_TO_CENTIDEGREES(angle) (((angle) * 100.0f) / RAD)
#define CENTIDEGREES_TO_RADIANS(angle) (((angle) / 100.0f) * RAD)
#define CENTIMETERS_TO_CENTIFEET(cm) (cm * (328 / 100.0))
#define CENTIMETERS_TO_FEET(cm) (cm * (328 / 10000.0))
#define CENTIMETERS_TO_CENTIFEET(cm) (cm / 0.3048)
#define CENTIMETERS_TO_FEET(cm) (cm / 30.48)
#define CENTIMETERS_TO_METERS(cm) (cm / 100)
#define METERS_TO_CENTIMETERS(m) (m * 100)
#define CMSEC_TO_CENTIMPH(cms) (cms * 2.2369363)
#define CMSEC_TO_CENTIKPH(cms) (cms * 3.6)
#define CMSEC_TO_CENTIKNOTS(cms) (cms * 1.943845)
// copied from https://code.google.com/p/cxutil/source/browse/include/cxutil/utility.h#70
#define _CHOOSE2(binoper, lexpr, lvar, rexpr, rvar) \
( __extension__ ({ \

71
src/main/config/config_eeprom.c
View file

@ -25,12 +25,14 @@
#include "build/build_config.h"
#include "common/crc.h"
#include "common/utils.h"
#include "config/config_eeprom.h"
#include "config/config_streamer.h"
#include "config/parameter_group.h"
#include "drivers/system.h"
#include "drivers/flash.h"
#include "fc/config.h"
@ -76,6 +78,31 @@ typedef struct {
uint32_t word;
} PG_PACKED packingTest_t;
#if defined(CONFIG_IN_EXTERNAL_FLASH)
bool loadEEPROMFromExternalFlash(void)
{
const flashPartition_t *flashPartition = flashPartitionFindByType(FLASH_PARTITION_TYPE_CONFIG);
const flashGeometry_t *flashGeometry = flashGetGeometry();
uint32_t flashStartAddress = flashPartition->startSector * flashGeometry->sectorSize;
uint32_t totalBytesRead = 0;
int bytesRead = 0;
bool success = false;
do {
bytesRead = flashReadBytes(flashStartAddress + totalBytesRead, &eepromData[totalBytesRead], EEPROM_SIZE - totalBytesRead);
if (bytesRead > 0) {
totalBytesRead += bytesRead;
success = (totalBytesRead == EEPROM_SIZE);
}
} while (!success && bytesRead > 0);
return success;
}
#endif /* defined(CONFIG_IN_EXTERNAL_FLASH) */
void initEEPROM(void)
{
// Verify that this architecture packs as expected.
@ -86,6 +113,14 @@ void initEEPROM(void)
BUILD_BUG_ON(sizeof(configHeader_t) != 1);
BUILD_BUG_ON(sizeof(configFooter_t) != 2);
BUILD_BUG_ON(sizeof(configRecord_t) != 6);
#if defined(CONFIG_IN_EXTERNAL_FLASH)
bool eepromLoaded = loadEEPROMFromExternalFlash();
if (!eepromLoaded) {
// Flash read failed - just die now
failureMode(FAILURE_FLASH_READ_FAILED);
}
#endif
}
static uint16_t updateCRC(uint16_t crc, const void *data, uint32_t length)
@ -217,7 +252,9 @@ static bool writeSettingsToEEPROM(void)
.format = EEPROM_CONF_VERSION,
};
config_streamer_write(&streamer, (uint8_t *)&header, sizeof(header));
if (config_streamer_write(&streamer, (uint8_t *)&header, sizeof(header)) < 0) {
return false;
}
uint16_t crc = updateCRC(0, (uint8_t *)&header, sizeof(header));
PG_FOREACH(reg) {
const uint16_t regSize = pgSize(reg);
@ -231,9 +268,13 @@ static bool writeSettingsToEEPROM(void)
if (pgIsSystem(reg)) {
// write the only instance
record.flags |= CR_CLASSICATION_SYSTEM;
config_streamer_write(&streamer, (uint8_t *)&record, sizeof(record));
if (config_streamer_write(&streamer, (uint8_t *)&record, sizeof(record)) < 0) {
return false;
}
crc = updateCRC(crc, (uint8_t *)&record, sizeof(record));
config_streamer_write(&streamer, reg->address, regSize);
if (config_streamer_write(&streamer, reg->address, regSize) < 0) {
return false;
}
crc = updateCRC(crc, reg->address, regSize);
} else {
// write one instance for each profile
@ -241,10 +282,14 @@ static bool writeSettingsToEEPROM(void)
record.flags = 0;
record.flags |= ((profileIndex + 1) & CR_CLASSIFICATION_MASK);
config_streamer_write(&streamer, (uint8_t *)&record, sizeof(record));
if (config_streamer_write(&streamer, (uint8_t *)&record, sizeof(record)) < 0) {
return false;
}
crc = updateCRC(crc, (uint8_t *)&record, sizeof(record));
const uint8_t *address = reg->address + (regSize * profileIndex);
config_streamer_write(&streamer, address, regSize);
if (config_streamer_write(&streamer, address, regSize) < 0) {
return false;
}
crc = updateCRC(crc, address, regSize);
}
}
@ -254,13 +299,19 @@ static bool writeSettingsToEEPROM(void)
.terminator = 0,
};
config_streamer_write(&streamer, (uint8_t *)&footer, sizeof(footer));
if (config_streamer_write(&streamer, (uint8_t *)&footer, sizeof(footer)) < 0) {
return false;
}
crc = updateCRC(crc, (uint8_t *)&footer, sizeof(footer));
// append checksum now
config_streamer_write(&streamer, (uint8_t *)&crc, sizeof(crc));
if (config_streamer_write(&streamer, (uint8_t *)&crc, sizeof(crc)) < 0) {
return false;
}
config_streamer_flush(&streamer);
if (config_streamer_flush(&streamer) < 0) {
return false;
}
bool success = config_streamer_finish(&streamer) == 0;
@ -274,6 +325,10 @@ void writeConfigToEEPROM(void)
for (int attempt = 0; attempt < 3 && !success; attempt++) {
if (writeSettingsToEEPROM()) {
success = true;
#ifdef CONFIG_IN_EXTERNAL_FLASH
// copy it back from flash to the in-memory buffer.
success = loadEEPROMFromExternalFlash();
#endif
}
}

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

@ -24,8 +24,6 @@
#include "common/axis.h"
#include "common/color.h"
#include "drivers/rx_pwm.h"
#include "fc/config.h"
#include "fc/rc_controls.h"
#include "flight/servos.h"

14
src/main/config/config_streamer.c
View file

@ -19,6 +19,11 @@
#include "platform.h"
#include "drivers/system.h"
#include "config/config_streamer.h"
#include "build/build_config.h"
#if !defined(CONFIG_IN_FLASH)
SLOW_RAM uint8_t eepromData[EEPROM_SIZE];
#endif
// Helper functions
extern void config_streamer_impl_unlock(void);
@ -35,6 +40,7 @@ void config_streamer_start(config_streamer_t *c, uintptr_t base, int size)
// base must start at FLASH_PAGE_SIZE boundary when using embedded flash.
c->address = base;
c->size = size;
c->end = base + size;
if (!c->unlocked) {
config_streamer_impl_unlock();
c->unlocked = true;
@ -44,6 +50,11 @@ void config_streamer_start(config_streamer_t *c, uintptr_t base, int size)
int config_streamer_write(config_streamer_t *c, const uint8_t *p, uint32_t size)
{
if ((c->address + size) > c->end) {
// trying to write past end of streamer
return -1;
}
for (const uint8_t *pat = p; pat != (uint8_t*)p + size; pat++) {
c->buffer.b[c->at++] = *pat;
@ -63,6 +74,9 @@ int config_streamer_status(config_streamer_t *c)
int config_streamer_flush(config_streamer_t *c)
{
if (c->at != 0) {
if ((c->address + c->at) > c->end) {
return -1;
}
memset(c->buffer.b + c->at, 0, sizeof(c->buffer) - c->at);
c->err = config_streamer_impl_write_word(c, &c->buffer.w);
c->at = 0;

8
src/main/config/config_streamer.h
View file

@ -20,10 +20,15 @@
#include <stdint.h>
#include <stdbool.h>
#include "drivers/flash_m25p16.h"
// Streams data out to the EEPROM, padding to the write size as
// needed, and updating the checksum as it goes.
#if defined(STM32H7)
#ifdef CONFIG_IN_EXTERNAL_FLASH
#define CONFIG_STREAMER_BUFFER_SIZE M25P16_PAGESIZE // Must match flash device page size
typedef uint32_t config_streamer_buffer_align_type_t;
#elif defined(STM32H7)
#define CONFIG_STREAMER_BUFFER_SIZE 32 // Flash word = 256-bits
typedef uint64_t config_streamer_buffer_align_type_t;
#else
@ -33,6 +38,7 @@ typedef uint32_t config_streamer_buffer_align_type_t;
typedef struct config_streamer_s {
uintptr_t address;
uintptr_t end;
int size;
union {
uint8_t b[CONFIG_STREAMER_BUFFER_SIZE];

77
src/main/config/config_streamer_extflash.c Normal file
View file

@ -0,0 +1,77 @@
/*
* 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 <string.h>
#include "platform.h"
#include "drivers/system.h"
#include "drivers/flash.h"
#include "config/config_streamer.h"
#if defined(CONFIG_IN_EXTERNAL_FLASH)
static bool streamerLocked = true;
void config_streamer_impl_unlock(void)
{
const flashGeometry_t *flashGeometry = flashGetGeometry();
if (flashGeometry->pageSize == CONFIG_STREAMER_BUFFER_SIZE) {
// streamer needs to buffer exactly one flash page
streamerLocked = false;
}
}
void config_streamer_impl_lock(void)
{
streamerLocked = true;
}
int config_streamer_impl_write_word(config_streamer_t *c, config_streamer_buffer_align_type_t *buffer)
{
if (streamerLocked) {
return -1;
}
uint32_t dataOffset = (uint32_t)(c->address - (uintptr_t)&eepromData[0]);
const flashPartition_t *flashPartition = flashPartitionFindByType(FLASH_PARTITION_TYPE_CONFIG);
const flashGeometry_t *flashGeometry = flashGetGeometry();
uint32_t flashStartAddress = flashPartition->startSector * flashGeometry->sectorSize;
uint32_t flashOverflowAddress = ((flashPartition->endSector + 1) * flashGeometry->sectorSize); // +1 to sector for inclusive
uint32_t flashAddress = flashStartAddress + dataOffset;
if (flashAddress + CONFIG_STREAMER_BUFFER_SIZE > flashOverflowAddress) {
return -2; // address is past end of partition
}
uint32_t flashSectorSize = flashGeometry->sectorSize;
if (flashAddress % flashSectorSize == 0) {
flashEraseSector(flashAddress);
}
if (flashPageProgram(flashAddress, (uint8_t *)buffer, CONFIG_STREAMER_BUFFER_SIZE) == flashAddress) {
// returned same address: programming failed
return -3;
}
c->address += CONFIG_STREAMER_BUFFER_SIZE;
return 0;
}
#endif

61
src/main/config/config_streamer_ram.c Normal file
View file

@ -0,0 +1,61 @@
/*
* 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 <string.h>
#include "platform.h"
#include "drivers/system.h"
#include "config/config_streamer.h"
#if defined(CONFIG_IN_RAM)
static bool streamerLocked = true;
void config_streamer_impl_unlock(void)
{
streamerLocked = false;
}
void config_streamer_impl_lock(void)
{
streamerLocked = true;
}
int config_streamer_impl_write_word(config_streamer_t *c, config_streamer_buffer_align_type_t *buffer)
{
if (streamerLocked) {
return -1;
}
if (c->address == (uintptr_t)&eepromData[0]) {
memset(eepromData, 0, sizeof(eepromData));
}
config_streamer_buffer_align_type_t *destAddr = (config_streamer_buffer_align_type_t *)c->address;
config_streamer_buffer_align_type_t *srcAddr = buffer;
uint8_t nRows = CONFIG_STREAMER_BUFFER_SIZE / sizeof(config_streamer_buffer_align_type_t);
do {
*destAddr++ = *srcAddr++;
} while(--nRows != 0);
c->address += CONFIG_STREAMER_BUFFER_SIZE;
return 0;
}
#endif

2
src/main/config/config_streamer_stm32f3.c
View file

@ -20,7 +20,7 @@
#include "drivers/system.h"
#include "config/config_streamer.h"
#if defined(STM32F3)
#if defined(STM32F3) && !defined(CONFIG_IN_RAM) && !defined(CONFIG_IN_EXTERNAL_FLASH)
#define FLASH_PAGE_SIZE 0x800

2
src/main/config/config_streamer_stm32f4.c
View file

@ -20,7 +20,7 @@
#include "drivers/system.h"
#include "config/config_streamer.h"
#if defined(STM32F4)
#if defined(STM32F4) && !defined(CONFIG_IN_RAM) && !defined(CONFIG_IN_EXTERNAL_FLASH)
/*
Sector 0 0x08000000 - 0x08003FFF 16 Kbytes

2
src/main/config/config_streamer_stm32f7.c
View file

@ -20,7 +20,7 @@
#include "drivers/system.h"
#include "config/config_streamer.h"
#if defined(STM32F7)
#if defined(STM32F7) && !defined(CONFIG_IN_RAM) && !defined(CONFIG_IN_EXTERNAL_FLASH)
#if defined(STM32F745xx) || defined(STM32F746xx) || defined(STM32F765xx)
/*

2
src/main/config/config_streamer_stm32h7.c
View file

@ -20,7 +20,7 @@
#include "drivers/system.h"
#include "config/config_streamer.h"
#if defined(STM32H7)
#if defined(STM32H7) && !defined(CONFIG_IN_RAM) && !defined(CONFIG_IN_EXTERNAL_FLASH)
#if defined(STM32H743xx)
/* Sectors 0-7 of 128K each */

117
src/main/drivers/accgyro/accgyro_adxl345.c
View file

@ -1,117 +0,0 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <string.h>
#include <stdint.h>
#include <platform.h>
#include "drivers/system.h"
#include "drivers/bus.h"
#include "drivers/sensor.h"
#include "drivers/accgyro/accgyro.h"
#include "drivers/accgyro/accgyro_adxl345.h"
#ifdef USE_IMU_ADXL345
// ADXL345, Alternative address mode 0x53
#define ADXL345_ADDRESS 0x53
// Registers
#define ADXL345_BW_RATE 0x2C
#define ADXL345_POWER_CTL 0x2D
#define ADXL345_INT_ENABLE 0x2E
#define ADXL345_DATA_FORMAT 0x31
#define ADXL345_DATA_OUT 0x32
#define ADXL345_FIFO_CTL 0x38
// BW_RATE values
#define ADXL345_RATE_50 0x09
#define ADXL345_RATE_100 0x0A
#define ADXL345_RATE_200 0x0B
#define ADXL345_RATE_400 0x0C
#define ADXL345_RATE_800 0x0D
#define ADXL345_RATE_1600 0x0E
#define ADXL345_RATE_3200 0x0F
// various register values
#define ADXL345_POWER_MEAS 0x08
#define ADXL345_FULL_RANGE 0x08
#define ADXL345_RANGE_2G 0x00
#define ADXL345_RANGE_4G 0x01
#define ADXL345_RANGE_8G 0x02
#define ADXL345_RANGE_16G 0x03
#define ADXL345_FIFO_STREAM 0x80
static void adxl345Init(accDev_t *acc)
{
busWrite(acc->busDev, ADXL345_POWER_CTL, ADXL345_POWER_MEAS);
busWrite(acc->busDev, ADXL345_DATA_FORMAT, ADXL345_FULL_RANGE | ADXL345_RANGE_8G);
busWrite(acc->busDev, ADXL345_BW_RATE, ADXL345_RATE_100);
acc->acc_1G = 256; // 3.3V operation
}
static bool adxl345Read(accDev_t *acc)
{
uint8_t buf[6];
if (!busReadBuf(acc->busDev, ADXL345_DATA_OUT, buf, 6)) {
return false;
}
acc->ADCRaw[0] = buf[0] + (buf[1] << 8);
acc->ADCRaw[1] = buf[2] + (buf[3] << 8);
acc->ADCRaw[2] = buf[4] + (buf[5] << 8);
return true;
}
static bool deviceDetect(busDevice_t * dev)
{
bool ack = false;
uint8_t sig = 0;
ack = busRead(dev, 0x00, &sig);
if (!ack || sig != 0xE5)
return false;
return true;
}
bool adxl345Detect(accDev_t *acc)
{
acc->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_ADXL345, acc->imuSensorToUse, OWNER_MPU);
if (acc->busDev == NULL) {
return false;
}
if (!deviceDetect(acc->busDev)) {
busDeviceDeInit(acc->busDev);
return false;
}
acc->initFn = adxl345Init;
acc->readFn = adxl345Read;
acc->accAlign = acc->busDev->param;
return true;
}
#endif

93
src/main/drivers/accgyro/accgyro_bma280.c
View file

@ -1,93 +0,0 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <string.h>
#include <stdint.h>
#include <platform.h>
#include "drivers/system.h"
#include "drivers/bus.h"
#include "drivers/sensor.h"
#include "drivers/accgyro/accgyro.h"
#include "drivers/accgyro/accgyro_bma280.h"
#ifdef USE_IMU_BMA280
// BMA280, default I2C address mode 0x18
#define BMA280_WHOAMI 0x00
#define BMA280_ADDRESS 0x18
#define BMA280_ACC_X_LSB 0x02
#define BMA280_PMU_BW 0x10
#define BMA280_PMU_RANGE 0x0F
static void bma280Init(accDev_t *acc)
{
busWrite(acc->busDev, BMA280_PMU_RANGE, 0x08); // +-8g range
busWrite(acc->busDev, BMA280_PMU_BW, 0x0E); // 500Hz BW
acc->acc_1G = 512 * 8;
}
static bool bma280Read(accDev_t *acc)
{
uint8_t buf[6];
if (!busReadBuf(acc->busDev, BMA280_ACC_X_LSB, buf, 6)) {
return false;
}
// Data format is lsb<5:0><crap><new_data_bit> | msb<13:6>
acc->ADCRaw[0] = (int16_t)((buf[0] >> 2) + (buf[1] << 8));
acc->ADCRaw[1] = (int16_t)((buf[2] >> 2) + (buf[3] << 8));
acc->ADCRaw[2] = (int16_t)((buf[4] >> 2) + (buf[5] << 8));
return true;
}
static bool deviceDetect(busDevice_t * dev)
{
bool ack = false;
uint8_t sig = 0;
ack = busRead(dev, BMA280_WHOAMI, &sig);
if (!ack || sig != 0xFB)
return false;
return true;
}
bool bma280Detect(accDev_t *acc)
{
acc->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_BMA280, acc->imuSensorToUse, OWNER_MPU);
if (acc->busDev == NULL) {
return false;
}
if (!deviceDetect(acc->busDev)) {
busDeviceDeInit(acc->busDev);
return false;
}
acc->initFn = bma280Init;
acc->readFn = bma280Read;
acc->accAlign = acc->busDev->param;
return true;
}
#endif

354
src/main/drivers/accgyro/accgyro_bmi270.c Normal file
View file

@ -0,0 +1,354 @@
/*
* 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/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include "platform.h"
#if defined(USE_IMU_BMI270)
#include "build/debug.h"
#include "common/axis.h"
#include "common/maths.h"
#include "common/utils.h"
#include "drivers/system.h"
#include "drivers/time.h"
#include "drivers/io.h"
#include "drivers/exti.h"
#include "drivers/bus.h"
#include "drivers/sensor.h"
#include "drivers/accgyro/accgyro.h"
#include "drivers/accgyro/accgyro_bmi270.h"
#define BMI270_CONFIG_SIZE 328
// Declaration for the device config (microcode) that must be uploaded to the sensor
extern const uint8_t bmi270_maximum_fifo_config_file[BMI270_CONFIG_SIZE];
// BMI270 registers (not the complete list)
typedef enum {
BMI270_REG_CHIP_ID = 0x00,
BMI270_REG_ERR_REG = 0x02,
BMI270_REG_STATUS = 0x03,
BMI270_REG_ACC_DATA_X_LSB = 0x0C,
BMI270_REG_GYR_DATA_X_LSB = 0x12,
BMI270_REG_SENSORTIME_0 = 0x18,
BMI270_REG_SENSORTIME_1 = 0x19,
BMI270_REG_SENSORTIME_2 = 0x1A,
BMI270_REG_EVENT = 0x1B,
BMI270_REG_INT_STATUS_0 = 0x1C,
BMI270_REG_INT_STATUS_1 = 0x1D,
BMI270_REG_INTERNAL_STATUS = 0x21,
BMI270_REG_TEMPERATURE_LSB = 0x22,
BMI270_REG_TEMPERATURE_MSB = 0x23,
BMI270_REG_FIFO_LENGTH_LSB = 0x24,
BMI270_REG_FIFO_LENGTH_MSB = 0x25,
BMI270_REG_FIFO_DATA = 0x26,
BMI270_REG_ACC_CONF = 0x40,
BMI270_REG_ACC_RANGE = 0x41,
BMI270_REG_GYRO_CONF = 0x42,
BMI270_REG_GYRO_RANGE = 0x43,
BMI270_REG_AUX_CONF = 0x44,
BMI270_REG_FIFO_DOWNS = 0x45,
BMI270_REG_FIFO_WTM_0 = 0x46,
BMI270_REG_FIFO_WTM_1 = 0x47,
BMI270_REG_FIFO_CONFIG_0 = 0x48,
BMI270_REG_FIFO_CONFIG_1 = 0x49,
BMI270_REG_SATURATION = 0x4A,
BMI270_REG_INT1_IO_CTRL = 0x53,
BMI270_REG_INT2_IO_CTRL = 0x54,
BMI270_REG_INT_LATCH = 0x55,
BMI270_REG_INT1_MAP_FEAT = 0x56,
BMI270_REG_INT2_MAP_FEAT = 0x57,
BMI270_REG_INT_MAP_DATA = 0x58,
BMI270_REG_INIT_CTRL = 0x59,
BMI270_REG_INIT_DATA = 0x5E,
BMI270_REG_ACC_SELF_TEST = 0x6D,
BMI270_REG_GYR_SELF_TEST_AXES = 0x6E,
BMI270_REG_PWR_CONF = 0x7C,
BMI270_REG_PWR_CTRL = 0x7D,
BMI270_REG_CMD = 0x7E,
} bmi270Register_e;
#define BMI270_CHIP_ID 0x24
#define BMI270_CMD_SOFTRESET 0xB6
#define BMI270_PWR_CONF_HP 0x00
#define BMI270_PWR_CTRL_GYR_EN 0x02
#define BMI270_PWR_CTRL_ACC_EN 0x04
#define BMI270_PWR_CTRL_TEMP_EN 0x08
#define BMI270_ACC_CONF_HP 0x80
#define BMI270_ACC_RANGE_8G 0x02
#define BMI270_ACC_RANGE_16G 0x03
#define BMI270_GYRO_CONF_NOISE_PERF 0x40
#define BMI270_GYRO_CONF_FILTER_PERF 0x80
#define BMI270_GYRO_RANGE_2000DPS 0x08
#define BMI270_INT_MAP_DATA_DRDY_INT1 0x04
#define BMI270_INT1_IO_CTRL_ACTIVE_HIGH 0x02
#define BMI270_INT1_IO_CTRL_OUTPUT_EN 0x08
#define BMI270_ODR_400 0x0A
#define BMI270_ODR_800 0x0B
#define BMI270_ODR_1600 0x0C
#define BMI270_ODR_3200 0x0D
#define BMI270_BWP_OSR4 0x00
#define BMI270_BWP_OSR2 0x10
#define BMI270_BWP_NORM 0x20
typedef struct __attribute__ ((__packed__)) bmi270ContextData_s {
uint16_t chipMagicNumber;
uint8_t lastReadStatus;
uint8_t __padding_dummy;
uint8_t accRaw[6];
uint8_t gyroRaw[6];
} bmi270ContextData_t;
STATIC_ASSERT(sizeof(bmi270ContextData_t) < BUS_SCRATCHPAD_MEMORY_SIZE, busDevice_scratchpad_memory_too_small);
static const gyroFilterAndRateConfig_t gyroConfigs[] = {
{ GYRO_LPF_256HZ, 3200, { BMI270_BWP_NORM | BMI270_ODR_3200} },
{ GYRO_LPF_256HZ, 1600, { BMI270_BWP_NORM | BMI270_ODR_1600} },
{ GYRO_LPF_256HZ, 800, { BMI270_BWP_NORM | BMI270_ODR_800 } },
{ GYRO_LPF_188HZ, 800, { BMI270_BWP_OSR2 | BMI270_ODR_800 } },
{ GYRO_LPF_188HZ, 400, { BMI270_BWP_NORM | BMI270_ODR_400 } },
{ GYRO_LPF_98HZ, 800, { BMI270_BWP_OSR4 | BMI270_ODR_800 } },
{ GYRO_LPF_98HZ, 400, { BMI270_BWP_OSR2 | BMI270_ODR_400 } },
{ GYRO_LPF_42HZ, 800, { BMI270_BWP_OSR4 | BMI270_ODR_800 } },
{ GYRO_LPF_42HZ, 400, { BMI270_BWP_OSR4 | BMI270_ODR_400 } },
};
// Toggle the CS to switch the device into SPI mode.
// Device switches initializes as I2C and switches to SPI on a low to high CS transition
static void bmi270EnableSPI(busDevice_t *dev)
{
IOLo(dev->busdev.spi.csnPin);
delay(1);
IOHi(dev->busdev.spi.csnPin);
delay(10);
}
static bool bmi270DeviceDetect(busDevice_t * busDev)
{
uint8_t id[2];
busSetSpeed(busDev, BUS_SPEED_INITIALIZATION);
bmi270EnableSPI(busDev);
busReadBuf(busDev, BMI270_REG_CHIP_ID, &id[0], 2);
if (id[1] == BMI270_CHIP_ID) {
return true;
}
return false;
}
static void bmi270UploadConfig(busDevice_t * busDev)
{
busWrite(busDev, BMI270_REG_PWR_CONF, 0);
delay(1);
busWrite(busDev, BMI270_REG_INIT_CTRL, 0);
delay(1);
// Transfer the config file
uint8_t reg = BMI270_REG_INIT_DATA;
busTransferDescriptor_t tfDesc[] = {
{.rxBuf = NULL, .txBuf=&reg, .length=1},
{.rxBuf = NULL, .txBuf=(uint8_t *)bmi270_maximum_fifo_config_file, .length=sizeof(bmi270_maximum_fifo_config_file)}
};
spiBusTransferMultiple(busDev, tfDesc, 2);
delay(10);
busWrite(busDev, BMI270_REG_INIT_CTRL, 1);
delay(1);
}
static void bmi270AccAndGyroInit(gyroDev_t *gyro)
{
busDevice_t * busDev = gyro->busDev;
gyroIntExtiInit(gyro);
// Perform a soft reset to set all configuration to default
// Delay 100ms before continuing configuration
busWrite(busDev, BMI270_REG_CMD, BMI270_CMD_SOFTRESET);
delay(100);
// Use standard bus speed
busSetSpeed(busDev, BUS_SPEED_STANDARD);
// Toggle the chip into SPI mode
bmi270EnableSPI(busDev);
bmi270UploadConfig(busDev);
// Configure the accelerometer
busWrite(busDev, BMI270_REG_ACC_CONF, BMI270_ODR_1600 | BMI270_BWP_OSR4 | BMI270_ACC_CONF_HP);
delay(1);
// Configure the accelerometer full-scale range
busWrite(busDev, BMI270_REG_ACC_RANGE, BMI270_ACC_RANGE_8G);
delay(1);
// Configure the gyro
// Figure out suitable filter configuration
const gyroFilterAndRateConfig_t * config = chooseGyroConfig(gyro->lpf, 1000000 / gyro->requestedSampleIntervalUs, &gyroConfigs[0], ARRAYLEN(gyroConfigs));
gyro->sampleRateIntervalUs = 1000000 / config->gyroRateHz;
busWrite(busDev, BMI270_REG_GYRO_CONF, config->gyroConfigValues[0] | BMI270_GYRO_CONF_NOISE_PERF | BMI270_GYRO_CONF_FILTER_PERF);
delay(1);
// Configure the gyro full-range scale
busWrite(busDev, BMI270_REG_GYRO_RANGE, BMI270_GYRO_RANGE_2000DPS);
delay(1);
// Configure the gyro data ready interrupt
#ifdef USE_MPU_DATA_READY_SIGNAL
busWrite(busDev, BMI270_REG_INT_MAP_DATA, BMI270_INT_MAP_DATA_DRDY_INT1);
delay(1);
#endif
// Configure the behavior of the INT1 pin
busWrite(busDev, BMI270_REG_INT1_IO_CTRL, BMI270_INT1_IO_CTRL_ACTIVE_HIGH | BMI270_INT1_IO_CTRL_OUTPUT_EN);
delay(1);
// Configure the device for performance mode
busWrite(busDev, BMI270_REG_PWR_CONF, BMI270_PWR_CONF_HP);
delay(1);
// Enable the gyro and accelerometer
busWrite(busDev, BMI270_REG_PWR_CTRL, BMI270_PWR_CTRL_GYR_EN | BMI270_PWR_CTRL_ACC_EN);
delay(1);
}
static bool bmi270yroReadScratchpad(gyroDev_t *gyro)
{
bmi270ContextData_t * ctx = busDeviceGetScratchpadMemory(gyro->busDev);
ctx->lastReadStatus = busReadBuf(gyro->busDev, BMI270_REG_ACC_DATA_X_LSB, &ctx->__padding_dummy, 6 + 6 + 1);
if (ctx->lastReadStatus) {
gyro->gyroADCRaw[X] = (int16_t)((ctx->gyroRaw[1] << 8) | ctx->gyroRaw[0]);
gyro->gyroADCRaw[Y] = (int16_t)((ctx->gyroRaw[3] << 8) | ctx->gyroRaw[2]);
gyro->gyroADCRaw[Z] = (int16_t)((ctx->gyroRaw[5] << 8) | ctx->gyroRaw[4]);
return true;
}
return false;
}
static bool bmi270AccReadScratchpad(accDev_t *acc)
{
bmi270ContextData_t * ctx = busDeviceGetScratchpadMemory(acc->busDev);
if (ctx->lastReadStatus) {
acc->ADCRaw[X] = (int16_t)((ctx->accRaw[1] << 8) | ctx->accRaw[0]);
acc->ADCRaw[Y] = (int16_t)((ctx->accRaw[3] << 8) | ctx->accRaw[2]);
acc->ADCRaw[Z] = (int16_t)((ctx->accRaw[5] << 8) | ctx->accRaw[4]);
return true;
}
return false;
}
static bool bmi270TemperatureRead(gyroDev_t *gyro, int16_t * data)
{
uint8_t buffer[3];
bool readStatus = busReadBuf(gyro->busDev, BMI270_REG_TEMPERATURE_LSB, &buffer[0], sizeof(buffer));
if (readStatus) {
// Convert to degC*10: degC = raw / 512 + 23
*data = 230 + ((10 * (int32_t)((int16_t)((buffer[2] << 8) | buffer[1]))) >> 9);
return true;
}
return false;
}
static void bmi270GyroInit(gyroDev_t *gyro)
{
bmi270AccAndGyroInit(gyro);
}
static void bmi270AccInit(accDev_t *acc)
{
// sensor is configured during gyro init
acc->acc_1G = 4096; // 8G sensor scale
}
bool bmi270AccDetect(accDev_t *acc)
{
acc->busDev = busDeviceOpen(BUSTYPE_SPI, DEVHW_BMI270, acc->imuSensorToUse);
if (acc->busDev == NULL) {
return false;
}
bmi270ContextData_t * ctx = busDeviceGetScratchpadMemory(acc->busDev);
if (ctx->chipMagicNumber != 0xB270) {
return false;
}
acc->initFn = bmi270AccInit;
acc->readFn = bmi270AccReadScratchpad;
return true;
}
bool bmi270GyroDetect(gyroDev_t *gyro)
{
gyro->busDev = busDeviceInit(BUSTYPE_SPI, DEVHW_BMI270, gyro->imuSensorToUse, OWNER_MPU);
if (gyro->busDev == NULL) {
return false;
}
if (!bmi270DeviceDetect(gyro->busDev)) {
busDeviceDeInit(gyro->busDev);
return false;
}
// Magic number for ACC detection to indicate that we have detected BMI270 gyro
bmi270ContextData_t * ctx = busDeviceGetScratchpadMemory(gyro->busDev);
ctx->chipMagicNumber = 0xB270;
gyro->initFn = bmi270GyroInit;
gyro->readFn = bmi270yroReadScratchpad;
gyro->temperatureFn = bmi270TemperatureRead;
gyro->intStatusFn = gyroCheckDataReady;
gyro->scale = 1.0f / 16.4f; // 2000 dps
return true;
}
#endif // USE_IMU_BMI270

26
src/main/drivers/accgyro/accgyro_bmi270.h Normal file
View file

@ -0,0 +1,26 @@
/*
* 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/>.
*/
#pragma once
#include "drivers/bus.h"
bool bmi270AccDetect(accDev_t *acc);
bool bmi270GyroDetect(gyroDev_t *gyro);

213
src/main/drivers/accgyro/accgyro_bmi270_maximum_fifo.c Normal file
View file

@ -0,0 +1,213 @@
/**
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
*
* BSD-3-Clause
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* @file bmi270_maximum_fifo.c
* @date 2020-11-04
* @version v2.63.1
*
*/
/***************************************************************************/
/*! Header files
****************************************************************************/
//#include "bmi270_maximum_fifo.h"
#include "platform.h"
/***************************************************************************/
/*! Global Variable
****************************************************************************/
/*! @name Global array that stores the configuration file of BMI270 */
const uint8_t bmi270_maximum_fifo_config_file[] = {
0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x1a, 0x00, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00,
0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0x90, 0x32, 0x21, 0x2e, 0x59, 0xf5,
0x10, 0x30, 0x21, 0x2e, 0x6a, 0xf5, 0x1a, 0x24, 0x22, 0x00, 0x80, 0x2e, 0x3b, 0x00, 0xc8, 0x2e, 0x44, 0x47, 0x22,
0x00, 0x37, 0x00, 0xa4, 0x00, 0xff, 0x0f, 0xd1, 0x00, 0x07, 0xad, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1,
0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00,
0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x11, 0x24, 0xfc, 0xf5, 0x80, 0x30, 0x40, 0x42, 0x50, 0x50, 0x00, 0x30, 0x12, 0x24, 0xeb,
0x00, 0x03, 0x30, 0x00, 0x2e, 0xc1, 0x86, 0x5a, 0x0e, 0xfb, 0x2f, 0x21, 0x2e, 0xfc, 0xf5, 0x13, 0x24, 0x63, 0xf5,
0xe0, 0x3c, 0x48, 0x00, 0x22, 0x30, 0xf7, 0x80, 0xc2, 0x42, 0xe1, 0x7f, 0x3a, 0x25, 0xfc, 0x86, 0xf0, 0x7f, 0x41,
0x33, 0x98, 0x2e, 0xc2, 0xc4, 0xd6, 0x6f, 0xf1, 0x30, 0xf1, 0x08, 0xc4, 0x6f, 0x11, 0x24, 0xff, 0x03, 0x12, 0x24,
0x00, 0xfc, 0x61, 0x09, 0xa2, 0x08, 0x36, 0xbe, 0x2a, 0xb9, 0x13, 0x24, 0x38, 0x00, 0x64, 0xbb, 0xd1, 0xbe, 0x94,
0x0a, 0x71, 0x08, 0xd5, 0x42, 0x21, 0xbd, 0x91, 0xbc, 0xd2, 0x42, 0xc1, 0x42, 0x00, 0xb2, 0xfe, 0x82, 0x05, 0x2f,
0x50, 0x30, 0x21, 0x2e, 0x21, 0xf2, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0xf0, 0x6f, 0x02, 0x30, 0x02, 0x42, 0x20,
0x26, 0xe0, 0x6f, 0x02, 0x31, 0x03, 0x40, 0x9a, 0x0a, 0x02, 0x42, 0xf0, 0x37, 0x05, 0x2e, 0x5e, 0xf7, 0x10, 0x08,
0x12, 0x24, 0x1e, 0xf2, 0x80, 0x42, 0x83, 0x84, 0xf1, 0x7f, 0x0a, 0x25, 0x13, 0x30, 0x83, 0x42, 0x3b, 0x82, 0xf0,
0x6f, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x12, 0x40, 0x52, 0x42, 0x00, 0x2e, 0x12, 0x40, 0x52, 0x42, 0x3e, 0x84,
0x00, 0x40, 0x40, 0x42, 0x7e, 0x82, 0xe1, 0x7f, 0xf2, 0x7f, 0x98, 0x2e, 0x6a, 0xd6, 0x21, 0x30, 0x23, 0x2e, 0x61,
0xf5, 0xeb, 0x2c, 0xe1, 0x6f
};
// The rest of this is not needed, avoid compiler errors due to pedantic settings
#if false
/*! @name Global array that stores the feature input configuration of BMI270 */
const struct bmi2_feature_config bmi270_maximum_fifo_feat_in[BMI270_MAXIMUM_FIFO_MAX_FEAT_IN] = {
};
/*! @name Global array that stores the feature output configuration */
const struct bmi2_feature_config bmi270_maximum_fifo_feat_out[BMI270_MAXIMUM_FIFO_MAX_FEAT_OUT] = {
};
/******************************************************************************/
/*! Local Function Prototypes
******************************************************************************/
/*!
* @brief This internal API is used to validate the device pointer for
* null conditions.
*
* @param[in] dev : Structure instance of bmi2_dev.
*
* @return Result of API execution status
* @retval 0 -> Success
* @retval < 0 -> Fail
*/
static int8_t null_ptr_check(const struct bmi2_dev *dev);
/***************************************************************************/
/*! User Interface Definitions
****************************************************************************/
/*!
* @brief This API:
* 1) updates the device structure with address of the configuration file.
* 2) Initializes BMI270 sensor.
* 3) Writes the configuration file.
* 4) Updates the feature offset parameters in the device structure.
* 5) Updates the maximum number of pages, in the device structure.
*/
int8_t bmi270_maximum_fifo_init(struct bmi2_dev *dev)
{
/* Variable to define result */
int8_t rslt;
/* Null-pointer check */
rslt = null_ptr_check(dev);
if (rslt == BMI2_OK)
{
/* Assign chip id of BMI270 */
dev->chip_id = BMI270_MAXIMUM_FIFO_CHIP_ID;
dev->config_size = sizeof(bmi270_maximum_fifo_config_file);
/* Enable the variant specific features if any */
dev->variant_feature = BMI2_GYRO_CROSS_SENS_ENABLE | BMI2_MAXIMUM_FIFO_VARIANT;
/* An extra dummy byte is read during SPI read */
if (dev->intf == BMI2_SPI_INTF)
{
dev->dummy_byte = 1;
}
else
{
dev->dummy_byte = 0;
}
/* If configuration file pointer is not assigned any address */
if (!dev->config_file_ptr)
{
/* Give the address of the configuration file array to
* the device pointer
*/
dev->config_file_ptr = bmi270_maximum_fifo_config_file;
}
/* Initialize BMI2 sensor */
rslt = bmi2_sec_init(dev);
if (rslt == BMI2_OK)
{
/* Assign the offsets of the feature input
* configuration to the device structure
*/
dev->feat_config = bmi270_maximum_fifo_feat_in;
/* Assign the offsets of the feature output to
* the device structure
*/
dev->feat_output = bmi270_maximum_fifo_feat_out;
/* Assign the maximum number of pages to the
* device structure
*/
dev->page_max = BMI270_MAXIMUM_FIFO_MAX_PAGE_NUM;
/* Assign maximum number of input sensors/
* features to device structure
*/
dev->input_sens = BMI270_MAXIMUM_FIFO_MAX_FEAT_IN;
/* Assign maximum number of output sensors/
* features to device structure
*/
dev->out_sens = BMI270_MAXIMUM_FIFO_MAX_FEAT_OUT;
/* Get the gyroscope cross axis sensitivity */
rslt = bmi2_get_gyro_cross_sense(dev);
}
}
return rslt;
}
/***************************************************************************/
/*! Local Function Definitions
****************************************************************************/
/*!
* @brief This internal API is used to validate the device structure pointer for
* null conditions.
*/
static int8_t null_ptr_check(const struct bmi2_dev *dev)
{
/* Variable to define result */
int8_t rslt = BMI2_OK;
if ((dev == NULL) || (dev->read == NULL) || (dev->write == NULL) || (dev->delay_us == NULL))
{
/* Device structure pointer is not valid */
rslt = BMI2_E_NULL_PTR;
}
return rslt;
}
#endif

272
src/main/drivers/accgyro/accgyro_icm42605.c Normal file
View file

@ -0,0 +1,272 @@
/*
* This file is part of INAV.
*
* INAV is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* INAV is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with INAV. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include "platform.h"
#include "build/debug.h"
#include "common/axis.h"
#include "common/maths.h"
#include "common/utils.h"
#include "drivers/system.h"
#include "drivers/time.h"
#include "drivers/exti.h"
#include "drivers/sensor.h"
#include "drivers/accgyro/accgyro.h"
#include "drivers/accgyro/accgyro_mpu.h"
#include "drivers/accgyro/accgyro_icm42605.h"
#if defined(USE_IMU_ICM42605)
#define ICM42605_RA_PWR_MGMT0 0x4E
#define ICM42605_PWR_MGMT0_ACCEL_MODE_LN (3 << 0)
#define ICM42605_PWR_MGMT0_GYRO_MODE_LN (3 << 2)
#define ICM42605_PWR_MGMT0_TEMP_DISABLE_OFF (0 << 5)
#define ICM42605_PWR_MGMT0_TEMP_DISABLE_ON (1 << 5)
#define ICM42605_RA_GYRO_CONFIG0 0x4F
#define ICM42605_RA_ACCEL_CONFIG0 0x50
#define ICM42605_RA_GYRO_ACCEL_CONFIG0 0x52
#define ICM42605_ACCEL_UI_FILT_BW_LOW_LATENCY (14 << 4)
#define ICM42605_GYRO_UI_FILT_BW_LOW_LATENCY (14 << 0)
#define ICM42605_RA_GYRO_DATA_X1 0x25
#define ICM42605_RA_ACCEL_DATA_X1 0x1F
#define ICM42605_RA_INT_CONFIG 0x14
#define ICM42605_INT1_MODE_PULSED (0 << 2)
#define ICM42605_INT1_MODE_LATCHED (1 << 2)
#define ICM42605_INT1_DRIVE_CIRCUIT_OD (0 << 1)
#define ICM42605_INT1_DRIVE_CIRCUIT_PP (1 << 1)
#define ICM42605_INT1_POLARITY_ACTIVE_LOW (0 << 0)
#define ICM42605_INT1_POLARITY_ACTIVE_HIGH (1 << 0)
#define ICM42605_RA_INT_CONFIG0 0x63
#define ICM42605_UI_DRDY_INT_CLEAR_ON_SBR ((0 << 5) || (0 << 4))
#define ICM42605_UI_DRDY_INT_CLEAR_ON_SBR_DUPLICATE ((0 << 5) || (0 << 4)) // duplicate settings in datasheet, Rev 1.2.
#define ICM42605_UI_DRDY_INT_CLEAR_ON_F1BR ((1 << 5) || (0 << 4))
#define ICM42605_UI_DRDY_INT_CLEAR_ON_SBR_AND_F1BR ((1 << 5) || (1 << 4))
#define ICM42605_RA_INT_CONFIG1 0x64
#define ICM42605_INT_ASYNC_RESET_BIT 4
#define ICM42605_INT_TDEASSERT_DISABLE_BIT 5
#define ICM42605_INT_TDEASSERT_ENABLED (0 << ICM42605_INT_TDEASSERT_DISABLE_BIT)
#define ICM42605_INT_TDEASSERT_DISABLED (1 << ICM42605_INT_TDEASSERT_DISABLE_BIT)
#define ICM42605_INT_TPULSE_DURATION_BIT 6
#define ICM42605_INT_TPULSE_DURATION_100 (0 << ICM42605_INT_TPULSE_DURATION_BIT)
#define ICM42605_INT_TPULSE_DURATION_8 (1 << ICM42605_INT_TPULSE_DURATION_BIT)
#define ICM42605_RA_INT_SOURCE0 0x65
#define ICM42605_UI_DRDY_INT1_EN_DISABLED (0 << 3)
#define ICM42605_UI_DRDY_INT1_EN_ENABLED (1 << 3)
static void icm42605AccInit(accDev_t *acc)
{
acc->acc_1G = 512 * 4;
}
static bool icm42605AccRead(accDev_t *acc)
{
uint8_t data[6];
const bool ack = busReadBuf(acc->busDev, ICM42605_RA_ACCEL_DATA_X1, data, 6);
if (!ack) {
return false;
}
acc->ADCRaw[X] = (int16_t)((data[0] << 8) | data[1]);
acc->ADCRaw[Y] = (int16_t)((data[2] << 8) | data[3]);
acc->ADCRaw[Z] = (int16_t)((data[4] << 8) | data[5]);
return true;
}
bool icm42605AccDetect(accDev_t *acc)
{
acc->busDev = busDeviceOpen(BUSTYPE_ANY, DEVHW_ICM42605, acc->imuSensorToUse);
if (acc->busDev == NULL) {
return false;
}
mpuContextData_t * ctx = busDeviceGetScratchpadMemory(acc->busDev);
if (ctx->chipMagicNumber != 0x4265) {
return false;
}
acc->initFn = icm42605AccInit;
acc->readFn = icm42605AccRead;
acc->accAlign = acc->busDev->param;
return true;
}
static const gyroFilterAndRateConfig_t icm42605GyroConfigs[] = {
/* DLPF ODR */
{ GYRO_LPF_256HZ, 8000, { 6, 3 } }, /* 400 Hz LPF */
{ GYRO_LPF_256HZ, 4000, { 5, 4 } }, /* 250 Hz LPF */
{ GYRO_LPF_256HZ, 2000, { 3, 5 } }, /* 250 Hz LPF */
{ GYRO_LPF_256HZ, 1000, { 1, 6 } }, /* 250 Hz LPF */
{ GYRO_LPF_256HZ, 500, { 0, 15 } }, /* 250 Hz LPF */
{ GYRO_LPF_188HZ, 1000, { 3, 6 } }, /* 125 HZ */
{ GYRO_LPF_188HZ, 500, { 1, 15 } }, /* 125 HZ */
{ GYRO_LPF_98HZ, 1000, { 4, 6 } }, /* 100 HZ*/
{ GYRO_LPF_98HZ, 500, { 2, 15 } }, /* 100 HZ*/
{ GYRO_LPF_42HZ, 1000, { 6, 6 } }, /* 50 HZ */
{ GYRO_LPF_42HZ, 500, { 4, 15 } },
{ GYRO_LPF_20HZ, 1000, { 7, 6 } }, /* 25 HZ */
{ GYRO_LPF_20HZ, 500, { 6, 15 } },
{ GYRO_LPF_10HZ, 1000, { 7, 6 } }, /* 25 HZ */
{ GYRO_LPF_10HZ, 500, { 7, 15 } } /* 12.5 HZ */
};
static void icm42605AccAndGyroInit(gyroDev_t *gyro)
{
busDevice_t * dev = gyro->busDev;
const gyroFilterAndRateConfig_t * config = chooseGyroConfig(gyro->lpf, 1000000 / gyro->requestedSampleIntervalUs,
&icm42605GyroConfigs[0], ARRAYLEN(icm42605GyroConfigs));
gyro->sampleRateIntervalUs = 1000000 / config->gyroRateHz;
gyroIntExtiInit(gyro);
busSetSpeed(dev, BUS_SPEED_INITIALIZATION);
busWrite(dev, ICM42605_RA_PWR_MGMT0, ICM42605_PWR_MGMT0_TEMP_DISABLE_OFF | ICM42605_PWR_MGMT0_ACCEL_MODE_LN | ICM42605_PWR_MGMT0_GYRO_MODE_LN);
delay(15);
/* ODR and dynamic range */
busWrite(dev, ICM42605_RA_GYRO_CONFIG0, (0x00) << 5 | (config->gyroConfigValues[1] & 0x0F)); /* 2000 deg/s */
delay(15);
busWrite(dev, ICM42605_RA_ACCEL_CONFIG0, (0x00) << 5 | (config->gyroConfigValues[1] & 0x0F)); /* 16 G deg/s */
delay(15);
/* LPF bandwidth */
busWrite(dev, ICM42605_RA_GYRO_ACCEL_CONFIG0, (config->gyroConfigValues[1]) | (config->gyroConfigValues[1] << 4));
delay(15);
busWrite(dev, ICM42605_RA_INT_CONFIG, ICM42605_INT1_MODE_PULSED | ICM42605_INT1_DRIVE_CIRCUIT_PP | ICM42605_INT1_POLARITY_ACTIVE_HIGH);
delay(15);
busWrite(dev, ICM42605_RA_INT_CONFIG0, ICM42605_UI_DRDY_INT_CLEAR_ON_SBR);
delay(100);
#ifdef USE_MPU_DATA_READY_SIGNAL
uint8_t intConfig1Value;
busWrite(dev, ICM42605_RA_INT_SOURCE0, ICM42605_UI_DRDY_INT1_EN_ENABLED);
// Datasheet says: "User should change setting to 0 from default setting of 1, for proper INT1 and INT2 pin operation"
busRead(dev, ICM42605_RA_INT_CONFIG1, &intConfig1Value);
intConfig1Value &= ~(1 << ICM42605_INT_ASYNC_RESET_BIT);
intConfig1Value |= (ICM42605_INT_TPULSE_DURATION_8 | ICM42605_INT_TDEASSERT_DISABLED);
busWrite(dev, ICM42605_RA_INT_CONFIG1, intConfig1Value);
delay(15);
#endif
busSetSpeed(dev, BUS_SPEED_FAST);
}
static bool icm42605DeviceDetect(busDevice_t * dev)
{
uint8_t tmp;
uint8_t attemptsRemaining = 5;
busSetSpeed(dev, BUS_SPEED_INITIALIZATION);
busWrite(dev, ICM42605_RA_PWR_MGMT0, 0x00);
do {
delay(150);
busRead(dev, MPU_RA_WHO_AM_I, &tmp);
switch (tmp) {
/* ICM42605 and ICM42688P share the register structure*/
case ICM42605_WHO_AM_I_CONST:
case ICM42688P_WHO_AM_I_CONST:
return true;
default:
// Retry detection
break;
}
} while (attemptsRemaining--);
return false;
}
static bool icm42605GyroRead(gyroDev_t *gyro)
{
uint8_t data[6];
const bool ack = busReadBuf(gyro->busDev, ICM42605_RA_GYRO_DATA_X1, data, 6);
if (!ack) {
return false;
}
gyro->gyroADCRaw[X] = (int16_t)((data[0] << 8) | data[1]);
gyro->gyroADCRaw[Y] = (int16_t)((data[2] << 8) | data[3]);
gyro->gyroADCRaw[Z] = (int16_t)((data[4] << 8) | data[5]);
return true;
}
bool icm42605GyroDetect(gyroDev_t *gyro)
{
gyro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_ICM42605, gyro->imuSensorToUse, OWNER_MPU);
if (gyro->busDev == NULL) {
return false;
}
if (!icm42605DeviceDetect(gyro->busDev)) {
busDeviceDeInit(gyro->busDev);
return false;
}
// Magic number for ACC detection to indicate that we have detected icm42605 gyro
mpuContextData_t * ctx = busDeviceGetScratchpadMemory(gyro->busDev);
ctx->chipMagicNumber = 0x4265;
gyro->initFn = icm42605AccAndGyroInit;
gyro->readFn = icm42605GyroRead;
gyro->intStatusFn = gyroCheckDataReady;
gyro->temperatureFn = NULL;
gyro->scale = 1.0f / 16.4f; // 16.4 dps/lsb scalefactor
gyro->gyroAlign = gyro->busDev->param;
return true;
}
#endif

5
src/main/drivers/rangefinder/rangefinder_hcsr04_i2c.h → src/main/drivers/accgyro/accgyro_icm42605.h
View file

@ -17,6 +17,5 @@
#pragma once
#define RANGEFINDER_HCSR04_i2C_TASK_PERIOD_MS 100
bool hcsr04i2c0Detect(rangefinderDev_t *dev);
bool icm42605AccDetect(accDev_t *acc);
bool icm42605GyroDetect(gyroDev_t *gyro);

145
src/main/drivers/accgyro/accgyro_l3g4200d.c
View file

@ -1,145 +0,0 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <string.h>
#include <stdint.h>
#include <platform.h>
#include "drivers/system.h"
#include "drivers/time.h"
#include "drivers/bus_i2c.h"
#include "common/maths.h"
#include "common/axis.h"
#include "drivers/sensor.h"
#include "drivers/accgyro/accgyro.h"
#include "drivers/accgyro/accgyro_l3g4200d.h"
// L3G4200D, Standard address 0x68
#define L3G4200D_ADDRESS 0x68
#define L3G4200D_ID 0xD3
#define L3G4200D_AUTOINCR 0x80
// Registers
#define L3G4200D_WHO_AM_I 0x0F
#define L3G4200D_CTRL_REG1 0x20
#define L3G4200D_CTRL_REG2 0x21
#define L3G4200D_CTRL_REG3 0x22
#define L3G4200D_CTRL_REG4 0x23
#define L3G4200D_CTRL_REG5 0x24
#define L3G4200D_REFERENCE 0x25
#define L3G4200D_STATUS_REG 0x27
#define L3G4200D_GYRO_OUT 0x28
// Bits
#define L3G4200D_POWER_ON 0x0F
#define L3G4200D_FS_SEL_2000DPS 0xF0
#define L3G4200D_DLPF_32HZ 0x00
#define L3G4200D_DLPF_54HZ 0x40
#define L3G4200D_DLPF_78HZ 0x80
#define L3G4200D_DLPF_93HZ 0xC0
static void l3g4200dInit(gyroDev_t *gyro)
{
bool ack;
uint8_t mpuLowPassFilter = L3G4200D_DLPF_32HZ;
switch (gyro->lpf) {
default:
case 3: // BITS_DLPF_CFG_42HZ
mpuLowPassFilter = L3G4200D_DLPF_32HZ;
break;
case 2: // BITS_DLPF_CFG_98HZ
mpuLowPassFilter = L3G4200D_DLPF_54HZ;
break;
case 1: // BITS_DLPF_CFG_188HZ
mpuLowPassFilter = L3G4200D_DLPF_78HZ;
break;
case 0: // BITS_DLPF_CFG_256HZ
mpuLowPassFilter = L3G4200D_DLPF_93HZ;
break;
}
delay(100);
ack = busWrite(gyro->busDev, L3G4200D_CTRL_REG4, L3G4200D_FS_SEL_2000DPS);
if (!ack) {
failureMode(FAILURE_ACC_INIT);
}
delay(5);
busWrite(gyro->busDev, L3G4200D_CTRL_REG1, L3G4200D_POWER_ON | mpuLowPassFilter);
}
// Read 3 gyro values into user-provided buffer. No overrun checking is done.
static bool l3g4200dRead(gyroDev_t *gyro)
{
uint8_t buf[6];
if (!busReadBuf(gyro->busDev, L3G4200D_AUTOINCR | L3G4200D_GYRO_OUT, buf, 6)) {
return false;
}
gyro->gyroADCRaw[X] = (int16_t)((buf[0] << 8) | buf[1]);
gyro->gyroADCRaw[Y] = (int16_t)((buf[2] << 8) | buf[3]);
gyro->gyroADCRaw[Z] = (int16_t)((buf[4] << 8) | buf[5]);
return true;
}
static bool deviceDetect(busDevice_t * busDev)
{
busSetSpeed(busDev, BUS_SPEED_INITIALIZATION);
for (int retry = 0; retry < 5; retry++) {
uint8_t deviceid;
delay(150);
bool ack = busRead(busDev, L3G4200D_WHO_AM_I, &deviceid);
if (ack && deviceid == L3G4200D_ID) {
return true;
}
};
return false;
}
bool l3g4200dDetect(gyroDev_t *gyro)
{
gyro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_L3G4200, gyro->imuSensorToUse, OWNER_MPU);
if (gyro->busDev == NULL) {
return false;
}
if (!deviceDetect(gyro->busDev)) {
busDeviceDeInit(gyro->busDev);
return false;
}
gyro->initFn = l3g4200dInit;
gyro->readFn = l3g4200dRead;
gyro->scale = 1.0f / 14.2857f; // 14.2857dps/lsb scalefactor
gyro->gyroAlign = gyro->busDev->param;
return true;
}

20
src/main/drivers/accgyro/accgyro_l3g4200d.h
View file

@ -1,20 +0,0 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
bool l3g4200dDetect(gyroDev_t *gyro);

144
src/main/drivers/accgyro/accgyro_mma845x.c
View file

@ -1,144 +0,0 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include "platform.h"
#include "drivers/time.h"
#include "drivers/io.h"
#include "drivers/bus_i2c.h"
#include "drivers/sensor.h"
#include "drivers/accgyro/accgyro.h"
#include "drivers/accgyro/accgyro_mma845x.h"
// MMA8452QT, Standard address 0x1C
// ACC_INT2 routed to PA5
#define MMA8452_ADDRESS 0x1C
#define MMA8452_DEVICE_SIGNATURE 0x2A
#define MMA8451_DEVICE_SIGNATURE 0x1A
#define MMA8452_STATUS 0x00
#define MMA8452_OUT_X_MSB 0x01
#define MMA8452_WHO_AM_I 0x0D
#define MMA8452_XYZ_DATA_CFG 0x0E
#define MMA8452_HP_FILTER_CUTOFF 0x0F
#define MMA8452_CTRL_REG1 0x2A
#define MMA8452_CTRL_REG2 0x2B
#define MMA8452_CTRL_REG3 0x2C
#define MMA8452_CTRL_REG4 0x2D
#define MMA8452_CTRL_REG5 0x2E
#define MMA8452_FS_RANGE_8G 0x02
#define MMA8452_FS_RANGE_4G 0x01
#define MMA8452_FS_RANGE_2G 0x00
#define MMA8452_HPF_CUTOFF_LV1 0x00
#define MMA8452_HPF_CUTOFF_LV2 0x01
#define MMA8452_HPF_CUTOFF_LV3 0x02
#define MMA8452_HPF_CUTOFF_LV4 0x03
#define MMA8452_CTRL_REG2_B7_ST 0x80
#define MMA8452_CTRL_REG2_B6_RST 0x40
#define MMA8452_CTRL_REG2_B4_SMODS1 0x10
#define MMA8452_CTRL_REG2_B3_SMODS0 0x08
#define MMA8452_CTRL_REG2_B2_SLPE 0x04
#define MMA8452_CTRL_REG2_B1_MODS1 0x02
#define MMA8452_CTRL_REG2_B0_MODS0 0x01
#define MMA8452_CTRL_REG2_MODS_LP 0x03
#define MMA8452_CTRL_REG2_MODS_HR 0x02
#define MMA8452_CTRL_REG2_MODS_LNLP 0x01
#define MMA8452_CTRL_REG2_MODS_NOR 0x00
#define MMA8452_CTRL_REG3_IPOL 0x02
#define MMA8452_CTRL_REG4_INT_EN_DRDY 0x01
#define MMA8452_CTRL_REG1_LNOISE 0x04
#define MMA8452_CTRL_REG1_ACTIVE 0x01
/*
static inline void mma8451ConfigureInterrupt(void)
{
busWrite(acc->busDev, MMA8452_CTRL_REG3, MMA8452_CTRL_REG3_IPOL); // Interrupt polarity (active HIGH)
busWrite(acc->busDev, MMA8452_CTRL_REG4, MMA8452_CTRL_REG4_INT_EN_DRDY); // Enable DRDY interrupt (unused by this driver)
busWrite(acc->busDev, MMA8452_CTRL_REG5, 0); // DRDY routed to INT2
}
*/
static void mma8452Init(accDev_t *acc)
{
busWrite(acc->busDev, MMA8452_CTRL_REG1, 0); // Put device in standby to configure stuff
busWrite(acc->busDev, MMA8452_XYZ_DATA_CFG, MMA8452_FS_RANGE_8G);
busWrite(acc->busDev, MMA8452_HP_FILTER_CUTOFF, MMA8452_HPF_CUTOFF_LV4);
busWrite(acc->busDev, MMA8452_CTRL_REG2, MMA8452_CTRL_REG2_MODS_HR | MMA8452_CTRL_REG2_MODS_HR << 3); // High resolution measurement in both sleep and active modes
// mma8451ConfigureInterrupt();
busWrite(acc->busDev, MMA8452_CTRL_REG1, MMA8452_CTRL_REG1_LNOISE | MMA8452_CTRL_REG1_ACTIVE); // Turn on measurements, low noise at max scale mode, Data Rate 800Hz. LNoise mode makes range +-4G.
acc->acc_1G = 256;
}
static bool mma8452Read(accDev_t *acc)
{
uint8_t buf[6];
if (!busReadBuf(acc->busDev, MMA8452_OUT_X_MSB, buf, 6)) {
return false;
}
acc->ADCRaw[0] = ((int16_t)((buf[0] << 8) | buf[1]) >> 2) / 4;
acc->ADCRaw[1] = ((int16_t)((buf[2] << 8) | buf[3]) >> 2) / 4;
acc->ADCRaw[2] = ((int16_t)((buf[4] << 8) | buf[5]) >> 2) / 4;
return true;
}
static bool deviceDetect(busDevice_t * busDev)
{
bool ack = false;
uint8_t sig = 0;
ack = busRead(busDev, MMA8452_WHO_AM_I, &sig);
if (!ack || (sig != MMA8452_DEVICE_SIGNATURE && sig != MMA8451_DEVICE_SIGNATURE))
return false;
return true;
}
bool mma8452Detect(accDev_t *acc)
{
acc->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_MMA8452, acc->imuSensorToUse, OWNER_MPU);
if (acc->busDev == NULL) {
return false;
}
if (!deviceDetect(acc->busDev)) {
busDeviceDeInit(acc->busDev);
return false;
}
acc->initFn = mma8452Init;
acc->readFn = mma8452Read;
acc->accAlign = acc->busDev->param;
return true;
}

20
src/main/drivers/accgyro/accgyro_mma845x.h
View file

@ -1,20 +0,0 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
bool mma8452Detect(accDev_t *acc);

2
src/main/drivers/accgyro/accgyro_mpu.h
View file

@ -35,6 +35,8 @@
#define ICM20602_WHO_AM_I_CONST (0x12)
#define ICM20608G_WHO_AM_I_CONST (0xAF)
#define ICM20689_WHO_AM_I_CONST (0x98)
#define ICM42605_WHO_AM_I_CONST (0x42)
#define ICM42688P_WHO_AM_I_CONST (0x47)
// RA = Register Address

41
src/main/drivers/barometer/barometer_bmp388.c
View file

@ -197,7 +197,13 @@ static bool bmp388StartUP(baroDev_t *baro)
static bool bmp388GetUP(baroDev_t *baro)
{
busReadBuf(baro->busDev, BMP388_DATA_0_REG, sensor_data, BMP388_DATA_FRAME_SIZE + 1);
if (baro->busDev->busType == BUSTYPE_SPI) {
// In SPI mode, first byte read is a dummy byte
busReadBuf(baro->busDev, BMP388_DATA_0_REG, &sensor_data[0], BMP388_DATA_FRAME_SIZE + 1);
} else {
// In I2C mode, no dummy byte is read
busReadBuf(baro->busDev, BMP388_DATA_0_REG, &sensor_data[1], BMP388_DATA_FRAME_SIZE);
}
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;
@ -289,14 +295,26 @@ STATIC_UNIT_TESTED bool bmp388Calculate(baroDev_t *baro, int32_t *pressure, int3
#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];
uint8_t chipId[2];
uint8_t nRead;
uint8_t * pId;
if (busDev->busType == BUSTYPE_SPI) {
// In SPI mode, first byte read is a dummy byte
nRead = 2;
pId = &chipId[1];
} else {
// In I2C mode, no dummy byte is read
nRead = 1;
pId = &chipId[0];
}
for (int retry = 0; retry < DETECTION_MAX_RETRY_COUNT; retry++) {
delay(100);
bool ack = busReadBuf(busDev, BMP388_CHIP_ID_REG, chipId, 2);
bool ack = busReadBuf(busDev, BMP388_CHIP_ID_REG, chipId, nRead);
if (ack && chipId[1] == BMP388_DEFAULT_CHIP_ID) {
if (ack && *pId == BMP388_DEFAULT_CHIP_ID) {
return true;
}
};
@ -318,11 +336,16 @@ bool bmp388Detect(baroDev_t *baro)
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));
if (baro->busDev->busType == BUSTYPE_SPI) {
// In SPI mode, first byte read is a dummy byte
uint8_t calibration_buf[sizeof(bmp388_calib_param_t) + 1];
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));
} else {
// In I2C mode, no dummy byte is read
busReadBuf(baro->busDev, BMP388_TRIMMING_NVM_PAR_T1_LSB_REG, (uint8_t*)&bmp388_cal, sizeof(bmp388_calib_param_t));
}
// set oversampling + power mode (forced), and start sampling
busWrite(baro->busDev, BMP388_OSR_REG,

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

@ -76,14 +76,10 @@ typedef enum {
DEVHW_NONE = 0,
/* Dedicated ACC chips */
DEVHW_BMA280,
DEVHW_ADXL345,
DEVHW_MMA8452,
DEVHW_LSM303DLHC,
/* Dedicated GYRO chips */
DEVHW_L3GD20,
DEVHW_L3G4200,
/* Combined ACC/GYRO chips */
DEVHW_MPU3050,
@ -94,6 +90,8 @@ typedef enum {
DEVHW_BMI088_GYRO,
DEVHW_BMI088_ACC,
DEVHW_ICM20689,
DEVHW_ICM42605,
DEVHW_BMI270,
/* Combined ACC/GYRO/MAG chips */
DEVHW_MPU9250,
@ -119,6 +117,8 @@ typedef enum {
DEVHW_MAG3110,
DEVHW_LIS3MDL,
DEVHW_RM3100,
DEVHW_VCM5883,
DEVHW_MLX90393,
/* Temp sensor chips */
DEVHW_LM75_0,
@ -138,10 +138,10 @@ typedef enum {
/* Rangefinder modules */
DEVHW_SRF10,
DEVHW_HCSR04_I2C, // DIY-style adapter
DEVHW_VL53L0X,
DEVHW_VL53L1X,
DEVHW_US42,
DEVHW_TOF10120_I2C,
/* Other hardware */
DEVHW_MS4525, // Pitot meter

176
src/main/drivers/compass/compass_mlx90393.c Normal file
View file

@ -0,0 +1,176 @@
/*
* This file is part of iNav.
*
* iNav is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* iNav is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with iNav. If not, see <http://www.gnu.org/licenses/>.
*/
#include "platform.h"
#ifdef USE_MAG_MLX90393
#include <stdbool.h>
#include <stdint.h>
#include <math.h>
#include "build/debug.h"
#include "common/axis.h"
#include "common/maths.h"
#include "common/utils.h"
#include "drivers/time.h"
#include "drivers/io.h"
#include "drivers/bus.h"
#include "drivers/sensor.h"
#include "drivers/compass/compass.h"
#define MLX90393_MEASURE_3D 0b00001110
#define MLX90393_NOP 0b00000000
#define MLX90393_START_BURST_MODE 0b00010000 //uses with zyxt flags
#define MLX90393_START_WAKE_UP_ON_CHANGE_MODE 0b00100000 //uses with zyxt flags
#define MLX90393_START_SINGLE_MEASUREMENT_MODE 0b00110000 //uses with zyxt flags
#define MLX90393_READ_MEASUREMENT 0b01000000 //uses with zyxt flags
#define MLX90393_READ_REGISTER 0b01010000
#define MLX90393_WRITE_REGISTER 0b01100000
#define MLX90393_EXIT_MODE 0b10000000
#define MLX90393_MEMORY_RECALL 0b11010000
#define MLX90393_MEMORY_STORE 0b11100000
#define MLX90393_RESET 0b11110000
#define MLX90393_BURST_MODE_FLAG 0b10000000
#define MLX90393_WAKE_UP_ON_CHANGE_MODE_FLAG 0b01000000
#define MLX90393_SINGLE_MEASUREMENT_MODE_FLAG 0b00100000
#define MLX90393_ERROR_FLAG 0b00010000
#define MLX90393_SINGLE_ERROR_DETECTION_FLAG 0b00001000
#define MLX90393_RESET_FLAG 0b00000100
#define MLX90393_D1_FLAG 0b00000010
#define MLX90393_D0_FLAG 0b00000001
#define DETECTION_MAX_RETRY_COUNT 5
#define REG_BUF_LEN 3
// Register 1
#define GAIN_SEL_HALLCONF_REG 0x0 //from datasheet 0x0 << 2 = 0x0
// GAIN - 0b111
// Hall_conf - 0xC
#define GAIN_SEL_HALLCONF_REG_VALUE 0x007C
// Register 2
#define TCMP_EN_REG 0x4 //from datasheet 0x1 << 2 = 0x4
// Burst Data Rate 0b000100
#define TCMP_EN_REG_VALUE 0x62C4
// Register 3
#define RES_XYZ_OSR_FLT_REG 0x8 //from datasheet 0x2 << 2 = 0x8
// Oversampling 0b01
// Filtering 0b111
#define RES_XYZ_OSR_FLT_REG_VALUE 0x001D
static void mlx90393WriteRegister(magDev_t * mag, uint8_t reg_val, uint16_t value) {
uint8_t buf[REG_BUF_LEN] = {0};
buf[0] = (value >> 8) & 0xFF;
buf[1] = (value >> 0) & 0xFF;
buf[2] = reg_val;
busWriteBuf(mag->busDev, MLX90393_WRITE_REGISTER, buf, REG_BUF_LEN);
// PAUSE
delay(20);
// To skip ACK FLAG of Write
uint8_t sig = 0;
busRead(mag->busDev, MLX90393_NOP, &sig);
return;
}
// =======================================================================================
static bool mlx90393Read(magDev_t * mag)
{
uint8_t buf[7] = {0};
busReadBuf(mag->busDev, MLX90393_READ_MEASUREMENT | MLX90393_MEASURE_3D, buf, 7);
mag->magADCRaw[X] = ((short)(buf[1] << 8 | buf[2]));
mag->magADCRaw[Y] = ((short)(buf[3] << 8 | buf[4]));
mag->magADCRaw[Z] = ((short)(buf[5] << 8 | buf[6]));
return true;
}
static bool deviceDetect(magDev_t * mag)
{
for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) {
uint8_t sig = 0;
bool ack = busRead(mag->busDev, MLX90393_RESET, &sig);
delay(20);
if (ack && ((sig & MLX90393_RESET_FLAG) == MLX90393_RESET_FLAG)) { // Check Reset Flag -> MLX90393
return true;
}
}
return false;
}
//--------------------------------------------------
static bool mlx90393Init(magDev_t * mag)
{
uint8_t sig = 0;
delay(20);
// Remove reset flag
busRead(mag->busDev, MLX90393_NOP, &sig);
// Exit if already in burst mode. (For example when external i2c source power the bus.)
busRead(mag->busDev, MLX90393_EXIT_MODE, &sig);
// Writing Registers
mlx90393WriteRegister(mag, GAIN_SEL_HALLCONF_REG, GAIN_SEL_HALLCONF_REG_VALUE);
mlx90393WriteRegister(mag, TCMP_EN_REG, TCMP_EN_REG_VALUE);
mlx90393WriteRegister(mag, RES_XYZ_OSR_FLT_REG, RES_XYZ_OSR_FLT_REG_VALUE);
// Start burst mode
busRead(mag->busDev, MLX90393_START_BURST_MODE | MLX90393_MEASURE_3D, &sig);
return true;
}
// ==========================================================================
bool mlx90393Detect(magDev_t * mag)
{
mag->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_MLX90393, mag->magSensorToUse, OWNER_COMPASS);
if (mag->busDev == NULL) {
return false;
}
if (!deviceDetect(mag)) {
busDeviceDeInit(mag->busDev);
return false;
}
mag->init = mlx90393Init;
mag->read = mlx90393Read;
return true;
}
#endif

10
src/main/drivers/accgyro/accgyro_bma280.h → src/main/drivers/compass/compass_mlx90393.h
View file

@ -1,20 +1,20 @@
/*
* This file is part of Cleanflight.
* This file is part of iNav.
*
* Cleanflight is free software: you can redistribute it and/or modify
* iNav is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* iNav is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
* along with iNav. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
bool bma280Detect(accDev_t *acc);
bool mlx90393Detect(magDev_t* mag);

120
src/main/drivers/compass/compass_vcm5883.c Normal file
View file

@ -0,0 +1,120 @@
/*
* 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 "platform.h"
#ifdef USE_MAG_VCM5883
#include <stdbool.h>
#include <stdint.h>
#include <math.h>
#include "build/build_config.h"
#include "common/axis.h"
#include "common/maths.h"
#include "common/utils.h"
#include "drivers/time.h"
#include "drivers/bus_i2c.h"
#include "sensors/boardalignment.h"
#include "sensors/sensors.h"
#include "drivers/sensor.h"
#include "drivers/compass/compass.h"
#include "drivers/compass/compass_vcm5883.h"
#include "build/debug.h"
#define VCM5883_REGISTER_ADDR_CNTL1 0x0B
#define VCM5883_REGISTER_ADDR_CNTL2 0x0A
#define VCM5883_REGISTER_ADDR_CHIPID 0x0C
#define VCM5883_REGISTER_ADDR_OUTPUT_X 0x00
#define VCM5883_INITIAL_CONFIG 0b0100
#define DETECTION_MAX_RETRY_COUNT 5
static bool deviceDetect(magDev_t * mag)
{
for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) {
busWrite(mag->busDev, VCM5883_REGISTER_ADDR_CNTL2, 0b01000001);
delay(30);
uint8_t sig = 0;
bool ack = busRead(mag->busDev, VCM5883_REGISTER_ADDR_CHIPID, &sig);
if (ack && sig == 0x82) {
return true;
}
}
return false;
}
static bool vcm5883Init(magDev_t * mag) {
UNUSED(mag);
return true;
}
static bool vcm5883Read(magDev_t * mag)
{
uint8_t buf[6];
// set magData to zero for case of failed read
mag->magADCRaw[X] = 0;
mag->magADCRaw[Y] = 0;
mag->magADCRaw[Z] = 0;
bool ack = busReadBuf(mag->busDev, VCM5883_REGISTER_ADDR_OUTPUT_X, buf, 6);
if (!ack) {
return false;
}
mag->magADCRaw[X] = (int16_t)(buf[1] << 8 | buf[0]);
mag->magADCRaw[Y] = (int16_t)(buf[3] << 8 | buf[2]);
mag->magADCRaw[Z] = (int16_t)(buf[5] << 8 | buf[4]);
return true;
}
bool vcm5883Detect(magDev_t * mag)
{
mag->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_VCM5883, mag->magSensorToUse, OWNER_COMPASS);
if (mag->busDev == NULL) {
return false;
}
if (!deviceDetect(mag)) {
busDeviceDeInit(mag->busDev);
return false;
}
mag->init = vcm5883Init;
mag->read = vcm5883Read;
return true;
}
#endif

6
src/main/io/esc_serialshot.h → src/main/drivers/compass/compass_vcm5883.h
View file

@ -1,5 +1,5 @@
/*
* This file is part of INAV Project.
* 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,
@ -24,6 +24,4 @@
#pragma once
bool serialshotInitialize(void);
void serialshotUpdateMotor(int index, uint16_t value);
void serialshotSendUpdate(void);
bool vcm5883Detect(magDev_t *mag);

10
src/main/drivers/flash.c
View file

@ -196,7 +196,15 @@ static void flashConfigurePartitions(void)
#endif
#if defined(CONFIG_IN_EXTERNAL_FLASH)
createPartition(FLASH_PARTITION_TYPE_CONFIG, EEPROM_SIZE, &endSector);
uint32_t configSize = EEPROM_SIZE;
flashSector_t configSectors = (configSize / flashGeometry->sectorSize);
if (configSize % flashGeometry->sectorSize > 0) {
configSectors++; // needs a portion of a sector.
}
configSize = configSectors * flashGeometry->sectorSize;
createPartition(FLASH_PARTITION_TYPE_CONFIG, configSize, &endSector);
#endif
#ifdef USE_FLASHFS

45
src/main/drivers/flash_m25p16.c
View file

@ -68,6 +68,7 @@ static flashGeometry_t geometry = {.pageSize = M25P16_PAGESIZE};
static busDevice_t * busDev = NULL;
static bool isLargeFlash = false;
static uint32_t timeoutAt = 0;
/*
* Whether we've performed an action that could have made the device busy for writes.
@ -114,15 +115,30 @@ bool m25p16_isReady(void)
return !couldBeBusy;
}
static void m25p16_setTimeout(uint32_t timeoutMillis)
{
uint32_t now = millis();
timeoutAt = now + timeoutMillis;
}
bool m25p16_waitForReady(uint32_t timeoutMillis)
{
uint32_t time = millis();
uint32_t timeoutAtUse;
if (timeoutMillis == 0) {
timeoutAtUse = timeoutAt;
}
else {
timeoutAtUse = millis() + timeoutMillis;
}
while (!m25p16_isReady()) {
if (timeoutMillis && (millis() - time > timeoutMillis)) {
uint32_t now = millis();
if (now >= timeoutAtUse) {
return false;
}
}
timeoutAt = 0;
return true;
}
@ -216,10 +232,6 @@ static bool m25p16_readIdentification(void)
*/
bool m25p16_init(int flashNumToUse)
{
if (busDev) {
return true;
}
busDev = busDeviceInit(BUSTYPE_SPI, DEVHW_M25P16, flashNumToUse, OWNER_FLASH);
if (busDev == NULL) {
return false;
@ -252,20 +264,28 @@ void m25p16_eraseSector(uint32_t address)
m25p16_setCommandAddress(&out[1], address, isLargeFlash);
m25p16_waitForReady(SECTOR_ERASE_TIMEOUT_MILLIS);
if (!m25p16_waitForReady(0)) {
return;
}
m25p16_writeEnable();
busTransfer(busDev, NULL, out, isLargeFlash ? 5 : 4);
m25p16_setTimeout(SECTOR_ERASE_TIMEOUT_MILLIS);
}
void m25p16_eraseCompletely(void)
{
m25p16_waitForReady(BULK_ERASE_TIMEOUT_MILLIS);
if (!m25p16_waitForReady(0)) {
return;
}
m25p16_writeEnable();
m25p16_performOneByteCommand(M25P16_INSTRUCTION_BULK_ERASE);
m25p16_setTimeout(BULK_ERASE_TIMEOUT_MILLIS);
}
/**
@ -294,12 +314,17 @@ uint32_t m25p16_pageProgram(uint32_t address, const uint8_t *data, int length)
m25p16_setCommandAddress(&command[1], address, isLargeFlash);
m25p16_waitForReady(DEFAULT_TIMEOUT_MILLIS);
if (!m25p16_waitForReady(0)) {
// return same address to indicate timeout
return address;
}
m25p16_writeEnable();
busTransferMultiple(busDev, txn, 2);
m25p16_setTimeout(DEFAULT_TIMEOUT_MILLIS);
return address + length;
}
@ -322,7 +347,7 @@ int m25p16_readBytes(uint32_t address, uint8_t *buffer, int length)
m25p16_setCommandAddress(&command[1], address, isLargeFlash);
if (!m25p16_waitForReady(DEFAULT_TIMEOUT_MILLIS)) {
if (!m25p16_waitForReady(0)) {
return 0;
}

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

@ -5,6 +5,8 @@
#include "build/build_config.h"
#ifdef USE_PWM_DRIVER_PCA9685
#include "common/time.h"
#include "common/maths.h"
@ -141,3 +143,5 @@ bool pca9685Initialize(void)
return true;
}
#endif

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

@ -23,232 +23,219 @@
#ifdef USE_OSD
#define SYM_RSSI 0x01 // 001 Icon RSSI
#define SYM_AH_LEFT 0x02 // 002 Arrow left
#define SYM_AH_RIGHT 0x03 // 003 Arrow right
#define SYM_THR 0x04 // 004 Throttle
#define SYM_AH_DECORATION_UP 0x05 // 005 Arrow up AHI
#define SYM_VOLT 0x06 // 006 V
#define SYM_MAH 0x07 // 007 MAH
#define SYM_RSSI 0x01 // 001 Icon RSSI
#define SYM_LQ 0x02 // 002 LQ
#define SYM_LAT 0x03 // 003 GPS LAT
#define SYM_LON 0x04 // 004 GPS LON
#define SYM_AZIMUTH 0x05 // 005 Azimuth
#define SYM_TELEMETRY_0 0x06 // 006 Antenna tracking telemetry
#define SYM_TELEMETRY_1 0x07 // 007 Antenna tracking telemetry
#define SYM_SAT_L 0x08 // 008 Sats left
#define SYM_SAT_R 0x09 // 009 Sats right
#define SYM_HOME_NEAR 0x0A // 010 Home, near
#define SYM_DEGREES 0x0B // 011 ° heading angle
#define SYM_HEADING 0x0C // 012 Compass Heading symbol
#define SYM_SCALE 0x0D // 013 Map scale
#define SYM_HDP_L 0x0E // 014 HDOP left
#define SYM_HDP_R 0x0F // 015 HDOP right
#define SYM_HOME 0x10 // 016 Home icon
#define SYM_2RSS 0x11 // 017 RSSI 2
#define SYM_DB 0x12 // 018 dB
#define SYM_DBM 0x13 // 019 dBm
#define SYM_SNR 0x14 // 020 SNR
#define SYM_AH_KM 0x08 // 008 Ah/km
#define SYM_AH_MI 0x09 // 009 Ah/mi
#define SYM_MAH_MI_0 0x0A // 010 mAh/mi left
#define SYM_MAH_MI_1 0x0B // 011 mAh/mi right
#define SYM_LQ 0x0C // 012 LQ
#define SYM_AH_DECORATION_UP 0x15 // 021 Arrow up AHI
#define SYM_AH_DECORATION_DOWN 0x16 // 022 Arrow down AHI
#define SYM_DIRECTION 0x17 // 023 to 030, directional little arrows
#define SYM_TEMP_F 0x0D // 013 °F
#define SYM_TEMP_C 0x0E // 014 °C
#define SYM_FT 0x0F // 015 FT
#define SYM_VOLT 0x1F // 031 VOLTS
#define SYM_MAH 0x99 // 153 mAh
// 0x21 // 033 ASCII !
#define SYM_AH_KM 0x22 // 034 Ah/km
// 0x23 // 035 ASCII #
#define SYM_AH_MI 0x24 // 036 Ah/mi
// 0x25 // 037 ASCII %
// 0x26 // 038 ASCII &
#define SYM_VTX_POWER 0x27 // 039 VTx Power
// 0x28 // 040 to 062 ASCII
#define SYM_AH_NM 0x3F // 063 Ah/NM
// 0x40 // 064 to 095 ASCII
#define SYM_MAH_NM_0 0x60 // 096 mAh/NM left
#define SYM_MAH_NM_1 0x61 // 097 mAh/NM right
#define SYM_MAH_KM_0 0x6B // 107 MAH/KM left
#define SYM_MAH_KM_1 0x6C // 108 MAH/KM right
#define SYM_MILLIOHM 0x62 // 098 battery impedance Mohm
#define SYM_AH_DECORATION_MIN 0x10 // 016 to 021 Scrolling
#define SYM_AH_DECORATION 0x13 // 019 Scrolling
#define SYM_AH_DECORATION_MAX 0x15 // 021 Scrolling
#define SYM_BATT_FULL 0x63 // 099 Battery full
#define SYM_BATT_5 0x64 // 100 Battery
#define SYM_BATT_4 0x65 // 101 Battery
#define SYM_BATT_3 0x66 // 102 Battery
#define SYM_BATT_2 0x67 // 103 Battery
#define SYM_BATT_1 0x68 // 104 Battery
#define SYM_BATT_EMPTY 0x69 // 105 Battery empty
#define SYM_AMP 0x6A // 106 AMPS
#define SYM_WH 0x6D // 109 WH
#define SYM_WH_KM 0x6E // 110 WH/KM
#define SYM_WH_MI 0x6F // 111 WH/MI
#define SYM_WH_NM 0x70 // 112 Wh/NM
#define SYM_WATT 0x71 // 113 WATTS
#define SYM_MW 0x72 // 114 mW
#define SYM_KILOWATT 0x73 // 115 kW
#define SYM_FT 0x74 // 116 FEET
#define SYM_TRIP_DIST 0x75 // 117 Trip distance
#define SYM_TOTAL 0x75 // 117 Total
#define SYM_ALT_M 0x76 // 118 ALT M
#define SYM_ALT_KM 0x77 // 119 ALT KM
#define SYM_ALT_FT 0x78 // 120 ALT FT
#define SYM_ALT_KFT 0x79 // 121 Alt KFT
#define SYM_DIST_M 0x7A // 122 DIS M
// 0x7B // 123 to 125 ASCII
#define SYM_DIST_KM 0x7E // 126 DIST KM
#define SYM_DIST_FT 0x7F // 127 DIST FT
#define SYM_DIST_MI 0x80 // 128 DIST MI
#define SYM_DIST_NM 0x81 // 129 DIST NM
#define SYM_M 0x82 // 130 M
#define SYM_KM 0x83 // 131 KM
#define SYM_MI 0x84 // 132 MI
#define SYM_NM 0x85 // 133 NM
#define SYM_WIND_HORIZONTAL 0x86 // 134 Air speed horizontal
#define SYM_WIND_VERTICAL 0x87 // 135 Air speed vertical
#define SYM_3D_KMH 0x88 // 136 KM/H 3D
#define SYM_3D_MPH 0x89 // 137 MPH 3D
#define SYM_3D_KT 0x8A // 138 Knots 3D
#define SYM_RPM 0x8B // 139 RPM
#define SYM_AIR 0x8C // 140 Air speed
#define SYM_FTS 0x8D // 141 FT/S
#define SYM_100FTM 0x8E // 142 100 Feet per Min
#define SYM_MS 0x8F // 143 M/S
#define SYM_KMH 0x90 // 144 KM/H
#define SYM_MPH 0x91 // 145 MPH
#define SYM_KT 0x92 // 146 Knots
#define SYM_MAH_MI_0 0x93 // 147 mAh/mi left
#define SYM_MAH_MI_1 0x94 // 148 mAh/mi right
#define SYM_THR 0x95 // 149 Throttle
#define SYM_TEMP_F 0x96 // 150 °F
#define SYM_TEMP_C 0x97 // 151 °C
// 0x98 // 152 Home point map
#define SYM_BLANK 0x20 // 32 blank (space)
#define SYM_ON_H 0x9A // 154 ON HR
#define SYM_FLY_H 0x9B // 155 FLY HR
#define SYM_ON_M 0x9E // 158 On MIN
#define SYM_FLY_M 0x9F // 159 FL MIN
#define SYM_GLIDESLOPE 0x9C // 156 Glideslope
#define SYM_WAYPOINT 0x9D // 157 Waypoint
#define SYM_CLOCK 0xA0 // 160 Clock
#define SYM_ZERO_HALF_TRAILING_DOT 0xA1 // 161 to 170 Numbers with trailing dot
#define SYM_ZERO_HALF_LEADING_DOT 0xB1 // 177 to 186 Numbers with leading dot
#define SYM_AUTO_THR0 0xAB // 171 Auto-throttle left
#define SYM_AUTO_THR1 0xAC // 172 Auto-throttle right
#define SYM_ROLL_LEFT 0xAD // 173 Sym roll left
#define SYM_ROLL_LEVEL 0xAE // 174 Sym roll horizontal
#define SYM_ROLL_RIGHT 0xAF // 175 Sym roll right
#define SYM_PITCH_UP 0xB0 // 176 Pitch up
#define SYM_PITCH_DOWN 0xBB // 187 Pitch down
#define SYM_GFORCE 0xBC // 188 Gforce (all axis)
#define SYM_GFORCE_X 0xBD // 189 Gforce X
#define SYM_GFORCE_Y 0xBE // 190 Gforce Y
#define SYM_GFORCE_Z 0xBF // 191 Gforce Z
#define SYM_BARO_TEMP 0xC0 // 192
#define SYM_IMU_TEMP 0xC1 // 193
#define SYM_TEMP 0xC2 // 194 Thermometer icon
#define SYM_TEMP_SENSOR_FIRST 0xC2 // 194
#define SYM_ESC_TEMP 0xC3 // 195
#define SYM_TEMP_SENSOR_LAST 0xC7 // 199
#define TEMP_SENSOR_SYM_COUNT (SYM_TEMP_SENSOR_LAST - SYM_TEMP_SENSOR_FIRST + 1)
#define SYM_HEADING_N 0xC8 // 200 Heading Graphic north
#define SYM_HEADING_S 0xC9 // 201 Heading Graphic south
#define SYM_HEADING_E 0xCA // 202 Heading Graphic east
#define SYM_HEADING_W 0xCB // 203 Heading Graphic west
#define SYM_HEADING_DIVIDED_LINE 0xCC // 204 Heading Graphic
#define SYM_HEADING_LINE 0xCD // 205 Heading Graphic
#define SYM_MAX 0xCE // 206 MAX symbol
#define SYM_LOGO_START 0x101 // 257 to 280, INAV logo
#define SYM_LOGO_WIDTH 6
#define SYM_LOGO_HEIGHT 4
#define SYM_AH_LEFT 0x12C // 300 Arrow left
#define SYM_AH_RIGHT 0x12D // 301 Arrow right
#define SYM_AH_DECORATION_MIN 0x12E // 302 to 307 Scrolling
#define SYM_AH_DECORATION 0x131 // 305 Scrolling
#define SYM_AH_DECORATION_MAX 0x133 // 307 Scrolling
#define SYM_AH_DECORATION_COUNT (SYM_AH_DECORATION_MAX - SYM_AH_DECORATION_MIN + 1) // Scrolling
#define SYM_WIND_HORIZONTAL 0x16 // 022 Air speed horizontal
#define SYM_WIND_VERTICAL 0x17 // 023 Air speed vertical
#define SYM_AH_CH_LEFT 0x13A // 314 Crossair left
#define SYM_AH_CH_RIGHT 0x13B // 315 Crossair right
#define SYM_HEADING_N 0x18 // 024 Heading Graphic north
#define SYM_HEADING_S 0x19 // 025 Heading Graphic south
#define SYM_HEADING_E 0x1A // 026 Heading Graphic east
#define SYM_HEADING_W 0x1B // 027 Heading Graphic west
#define SYM_HEADING_DIVIDED_LINE 0x1C // 028 Heading Graphic
#define SYM_HEADING_LINE 0x1D // 029 Heading Graphic
#define SYM_ARROW_UP 0x13C // 316 Direction arrow 0°
#define SYM_ARROW_2 0x13D // 317 Direction arrow 22.5°
#define SYM_ARROW_3 0x13E // 318 Direction arrow 45°
#define SYM_ARROW_4 0x13F // 319 Direction arrow 67.5°
#define SYM_ARROW_RIGHT 0x140 // 320 Direction arrow 90°
#define SYM_ARROW_6 0x141 // 321 Direction arrow 112.5°
#define SYM_ARROW_7 0x142 // 322 Direction arrow 135°
#define SYM_ARROW_8 0x143 // 323 Direction arrow 157.5°
#define SYM_ARROW_DOWN 0x144 // 324 Direction arrow 180°
#define SYM_ARROW_10 0x145 // 325 Direction arrow 202.5°
#define SYM_ARROW_11 0x146 // 326 Direction arrow 225°
#define SYM_ARROW_12 0x147 // 327 Direction arrow 247.5°
#define SYM_ARROW_LEFT 0x148 // 328 Direction arrow 270°
#define SYM_ARROW_14 0x149 // 329 Direction arrow 292.5°
#define SYM_ARROW_15 0x14A // 330 Direction arrow 315°
#define SYM_ARROW_16 0x14B // 331 Direction arrow 337.5°
#define SYM_SAT_L 0x1E // 030 Sats left
#define SYM_SAT_R 0x1F // 031 Sats right
#define SYM_AH_H_START 0x14C // 332 to 340 Horizontal AHI
#define SYM_AH_V_START 0x15A // 346 to 351 Vertical AHI
#define SYM_BLANK 0x20 // 032 blank (space)
#define SYM_VARIO_UP_2A 0x155 // 341 Vario up up
#define SYM_VARIO_UP_1A 0x156 // 342 Vario up
#define SYM_VARIO_DOWN_1A 0x157 // 343 Vario down
#define SYM_VARIO_DOWN_2A 0x158 // 344 Vario down down
#define SYM_ALT 0x159 // 345 ALT
// 0x21 // 033 ASCII !
#define SYM_HUD_SIGNAL_0 0x160 // 352 Hud signal icon Lost
#define SYM_HUD_SIGNAL_1 0x161 // 353 Hud signal icon 25%
#define SYM_HUD_SIGNAL_2 0x162 // 354 Hud signal icon 50%
#define SYM_HUD_SIGNAL_3 0x163 // 355 Hud signal icon 75%
#define SYM_HUD_SIGNAL_4 0x164 // 356 Hud signal icon 100%
#define SYM_TRIP_DIST 0x22 // 034 Trip distance
#define SYM_TOTAL 0x22 // 034 Total
#define SYM_HOME_DIST 0x165 // 357 DIST
#define SYM_AH_CH_CENTER 0x166 // 358 Crossair center
// 0x23 // 035 ASCII #
#define SYM_AH_CH_TYPE3 0x190 // 400 to 402, crosshair 3
#define SYM_AH_CH_TYPE4 0x193 // 403 to 405, crosshair 4
#define SYM_AH_CH_TYPE5 0x196 // 406 to 408, crosshair 5
#define SYM_AH_CH_TYPE6 0x199 // 409 to 411, crosshair 6
#define SYM_AH_CH_TYPE7 0x19C // 412 to 414, crosshair 7
#define SYM_AH_CH_TYPE8 0x19F // 415 to 417, crosshair 8
#define SYM_AH_DECORATION_DOWN 0x24 // 036 Arrow down AHI
// 0x25 // 037 ASCII %
#define SYM_AH_CH_LEFT 0x26 // 038 Crossair left
#define SYM_AH_CH_RIGHT 0x27 // 039 Crossair right
// 0x28 // 040 to 062 ASCII
#define SYM_MILLIOHM 0x3F // 063 battery impedance Mohm
// 0x40 // 064 to 095 ASCII
#define SYM_ARROW_UP 0x60 // 096 Direction arrow 0°
#define SYM_ARROW_2 0x61 // 097 Direction arrow 22.5°
#define SYM_ARROW_3 0x62 // 098 Direction arrow 45°
#define SYM_ARROW_4 0x63 // 099 Direction arrow 67.5°
#define SYM_ARROW_RIGHT 0x64 // 100 Direction arrow 90°
#define SYM_ARROW_6 0x65 // 101 Direction arrow 112.5°
#define SYM_ARROW_7 0x66 // 102 Direction arrow 135°
#define SYM_ARROW_8 0x67 // 103 Direction arrow 157.5°
#define SYM_ARROW_DOWN 0x68 // 104 Direction arrow 180°
#define SYM_ARROW_10 0x69 // 105 Direction arrow 202.5°
#define SYM_ARROW_11 0x6A // 106 Direction arrow 225°
#define SYM_ARROW_12 0x6B // 107 Direction arrow 247.5°
#define SYM_ARROW_LEFT 0x6C // 108 Direction arrow 270°
#define SYM_ARROW_14 0x6D // 109 Direction arrow 292.5°
#define SYM_ARROW_15 0x6E // 110 Direction arrow 315°
#define SYM_ARROW_16 0x6F // 111 Direction arrow 337.5°
#define SYM_ON_H 0x70 // 112 ON HR
#define SYM_FLY_H 0x71 // 113 FLY HR
#define SYM_DIRECTION 0x72 // 114 to 121, directional little arrows
#define SYM_HOME_NEAR 0x7A // 122 Home, near
// 0x7B // 123 to 125 ASCII
#define SYM_AH_CH_CENTER 0x7E // 126 Crossair center
#define SYM_GLIDESLOPE 0x7F // 127 Glideslope
#define SYM_AH_H_START 0x80 // 128 to 136 Horizontal AHI
#define SYM_3D_KMH 0x89 // 137 KM/H 3D
#define SYM_3D_MPH 0x8A // 138 MPH 3D
#define SYM_RPM 0x8B // 139 RPM
#define SYM_WAYPOINT 0x8C // 140 Waypoint
#define SYM_AZIMUTH 0x8D // 141 Azimuth
#define SYM_TELEMETRY_0 0x8E // 142 Antenna tracking telemetry
#define SYM_TELEMETRY_1 0x8F // 143 Antenna tracking telemetry
#define SYM_BATT_FULL 0x90 // 144 Battery full
#define SYM_BATT_5 0x91 // 145 Battery
#define SYM_BATT_4 0x92 // 146 Battery
#define SYM_BATT_3 0x93 // 147 Battery
#define SYM_BATT_2 0x94 // 148 Battery
#define SYM_BATT_1 0x95 // 149 Battery
#define SYM_BATT_EMPTY 0x96 // 150 Battery empty
#define SYM_AIR 0x97 // 151 Air speed
// 0x98 // 152 Home point map
#define SYM_FTS 0x99 // 153 FT/S
#define SYM_AMP 0x9A // 154 A
#define SYM_ON_M 0x9B // 155 On MN
#define SYM_FLY_M 0x9C // 156 FL MN
#define SYM_MAH_KM_0 0x9D // 157 MAH/KM left
#define SYM_MAH_KM_1 0x9E // 158 MAH/KM right
#define SYM_MS 0x9F // 159 M/S
#define SYM_HOME_DIST 0xA0 // 160 DIS
#define SYM_KMH 0xA1 // 161 KM/H
#define SYM_VARIO_UP_2A 0xA2 // 162 Vario up up
#define SYM_VARIO_UP_1A 0xA3 // 163 Vario up
#define SYM_VARIO_DOWN_1A 0xA4 // 164 Vario down
#define SYM_VARIO_DOWN_2A 0xA5 // 165 Vario down down
#define SYM_LAT 0xA6 // 166 GPS LAT
#define SYM_LON 0xA7 // 167 GPS LON
#define SYM_DEGREES 0xA8 // 168 ° heading angle
#define SYM_HEADING 0xA9 // 169 Compass Heading symbol
#define SYM_ALT 0xAA // 170 ALT
#define SYM_WH 0xAB // 171 WH
#define SYM_WH_KM 0xAC // 172 WH/KM
#define SYM_WH_MI 0xAD // 173 WH/MI
#define SYM_WATT 0xAE // 174 W
#define SYM_SCALE 0xAF // 175 Map scale
#define SYM_MPH 0xB0 // 176 MPH
#define SYM_ALT_M 0xB1 // 177 ALT M
#define SYM_ALT_KM 0xB2 // 178 ALT KM
#define SYM_ALT_FT 0xB3 // 179 ALT FT
#define SYM_ALT_KFT 0xB4 // 180 DIS KFT
#define SYM_DIST_M 0xB5 // 181 DIS M
#define SYM_DIST_KM 0xB6 // 182 DIM KM
#define SYM_DIST_FT 0xB7 // 183 DIS FT
#define SYM_DIST_MI 0xB8 // 184 DIS MI
#define SYM_M 0xB9 // 185 M
#define SYM_KM 0xBA // 186 KM
#define SYM_MI 0xBB // 187 MI
#define SYM_CLOCK 0xBC // 188 Clock
#define SYM_HDP_L 0xBD // 189 HDOP left
#define SYM_HDP_R 0xBE // 190 HDOP right
#define SYM_HOME 0xBF // 191 Home icon
#define SYM_ZERO_HALF_TRAILING_DOT 0xC0 // 192 to 201 Numbers with trailing dot
#define SYM_AUTO_THR0 0xCA // 202 Auto-throttle left
#define SYM_AUTO_THR1 0xCB // 203 Auto-throttle right
#define SYM_ROLL_LEFT 0xCC // 204 Sym roll left
#define SYM_ROLL_LEVEL 0xCD // 205 Sym roll horizontal
#define SYM_ROLL_RIGHT 0xCE // 206 Sym roll right
#define SYM_PITCH_UP 0xCF // 207 Pitch up
#define SYM_ZERO_HALF_LEADING_DOT 0xD0 // 208 to 217 Numbers with leading dot
#define SYM_AH_CH_AIRCRAFT0 0xDA // 218 Crossair aircraft left
#define SYM_AH_CH_AIRCRAFT1 0xDB // 219 Crossair aircraft
#define SYM_AH_CH_AIRCRAFT2 0xDC // 220 Crossair aircraft center
#define SYM_AH_CH_AIRCRAFT3 0xDD // 221 Crossair aircraft
#define SYM_AH_CH_AIRCRAFT4 0xDE // 222 Crossair aircraft right
#define SYM_PITCH_DOWN 0xDF // 223 Pitch down
#define SYM_AH_V_START 0xE0 // 224 to 229 Vertical AHI
#define SYM_GFORCE 0xE6 // 230 Gforce (all axis)
#define SYM_GFORCE_X 0xE7 // 231 Gforce X
#define SYM_GFORCE_Y 0xE8 // 232 Gforce Y
#define SYM_GFORCE_Z 0xE9 // 233 Gforce Z
#define SYM_BARO_TEMP 0xF0 // 240
#define SYM_IMU_TEMP 0xF1 // 241
#define SYM_TEMP 0xF2 // 242
#define SYM_ESC_TEMP 0xF3 // 243
#define SYM_TEMP_SENSOR_FIRST 0xF2 // 242
#define SYM_TEMP_SENSOR_LAST 0xF7 // 247
#define TEMP_SENSOR_SYM_COUNT (SYM_TEMP_SENSOR_LAST - SYM_TEMP_SENSOR_FIRST + 1)
#define SYM_HUD_SIGNAL_0 0xF8 // 248 Hud signal icon Lost
#define SYM_HUD_SIGNAL_1 0xF9 // 249 Hud signal icon 25%
#define SYM_HUD_SIGNAL_2 0xFA // 250 Hud signal icon 50%
#define SYM_HUD_SIGNAL_3 0xFB // 251 Hud signal icon 75%
#define SYM_HUD_SIGNAL_4 0xFC // 252 Hud signal icon 100%
#define SYM_LOGO_START 0x101 // 257 to 280, INAV logo
#define SYM_LOGO_WIDTH 6
#define SYM_LOGO_HEIGHT 4
#define SYM_AH_CH_TYPE3 0x190 // 400 to 402, crosshair 3
#define SYM_AH_CH_TYPE4 0x193 // 403 to 405, crosshair 4
#define SYM_AH_CH_TYPE5 0x196 // 406 to 408, crosshair 5
#define SYM_AH_CH_TYPE6 0x199 // 409 to 411, crosshair 6
#define SYM_AH_CH_TYPE7 0x19C // 412 to 414, crosshair 7
#define SYM_HUD_ARROWS_L1 0x1A2 // 418 1 arrow left
#define SYM_HUD_ARROWS_L2 0x1A3 // 419 2 arrows left
#define SYM_HUD_ARROWS_L3 0x1A4 // 420 3 arrows left
#define SYM_HUD_ARROWS_R1 0x1A5 // 421 1 arrow right
#define SYM_HUD_ARROWS_R2 0x1A6 // 422 2 arrows right
#define SYM_HUD_ARROWS_R3 0x1A7 // 423 3 arrows right
#define SYM_HUD_ARROWS_U1 0x1A8 // 424 1 arrow up
#define SYM_HUD_ARROWS_U2 0x1A9 // 425 2 arrows up
#define SYM_HUD_ARROWS_U3 0x1AA // 426 3 arrows up
#define SYM_HUD_ARROWS_D1 0x1AB // 427 1 arrow down
#define SYM_HUD_ARROWS_D2 0x1AC // 428 2 arrows down
#define SYM_HUD_ARROWS_D3 0x1AD // 429 3 arrows down
#define SYM_2RSS 0xEA // RSSI 2
#define SYM_DB 0xEB // dB
#define SYM_DBM 0xEC // dBm
#define SYM_SNR 0xEE // SNR
#define SYM_MW 0xED // mW
#define SYM_KILOWATT 0xEF // 239 kW
#define SYM_AH_CH_AIRCRAFT0 0x1A2 // 418 Crossair aircraft left
#define SYM_AH_CH_AIRCRAFT1 0x1A3 // 419 Crossair aircraft
#define SYM_AH_CH_AIRCRAFT2 0x1A4 // 420 Crossair aircraft center
#define SYM_AH_CH_AIRCRAFT3 0x1A5 // 421 Crossair aircraft
#define SYM_AH_CH_AIRCRAFT4 0x1A6 // 422 Crossair aircraft RIGHT
#define SYM_HUD_ARROWS_L1 0x1AE // 430 1 arrow left
#define SYM_HUD_ARROWS_L2 0x1AF // 431 2 arrows left
#define SYM_HUD_ARROWS_L3 0x1B0 // 432 3 arrows left
#define SYM_HUD_ARROWS_R1 0x1B1 // 433 1 arrow right
#define SYM_HUD_ARROWS_R2 0x1B2 // 434 2 arrows right
#define SYM_HUD_ARROWS_R3 0x1B3 // 435 3 arrows right
#define SYM_HUD_ARROWS_U1 0x1B4 // 436 1 arrow up
#define SYM_HUD_ARROWS_U2 0x1B5 // 437 2 arrows up
#define SYM_HUD_ARROWS_U3 0x1B6 // 438 3 arrows up
#define SYM_HUD_ARROWS_D1 0x1B7 // 439 1 arrow down
#define SYM_HUD_ARROWS_D2 0x1B8 // 440 2 arrows down
#define SYM_HUD_ARROWS_D3 0x1B9 // 441 3 arrows down
#else

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

@ -36,7 +36,6 @@
#include "drivers/pwm_mapping.h"
#include "drivers/serial.h"
#include "drivers/serial_uart.h"
//#include "drivers/rx_pwm.h"
#include "sensors/rangefinder.h"
@ -67,16 +66,13 @@ static const char * pwmInitErrorMsg[] = {
};
static const motorProtocolProperties_t motorProtocolProperties[] = {
[PWM_TYPE_STANDARD] = { .usesHwTimer = true, .isDSHOT = false, .isSerialShot = false },
[PWM_TYPE_ONESHOT125] = { .usesHwTimer = true, .isDSHOT = false, .isSerialShot = false },
[PWM_TYPE_ONESHOT42] = { .usesHwTimer = true, .isDSHOT = false, .isSerialShot = false },
[PWM_TYPE_MULTISHOT] = { .usesHwTimer = true, .isDSHOT = false, .isSerialShot = false },
[PWM_TYPE_BRUSHED] = { .usesHwTimer = true, .isDSHOT = false, .isSerialShot = false },
[PWM_TYPE_DSHOT150] = { .usesHwTimer = true, .isDSHOT = true, .isSerialShot = false },
[PWM_TYPE_DSHOT300] = { .usesHwTimer = true, .isDSHOT = true, .isSerialShot = false },
[PWM_TYPE_DSHOT600] = { .usesHwTimer = true, .isDSHOT = true, .isSerialShot = false },
[PWM_TYPE_DSHOT1200] = { .usesHwTimer = true, .isDSHOT = true, .isSerialShot = false },
[PWM_TYPE_SERIALSHOT] = { .usesHwTimer = false, .isDSHOT = false, .isSerialShot = true },
[PWM_TYPE_STANDARD] = { .usesHwTimer = true, .isDSHOT = false },
[PWM_TYPE_ONESHOT125] = { .usesHwTimer = true, .isDSHOT = false },
[PWM_TYPE_MULTISHOT] = { .usesHwTimer = true, .isDSHOT = false },
[PWM_TYPE_BRUSHED] = { .usesHwTimer = true, .isDSHOT = false },
[PWM_TYPE_DSHOT150] = { .usesHwTimer = true, .isDSHOT = true },
[PWM_TYPE_DSHOT300] = { .usesHwTimer = true, .isDSHOT = true },
[PWM_TYPE_DSHOT600] = { .usesHwTimer = true, .isDSHOT = true },
};
pwmInitError_e getPwmInitError(void)
@ -199,17 +195,6 @@ static bool checkPwmTimerConflicts(const timerHardware_t *timHw)
#endif
#endif
#ifdef USE_RANGEFINDER_HCSR04
// HC-SR04 has a dedicated connection to FC and require two pins
if (rangefinderConfig()->rangefinder_hardware == RANGEFINDER_HCSR04) {
const rangefinderHardwarePins_t *rangefinderHardwarePins = rangefinderGetHardwarePins();
if (rangefinderHardwarePins && (timHw->tag == rangefinderHardwarePins->triggerTag || timHw->tag == rangefinderHardwarePins->echoTag)) {
return true;
}
}
#endif
return false;
}

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

@ -39,14 +39,11 @@
typedef enum {
PWM_TYPE_STANDARD = 0,
PWM_TYPE_ONESHOT125,
PWM_TYPE_ONESHOT42,
PWM_TYPE_MULTISHOT,
PWM_TYPE_BRUSHED,
PWM_TYPE_DSHOT150,
PWM_TYPE_DSHOT300,
PWM_TYPE_DSHOT600,
PWM_TYPE_DSHOT1200,
PWM_TYPE_SERIALSHOT,
} motorPwmProtocolTypes_e;
typedef enum {
@ -73,7 +70,6 @@ typedef struct rangefinderIOConfig_s {
typedef struct {
bool usesHwTimer;
bool isDSHOT;
bool isSerialShot;
} motorProtocolProperties_t;
bool pwmMotorAndServoInit(void);

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

@ -37,7 +37,6 @@ FILE_COMPILE_FOR_SPEED
#include "drivers/io_pca9685.h"
#include "io/pwmdriver_i2c.h"
#include "io/esc_serialshot.h"
#include "io/servo_sbus.h"
#include "sensors/esc_sensor.h"
@ -50,7 +49,6 @@ FILE_COMPILE_FOR_SPEED
#define MULTISHOT_20US_MULT (MULTISHOT_TIMER_HZ * 20 / 1000000.0f / 1000.0f)
#ifdef USE_DSHOT
#define MOTOR_DSHOT1200_HZ 24000000
#define MOTOR_DSHOT600_HZ 12000000
#define MOTOR_DSHOT300_HZ 6000000
#define MOTOR_DSHOT150_HZ 3000000
@ -100,7 +98,7 @@ static pwmWriteFuncPtr motorWritePtr = NULL; // Function to write val
static pwmOutputPort_t * servos[MAX_SERVOS];
static pwmWriteFuncPtr servoWritePtr = NULL; // Function to write value to motors
#if defined(USE_DSHOT) || defined(USE_SERIALSHOT)
#if defined(USE_DSHOT)
static timeUs_t digitalMotorUpdateIntervalUs = 0;
static timeUs_t digitalMotorLastUpdateUs;
#endif
@ -148,7 +146,7 @@ static pwmOutputPort_t *pwmOutAllocatePort(void)
return p;
}
static pwmOutputPort_t *pwmOutConfigMotor(const timerHardware_t *timHw, uint32_t hz, uint16_t period, uint16_t value, bool enableOutput)
static pwmOutputPort_t *pwmOutConfig(const timerHardware_t *timHw, resourceOwner_e owner, uint32_t hz, uint16_t period, uint16_t value, bool enableOutput)
{
// Attempt to allocate TCH
TCH_t * tch = timerGetTCH(timHw);
@ -163,7 +161,7 @@ static pwmOutputPort_t *pwmOutConfigMotor(const timerHardware_t *timHw, uint32_t
}
const IO_t io = IOGetByTag(timHw->tag);
IOInit(io, OWNER_MOTOR, RESOURCE_OUTPUT, allocatedOutputPortCount);
IOInit(io, owner, RESOURCE_OUTPUT, allocatedOutputPortCount);
if (enableOutput) {
IOConfigGPIOAF(io, IOCFG_AF_PP, timHw->alternateFunction);
@ -230,7 +228,7 @@ static pwmOutputPort_t * motorConfigPwm(const timerHardware_t *timerHardware, fl
const uint32_t timerHz = baseClockHz / prescaler;
const uint32_t period = timerHz / motorPwmRateHz;
pwmOutputPort_t * port = pwmOutConfigMotor(timerHardware, timerHz, period, 0, enableOutput);
pwmOutputPort_t * port = pwmOutConfig(timerHardware, OWNER_MOTOR, timerHz, period, 0, enableOutput);
if (port) {
port->pulseScale = ((sLen == 0) ? period : (sLen * timerHz)) / 1000.0f;
@ -245,8 +243,6 @@ static pwmOutputPort_t * motorConfigPwm(const timerHardware_t *timerHardware, fl
uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType)
{
switch (pwmProtocolType) {
case(PWM_TYPE_DSHOT1200):
return MOTOR_DSHOT1200_HZ;
case(PWM_TYPE_DSHOT600):
return MOTOR_DSHOT600_HZ;
case(PWM_TYPE_DSHOT300):
@ -260,7 +256,7 @@ uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType)
static pwmOutputPort_t * motorConfigDshot(const timerHardware_t * timerHardware, uint32_t dshotHz, bool enableOutput)
{
// Try allocating new port
pwmOutputPort_t * port = pwmOutConfigMotor(timerHardware, dshotHz, DSHOT_MOTOR_BITLENGTH, 0, enableOutput);
pwmOutputPort_t * port = pwmOutConfig(timerHardware, OWNER_MOTOR, dshotHz, DSHOT_MOTOR_BITLENGTH, 0, enableOutput);
if (!port) {
return NULL;
@ -304,7 +300,7 @@ static uint16_t prepareDshotPacket(const uint16_t value, bool requestTelemetry)
}
#endif
#if defined(USE_DSHOT) || defined(USE_SERIALSHOT)
#if defined(USE_DSHOT)
static void motorConfigDigitalUpdateInterval(uint16_t motorPwmRateHz)
{
digitalMotorUpdateIntervalUs = 1000000 / motorPwmRateHz;
@ -325,14 +321,9 @@ bool isMotorProtocolDshot(void)
return getMotorProtocolProperties(initMotorProtocol)->isDSHOT;
}
bool isMotorProtocolSerialShot(void)
{
return getMotorProtocolProperties(initMotorProtocol)->isSerialShot;
}
bool isMotorProtocolDigital(void)
{
return isMotorProtocolDshot() || isMotorProtocolSerialShot();
return isMotorProtocolDshot();
}
void pwmRequestMotorTelemetry(int motorIndex)
@ -366,7 +357,7 @@ static int getDShotCommandRepeats(dshotCommands_e cmd) {
switch (cmd) {
case DSHOT_CMD_SPIN_DIRECTION_NORMAL:
case DSHOT_CMD_SPIN_DIRECTION_REVERSED:
repeats = 6;
repeats = 10;
break;
default:
break;
@ -441,16 +432,6 @@ void pwmCompleteMotorUpdate(void) {
}
}
#endif
#ifdef USE_SERIALSHOT
if (isMotorProtocolSerialShot()) {
for (int index = 0; index < motorCount; index++) {
serialshotUpdateMotor(index, motors[index].value);
}
serialshotSendUpdate();
}
#endif
}
#else // digital motor protocol
@ -481,13 +462,11 @@ void pwmMotorPreconfigure(void)
case PWM_TYPE_STANDARD:
case PWM_TYPE_BRUSHED:
case PWM_TYPE_ONESHOT125:
case PWM_TYPE_ONESHOT42:
case PWM_TYPE_MULTISHOT:
motorWritePtr = pwmWriteStandard;
break;
#ifdef USE_DSHOT
case PWM_TYPE_DSHOT1200:
case PWM_TYPE_DSHOT600:
case PWM_TYPE_DSHOT300:
case PWM_TYPE_DSHOT150:
@ -495,15 +474,6 @@ void pwmMotorPreconfigure(void)
motorWritePtr = pwmWriteDigital;
break;
#endif
#ifdef USE_SERIALSHOT
case PWM_TYPE_SERIALSHOT:
// Kick off SerialShot driver initalization
serialshotInitialize();
motorConfigDigitalUpdateInterval(motorConfig()->motorPwmRate);
motorWritePtr = pwmWriteDigital;
break;
#endif
}
}
@ -518,16 +488,11 @@ bool pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, bo
motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 125e-6f, 125e-6f, motorConfig()->motorPwmRate, enableOutput);
break;
case PWM_TYPE_ONESHOT42:
motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 42e-6f, 42e-6f, motorConfig()->motorPwmRate, enableOutput);
break;
case PWM_TYPE_MULTISHOT:
motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 5e-6f, 20e-6f, motorConfig()->motorPwmRate, enableOutput);
break;
#ifdef USE_DSHOT
case PWM_TYPE_DSHOT1200:
case PWM_TYPE_DSHOT600:
case PWM_TYPE_DSHOT300:
case PWM_TYPE_DSHOT150:
@ -615,7 +580,7 @@ void pwmServoPreconfigure(void)
bool pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse, bool enableOutput)
{
pwmOutputPort_t * port = pwmOutConfigMotor(timerHardware, PWM_TIMER_HZ, PWM_TIMER_HZ / servoPwmRate, servoCenterPulse, enableOutput);
pwmOutputPort_t * port = pwmOutConfig(timerHardware, OWNER_SERVO, PWM_TIMER_HZ, PWM_TIMER_HZ / servoPwmRate, servoCenterPulse, enableOutput);
if (port) {
servos[servoIndex] = port;

7
src/main/drivers/rangefinder/rangefinder.h
View file

@ -27,13 +27,6 @@
struct rangefinderDev_s;
typedef struct rangefinderHardwarePins_s {
ioTag_t triggerTag;
ioTag_t echoTag;
} rangefinderHardwarePins_t;
struct rangefinderDev_s;
typedef void (*rangefinderOpInitFuncPtr)(struct rangefinderDev_s * dev);
typedef void (*rangefinderOpStartFuncPtr)(struct rangefinderDev_s * dev);
typedef int32_t (*rangefinderOpReadFuncPtr)(struct rangefinderDev_s * dev);

221
src/main/drivers/rangefinder/rangefinder_hcsr04.c
View file

@ -1,221 +0,0 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <platform.h>
#if defined(USE_RANGEFINDER_HCSR04)
#include "build/build_config.h"
#include "common/time.h"
#include "drivers/time.h"
#include "drivers/exti.h"
#include "drivers/io.h"
#include "drivers/nvic.h"
#include "drivers/rcc.h"
#include "drivers/rangefinder/rangefinder.h"
#include "drivers/rangefinder/rangefinder_hcsr04.h"
#define HCSR04_MAX_RANGE_CM 400 // 4m, from HC-SR04 spec sheet
#define HCSR04_DETECTION_CONE_DECIDEGREES 300 // recommended cone angle30 degrees, from HC-SR04 spec sheet
#define HCSR04_DETECTION_CONE_EXTENDED_DECIDEGREES 450 // in practice 45 degrees seems to work well
/* HC-SR04 consists of ultrasonic transmitter, receiver, and control circuits.
* When triggered it sends out a series of 40KHz ultrasonic pulses and receives
* echo from an object. The distance between the unit and the object is calculated
* by measuring the traveling time of sound and output it as the width of a TTL pulse.
*
* *** Warning: HC-SR04 operates at +5V ***
*
*/
static volatile timeDelta_t hcsr04SonarPulseTravelTime = 0;
static volatile timeMs_t lastMeasurementReceivedAt;
static volatile int32_t lastCalculatedDistance = RANGEFINDER_OUT_OF_RANGE;
static timeMs_t lastMeasurementStartedAt = 0;
#ifdef USE_EXTI
static extiCallbackRec_t hcsr04_extiCallbackRec;
#endif
static IO_t echoIO;
static IO_t triggerIO;
#if !defined(UNIT_TEST)
void hcsr04_extiHandler(extiCallbackRec_t* cb)
{
static timeUs_t timing_start;
UNUSED(cb);
if (IORead(echoIO) != 0) {
timing_start = micros();
} else {
const timeUs_t timing_stop = micros();
if (timing_stop > timing_start) {
lastMeasurementReceivedAt = millis();
hcsr04SonarPulseTravelTime = timing_stop - timing_start;
}
}
}
#endif
void hcsr04_init(rangefinderDev_t *dev)
{
UNUSED(dev);
}
#define HCSR04_MinimumFiringIntervalMs 60
/*
* Start a range reading
* Called periodically by the scheduler
* Measurement reading is done asynchronously, using interrupt
*/
void hcsr04_start_reading(void)
{
#if !defined(UNIT_TEST)
#ifdef RANGEFINDER_HCSR04_TRIG_INVERTED
IOLo(triggerIO);
delayMicroseconds(11);
IOHi(triggerIO);
#else
IOHi(triggerIO);
delayMicroseconds(11);
IOLo(triggerIO);
#endif
#endif
}
void hcsr04_update(rangefinderDev_t *dev)
{
UNUSED(dev);
const timeMs_t timeNowMs = millis();
// the firing interval of the trigger signal should be greater than 60ms
// to avoid interference between consecutive measurements
if (timeNowMs > lastMeasurementStartedAt + HCSR04_MinimumFiringIntervalMs) {
// We should have a valid measurement within 60ms of trigger
if ((lastMeasurementReceivedAt - lastMeasurementStartedAt) <= HCSR04_MinimumFiringIntervalMs) {
// The speed of sound is 340 m/s or approx. 29 microseconds per centimeter.
// The ping travels out and back, so to find the distance of the
// object we take half of the distance traveled.
// 340 m/s = 0.034 cm/microsecond = 29.41176471 *2 = 58.82352941 rounded to 59
lastCalculatedDistance = hcsr04SonarPulseTravelTime / 59;
if (lastCalculatedDistance > HCSR04_MAX_RANGE_CM) {
lastCalculatedDistance = RANGEFINDER_OUT_OF_RANGE;
}
}
else {
// No measurement within reasonable time - indicate failure
lastCalculatedDistance = RANGEFINDER_HARDWARE_FAILURE;
}
// Trigger a new measurement
lastMeasurementStartedAt = timeNowMs;
hcsr04_start_reading();
}
}
/**
* Get the distance that was measured by the last pulse, in centimeters.
*/
int32_t hcsr04_get_distance(rangefinderDev_t *dev)
{
UNUSED(dev);
return lastCalculatedDistance;
}
bool hcsr04Detect(rangefinderDev_t *dev, const rangefinderHardwarePins_t * rangefinderHardwarePins)
{
bool detected = false;
#if defined(STM32F3) || defined(STM32F4)
RCC_ClockCmd(RCC_APB2(SYSCFG), ENABLE);
#endif
triggerIO = IOGetByTag(rangefinderHardwarePins->triggerTag);
echoIO = IOGetByTag(rangefinderHardwarePins->echoTag);
if (IOGetOwner(triggerIO) != OWNER_FREE) {
return false;
}
if (IOGetOwner(echoIO) != OWNER_FREE) {
return false;
}
// trigger pin
IOInit(triggerIO, OWNER_RANGEFINDER, RESOURCE_OUTPUT, 0);
IOConfigGPIO(triggerIO, IOCFG_OUT_PP);
IOLo(triggerIO);
delay(100);
// echo pin
IOInit(echoIO, OWNER_RANGEFINDER, RESOURCE_INPUT, 0);
IOConfigGPIO(echoIO, IOCFG_IN_FLOATING);
// HC-SR04 echo line should be low by default and should return a response pulse when triggered
if (IORead(echoIO) == false) {
for (int i = 0; i < 5 && !detected; i++) {
timeMs_t requestTime = millis();
hcsr04_start_reading();
while ((millis() - requestTime) < HCSR04_MinimumFiringIntervalMs) {
if (IORead(echoIO) == true) {
detected = true;
break;
}
}
}
}
if (detected) {
// Hardware detected - configure the driver
#ifdef USE_EXTI
EXTIHandlerInit(&hcsr04_extiCallbackRec, hcsr04_extiHandler);
EXTIConfig(echoIO, &hcsr04_extiCallbackRec, NVIC_PRIO_SONAR_EXTI, EXTI_Trigger_Rising_Falling); // TODO - priority!
EXTIEnable(echoIO, true);
#endif
dev->delayMs = 100;
dev->maxRangeCm = HCSR04_MAX_RANGE_CM;
dev->detectionConeDeciDegrees = HCSR04_DETECTION_CONE_DECIDEGREES;
dev->detectionConeExtendedDeciDegrees = HCSR04_DETECTION_CONE_EXTENDED_DECIDEGREES;
dev->init = &hcsr04_init;
dev->update = &hcsr04_update;
dev->read = &hcsr04_get_distance;
return true;
}
else {
// Not detected - free resources
IORelease(triggerIO);
IORelease(echoIO);
return false;
}
}
#endif

24
src/main/drivers/rangefinder/rangefinder_hcsr04.h
View file

@ -1,24 +0,0 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "drivers/rangefinder/rangefinder.h"
#define RANGEFINDER_HCSR04_TASK_PERIOD_MS 70
bool hcsr04Detect(rangefinderDev_t *dev, const rangefinderHardwarePins_t * rangefinderHardwarePins);

125
src/main/drivers/rangefinder/rangefinder_hcsr04_i2c.c
View file

@ -1,125 +0,0 @@
/*
* This file is part of INAV.
*
* INAV is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* INAV is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with INAV. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include "platform.h"
#if defined(USE_RANGEFINDER) && defined(USE_RANGEFINDER_HCSR04_I2C)
#include "build/build_config.h"
#include "drivers/time.h"
#include "drivers/bus_i2c.h"
#include "drivers/rangefinder/rangefinder.h"
#include "drivers/rangefinder/rangefinder_hcsr04_i2c.h"
#include "build/debug.h"
#define HCSR04_I2C_MAX_RANGE_CM 400
#define HCSR04_I2C_DETECTION_CONE_DECIDEGREES 300
#define HCSR04_I2C_DETECTION_CONE_EXTENDED_DECIDEGREES 450
#define HCSR04_I2C_Address 0x14
#define HCSR04_I2C_REGISTRY_STATUS 0x00
#define HCSR04_I2C_REGISTRY_DISTANCE_HIGH 0x01
#define HCSR04_I2C_REGISTRY_DISTANCE_LOW 0x02
volatile int32_t hcsr04i2cMeasurementCm = RANGEFINDER_OUT_OF_RANGE;
static bool isHcsr04i2cResponding = false;
static void hcsr04i2cInit(rangefinderDev_t *rangefinder)
{
UNUSED(rangefinder);
}
void hcsr04i2cUpdate(rangefinderDev_t *rangefinder)
{
uint8_t response[3];
isHcsr04i2cResponding = busReadBuf(rangefinder->busDev, HCSR04_I2C_REGISTRY_STATUS, response, 3);
if (!isHcsr04i2cResponding) {
hcsr04i2cMeasurementCm = RANGEFINDER_HARDWARE_FAILURE;
return;
}
if (response[HCSR04_I2C_REGISTRY_STATUS] == 0) {
hcsr04i2cMeasurementCm =
(int32_t)((int32_t)response[HCSR04_I2C_REGISTRY_DISTANCE_HIGH] << 8) +
response[HCSR04_I2C_REGISTRY_DISTANCE_LOW];
} else {
/*
* Rangefinder is reporting out-of-range situation
*/
hcsr04i2cMeasurementCm = RANGEFINDER_OUT_OF_RANGE;
}
}
/**
* Get the distance that was measured by the last pulse, in centimeters.
*/
int32_t hcsr04i2cGetDistance(rangefinderDev_t *rangefinder)
{
UNUSED(rangefinder);
return hcsr04i2cMeasurementCm;
}
static bool deviceDetect(busDevice_t * busDev)
{
for (int retry = 0; retry < 5; retry++) {
uint8_t inquiryResult;
delay(150);
bool ack = busRead(busDev, HCSR04_I2C_REGISTRY_STATUS, &inquiryResult);
if (ack) {
return true;
}
};
return false;
}
bool hcsr04i2c0Detect(rangefinderDev_t *rangefinder)
{
rangefinder->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_HCSR04_I2C, 0, OWNER_RANGEFINDER);
if (rangefinder->busDev == NULL) {
return false;
}
if (!deviceDetect(rangefinder->busDev)) {
busDeviceDeInit(rangefinder->busDev);
return false;
}
rangefinder->delayMs = RANGEFINDER_HCSR04_i2C_TASK_PERIOD_MS;
rangefinder->maxRangeCm = HCSR04_I2C_MAX_RANGE_CM;
rangefinder->detectionConeDeciDegrees = HCSR04_I2C_DETECTION_CONE_DECIDEGREES;
rangefinder->detectionConeExtendedDeciDegrees = HCSR04_I2C_DETECTION_CONE_EXTENDED_DECIDEGREES;
rangefinder->init = &hcsr04i2cInit;
rangefinder->update = &hcsr04i2cUpdate;
rangefinder->read = &hcsr04i2cGetDistance;
return true;
}
#endif

124
src/main/drivers/rangefinder/rangefinder_tof10120_i2c.c Normal file
View file

@ -0,0 +1,124 @@
/*
* This file is part of INAV.
*
* INAV is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* INAV is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with INAV. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <math.h>
#include "platform.h"
#if defined(USE_RANGEFINDER) && defined(USE_RANGEFINDER_TOF10120_I2C)
#include "drivers/time.h"
#include "drivers/rangefinder/rangefinder.h"
#include "drivers/rangefinder/rangefinder_tof10120_i2c.h"
#define TOF10120_I2C_MAX_RANGE_CM 200
#define TOF10120_I2C_MAX_RANGE_MM TOF10120_I2C_MAX_RANGE_CM * 10
#define TOF10120_I2C_DETECTION_CONE_DECIDEGREES 300
#define TOF10120_I2C_DETECTION_CONE_EXTENDED_DECIDEGREES 450
#define TOF10120_I2C_ADDRESS 0x52
#define TOF10120_I2C_REGISTRY_DISTANCE 0x04 // address of first byte for reading filtered distance
#define TOF10120_I2C_REGISTRY_RT_DISTANCE 0x00 // address of first byte for reading real-time raw distance
#define TOF10120_I2C_REGISTRY_BUS_ADDR_CONFIG 0x0f
#define TOF10120_I2C_REGISTRY_SENDING_METHOD_CONFIG 0x09
volatile int32_t tof10120MeasurementCm = RANGEFINDER_OUT_OF_RANGE;
static bool isTof10120Responding = false;
static void tof10120i2cInit(rangefinderDev_t *rangefinder)
{
busWrite(rangefinder->busDev, TOF10120_I2C_REGISTRY_SENDING_METHOD_CONFIG, 0x01);
delay(100);
}
void tof10120i2cUpdate(rangefinderDev_t *rangefinder)
{
uint8_t buffer[2];
uint16_t distance_mm;
isTof10120Responding = busReadBuf(rangefinder->busDev, TOF10120_I2C_REGISTRY_RT_DISTANCE, buffer, sizeof(buffer));
if (!isTof10120Responding) {
return;
}
distance_mm = (buffer[0] << 8) | buffer[1];
if (distance_mm >= TOF10120_I2C_MAX_RANGE_MM) {
tof10120MeasurementCm = RANGEFINDER_OUT_OF_RANGE;
return;
}
tof10120MeasurementCm = (int32_t)roundf(distance_mm / 10);
}
/**
* Get the distance that was measured by the last pulse, in centimeters.
*/
int32_t tof10120i2cGetDistance(rangefinderDev_t *rangefinder)
{
UNUSED(rangefinder);
if (isTof10120Responding) {
return tof10120MeasurementCm;
} else {
return RANGEFINDER_HARDWARE_FAILURE;
}
}
static bool deviceDetect(rangefinderDev_t *rangefinder)
{
busWrite(rangefinder->busDev, TOF10120_I2C_REGISTRY_SENDING_METHOD_CONFIG, 0x01);
delay(100);
uint8_t response = 0;
if (!busRead(rangefinder->busDev, TOF10120_I2C_REGISTRY_BUS_ADDR_CONFIG, &response) || !response) {
return false;
}
return true;
}
bool tof10120Detect(rangefinderDev_t *rangefinder)
{
rangefinder->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_TOF10120_I2C, 0, OWNER_RANGEFINDER);
if (rangefinder->busDev == NULL) {
return false;
}
if (!deviceDetect(rangefinder)) {
busDeviceDeInit(rangefinder->busDev);
return false;
}
rangefinder->delayMs = RANGEFINDER_TOF10120_I2C_TASK_PERIOD_MS;
rangefinder->maxRangeCm = TOF10120_I2C_MAX_RANGE_CM;
rangefinder->detectionConeDeciDegrees = TOF10120_I2C_DETECTION_CONE_DECIDEGREES;
rangefinder->detectionConeExtendedDeciDegrees = TOF10120_I2C_DETECTION_CONE_EXTENDED_DECIDEGREES;
rangefinder->init = &tof10120i2cInit;
rangefinder->update = &tof10120i2cUpdate;
rangefinder->read = &tof10120i2cGetDistance;
return true;
}
#endif

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