mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-23 08:15:26 +03:00
Merge remote-tracking branch 'upstream/master' into abo_landing_detection
This commit is contained in:
breadoven
commit
fe5e873356
354 changed files with 9601 additions and 7326 deletions
2
.github/workflows/ci.yml
vendored
2
.github/workflows/ci.yml
vendored
|
|
@ -16,6 +16,8 @@ jobs:
|
|||
- name: Install dependencies
|
||||
run: sudo apt-get update && sudo apt-get -y install ninja-build
|
||||
- name: Setup environment
|
||||
env:
|
||||
ACTIONS_ALLOW_UNSECURE_COMMANDS: true
|
||||
run: |
|
||||
# This is the hash of the commit for the PR
|
||||
# when the action is triggered by PR, empty otherwise
|
||||
|
|
|
|||
|
|
@ -37,7 +37,7 @@ else()
|
|||
include("${CMAKE_CURRENT_SOURCE_DIR}/cmake/${TOOLCHAIN}-checks.cmake")
|
||||
endif()
|
||||
|
||||
project(INAV VERSION 3.1.0)
|
||||
project(INAV VERSION 5.0.0)
|
||||
|
||||
enable_language(ASM)
|
||||
|
||||
|
|
|
|||
17
Notes.md
17
Notes.md
|
|
@ -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
10
README.md
|
|
@ -2,6 +2,13 @@
|
|||
|
||||
|
||||
|
||||
# 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)
|
||||
|
||||
|
|
|
|||
|
|
@ -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})
|
||||
|
|
|
|||
|
|
@ -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}
|
||||
)
|
||||
|
|
|
|||
|
|
@ -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}
|
||||
)
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -114,7 +114,7 @@ set(STM32H7_HAL_SRC
|
|||
# stm32h7xx_ll_rng.c
|
||||
# stm32h7xx_ll_rtc.c
|
||||
stm32h7xx_ll_sdmmc.c
|
||||
# stm32h7xx_ll_spi.c
|
||||
stm32h7xx_ll_spi.c
|
||||
# stm32h7xx_ll_swpmi.c
|
||||
stm32h7xx_ll_tim.c
|
||||
# stm32h7xx_ll_usart.c
|
||||
|
|
@ -142,11 +142,13 @@ 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
|
||||
drivers/dma_stm32h7xx.c
|
||||
drivers/bus_spi_hal.c
|
||||
drivers/bus_spi_hal_ll.c
|
||||
drivers/memprot.h
|
||||
drivers/memprot_hal.c
|
||||
drivers/memprot_stm32h7xx.c
|
||||
|
|
@ -156,7 +158,8 @@ main_sources(STM32H7_SRC
|
|||
drivers/system_stm32h7xx.c
|
||||
drivers/serial_uart_stm32h7xx.c
|
||||
drivers/serial_uart_hal.c
|
||||
# drivers/sdcard/sdmmc_sdio_h7xx.c
|
||||
drivers/sdio.h
|
||||
drivers/sdcard/sdmmc_sdio_h7xx.c
|
||||
)
|
||||
|
||||
main_sources(STM32H7_MSC_SRC
|
||||
|
|
@ -186,8 +189,6 @@ function(target_stm32h7xx)
|
|||
|
||||
OPENOCD_TARGET stm32h7x
|
||||
|
||||
DISABLE_MSC # This should be temporary
|
||||
|
||||
# BOOTLOADER
|
||||
|
||||
${ARGN}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -6,6 +6,9 @@ When armed, the aircraft is ready to fly and the motors will spin when throttle
|
|||
|
||||
Arming and disarming is done using a switch, set up on the modes page. (NOTE: Stick arming was removed in iNav 2.2)
|
||||
|
||||
**YAW STICK ARMING OVERRIDE:**
|
||||
Arming is disabled when Nav modes are configured and no GPS lock is available or if a WP mission is loaded but the first WP is farther than the `nav_wp_safe_distance` setting. This Arming block can be bypassed if need be by setting `nav_extra_arming_safety` to `ALLOW_BYPASS` and moving the Yaw stick to the high position when the Arm switch is used. This bypasses GPS Arm blocking pre INAV 4.0.0 and both GPS and "First WP too far" Arm blocking from INAV 4.0.0.
|
||||
|
||||
## Stick Positions
|
||||
|
||||
The three stick positions are:
|
||||
|
|
@ -36,9 +39,13 @@ The stick positions are combined to activate different functions:
|
|||
| Save current waypoint mission | LOW | CENTER | HIGH | LOW |
|
||||
| Load current waypoint mission | LOW | CENTER | HIGH | HIGH |
|
||||
| Unload waypoint mission | LOW | CENTER | LOW | HIGH |
|
||||
| Increase WP mission index | LOW | CENTER | CENTER | HIGH |
|
||||
| Decrease WP mission index | LOW | CENTER | CENTER | LOW |
|
||||
| Bypass Nav Arm disable | LOW | HIGH | CENTER | CENTER |
|
||||
| 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/).
|
||||
|
||||
|
||||
## Yaw control
|
||||
|
|
|
|||
|
|
@ -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 4.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 basic gyros and accelerometers.
|
||||
* __SET-THR:__ (Pre INAV 4.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 basic 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
|
||||
|
||||
|
|
|
|||
|
|
@ -28,7 +28,6 @@ The following adjustments can be made in flight as well as on the ground.
|
|||
* Throttle Expo
|
||||
* Pitch, Roll, Yaw Rates
|
||||
* Pitch, Roll, Yaw PIDs
|
||||
* Rate profile
|
||||
* Manual rates
|
||||
* FW cruise_throttle, pitch2thr, min_throttle_down_pitch_angle
|
||||
* Board alignment
|
||||
|
|
@ -62,7 +61,7 @@ Hint: With OpenTX transmitters you can combine two momentary OFF-ON switches to
|
|||
|
||||
The CLI command `adjrange` is used to configure adjustment ranges.
|
||||
|
||||
12 adjustment ranges can be defined.
|
||||
20 adjustment ranges can be defined.
|
||||
4 adjustments can be made at the same time, each simultaneous adjustment requires an adjustment slot.
|
||||
|
||||
Show the current ranges using:
|
||||
|
|
@ -77,12 +76,12 @@ Configure a range using:
|
|||
|
||||
| Argument | Value | Meaning |
|
||||
| -------- | ----- |-------- |
|
||||
| Index | 0 - 11 | Select the adjustment range to configure |
|
||||
| Index | 0 - 19 | Select the adjustment range to configure |
|
||||
| Slot | 0 - 3 | Select the adjustment slot to use |
|
||||
| Range Channel | 0 based index, AUX1 = 0, AUX2 = 1 | The AUX channel to use to select an adjustment for a switch/pot |
|
||||
| Range Start | 900 - 2100. Steps of 25, e.g. 900, 925, 950... | Start of range |
|
||||
| Range End | 900 - 2100 | End of range |
|
||||
| Adjustment function | 0 - 11 | See Adjustment function table |
|
||||
| Adjustment function | 0 - 56 | See Adjustment function table |
|
||||
| Adjustment channel | 0 based index, AUX1 = 0, AUX2 = 1 | The channel that is controlled by a 3 Position switch/Pot |
|
||||
|
||||
Range Start/End values should match the values sent by your receiver.
|
||||
|
|
@ -100,8 +99,8 @@ this reason ensure that you define enough ranges to cover the range channel's us
|
|||
|
||||
### Adjustment function
|
||||
|
||||
| Value | Adjustment | Notes |
|
||||
| ----- | ---------- |------ |
|
||||
| Value | Adjustment |
|
||||
| ----- | ---------- |
|
||||
| 0 | None |
|
||||
| 1 | RC RATE |
|
||||
| 2 | RC_EXPO |
|
||||
|
|
@ -111,47 +110,54 @@ this reason ensure that you define enough ranges to cover the range channel's us
|
|||
| 6 | PITCH_ROLL_P |
|
||||
| 7 | PITCH_ROLL_I |
|
||||
| 8 | PITCH_ROLL_D |
|
||||
| 9 | YAW_P |
|
||||
| 10 | YAW_I |
|
||||
| 11 | YAW_D_FF |
|
||||
| 12 | RATE_PROFILE | Switch between 3 rate profiles using a 3 position switch. |
|
||||
| 13 | PITCH_RATE |
|
||||
| 14 | ROLL_RATE |
|
||||
| 15 | PITCH_P |
|
||||
| 16 | PITCH_I |
|
||||
| 17 | PITCH_D_FF |
|
||||
| 18 | ROLL_P |
|
||||
| 19 | ROLL_I |
|
||||
| 20 | ROLL_D_FF |
|
||||
| 21 | RC_YAW_EXPO |
|
||||
| 22 | MANUAL_RC_EXPO |
|
||||
| 23 | MANUAL_RC_YAW_EXPO |
|
||||
| 24 | MANUAL_PITCH_ROLL_RATE |
|
||||
| 25 | MANUAL_ROLL_RATE |
|
||||
| 26 | MANUAL_PITCH_RATE |
|
||||
| 27 | MANUAL_YAW_RATE |
|
||||
| 28 | NAV_FW_CRUISE_THROTTLE |
|
||||
| 29 | NAV_FW_PITCH2THR |
|
||||
| 30 | ROLL_BOARD_ALIGNMENT |
|
||||
| 31 | PITCH_BOARD_ALIGNMENT |
|
||||
| 32 | LEVEL_P |
|
||||
| 33 | LEVEL_I |
|
||||
| 34 | LEVEL_D |
|
||||
| 35 | POS_XY_P |
|
||||
| 36 | POS_XY_I |
|
||||
| 37 | POS_XY_D |
|
||||
| 38 | POS_Z_P |
|
||||
| 39 | POS_Z_I |
|
||||
| 40 | POS_Z_D |
|
||||
| 41 | HEADING_P |
|
||||
| 42 | VEL_XY_P |
|
||||
| 43 | VEL_XY_I |
|
||||
| 44 | VEL_XY_D |
|
||||
| 45 | VEL_Z_P |
|
||||
| 46 | VEL_Z_I |
|
||||
| 47 | VEL_Z_D |
|
||||
| 48 | FW_MIN_THROTTLE_DOWN_PITCH_ANGLE |
|
||||
| 49 | ADJUSTMENT_VTX_POWER_LEVEL |
|
||||
| 9 | PITCH_ROLL_FF |
|
||||
| 10 | PITCH_P |
|
||||
| 11 | PITCH_I |
|
||||
| 12 | PITCH_D |
|
||||
| 13 | PITCH_FF |
|
||||
| 14 | ROLL_P |
|
||||
| 15 | ROLL_I |
|
||||
| 16 | ROLL_D |
|
||||
| 17 | ROL_FF |
|
||||
| 18 | YAW_P |
|
||||
| 19 | YAW_I |
|
||||
| 20 | YAW_D |
|
||||
| 21 | YAW_FF
|
||||
| 22 | Unused |
|
||||
| 23 | PITCH_RATE |
|
||||
| 24 | ROLL_RATE |
|
||||
| 25 | RC_YAW_EXPO |
|
||||
| 26 | MANUAL_RC_EXPO |
|
||||
| 27 | MANUAL_RC_YAW_EXPO |
|
||||
| 28 | MANUAL_PITCH_ROLL_RATE |
|
||||
| 29 | MANUAL_ROLL_RATE |
|
||||
| 30 | MANUAL_PITCH_RATE |
|
||||
| 31 | MANUAL_YAW_RATE |
|
||||
| 32 | NAV_FW_CRUISE_THROTTLE |
|
||||
| 33 | NAV_FW_PITCH2THR |
|
||||
| 34 | ROLL_BOARD_ALIGNMENT |
|
||||
| 35 | PITCH_BOARD_ALIGNMENT |
|
||||
| 36 | LEVEL_P |
|
||||
| 37 | LEVEL_I |
|
||||
| 38 | LEVEL_D |
|
||||
| 39 | POS_XY_P |
|
||||
| 40 | POS_XY_I |
|
||||
| 41 | POS_XY_D |
|
||||
| 42 | POS_Z_P |
|
||||
| 43 | POS_Z_I |
|
||||
| 44 | POS_Z_D |
|
||||
| 45 | HEADING_P |
|
||||
| 46 | VEL_XY_P |
|
||||
| 47 | VEL_XY_I |
|
||||
| 48 | VEL_XY_D |
|
||||
| 49 | VEL_Z_P |
|
||||
| 50 | VEL_Z_I |
|
||||
| 51 | VEL_Z_D |
|
||||
| 52 | FW_MIN_THROTTLE_DOWN_PITCH_ANGLE |
|
||||
| 53 | ADJUSTMENT_VTX_POWER_LEVEL |
|
||||
| 54 | TPA |
|
||||
| 55 | TPA_BREAKPOINT |
|
||||
| 56 | NAV_FW_CONTROL_SMOOTHNESS |
|
||||
|
||||
## Examples
|
||||
|
||||
|
|
@ -193,9 +199,9 @@ range.
|
|||
adjrange 3 2 1 900 1150 6 3
|
||||
adjrange 4 2 1 1150 1300 7 3
|
||||
adjrange 5 2 1 1300 1500 8 3
|
||||
adjrange 6 2 1 1500 1700 9 3
|
||||
adjrange 7 2 1 1700 1850 10 3
|
||||
adjrange 8 2 1 1850 2100 11 3
|
||||
adjrange 6 2 1 1500 1700 18 3
|
||||
adjrange 7 2 1 1700 1850 19 3
|
||||
adjrange 8 2 1 1850 2100 20 3
|
||||
```
|
||||
|
||||
explained:
|
||||
|
|
@ -210,38 +216,19 @@ explained:
|
|||
(1) in the range 1300-1500 then use adjustment Pitch/Roll D (8) when aux 4
|
||||
(3) is in the appropriate position.
|
||||
* configure adjrange 6 to use adjustment slot 3 (2) so that when aux2
|
||||
(1) in the range 1500-1700 then use adjustment Yaw P (9) when aux 4
|
||||
(1) in the range 1500-1700 then use adjustment Yaw P (18) when aux 4
|
||||
(3) is in the appropriate position.
|
||||
* configure adjrange 7 to use adjustment slot 3 (2) so that when aux2
|
||||
(1) in the range 1700-1850 then use adjustment Yaw I (10) when aux 4
|
||||
(1) in the range 1700-1850 then use adjustment Yaw I (19) when aux 4
|
||||
(3) is in the appropriate position.
|
||||
* configure adjrange 8 to use adjustment slot 3 (2) so that when aux2
|
||||
(1) in the range 1850-2100 then use adjustment Yaw D (11) when aux 4
|
||||
(1) in the range 1850-2100 then use adjustment Yaw D (20) when aux 4
|
||||
(3) is in the appropriate position.
|
||||
|
||||
### Example 4 - Use a single 3 position switch to change between 3 different rate profiles
|
||||
|
||||
adjrange 11 3 3 900 2100 12 3
|
||||
|
||||
explained:
|
||||
|
||||
* configure adjrange 11 to use adjustment slot 4 (3) so that when aux4
|
||||
(3) in the range 900-2100 then use adjustment Rate Profile (12) when aux 4
|
||||
(3) is in the appropriate position.
|
||||
|
||||
When the switch is low, rate profile 0 is selcted.
|
||||
When the switch is medium, rate profile 1 is selcted.
|
||||
When the switch is high, rate profile 2 is selcted.
|
||||
|
||||
|
||||
### Configurator examples
|
||||
|
||||
The following 5 images show valid configurations. In all cases the entire usable range for the Range Channel is used.
|
||||
|
||||
|
||||
|
||||
---
|
||||
|
||||
|
||||
|
||||
---
|
||||
|
|
|
|||
|
|
@ -36,17 +36,22 @@ PID meaning:
|
|||
|
||||
## NAV RTH - return to home mode
|
||||
|
||||
Home for RTH is the position where vehicle was armed. This position may be offset by the CLI settings `nav_rth_home_offset_distance` and `nav_rth_home_offset_direction`. RTH requires accelerometer, compass and GPS sensors.
|
||||
Home for RTH is the position where vehicle was first armed. This position may be offset by the CLI settings `nav_rth_home_offset_distance` and `nav_rth_home_offset_direction`. This position may also be overridden with Safehomes. RTH requires accelerometer, compass and GPS sensors.
|
||||
|
||||
If barometer is NOT present, RTH will fly directly to home, altitude control here is up to pilot.
|
||||
|
||||
If barometer is present, RTH will maintain altitude during the return and when home is reached copter will attempt automated landing.
|
||||
When deciding what altitude to maintain, RTH has 4 different modes of operation (controlled by *nav_rth_alt_mode* and *nav_rth_altitude* cli variables):
|
||||
If barometer is present, RTH will maintain altitude during the return. When home is reached, a copter will attempt automated landing. An airplane will either loiter around the home position, or attempt an automated landing, depending on your settings.
|
||||
When deciding what altitude to maintain, RTH has 6 different modes of operation (controlled by *nav_rth_alt_mode* and *nav_rth_altitude* cli variables):
|
||||
* 0 (NAV_RTH_NO_ALT) - keep current altitude during whole RTH sequence (*nav_rth_altitude* is ignored)
|
||||
* 1 (NAV_RTH_EXTRA_ALT) - climb to current altitude plus extra margin prior to heading home (*nav_rth_altitude* defines the extra altitude (cm))
|
||||
* 2 (NAV_RTH_CONST_ALT) - climb/descend to predefined altitude before heading home (*nav_rth_altitude* defined altitude above launch point (cm))
|
||||
* 3 (NAV_RTH_MAX_ALT) - track maximum altitude of the whole flight, climb to that altitude prior to the return (*nav_rth_altitude* is ignored)
|
||||
* 4 (NAV_RTH_AT_LEAST_ALT) - same as 2 (NAV_RTH_CONST_ALT), but only climb, do not descend
|
||||
* 5 (NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT) - Same as 4 (NAV_RTH_AT_LEAST_ALT). But, if above the RTH Altitude, the aircraft will gradually descend to the RTH Altitude. The target is to reach the RTH Altitude as it arrives at the home point. This is to save energy during the RTH.
|
||||
|
||||
## NAV WP - Waypoint mode
|
||||
|
||||
NAV WP allows the craft to autonomously navigate a set route defined by waypoints that are loaded into the FC as a predefined mission.
|
||||
|
||||
## CLI command `wp` to manage waypoints
|
||||
|
||||
|
|
@ -80,7 +85,7 @@ Parameters:
|
|||
|
||||
* `<p3>` - Reserved for future use. If `p2` is provided, then `p3` is also required.
|
||||
|
||||
* `<flag>` - Last waypoint must have set `flag` to 165 (0xA5), otherwise 0.
|
||||
* `<flag>` - Last waypoint must have `flag` set to 165 (0xA5).
|
||||
|
||||
`wp save` - Checks list of waypoints and save from FC to EEPROM (warning: it also saves all unsaved CLI settings like normal `save`).
|
||||
|
||||
|
|
@ -110,3 +115,45 @@ wp 59 0 0 0 0 0 0 0 0
|
|||
```
|
||||
|
||||
Note that the `wp` CLI command shows waypoint list indices, while the MW-XML definition used by mwp, ezgui and the configurator use WP numbers.
|
||||
|
||||
**Multi-missions**\
|
||||
Multi-missions allows up to 9 missions to be stored in the FC at the same time. It is possible to load them into the FC using the CLI. This is acheived by entering single missions into the CLI followed by `wp save` **after** the final mission has been entered (the single missions can be entered one after the other or as a single block entry, it doesn't matter). All missions will then be saved as a Multi Mission in the FC. Saved multi missions display consecutive WP indices from 0 to the last WP in the last mission when displayed using the `wp` command.
|
||||
|
||||
E.g. to enter 3 missions in the CLI enter each mission as a single mission (start WP index for each mission must be 0).
|
||||
```
|
||||
wp 0 1 545722109 -32869291 5000 0 0 0 0
|
||||
wp 1 1 545708178 -32642698 5000 0 0 0 0
|
||||
wp 2 1 545698227 -32385206 5000 0 0 0 165
|
||||
...
|
||||
wp 0 1 545599696 -32958555 5000 0 0 0 0
|
||||
wp 1 1 545537978 -32958555 5000 0 0 0 0
|
||||
wp 2 1 545547933 -32864141 5000 0 0 0 0
|
||||
wp 3 1 545597705 -32695913 5000 0 0 0 0
|
||||
wp 4 1 545552910 -32598066 5000 0 0 0 0
|
||||
wp 5 6 0 0 0 0 0 0 165
|
||||
...
|
||||
wp 0 1 545714148 -32501936 5000 0 0 0 165
|
||||
|
||||
# wp save
|
||||
```
|
||||
|
||||
Multi Mission after saving:
|
||||
```
|
||||
# wp
|
||||
# wp 10 valid
|
||||
wp 0 1 545722109 -32869291 5000 0 0 0 0
|
||||
wp 1 1 545708178 -32642698 5000 0 0 0 0
|
||||
wp 2 1 545698227 -32385206 5000 0 0 0 165
|
||||
wp 3 1 545599696 -32958555 5000 0 0 0 0
|
||||
wp 4 1 545537978 -32958555 5000 0 0 0 0
|
||||
wp 5 1 545547933 -32864141 5000 0 0 0 0
|
||||
wp 6 1 545597705 -32695913 5000 0 0 0 0
|
||||
wp 7 1 545552910 -32598066 5000 0 0 0 0
|
||||
wp 8 6 0 0 0 0 0 0 165
|
||||
wp 9 1 545714148 -32501936 5000 0 0 0 165
|
||||
wp 10 0 0 0 0 0 0 0 0
|
||||
wp 11 0 0 0 0 0 0 0 0
|
||||
wp 12 0 0 0 0 0 0 0 0
|
||||
...
|
||||
wp 59 0 0 0 0 0 0 0 0
|
||||
```
|
||||
165
docs/Profiles.md
165
docs/Profiles.md
|
|
@ -2,63 +2,162 @@
|
|||
|
||||
A profile is a set of configuration settings.
|
||||
|
||||
Currently three profiles are supported. The default profile is profile `0`.
|
||||
Currently three profiles are supported. The default profile is profile `1`.
|
||||
|
||||
## Changing profiles
|
||||
|
||||
### Stick Commands
|
||||
Profiles can be selected using a GUI or the following stick combinations:
|
||||
|
||||
| Profile | Throttle | Yaw | Pitch | Roll |
|
||||
| ------- | -------- | ----- | ------ | ------ |
|
||||
| 0 | Down | Left | Middle | Left |
|
||||
| 1 | Down | Left | Up | Middle |
|
||||
| 2 | Down | Left | Middle | Right |
|
||||
| 1 | Down | Left | Middle | Left |
|
||||
| 2 | Down | Left | Up | Middle |
|
||||
| 3 | Down | Left | Middle | Right |
|
||||
|
||||
The CLI `profile` command can also be used:
|
||||
### CLI
|
||||
The CLI `profile` command can also be used to change profiles:
|
||||
|
||||
```
|
||||
profile <index>
|
||||
```
|
||||
|
||||
# Rate Profiles
|
||||
### Programming (4.0.0 onwards)
|
||||
You can change profiles using the programming frame work. This allows a lot of flexability in how you change profiles.
|
||||
|
||||
INAV supports rate profiles in addition to regular profiles.
|
||||
For example, using a simple switch on channel 15.
|
||||
|
||||
Rate profiles contain settings that adjust how your craft behaves based on control input.
|
||||
[](https://i.imgur.com/SS9CaaO.png)
|
||||
|
||||
Three rate profiles are supported.
|
||||
Or using the speed to change profiles. In this example:
|
||||
- when lower than 25 cm/s (basically not flying), profiles are not effected.
|
||||
- Below 2682 cm/s (60 mph | 97 Km/h) use Profile 1
|
||||
- Above 5364 cm/s (120 mph | 193 Km/h) use Profile 3
|
||||
- Between 2683 and 5364 cm/s, use Profile 2
|
||||
|
||||
Rate profiles can be selected while flying using the inflight adjustments feature.
|
||||
[](https://i.imgur.com/WjkuhhW.png)
|
||||
|
||||
Each normal profile has a setting called 'default_rate_profile`. When a profile is activated the
|
||||
corresponding rate profile is also activated.
|
||||
|
||||
Profile 0 has a default rate profile of 0.
|
||||
Profile 1 has a default rate profile of 1.
|
||||
Profile 2 has a default rate profile of 2.
|
||||
|
||||
The defaults are set this way so that it's simple to configure a profile and a rate profile at the same.
|
||||
|
||||
The current rate profile can be shown or set using the CLI `rateprofile` command:
|
||||
|
||||
```
|
||||
rateprofile <index>
|
||||
```
|
||||
|
||||
The values contained within a rate profile can be seen by using the CLI `dump rates` command.
|
||||
## Profile Contents
|
||||
The values contained within a profile can be seen by using the CLI `dump profile` command.
|
||||
|
||||
e.g
|
||||
```
|
||||
# dump rates
|
||||
# dump profile
|
||||
|
||||
# rateprofile
|
||||
rateprofile 0
|
||||
# profile
|
||||
profile 1
|
||||
|
||||
set rc_expo = 65
|
||||
set mc_p_pitch = 40
|
||||
set mc_i_pitch = 30
|
||||
set mc_d_pitch = 23
|
||||
set mc_cd_pitch = 60
|
||||
set mc_p_roll = 40
|
||||
set mc_i_roll = 30
|
||||
set mc_d_roll = 23
|
||||
set mc_cd_roll = 60
|
||||
set mc_p_yaw = 85
|
||||
set mc_i_yaw = 45
|
||||
set mc_d_yaw = 0
|
||||
set mc_cd_yaw = 60
|
||||
set mc_p_level = 20
|
||||
set mc_i_level = 15
|
||||
set mc_d_level = 75
|
||||
set fw_p_pitch = 5
|
||||
set fw_i_pitch = 7
|
||||
set fw_d_pitch = 0
|
||||
set fw_ff_pitch = 50
|
||||
set fw_p_roll = 5
|
||||
set fw_i_roll = 7
|
||||
set fw_d_roll = 0
|
||||
set fw_ff_roll = 50
|
||||
set fw_p_yaw = 6
|
||||
set fw_i_yaw = 10
|
||||
set fw_d_yaw = 0
|
||||
set fw_ff_yaw = 60
|
||||
set fw_p_level = 20
|
||||
set fw_i_level = 5
|
||||
set fw_d_level = 75
|
||||
set max_angle_inclination_rll = 300
|
||||
set max_angle_inclination_pit = 300
|
||||
set dterm_lpf_hz = 110
|
||||
set dterm_lpf_type = PT2
|
||||
set dterm_lpf2_hz = 0
|
||||
set dterm_lpf2_type = PT1
|
||||
set yaw_lpf_hz = 0
|
||||
set fw_iterm_throw_limit = 165
|
||||
set fw_loiter_direction = RIGHT
|
||||
set fw_reference_airspeed = 1500.000
|
||||
set fw_turn_assist_yaw_gain = 1.000
|
||||
set fw_turn_assist_pitch_gain = 1.000
|
||||
set fw_iterm_limit_stick_position = 0.500
|
||||
set fw_yaw_iterm_freeze_bank_angle = 0
|
||||
set pidsum_limit = 500
|
||||
set pidsum_limit_yaw = 350
|
||||
set iterm_windup = 50
|
||||
set rate_accel_limit_roll_pitch = 0
|
||||
set rate_accel_limit_yaw = 10000
|
||||
set heading_hold_rate_limit = 90
|
||||
set nav_mc_pos_z_p = 50
|
||||
set nav_mc_vel_z_p = 100
|
||||
set nav_mc_vel_z_i = 50
|
||||
set nav_mc_vel_z_d = 10
|
||||
set nav_mc_pos_xy_p = 65
|
||||
set nav_mc_vel_xy_p = 40
|
||||
set nav_mc_vel_xy_i = 15
|
||||
set nav_mc_vel_xy_d = 100
|
||||
set nav_mc_vel_xy_ff = 40
|
||||
set nav_mc_heading_p = 60
|
||||
set nav_mc_vel_xy_dterm_lpf_hz = 2.000
|
||||
set nav_mc_vel_xy_dterm_attenuation = 90
|
||||
set nav_mc_vel_xy_dterm_attenuation_start = 10
|
||||
set nav_mc_vel_xy_dterm_attenuation_end = 60
|
||||
set nav_fw_pos_z_p = 40
|
||||
set nav_fw_pos_z_i = 5
|
||||
set nav_fw_pos_z_d = 10
|
||||
set nav_fw_pos_xy_p = 75
|
||||
set nav_fw_pos_xy_i = 5
|
||||
set nav_fw_pos_xy_d = 8
|
||||
set nav_fw_heading_p = 60
|
||||
set nav_fw_pos_hdg_p = 30
|
||||
set nav_fw_pos_hdg_i = 2
|
||||
set nav_fw_pos_hdg_d = 0
|
||||
set nav_fw_pos_hdg_pidsum_limit = 350
|
||||
set mc_iterm_relax = RP
|
||||
set mc_iterm_relax_cutoff = 15
|
||||
set d_boost_min = 0.500
|
||||
set d_boost_max = 1.250
|
||||
set d_boost_max_at_acceleration = 7500.000
|
||||
set d_boost_gyro_delta_lpf_hz = 80
|
||||
set antigravity_gain = 1.000
|
||||
set antigravity_accelerator = 1.000
|
||||
set antigravity_cutoff_lpf_hz = 15
|
||||
set pid_type = AUTO
|
||||
set mc_cd_lpf_hz = 30
|
||||
set fw_level_pitch_trim = 0.000
|
||||
set smith_predictor_strength = 0.500
|
||||
set smith_predictor_delay = 0.000
|
||||
set smith_predictor_lpf_hz = 50
|
||||
set fw_level_pitch_gain = 5.000
|
||||
set thr_mid = 50
|
||||
set thr_expo = 0
|
||||
set roll_pitch_rate = 0
|
||||
set yaw_rate = 0
|
||||
set tpa_rate = 0
|
||||
set tpa_breakpoint = 1500
|
||||
set fw_tpa_time_constant = 0
|
||||
set rc_expo = 70
|
||||
set rc_yaw_expo = 20
|
||||
set roll_rate = 20
|
||||
set pitch_rate = 20
|
||||
set yaw_rate = 20
|
||||
set manual_rc_expo = 70
|
||||
set manual_rc_yaw_expo = 20
|
||||
set manual_roll_rate = 100
|
||||
set manual_pitch_rate = 100
|
||||
set manual_yaw_rate = 100
|
||||
set fpv_mix_degrees = 0
|
||||
set rate_dynamics_center_sensitivity = 100
|
||||
set rate_dynamics_end_sensitivity = 100
|
||||
set rate_dynamics_center_correction = 10
|
||||
set rate_dynamics_end_correction = 10
|
||||
set rate_dynamics_center_weight = 0
|
||||
set rate_dynamics_end_weight = 0
|
||||
|
||||
```
|
||||
|
|
|
|||
|
|
@ -74,7 +74,8 @@ 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. |
|
||||
| 42 | SET_PROFILE | Sets the active config profile (PIDFF/Rates/Filters/etc) to `Operand A`. `Operand A` must be a valid profile number, currently from 1 to 3. If not, the profile will not change |
|
||||
|
||||
### Operands
|
||||
|
||||
|
|
@ -127,6 +128,8 @@ IPF can be edited using INAV Configurator user interface, of via CLI
|
|||
| 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 |
|
||||
| 35 | LOITER_RADIUS | The current loiter radius in cm. |
|
||||
| 36 | ACTIVE_PROFILE | integer for the active config profile `[1..MAX_PROFILE_COUNT]` |
|
||||
|
||||
#### ACTIVE_WAYPOINT_ACTION
|
||||
|
||||
|
|
|
|||
|
|
@ -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`.
|
||||
524
docs/Settings.md
524
docs/Settings.md
File diff suppressed because it is too large
Load diff
|
|
@ -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.
|
||||
|
|
|
|||
Binary file not shown.
|
Before Width: | Height: | Size: 666 KiB After Width: | Height: | Size: 819 KiB |
File diff suppressed because it is too large
Load diff
|
Before Width: | Height: | Size: 46 KiB After Width: | Height: | Size: 54 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 696 KiB After Width: | Height: | Size: 666 KiB |
|
|
@ -71,7 +71,7 @@ Airplanes
|
|||
|
||||
Buzzer is supported with additional switching MOSFET transistor when connected to PWM Output #9.
|
||||
|
||||
|
||||
|
||||
|
||||
## SD Card Detection
|
||||
|
||||
|
|
@ -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.
|
||||
|
||||
|
||||
|
||||
0
docs/Board - FLYWOOF411.md → docs/boards/FLYWOOF411.md
Executable file → Normal file
0
docs/Board - FLYWOOF411.md → docs/boards/FLYWOOF411.md
Executable file → Normal file
0
docs/Board - KroozX.md → docs/boards/KroozX.md
Executable file → Normal file
0
docs/Board - KroozX.md → docs/boards/KroozX.md
Executable file → Normal file
|
|
@ -1,7 +1,5 @@
|
|||
# Board - [MATEKSYS F405-Wing](https://inavflight.com/shop/p/MATEKF405WING)
|
||||
|
||||
|
||||
|
||||
## Specification:
|
||||
|
||||
* STM32F405 CPU
|
||||
0
docs/Board - NOX.md → docs/boards/NOX.md
Executable file → Normal file
0
docs/Board - NOX.md → docs/boards/NOX.md
Executable file → Normal file
|
|
@ -5,13 +5,14 @@
|
|||
* For Omnibus F4 Pro (with BMP280 baro, current sensor and SD Card) use **OMNIBUSF4PRO** target (LED strip on dedicated connection)
|
||||
* For Onnibus F4 Pro clones (Banggood, AliExpress, eBay, etc.) use **OMNIBUSF4PRO_LEDSTRIPM5** target (LED strip on M5 pin)
|
||||
* For Omnibus F4 Pro Corner use **OMNIBUSF4PRO** target
|
||||
* For Omnibus F4 AIO, see targets listed below
|
||||
|
||||
## Features
|
||||
|
||||
* STM32F405 CPU
|
||||
* Integrated Accelerometer/Gyro MPU6000 or MPU6500 via SPI bus
|
||||
* 6 motor outputs
|
||||
* 3 UART ports (UART1, UART3, UART6)
|
||||
* 3-5 UART ports (UART1, UART3, UART6)
|
||||
* External I2C bus, pins shared with UART3, cannot be used simultaneously
|
||||
* Inverter for SBUS
|
||||
* Blackbox via SDCard or integrated 128mbit flash memory
|
||||
|
|
@ -51,6 +52,14 @@ More target options:
|
|||
* PPM and UART6 cannot be used together, there is no jumper to disconnect PPM input from UART6 RX
|
||||
* Uses target **OMNIBUSF4V3**
|
||||
|
||||
### Omnibus F4 v6
|
||||
|
||||
* Adds more UARTs (total of 5)
|
||||
* Softserial 1 is on TX1 for Smartport
|
||||
* Note that in multirotor configuration, servos are not enabled on S5 and S6
|
||||
* Uses target **OMNIBUSF4V6**
|
||||
|
||||
|
||||
### [Omnibus F4 Pro](https://inavflight.com/shop/p/OMNIBUSF4PRO)
|
||||
|
||||
* Sometimes called Omnibus F4 v2 Pro, but also exists v3, v4 & v5 versions with no functional differences
|
||||
|
|
@ -155,20 +164,30 @@ 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
|
||||
|
||||
|
||||
|
||||
| Pad | SoftwareSerial Role |
|
||||
| ---- | ---- |
|
||||
| CH5 | RX |
|
||||
| CH6 | TX |
|
||||
|
||||
|
||||
|
||||
|
||||
### Omnibus F4 v3/v4/v5 SoftwareSerial Connections
|
||||
|
||||
The SOFTSERIAL1 is an uni-directional port mapped to UART6-TX pin.
|
||||
Softserial mappings can be selected by choosing between different build targets.
|
||||
Some of these can be used for Smartport, FPort, or other inverted protocols.
|
||||
|
||||
OMNIBUSF4V3: Softserial1 on UART6-TX pin.
|
||||
OMNIBUSF4V3_S6_SS: Softserial1 RX or TX on S6 (motor 6)
|
||||
OMNIBUSF4V3_S5_S6_2SS: Softserial1 on S5 RX or TX (motor 5) and Softserial2 on S6 (motor 6)
|
||||
OMNIBUSF4V3_S5S6_SS: Softserial1 on S5/RX and S6/TX
|
||||
|
||||
With the OMNIBUSF4V3 target, SOFTSERIAL1 is an uni-directional port mapped to UART6-TX pin.
|
||||
When enabled, the UART6 is still available as hardware port but it's then RX-only port (good for e.g. receiving S.BUS input). TX instead is controlled in software and can be used for transmitting one-way telemetry (e.g. LTM). Please note that UART6-TX line passes programmable inverter on those boards, so it is a pure output-only port. SmartPort/FPort telemetry requires bi-directional communication, so it is not possible on this port without hardware modification (bypassing the inverter).
|
||||
|
||||
SmartPort / FPort is possible without a hardware inverter by using one of the OMNIBUSF4V3___SS builds and connecting SmartPort to the motor 5 or 6 pad.
|
||||
|
||||
## Where to buy:
|
||||
|
||||
* [Omnibus F4 v5](https://inavflight.com/shop/p/OMNIBUSF4V5)
|
||||
|
|
@ -181,22 +200,22 @@ Following diagrams applies to _Pro_ version with integrated current meter and JS
|
|||
|
||||
## Board layout
|
||||
|
||||
|
||||
|
||||
|
||||
## Flying wing motor and servos
|
||||
|
||||
|
||||
|
||||
|
||||
## RX setup
|
||||
|
||||
|
||||
|
||||
|
||||
## FPV setup
|
||||
|
||||
|
||||
|
||||
|
||||
## GPS setup
|
||||
|
||||
|
||||
|
||||
|
||||
_Diagrams created by Albert Kravcov (skaman82)_
|
||||
|
|
@ -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.
|
||||
|
|
@ -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.
|
||||
|
|
|
|||
61
docs/development/wp_mission_schema/README.md
Normal file
61
docs/development/wp_mission_schema/README.md
Normal file
|
|
@ -0,0 +1,61 @@
|
|||
# XML Mission File Definition
|
||||
|
||||
## Overview
|
||||
|
||||
Historically, mission planners interoperating with inav (and multiwii) missions have used the XML mission file format defined by EOSBandi for MultiWii 2.3 (2013).
|
||||
|
||||
The format is defined the XSD schema here.
|
||||
|
||||
* Lower case tags are preferred by inav. Older tools may prefer uppercase (the original MW usage).
|
||||
* For inav 4.0 and later, the `missionitem/flags` attribute is required for "fly-by home" waypoints and multi-mission files.
|
||||
* For inav 4.0 and later, multi-mission files; mission segments are delimited by a flag value of `165` (the MSP protocol 'last WP' value).
|
||||
* For multi-mission files, the waypoints may be numbered either sequentially across the whole file, or "reset-numbering" within each mission segment. The latter may (or not) be considered to be more "human readable", particularly where `JUMP` is used.
|
||||
* The `mwp` tag was introduced by the eponymous mission planner. Other mission planners may consider that reusing some of the tags (`cx`, `cy` - centre location, `zoom` TMS zoom level, `home-x`, `home-y` - home location) is useful.
|
||||
* `meta` may be used as a synonym for `mwp`.
|
||||
* The `version` tag may be intepreted by mission planners as they see fit. For example, the (obsolete) Android 'ez-gui' application requires '2.3-pre8'. For multi-mission files it is recommended to use another `version`.
|
||||
* the `mwp` / `meta` element may be interleaved with `missionitem` in a multi-mission file to provide mission segment specific home, centre locations and zoom.
|
||||
|
||||
## Examples
|
||||
|
||||
### Multi-mission file with sequential numbering
|
||||
|
||||
```
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<mission>
|
||||
<version value="2.3-pre8"></version>
|
||||
<mwp zoom="14" cx="-3.2632398333333335" cy="54.570950466666666" home-x="0" home-y="0" save-date="2021-11-12T14:07:03Z" generator="impload"></mwp>
|
||||
<missionitem no="1" action="WAYPOINT" lat="54.5722109" lon="-3.2869291" alt="50" parameter1="0" parameter2="0" parameter3="0"></missionitem>
|
||||
<missionitem no="2" action="WAYPOINT" lat="54.5708178" lon="-3.2642698" alt="50" parameter1="0" parameter2="0" parameter3="0"></missionitem>
|
||||
<missionitem no="3" action="WAYPOINT" lat="54.5698227" lon="-3.2385206" alt="50" parameter1="0" parameter2="0" parameter3="0" flag="165"></missionitem>
|
||||
<missionitem no="4" action="WAYPOINT" lat="54.5599696" lon="-3.2958555" alt="50" parameter1="0" parameter2="0" parameter3="0"></missionitem>
|
||||
<missionitem no="5" action="WAYPOINT" lat="54.5537978" lon="-3.2958555" alt="50" parameter1="0" parameter2="0" parameter3="0"></missionitem>
|
||||
<missionitem no="6" action="WAYPOINT" lat="54.5547933" lon="-3.2864141" alt="50" parameter1="0" parameter2="0" parameter3="0"></missionitem>
|
||||
<missionitem no="7" action="WAYPOINT" lat="54.5597705" lon="-3.2695913" alt="50" parameter1="0" parameter2="0" parameter3="0"></missionitem>
|
||||
<missionitem no="8" action="WAYPOINT" lat="54.555291" lon="-3.2598066" alt="50" parameter1="0" parameter2="0" parameter3="0"></missionitem>
|
||||
<missionitem no="9" action="JUMP" lat="0" lon="0" alt="0" parameter1="1" parameter2="0" parameter3="0" flag="165"></missionitem>
|
||||
<missionitem no="10" action="WAYPOINT" lat="54.5714148" lon="-3.2501936" alt="50" parameter1="0" parameter2="0" parameter3="0" flag="165"></missionitem>
|
||||
</mission>
|
||||
```
|
||||
|
||||
### Multi-mission file with "reset" numbering and per-segment metadata with `meta` tag
|
||||
|
||||
```
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<mission>
|
||||
<!--mw planner 0.01-->
|
||||
<version value="42"></version>
|
||||
<meta save-date="2021-11-12T14:22:05+0000" zoom="14" cx="-3.2627249" cy="54.5710168" home-x="-3.2989342" home-y="54.5707123" generator="mwp (mwptools)"><details><distance units="m" value="3130"></distance></details></meta>
|
||||
<missionitem no="1" action="WAYPOINT" lat="54.5722109" lon="-3.2869291" alt="50" parameter1="0" parameter2="0" parameter3="0" flag="0"></missionitem>
|
||||
<missionitem no="2" action="WAYPOINT" lat="54.5708178" lon="-3.2642698" alt="50" parameter1="0" parameter2="0" parameter3="0" flag="0"></missionitem>
|
||||
<missionitem no="3" action="WAYPOINT" lat="54.5698227" lon="-3.2385206" alt="50" parameter1="0" parameter2="0" parameter3="0" flag="165"></missionitem>
|
||||
<meta save-date="2021-11-12T14:22:05+0000" zoom="15" cx="-3.2778311" cy="54.5568837" home-x="-3.2983737" home-y="54.5622331" generator="mwp (mwptools)"><details><distance units="m" value="9029"></distance><nav-speed units="m/s" value="10"></nav-speed><fly-time units="s" value="929"></fly-time><loiter-time units="s" value="0"></loiter-time></details></meta>
|
||||
<missionitem no="1" action="WAYPOINT" lat="54.5599696" lon="-3.2958555" alt="50" parameter1="0" parameter2="0" parameter3="0" flag="0"></missionitem>
|
||||
<missionitem no="2" action="WAYPOINT" lat="54.5537978" lon="-3.2958555" alt="50" parameter1="0" parameter2="0" parameter3="0" flag="0"></missionitem>
|
||||
<missionitem no="3" action="WAYPOINT" lat="54.5547933" lon="-3.2864141" alt="50" parameter1="0" parameter2="0" parameter3="0" flag="0"></missionitem>
|
||||
<missionitem no="4" action="WAYPOINT" lat="54.5597705" lon="-3.2695913" alt="50" parameter1="0" parameter2="0" parameter3="0" flag="0"></missionitem>
|
||||
<missionitem no="5" action="WAYPOINT" lat="54.5552910" lon="-3.2598066" alt="50" parameter1="0" parameter2="0" parameter3="0" flag="0"></missionitem>
|
||||
<missionitem no="6" action="JUMP" lat="0.0000000" lon="0.0000000" alt="0" parameter1="1" parameter2="1" parameter3="0" flag="165"></missionitem>
|
||||
<meta save-date="2021-11-12T14:22:05+0000" zoom="20" cx="-3.2501936" cy="54.5714148" generator="mwp (mwptools)"><details><distance units="m" value="0"></distance></details></meta>
|
||||
<missionitem no="1" action="WAYPOINT" lat="54.5714148" lon="-3.2501936" alt="50" parameter1="0" parameter2="0" parameter3="0" flag="165"></missionitem>
|
||||
</mission>
|
||||
```
|
||||
135
docs/development/wp_mission_schema/mw-mission.xsd
Normal file
135
docs/development/wp_mission_schema/mw-mission.xsd
Normal file
|
|
@ -0,0 +1,135 @@
|
|||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<!-- XSchema for MW / iNav missions
|
||||
This file is part of inav
|
||||
usage e.g. xmllint --noout --schema mw-mission.xsd example.mission
|
||||
Updated 2021-11-12 for 'meta' substitution.
|
||||
-->
|
||||
<xs:schema xmlns:xs="http://www.w3.org/2001/XMLSchema" elementFormDefault="qualified">
|
||||
<xs:element name="mission">
|
||||
<xs:complexType>
|
||||
<xs:sequence>
|
||||
<xs:element ref="version"/>
|
||||
<xs:choice minOccurs="0" maxOccurs="unbounded">
|
||||
<xs:element ref="missionitem"/>
|
||||
<xs:element ref="mwp"/>
|
||||
</xs:choice>
|
||||
</xs:sequence>
|
||||
</xs:complexType>
|
||||
</xs:element>
|
||||
<xs:element name="version">
|
||||
<xs:complexType>
|
||||
<xs:attribute name="value" use="required"/>
|
||||
</xs:complexType>
|
||||
</xs:element>
|
||||
<xs:element name="missionitem">
|
||||
<xs:complexType>
|
||||
<xs:attribute name="action" use="required">
|
||||
<xs:simpleType>
|
||||
<xs:restriction base="xs:token">
|
||||
<xs:enumeration value="UNASSIGNED"/>
|
||||
<xs:enumeration value="WAYPOINT"/>
|
||||
<xs:enumeration value="POSHOLD_UNLIM"/>
|
||||
<xs:enumeration value="POSHOLD_TIME"/>
|
||||
<xs:enumeration value="RTH"/>
|
||||
<xs:enumeration value="SET_POI"/>
|
||||
<xs:enumeration value="JUMP"/>
|
||||
<xs:enumeration value="SET_HEAD"/>
|
||||
<xs:enumeration value="LAND"/>
|
||||
</xs:restriction>
|
||||
</xs:simpleType>
|
||||
</xs:attribute>
|
||||
<!-- no min,max as may be AMSL -->
|
||||
<xs:attribute name="alt" use="required" type="xs:integer"/>
|
||||
<!--
|
||||
flag is not strictly required unless the WP is "Flyby Home"
|
||||
or the are multiple mission segments 'multi-mission', inav 4.0
|
||||
-->
|
||||
<xs:attribute name="flag" use="optional" type="xs:integer"/>
|
||||
<!--
|
||||
Locations are decimal degrees, WGS84 (EPSG:4326)
|
||||
-->
|
||||
<xs:attribute name="lat" use="required">
|
||||
<xs:simpleType>
|
||||
<xs:restriction base="xs:decimal">
|
||||
<xs:minInclusive value="-90"/>
|
||||
<xs:maxInclusive value="90"/>
|
||||
</xs:restriction>
|
||||
</xs:simpleType>
|
||||
</xs:attribute>
|
||||
<xs:attribute name="lon" use="required">
|
||||
<xs:simpleType>
|
||||
<xs:restriction base="xs:decimal">
|
||||
<xs:minInclusive value="-180"/>
|
||||
<xs:maxInclusive value="180"/>
|
||||
</xs:restriction>
|
||||
</xs:simpleType>
|
||||
</xs:attribute>
|
||||
<!-- not really needed, but for historic compatibility ... -->
|
||||
<xs:attribute name="no" use="required" type="xs:integer"/>
|
||||
<xs:attribute name="parameter1" use="required" type="xs:integer"/>
|
||||
<xs:attribute name="parameter2" use="required" type="xs:integer"/>
|
||||
<xs:attribute name="parameter3" use="required" type="xs:integer"/>
|
||||
</xs:complexType>
|
||||
</xs:element>
|
||||
<xs:element name="mwp">
|
||||
<xs:complexType>
|
||||
<xs:sequence>
|
||||
<xs:element ref="details" minOccurs="0"/>
|
||||
</xs:sequence>
|
||||
<xs:attribute name="cx" type="xs:decimal"/>
|
||||
<xs:attribute name="cy" type="xs:decimal"/>
|
||||
<xs:attribute name="generator"/>
|
||||
<xs:attribute name="home-x" type="xs:decimal"/>
|
||||
<xs:attribute name="home-y" type="xs:decimal"/>
|
||||
<xs:attribute name="save-date"/>
|
||||
<xs:attribute name="zoom">
|
||||
<xs:simpleType>
|
||||
<xs:restriction base="xs:integer">
|
||||
<xs:minInclusive value="0"/>
|
||||
<xs:maxInclusive value="20"/>
|
||||
</xs:restriction>
|
||||
</xs:simpleType>
|
||||
</xs:attribute>
|
||||
</xs:complexType>
|
||||
</xs:element>
|
||||
<xs:element name="details">
|
||||
<xs:complexType>
|
||||
<xs:sequence>
|
||||
<xs:element ref="distance"/>
|
||||
<xs:sequence minOccurs="0">
|
||||
<xs:element ref="nav-speed"/>
|
||||
<xs:element ref="fly-time"/>
|
||||
<xs:element ref="loiter-time"/>
|
||||
</xs:sequence>
|
||||
</xs:sequence>
|
||||
</xs:complexType>
|
||||
</xs:element>
|
||||
<xs:element name="distance">
|
||||
<xs:complexType>
|
||||
<xs:attribute name="units" use="required" type="xs:NCName"/>
|
||||
<xs:attribute name="value" use="required" type="xs:integer"/>
|
||||
</xs:complexType>
|
||||
</xs:element>
|
||||
<xs:element name="nav-speed">
|
||||
<xs:complexType>
|
||||
<xs:attribute name="units" use="required"/>
|
||||
<xs:attribute name="value" use="required" type="xs:integer"/>
|
||||
</xs:complexType>
|
||||
</xs:element>
|
||||
<xs:element name="fly-time">
|
||||
<xs:complexType>
|
||||
<xs:attribute name="units" use="required" type="xs:NCName"/>
|
||||
<xs:attribute name="value" use="required" type="xs:integer"/>
|
||||
</xs:complexType>
|
||||
</xs:element>
|
||||
<xs:element name="loiter-time">
|
||||
<xs:complexType>
|
||||
<xs:attribute name="units" use="required" type="xs:NCName"/>
|
||||
<xs:attribute name="value" use="required" type="xs:integer"/>
|
||||
</xs:complexType>
|
||||
</xs:element>
|
||||
<xs:element name="meta" substitutionGroup="mwp"/>
|
||||
<xs:element name="MISSION" substitutionGroup="mission"/>
|
||||
<xs:element name="MISSIONITEM" substitutionGroup="missionitem"/>
|
||||
<xs:element name="VERSION" substitutionGroup="version"/>
|
||||
</xs:schema>
|
||||
|
|
@ -28,8 +28,10 @@
|
|||
#ifdef USE_FULL_ASSERT
|
||||
#include "stm32_assert.h"
|
||||
#else
|
||||
#ifndef assert_param
|
||||
#define assert_param(expr) ((void)0U)
|
||||
#endif
|
||||
#endif
|
||||
|
||||
/** @addtogroup STM32H7xx_LL_Driver
|
||||
* @{
|
||||
|
|
|
|||
|
|
@ -82,12 +82,13 @@ typedef struct _USBD_STORAGE
|
|||
|
||||
typedef struct
|
||||
{
|
||||
// bot_data at start of structure to ensure cache alignment
|
||||
uint8_t bot_data[MSC_MEDIA_PACKET];
|
||||
uint32_t max_lun;
|
||||
uint32_t interface;
|
||||
uint8_t bot_state;
|
||||
uint8_t bot_status;
|
||||
uint16_t bot_data_length;
|
||||
uint8_t bot_data[MSC_MEDIA_PACKET];
|
||||
USBD_MSC_BOT_CBWTypeDef cbw;
|
||||
USBD_MSC_BOT_CSWTypeDef csw;
|
||||
|
||||
|
|
|
|||
|
|
@ -265,6 +265,8 @@ __ALIGN_BEGIN uint8_t USBD_MSC_DeviceQualifierDesc[USB_LEN_DEV_QUALIFIER_DESC]
|
|||
* @}
|
||||
*/
|
||||
|
||||
static USBD_MSC_BOT_HandleTypeDef ClassData __attribute__((aligned(32)));
|
||||
|
||||
|
||||
/** @defgroup MSC_CORE_Private_Functions
|
||||
* @{
|
||||
|
|
@ -301,7 +303,7 @@ uint8_t USBD_MSC_Init(USBD_HandleTypeDef *pdev, uint8_t cfgidx)
|
|||
USBD_LL_OpenEP(pdev, MSC_EPIN_ADDR, USBD_EP_TYPE_BULK, MSC_MAX_FS_PACKET);
|
||||
pdev->ep_in[MSC_EPIN_ADDR & 0xFU].is_used = 1U;
|
||||
}
|
||||
pdev->pMSC_ClassData = USBD_malloc(sizeof(USBD_MSC_BOT_HandleTypeDef));
|
||||
pdev->pMSC_ClassData = (void*)&ClassData;
|
||||
|
||||
if (pdev->pMSC_ClassData == NULL)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -181,7 +181,8 @@ int8_t SCSI_ProcessCmd(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *cmd)
|
|||
*/
|
||||
static int8_t SCSI_TestUnitReady(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *params)
|
||||
{
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *)pdev->pClassData;
|
||||
UNUSED(params);
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *)pdev->pMSC_ClassData;
|
||||
|
||||
/* case 9 : Hi > D0 */
|
||||
if (hmsc->cbw.dDataLength != 0U)
|
||||
|
|
@ -191,7 +192,7 @@ static int8_t SCSI_TestUnitReady(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t
|
|||
return -1;
|
||||
}
|
||||
|
||||
if (((USBD_StorageTypeDef *)pdev->pUserData)->IsReady(lun) != 0)
|
||||
if (((USBD_StorageTypeDef *)pdev->pMSC_UserData)->IsReady(lun) != 0)
|
||||
{
|
||||
SCSI_SenseCode(pdev, lun, NOT_READY, MEDIUM_NOT_PRESENT);
|
||||
hmsc->bot_state = USBD_BOT_NO_DATA;
|
||||
|
|
@ -214,7 +215,7 @@ static int8_t SCSI_Inquiry(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *par
|
|||
{
|
||||
uint8_t *pPage;
|
||||
uint16_t len;
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *)pdev->pClassData;
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *)pdev->pMSC_ClassData;
|
||||
|
||||
if (params[1] & 0x01U)/*Evpd is set*/
|
||||
{
|
||||
|
|
@ -229,7 +230,7 @@ static int8_t SCSI_Inquiry(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *par
|
|||
}
|
||||
else
|
||||
{
|
||||
pPage = (uint8_t *)(void *) & ((USBD_StorageTypeDef *)pdev->pUserData)->pInquiry[lun * STANDARD_INQUIRY_DATA_LEN];
|
||||
pPage = (uint8_t *)(void *) & ((USBD_StorageTypeDef *)pdev->pMSC_UserData)->pInquiry[lun * STANDARD_INQUIRY_DATA_LEN];
|
||||
len = (uint16_t)pPage[4] + 5U;
|
||||
|
||||
if (params[4] <= len)
|
||||
|
|
@ -257,9 +258,10 @@ static int8_t SCSI_Inquiry(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *par
|
|||
*/
|
||||
static int8_t SCSI_ReadCapacity10(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *params)
|
||||
{
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *)pdev->pClassData;
|
||||
UNUSED(params);
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *)pdev->pMSC_ClassData;
|
||||
|
||||
if (((USBD_StorageTypeDef *)pdev->pUserData)->GetCapacity(lun, &hmsc->scsi_blk_nbr, &hmsc->scsi_blk_size) != 0)
|
||||
if (((USBD_StorageTypeDef *)pdev->pMSC_UserData)->GetCapacity(lun, &hmsc->scsi_blk_nbr, &hmsc->scsi_blk_size) != 0)
|
||||
{
|
||||
SCSI_SenseCode(pdev, lun, NOT_READY, MEDIUM_NOT_PRESENT);
|
||||
return -1;
|
||||
|
|
@ -290,7 +292,8 @@ static int8_t SCSI_ReadCapacity10(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_
|
|||
*/
|
||||
static int8_t SCSI_ReadFormatCapacity(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *params)
|
||||
{
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *)pdev->pClassData;
|
||||
UNUSED(params);
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *)pdev->pMSC_ClassData;
|
||||
|
||||
uint16_t blk_size;
|
||||
uint32_t blk_nbr;
|
||||
|
|
@ -301,7 +304,7 @@ static int8_t SCSI_ReadFormatCapacity(USBD_HandleTypeDef *pdev, uint8_t lun, ui
|
|||
hmsc->bot_data[i] = 0U;
|
||||
}
|
||||
|
||||
if (((USBD_StorageTypeDef *)pdev->pUserData)->GetCapacity(lun, &blk_nbr, &blk_size) != 0U)
|
||||
if (((USBD_StorageTypeDef *)pdev->pMSC_UserData)->GetCapacity(lun, &blk_nbr, &blk_size) != 0U)
|
||||
{
|
||||
SCSI_SenseCode(pdev, lun, NOT_READY, MEDIUM_NOT_PRESENT);
|
||||
return -1;
|
||||
|
|
@ -332,7 +335,9 @@ static int8_t SCSI_ReadFormatCapacity(USBD_HandleTypeDef *pdev, uint8_t lun, ui
|
|||
*/
|
||||
static int8_t SCSI_ModeSense6(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *params)
|
||||
{
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *)pdev->pClassData;
|
||||
UNUSED(lun);
|
||||
UNUSED(params);
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *)pdev->pMSC_ClassData;
|
||||
uint16_t len = 8U;
|
||||
hmsc->bot_data_length = len;
|
||||
|
||||
|
|
@ -353,8 +358,10 @@ static int8_t SCSI_ModeSense6(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *p
|
|||
*/
|
||||
static int8_t SCSI_ModeSense10(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *params)
|
||||
{
|
||||
UNUSED(lun);
|
||||
UNUSED(params);
|
||||
uint16_t len = 8U;
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *)pdev->pClassData;
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *)pdev->pMSC_ClassData;
|
||||
|
||||
hmsc->bot_data_length = len;
|
||||
|
||||
|
|
@ -377,8 +384,9 @@ static int8_t SCSI_ModeSense10(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *
|
|||
|
||||
static int8_t SCSI_RequestSense(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *params)
|
||||
{
|
||||
UNUSED(lun);
|
||||
uint8_t i;
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *)pdev->pClassData;
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *)pdev->pMSC_ClassData;
|
||||
|
||||
for (i = 0U ; i < REQUEST_SENSE_DATA_LEN; i++)
|
||||
{
|
||||
|
|
@ -421,7 +429,8 @@ static int8_t SCSI_RequestSense(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t
|
|||
*/
|
||||
void SCSI_SenseCode(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t sKey, uint8_t ASC)
|
||||
{
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *)pdev->pClassData;
|
||||
UNUSED(lun);
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *)pdev->pMSC_ClassData;
|
||||
|
||||
hmsc->scsi_sense[hmsc->scsi_sense_tail].Skey = sKey;
|
||||
hmsc->scsi_sense[hmsc->scsi_sense_tail].w.ASC = ASC << 8;
|
||||
|
|
@ -440,7 +449,9 @@ void SCSI_SenseCode(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t sKey, uint8_
|
|||
*/
|
||||
static int8_t SCSI_StartStopUnit(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *params)
|
||||
{
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *) pdev->pClassData;
|
||||
UNUSED(lun);
|
||||
UNUSED(params);
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *) pdev->pMSC_ClassData;
|
||||
hmsc->bot_data_length = 0U;
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -454,7 +465,7 @@ static int8_t SCSI_StartStopUnit(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t
|
|||
*/
|
||||
static int8_t SCSI_Read10(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *params)
|
||||
{
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *) pdev->pClassData;
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *) pdev->pMSC_ClassData;
|
||||
|
||||
if (hmsc->bot_state == USBD_BOT_IDLE) /* Idle */
|
||||
{
|
||||
|
|
@ -465,7 +476,7 @@ static int8_t SCSI_Read10(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *params
|
|||
return -1;
|
||||
}
|
||||
|
||||
if (((USBD_StorageTypeDef *)pdev->pUserData)->IsReady(lun) != 0)
|
||||
if (((USBD_StorageTypeDef *)pdev->pMSC_UserData)->IsReady(lun) != 0)
|
||||
{
|
||||
SCSI_SenseCode(pdev, lun, NOT_READY, MEDIUM_NOT_PRESENT);
|
||||
return -1;
|
||||
|
|
@ -508,7 +519,7 @@ static int8_t SCSI_Read10(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *params
|
|||
|
||||
static int8_t SCSI_Write10(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *params)
|
||||
{
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *) pdev->pClassData;
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *) pdev->pMSC_ClassData;
|
||||
uint32_t len;
|
||||
|
||||
if (hmsc->bot_state == USBD_BOT_IDLE) /* Idle */
|
||||
|
|
@ -521,14 +532,14 @@ static int8_t SCSI_Write10(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *para
|
|||
}
|
||||
|
||||
/* Check whether Media is ready */
|
||||
if (((USBD_StorageTypeDef *)pdev->pUserData)->IsReady(lun) != 0)
|
||||
if (((USBD_StorageTypeDef *)pdev->pMSC_UserData)->IsReady(lun) != 0)
|
||||
{
|
||||
SCSI_SenseCode(pdev, lun, NOT_READY, MEDIUM_NOT_PRESENT);
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* Check If media is write-protected */
|
||||
if (((USBD_StorageTypeDef *)pdev->pUserData)->IsWriteProtected(lun) != 0)
|
||||
if (((USBD_StorageTypeDef *)pdev->pMSC_UserData)->IsWriteProtected(lun) != 0)
|
||||
{
|
||||
SCSI_SenseCode(pdev, lun, NOT_READY, WRITE_PROTECTED);
|
||||
return -1;
|
||||
|
|
@ -582,7 +593,7 @@ static int8_t SCSI_Write10(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *para
|
|||
|
||||
static int8_t SCSI_Verify10(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *params)
|
||||
{
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *) pdev->pClassData;
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *) pdev->pMSC_ClassData;
|
||||
|
||||
if ((params[1] & 0x02U) == 0x02U)
|
||||
{
|
||||
|
|
@ -610,7 +621,7 @@ static int8_t SCSI_Verify10(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *par
|
|||
static int8_t SCSI_CheckAddressRange(USBD_HandleTypeDef *pdev, uint8_t lun,
|
||||
uint32_t blk_offset, uint32_t blk_nbr)
|
||||
{
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *) pdev->pClassData;
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *) pdev->pMSC_ClassData;
|
||||
|
||||
if ((blk_offset + blk_nbr) > hmsc->scsi_blk_nbr)
|
||||
{
|
||||
|
|
@ -628,12 +639,12 @@ static int8_t SCSI_CheckAddressRange(USBD_HandleTypeDef *pdev, uint8_t lun,
|
|||
*/
|
||||
static int8_t SCSI_ProcessRead(USBD_HandleTypeDef *pdev, uint8_t lun)
|
||||
{
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *)pdev->pClassData;
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *)pdev->pMSC_ClassData;
|
||||
uint32_t len = hmsc->scsi_blk_len * hmsc->scsi_blk_size;
|
||||
|
||||
len = MIN(len, MSC_MEDIA_PACKET);
|
||||
|
||||
if (((USBD_StorageTypeDef *)pdev->pUserData)->Read(lun,
|
||||
if (((USBD_StorageTypeDef *)pdev->pMSC_UserData)->Read(lun,
|
||||
hmsc->bot_data,
|
||||
hmsc->scsi_blk_addr,
|
||||
(len / hmsc->scsi_blk_size)) < 0)
|
||||
|
|
@ -666,12 +677,12 @@ static int8_t SCSI_ProcessRead(USBD_HandleTypeDef *pdev, uint8_t lun)
|
|||
|
||||
static int8_t SCSI_ProcessWrite(USBD_HandleTypeDef *pdev, uint8_t lun)
|
||||
{
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *) pdev->pClassData;
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *) pdev->pMSC_ClassData;
|
||||
uint32_t len = hmsc->scsi_blk_len * hmsc->scsi_blk_size;
|
||||
|
||||
len = MIN(len, MSC_MEDIA_PACKET);
|
||||
|
||||
if (((USBD_StorageTypeDef *)pdev->pUserData)->Write(lun, hmsc->bot_data,
|
||||
if (((USBD_StorageTypeDef *)pdev->pMSC_UserData)->Write(lun, hmsc->bot_data,
|
||||
hmsc->scsi_blk_addr,
|
||||
(len / hmsc->scsi_blk_size)) < 0)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -71,30 +71,21 @@ 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_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_icm42605.c
|
||||
drivers/accgyro/accgyro_icm42605.h
|
||||
drivers/accgyro/accgyro_mpu.c
|
||||
drivers/accgyro/accgyro_mpu.h
|
||||
drivers/accgyro/accgyro_mpu3050.c
|
||||
drivers/accgyro/accgyro_mpu3050.h
|
||||
drivers/accgyro/accgyro_mpu6000.c
|
||||
drivers/accgyro/accgyro_mpu6000.h
|
||||
drivers/accgyro/accgyro_mpu6050.c
|
||||
|
|
@ -162,6 +153,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 +223,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 +233,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 +314,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 +342,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 +402,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 +570,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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -633,18 +653,11 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
|
|||
#endif
|
||||
|
||||
case FLIGHT_LOG_FIELD_CONDITION_FIXED_WING_NAV:
|
||||
#ifdef USE_NAV
|
||||
return STATE(FIXED_WING_LEGACY);
|
||||
#else
|
||||
return false;
|
||||
#endif
|
||||
|
||||
return STATE(FIXED_WING_LEGACY) && blackboxIncludeFlag(BLACKBOX_FEATURE_NAV_PID);
|
||||
|
||||
case FLIGHT_LOG_FIELD_CONDITION_MC_NAV:
|
||||
#ifdef USE_NAV
|
||||
return !STATE(FIXED_WING_LEGACY);
|
||||
#else
|
||||
return false;
|
||||
#endif
|
||||
return !STATE(FIXED_WING_LEGACY) && blackboxIncludeFlag(BLACKBOX_FEATURE_NAV_PID);
|
||||
|
||||
case FLIGHT_LOG_FIELD_CONDITION_RSSI:
|
||||
// Assumes blackboxStart() is called after rxInit(), which should be true since
|
||||
|
|
@ -657,9 +670,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 +765,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,9 +789,12 @@ static void writeIntraframe(void)
|
|||
}
|
||||
|
||||
// Write raw stick positions
|
||||
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_RC_DATA)) {
|
||||
blackboxWriteSigned16VBArray(blackboxCurrent->rcData, 4);
|
||||
}
|
||||
|
||||
// Write roll, pitch and yaw first:
|
||||
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_RC_COMMAND)) {
|
||||
blackboxWriteSigned16VBArray(blackboxCurrent->rcCommand, 3);
|
||||
|
||||
/*
|
||||
|
|
@ -766,6 +802,7 @@ static void writeIntraframe(void)
|
|||
* Throttle lies in range [minthrottle..maxthrottle]:
|
||||
*/
|
||||
blackboxWriteUnsignedVB(blackboxCurrent->rcCommand[THROTTLE] - getThrottleIdleValue());
|
||||
}
|
||||
|
||||
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {
|
||||
/*
|
||||
|
|
@ -811,13 +848,20 @@ static void writeIntraframe(void)
|
|||
}
|
||||
|
||||
blackboxWriteSigned16VBArray(blackboxCurrent->gyroADC, 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);
|
||||
}
|
||||
|
||||
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());
|
||||
|
||||
|
|
@ -826,6 +870,7 @@ static void writeIntraframe(void)
|
|||
for (int x = 1; x < motorCount; x++) {
|
||||
blackboxWriteSignedVB(blackboxCurrent->motor[x] - blackboxCurrent->motor[0]);
|
||||
}
|
||||
}
|
||||
|
||||
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_SERVOS)) {
|
||||
for (int x = 0; x < MAX_SUPPORTED_SERVOS; x++) {
|
||||
|
|
@ -834,10 +879,13 @@ static void writeIntraframe(void)
|
|||
}
|
||||
}
|
||||
|
||||
#ifdef NAV_BLACKBOX
|
||||
blackboxWriteSignedVB(blackboxCurrent->navState);
|
||||
|
||||
blackboxWriteSignedVB(blackboxCurrent->navFlags);
|
||||
|
||||
/*
|
||||
* NAV_POS fields
|
||||
*/
|
||||
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NAV_POS)) {
|
||||
blackboxWriteSignedVB(blackboxCurrent->navEPH);
|
||||
blackboxWriteSignedVB(blackboxCurrent->navEPV);
|
||||
|
||||
|
|
@ -849,10 +897,6 @@ static void writeIntraframe(void)
|
|||
blackboxWriteSignedVB(blackboxCurrent->navRealVel[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]);
|
||||
}
|
||||
|
|
@ -862,7 +906,13 @@ static void writeIntraframe(void)
|
|||
}
|
||||
|
||||
blackboxWriteSignedVB(blackboxCurrent->navSurface);
|
||||
#endif
|
||||
}
|
||||
|
||||
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NAV_ACC)) {
|
||||
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
|
||||
blackboxWriteSignedVB(blackboxCurrent->navAccNEU[x]);
|
||||
}
|
||||
}
|
||||
|
||||
//Rotate our history buffers:
|
||||
|
||||
|
|
@ -943,6 +993,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 +1034,22 @@ static void writeInterframe(void)
|
|||
*/
|
||||
|
||||
// rcData
|
||||
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);
|
||||
}
|
||||
|
||||
// rcCommand
|
||||
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);
|
||||
}
|
||||
|
||||
//Check for sensors that are updated periodically (so deltas are normally zero)
|
||||
int optionalFieldCount = 0;
|
||||
|
|
@ -1039,21 +1096,35 @@ 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);
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
/*
|
||||
* NAV_POS fields
|
||||
*/
|
||||
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NAV_POS)) {
|
||||
blackboxWriteSignedVB(blackboxCurrent->navEPH - blackboxLast->navEPH);
|
||||
blackboxWriteSignedVB(blackboxCurrent->navEPV - blackboxLast->navEPV);
|
||||
|
||||
|
|
@ -1065,9 +1136,6 @@ static void writeInterframe(void)
|
|||
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]->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);
|
||||
|
|
@ -1078,7 +1146,13 @@ static void writeInterframe(void)
|
|||
}
|
||||
|
||||
blackboxWriteSignedVB(blackboxCurrent->navSurface - blackboxLast->navSurface);
|
||||
#endif
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
//Rotate our history buffers
|
||||
blackboxHistory[2] = blackboxHistory[1];
|
||||
|
|
@ -1387,21 +1461,19 @@ static void loadMainState(timeUs_t currentTimeUs)
|
|||
|
||||
blackboxCurrent->time = currentTimeUs;
|
||||
|
||||
#ifdef USE_NAV
|
||||
const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers();
|
||||
#endif
|
||||
|
||||
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
|
||||
blackboxCurrent->axisPID_Setpoint[i] = axisPID_Setpoint[i];
|
||||
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
|
||||
blackboxCurrent->magADC[i] = mag.magADC[i];
|
||||
#endif
|
||||
#ifdef USE_NAV
|
||||
if (!STATE(FIXED_WING_LEGACY)) {
|
||||
// log requested velocity in cm/s
|
||||
blackboxCurrent->mcPosAxisP[i] = lrintf(nav_pids->pos[i].output_constrained);
|
||||
|
|
@ -1413,10 +1485,8 @@ static void loadMainState(timeUs_t currentTimeUs)
|
|||
blackboxCurrent->mcVelAxisPID[3][i] = lrintf(nav_pids->vel[i].feedForward);
|
||||
blackboxCurrent->mcVelAxisOutput[i] = lrintf(nav_pids->vel[i].output_constrained);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
#ifdef USE_NAV
|
||||
if (STATE(FIXED_WING_LEGACY)) {
|
||||
|
||||
// log requested pitch in decidegrees
|
||||
|
|
@ -1437,7 +1507,6 @@ static void loadMainState(timeUs_t currentTimeUs)
|
|||
blackboxCurrent->mcSurfacePID[2] = lrintf(nav_pids->surface.derivative / 10);
|
||||
blackboxCurrent->mcSurfacePIDOutput = lrintf(nav_pids->surface.output_constrained / 10);
|
||||
}
|
||||
#endif
|
||||
|
||||
for (int i = 0; i < 4; i++) {
|
||||
blackboxCurrent->rcData[i] = rxGetChannelValue(i);
|
||||
|
|
@ -1479,7 +1548,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 +1560,6 @@ static void loadMainState(timeUs_t currentTimeUs)
|
|||
blackboxCurrent->navTargetPos[i] = navTargetPosition[i];
|
||||
}
|
||||
blackboxCurrent->navSurface = navActualSurface;
|
||||
#endif
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -1690,15 +1757,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,14 +1796,9 @@ 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
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz", "%d,%d", gyroConfig()->gyro_notch_hz,
|
||||
0);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_notch_cutoff,
|
||||
1);
|
||||
BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz", "%d", accelerometerConfig()->acc_lpf_hz);
|
||||
BLACKBOX_PRINT_HEADER_LINE("acc_hardware", "%d", accelerometerConfig()->acc_hardware);
|
||||
#ifdef USE_BARO
|
||||
|
|
@ -1749,9 +1814,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);
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -101,9 +101,7 @@ static const OSD_Entry menuFeaturesEntries[] =
|
|||
OSD_LABEL_ENTRY("--- FEATURES ---"),
|
||||
OSD_SUBMENU_ENTRY("BLACKBOX", &cmsx_menuBlackbox),
|
||||
OSD_SUBMENU_ENTRY("MIXER & SERVOS", &cmsx_menuMixerServo),
|
||||
#if defined(USE_NAV)
|
||||
OSD_SUBMENU_ENTRY("NAVIGATION", &cmsx_menuNavigation),
|
||||
#endif
|
||||
#if defined(USE_VTX_CONTROL)
|
||||
OSD_SUBMENU_ENTRY("VTX", &cmsx_menuVtxControl),
|
||||
#endif // VTX_CONTROL
|
||||
|
|
|
|||
|
|
@ -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),
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -26,8 +26,6 @@
|
|||
|
||||
#include "platform.h"
|
||||
|
||||
#if defined(USE_NAV)
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
|
|
@ -44,13 +42,15 @@ static const OSD_Entry cmsx_menuNavSettingsEntries[] =
|
|||
OSD_LABEL_ENTRY("-- BASIC SETTINGS --"),
|
||||
|
||||
OSD_SETTING_ENTRY("CONTROL MODE", SETTING_NAV_USER_CONTROL_MODE),
|
||||
OSD_SETTING_ENTRY("MAX NAV SPEED", SETTING_NAV_AUTO_SPEED),
|
||||
OSD_SETTING_ENTRY("MC NAV SPEED", SETTING_NAV_AUTO_SPEED),
|
||||
OSD_SETTING_ENTRY("MC MAX NAV SPEED", SETTING_NAV_MAX_AUTO_SPEED),
|
||||
OSD_SETTING_ENTRY("MAX CRUISE SPEED", SETTING_NAV_MANUAL_SPEED),
|
||||
OSD_SETTING_ENTRY("MAX NAV CLIMB RATE", SETTING_NAV_AUTO_CLIMB_RATE),
|
||||
OSD_SETTING_ENTRY("MAX AH CLIMB RATE", SETTING_NAV_MANUAL_CLIMB_RATE),
|
||||
OSD_SETTING_ENTRY("MC MAX BANK ANGLE", SETTING_NAV_MC_BANK_ANGLE),
|
||||
OSD_SETTING_ENTRY("MID THR FOR AH", SETTING_NAV_USE_MIDTHR_FOR_ALTHOLD),
|
||||
OSD_SETTING_ENTRY("MC HOVER THR", SETTING_NAV_MC_HOVER_THR),
|
||||
OSD_SETTING_ENTRY("MISSION RESTART", SETTING_NAV_WP_MISSION_RESTART),
|
||||
|
||||
OSD_BACK_AND_END_ENTRY,
|
||||
};
|
||||
|
|
@ -72,7 +72,10 @@ static const CMS_Menu cmsx_menuNavSettings = {
|
|||
|
||||
OSD_SETTING_ENTRY("RTH ALT MODE", SETTING_NAV_RTH_ALT_MODE),
|
||||
OSD_SETTING_ENTRY("RTH ALT", SETTING_NAV_RTH_ALTITUDE),
|
||||
OSD_SETTING_ENTRY("RTH HOME ALT", SETTING_NAV_RTH_HOME_ALTITUDE),
|
||||
OSD_SETTING_ENTRY("CLIMB FIRST", SETTING_NAV_RTH_CLIMB_FIRST),
|
||||
OSD_SETTING_ENTRY("CLIMB 1ST STAGE MODE", SETTING_NAV_RTH_CLIMB_FIRST_STAGE_MODE),
|
||||
OSD_SETTING_ENTRY("CLIMB 1ST STAGE ALT", SETTING_NAV_RTH_CLIMB_FIRST_STAGE_ALTITUDE),
|
||||
OSD_SETTING_ENTRY("TAIL FIRST", SETTING_NAV_RTH_TAIL_FIRST),
|
||||
OSD_SETTING_ENTRY("LAND AFTER RTH", SETTING_NAV_RTH_ALLOW_LANDING),
|
||||
OSD_SETTING_ENTRY("LAND MINALT VSPD", SETTING_NAV_LAND_MINALT_VSPD),
|
||||
|
|
@ -190,7 +193,9 @@ static const OSD_Entry cmsx_menuMissionSettingsEntries[] =
|
|||
OSD_SETTING_ENTRY("WP LOAD ON BOOT", SETTING_NAV_WP_LOAD_ON_BOOT),
|
||||
OSD_SETTING_ENTRY("WP REACHED RADIUS", SETTING_NAV_WP_RADIUS),
|
||||
OSD_SETTING_ENTRY("WP SAFE DISTANCE", SETTING_NAV_WP_SAFE_DISTANCE),
|
||||
|
||||
#ifdef USE_MULTI_MISSION
|
||||
OSD_SETTING_ENTRY("MULTI MISSION NUMBER", SETTING_NAV_WP_MULTI_MISSION_INDEX),
|
||||
#endif
|
||||
OSD_BACK_AND_END_ENTRY,
|
||||
};
|
||||
|
||||
|
|
@ -227,5 +232,3 @@ const CMS_Menu cmsx_menuNavigation = {
|
|||
.onGlobalExit = NULL,
|
||||
.entries = cmsx_menuNavigationEntries
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -146,6 +146,7 @@ static const OSD_Entry menuCrsfRxEntries[]=
|
|||
OSD_SETTING_ENTRY("LQ FORMAT", SETTING_OSD_CRSF_LQ_FORMAT),
|
||||
OSD_SETTING_ENTRY("LQ ALARM LEVEL", SETTING_OSD_LINK_QUALITY_ALARM),
|
||||
OSD_SETTING_ENTRY("SNR ALARM LEVEL", SETTING_OSD_SNR_ALARM),
|
||||
OSD_SETTING_ENTRY("RX SENSITIVITY", SETTING_OSD_RSSI_DBM_MIN),
|
||||
OSD_ELEMENT_ENTRY("RX RSSI DBM", OSD_CRSF_RSSI_DBM),
|
||||
OSD_ELEMENT_ENTRY("RX LQ", OSD_CRSF_LQ),
|
||||
OSD_ELEMENT_ENTRY("RX SNR ALARM", OSD_CRSF_SNR_DB),
|
||||
|
|
@ -426,7 +427,6 @@ static const OSD_Entry menuOsdHud2Entries[] = {
|
|||
OSD_SETTING_ENTRY("RADAR MAX AIRCRAFT", SETTING_OSD_HUD_RADAR_DISP),
|
||||
OSD_SETTING_ENTRY("RADAR MIN RANGE", SETTING_OSD_HUD_RADAR_RANGE_MIN),
|
||||
OSD_SETTING_ENTRY("RADAR MAX RANGE", SETTING_OSD_HUD_RADAR_RANGE_MAX),
|
||||
OSD_SETTING_ENTRY("RADAR DET. NEAREST", SETTING_OSD_HUD_RADAR_NEAREST),
|
||||
OSD_SETTING_ENTRY("NEXT WAYPOINTS", SETTING_OSD_HUD_WP_DISP),
|
||||
OSD_BACK_ENTRY,
|
||||
OSD_END_ENTRY,
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -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);
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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__ ({ \
|
||||
|
|
|
|||
|
|
@ -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
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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"
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
77
src/main/config/config_streamer_extflash.c
Normal 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
61
src/main/config/config_streamer_ram.c
Normal 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
|
||||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
/*
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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 adxl345Detect(accDev_t *acc);
|
||||
|
|
@ -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
|
||||
355
src/main/drivers/accgyro/accgyro_bmi270.c
Normal file
355
src/main/drivers/accgyro/accgyro_bmi270.c
Normal file
|
|
@ -0,0 +1,355 @@
|
|||
/*
|
||||
* 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=®, .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
|
||||
gyro->gyroAlign = gyro->busDev->param;
|
||||
return true;
|
||||
}
|
||||
#endif // USE_IMU_BMI270
|
||||
26
src/main/drivers/accgyro/accgyro_bmi270.h
Normal file
26
src/main/drivers/accgyro/accgyro_bmi270.h
Normal 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
213
src/main/drivers/accgyro/accgyro_bmi270_maximum_fifo.c
Normal 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
272
src/main/drivers/accgyro/accgyro_icm42605.c
Normal 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
|
||||
|
|
@ -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);
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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);
|
||||
|
|
@ -1,124 +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 "build/build_config.h"
|
||||
#include "build/debug.h"
|
||||
|
||||
#include "common/maths.h"
|
||||
|
||||
#include "drivers/time.h"
|
||||
#include "drivers/io.h"
|
||||
#include "drivers/bus_spi.h"
|
||||
|
||||
#include "drivers/sensor.h"
|
||||
#include "drivers/accgyro/accgyro.h"
|
||||
#include "drivers/accgyro/accgyro_l3gd20.h"
|
||||
|
||||
#ifdef USE_IMU_L3GD20
|
||||
|
||||
#define READ_CMD ((uint8_t)0x80)
|
||||
#define MULTIPLEBYTE_CMD ((uint8_t)0x40)
|
||||
#define DUMMY_BYTE ((uint8_t)0x00)
|
||||
|
||||
#define CTRL_REG1_ADDR 0x20
|
||||
#define CTRL_REG4_ADDR 0x23
|
||||
#define CTRL_REG5_ADDR 0x24
|
||||
#define OUT_TEMP_ADDR 0x26
|
||||
#define OUT_X_L_ADDR 0x28
|
||||
|
||||
#define MODE_ACTIVE ((uint8_t)0x08)
|
||||
|
||||
#define OUTPUT_DATARATE_1 ((uint8_t)0x00)
|
||||
#define OUTPUT_DATARATE_2 ((uint8_t)0x40)
|
||||
#define OUTPUT_DATARATE_3 ((uint8_t)0x80)
|
||||
#define OUTPUT_DATARATE_4 ((uint8_t)0xC0)
|
||||
|
||||
#define AXES_ENABLE ((uint8_t)0x07)
|
||||
|
||||
#define BANDWIDTH_1 ((uint8_t)0x00)
|
||||
#define BANDWIDTH_2 ((uint8_t)0x10)
|
||||
#define BANDWIDTH_3 ((uint8_t)0x20)
|
||||
#define BANDWIDTH_4 ((uint8_t)0x30)
|
||||
|
||||
#define FULLSCALE_250 ((uint8_t)0x00)
|
||||
#define FULLSCALE_500 ((uint8_t)0x10)
|
||||
#define FULLSCALE_2000 ((uint8_t)0x20)
|
||||
|
||||
#define BLOCK_DATA_UPDATE_CONTINUOUS ((uint8_t)0x00)
|
||||
|
||||
#define BLE_MSB ((uint8_t)0x40)
|
||||
|
||||
#define BOOT ((uint8_t)0x80)
|
||||
|
||||
void l3gd20GyroInit(gyroDev_t *gyro)
|
||||
{
|
||||
busWrite(gyro->busDev, CTRL_REG5_ADDR, BOOT);
|
||||
delay(1);
|
||||
|
||||
busWrite(gyro->busDev, CTRL_REG1_ADDR, MODE_ACTIVE | OUTPUT_DATARATE_3 | AXES_ENABLE | BANDWIDTH_3);
|
||||
delay(1);
|
||||
|
||||
busWrite(gyro->busDev, CTRL_REG4_ADDR, BLOCK_DATA_UPDATE_CONTINUOUS | BLE_MSB | FULLSCALE_2000);
|
||||
delay(100);
|
||||
}
|
||||
|
||||
static bool l3gd20GyroRead(gyroDev_t *gyro)
|
||||
{
|
||||
uint8_t buf[6];
|
||||
|
||||
busReadBuf(gyro->busDev, OUT_X_L_ADDR | READ_CMD | MULTIPLEBYTE_CMD, buf, 6);
|
||||
|
||||
gyro->gyroADCRaw[0] = (int16_t)((buf[0] << 8) | buf[1]);
|
||||
gyro->gyroADCRaw[1] = (int16_t)((buf[2] << 8) | buf[3]);
|
||||
gyro->gyroADCRaw[2] = (int16_t)((buf[4] << 8) | buf[5]);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool deviceDetect(busDevice_t * dev)
|
||||
{
|
||||
UNUSED(dev);
|
||||
return true; // blindly assume it's present, for now.
|
||||
}
|
||||
|
||||
bool l3gd20Detect(gyroDev_t *gyro)
|
||||
{
|
||||
gyro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_L3GD20, gyro->imuSensorToUse, OWNER_MPU);
|
||||
if (gyro->busDev == NULL) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!deviceDetect(gyro->busDev)) {
|
||||
busDeviceDeInit(gyro->busDev);
|
||||
return false;
|
||||
}
|
||||
|
||||
gyro->initFn = l3gd20GyroInit;
|
||||
gyro->readFn = l3gd20GyroRead;
|
||||
|
||||
// Page 9 in datasheet, So - Sensitivity, Full Scale = 2000, 70 mdps/digit
|
||||
gyro->scale = 0.07f;
|
||||
gyro->gyroAlign = gyro->busDev->param;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -1,170 +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"
|
||||
|
||||
#ifdef USE_IMU_LSM303DLHC
|
||||
|
||||
#include "build/debug.h"
|
||||
|
||||
#include "common/maths.h"
|
||||
#include "common/axis.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_lsm303dlhc.h"
|
||||
|
||||
// Addresses (7 bit address format)
|
||||
|
||||
#define LSM303DLHC_ACCEL_ADDRESS 0x19
|
||||
#define LSM303DLHC_MAG_ADDRESS 0x1E
|
||||
|
||||
/**
|
||||
* Address Auto Increment - See LSM303DLHC datasheet, Section 5.1.1 I2C operation.
|
||||
* http://www.st.com/web/en/resource/technical/document/datasheet/DM00027543.pdf
|
||||
*
|
||||
* "The I2C embedded inside the LSM303DLHC behaves like a slave device and the following protocol must be adhered to.
|
||||
* After the START condition (ST) a slave address is sent, once a slave acknowledge (SAK) has been returned, an 8-bit
|
||||
* sub-address (SUB) is transmitted; the 7 LSBs represent the actual register address while the MSB enables address
|
||||
* autoincrement.
|
||||
*
|
||||
* If the MSB of the SUB field is ‘1’, the SUB (register address) is automatically increased to allow multiple data
|
||||
* Read/Write.
|
||||
*
|
||||
* To minimize the communication between the master and magnetic digital interface of LSM303DLHC, the address pointer
|
||||
* updates automatically without master intervention. This automatic address pointer update has two additional
|
||||
* features. First, when address 12 or higher is accessed, the pointer updates to address 00, and secondly, when
|
||||
* address 08 is reached, the pointer rolls back to address 03. Logically, the address pointer operation functions
|
||||
* as shown below.
|
||||
* 1) If (address pointer = 08) then the address pointer = 03
|
||||
* Or else, if (address pointer >= 12) then the address pointer = 0
|
||||
* Or else, (address pointer) = (address pointer) + 1
|
||||
*
|
||||
* The address pointer value itself cannot be read via the I2C bus"
|
||||
*/
|
||||
#define AUTO_INCREMENT_ENABLE 0x80
|
||||
|
||||
// Registers
|
||||
|
||||
#define LSM303DLHC_STATUS_REG_A 0x27 /* Status register acceleration */
|
||||
#define CTRL_REG1_A 0x20
|
||||
#define CTRL_REG4_A 0x23
|
||||
#define CTRL_REG5_A 0x24
|
||||
#define OUT_X_L_A 0x28
|
||||
#define CRA_REG_M 0x00
|
||||
#define CRB_REG_M 0x01
|
||||
#define MR_REG_M 0x02
|
||||
#define OUT_X_H_M 0x03
|
||||
|
||||
///////////////////////////////////////
|
||||
|
||||
#define ODR_1344_HZ 0x90
|
||||
#define AXES_ENABLE 0x07
|
||||
|
||||
#define FULLSCALE_2G 0x00
|
||||
#define FULLSCALE_4G 0x10
|
||||
#define FULLSCALE_8G 0x20
|
||||
#define FULLSCALE_16G 0x30
|
||||
|
||||
#define BOOT 0x80
|
||||
|
||||
///////////////////////////////////////
|
||||
|
||||
#define ODR_75_HZ 0x18
|
||||
#define ODR_15_HZ 0x10
|
||||
|
||||
#define FS_1P3_GA 0x20
|
||||
#define FS_1P9_GA 0x40
|
||||
#define FS_2P5_GA 0x60
|
||||
#define FS_4P0_GA 0x80
|
||||
#define FS_4P7_GA 0xA0
|
||||
#define FS_5P6_GA 0xC0
|
||||
#define FS_8P1_GA 0xE0
|
||||
|
||||
#define CONTINUOUS_CONVERSION 0x00
|
||||
|
||||
void lsm303dlhcAccInit(accDev_t *acc)
|
||||
{
|
||||
busWrite(acc->busDev, CTRL_REG5_A, BOOT);
|
||||
delay(100);
|
||||
|
||||
busWrite(acc->busDev, CTRL_REG1_A, ODR_1344_HZ | AXES_ENABLE);
|
||||
delay(10);
|
||||
|
||||
busWrite(acc->busDev, CTRL_REG4_A, FULLSCALE_4G);
|
||||
delay(100);
|
||||
|
||||
acc->acc_1G = 512 * 8;
|
||||
}
|
||||
|
||||
// Read 3 gyro values into user-provided buffer. No overrun checking is done.
|
||||
static bool lsm303dlhcAccRead(accDev_t *acc)
|
||||
{
|
||||
uint8_t buf[6];
|
||||
|
||||
bool ack = busReadBuf(acc->busDev, AUTO_INCREMENT_ENABLE | OUT_X_L_A, buf, 6);
|
||||
|
||||
if (!ack) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// the values range from -8192 to +8191
|
||||
acc->ADCRaw[X] = (int16_t)((buf[1] << 8) | buf[0]) / 2;
|
||||
acc->ADCRaw[Y] = (int16_t)((buf[3] << 8) | buf[2]) / 2;
|
||||
acc->ADCRaw[Z] = (int16_t)((buf[5] << 8) | buf[4]) / 2;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool deviceDetect(busDevice_t * busDev)
|
||||
{
|
||||
bool ack = false;
|
||||
uint8_t status = 0;
|
||||
|
||||
ack = busRead(busDev, LSM303DLHC_STATUS_REG_A, &status);
|
||||
if (!ack)
|
||||
return false;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool lsm303dlhcAccDetect(accDev_t *acc)
|
||||
{
|
||||
acc->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_LSM303DLHC, acc->imuSensorToUse, OWNER_MPU);
|
||||
if (acc->busDev == NULL) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!deviceDetect(acc->busDev)) {
|
||||
busDeviceDeInit(acc->busDev);
|
||||
return false;
|
||||
}
|
||||
|
||||
acc->initFn = lsm303dlhcAccInit;
|
||||
acc->readFn = lsm303dlhcAccRead;
|
||||
acc->accAlign = acc->busDev->param;
|
||||
return true;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -26,7 +26,7 @@
|
|||
// MPU6050
|
||||
#define MPU_RA_WHO_AM_I_LEGACY 0x00
|
||||
|
||||
#define MPUx0x0_WHO_AM_I_CONST (0x68) // MPU3050, 6000 and 6050
|
||||
#define MPUx0x0_WHO_AM_I_CONST (0x68) // MPU6000 and 6050
|
||||
#define MPU6000_WHO_AM_I_CONST (0x68)
|
||||
#define MPU6500_WHO_AM_I_CONST (0x70)
|
||||
#define MPU9250_WHO_AM_I_CONST (0x71)
|
||||
|
|
@ -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
|
||||
|
|
|
|||
Some files were not shown because too many files have changed in this diff Show more
Loading…
Add table
Add a link
Reference in a new issue