diff --git a/CMakeLists.txt b/CMakeLists.txt index 9e7de58298..edb0b7640e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -37,7 +37,7 @@ else() include("${CMAKE_CURRENT_SOURCE_DIR}/cmake/${TOOLCHAIN}-checks.cmake") endif() -project(INAV VERSION 3.1.0) +project(INAV VERSION 4.0.0) enable_language(ASM) diff --git a/Notes.md b/Notes.md deleted file mode 100644 index 89d041d2b1..0000000000 --- a/Notes.md +++ /dev/null @@ -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) diff --git a/README.md b/README.md index f34db4d136..d07d7d0eab 100644 --- a/README.md +++ b/README.md @@ -1,11 +1,13 @@ # INAV - navigation capable flight controller -## F3 based flight controllers - -> STM32 F3 flight controllers like Omnibus F3 or SP Racing F3 are deprecated and soon they will reach the end of support in INAV. If you are still using F3 boards, please migrate to F4 or F7. - ![INAV](http://static.rcgroups.net/forums/attachments/6/1/0/3/7/6/a9088858-102-inav.png) -![Travis CI status](https://travis-ci.org/iNavFlight/inav.svg?branch=master) + +# 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 @@ -16,9 +18,9 @@ * Fully configurable mixer that allows to run any hardware you want: multirotor, fixed wing, rovers, boats and other experimental devices * Multiple sensor support: GPS, Pitot tube, sonar, lidar, temperature, ESC with BlHeli_32 telemetry * SmartAudio and IRC Tramp VTX support -* DSHOT and Multishot ESCs * Blackbox flight recorder logging * On Screen Display (OSD) - both character and pixel style +* DJI OSD integration: all elements, system messages and warnings * Telemetry: SmartPort, FPort, MAVlink, LTM * Multi-color RGB LED Strip support * Advanced gyro filtering: Matrix Filter and RPM filter @@ -52,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) diff --git a/cmake/arm-none-eabi-checks.cmake b/cmake/arm-none-eabi-checks.cmake index b18ace0bec..0d7633bd44 100644 --- a/cmake/arm-none-eabi-checks.cmake +++ b/cmake/arm-none-eabi-checks.cmake @@ -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}) diff --git a/cmake/settings.cmake b/cmake/settings.cmake index 5d4e4e3862..065f3a87db 100644 --- a/cmake/settings.cmake +++ b/cmake/settings.cmake @@ -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} ) diff --git a/cmake/stm32.cmake b/cmake/stm32.cmake index ab29a524b8..e3f749bf7d 100644 --- a/cmake/stm32.cmake +++ b/cmake/stm32.cmake @@ -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 $ ${bin} BYPRODUCTS ${bin} ) diff --git a/cmake/stm32f3.cmake b/cmake/stm32f3.cmake index 2d0ef50704..5410d45b43 100644 --- a/cmake/stm32f3.cmake +++ b/cmake/stm32f3.cmake @@ -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 diff --git a/cmake/stm32f4.cmake b/cmake/stm32f4.cmake index 37fd1cdad5..64fae1fe2f 100644 --- a/cmake/stm32f4.cmake +++ b/cmake/stm32f4.cmake @@ -36,6 +36,8 @@ main_sources(STM32F4_SRC target/system_stm32f4xx.c config/config_streamer_stm32f4.c + config/config_streamer_ram.c + config/config_streamer_extflash.c drivers/adc_stm32f4xx.c drivers/adc_stm32f4xx.c @@ -124,6 +126,7 @@ exclude_basenames(STM32F411_OR_F427_STDPERIPH_SRC ${STM32F411_OR_F427_STDPERIPH_ set(STM32F411_COMPILE_DEFINITIONS STM32F411xE MCU_FLASH_SIZE=512 + OPTIMIZATION -Os ) function(target_stm32f411xe name) @@ -132,9 +135,9 @@ function(target_stm32f411xe name) STARTUP startup_stm32f411xe.s SOURCES ${STM32F411_OR_F427_STDPERIPH_SRC} COMPILE_DEFINITIONS ${STM32F411_COMPILE_DEFINITIONS} - LINKER_SCRIPT stm32_flash_f411xe + LINKER_SCRIPT stm32_flash_f411xe SVD STM32F411 - ${ARGN} + ${ARGN} ) endfunction() diff --git a/cmake/stm32f7.cmake b/cmake/stm32f7.cmake index 238920bcf8..6aa2a9fd80 100644 --- a/cmake/stm32f7.cmake +++ b/cmake/stm32f7.cmake @@ -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 diff --git a/cmake/stm32h7.cmake b/cmake/stm32h7.cmake index 758f9dca25..2f232a4600 100644 --- a/cmake/stm32h7.cmake +++ b/cmake/stm32h7.cmake @@ -142,6 +142,8 @@ main_sources(STM32H7_SRC target/system_stm32h7xx.c config/config_streamer_stm32h7.c + config/config_streamer_ram.c + config/config_streamer_extflash.c drivers/adc_stm32h7xx.c drivers/bus_i2c_hal.c @@ -217,4 +219,4 @@ macro(define_target_stm32h7 subfamily size) endfunction() endmacro() -define_target_stm32h7(43 i) \ No newline at end of file +define_target_stm32h7(43 i) diff --git a/docs/Boards.md b/docs/Boards.md index 8005778cfe..28e54bbdb5 100644 --- a/docs/Boards.md +++ b/docs/Boards.md @@ -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 @@ -46,4 +46,4 @@ Boards based on STM32F1 CPUs are no longer supported by latest INAV version. Las ### Not recommended for new setups -F1 and F3 boards are no longer recommended. Users should choose a board from the supported F4 or F7 devices available in the latest release. \ No newline at end of file +F1 and F3 boards are no longer recommended. Users should choose a board from the supported F4 or F7 devices available in the latest release. diff --git a/docs/Controls.md b/docs/Controls.md index d245d92b04..bad602e472 100644 --- a/docs/Controls.md +++ b/docs/Controls.md @@ -34,10 +34,12 @@ The stick positions are combined to activate different functions: | Trim Acc Forwards | HIGH | CENTER | HIGH | CENTER | | Trim Acc Backwards | HIGH | CENTER | LOW | CENTER | | Save current waypoint mission | LOW | CENTER | HIGH | LOW | -| Load/unload current waypoint mission | LOW | CENTER | HIGH | HIGH | +| Load current waypoint mission | LOW | CENTER | HIGH | HIGH | +| Unload waypoint mission | LOW | CENTER | LOW | HIGH | | Save setting | LOW | LOW | LOW | HIGH | | Enter OSD Menu (CMS) | CENTER | LOW | HIGH | CENTER | +For graphical stick position in all transmitter modes, check out [this page](https://www.mrd-rc.com/tutorials-tools-and-testing/inav-flight/inav-stick-commands-for-all-transmitter-modes/). ![Stick Positions](assets/images/StickPositions.png) ## Yaw control diff --git a/docs/Failsafe.md b/docs/Failsafe.md index f9bbfcfde4..00ea31a730 100644 --- a/docs/Failsafe.md +++ b/docs/Failsafe.md @@ -9,12 +9,13 @@ Failsafe is a state the flight controller is meant to enter when the radio recei If the failsafe happens while the flight controller is disarmed, it only prevent arming. If it happens while armed, the failsafe policy configured in `failsafe_procedure` is engaged. The available procedures are: * __DROP:__ Just kill the motors and disarm (crash the craft). -* __SET-THR:__ Enable auto-level mode (for multirotor) or enter preconfigured roll/pitch/yaw spiral down (for airplanes) and set the throttle to a predefined value (`failsafe_throttle`) for a predefined time (`failsafe_off_delay`). This is meant to get the craft to a safe-ish landing (or more realistically, a controlled crash), it doesn't require any extra sensor other than gyros and accelerometers. -* __RTH:__ (Return To Home) One of the key features of inav, it automatically navigates the craft back to the home position and (optionally) lands it. Similarly to all other automated navigation methods, it requires GPS for any type of craft, plus compass and barometer for multicopters. +* __LAND:__ (replaces **SET-THR** from INAV V3.1.0) Performs an Emergency Landing. Enables auto-level mode (for multirotor) or enters a preconfigured roll/pitch/yaw spiral down (for airplanes). If altitude sensors are working an actively controlled descent is performed using the Emergency Landing descent speed (`nav_emerg_landing_speed`). If altitude sensors are unavailable descent is performed with the throttle set to a predefined value (`failsafe_throttle`). The descent can be limited to a predefined time (`failsafe_off_delay`) after which the craft disarms. This is meant to get the craft to a safe-ish landing (or more realistically, a controlled crash), Other than using altitude sensors for an actively controlled descent it doesn't require any extra sensors other than gyros and accelerometers. +* __SET-THR:__ (Pre INAV V3.1.0) Same as **LAND** except it doesn't use an Emergency Landing but is limited instead to just setting the throttle to a predefined value (`failsafe_throttle`) to perform a descent. It doesn't require any extra sensors other than gyros and accelerometers. +* __RTH:__ (Return To Home) One of the key features of INAV, it automatically navigates the craft back to the home position and (optionally) lands it. Similarly to all other automated navigation methods, it requires GPS for any type of craft, plus compass and barometer for multicopters. * __NONE:__ Do nothing. This is meant to let the craft perform a fully automated flight (eg. waypoint flight) outside of radio range. Highly unsafe when used with manual flight. Note that: -* Should the failsafe disarm the flight controller (**DROP**, **SET-THR** after `failsafe_off_delay` or **RTH** with `nav_disarm_on_landing` ON), the flight controller will be disarmed and re-arming will be locked until the signal from the receiver is restored for 30 seconds AND the arming switch is in the OFF position (when an arm switch is in use). +* Should the failsafe disarm the flight controller (**DROP**, **LAND/SET-THR** after `failsafe_off_delay` or **RTH** with `nav_disarm_on_landing` ON), the flight controller will be disarmed and re-arming will be locked until the signal from the receiver is restored for 30 seconds AND the arming switch is in the OFF position (when an arm switch is in use). * Prior to starting failsafe it is checked if the throttle position has been below `min_throttle` for the last `failsafe_throttle_low_delay` seconds. If it was, the craft is assumed to be on the ground and is simply disarmed. This feature can be disabled completely by setting `failsafe_throttle_low_delay` to zero, which may be necessary to do if the craft may fly long with zero throttle (eg. gliders). @@ -22,9 +23,9 @@ Note that: * If the craft is landed but armed, the failsafe may make the motors and props spin again and even make the craft take off (in case of **RTH** failsafe). Take expecially care of this when using `MOTOR_STOP` feature. **Props will spin up without warning**. Have a look at the `failsafe_throttle_low_delay` setting explained above to learn when this could happen. -* If any required navigation sensor becomes unavailable during a Return to Home (eg. loss of GPS fix), an emergency landing similar to **SET-THR** but with barometer support will be performed after a short timeout. An emergency landing would also be performed right when the failsafe is triggered if any required sensor is reported as unavailable. +* If any required navigation sensor becomes unavailable during a Return to Home (eg. loss of GPS fix), an emergency landing, as used by the **LAND** procedure, will be performed after a short timeout. An emergency landing would also be performed right when the failsafe is triggered if any required sensor is reported as unavailable. -* **SET-THR** doesn't control the descend in any other way than setting a fixed throttle. Thus, appropriate testing must be performed to find the right throttle value. Consider that a constant throttle setting will yield different thrusts depending on battery voltage, so when you evaluate the throttle value do it with a loaded battery. Failure to do so may cause a flyaway. +* The **SET-THR** procedure doesn't control descent in any way other than setting a fixed throttle. This is also the case for the **LAND** procedure when altitude sensors are unavailable. Thus, appropriate testing must be performed to find the right throttle value. Consider that a constant throttle setting will yield different thrusts depending on battery voltage, so when you evaluate the throttle value do it with a loaded battery. Failure to do so may cause a flyaway. * When the failsafe mode is aborted (RC signal restored/failsafe switch set to OFF), the current stick positions will be enforced immediately. Be ready to react quickly. @@ -48,7 +49,7 @@ Failsafe delays are configured in 0.1 second units. Distances are in centimeters #### `failsafe_procedure` -Selects the failsafe procedure. Valid procedures are **DROP**, **SET-THR**, **RTH** and **NONE**. See above for a description of each one. +Selects the failsafe procedure. Valid procedures are **DROP**, **LAND/SET-THR**, **RTH** and **NONE**. See above for a description of each one. #### `failsafe_delay` @@ -122,25 +123,29 @@ Enables landing when home position is reached. If OFF the craft will hover indef Instructs the flight controller to disarm the craft when landing is detected -### Parameters relevant to **SET-THR** failsafe procedure +### Parameters relevant to **LAND/SET-THR** failsafe procedure #### `failsafe_off_delay` Delay after failsafe activates before motors finally turn off. This is the amount of time 'failsafe_throttle' is active. If you fly at higher altitudes you may need more time to descend safely. Set to zero to keep `failsafe_throttle` active indefinitely. +#### `nav_emerg_landing_speed` + +(**LAND** only) Actively controlled descent speed when altitude sensors are available. If altitude sensors aren't available landing descent falls back to using the fixed thottle setting `failsafe_throttle` so ensure this is also set correctly. + #### `failsafe_throttle` Throttle level used for landing. Specify a value that causes the aircraft to descend at about 1M/sec. Default is set to 1000 which should correspond to throttle off. #### `failsafe_fw_roll_angle` -This parameter defines amount of roll angle (in 1/10 deg units) to execute on failsafe. Negative = LEFT +This parameter defines the amount of roll angle (in 1/10 deg units) to execute on failsafe. Negative = LEFT #### `failsafe_fw_pitch_angle` -This parameter defines amount of pitch angle (in 1/10 deg units) to execute on `SET-THR` failsafe for an airplane. Negative = CLIMB +This parameter defines the amount of pitch angle (in 1/10 deg units) to execute on failsafe for an airplane. Negative = CLIMB #### `failsafe_fw_yaw_rate` -This parameter defines amount of yaw rate (in deg per second units) to execute on `SET-THR` failsafe for an airplane. Negative = LEFT +This parameter defines the amount of yaw rate (in deg per second units) to execute on failsafe for an airplane. Negative = LEFT diff --git a/docs/Inflight Adjustments.md b/docs/Inflight Adjustments.md index fee4046954..422ba71eb5 100644 --- a/docs/Inflight Adjustments.md +++ b/docs/Inflight Adjustments.md @@ -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. -![Configurator example 1](Screenshots/adjustments-rate-profile-selection-via-3pos.png) - ---- - ![Configurator example 2](Screenshots/adjustments-pitch-and-roll-rate-adjustment-via-3pos.png) --- @@ -263,4 +250,4 @@ The following examples shows __incorrect__ configurations - the entire usable ra In the following example, the incorrect configuraton (above) has been corrected by adding a range that makes 'No changes'. -![Configurator example 7](Screenshots/adjustments-incorrect-config-2-corrected.png) \ No newline at end of file +![Configurator example 7](Screenshots/adjustments-incorrect-config-2-corrected.png) diff --git a/docs/Profiles.md b/docs/Profiles.md index 890a47a946..80d432d7d4 100644 --- a/docs/Profiles.md +++ b/docs/Profiles.md @@ -2,7 +2,7 @@ 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 @@ -10,9 +10,9 @@ 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: @@ -20,45 +20,127 @@ The CLI `profile` command can also be used: profile ``` -# Rate Profiles - -INAV supports rate profiles in addition to regular profiles. - -Rate profiles contain settings that adjust how your craft behaves based on control input. - -Three rate profiles are supported. - -Rate profiles can be selected while flying using the inflight adjustments feature. - -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 -``` - -The values contained within a rate profile can be seen by using the CLI `dump rates` command. +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 + ``` diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md index b6f2780870..91a1003693 100644 --- a/docs/Programming Framework.md +++ b/docs/Programming Framework.md @@ -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 @@ -125,8 +126,10 @@ IPF can be edited using INAV Configurator user interface, of via CLI | 30 | ACTIVE_WAYPOINT_ACTION | See ACTIVE_WAYPOINT_ACTION paragraph | | 31 | 3D HOME_DISTANCE | in `meters`, calculated from HOME_DISTANCE and ALTITUDE using Pythagorean theorem | | 32 | CROSSFIRE LQ | Crossfire Link quality as returned by the CRSF protocol | -| 33 | CROSSFIRE SNR | Crossfire SNR as returned by the CRSF protocol | -| 34 | GPS_VALID | boolean `0`/`1`. True when the GPS has a valid 3D Fix | +| 33 | CROSSFIRE SNR | Crossfire SNR as returned by the CRSF protocol | +| 34 | GPS_VALID | boolean `0`/`1`. True when the GPS has a valid 3D Fix | +| 35 | LOITER_RADIUS | The current loiter radius in cm. | +| 36 | ACTIVE_PROFILE | integer for the active config profile `[1..MAX_PROFILE_COUNT]` | #### ACTIVE_WAYPOINT_ACTION diff --git a/docs/Rangefinder.md b/docs/Rangefinder.md index 620430a6ab..66fcb91345 100644 --- a/docs/Rangefinder.md +++ b/docs/Rangefinder.md @@ -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`. \ No newline at end of file +iNav does not support `HC-SR04` and `US-100`. No constrains for I2C like `VL53L0X` or `INAV_I2C`. \ No newline at end of file diff --git a/docs/Settings.md b/docs/Settings.md index eb616a0606..d0b26b0a68 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -452,6 +452,16 @@ If the remaining battery capacity goes below this threshold the beeper will emit --- +### beeper_pwm_mode + +Allows disabling PWM mode for beeper on some targets. Switch from ON to OFF if the external beeper sound is weak. Do not switch from OFF to ON without checking if the board supports PWM beeper mode + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + ### blackbox_device Selection of where to write blackbox data @@ -562,16 +572,6 @@ ADC , VIRTUAL, NONE. The virtual current sensor, once calibrated, estimates the --- -### d_boost_factor - -_// TODO_ - -| Default | Min | Max | -| --- | --- | --- | -| 1.25 | 1 | 3 | - ---- - ### d_boost_gyro_delta_lpf_hz _// TODO_ @@ -582,6 +582,16 @@ _// TODO_ --- +### d_boost_max + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| 1.25 | 1 | 3 | + +--- + ### d_boost_max_at_acceleration _// TODO_ @@ -592,6 +602,16 @@ _// TODO_ --- +### d_boost_min + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| 0.5 | 0 | 1 | + +--- + ### deadband These are values (in us) by how much RC input can be different before it's considered valid. For transmitters with jitter on outputs, this value can be increased. Defaults are zero, but can be increased up to 10 or so if rc inputs twitch while idle. @@ -734,11 +754,11 @@ Cutoff frequency for stage 2 D-term low pass filter ### dterm_lpf2_type -Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. +Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`, `PT2`, `PT3`. | Default | Min | Max | | --- | --- | --- | -| BIQUAD | | | +| PT1 | | | --- @@ -748,17 +768,17 @@ Dterm low pass filter cutoff frequency. Default setting is very conservative and | Default | Min | Max | | --- | --- | --- | -| 40 | 0 | 500 | +| 110 | 0 | 500 | --- ### dterm_lpf_type -Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. +Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`, `PT2`, `PT3`. | Default | Min | Max | | --- | --- | --- | -| BIQUAD | | | +| PT2 | | | --- @@ -768,17 +788,17 @@ Enable/disable dynamic gyro notch also known as Matrix Filter | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| ON | | | --- ### dynamic_gyro_notch_min_hz -Minimum frequency for dynamic notches. Default value of `150` works best with 5" multirors. Should be lowered with increased size of propellers. Values around `100` work fine on 7" drones. 10" can go down to `60` - `70` +Minimum frequency for dynamic notches. Default value of `150` works best with 5" multirotors. Should be lowered with increased size of propellers. Values around `100` work fine on 7" drones. 10" can go down to `60` - `70` | Default | Min | Max | | --- | --- | --- | -| 150 | 30 | 1000 | +| 50 | 30 | 1000 | --- @@ -792,16 +812,6 @@ Q factor for dynamic notches --- -### dynamic_gyro_notch_range - -Range for dynamic gyro notches. `MEDIUM` for 5", `HIGH` for 3" and `MEDIUM`/`LOW` for 7" and bigger propellers - -| Default | Min | Max | -| --- | --- | --- | -| MEDIUM | | | - ---- - ### eleres_freq _// TODO_ @@ -894,7 +904,7 @@ Time in deciseconds to wait before activating failsafe when signal is lost. See ### failsafe_fw_pitch_angle -Amount of dive/climb when `SET-THR` failsafe is active on a fixed-wing machine. In 1/10 deg (deci-degrees). Negative values = climb +Amount of dive/climb when `LAND` (or old `SET-THR`) failsafe is active on a fixed-wing machine. In 1/10 deg (deci-degrees). Negative values = climb | Default | Min | Max | | --- | --- | --- | @@ -904,7 +914,7 @@ Amount of dive/climb when `SET-THR` failsafe is active on a fixed-wing machine. ### failsafe_fw_roll_angle -Amount of banking when `SET-THR` failsafe is active on a fixed-wing machine. In 1/10 deg (deci-degrees). Negative values = left roll +Amount of banking when `LAND` (or old `SET-THR`) failsafe is active on a fixed-wing machine. In 1/10 deg (deci-degrees). Negative values = left roll | Default | Min | Max | | --- | --- | --- | @@ -914,7 +924,7 @@ Amount of banking when `SET-THR` failsafe is active on a fixed-wing machine. In ### failsafe_fw_yaw_rate -Requested yaw rate to execute when `SET-THR` failsafe is active on a fixed-wing machine. In deg/s. Negative values = left turn +Requested yaw rate to execute when `LAND` (or old `SET-THR`) failsafe is active on a fixed-wing machine. In deg/s. Negative values = left turn | Default | Min | Max | | --- | --- | --- | @@ -998,7 +1008,7 @@ What failsafe procedure to initiate in Stage 2. See [Failsafe documentation](Fai | Default | Min | Max | | --- | --- | --- | -| SET-THR | | | +| LAND | | | --- @@ -1158,13 +1168,13 @@ The target percentage of maximum mixer output used for determining the rates in | Default | Min | Max | | --- | --- | --- | -| 90 | 50 | 100 | +| 80 | 50 | 100 | --- ### fw_autotune_min_stick -Minimum stick input [%] to consider overshoot/undershoot detection +Minimum stick input [%], after applying deadband and expo, to start recording the plane's response to stick input. | Default | Min | Max | | --- | --- | --- | @@ -1522,36 +1532,6 @@ Enable use of Galileo satellites. This is at the expense of other regional const --- -### gyro_abg_alpha - -Alpha factor for Gyro Alpha-Beta-Gamma filter - -| Default | Min | Max | -| --- | --- | --- | -| 0 | 0 | 1 | - ---- - -### gyro_abg_boost - -Boost factor for Gyro Alpha-Beta-Gamma filter - -| Default | Min | Max | -| --- | --- | --- | -| 0.35 | 0 | 2 | - ---- - -### gyro_abg_half_life - -Sample half-life for Gyro Alpha-Beta-Gamma filter - -| Default | Min | Max | -| --- | --- | --- | -| 0.5 | 0 | 10 | - ---- - ### gyro_anti_aliasing_lpf_hz Gyro processing anti-aliasing filter cutoff frequency. In normal operation this filter setting should never be changed. In Hz @@ -1658,7 +1638,7 @@ _// TODO_ | Default | Min | Max | | --- | --- | --- | -| 0 | 0 | 1 | +| 0 | 0 | 2 | --- @@ -1928,7 +1908,7 @@ Inertial Measurement Unit KP Gain for accelerometer measurements | Default | Min | Max | | --- | --- | --- | -| 2500 | | 65535 | +| 1000 | | 65535 | --- @@ -1938,7 +1918,7 @@ Inertial Measurement Unit KP Gain for compass measurements | Default | Min | Max | | --- | --- | --- | -| 10000 | | 65535 | +| 5000 | | 65535 | --- @@ -2458,7 +2438,7 @@ Exposition value used for the PITCH/ROLL axes by the `MANUAL` flight mode [0-100 | Default | Min | Max | | --- | --- | --- | -| 70 | 0 | 100 | +| 35 | 0 | 100 | --- @@ -2832,26 +2812,6 @@ When powering up, gyro bias is calculated. If the model is shaking/moving during --- -### motor_accel_time - -Minimum time for the motor(s) to accelerate from 0 to 100% throttle (ms) [0-1000] - -| Default | Min | Max | -| --- | --- | --- | -| 0 | 0 | 1000 | - ---- - -### motor_decel_time - -Minimum time for the motor(s) to deccelerate from 100 to 0% throttle (ms) [0-1000] - -| Default | Min | Max | -| --- | --- | --- | -| 0 | 0 | 1000 | - ---- - ### motor_direction_inverted Use if you need to inverse yaw motor direction. @@ -2924,7 +2884,7 @@ Maximum climb/descent rate that UAV is allowed to reach during navigation modes. ### nav_auto_speed -Maximum velocity firmware is allowed in full auto modes (RTH, WP) [cm/s] [Multirotor only] +Speed in fully autonomous modes (RTH, WP) [cm/s]. Used for WP mode when no specific WP speed set. [Multirotor only] | Default | Min | Max | | --- | --- | --- | @@ -3424,7 +3384,7 @@ Maximum climb/descent rate firmware is allowed when processing pilot input for A ### nav_manual_speed -Maximum velocity firmware is allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only] +Maximum speed allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only] | Default | Min | Max | | --- | --- | --- | @@ -3442,6 +3402,16 @@ Max allowed altitude (above Home Point) that applies to all NAV modes (including --- +### nav_max_auto_speed + +Maximum speed allowed in fully autonomous modes (RTH, WP) [cm/s] [Multirotor only] + +| Default | Min | Max | +| --- | --- | --- | +| 1000 | 10 | 2000 | + +--- + ### nav_max_terrain_follow_alt Max allowed above the ground altitude for terrain following mode @@ -4132,6 +4102,16 @@ Value above which to make the OSD distance from home indicator blink (meters) --- +### osd_esc_rpm_precision + +Number of characters used to display the RPM value. + +| Default | Min | Max | +| --- | --- | --- | +| 3 | 3 | 6 | + +--- + ### osd_esc_temp_alarm_max Temperature above which the IMU temperature OSD element will start blinking (decidegrees centigrade) @@ -4552,6 +4532,16 @@ Display minimum voltage of the `BATTERY` or the average per `CELL` in the OSD st --- +### osd_stats_page_auto_swap_time + +Auto swap display time interval between disarm stats pages (seconds). Reverts to manual control when Roll stick used to change pages. Disabled when set to 0. + +| Default | Min | Max | +| --- | --- | --- | +| 3 | 0 | 10 | + +--- + ### osd_telemetry To enable OSD telemetry for antenna tracker. Possible values are `OFF`, `ON` and `TEST` @@ -4782,6 +4772,66 @@ Limits acceleration of YAW rotation speed that can be requested by stick input. --- +### rate_dynamics_center_correction + +The center stick correction for Rate Dynamics + +| Default | Min | Max | +| --- | --- | --- | +| 10 | 10 | 95 | + +--- + +### rate_dynamics_center_sensitivity + +The center stick sensitivity for Rate Dynamics + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 25 | 175 | + +--- + +### rate_dynamics_center_weight + +The center stick weight for Rate Dynamics + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 95 | + +--- + +### rate_dynamics_end_correction + +The end stick correction for Rate Dynamics + +| Default | Min | Max | +| --- | --- | --- | +| 10 | 10 | 95 | + +--- + +### rate_dynamics_end_sensitivity + +The end stick sensitivity for Rate Dynamics + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 25 | 175 | + +--- + +### rate_dynamics_end_weight + +The end stick weight for Rate Dynamics + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 95 | + +--- + ### rc_expo Exposition value used for the PITCH/ROLL axes by all the stabilized flights modes (all but `MANUAL`) @@ -5128,7 +5178,7 @@ Enable Kalman filter on the gyro data | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| ON | | | --- @@ -5654,7 +5704,7 @@ These are values (in us) by how much RC input can be different before it's consi ### yaw_lpf_hz -Yaw low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below) +Yaw P term low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below) | Default | Min | Max | | --- | --- | --- | diff --git a/docs/Telemetry.md b/docs/Telemetry.md index 2b5972370e..346422e6e4 100644 --- a/docs/Telemetry.md +++ b/docs/Telemetry.md @@ -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. diff --git a/docs/assets/betafpvf722.png b/docs/assets/betafpvf722.png new file mode 100644 index 0000000000..626deacaa9 Binary files /dev/null and b/docs/assets/betafpvf722.png differ diff --git a/docs/assets/images/StickPositions.png b/docs/assets/images/StickPositions.png index 98281caeef..4f7365952d 100644 Binary files a/docs/assets/images/StickPositions.png and b/docs/assets/images/StickPositions.png differ diff --git a/docs/assets/images/StickPositions.svg b/docs/assets/images/StickPositions.svg index 49750eb870..3f32b52523 100644 --- a/docs/assets/images/StickPositions.svg +++ b/docs/assets/images/StickPositions.svg @@ -2,23 +2,23 @@ + inkscape:export-ydpi="74.996788" + xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape" + xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd" + xmlns="http://www.w3.org/2000/svg" + xmlns:svg="http://www.w3.org/2000/svg" + xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#" + xmlns:cc="http://creativecommons.org/ns#" + xmlns:dc="http://purl.org/dc/elements/1.1/"> + inkscape:object-nodes="true" + inkscape:pagecheckerboard="0" /> @@ -58,7 +59,7 @@ image/svg+xml - + @@ -160,7 +161,7 @@ x="533.55804" style="font-style:normal;font-weight:normal;line-height:0%;font-family:sans-serif;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" xml:space="preserve">Profile 2 + style="font-size:286.599px;line-height:1.25;text-align:end;text-anchor:end">Profile 2 Calibrate Gyro + style="font-size:286.599px;line-height:1.25;text-align:end;text-anchor:end">Calibrate Gyro Calibrate Acc + style="font-size:286.599px;line-height:1.25;text-align:end;text-anchor:end">Calibrate Acc Trim Acc Left + style="font-size:286.599px;line-height:1.25;text-align:end;text-anchor:end">Trim Acc Left Trim Acc Right + style="font-size:286.599px;line-height:1.25;text-align:end;text-anchor:end">Trim Acc Right Trim Acc Backwards + style="font-size:286.599px;line-height:1.25;text-align:end;text-anchor:end">Trim Acc Backwards + + Load waypoint mission + Unload waypoint mission + style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.0566;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" /> + + Save Setting + style="font-size:286.599px;line-height:1.25;text-align:end;text-anchor:end">Save Setting Enter OSD Menu (CMS) + style="font-size:286.599px;line-height:1.25;text-align:end;text-anchor:end">Enter OSD Menu (CMS) Mode 2 Stick Functions + style="font-size:352.858px;line-height:1.25;text-align:center;text-anchor:middle">Mode 2 Stick Functions Battery profile 1 + style="font-size:286.599px;line-height:1.25;text-align:end;text-anchor:end">Battery profile 1 Battery profile 3 + style="font-size:286.599px;line-height:1.25;text-align:end;text-anchor:end">Battery profile 3 I2C and UART3 can not be used at the same time! Connect the serial receiver to a different serial port when I2C device like MAG is used. + +![BetaFPVF722 I2C](../assets/betafpvf722.png) diff --git a/docs/Board - DALRCF405.md b/docs/boards/DALRCF405.md similarity index 100% rename from docs/Board - DALRCF405.md rename to docs/boards/DALRCF405.md diff --git a/docs/Board - DALRCF722DUAL.md b/docs/boards/DALRCF722DUAL.md similarity index 100% rename from docs/Board - DALRCF722DUAL.md rename to docs/boards/DALRCF722DUAL.md diff --git a/docs/Board - FLYWOOF411.md b/docs/boards/FLYWOOF411.md old mode 100755 new mode 100644 similarity index 100% rename from docs/Board - FLYWOOF411.md rename to docs/boards/FLYWOOF411.md diff --git a/docs/Board - FOXEERF405.md b/docs/boards/FOXEERF405.md similarity index 100% rename from docs/Board - FOXEERF405.md rename to docs/boards/FOXEERF405.md diff --git a/docs/Board - FOXEERF722DUAL.md b/docs/boards/FOXEERF722DUAL.md similarity index 100% rename from docs/Board - FOXEERF722DUAL.md rename to docs/boards/FOXEERF722DUAL.md diff --git a/docs/Board - FrSky Pilot.md b/docs/boards/FrSky Pilot.md similarity index 100% rename from docs/Board - FrSky Pilot.md rename to docs/boards/FrSky Pilot.md diff --git a/docs/Board - IFLIGHTF7_TWING.md b/docs/boards/IFLIGHTF7_TWING.md similarity index 100% rename from docs/Board - IFLIGHTF7_TWING.md rename to docs/boards/IFLIGHTF7_TWING.md diff --git a/docs/Board - KroozX.md b/docs/boards/KroozX.md old mode 100755 new mode 100644 similarity index 100% rename from docs/Board - KroozX.md rename to docs/boards/KroozX.md diff --git a/docs/Board - Matek F411 Wing.md b/docs/boards/Matek F411 Wing.md similarity index 100% rename from docs/Board - Matek F411 Wing.md rename to docs/boards/Matek F411 Wing.md diff --git a/docs/Board - Matek F411-WSE.md b/docs/boards/Matek F411-WSE.md similarity index 100% rename from docs/Board - Matek F411-WSE.md rename to docs/boards/Matek F411-WSE.md diff --git a/docs/Board - Matek F722-STD.md b/docs/boards/Matek F722-STD.md similarity index 100% rename from docs/Board - Matek F722-STD.md rename to docs/boards/Matek F722-STD.md diff --git a/docs/Board - MatekF405 Wing.md b/docs/boards/MatekF405 Wing.md similarity index 78% rename from docs/Board - MatekF405 Wing.md rename to docs/boards/MatekF405 Wing.md index f7f1c3aa73..5a92ce0d89 100644 --- a/docs/Board - MatekF405 Wing.md +++ b/docs/boards/MatekF405 Wing.md @@ -1,7 +1,5 @@ # Board - [MATEKSYS F405-Wing](https://inavflight.com/shop/p/MATEKF405WING) -![Matek F405 Wing](https://quadmeup.com/wp-content/uploads/2018/12/DSC_0007.jpg) - ## Specification: * STM32F405 CPU @@ -22,4 +20,4 @@ ## Where to buy: -* [Banggood](https://inavflight.com/shop/p/MATEKF405WING) \ No newline at end of file +* [Banggood](https://inavflight.com/shop/p/MATEKF405WING) diff --git a/docs/Board - MatekF405.md b/docs/boards/MatekF405.md similarity index 100% rename from docs/Board - MatekF405.md rename to docs/boards/MatekF405.md diff --git a/docs/Board - MatekF722-SE.md b/docs/boards/MatekF722-SE.md similarity index 100% rename from docs/Board - MatekF722-SE.md rename to docs/boards/MatekF722-SE.md diff --git a/docs/Board - MatekF722.md b/docs/boards/MatekF722.md similarity index 100% rename from docs/Board - MatekF722.md rename to docs/boards/MatekF722.md diff --git a/docs/Board - MatekF765-Wing.md b/docs/boards/MatekF765-Wing.md similarity index 100% rename from docs/Board - MatekF765-Wing.md rename to docs/boards/MatekF765-Wing.md diff --git a/docs/Board - NOX.md b/docs/boards/NOX.md old mode 100755 new mode 100644 similarity index 100% rename from docs/Board - NOX.md rename to docs/boards/NOX.md diff --git a/docs/Board - Omnibus F4.md b/docs/boards/Omnibus F4.md similarity index 92% rename from docs/Board - Omnibus F4.md rename to docs/boards/Omnibus F4.md index d4e09dd5b8..67e9a23c59 100644 --- a/docs/Board - Omnibus F4.md +++ b/docs/boards/Omnibus F4.md @@ -155,14 +155,14 @@ This board allows for single **SoftwareSerial** port on small soldering pads loc Please note that this is *not* the motor PWM5/PWM6 pins, but small surface mount pads CH5/CH6. ### Omnibus F4 Pro SoftwareSerial Connections -![Omnibus F4 Pro SoftwareSerial Connections](assets/images/omnibusf4pro_ss.jpg) +![Omnibus F4 Pro SoftwareSerial Connections](../assets/images/omnibusf4pro_ss.jpg) | Pad | SoftwareSerial Role | | ---- | ---- | | CH5 | RX | | CH6 | TX | -![Omnibus F4 Pro SmartPort using SoftwareSerial](assets/images/omnibusf4pro_ss.png) +![Omnibus F4 Pro SmartPort using SoftwareSerial](../assets/images/omnibusf4pro_ss.png) ### Omnibus F4 v3/v4/v5 SoftwareSerial Connections @@ -181,22 +181,22 @@ Following diagrams applies to _Pro_ version with integrated current meter and JS ## Board layout -![Omnibus F4 Pro Board Layout](assets/images/omnibusf4pro.png) +![Omnibus F4 Pro Board Layout](../assets/images/omnibusf4pro.png) ## Flying wing motor and servos -![Omnibus F4 Pro Flying Wing Setup](assets/images/omnibusf4pro_flyingwing_setup.png) +![Omnibus F4 Pro Flying Wing Setup](../assets/images/omnibusf4pro_flyingwing_setup.png) ## RX setup -![Omnibus F4 Pro RX Setup](assets/images/omnibusf4pro_rx.png) +![Omnibus F4 Pro RX Setup](../assets/images/omnibusf4pro_rx.png) ## FPV setup -![Omnibus F4 Pro FPV Setup](assets/images/omnibusf4pro_fpv_setup.png) +![Omnibus F4 Pro FPV Setup](../assets/images/omnibusf4pro_fpv_setup.png) ## GPS setup -![Omnibus F4 Pro GPS Setup](assets/images/omnibusf4pro_gps_setup.png) +![Omnibus F4 Pro GPS Setup](../assets/images/omnibusf4pro_gps_setup.png) _Diagrams created by Albert Kravcov (skaman82)_ diff --git a/docs/Board - Omnibus F7.md b/docs/boards/Omnibus F7.md similarity index 100% rename from docs/Board - Omnibus F7.md rename to docs/boards/Omnibus F7.md diff --git a/docs/Board - PixRacer R14.md b/docs/boards/PixRacer R14.md similarity index 100% rename from docs/Board - PixRacer R14.md rename to docs/boards/PixRacer R14.md diff --git a/docs/Board - Revolution.md b/docs/boards/Revolution.md similarity index 100% rename from docs/Board - Revolution.md rename to docs/boards/Revolution.md diff --git a/docs/Board - WingFC.md b/docs/boards/WingFC.md similarity index 100% rename from docs/Board - WingFC.md rename to docs/boards/WingFC.md diff --git a/docs/Board - YuPiF4.md b/docs/boards/YuPiF4.md similarity index 100% rename from docs/Board - YuPiF4.md rename to docs/boards/YuPiF4.md diff --git a/docs/development/Atomic Barrier.md b/docs/development/Atomic Barrier.md deleted file mode 100644 index d87275b54f..0000000000 --- a/docs/development/Atomic Barrier.md +++ /dev/null @@ -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. diff --git a/docs/development/Building in Linux.md b/docs/development/Building in Linux.md index 73287a216b..765286d012 100644 --- a/docs/development/Building in Linux.md +++ b/docs/development/Building in Linux.md @@ -65,7 +65,7 @@ The `git clone` creates an `inav` directory; we can enter this directory, config ## Build tooling -For 2.6 and later, inav uses `cmake` as its primary build tool. `cmake` simplies various platform and hardware dependencies required to cross compile multiple targets. `cmake` still uses GNU `make` to invoke the actual compiler. It is necessary to configure the build enviroment with `cmake` before we can build any firmware. +For 2.6 and later, inav uses `cmake` as its primary build tool. `cmake` simplifies various platform and hardware dependencies required to cross compile multiple targets. `cmake` still uses GNU `make` to invoke the actual compiler. It is necessary to configure the build environment with `cmake` before we can build any firmware. ## Using `cmake` @@ -96,14 +96,14 @@ Once `cmake` has generated the `build/Makefile`, this `Makfile` (with `make`) is The generated `Makefile` uses different a target selection mechanism from the older (pre 2.6) top level `Makefile`; you can generate a list of targets with `make help` (or, as the list is extremely long), pipe this into a pager, e.g. `make help | less`. -Typically, to build a single target, just pass the target name to `make`; note that unlike eariler releases, `make` without a target specified will build **all** targets. +Typically, to build a single target, just pass the target name to `make`; note that unlike earlier releases, `make` without a target specified will build **all** targets. ``` # Build the MATEKF405 firmware make MATEKF405 ``` -One can also build multiple targets from a sinlge `make` command: +One can also build multiple targets from a single `make` command: ``` make MATEKF405 MATEKF722 @@ -128,7 +128,7 @@ make clean_MATEKF405 make clean_MATEKF405 clean_MATEKF722 ``` -### `cmake` cache maintainance +### `cmake` cache maintenance `cmake` caches the build environment, so you don't need to rerun `cmake` each time you build a target. Two `make` options are provided to maintain the `cmake` cache. @@ -141,7 +141,7 @@ It is unlikely that the typical user will need to employ these options, other th In order to update your local firmware build: -* Navigate to the local iNav repository +* Navigate to the local inav repository * Use the following steps to pull the latest changes and rebuild your local version of inav firmware from the `build` directory: ``` @@ -154,3 +154,9 @@ $ make ## Advanced Usage For more advanced development information and `git` usage, please refer to the [development guide](https://github.com/iNavFlight/inav/blob/master/docs/development/Development.md). + +## Unsupported platforms + +If you're using a host platform for which Arm does not supply a cross-compiler (Arm32, IA32), and the distro either does not package a suitable compiler or it's too old, then you can usually find a suitable compiler in the [xpack devtools collection](https://github.com/xpack-dev-tools/arm-none-eabi-gcc-xpack). + +You will need to configure `cmake` to use the external compiler. diff --git a/docs/development/Building in Windows 10 with Linux Subsystem.md b/docs/development/Building in Windows 10 or 11 with Linux Subsystem.md similarity index 69% rename from docs/development/Building in Windows 10 with Linux Subsystem.md rename to docs/development/Building in Windows 10 or 11 with Linux Subsystem.md index 2b8439c543..0b12a48c12 100644 --- a/docs/development/Building in Windows 10 with Linux Subsystem.md +++ b/docs/development/Building in Windows 10 or 11 with Linux Subsystem.md @@ -20,13 +20,13 @@ Install Ubuntu: NOTE: from this point all commands are entered into the Ubunto shell command window Update the repo packages: -1. `sudo apt update` +- `sudo apt update` Install Git, Make, gcc and Ruby -1. `sudo apt-get install git` -1. `sudo apt-get install make` -1. `sudo apt-get install cmake` -1. `sudo apt-get install ruby` +- `sudo apt-get install git make cmake ruby` + +Install python and python-yaml to allow updates to settings.md +- `sudo apt-get install python3 python-yaml` ### CMAKE and Ubuntu 18_04 @@ -78,6 +78,12 @@ cd build make MATEKF722 ``` +## Updating the documents +``` +cd /mnt/c/inav +python3 src/utils/update_cli_docs.py +``` + ## Flashing: Launch windows configurator GUI and from within the firmware flasher select `Load firmware[Local]` Hex files can be found in the folder `c:\inav\build` @@ -132,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. diff --git a/docs/development/Building in Windows 10 with MSYS2.md b/docs/development/Building in Windows 10 or 11 with MSYS2.md similarity index 100% rename from docs/development/Building in Windows 10 with MSYS2.md rename to docs/development/Building in Windows 10 or 11 with MSYS2.md diff --git a/docs/development/IDE - Visual Studio Code with Windows 10.md b/docs/development/IDE - Visual Studio Code with Windows 10.md index 6095fec4e3..ee83ce74af 100644 --- a/docs/development/IDE - Visual Studio Code with Windows 10.md +++ b/docs/development/IDE - Visual Studio Code with Windows 10.md @@ -79,30 +79,6 @@ Edit `./.vscode/tasks.json` to enable Building with `Ctrl + Shift + B` keyboard // for the documentation about the tasks.json format "version": "2.0.0", "tasks": [ - { - "label": "Build Matek F722-SE", - "type": "shell", - "command": "make MATEKF722SE", - "group": "build", - "problemMatcher": [], - "options": { - "cwd": "${workspaceFolder}/build" - } - }, - { - "label": "Build Matek F722", - "type": "shell", - "command": "make MATEKF722", - "group": { - "kind": "build", - "isDefault": true - }, - "problemMatcher": [], - "options": { - "cwd": "${workspaceFolder}/build" - } - } - , { "label": "Install/Update CMAKE", "type": "shell", @@ -112,6 +88,37 @@ Edit `./.vscode/tasks.json` to enable Building with `Ctrl + Shift + B` keyboard "options": { "cwd": "${workspaceFolder}" } + }, + { + "label": "Compile autogenerated docs", + "type": "shell", + "command": "python3 src/utils/update_cli_docs.py", + "problemMatcher": [], + "options": { + "cwd": "${workspaceFolder}" + } + }, + // Example of building a single target + { + "label": "Build Matek F722-WPX", + "type": "shell", + "command": "make MATEKF722WPX", + "group": "build", + "problemMatcher": [], + "options": { + "cwd": "${workspaceFolder}/build" + } + }, + // Example of building multiple targets + { + "label": "Build Matek F405-STD & WING", + "type": "shell", + "command": "make MATEKF405 MATEKF405SE", + "group": "build", + "problemMatcher": [], + "options": { + "cwd": "${workspaceFolder}/build" + } } ] } diff --git a/lib/main/STM32H7/Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_ll_exti.c b/lib/main/STM32H7/Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_ll_exti.c index 3d1d792425..d30bc514b9 100644 --- a/lib/main/STM32H7/Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_ll_exti.c +++ b/lib/main/STM32H7/Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_ll_exti.c @@ -20,11 +20,6 @@ /* Includes ------------------------------------------------------------------*/ #include "stm32h7xx_ll_exti.h" -#ifdef USE_FULL_ASSERT -#include "stm32_assert.h" -#else -#define assert_param(expr) ((void)0U) -#endif /** @addtogroup STM32H7xx_LL_Driver * @{ diff --git a/lib/main/STM32H7/Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_ll_i2c.c b/lib/main/STM32H7/Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_ll_i2c.c index a1eac83a9c..ed7104bbeb 100644 --- a/lib/main/STM32H7/Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_ll_i2c.c +++ b/lib/main/STM32H7/Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_ll_i2c.c @@ -21,11 +21,6 @@ /* Includes ------------------------------------------------------------------*/ #include "stm32h7xx_ll_i2c.h" #include "stm32h7xx_ll_bus.h" -#ifdef USE_FULL_ASSERT -#include "stm32_assert.h" -#else -#define assert_param(expr) ((void)0U) -#endif /** @addtogroup STM32H7xx_LL_Driver * @{ diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 00968141ee..47edcaeee4 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -71,26 +71,23 @@ main_sources(COMMON_SRC drivers/accgyro/accgyro.c drivers/accgyro/accgyro.h - drivers/accgyro/accgyro_adxl345.c - drivers/accgyro/accgyro_adxl345.h - drivers/accgyro/accgyro_bma280.c - drivers/accgyro/accgyro_bma280.h drivers/accgyro/accgyro_bmi088.c drivers/accgyro/accgyro_bmi088.h drivers/accgyro/accgyro_bmi160.c drivers/accgyro/accgyro_bmi160.h + drivers/accgyro/accgyro_bmi270.c + drivers/accgyro/accgyro_bmi270.h + drivers/accgyro/accgyro_bmi270_maximum_fifo.c drivers/accgyro/accgyro_fake.c drivers/accgyro/accgyro_fake.h drivers/accgyro/accgyro_icm20689.c drivers/accgyro/accgyro_icm20689.h - drivers/accgyro/accgyro_l3g4200d.c - drivers/accgyro/accgyro_l3g4200d.h + drivers/accgyro/accgyro_icm42605.c + drivers/accgyro/accgyro_icm42605.h drivers/accgyro/accgyro_l3gd20.c drivers/accgyro/accgyro_l3gd20.h drivers/accgyro/accgyro_lsm303dlhc.c drivers/accgyro/accgyro_lsm303dlhc.h - drivers/accgyro/accgyro_mma845x.c - drivers/accgyro/accgyro_mma845x.h drivers/accgyro/accgyro_mpu.c drivers/accgyro/accgyro_mpu.h drivers/accgyro/accgyro_mpu3050.c @@ -162,6 +159,10 @@ main_sources(COMMON_SRC drivers/compass/compass_qmc5883l.h drivers/compass/compass_rm3100.c drivers/compass/compass_rm3100.h + drivers/compass/compass_vcm5883.c + drivers/compass/compass_vcm5883.h + drivers/compass/compass_mlx90393.c + drivers/compass/compass_mlx90393.h drivers/compass/compass_msp.c drivers/compass/compass_msp.h @@ -228,10 +229,6 @@ main_sources(COMMON_SRC drivers/pinio.c drivers/pinio.h - drivers/rangefinder/rangefinder_hcsr04.c - drivers/rangefinder/rangefinder_hcsr04.h - drivers/rangefinder/rangefinder_hcsr04_i2c.c - drivers/rangefinder/rangefinder_hcsr04_i2c.h drivers/rangefinder/rangefinder_srf10.c drivers/rangefinder/rangefinder_srf10.h drivers/rangefinder/rangefinder_vl53l0x.c @@ -242,13 +239,13 @@ main_sources(COMMON_SRC drivers/rangefinder/rangefinder_virtual.h drivers/rangefinder/rangefinder_us42.c drivers/rangefinder/rangefinder_us42.h + drivers/rangefinder/rangefinder_tof10120_i2c.c + drivers/rangefinder/rangefinder_tof10120_i2c.h drivers/resource.c drivers/resource.h drivers/rcc.c drivers/rcc.h - drivers/rx_pwm.c - drivers/rx_pwm.h drivers/rx_spi.c drivers/rx_spi.h drivers/serial.c @@ -323,6 +320,8 @@ main_sources(COMMON_SRC flight/kalman.h flight/smith_predictor.c flight/smith_predictor.h + flight/rate_dynamics.c + flight/rate_dynamics.h flight/mixer.c flight/mixer.h flight/pid.c @@ -349,8 +348,6 @@ main_sources(COMMON_SRC io/beeper.c io/beeper.h - io/esc_serialshot.c - io/esc_serialshot.h io/servo_sbus.c io/servo_sbus.h io/frsky_osd.c @@ -411,8 +408,6 @@ main_sources(COMMON_SRC rx/msp.h rx/msp_override.c rx/msp_override.h - rx/pwm.c - rx/pwm.h rx/frsky_crc.c rx/frsky_crc.h rx/rx.c @@ -581,6 +576,8 @@ main_sources(COMMON_SRC telemetry/ghst.h telemetry/hott.c telemetry/hott.h + telemetry/jetiexbus.c + telemetry/jetiexbus.h telemetry/ibus_shared.c telemetry/ibus_shared.h telemetry/ibus.c diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 77225d905f..68f22eadef 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.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 @@ -634,14 +654,14 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition) case FLIGHT_LOG_FIELD_CONDITION_FIXED_WING_NAV: #ifdef USE_NAV - return STATE(FIXED_WING_LEGACY); + return STATE(FIXED_WING_LEGACY) && blackboxIncludeFlag(BLACKBOX_FEATURE_NAV_PID); #else return false; #endif case FLIGHT_LOG_FIELD_CONDITION_MC_NAV: #ifdef USE_NAV - return !STATE(FIXED_WING_LEGACY); + return !STATE(FIXED_WING_LEGACY) && blackboxIncludeFlag(BLACKBOX_FEATURE_NAV_PID); #else return false; #endif @@ -657,9 +677,28 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition) case FLIGHT_LOG_FIELD_CONDITION_DEBUG: return debugMode != DEBUG_NONE; + case FLIGHT_LOG_FIELD_CONDITION_NAV_ACC: + return blackboxIncludeFlag(BLACKBOX_FEATURE_NAV_ACC); + + case FLIGHT_LOG_FIELD_CONDITION_NAV_POS: + return blackboxIncludeFlag(BLACKBOX_FEATURE_NAV_POS); + + case FLIGHT_LOG_FIELD_CONDITION_ACC: + return blackboxIncludeFlag(BLACKBOX_FEATURE_ACC); + + case FLIGHT_LOG_FIELD_CONDITION_ATTITUDE: + return blackboxIncludeFlag(BLACKBOX_FEATURE_ATTITUDE); + + case FLIGHT_LOG_FIELD_CONDITION_RC_COMMAND: + return blackboxIncludeFlag(BLACKBOX_FEATURE_RC_COMMAND); + + case FLIGHT_LOG_FIELD_CONDITION_RC_DATA: + return blackboxIncludeFlag(BLACKBOX_FEATURE_RC_DATA); + case FLIGHT_LOG_FIELD_CONDITION_NEVER: return false; + default: return false; } @@ -733,6 +772,7 @@ static void writeIntraframe(void) blackboxWriteSignedVB(blackboxCurrent->axisPID_D[x]); } } + blackboxWriteSignedVBArray(blackboxCurrent->axisPID_F, XYZ_AXIS_COUNT); if (testBlackboxCondition(CONDITION(FIXED_WING_NAV))) { blackboxWriteSignedVBArray(blackboxCurrent->fwAltPID, 3); @@ -756,16 +796,20 @@ static void writeIntraframe(void) } // Write raw stick positions - blackboxWriteSigned16VBArray(blackboxCurrent->rcData, 4); + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_RC_DATA)) { + blackboxWriteSigned16VBArray(blackboxCurrent->rcData, 4); + } // Write roll, pitch and yaw first: - blackboxWriteSigned16VBArray(blackboxCurrent->rcCommand, 3); + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_RC_COMMAND)) { + blackboxWriteSigned16VBArray(blackboxCurrent->rcCommand, 3); - /* - * Write the throttle separately from the rest of the RC data so we can apply a predictor to it. - * Throttle lies in range [minthrottle..maxthrottle]: - */ - blackboxWriteUnsignedVB(blackboxCurrent->rcCommand[THROTTLE] - getThrottleIdleValue()); + /* + * Write the throttle separately from the rest of the RC data so we can apply a predictor to it. + * Throttle lies in range [minthrottle..maxthrottle]: + */ + blackboxWriteUnsignedVB(blackboxCurrent->rcCommand[THROTTLE] - getThrottleIdleValue()); + } if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) { /* @@ -811,20 +855,28 @@ static void writeIntraframe(void) } blackboxWriteSigned16VBArray(blackboxCurrent->gyroADC, XYZ_AXIS_COUNT); - blackboxWriteSigned16VBArray(blackboxCurrent->accADC, XYZ_AXIS_COUNT); - blackboxWriteSigned16VBArray(blackboxCurrent->attitude, XYZ_AXIS_COUNT); + + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ACC)) { + blackboxWriteSigned16VBArray(blackboxCurrent->accADC, XYZ_AXIS_COUNT); + } + + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ATTITUDE)) { + blackboxWriteSigned16VBArray(blackboxCurrent->attitude, XYZ_AXIS_COUNT); + } if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_DEBUG)) { blackboxWriteSignedVBArray(blackboxCurrent->debug, DEBUG32_VALUE_COUNT); } - //Motors can be below minthrottle when disarmed, but that doesn't happen much - blackboxWriteUnsignedVB(blackboxCurrent->motor[0] - getThrottleIdleValue()); + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_MOTORS)) { + //Motors can be below minthrottle when disarmed, but that doesn't happen much + blackboxWriteUnsignedVB(blackboxCurrent->motor[0] - getThrottleIdleValue()); - //Motors tend to be similar to each other so use the first motor's value as a predictor of the others - const int motorCount = getMotorCount(); - for (int x = 1; x < motorCount; x++) { - blackboxWriteSignedVB(blackboxCurrent->motor[x] - blackboxCurrent->motor[0]); + //Motors tend to be similar to each other so use the first motor's value as a predictor of the others + const int motorCount = getMotorCount(); + for (int x = 1; x < motorCount; x++) { + blackboxWriteSignedVB(blackboxCurrent->motor[x] - blackboxCurrent->motor[0]); + } } if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_SERVOS)) { @@ -834,36 +886,41 @@ static void writeIntraframe(void) } } -#ifdef NAV_BLACKBOX blackboxWriteSignedVB(blackboxCurrent->navState); - blackboxWriteSignedVB(blackboxCurrent->navFlags); - blackboxWriteSignedVB(blackboxCurrent->navEPH); - blackboxWriteSignedVB(blackboxCurrent->navEPV); - for (int x = 0; x < XYZ_AXIS_COUNT; x++) { - blackboxWriteSignedVB(blackboxCurrent->navPos[x]); + /* + * NAV_POS fields + */ + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NAV_POS)) { + blackboxWriteSignedVB(blackboxCurrent->navEPH); + blackboxWriteSignedVB(blackboxCurrent->navEPV); + + for (int x = 0; x < XYZ_AXIS_COUNT; x++) { + blackboxWriteSignedVB(blackboxCurrent->navPos[x]); + } + + for (int x = 0; x < XYZ_AXIS_COUNT; x++) { + blackboxWriteSignedVB(blackboxCurrent->navRealVel[x]); + } + + for (int x = 0; x < XYZ_AXIS_COUNT; x++) { + blackboxWriteSignedVB(blackboxCurrent->navTargetVel[x]); + } + + for (int x = 0; x < XYZ_AXIS_COUNT; x++) { + blackboxWriteSignedVB(blackboxCurrent->navTargetPos[x]); + } + + blackboxWriteSignedVB(blackboxCurrent->navSurface); } - for (int x = 0; x < XYZ_AXIS_COUNT; x++) { - blackboxWriteSignedVB(blackboxCurrent->navRealVel[x]); + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NAV_ACC)) { + for (int x = 0; x < XYZ_AXIS_COUNT; x++) { + blackboxWriteSignedVB(blackboxCurrent->navAccNEU[x]); + } } - for (int x = 0; x < XYZ_AXIS_COUNT; x++) { - blackboxWriteSignedVB(blackboxCurrent->navAccNEU[x]); - } - - for (int x = 0; x < XYZ_AXIS_COUNT; x++) { - blackboxWriteSignedVB(blackboxCurrent->navTargetVel[x]); - } - - for (int x = 0; x < XYZ_AXIS_COUNT; x++) { - blackboxWriteSignedVB(blackboxCurrent->navTargetPos[x]); - } - - blackboxWriteSignedVB(blackboxCurrent->navSurface); -#endif - //Rotate our history buffers: //The current state becomes the new "before" state @@ -943,6 +1000,9 @@ static void writeInterframe(void) } } + arraySubInt32(deltas, blackboxCurrent->axisPID_F, blackboxLast->axisPID_F, XYZ_AXIS_COUNT); + blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT); + if (testBlackboxCondition(CONDITION(FIXED_WING_NAV))) { arraySubInt32(deltas, blackboxCurrent->fwAltPID, blackboxLast->fwAltPID, 3); @@ -981,18 +1041,22 @@ static void writeInterframe(void) */ // rcData - for (int x = 0; x < 4; x++) { - deltas[x] = blackboxCurrent->rcData[x] - blackboxLast->rcData[x]; - } + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_RC_DATA)) { + for (int x = 0; x < 4; x++) { + deltas[x] = blackboxCurrent->rcData[x] - blackboxLast->rcData[x]; + } - blackboxWriteTag8_4S16(deltas); + blackboxWriteTag8_4S16(deltas); + } // rcCommand - for (int x = 0; x < 4; x++) { - deltas[x] = blackboxCurrent->rcCommand[x] - blackboxLast->rcCommand[x]; - } + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_RC_COMMAND)) { + for (int x = 0; x < 4; x++) { + deltas[x] = blackboxCurrent->rcCommand[x] - blackboxLast->rcCommand[x]; + } - blackboxWriteTag8_4S16(deltas); + blackboxWriteTag8_4S16(deltas); + } //Check for sensors that are updated periodically (so deltas are normally zero) int optionalFieldCount = 0; @@ -1039,47 +1103,64 @@ static void writeInterframe(void) //Since gyros, accs and motors are noisy, base their predictions on the average of the history: blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, gyroADC), XYZ_AXIS_COUNT); - blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, accADC), XYZ_AXIS_COUNT); - blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, attitude), XYZ_AXIS_COUNT); + + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ACC)) { + blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, accADC), XYZ_AXIS_COUNT); + } + + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ATTITUDE)) { + blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, attitude), XYZ_AXIS_COUNT); + } + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_DEBUG)) { blackboxWriteArrayUsingAveragePredictor32(offsetof(blackboxMainState_t, debug), DEBUG32_VALUE_COUNT); } - blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, motor), getMotorCount()); + + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_MOTORS)) { + blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, motor), getMotorCount()); + } if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_SERVOS)) { blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, servo), MAX_SUPPORTED_SERVOS); } -#ifdef NAV_BLACKBOX blackboxWriteSignedVB(blackboxCurrent->navState - blackboxLast->navState); blackboxWriteSignedVB(blackboxCurrent->navFlags - blackboxLast->navFlags); - blackboxWriteSignedVB(blackboxCurrent->navEPH - blackboxLast->navEPH); - blackboxWriteSignedVB(blackboxCurrent->navEPV - blackboxLast->navEPV); - for (int x = 0; x < XYZ_AXIS_COUNT; x++) { - blackboxWriteSignedVB(blackboxCurrent->navPos[x] - blackboxLast->navPos[x]); + /* + * NAV_POS fields + */ + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NAV_POS)) { + blackboxWriteSignedVB(blackboxCurrent->navEPH - blackboxLast->navEPH); + blackboxWriteSignedVB(blackboxCurrent->navEPV - blackboxLast->navEPV); + + for (int x = 0; x < XYZ_AXIS_COUNT; x++) { + blackboxWriteSignedVB(blackboxCurrent->navPos[x] - blackboxLast->navPos[x]); + } + + for (int x = 0; x < XYZ_AXIS_COUNT; x++) { + blackboxWriteSignedVB(blackboxHistory[0]->navRealVel[x] - (blackboxHistory[1]->navRealVel[x] + blackboxHistory[2]->navRealVel[x]) / 2); + } + + + for (int x = 0; x < XYZ_AXIS_COUNT; x++) { + blackboxWriteSignedVB(blackboxHistory[0]->navTargetVel[x] - (blackboxHistory[1]->navTargetVel[x] + blackboxHistory[2]->navTargetVel[x]) / 2); + } + + for (int x = 0; x < XYZ_AXIS_COUNT; x++) { + blackboxWriteSignedVB(blackboxHistory[0]->navTargetPos[x] - blackboxLast->navTargetPos[x]); + } + + blackboxWriteSignedVB(blackboxCurrent->navSurface - blackboxLast->navSurface); } - for (int x = 0; x < XYZ_AXIS_COUNT; x++) { - blackboxWriteSignedVB(blackboxHistory[0]->navRealVel[x] - (blackboxHistory[1]->navRealVel[x] + blackboxHistory[2]->navRealVel[x]) / 2); + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NAV_ACC)) { + for (int x = 0; x < XYZ_AXIS_COUNT; x++) { + blackboxWriteSignedVB(blackboxHistory[0]->navAccNEU[x] - (blackboxHistory[1]->navAccNEU[x] + blackboxHistory[2]->navAccNEU[x]) / 2); + } } - for (int x = 0; x < XYZ_AXIS_COUNT; x++) { - blackboxWriteSignedVB(blackboxHistory[0]->navAccNEU[x] - (blackboxHistory[1]->navAccNEU[x] + blackboxHistory[2]->navAccNEU[x]) / 2); - } - - for (int x = 0; x < XYZ_AXIS_COUNT; x++) { - blackboxWriteSignedVB(blackboxHistory[0]->navTargetVel[x] - (blackboxHistory[1]->navTargetVel[x] + blackboxHistory[2]->navTargetVel[x]) / 2); - } - - for (int x = 0; x < XYZ_AXIS_COUNT; x++) { - blackboxWriteSignedVB(blackboxHistory[0]->navTargetPos[x] - blackboxLast->navTargetPos[x]); - } - - blackboxWriteSignedVB(blackboxCurrent->navSurface - blackboxLast->navSurface); -#endif - //Rotate our history buffers blackboxHistory[2] = blackboxHistory[1]; blackboxHistory[1] = blackboxHistory[0]; @@ -1396,6 +1477,7 @@ static void loadMainState(timeUs_t currentTimeUs) blackboxCurrent->axisPID_P[i] = axisPID_P[i]; blackboxCurrent->axisPID_I[i] = axisPID_I[i]; blackboxCurrent->axisPID_D[i] = axisPID_D[i]; + blackboxCurrent->axisPID_F[i] = axisPID_F[i]; blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i]); blackboxCurrent->accADC[i] = lrintf(acc.accADCf[i] * acc.dev.acc_1G); #ifdef USE_MAG @@ -1479,7 +1561,6 @@ static void loadMainState(timeUs_t currentTimeUs) blackboxCurrent->servo[i] = servo[i]; } -#ifdef NAV_BLACKBOX blackboxCurrent->navState = navCurrentState; blackboxCurrent->navFlags = navFlags; blackboxCurrent->navEPH = navEPH; @@ -1492,7 +1573,6 @@ static void loadMainState(timeUs_t currentTimeUs) blackboxCurrent->navTargetPos[i] = navTargetPosition[i]; } blackboxCurrent->navSurface = navActualSurface; -#endif } /** @@ -1690,15 +1770,18 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE("rates", "%d,%d,%d", currentControlRateProfile->stabilized.rates[ROLL], currentControlRateProfile->stabilized.rates[PITCH], currentControlRateProfile->stabilized.rates[YAW]); - BLACKBOX_PRINT_HEADER_LINE("rollPID", "%d,%d,%d", pidBank()->pid[PID_ROLL].P, + BLACKBOX_PRINT_HEADER_LINE("rollPID", "%d,%d,%d,%d", pidBank()->pid[PID_ROLL].P, pidBank()->pid[PID_ROLL].I, - pidBank()->pid[PID_ROLL].D); - BLACKBOX_PRINT_HEADER_LINE("pitchPID", "%d,%d,%d", pidBank()->pid[PID_PITCH].P, + pidBank()->pid[PID_ROLL].D, + pidBank()->pid[PID_ROLL].FF); + BLACKBOX_PRINT_HEADER_LINE("pitchPID", "%d,%d,%d,%d", pidBank()->pid[PID_PITCH].P, pidBank()->pid[PID_PITCH].I, - pidBank()->pid[PID_PITCH].D); - BLACKBOX_PRINT_HEADER_LINE("yawPID", "%d,%d,%d", pidBank()->pid[PID_YAW].P, + pidBank()->pid[PID_PITCH].D, + pidBank()->pid[PID_PITCH].FF); + BLACKBOX_PRINT_HEADER_LINE("yawPID", "%d,%d,%d,%d", pidBank()->pid[PID_YAW].P, pidBank()->pid[PID_YAW].I, - pidBank()->pid[PID_YAW].D); + pidBank()->pid[PID_YAW].D, + pidBank()->pid[PID_YAW].FF); BLACKBOX_PRINT_HEADER_LINE("altPID", "%d,%d,%d", pidBank()->pid[PID_POS_Z].P, pidBank()->pid[PID_POS_Z].I, pidBank()->pid[PID_POS_Z].D); @@ -1726,7 +1809,6 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE("gyro_lpf_hz", "%d", gyroConfig()->gyro_main_lpf_hz); BLACKBOX_PRINT_HEADER_LINE("gyro_lpf_type", "%d", gyroConfig()->gyro_main_lpf_type); #ifdef USE_DYNAMIC_FILTERS - BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchRange", "%d", gyroConfig()->dynamicGyroNotchRange); BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchQ", "%d", gyroConfig()->dynamicGyroNotchQ); BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchMinHz", "%d", gyroConfig()->dynamicGyroNotchMinHz); #endif @@ -1749,9 +1831,7 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE("motor_pwm_rate", "%d", motorConfig()->motorPwmRate); BLACKBOX_PRINT_HEADER_LINE("debug_mode", "%d", systemConfig()->debug_mode); BLACKBOX_PRINT_HEADER_LINE("features", "%d", featureConfig()->enabledFeatures); -#ifdef NAV_BLACKBOX BLACKBOX_PRINT_HEADER_LINE("waypoints", "%d,%d", getWaypointCount(),isWaypointListValid()); -#endif BLACKBOX_PRINT_HEADER_LINE("acc_notch_hz", "%d", accelerometerConfig()->acc_notch_hz); BLACKBOX_PRINT_HEADER_LINE("acc_notch_cutoff", "%d", accelerometerConfig()->acc_notch_cutoff); BLACKBOX_PRINT_HEADER_LINE("pidSumLimit", "%d", pidProfile()->pidSumLimit); diff --git a/src/main/blackbox/blackbox.h b/src/main/blackbox/blackbox.h index 0b180e9f7c..854210441d 100644 --- a/src/main/blackbox/blackbox.h +++ b/src/main/blackbox/blackbox.h @@ -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); \ No newline at end of file diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h index f7f3dd7148..9b2a1da0ff 100644 --- a/src/main/blackbox/blackbox_fielddefs.h +++ b/src/main/blackbox/blackbox_fielddefs.h @@ -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, diff --git a/src/main/build/build_config.h b/src/main/build/build_config.h index ae4eb1a3a4..2ded047b05 100644 --- a/src/main/build/build_config.h +++ b/src/main/build/build_config.h @@ -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 diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 76f4cc5836..83e4dcbd9f 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -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; diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index a065c900a4..60105587ba 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -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), diff --git a/src/main/cms/cms_menu_misc.c b/src/main/cms/cms_menu_misc.c index 1180932505..68d01ae658 100644 --- a/src/main/cms/cms_menu_misc.c +++ b/src/main/cms/cms_menu_misc.c @@ -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, }; diff --git a/src/main/cms/cms_menu_navigation.c b/src/main/cms/cms_menu_navigation.c index e15ce1da80..1947233c76 100644 --- a/src/main/cms/cms_menu_navigation.c +++ b/src/main/cms/cms_menu_navigation.c @@ -44,7 +44,8 @@ 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), diff --git a/src/main/common/calibration.c b/src/main/common/calibration.c index 4c5efba684..be67a0932f 100644 --- a/src/main/common/calibration.c +++ b/src/main/common/calibration.c @@ -34,7 +34,7 @@ void zeroCalibrationStartS(zeroCalibrationScalar_t * s, timeMs_t window, float t { // Reset parameters and state s->params.state = ZERO_CALIBRATION_IN_PROGRESS; - s->params.startTimeMs = millis(); + s->params.startTimeMs = 0; s->params.windowSizeMs = window; s->params.stdDevThreshold = threshold; s->params.allowFailure = allowFailure; @@ -60,6 +60,13 @@ void zeroCalibrationAddValueS(zeroCalibrationScalar_t * s, const float v) return; } + // An unknown delay may have passed between `zeroCalibrationStartS` and first sample acquisition + // therefore our window measurement might be incorrect + // To account for that we reset the startTimeMs when acquiring the first sample + if (s->params.sampleCount == 0 && s->params.startTimeMs == 0) { + s->params.startTimeMs = millis(); + } + // Add value s->val.accumulatedValue += v; s->params.sampleCount++; diff --git a/src/main/common/constants.h b/src/main/common/constants.h index 6b2f9f69b6..cef3834fe6 100644 --- a/src/main/common/constants.h +++ b/src/main/common/constants.h @@ -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 diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 0656a5dba7..6738169fc2 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -109,6 +109,72 @@ void pt1FilterReset(pt1Filter_t *filter, float input) filter->state = input; } +/* + * PT2 LowPassFilter + */ +float pt2FilterGain(float f_cut, float dT) +{ + const float order = 2.0f; + const float orderCutoffCorrection = 1 / sqrtf(powf(2, 1.0f / order) - 1); + float RC = 1 / (2 * orderCutoffCorrection * M_PIf * f_cut); + // float RC = 1 / (2 * 1.553773974f * M_PIf * f_cut); + // where 1.553773974 = 1 / sqrt( (2^(1 / order) - 1) ) and order is 2 + return dT / (RC + dT); +} + +void pt2FilterInit(pt2Filter_t *filter, float k) +{ + filter->state = 0.0f; + filter->state1 = 0.0f; + filter->k = k; +} + +void pt2FilterUpdateCutoff(pt2Filter_t *filter, float k) +{ + filter->k = k; +} + +FAST_CODE float pt2FilterApply(pt2Filter_t *filter, float input) +{ + filter->state1 = filter->state1 + filter->k * (input - filter->state1); + filter->state = filter->state + filter->k * (filter->state1 - filter->state); + return filter->state; +} + +/* + * PT3 LowPassFilter + */ +float pt3FilterGain(float f_cut, float dT) +{ + const float order = 3.0f; + const float orderCutoffCorrection = 1 / sqrtf(powf(2, 1.0f / order) - 1); + float RC = 1 / (2 * orderCutoffCorrection * M_PIf * f_cut); + // float RC = 1 / (2 * 1.961459177f * M_PIf * f_cut); + // where 1.961459177 = 1 / sqrt( (2^(1 / order) - 1) ) and order is 3 + return dT / (RC + dT); +} + +void pt3FilterInit(pt3Filter_t *filter, float k) +{ + filter->state = 0.0f; + filter->state1 = 0.0f; + filter->state2 = 0.0f; + filter->k = k; +} + +void pt3FilterUpdateCutoff(pt3Filter_t *filter, float k) +{ + filter->k = k; +} + +FAST_CODE float pt3FilterApply(pt3Filter_t *filter, float input) +{ + filter->state1 = filter->state1 + filter->k * (input - filter->state1); + filter->state2 = filter->state2 + filter->k * (filter->state1 - filter->state2); + filter->state = filter->state + filter->k * (filter->state2 - filter->state); + return filter->state; +} + // rate_limit = maximum rate of change of the output value in units per second void rateLimitFilterInit(rateLimitFilter_t *filter) { @@ -251,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 \ No newline at end of file +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; + } + } +} \ No newline at end of file diff --git a/src/main/common/filter.h b/src/main/common/filter.h index a6406ee91c..c6cedc8649 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -27,6 +27,17 @@ typedef struct pt1Filter_s { float dT; float alpha; } pt1Filter_t; +typedef struct pt2Filter_s { + float state; + float state1; + float k; +} pt2Filter_t; +typedef struct pt3Filter_s { + float state; + float state1; + float state2; + float k; +} pt3Filter_t; /* this holds the data required to update samples thru a filter */ typedef struct biquadFilter_s { @@ -36,12 +47,16 @@ typedef struct biquadFilter_s { typedef union { biquadFilter_t biquad; - pt1Filter_t pt1; + 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 { @@ -87,6 +102,22 @@ float pt1FilterApply3(pt1Filter_t *filter, float input, float dT); float pt1FilterApply4(pt1Filter_t *filter, float input, float f_cut, float dt); void pt1FilterReset(pt1Filter_t *filter, float input); +/* + * PT2 LowPassFilter + */ +float pt2FilterGain(float f_cut, float dT); +void pt2FilterInit(pt2Filter_t *filter, float k); +void pt2FilterUpdateCutoff(pt2Filter_t *filter, float k); +float pt2FilterApply(pt2Filter_t *filter, float input); + +/* + * PT3 LowPassFilter + */ +float pt3FilterGain(float f_cut, float dT); +void pt3FilterInit(pt3Filter_t *filter, float k); +void pt3FilterUpdateCutoff(pt3Filter_t *filter, float k); +float pt3FilterApply(pt3Filter_t *filter, float input); + void rateLimitFilterInit(rateLimitFilter_t *filter); float rateLimitFilterApply4(rateLimitFilter_t *filter, float input, float rate_limit, float dT); @@ -100,4 +131,7 @@ float filterGetNotchQ(float centerFrequencyHz, float cutoffFrequencyHz); void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType); void alphaBetaGammaFilterInit(alphaBetaGammaFilter_t *filter, float alpha, float boostGain, float halfLife, float dT); -float alphaBetaGammaFilterApply(alphaBetaGammaFilter_t *filter, float input); \ No newline at end of file +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); \ No newline at end of file diff --git a/src/main/common/maths.c b/src/main/common/maths.c index a99c3f346b..4a07f7a8ef 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -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; } \ No newline at end of file diff --git a/src/main/common/maths.h b/src/main/common/maths.h index d84fc25751..4450723a2b 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -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__ ({ \ diff --git a/src/main/config/config_eeprom.c b/src/main/config/config_eeprom.c index e59c42b700..f64b2bb8a6 100755 --- a/src/main/config/config_eeprom.c +++ b/src/main/config/config_eeprom.c @@ -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 } } diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index b7c39e4b6e..aaf26ecb32 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -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" diff --git a/src/main/config/config_streamer.c b/src/main/config/config_streamer.c index c09bac00dd..6a598cc9e4 100644 --- a/src/main/config/config_streamer.c +++ b/src/main/config/config_streamer.c @@ -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; diff --git a/src/main/config/config_streamer.h b/src/main/config/config_streamer.h index 952c811efe..87b688406f 100644 --- a/src/main/config/config_streamer.h +++ b/src/main/config/config_streamer.h @@ -20,10 +20,15 @@ #include #include +#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]; diff --git a/src/main/config/config_streamer_extflash.c b/src/main/config/config_streamer_extflash.c new file mode 100644 index 0000000000..cb576ccc26 --- /dev/null +++ b/src/main/config/config_streamer_extflash.c @@ -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 . + */ + +#include +#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 diff --git a/src/main/config/config_streamer_ram.c b/src/main/config/config_streamer_ram.c new file mode 100644 index 0000000000..9a7e8578ab --- /dev/null +++ b/src/main/config/config_streamer_ram.c @@ -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 . + */ + +#include +#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 diff --git a/src/main/config/config_streamer_stm32f3.c b/src/main/config/config_streamer_stm32f3.c index 41c49d22b6..f56f86e7a9 100644 --- a/src/main/config/config_streamer_stm32f3.c +++ b/src/main/config/config_streamer_stm32f3.c @@ -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 @@ -57,4 +57,4 @@ int config_streamer_impl_write_word(config_streamer_t *c, config_streamer_buffer return 0; } -#endif \ No newline at end of file +#endif diff --git a/src/main/config/config_streamer_stm32f4.c b/src/main/config/config_streamer_stm32f4.c index dcd7b5feab..4f48831bc8 100644 --- a/src/main/config/config_streamer_stm32f4.c +++ b/src/main/config/config_streamer_stm32f4.c @@ -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 @@ -103,4 +103,4 @@ int config_streamer_impl_write_word(config_streamer_t *c, config_streamer_buffer return 0; } -#endif \ No newline at end of file +#endif diff --git a/src/main/config/config_streamer_stm32f7.c b/src/main/config/config_streamer_stm32f7.c index afcedc7ba0..61b148fce5 100644 --- a/src/main/config/config_streamer_stm32f7.c +++ b/src/main/config/config_streamer_stm32f7.c @@ -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) /* @@ -138,4 +138,4 @@ int config_streamer_impl_write_word(config_streamer_t *c, config_streamer_buffer return 0; } -#endif \ No newline at end of file +#endif diff --git a/src/main/config/config_streamer_stm32h7.c b/src/main/config/config_streamer_stm32h7.c index ee23ada22c..baa41ea92b 100644 --- a/src/main/config/config_streamer_stm32h7.c +++ b/src/main/config/config_streamer_stm32h7.c @@ -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 */ @@ -96,4 +96,4 @@ int config_streamer_impl_write_word(config_streamer_t *c, config_streamer_buffer return 0; } -#endif \ No newline at end of file +#endif diff --git a/src/main/drivers/accgyro/accgyro_adxl345.c b/src/main/drivers/accgyro/accgyro_adxl345.c deleted file mode 100644 index b867f85b4a..0000000000 --- a/src/main/drivers/accgyro/accgyro_adxl345.c +++ /dev/null @@ -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 . - */ - -#include -#include -#include - -#include - -#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 diff --git a/src/main/drivers/accgyro/accgyro_bma280.c b/src/main/drivers/accgyro/accgyro_bma280.c deleted file mode 100644 index a97b831bd2..0000000000 --- a/src/main/drivers/accgyro/accgyro_bma280.c +++ /dev/null @@ -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 . - */ - -#include -#include -#include - -#include - -#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> | 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 diff --git a/src/main/drivers/accgyro/accgyro_bmi270.c b/src/main/drivers/accgyro/accgyro_bmi270.c new file mode 100644 index 0000000000..c578df4b70 --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_bmi270.c @@ -0,0 +1,354 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include +#include + +#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 + return true; +} +#endif // USE_IMU_BMI270 diff --git a/src/main/drivers/accgyro/accgyro_bmi270.h b/src/main/drivers/accgyro/accgyro_bmi270.h new file mode 100644 index 0000000000..cab79c5dcd --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_bmi270.h @@ -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 . + */ + +#pragma once + +#include "drivers/bus.h" + +bool bmi270AccDetect(accDev_t *acc); +bool bmi270GyroDetect(gyroDev_t *gyro); diff --git a/src/main/drivers/accgyro/accgyro_bmi270_maximum_fifo.c b/src/main/drivers/accgyro/accgyro_bmi270_maximum_fifo.c new file mode 100644 index 0000000000..96100ffab2 --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_bmi270_maximum_fifo.c @@ -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 diff --git a/src/main/drivers/accgyro/accgyro_icm42605.c b/src/main/drivers/accgyro/accgyro_icm42605.c new file mode 100644 index 0000000000..5bf61e0760 --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_icm42605.c @@ -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 . + */ + +#include +#include +#include + +#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 diff --git a/src/main/drivers/rangefinder/rangefinder_hcsr04_i2c.h b/src/main/drivers/accgyro/accgyro_icm42605.h similarity index 87% rename from src/main/drivers/rangefinder/rangefinder_hcsr04_i2c.h rename to src/main/drivers/accgyro/accgyro_icm42605.h index 7d2ed6265e..5224222f26 100644 --- a/src/main/drivers/rangefinder/rangefinder_hcsr04_i2c.h +++ b/src/main/drivers/accgyro/accgyro_icm42605.h @@ -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); diff --git a/src/main/drivers/accgyro/accgyro_l3g4200d.c b/src/main/drivers/accgyro/accgyro_l3g4200d.c deleted file mode 100644 index e674973bf2..0000000000 --- a/src/main/drivers/accgyro/accgyro_l3g4200d.c +++ /dev/null @@ -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 . - */ - -#include -#include -#include - -#include - -#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; -} diff --git a/src/main/drivers/accgyro/accgyro_l3g4200d.h b/src/main/drivers/accgyro/accgyro_l3g4200d.h deleted file mode 100644 index c4366f2270..0000000000 --- a/src/main/drivers/accgyro/accgyro_l3g4200d.h +++ /dev/null @@ -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 . - */ - -#pragma once - -bool l3g4200dDetect(gyroDev_t *gyro); diff --git a/src/main/drivers/accgyro/accgyro_mma845x.c b/src/main/drivers/accgyro/accgyro_mma845x.c deleted file mode 100644 index 0a5a9844aa..0000000000 --- a/src/main/drivers/accgyro/accgyro_mma845x.c +++ /dev/null @@ -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 . - */ - -#include -#include - -#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; -} diff --git a/src/main/drivers/accgyro/accgyro_mma845x.h b/src/main/drivers/accgyro/accgyro_mma845x.h deleted file mode 100644 index bb8887ccf4..0000000000 --- a/src/main/drivers/accgyro/accgyro_mma845x.h +++ /dev/null @@ -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 . - */ - -#pragma once - -bool mma8452Detect(accDev_t *acc); diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h index b6b8cb38f3..16d7f5ac19 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.h +++ b/src/main/drivers/accgyro/accgyro_mpu.h @@ -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 diff --git a/src/main/drivers/barometer/barometer_bmp388.c b/src/main/drivers/barometer/barometer_bmp388.c index c35fbc05b4..000058534c 100644 --- a/src/main/drivers/barometer/barometer_bmp388.c +++ b/src/main/drivers/barometer/barometer_bmp388.c @@ -197,7 +197,13 @@ static bool bmp388StartUP(baroDev_t *baro) static bool bmp388GetUP(baroDev_t *baro) { - busReadBuf(baro->busDev, BMP388_DATA_0_REG, sensor_data, BMP388_DATA_FRAME_SIZE + 1); + if (baro->busDev->busType == BUSTYPE_SPI) { + // In SPI mode, first byte read is a dummy byte + busReadBuf(baro->busDev, BMP388_DATA_0_REG, &sensor_data[0], BMP388_DATA_FRAME_SIZE + 1); + } else { + // In I2C mode, no dummy byte is read + busReadBuf(baro->busDev, BMP388_DATA_0_REG, &sensor_data[1], BMP388_DATA_FRAME_SIZE); + } bmp388_up = sensor_data[1] << 0 | sensor_data[2] << 8 | sensor_data[3] << 16; bmp388_ut = sensor_data[4] << 0 | sensor_data[5] << 8 | sensor_data[6] << 16; @@ -289,14 +295,26 @@ STATIC_UNIT_TESTED bool bmp388Calculate(baroDev_t *baro, int32_t *pressure, int3 #define DETECTION_MAX_RETRY_COUNT 5 static bool deviceDetect(busDevice_t * busDev) { - for (int retry = 0; retry < DETECTION_MAX_RETRY_COUNT; retry++) { - uint8_t chipId[2]; + uint8_t chipId[2]; + uint8_t nRead; + uint8_t * pId; + if (busDev->busType == BUSTYPE_SPI) { + // In SPI mode, first byte read is a dummy byte + nRead = 2; + pId = &chipId[1]; + } else { + // In I2C mode, no dummy byte is read + nRead = 1; + pId = &chipId[0]; + } + + for (int retry = 0; retry < DETECTION_MAX_RETRY_COUNT; retry++) { delay(100); - bool ack = busReadBuf(busDev, BMP388_CHIP_ID_REG, chipId, 2); + bool ack = busReadBuf(busDev, BMP388_CHIP_ID_REG, chipId, nRead); - if (ack && chipId[1] == BMP388_DEFAULT_CHIP_ID) { + if (ack && *pId == BMP388_DEFAULT_CHIP_ID) { return true; } }; @@ -318,11 +336,16 @@ bool bmp388Detect(baroDev_t *baro) return false; } - uint8_t calibration_buf[sizeof(bmp388_calib_param_t) + 1]; - // read calibration - busReadBuf(baro->busDev, BMP388_TRIMMING_NVM_PAR_T1_LSB_REG, calibration_buf, sizeof(bmp388_calib_param_t) + 1); - memcpy(&bmp388_cal, calibration_buf + 1, sizeof(bmp388_calib_param_t)); + if (baro->busDev->busType == BUSTYPE_SPI) { + // In SPI mode, first byte read is a dummy byte + uint8_t calibration_buf[sizeof(bmp388_calib_param_t) + 1]; + busReadBuf(baro->busDev, BMP388_TRIMMING_NVM_PAR_T1_LSB_REG, calibration_buf, sizeof(bmp388_calib_param_t) + 1); + memcpy(&bmp388_cal, calibration_buf + 1, sizeof(bmp388_calib_param_t)); + } else { + // In I2C mode, no dummy byte is read + busReadBuf(baro->busDev, BMP388_TRIMMING_NVM_PAR_T1_LSB_REG, (uint8_t*)&bmp388_cal, sizeof(bmp388_calib_param_t)); + } // set oversampling + power mode (forced), and start sampling busWrite(baro->busDev, BMP388_OSR_REG, diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h index eefa81eb10..bc44825251 100755 --- a/src/main/drivers/bus.h +++ b/src/main/drivers/bus.h @@ -76,14 +76,10 @@ typedef enum { DEVHW_NONE = 0, /* Dedicated ACC chips */ - DEVHW_BMA280, - DEVHW_ADXL345, - DEVHW_MMA8452, DEVHW_LSM303DLHC, /* Dedicated GYRO chips */ DEVHW_L3GD20, - DEVHW_L3G4200, /* Combined ACC/GYRO chips */ DEVHW_MPU3050, @@ -94,6 +90,8 @@ typedef enum { DEVHW_BMI088_GYRO, DEVHW_BMI088_ACC, DEVHW_ICM20689, + DEVHW_ICM42605, + DEVHW_BMI270, /* Combined ACC/GYRO/MAG chips */ DEVHW_MPU9250, @@ -119,6 +117,8 @@ typedef enum { DEVHW_MAG3110, DEVHW_LIS3MDL, DEVHW_RM3100, + DEVHW_VCM5883, + DEVHW_MLX90393, /* Temp sensor chips */ DEVHW_LM75_0, @@ -138,10 +138,10 @@ typedef enum { /* Rangefinder modules */ DEVHW_SRF10, - DEVHW_HCSR04_I2C, // DIY-style adapter DEVHW_VL53L0X, DEVHW_VL53L1X, DEVHW_US42, + DEVHW_TOF10120_I2C, /* Other hardware */ DEVHW_MS4525, // Pitot meter diff --git a/src/main/drivers/bus_i2c_stm32f30x.c b/src/main/drivers/bus_i2c_stm32f30x.c index d39f74f7bf..915e295480 100644 --- a/src/main/drivers/bus_i2c_stm32f30x.c +++ b/src/main/drivers/bus_i2c_stm32f30x.c @@ -87,7 +87,7 @@ typedef struct i2cBusState_s { I2CDevice device; bool initialized; i2cState_t state; - uint32_t timeout; + timeUs_t timeout; /* Active transfer */ bool allowRawAccess; diff --git a/src/main/drivers/bus_i2c_stm32f40x.c b/src/main/drivers/bus_i2c_stm32f40x.c index 0f4d8b45d8..ae96d65f15 100644 --- a/src/main/drivers/bus_i2c_stm32f40x.c +++ b/src/main/drivers/bus_i2c_stm32f40x.c @@ -121,7 +121,7 @@ typedef struct i2cBusState_s { I2CDevice device; bool initialized; i2cState_t state; - uint32_t timeout; + timeUs_t timeout; /* Active transfer */ bool allowRawAccess; diff --git a/src/main/drivers/compass/compass_mlx90393.c b/src/main/drivers/compass/compass_mlx90393.c new file mode 100644 index 0000000000..c2b5e40114 --- /dev/null +++ b/src/main/drivers/compass/compass_mlx90393.c @@ -0,0 +1,176 @@ +/* + * This file is part of iNav. + * + * iNav is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * iNav is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with iNav. If not, see . + */ + +#include "platform.h" + +#ifdef USE_MAG_MLX90393 + +#include +#include + +#include + +#include "build/debug.h" + +#include "common/axis.h" +#include "common/maths.h" +#include "common/utils.h" + +#include "drivers/time.h" +#include "drivers/io.h" +#include "drivers/bus.h" + +#include "drivers/sensor.h" +#include "drivers/compass/compass.h" + +#define MLX90393_MEASURE_3D 0b00001110 + +#define MLX90393_NOP 0b00000000 +#define MLX90393_START_BURST_MODE 0b00010000 //uses with zyxt flags +#define MLX90393_START_WAKE_UP_ON_CHANGE_MODE 0b00100000 //uses with zyxt flags +#define MLX90393_START_SINGLE_MEASUREMENT_MODE 0b00110000 //uses with zyxt flags +#define MLX90393_READ_MEASUREMENT 0b01000000 //uses with zyxt flags +#define MLX90393_READ_REGISTER 0b01010000 +#define MLX90393_WRITE_REGISTER 0b01100000 +#define MLX90393_EXIT_MODE 0b10000000 +#define MLX90393_MEMORY_RECALL 0b11010000 +#define MLX90393_MEMORY_STORE 0b11100000 +#define MLX90393_RESET 0b11110000 + +#define MLX90393_BURST_MODE_FLAG 0b10000000 +#define MLX90393_WAKE_UP_ON_CHANGE_MODE_FLAG 0b01000000 +#define MLX90393_SINGLE_MEASUREMENT_MODE_FLAG 0b00100000 +#define MLX90393_ERROR_FLAG 0b00010000 +#define MLX90393_SINGLE_ERROR_DETECTION_FLAG 0b00001000 +#define MLX90393_RESET_FLAG 0b00000100 +#define MLX90393_D1_FLAG 0b00000010 +#define MLX90393_D0_FLAG 0b00000001 + +#define DETECTION_MAX_RETRY_COUNT 5 + +#define REG_BUF_LEN 3 + +// Register 1 +#define GAIN_SEL_HALLCONF_REG 0x0 //from datasheet 0x0 << 2 = 0x0 +// GAIN - 0b111 +// Hall_conf - 0xC +#define GAIN_SEL_HALLCONF_REG_VALUE 0x007C +// Register 2 +#define TCMP_EN_REG 0x4 //from datasheet 0x1 << 2 = 0x4 +// Burst Data Rate 0b000100 +#define TCMP_EN_REG_VALUE 0x62C4 +// Register 3 +#define RES_XYZ_OSR_FLT_REG 0x8 //from datasheet 0x2 << 2 = 0x8 +// Oversampling 0b01 +// Filtering 0b111 +#define RES_XYZ_OSR_FLT_REG_VALUE 0x001D + + +static void mlx90393WriteRegister(magDev_t * mag, uint8_t reg_val, uint16_t value) { + + uint8_t buf[REG_BUF_LEN] = {0}; + + buf[0] = (value >> 8) & 0xFF; + buf[1] = (value >> 0) & 0xFF; + buf[2] = reg_val; + + busWriteBuf(mag->busDev, MLX90393_WRITE_REGISTER, buf, REG_BUF_LEN); + + // PAUSE + delay(20); + // To skip ACK FLAG of Write + uint8_t sig = 0; + busRead(mag->busDev, MLX90393_NOP, &sig); + + return; +} + +// ======================================================================================= +static bool mlx90393Read(magDev_t * mag) +{ + + uint8_t buf[7] = {0}; + + busReadBuf(mag->busDev, MLX90393_READ_MEASUREMENT | MLX90393_MEASURE_3D, buf, 7); + + mag->magADCRaw[X] = ((short)(buf[1] << 8 | buf[2])); + mag->magADCRaw[Y] = ((short)(buf[3] << 8 | buf[4])); + mag->magADCRaw[Z] = ((short)(buf[5] << 8 | buf[6])); + + return true; + +} + +static bool deviceDetect(magDev_t * mag) +{ + for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) { + uint8_t sig = 0; + + bool ack = busRead(mag->busDev, MLX90393_RESET, &sig); + delay(20); + + if (ack && ((sig & MLX90393_RESET_FLAG) == MLX90393_RESET_FLAG)) { // Check Reset Flag -> MLX90393 + return true; + } + } + return false; +} + +//-------------------------------------------------- +static bool mlx90393Init(magDev_t * mag) +{ + uint8_t sig = 0; + + delay(20); + // Remove reset flag + busRead(mag->busDev, MLX90393_NOP, &sig); + // Exit if already in burst mode. (For example when external i2c source power the bus.) + busRead(mag->busDev, MLX90393_EXIT_MODE, &sig); + + // Writing Registers + mlx90393WriteRegister(mag, GAIN_SEL_HALLCONF_REG, GAIN_SEL_HALLCONF_REG_VALUE); + mlx90393WriteRegister(mag, TCMP_EN_REG, TCMP_EN_REG_VALUE); + mlx90393WriteRegister(mag, RES_XYZ_OSR_FLT_REG, RES_XYZ_OSR_FLT_REG_VALUE); + + // Start burst mode + busRead(mag->busDev, MLX90393_START_BURST_MODE | MLX90393_MEASURE_3D, &sig); + + return true; +} + +// ========================================================================== + +bool mlx90393Detect(magDev_t * mag) +{ + mag->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_MLX90393, mag->magSensorToUse, OWNER_COMPASS); + if (mag->busDev == NULL) { + return false; + } + + if (!deviceDetect(mag)) { + busDeviceDeInit(mag->busDev); + return false; + } + + mag->init = mlx90393Init; + mag->read = mlx90393Read; + + return true; +} + + +#endif diff --git a/src/main/drivers/accgyro/accgyro_bma280.h b/src/main/drivers/compass/compass_mlx90393.h similarity index 62% rename from src/main/drivers/accgyro/accgyro_bma280.h rename to src/main/drivers/compass/compass_mlx90393.h index b4624d66a8..5d73607294 100644 --- a/src/main/drivers/accgyro/accgyro_bma280.h +++ b/src/main/drivers/compass/compass_mlx90393.h @@ -1,20 +1,20 @@ /* - * This file is part of Cleanflight. + * This file is part of iNav. * - * Cleanflight is free software: you can redistribute it and/or modify + * iNav is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * - * Cleanflight is distributed in the hope that it will be useful, + * iNav is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . + * along with iNav. If not, see . */ #pragma once -bool bma280Detect(accDev_t *acc); +bool mlx90393Detect(magDev_t* mag); diff --git a/src/main/drivers/compass/compass_vcm5883.c b/src/main/drivers/compass/compass_vcm5883.c new file mode 100644 index 0000000000..9eb312929b --- /dev/null +++ b/src/main/drivers/compass/compass_vcm5883.c @@ -0,0 +1,120 @@ +/* + * This file is part of INAV. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#include "platform.h" + +#ifdef USE_MAG_VCM5883 + +#include +#include +#include + +#include "build/build_config.h" + +#include "common/axis.h" +#include "common/maths.h" +#include "common/utils.h" + +#include "drivers/time.h" +#include "drivers/bus_i2c.h" + +#include "sensors/boardalignment.h" +#include "sensors/sensors.h" + +#include "drivers/sensor.h" +#include "drivers/compass/compass.h" + +#include "drivers/compass/compass_vcm5883.h" + +#include "build/debug.h" + +#define VCM5883_REGISTER_ADDR_CNTL1 0x0B +#define VCM5883_REGISTER_ADDR_CNTL2 0x0A +#define VCM5883_REGISTER_ADDR_CHIPID 0x0C +#define VCM5883_REGISTER_ADDR_OUTPUT_X 0x00 +#define VCM5883_INITIAL_CONFIG 0b0100 + +#define DETECTION_MAX_RETRY_COUNT 5 +static bool deviceDetect(magDev_t * mag) +{ + for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) { + busWrite(mag->busDev, VCM5883_REGISTER_ADDR_CNTL2, 0b01000001); + delay(30); + + uint8_t sig = 0; + bool ack = busRead(mag->busDev, VCM5883_REGISTER_ADDR_CHIPID, &sig); + + if (ack && sig == 0x82) { + return true; + } + } + + return false; +} + +static bool vcm5883Init(magDev_t * mag) { + UNUSED(mag); + return true; +} + +static bool vcm5883Read(magDev_t * mag) +{ + uint8_t buf[6]; + + // set magData to zero for case of failed read + mag->magADCRaw[X] = 0; + mag->magADCRaw[Y] = 0; + mag->magADCRaw[Z] = 0; + + bool ack = busReadBuf(mag->busDev, VCM5883_REGISTER_ADDR_OUTPUT_X, buf, 6); + if (!ack) { + return false; + } + + mag->magADCRaw[X] = (int16_t)(buf[1] << 8 | buf[0]); + mag->magADCRaw[Y] = (int16_t)(buf[3] << 8 | buf[2]); + mag->magADCRaw[Z] = (int16_t)(buf[5] << 8 | buf[4]); + + return true; +} + +bool vcm5883Detect(magDev_t * mag) +{ + mag->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_VCM5883, mag->magSensorToUse, OWNER_COMPASS); + if (mag->busDev == NULL) { + return false; + } + + if (!deviceDetect(mag)) { + busDeviceDeInit(mag->busDev); + return false; + } + + mag->init = vcm5883Init; + mag->read = vcm5883Read; + + return true; +} + +#endif \ No newline at end of file diff --git a/src/main/io/esc_serialshot.h b/src/main/drivers/compass/compass_vcm5883.h similarity index 86% rename from src/main/io/esc_serialshot.h rename to src/main/drivers/compass/compass_vcm5883.h index c915cc9f4f..b21063bf06 100644 --- a/src/main/io/esc_serialshot.h +++ b/src/main/drivers/compass/compass_vcm5883.h @@ -1,5 +1,5 @@ /* - * This file is part of INAV Project. + * This file is part of INAV. * * This Source Code Form is subject to the terms of the Mozilla Public * License, v. 2.0. If a copy of the MPL was not distributed with this file, @@ -24,6 +24,4 @@ #pragma once -bool serialshotInitialize(void); -void serialshotUpdateMotor(int index, uint16_t value); -void serialshotSendUpdate(void); +bool vcm5883Detect(magDev_t *mag); \ No newline at end of file diff --git a/src/main/drivers/flash.c b/src/main/drivers/flash.c index f65c1fd497..647ac675a0 100644 --- a/src/main/drivers/flash.c +++ b/src/main/drivers/flash.c @@ -196,7 +196,15 @@ static void flashConfigurePartitions(void) #endif #if defined(CONFIG_IN_EXTERNAL_FLASH) - createPartition(FLASH_PARTITION_TYPE_CONFIG, EEPROM_SIZE, &endSector); + uint32_t configSize = EEPROM_SIZE; + flashSector_t configSectors = (configSize / flashGeometry->sectorSize); + + if (configSize % flashGeometry->sectorSize > 0) { + configSectors++; // needs a portion of a sector. + } + configSize = configSectors * flashGeometry->sectorSize; + + createPartition(FLASH_PARTITION_TYPE_CONFIG, configSize, &endSector); #endif #ifdef USE_FLASHFS diff --git a/src/main/drivers/flash_m25p16.c b/src/main/drivers/flash_m25p16.c index c2529b436b..ea22ffddde 100644 --- a/src/main/drivers/flash_m25p16.c +++ b/src/main/drivers/flash_m25p16.c @@ -68,6 +68,7 @@ static flashGeometry_t geometry = {.pageSize = M25P16_PAGESIZE}; static busDevice_t * busDev = NULL; static bool isLargeFlash = false; +static uint32_t timeoutAt = 0; /* * Whether we've performed an action that could have made the device busy for writes. @@ -114,15 +115,30 @@ bool m25p16_isReady(void) return !couldBeBusy; } +static void m25p16_setTimeout(uint32_t timeoutMillis) +{ + uint32_t now = millis(); + timeoutAt = now + timeoutMillis; +} + bool m25p16_waitForReady(uint32_t timeoutMillis) { - uint32_t time = millis(); + uint32_t timeoutAtUse; + if (timeoutMillis == 0) { + timeoutAtUse = timeoutAt; + } + else { + timeoutAtUse = millis() + timeoutMillis; + } + while (!m25p16_isReady()) { - if (timeoutMillis && (millis() - time > timeoutMillis)) { + uint32_t now = millis(); + if (now >= timeoutAtUse) { return false; } } + timeoutAt = 0; return true; } @@ -216,10 +232,6 @@ static bool m25p16_readIdentification(void) */ bool m25p16_init(int flashNumToUse) { - if (busDev) { - return true; - } - busDev = busDeviceInit(BUSTYPE_SPI, DEVHW_M25P16, flashNumToUse, OWNER_FLASH); if (busDev == NULL) { return false; @@ -252,20 +264,28 @@ void m25p16_eraseSector(uint32_t address) m25p16_setCommandAddress(&out[1], address, isLargeFlash); - m25p16_waitForReady(SECTOR_ERASE_TIMEOUT_MILLIS); + if (!m25p16_waitForReady(0)) { + return; + } m25p16_writeEnable(); busTransfer(busDev, NULL, out, isLargeFlash ? 5 : 4); + + m25p16_setTimeout(SECTOR_ERASE_TIMEOUT_MILLIS); } void m25p16_eraseCompletely(void) { - m25p16_waitForReady(BULK_ERASE_TIMEOUT_MILLIS); + if (!m25p16_waitForReady(0)) { + return; + } m25p16_writeEnable(); m25p16_performOneByteCommand(M25P16_INSTRUCTION_BULK_ERASE); + + m25p16_setTimeout(BULK_ERASE_TIMEOUT_MILLIS); } /** @@ -294,12 +314,17 @@ uint32_t m25p16_pageProgram(uint32_t address, const uint8_t *data, int length) m25p16_setCommandAddress(&command[1], address, isLargeFlash); - m25p16_waitForReady(DEFAULT_TIMEOUT_MILLIS); + if (!m25p16_waitForReady(0)) { + // return same address to indicate timeout + return address; + } m25p16_writeEnable(); busTransferMultiple(busDev, txn, 2); + m25p16_setTimeout(DEFAULT_TIMEOUT_MILLIS); + return address + length; } @@ -322,7 +347,7 @@ int m25p16_readBytes(uint32_t address, uint8_t *buffer, int length) m25p16_setCommandAddress(&command[1], address, isLargeFlash); - if (!m25p16_waitForReady(DEFAULT_TIMEOUT_MILLIS)) { + if (!m25p16_waitForReady(0)) { return 0; } diff --git a/src/main/drivers/io_pca9685.c b/src/main/drivers/io_pca9685.c index 12803c0176..1038496228 100644 --- a/src/main/drivers/io_pca9685.c +++ b/src/main/drivers/io_pca9685.c @@ -5,6 +5,8 @@ #include "build/build_config.h" +#ifdef USE_PWM_DRIVER_PCA9685 + #include "common/time.h" #include "common/maths.h" @@ -141,3 +143,5 @@ bool pca9685Initialize(void) return true; } + +#endif diff --git a/src/main/drivers/osd_symbols.h b/src/main/drivers/osd_symbols.h index 750ac0d8ec..229ba680a2 100644 --- a/src/main/drivers/osd_symbols.h +++ b/src/main/drivers/osd_symbols.h @@ -23,232 +23,220 @@ #ifdef USE_OSD -#define SYM_RSSI 0x01 // 001 Icon RSSI -#define SYM_AH_LEFT 0x02 // 002 Arrow left -#define SYM_AH_RIGHT 0x03 // 003 Arrow right -#define SYM_THR 0x04 // 004 Throttle -#define SYM_AH_DECORATION_UP 0x05 // 005 Arrow up AHI -#define SYM_VOLT 0x06 // 006 V -#define SYM_MAH 0x07 // 007 MAH +#define SYM_RSSI 0x01 // 001 Icon RSSI +#define SYM_LQ 0x02 // 002 LQ +#define SYM_LAT 0x03 // 003 GPS LAT +#define SYM_LON 0x04 // 004 GPS LON +#define SYM_AZIMUTH 0x05 // 005 Azimuth +#define SYM_TELEMETRY_0 0x06 // 006 Antenna tracking telemetry +#define SYM_TELEMETRY_1 0x07 // 007 Antenna tracking telemetry +#define SYM_SAT_L 0x08 // 008 Sats left +#define SYM_SAT_R 0x09 // 009 Sats right +#define SYM_HOME_NEAR 0x0A // 010 Home, near +#define SYM_DEGREES 0x0B // 011 ° heading angle +#define SYM_HEADING 0x0C // 012 Compass Heading symbol +#define SYM_SCALE 0x0D // 013 Map scale +#define SYM_HDP_L 0x0E // 014 HDOP left +#define SYM_HDP_R 0x0F // 015 HDOP right +#define SYM_HOME 0x10 // 016 Home icon +#define SYM_2RSS 0x11 // 017 RSSI 2 +#define SYM_DB 0x12 // 018 dB +#define SYM_DBM 0x13 // 019 dBm +#define SYM_SNR 0x14 // 020 SNR -#define SYM_AH_KM 0x08 // 008 Ah/km -#define SYM_AH_MI 0x09 // 009 Ah/mi -#define SYM_MAH_MI_0 0x0A // 010 mAh/mi left -#define SYM_MAH_MI_1 0x0B // 010 mAh/mi left -#define SYM_LQ 0x0C // 012 LQ +#define SYM_AH_DECORATION_UP 0x15 // 021 Arrow up AHI +#define SYM_AH_DECORATION_DOWN 0x16 // 022 Arrow down AHI +#define SYM_DIRECTION 0x17 // 023 to 030, directional little arrows -#define SYM_TEMP_F 0x0D // 013 °F -#define SYM_TEMP_C 0x0E // 014 °C -#define SYM_FT 0x0F // 015 FT +#define SYM_VOLT 0x1F // 031 VOLTS +#define SYM_MAH 0x99 // 153 mAh +// 0x21 // 033 ASCII ! +#define SYM_AH_KM 0x22 // 034 Ah/km +// 0x23 // 035 ASCII # +#define SYM_AH_MI 0x24 // 036 Ah/mi +// 0x25 // 037 ASCII % +// 0x26 // 038 ASCII & +#define SYM_VTX_POWER 0x27 // 039 VTx Power +// 0x28 // 040 to 062 ASCII +#define SYM_AH_NM 0x3F // 063 Ah/NM +// 0x40 // 064 to 095 ASCII +#define SYM_MAH_NM_0 0x60 // 096 mAh/NM left +#define SYM_MAH_NM_1 0x61 // 097 mAh/NM right +#define SYM_MAH_KM_0 0x6B // 107 MAH/KM left +#define SYM_MAH_KM_1 0x6C // 108 MAH/KM right +#define SYM_MILLIOHM 0x62 // 098 battery impedance Mohm -#define SYM_AH_DECORATION_MIN 0x10 // 016 to 021 Scrolling -#define SYM_AH_DECORATION 0x13 // 019 Scrolling -#define SYM_AH_DECORATION_MAX 0x15 // 021 Scrolling +#define SYM_BATT_FULL 0x63 // 099 Battery full +#define SYM_BATT_5 0x64 // 100 Battery +#define SYM_BATT_4 0x65 // 101 Battery +#define SYM_BATT_3 0x66 // 102 Battery +#define SYM_BATT_2 0x67 // 103 Battery +#define SYM_BATT_1 0x68 // 104 Battery +#define SYM_BATT_EMPTY 0x69 // 105 Battery empty + +#define SYM_AMP 0x6A // 106 AMPS +#define SYM_WH 0x6D // 109 WH +#define SYM_WH_KM 0x6E // 110 WH/KM +#define SYM_WH_MI 0x6F // 111 WH/MI +#define SYM_WH_NM 0x70 // 112 Wh/NM +#define SYM_WATT 0x71 // 113 WATTS +#define SYM_MW 0x72 // 114 mW +#define SYM_KILOWATT 0x73 // 115 kW + +#define SYM_FT 0x74 // 116 FEET +#define SYM_TRIP_DIST 0x75 // 117 Trip distance +#define SYM_TOTAL 0x75 // 117 Total +#define SYM_ALT_M 0x76 // 118 ALT M +#define SYM_ALT_KM 0x77 // 119 ALT KM +#define SYM_ALT_FT 0x78 // 120 ALT FT +#define SYM_ALT_KFT 0x79 // 121 Alt KFT +#define SYM_DIST_M 0x7A // 122 DIS M +// 0x7B // 123 to 125 ASCII +#define SYM_DIST_KM 0x7E // 126 DIST KM +#define SYM_DIST_FT 0x7F // 127 DIST FT +#define SYM_DIST_MI 0x80 // 128 DIST MI +#define SYM_DIST_NM 0x81 // 129 DIST NM +#define SYM_M 0x82 // 130 M +#define SYM_KM 0x83 // 131 KM +#define SYM_MI 0x84 // 132 MI +#define SYM_NM 0x85 // 133 NM + +#define SYM_WIND_HORIZONTAL 0x86 // 134 Air speed horizontal +#define SYM_WIND_VERTICAL 0x87 // 135 Air speed vertical +#define SYM_3D_KMH 0x88 // 136 KM/H 3D +#define SYM_3D_MPH 0x89 // 137 MPH 3D +#define SYM_3D_KT 0x8A // 138 Knots 3D +#define SYM_RPM 0x8B // 139 RPM +#define SYM_AIR 0x8C // 140 Air speed +#define SYM_FTS 0x8D // 141 FT/S +#define SYM_100FTM 0x8E // 142 100 Feet per Min +#define SYM_MS 0x8F // 143 M/S +#define SYM_KMH 0x90 // 144 KM/H +#define SYM_MPH 0x91 // 145 MPH +#define SYM_KT 0x92 // 146 Knots + +#define SYM_MAH_MI_0 0x93 // 147 mAh/mi left +#define SYM_MAH_MI_1 0x94 // 148 mAh/mi right +#define SYM_THR 0x95 // 149 Throttle +#define SYM_TEMP_F 0x96 // 150 °F +#define SYM_TEMP_C 0x97 // 151 °C +// 0x98 // 152 Home point map +#define SYM_BLANK 0x20 // 32 blank (space) +#define SYM_ON_H 0x9A // 154 ON HR +#define SYM_FLY_H 0x9B // 155 FLY HR +#define SYM_ON_M 0x9E // 158 On MIN +#define SYM_FLY_M 0x9F // 159 FL MIN +#define SYM_GLIDESLOPE 0x9C // 156 Glideslope +#define SYM_WAYPOINT 0x9D // 157 Waypoint +#define SYM_CLOCK 0xA0 // 160 Clock + +#define SYM_ZERO_HALF_TRAILING_DOT 0xA1 // 161 to 170 Numbers with trailing dot +#define SYM_ZERO_HALF_LEADING_DOT 0xB1 // 177 to 186 Numbers with leading dot + +#define SYM_AUTO_THR0 0xAB // 171 Auto-throttle left +#define SYM_AUTO_THR1 0xAC // 172 Auto-throttle right +#define SYM_ROLL_LEFT 0xAD // 173 Sym roll left +#define SYM_ROLL_LEVEL 0xAE // 174 Sym roll horizontal +#define SYM_ROLL_RIGHT 0xAF // 175 Sym roll right +#define SYM_PITCH_UP 0xB0 // 176 Pitch up +#define SYM_PITCH_DOWN 0xBB // 187 Pitch down +#define SYM_GFORCE 0xBC // 188 Gforce (all axis) +#define SYM_GFORCE_X 0xBD // 189 Gforce X +#define SYM_GFORCE_Y 0xBE // 190 Gforce Y +#define SYM_GFORCE_Z 0xBF // 191 Gforce Z + +#define SYM_BARO_TEMP 0xC0 // 192 +#define SYM_IMU_TEMP 0xC1 // 193 +#define SYM_TEMP 0xC2 // 194 Thermometer icon +#define SYM_TEMP_SENSOR_FIRST 0xC2 // 194 +#define SYM_ESC_TEMP 0xC3 // 195 +#define SYM_TEMP_SENSOR_LAST 0xC7 // 199 +#define TEMP_SENSOR_SYM_COUNT (SYM_TEMP_SENSOR_LAST - SYM_TEMP_SENSOR_FIRST + 1) + +#define SYM_HEADING_N 0xC8 // 200 Heading Graphic north +#define SYM_HEADING_S 0xC9 // 201 Heading Graphic south +#define SYM_HEADING_E 0xCA // 202 Heading Graphic east +#define SYM_HEADING_W 0xCB // 203 Heading Graphic west +#define SYM_HEADING_DIVIDED_LINE 0xCC // 204 Heading Graphic +#define SYM_HEADING_LINE 0xCD // 205 Heading Graphic +#define SYM_MAX 0xCE // 206 MAX symbol +#define SYM_PROFILE 0xCF // 207 Profile symbol + + +#define SYM_LOGO_START 0x101 // 257 to 280, INAV logo +#define SYM_LOGO_WIDTH 6 +#define SYM_LOGO_HEIGHT 4 + +#define SYM_AH_LEFT 0x12C // 300 Arrow left +#define SYM_AH_RIGHT 0x12D // 301 Arrow right +#define SYM_AH_DECORATION_MIN 0x12E // 302 to 307 Scrolling +#define SYM_AH_DECORATION 0x131 // 305 Scrolling +#define SYM_AH_DECORATION_MAX 0x133 // 307 Scrolling #define SYM_AH_DECORATION_COUNT (SYM_AH_DECORATION_MAX - SYM_AH_DECORATION_MIN + 1) // Scrolling -#define SYM_WIND_HORIZONTAL 0x16 // 022 Air speed horizontal -#define SYM_WIND_VERTICAL 0x17 // 023 Air speed vertical +#define SYM_AH_CH_LEFT 0x13A // 314 Crossair left +#define SYM_AH_CH_RIGHT 0x13B // 315 Crossair right -#define SYM_HEADING_N 0x18 // 024 Heading Graphic north -#define SYM_HEADING_S 0x19 // 025 Heading Graphic south -#define SYM_HEADING_E 0x1A // 026 Heading Graphic east -#define SYM_HEADING_W 0x1B // 027 Heading Graphic west -#define SYM_HEADING_DIVIDED_LINE 0x1C // 028 Heading Graphic -#define SYM_HEADING_LINE 0x1D // 029 Heading Graphic +#define SYM_ARROW_UP 0x13C // 316 Direction arrow 0° +#define SYM_ARROW_2 0x13D // 317 Direction arrow 22.5° +#define SYM_ARROW_3 0x13E // 318 Direction arrow 45° +#define SYM_ARROW_4 0x13F // 319 Direction arrow 67.5° +#define SYM_ARROW_RIGHT 0x140 // 320 Direction arrow 90° +#define SYM_ARROW_6 0x141 // 321 Direction arrow 112.5° +#define SYM_ARROW_7 0x142 // 322 Direction arrow 135° +#define SYM_ARROW_8 0x143 // 323 Direction arrow 157.5° +#define SYM_ARROW_DOWN 0x144 // 324 Direction arrow 180° +#define SYM_ARROW_10 0x145 // 325 Direction arrow 202.5° +#define SYM_ARROW_11 0x146 // 326 Direction arrow 225° +#define SYM_ARROW_12 0x147 // 327 Direction arrow 247.5° +#define SYM_ARROW_LEFT 0x148 // 328 Direction arrow 270° +#define SYM_ARROW_14 0x149 // 329 Direction arrow 292.5° +#define SYM_ARROW_15 0x14A // 330 Direction arrow 315° +#define SYM_ARROW_16 0x14B // 331 Direction arrow 337.5° -#define SYM_SAT_L 0x1E // 030 Sats left -#define SYM_SAT_R 0x1F // 031 Sats right +#define SYM_AH_H_START 0x14C // 332 to 340 Horizontal AHI +#define SYM_AH_V_START 0x15A // 346 to 351 Vertical AHI -#define SYM_BLANK 0x20 // 032 blank (space) +#define SYM_VARIO_UP_2A 0x155 // 341 Vario up up +#define SYM_VARIO_UP_1A 0x156 // 342 Vario up +#define SYM_VARIO_DOWN_1A 0x157 // 343 Vario down +#define SYM_VARIO_DOWN_2A 0x158 // 344 Vario down down +#define SYM_ALT 0x159 // 345 ALT -// 0x21 // 033 ASCII ! +#define SYM_HUD_SIGNAL_0 0x160 // 352 Hud signal icon Lost +#define SYM_HUD_SIGNAL_1 0x161 // 353 Hud signal icon 25% +#define SYM_HUD_SIGNAL_2 0x162 // 354 Hud signal icon 50% +#define SYM_HUD_SIGNAL_3 0x163 // 355 Hud signal icon 75% +#define SYM_HUD_SIGNAL_4 0x164 // 356 Hud signal icon 100% -#define SYM_TRIP_DIST 0x22 // 034 Trip distance -#define SYM_TOTAL 0x22 // 034 Total +#define SYM_HOME_DIST 0x165 // 357 DIST +#define SYM_AH_CH_CENTER 0x166 // 358 Crossair center -// 0x23 // 035 ASCII # +#define SYM_AH_CH_TYPE3 0x190 // 400 to 402, crosshair 3 +#define SYM_AH_CH_TYPE4 0x193 // 403 to 405, crosshair 4 +#define SYM_AH_CH_TYPE5 0x196 // 406 to 408, crosshair 5 +#define SYM_AH_CH_TYPE6 0x199 // 409 to 411, crosshair 6 +#define SYM_AH_CH_TYPE7 0x19C // 412 to 414, crosshair 7 +#define SYM_AH_CH_TYPE8 0x19F // 415 to 417, crosshair 8 -#define SYM_AH_DECORATION_DOWN 0x24 // 036 Arrow down AHI - -// 0x25 // 037 ASCII % - -#define SYM_AH_CH_LEFT 0x26 // 038 Crossair left -#define SYM_AH_CH_RIGHT 0x27 // 039 Crossair right - -// 0x28 // 040 to 062 ASCII - -#define SYM_MILLIOHM 0x3F // 063 battery impedance Mohm - -// 0x40 // 064 to 095 ASCII - -#define SYM_ARROW_UP 0x60 // 096 Direction arrow 0° -#define SYM_ARROW_2 0x61 // 097 Direction arrow 22.5° -#define SYM_ARROW_3 0x62 // 098 Direction arrow 45° -#define SYM_ARROW_4 0x63 // 099 Direction arrow 67.5° -#define SYM_ARROW_RIGHT 0x64 // 100 Direction arrow 90° -#define SYM_ARROW_6 0x65 // 101 Direction arrow 112.5° -#define SYM_ARROW_7 0x66 // 102 Direction arrow 135° -#define SYM_ARROW_8 0x67 // 103 Direction arrow 157.5° -#define SYM_ARROW_DOWN 0x68 // 104 Direction arrow 180° -#define SYM_ARROW_10 0x69 // 105 Direction arrow 202.5° -#define SYM_ARROW_11 0x6A // 106 Direction arrow 225° -#define SYM_ARROW_12 0x6B // 107 Direction arrow 247.5° -#define SYM_ARROW_LEFT 0x6C // 108 Direction arrow 270° -#define SYM_ARROW_14 0x6D // 109 Direction arrow 292.5° -#define SYM_ARROW_15 0x6E // 110 Direction arrow 315° -#define SYM_ARROW_16 0x6F // 111 Direction arrow 337.5° - -#define SYM_ON_H 0x70 // 112 ON HR -#define SYM_FLY_H 0x71 // 113 FLY HR - -#define SYM_DIRECTION 0x72 // 114 to 121, directional little arrows - -#define SYM_HOME_NEAR 0x7A // 122 Home, near - -// 0x7B // 123 to 125 ASCII - -#define SYM_AH_CH_CENTER 0x7E // 126 Crossair center - -#define SYM_GLIDESLOPE 0x7F // 127 Glideslope - -#define SYM_AH_H_START 0x80 // 128 to 136 Horizontal AHI - -#define SYM_3D_KMH 0x89 // 137 KM/H 3D -#define SYM_3D_MPH 0x8A // 138 MPH 3D - -#define SYM_RPM 0x8B // 139 RPM -#define SYM_WAYPOINT 0x8C // 140 Waypoint -#define SYM_AZIMUTH 0x8D // 141 Azimuth - -#define SYM_TELEMETRY_0 0x8E // 142 Antenna tracking telemetry -#define SYM_TELEMETRY_1 0x8F // 143 Antenna tracking telemetry - -#define SYM_BATT_FULL 0x90 // 144 Battery full -#define SYM_BATT_5 0x91 // 145 Battery -#define SYM_BATT_4 0x92 // 146 Battery -#define SYM_BATT_3 0x93 // 147 Battery -#define SYM_BATT_2 0x94 // 148 Battery -#define SYM_BATT_1 0x95 // 149 Battery -#define SYM_BATT_EMPTY 0x96 // 150 Battery empty - -#define SYM_AIR 0x97 // 151 Air speed -// 0x98 // 152 Home point map -#define SYM_FTS 0x99 // 153 FT/S -#define SYM_AMP 0x9A // 154 A -#define SYM_ON_M 0x9B // 155 On MN -#define SYM_FLY_M 0x9C // 156 FL MN -#define SYM_MAH_KM_0 0x9D // 157 MAH/KM left -#define SYM_MAH_KM_1 0x9E // 158 MAH/KM right -#define SYM_MS 0x9F // 159 M/S -#define SYM_HOME_DIST 0xA0 // 160 DIS -#define SYM_KMH 0xA1 // 161 KM/H - -#define SYM_VARIO_UP_2A 0xA2 // 162 Vario up up -#define SYM_VARIO_UP_1A 0xA3 // 163 Vario up -#define SYM_VARIO_DOWN_1A 0xA4 // 164 Vario down -#define SYM_VARIO_DOWN_2A 0xA5 // 165 Vario down down - -#define SYM_LAT 0xA6 // 166 GPS LAT -#define SYM_LON 0xA7 // 167 GPS LON -#define SYM_DEGREES 0xA8 // 168 ° heading angle -#define SYM_HEADING 0xA9 // 169 Compass Heading symbol -#define SYM_ALT 0xAA // 170 ALT -#define SYM_WH 0xAB // 171 WH -#define SYM_WH_KM 0xAC // 172 WH/KM -#define SYM_WH_MI 0xAD // 173 WH/MI -#define SYM_WATT 0xAE // 174 W -#define SYM_SCALE 0xAF // 175 Map scale -#define SYM_MPH 0xB0 // 176 MPH -#define SYM_ALT_M 0xB1 // 177 ALT M -#define SYM_ALT_KM 0xB2 // 178 ALT KM -#define SYM_ALT_FT 0xB3 // 179 ALT FT -#define SYM_ALT_KFT 0xB4 // 180 DIS KFT -#define SYM_DIST_M 0xB5 // 181 DIS M -#define SYM_DIST_KM 0xB6 // 182 DIM KM -#define SYM_DIST_FT 0xB7 // 183 DIS FT -#define SYM_DIST_MI 0xB8 // 184 DIS MI -#define SYM_M 0xB9 // 185 M -#define SYM_KM 0xBA // 186 KM -#define SYM_MI 0xBB // 187 MI - -#define SYM_CLOCK 0xBC // 188 Clock -#define SYM_HDP_L 0xBD // 189 HDOP left -#define SYM_HDP_R 0xBE // 190 HDOP right -#define SYM_HOME 0xBF // 191 Home icon - -#define SYM_ZERO_HALF_TRAILING_DOT 0xC0 // 192 to 201 Numbers with trailing dot - -#define SYM_AUTO_THR0 0xCA // 202 Auto-throttle left -#define SYM_AUTO_THR1 0xCB // 203 Auto-throttle right - -#define SYM_ROLL_LEFT 0xCC // 204 Sym roll left -#define SYM_ROLL_LEVEL 0xCD // 205 Sym roll horizontal -#define SYM_ROLL_RIGHT 0xCE // 206 Sym roll right -#define SYM_PITCH_UP 0xCF // 207 Pitch up - -#define SYM_ZERO_HALF_LEADING_DOT 0xD0 // 208 to 217 Numbers with leading dot - -#define SYM_AH_CH_AIRCRAFT0 0xDA // 218 Crossair aircraft left -#define SYM_AH_CH_AIRCRAFT1 0xDB // 219 Crossair aircraft -#define SYM_AH_CH_AIRCRAFT2 0xDC // 220 Crossair aircraft center -#define SYM_AH_CH_AIRCRAFT3 0xDD // 221 Crossair aircraft -#define SYM_AH_CH_AIRCRAFT4 0xDE // 222 Crossair aircraft right - -#define SYM_PITCH_DOWN 0xDF // 223 Pitch down - -#define SYM_AH_V_START 0xE0 // 224 to 229 Vertical AHI - -#define SYM_GFORCE 0xE6 // 230 Gforce (all axis) -#define SYM_GFORCE_X 0xE7 // 231 Gforce X -#define SYM_GFORCE_Y 0xE8 // 232 Gforce Y -#define SYM_GFORCE_Z 0xE9 // 233 Gforce Z - -#define SYM_BARO_TEMP 0xF0 // 240 -#define SYM_IMU_TEMP 0xF1 // 241 -#define SYM_TEMP 0xF2 // 242 -#define SYM_ESC_TEMP 0xF3 // 243 - -#define SYM_TEMP_SENSOR_FIRST 0xF2 // 242 -#define SYM_TEMP_SENSOR_LAST 0xF7 // 247 -#define TEMP_SENSOR_SYM_COUNT (SYM_TEMP_SENSOR_LAST - SYM_TEMP_SENSOR_FIRST + 1) - -#define SYM_HUD_SIGNAL_0 0xF8 // 248 Hud signal icon Lost -#define SYM_HUD_SIGNAL_1 0xF9 // 249 Hud signal icon 25% -#define SYM_HUD_SIGNAL_2 0xFA // 250 Hud signal icon 50% -#define SYM_HUD_SIGNAL_3 0xFB // 251 Hud signal icon 75% -#define SYM_HUD_SIGNAL_4 0xFC // 252 Hud signal icon 100% - -#define SYM_LOGO_START 0x101 // 257 to 280, INAV logo -#define SYM_LOGO_WIDTH 6 -#define SYM_LOGO_HEIGHT 4 - -#define SYM_AH_CH_TYPE3 0x190 // 400 to 402, crosshair 3 -#define SYM_AH_CH_TYPE4 0x193 // 403 to 405, crosshair 4 -#define SYM_AH_CH_TYPE5 0x196 // 406 to 408, crosshair 5 -#define SYM_AH_CH_TYPE6 0x199 // 409 to 411, crosshair 6 -#define SYM_AH_CH_TYPE7 0x19C // 412 to 414, crosshair 7 - -#define SYM_HUD_ARROWS_L1 0x1A2 // 418 1 arrow left -#define SYM_HUD_ARROWS_L2 0x1A3 // 419 2 arrows left -#define SYM_HUD_ARROWS_L3 0x1A4 // 420 3 arrows left -#define SYM_HUD_ARROWS_R1 0x1A5 // 421 1 arrow right -#define SYM_HUD_ARROWS_R2 0x1A6 // 422 2 arrows right -#define SYM_HUD_ARROWS_R3 0x1A7 // 423 3 arrows right -#define SYM_HUD_ARROWS_U1 0x1A8 // 424 1 arrow up -#define SYM_HUD_ARROWS_U2 0x1A9 // 425 2 arrows up -#define SYM_HUD_ARROWS_U3 0x1AA // 426 3 arrows up -#define SYM_HUD_ARROWS_D1 0x1AB // 427 1 arrow down -#define SYM_HUD_ARROWS_D2 0x1AC // 428 2 arrows down -#define SYM_HUD_ARROWS_D3 0x1AD // 429 3 arrows down - -#define SYM_2RSS 0xEA // RSSI 2 -#define SYM_DB 0xEB // dB -#define SYM_DBM 0xEC // dBm -#define SYM_SNR 0xEE // SNR -#define SYM_MW 0xED // mW - -#define SYM_KILOWATT 0xEF // 239 kW +#define SYM_AH_CH_AIRCRAFT0 0x1A2 // 418 Crossair aircraft left +#define SYM_AH_CH_AIRCRAFT1 0x1A3 // 419 Crossair aircraft +#define SYM_AH_CH_AIRCRAFT2 0x1A4 // 420 Crossair aircraft center +#define SYM_AH_CH_AIRCRAFT3 0x1A5 // 421 Crossair aircraft +#define SYM_AH_CH_AIRCRAFT4 0x1A6 // 422 Crossair aircraft RIGHT +#define SYM_HUD_ARROWS_L1 0x1AE // 430 1 arrow left +#define SYM_HUD_ARROWS_L2 0x1AF // 431 2 arrows left +#define SYM_HUD_ARROWS_L3 0x1B0 // 432 3 arrows left +#define SYM_HUD_ARROWS_R1 0x1B1 // 433 1 arrow right +#define SYM_HUD_ARROWS_R2 0x1B2 // 434 2 arrows right +#define SYM_HUD_ARROWS_R3 0x1B3 // 435 3 arrows right +#define SYM_HUD_ARROWS_U1 0x1B4 // 436 1 arrow up +#define SYM_HUD_ARROWS_U2 0x1B5 // 437 2 arrows up +#define SYM_HUD_ARROWS_U3 0x1B6 // 438 3 arrows up +#define SYM_HUD_ARROWS_D1 0x1B7 // 439 1 arrow down +#define SYM_HUD_ARROWS_D2 0x1B8 // 440 2 arrows down +#define SYM_HUD_ARROWS_D3 0x1B9 // 441 3 arrows down #else diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index a7a27b4ede..e8e7c0ec9e 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -36,7 +36,6 @@ #include "drivers/pwm_mapping.h" #include "drivers/serial.h" #include "drivers/serial_uart.h" -//#include "drivers/rx_pwm.h" #include "sensors/rangefinder.h" @@ -67,16 +66,13 @@ static const char * pwmInitErrorMsg[] = { }; static const motorProtocolProperties_t motorProtocolProperties[] = { - [PWM_TYPE_STANDARD] = { .usesHwTimer = true, .isDSHOT = false, .isSerialShot = false }, - [PWM_TYPE_ONESHOT125] = { .usesHwTimer = true, .isDSHOT = false, .isSerialShot = false }, - [PWM_TYPE_ONESHOT42] = { .usesHwTimer = true, .isDSHOT = false, .isSerialShot = false }, - [PWM_TYPE_MULTISHOT] = { .usesHwTimer = true, .isDSHOT = false, .isSerialShot = false }, - [PWM_TYPE_BRUSHED] = { .usesHwTimer = true, .isDSHOT = false, .isSerialShot = false }, - [PWM_TYPE_DSHOT150] = { .usesHwTimer = true, .isDSHOT = true, .isSerialShot = false }, - [PWM_TYPE_DSHOT300] = { .usesHwTimer = true, .isDSHOT = true, .isSerialShot = false }, - [PWM_TYPE_DSHOT600] = { .usesHwTimer = true, .isDSHOT = true, .isSerialShot = false }, - [PWM_TYPE_DSHOT1200] = { .usesHwTimer = true, .isDSHOT = true, .isSerialShot = false }, - [PWM_TYPE_SERIALSHOT] = { .usesHwTimer = false, .isDSHOT = false, .isSerialShot = true }, + [PWM_TYPE_STANDARD] = { .usesHwTimer = true, .isDSHOT = false }, + [PWM_TYPE_ONESHOT125] = { .usesHwTimer = true, .isDSHOT = false }, + [PWM_TYPE_MULTISHOT] = { .usesHwTimer = true, .isDSHOT = false }, + [PWM_TYPE_BRUSHED] = { .usesHwTimer = true, .isDSHOT = false }, + [PWM_TYPE_DSHOT150] = { .usesHwTimer = true, .isDSHOT = true }, + [PWM_TYPE_DSHOT300] = { .usesHwTimer = true, .isDSHOT = true }, + [PWM_TYPE_DSHOT600] = { .usesHwTimer = true, .isDSHOT = true }, }; pwmInitError_e getPwmInitError(void) @@ -199,17 +195,6 @@ static bool checkPwmTimerConflicts(const timerHardware_t *timHw) #endif #endif - -#ifdef USE_RANGEFINDER_HCSR04 - // HC-SR04 has a dedicated connection to FC and require two pins - if (rangefinderConfig()->rangefinder_hardware == RANGEFINDER_HCSR04) { - const rangefinderHardwarePins_t *rangefinderHardwarePins = rangefinderGetHardwarePins(); - if (rangefinderHardwarePins && (timHw->tag == rangefinderHardwarePins->triggerTag || timHw->tag == rangefinderHardwarePins->echoTag)) { - return true; - } - } -#endif - return false; } diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index 7efbf39431..51ec456a77 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -39,14 +39,11 @@ typedef enum { PWM_TYPE_STANDARD = 0, PWM_TYPE_ONESHOT125, - PWM_TYPE_ONESHOT42, PWM_TYPE_MULTISHOT, PWM_TYPE_BRUSHED, PWM_TYPE_DSHOT150, PWM_TYPE_DSHOT300, PWM_TYPE_DSHOT600, - PWM_TYPE_DSHOT1200, - PWM_TYPE_SERIALSHOT, } motorPwmProtocolTypes_e; typedef enum { @@ -73,7 +70,6 @@ typedef struct rangefinderIOConfig_s { typedef struct { bool usesHwTimer; bool isDSHOT; - bool isSerialShot; } motorProtocolProperties_t; bool pwmMotorAndServoInit(void); diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 620c25a22a..f084c19a21 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -37,7 +37,6 @@ FILE_COMPILE_FOR_SPEED #include "drivers/io_pca9685.h" #include "io/pwmdriver_i2c.h" -#include "io/esc_serialshot.h" #include "io/servo_sbus.h" #include "sensors/esc_sensor.h" @@ -50,7 +49,6 @@ FILE_COMPILE_FOR_SPEED #define MULTISHOT_20US_MULT (MULTISHOT_TIMER_HZ * 20 / 1000000.0f / 1000.0f) #ifdef USE_DSHOT -#define MOTOR_DSHOT1200_HZ 24000000 #define MOTOR_DSHOT600_HZ 12000000 #define MOTOR_DSHOT300_HZ 6000000 #define MOTOR_DSHOT150_HZ 3000000 @@ -100,16 +98,14 @@ static pwmWriteFuncPtr motorWritePtr = NULL; // Function to write val static pwmOutputPort_t * servos[MAX_SERVOS]; static pwmWriteFuncPtr servoWritePtr = NULL; // Function to write value to motors -#if defined(USE_DSHOT) || defined(USE_SERIALSHOT) +#if defined(USE_DSHOT) static timeUs_t digitalMotorUpdateIntervalUs = 0; static timeUs_t digitalMotorLastUpdateUs; #endif -#ifdef BEEPER_PWM static pwmOutputPort_t beeperPwmPort; static pwmOutputPort_t *beeperPwm; static uint16_t beeperFrequency = 0; -#endif static uint8_t allocatedOutputPortCount = 0; @@ -150,7 +146,7 @@ static pwmOutputPort_t *pwmOutAllocatePort(void) return p; } -static pwmOutputPort_t *pwmOutConfigMotor(const timerHardware_t *timHw, uint32_t hz, uint16_t period, uint16_t value, bool enableOutput) +static pwmOutputPort_t *pwmOutConfig(const timerHardware_t *timHw, resourceOwner_e owner, uint32_t hz, uint16_t period, uint16_t value, bool enableOutput) { // Attempt to allocate TCH TCH_t * tch = timerGetTCH(timHw); @@ -165,7 +161,7 @@ static pwmOutputPort_t *pwmOutConfigMotor(const timerHardware_t *timHw, uint32_t } const IO_t io = IOGetByTag(timHw->tag); - IOInit(io, OWNER_MOTOR, RESOURCE_OUTPUT, allocatedOutputPortCount); + IOInit(io, owner, RESOURCE_OUTPUT, allocatedOutputPortCount); if (enableOutput) { IOConfigGPIOAF(io, IOCFG_AF_PP, timHw->alternateFunction); @@ -232,7 +228,7 @@ static pwmOutputPort_t * motorConfigPwm(const timerHardware_t *timerHardware, fl const uint32_t timerHz = baseClockHz / prescaler; const uint32_t period = timerHz / motorPwmRateHz; - pwmOutputPort_t * port = pwmOutConfigMotor(timerHardware, timerHz, period, 0, enableOutput); + pwmOutputPort_t * port = pwmOutConfig(timerHardware, OWNER_MOTOR, timerHz, period, 0, enableOutput); if (port) { port->pulseScale = ((sLen == 0) ? period : (sLen * timerHz)) / 1000.0f; @@ -247,8 +243,6 @@ static pwmOutputPort_t * motorConfigPwm(const timerHardware_t *timerHardware, fl uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType) { switch (pwmProtocolType) { - case(PWM_TYPE_DSHOT1200): - return MOTOR_DSHOT1200_HZ; case(PWM_TYPE_DSHOT600): return MOTOR_DSHOT600_HZ; case(PWM_TYPE_DSHOT300): @@ -262,7 +256,7 @@ uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType) static pwmOutputPort_t * motorConfigDshot(const timerHardware_t * timerHardware, uint32_t dshotHz, bool enableOutput) { // Try allocating new port - pwmOutputPort_t * port = pwmOutConfigMotor(timerHardware, dshotHz, DSHOT_MOTOR_BITLENGTH, 0, enableOutput); + pwmOutputPort_t * port = pwmOutConfig(timerHardware, OWNER_MOTOR, dshotHz, DSHOT_MOTOR_BITLENGTH, 0, enableOutput); if (!port) { return NULL; @@ -306,7 +300,7 @@ static uint16_t prepareDshotPacket(const uint16_t value, bool requestTelemetry) } #endif -#if defined(USE_DSHOT) || defined(USE_SERIALSHOT) +#if defined(USE_DSHOT) static void motorConfigDigitalUpdateInterval(uint16_t motorPwmRateHz) { digitalMotorUpdateIntervalUs = 1000000 / motorPwmRateHz; @@ -327,14 +321,9 @@ bool isMotorProtocolDshot(void) return getMotorProtocolProperties(initMotorProtocol)->isDSHOT; } -bool isMotorProtocolSerialShot(void) -{ - return getMotorProtocolProperties(initMotorProtocol)->isSerialShot; -} - bool isMotorProtocolDigital(void) { - return isMotorProtocolDshot() || isMotorProtocolSerialShot(); + return isMotorProtocolDshot(); } void pwmRequestMotorTelemetry(int motorIndex) @@ -368,7 +357,7 @@ static int getDShotCommandRepeats(dshotCommands_e cmd) { switch (cmd) { case DSHOT_CMD_SPIN_DIRECTION_NORMAL: case DSHOT_CMD_SPIN_DIRECTION_REVERSED: - repeats = 6; + repeats = 10; break; default: break; @@ -443,16 +432,6 @@ void pwmCompleteMotorUpdate(void) { } } #endif - -#ifdef USE_SERIALSHOT - if (isMotorProtocolSerialShot()) { - for (int index = 0; index < motorCount; index++) { - serialshotUpdateMotor(index, motors[index].value); - } - - serialshotSendUpdate(); - } -#endif } #else // digital motor protocol @@ -483,13 +462,11 @@ void pwmMotorPreconfigure(void) case PWM_TYPE_STANDARD: case PWM_TYPE_BRUSHED: case PWM_TYPE_ONESHOT125: - case PWM_TYPE_ONESHOT42: case PWM_TYPE_MULTISHOT: motorWritePtr = pwmWriteStandard; break; #ifdef USE_DSHOT - case PWM_TYPE_DSHOT1200: case PWM_TYPE_DSHOT600: case PWM_TYPE_DSHOT300: case PWM_TYPE_DSHOT150: @@ -497,15 +474,6 @@ void pwmMotorPreconfigure(void) motorWritePtr = pwmWriteDigital; break; #endif - -#ifdef USE_SERIALSHOT - case PWM_TYPE_SERIALSHOT: - // Kick off SerialShot driver initalization - serialshotInitialize(); - motorConfigDigitalUpdateInterval(motorConfig()->motorPwmRate); - motorWritePtr = pwmWriteDigital; - break; -#endif } } @@ -520,16 +488,11 @@ bool pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, bo motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 125e-6f, 125e-6f, motorConfig()->motorPwmRate, enableOutput); break; - case PWM_TYPE_ONESHOT42: - motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 42e-6f, 42e-6f, motorConfig()->motorPwmRate, enableOutput); - break; - case PWM_TYPE_MULTISHOT: motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 5e-6f, 20e-6f, motorConfig()->motorPwmRate, enableOutput); break; #ifdef USE_DSHOT - case PWM_TYPE_DSHOT1200: case PWM_TYPE_DSHOT600: case PWM_TYPE_DSHOT300: case PWM_TYPE_DSHOT150: @@ -617,7 +580,7 @@ void pwmServoPreconfigure(void) bool pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse, bool enableOutput) { - pwmOutputPort_t * port = pwmOutConfigMotor(timerHardware, PWM_TIMER_HZ, PWM_TIMER_HZ / servoPwmRate, servoCenterPulse, enableOutput); + pwmOutputPort_t * port = pwmOutConfig(timerHardware, OWNER_SERVO, PWM_TIMER_HZ, PWM_TIMER_HZ / servoPwmRate, servoCenterPulse, enableOutput); if (port) { servos[servoIndex] = port; @@ -634,7 +597,6 @@ void pwmWriteServo(uint8_t index, uint16_t value) } } -#ifdef BEEPER_PWM void pwmWriteBeeper(bool onoffBeep) { if (beeperPwm == NULL) @@ -666,4 +628,3 @@ void beeperPwmInit(ioTag_t tag, uint16_t frequency) pwmOutConfigTimer(beeperPwm, tch, PWM_TIMER_HZ, 1000000 / beeperFrequency, (1000000 / beeperFrequency) / 2); } } -#endif diff --git a/src/main/drivers/rangefinder/rangefinder.h b/src/main/drivers/rangefinder/rangefinder.h index 9efcf0dad0..797a29a6c0 100644 --- a/src/main/drivers/rangefinder/rangefinder.h +++ b/src/main/drivers/rangefinder/rangefinder.h @@ -27,13 +27,6 @@ struct rangefinderDev_s; -typedef struct rangefinderHardwarePins_s { - ioTag_t triggerTag; - ioTag_t echoTag; -} rangefinderHardwarePins_t; - -struct rangefinderDev_s; - typedef void (*rangefinderOpInitFuncPtr)(struct rangefinderDev_s * dev); typedef void (*rangefinderOpStartFuncPtr)(struct rangefinderDev_s * dev); typedef int32_t (*rangefinderOpReadFuncPtr)(struct rangefinderDev_s * dev); diff --git a/src/main/drivers/rangefinder/rangefinder_hcsr04.c b/src/main/drivers/rangefinder/rangefinder_hcsr04.c deleted file mode 100644 index fe935473bf..0000000000 --- a/src/main/drivers/rangefinder/rangefinder_hcsr04.c +++ /dev/null @@ -1,221 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#include -#include - -#include - -#if defined(USE_RANGEFINDER_HCSR04) - -#include "build/build_config.h" - -#include "common/time.h" - -#include "drivers/time.h" -#include "drivers/exti.h" -#include "drivers/io.h" -#include "drivers/nvic.h" -#include "drivers/rcc.h" - -#include "drivers/rangefinder/rangefinder.h" -#include "drivers/rangefinder/rangefinder_hcsr04.h" - -#define HCSR04_MAX_RANGE_CM 400 // 4m, from HC-SR04 spec sheet -#define HCSR04_DETECTION_CONE_DECIDEGREES 300 // recommended cone angle30 degrees, from HC-SR04 spec sheet -#define HCSR04_DETECTION_CONE_EXTENDED_DECIDEGREES 450 // in practice 45 degrees seems to work well - - -/* HC-SR04 consists of ultrasonic transmitter, receiver, and control circuits. - * When triggered it sends out a series of 40KHz ultrasonic pulses and receives - * echo from an object. The distance between the unit and the object is calculated - * by measuring the traveling time of sound and output it as the width of a TTL pulse. - * - * *** Warning: HC-SR04 operates at +5V *** - * - */ - -static volatile timeDelta_t hcsr04SonarPulseTravelTime = 0; -static volatile timeMs_t lastMeasurementReceivedAt; -static volatile int32_t lastCalculatedDistance = RANGEFINDER_OUT_OF_RANGE; -static timeMs_t lastMeasurementStartedAt = 0; - -#ifdef USE_EXTI -static extiCallbackRec_t hcsr04_extiCallbackRec; -#endif - -static IO_t echoIO; -static IO_t triggerIO; - -#if !defined(UNIT_TEST) -void hcsr04_extiHandler(extiCallbackRec_t* cb) -{ - static timeUs_t timing_start; - UNUSED(cb); - - if (IORead(echoIO) != 0) { - timing_start = micros(); - } else { - const timeUs_t timing_stop = micros(); - if (timing_stop > timing_start) { - lastMeasurementReceivedAt = millis(); - hcsr04SonarPulseTravelTime = timing_stop - timing_start; - } - } -} -#endif - -void hcsr04_init(rangefinderDev_t *dev) -{ - UNUSED(dev); -} - -#define HCSR04_MinimumFiringIntervalMs 60 - -/* - * Start a range reading - * Called periodically by the scheduler - * Measurement reading is done asynchronously, using interrupt - */ -void hcsr04_start_reading(void) -{ -#if !defined(UNIT_TEST) -#ifdef RANGEFINDER_HCSR04_TRIG_INVERTED - IOLo(triggerIO); - delayMicroseconds(11); - IOHi(triggerIO); -#else - IOHi(triggerIO); - delayMicroseconds(11); - IOLo(triggerIO); -#endif -#endif -} - -void hcsr04_update(rangefinderDev_t *dev) -{ - UNUSED(dev); - const timeMs_t timeNowMs = millis(); - - // the firing interval of the trigger signal should be greater than 60ms - // to avoid interference between consecutive measurements - if (timeNowMs > lastMeasurementStartedAt + HCSR04_MinimumFiringIntervalMs) { - // We should have a valid measurement within 60ms of trigger - if ((lastMeasurementReceivedAt - lastMeasurementStartedAt) <= HCSR04_MinimumFiringIntervalMs) { - // The speed of sound is 340 m/s or approx. 29 microseconds per centimeter. - // The ping travels out and back, so to find the distance of the - // object we take half of the distance traveled. - // 340 m/s = 0.034 cm/microsecond = 29.41176471 *2 = 58.82352941 rounded to 59 - - lastCalculatedDistance = hcsr04SonarPulseTravelTime / 59; - if (lastCalculatedDistance > HCSR04_MAX_RANGE_CM) { - lastCalculatedDistance = RANGEFINDER_OUT_OF_RANGE; - } - } - else { - // No measurement within reasonable time - indicate failure - lastCalculatedDistance = RANGEFINDER_HARDWARE_FAILURE; - } - - // Trigger a new measurement - lastMeasurementStartedAt = timeNowMs; - hcsr04_start_reading(); - } - -} - -/** - * Get the distance that was measured by the last pulse, in centimeters. - */ -int32_t hcsr04_get_distance(rangefinderDev_t *dev) -{ - UNUSED(dev); - return lastCalculatedDistance; -} - -bool hcsr04Detect(rangefinderDev_t *dev, const rangefinderHardwarePins_t * rangefinderHardwarePins) -{ - bool detected = false; - -#if defined(STM32F3) || defined(STM32F4) - RCC_ClockCmd(RCC_APB2(SYSCFG), ENABLE); -#endif - - triggerIO = IOGetByTag(rangefinderHardwarePins->triggerTag); - echoIO = IOGetByTag(rangefinderHardwarePins->echoTag); - - if (IOGetOwner(triggerIO) != OWNER_FREE) { - return false; - } - - if (IOGetOwner(echoIO) != OWNER_FREE) { - return false; - } - - // trigger pin - IOInit(triggerIO, OWNER_RANGEFINDER, RESOURCE_OUTPUT, 0); - IOConfigGPIO(triggerIO, IOCFG_OUT_PP); - IOLo(triggerIO); - delay(100); - - // echo pin - IOInit(echoIO, OWNER_RANGEFINDER, RESOURCE_INPUT, 0); - IOConfigGPIO(echoIO, IOCFG_IN_FLOATING); - - // HC-SR04 echo line should be low by default and should return a response pulse when triggered - if (IORead(echoIO) == false) { - for (int i = 0; i < 5 && !detected; i++) { - timeMs_t requestTime = millis(); - hcsr04_start_reading(); - - while ((millis() - requestTime) < HCSR04_MinimumFiringIntervalMs) { - if (IORead(echoIO) == true) { - detected = true; - break; - } - } - } - } - - if (detected) { - // Hardware detected - configure the driver -#ifdef USE_EXTI - EXTIHandlerInit(&hcsr04_extiCallbackRec, hcsr04_extiHandler); - EXTIConfig(echoIO, &hcsr04_extiCallbackRec, NVIC_PRIO_SONAR_EXTI, EXTI_Trigger_Rising_Falling); // TODO - priority! - EXTIEnable(echoIO, true); -#endif - - dev->delayMs = 100; - dev->maxRangeCm = HCSR04_MAX_RANGE_CM; - dev->detectionConeDeciDegrees = HCSR04_DETECTION_CONE_DECIDEGREES; - dev->detectionConeExtendedDeciDegrees = HCSR04_DETECTION_CONE_EXTENDED_DECIDEGREES; - - dev->init = &hcsr04_init; - dev->update = &hcsr04_update; - dev->read = &hcsr04_get_distance; - - return true; - } - else { - // Not detected - free resources - IORelease(triggerIO); - IORelease(echoIO); - return false; - } -} - -#endif diff --git a/src/main/drivers/rangefinder/rangefinder_hcsr04_i2c.c b/src/main/drivers/rangefinder/rangefinder_hcsr04_i2c.c deleted file mode 100644 index a38144c28d..0000000000 --- a/src/main/drivers/rangefinder/rangefinder_hcsr04_i2c.c +++ /dev/null @@ -1,125 +0,0 @@ -/* - * This file is part of INAV. - * - * INAV is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * INAV is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with INAV. If not, see . - */ - -#include -#include - -#include "platform.h" - -#if defined(USE_RANGEFINDER) && defined(USE_RANGEFINDER_HCSR04_I2C) - -#include "build/build_config.h" - -#include "drivers/time.h" -#include "drivers/bus_i2c.h" - -#include "drivers/rangefinder/rangefinder.h" -#include "drivers/rangefinder/rangefinder_hcsr04_i2c.h" - -#include "build/debug.h" - -#define HCSR04_I2C_MAX_RANGE_CM 400 -#define HCSR04_I2C_DETECTION_CONE_DECIDEGREES 300 -#define HCSR04_I2C_DETECTION_CONE_EXTENDED_DECIDEGREES 450 - -#define HCSR04_I2C_Address 0x14 - -#define HCSR04_I2C_REGISTRY_STATUS 0x00 -#define HCSR04_I2C_REGISTRY_DISTANCE_HIGH 0x01 -#define HCSR04_I2C_REGISTRY_DISTANCE_LOW 0x02 - -volatile int32_t hcsr04i2cMeasurementCm = RANGEFINDER_OUT_OF_RANGE; -static bool isHcsr04i2cResponding = false; - -static void hcsr04i2cInit(rangefinderDev_t *rangefinder) -{ - UNUSED(rangefinder); -} - -void hcsr04i2cUpdate(rangefinderDev_t *rangefinder) -{ - uint8_t response[3]; - - isHcsr04i2cResponding = busReadBuf(rangefinder->busDev, HCSR04_I2C_REGISTRY_STATUS, response, 3); - - if (!isHcsr04i2cResponding) { - hcsr04i2cMeasurementCm = RANGEFINDER_HARDWARE_FAILURE; - return; - } - - if (response[HCSR04_I2C_REGISTRY_STATUS] == 0) { - - hcsr04i2cMeasurementCm = - (int32_t)((int32_t)response[HCSR04_I2C_REGISTRY_DISTANCE_HIGH] << 8) + - response[HCSR04_I2C_REGISTRY_DISTANCE_LOW]; - - } else { - /* - * Rangefinder is reporting out-of-range situation - */ - hcsr04i2cMeasurementCm = RANGEFINDER_OUT_OF_RANGE; - } -} - -/** - * Get the distance that was measured by the last pulse, in centimeters. - */ -int32_t hcsr04i2cGetDistance(rangefinderDev_t *rangefinder) -{ - UNUSED(rangefinder); - return hcsr04i2cMeasurementCm; -} - -static bool deviceDetect(busDevice_t * busDev) -{ - for (int retry = 0; retry < 5; retry++) { - uint8_t inquiryResult; - delay(150); - - bool ack = busRead(busDev, HCSR04_I2C_REGISTRY_STATUS, &inquiryResult); - if (ack) { - return true; - } - }; - - return false; -} - -bool hcsr04i2c0Detect(rangefinderDev_t *rangefinder) -{ - rangefinder->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_HCSR04_I2C, 0, OWNER_RANGEFINDER); - if (rangefinder->busDev == NULL) { - return false; - } - - if (!deviceDetect(rangefinder->busDev)) { - busDeviceDeInit(rangefinder->busDev); - return false; - } - - rangefinder->delayMs = RANGEFINDER_HCSR04_i2C_TASK_PERIOD_MS; - rangefinder->maxRangeCm = HCSR04_I2C_MAX_RANGE_CM; - rangefinder->detectionConeDeciDegrees = HCSR04_I2C_DETECTION_CONE_DECIDEGREES; - rangefinder->detectionConeExtendedDeciDegrees = HCSR04_I2C_DETECTION_CONE_EXTENDED_DECIDEGREES; - - rangefinder->init = &hcsr04i2cInit; - rangefinder->update = &hcsr04i2cUpdate; - rangefinder->read = &hcsr04i2cGetDistance; - - return true; -} -#endif diff --git a/src/main/drivers/rangefinder/rangefinder_tof10120_i2c.c b/src/main/drivers/rangefinder/rangefinder_tof10120_i2c.c new file mode 100644 index 0000000000..5c47cefe7f --- /dev/null +++ b/src/main/drivers/rangefinder/rangefinder_tof10120_i2c.c @@ -0,0 +1,124 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#if defined(USE_RANGEFINDER) && defined(USE_RANGEFINDER_TOF10120_I2C) + +#include "drivers/time.h" +#include "drivers/rangefinder/rangefinder.h" +#include "drivers/rangefinder/rangefinder_tof10120_i2c.h" + +#define TOF10120_I2C_MAX_RANGE_CM 200 +#define TOF10120_I2C_MAX_RANGE_MM TOF10120_I2C_MAX_RANGE_CM * 10 +#define TOF10120_I2C_DETECTION_CONE_DECIDEGREES 300 +#define TOF10120_I2C_DETECTION_CONE_EXTENDED_DECIDEGREES 450 + +#define TOF10120_I2C_ADDRESS 0x52 + +#define TOF10120_I2C_REGISTRY_DISTANCE 0x04 // address of first byte for reading filtered distance +#define TOF10120_I2C_REGISTRY_RT_DISTANCE 0x00 // address of first byte for reading real-time raw distance + +#define TOF10120_I2C_REGISTRY_BUS_ADDR_CONFIG 0x0f +#define TOF10120_I2C_REGISTRY_SENDING_METHOD_CONFIG 0x09 + +volatile int32_t tof10120MeasurementCm = RANGEFINDER_OUT_OF_RANGE; +static bool isTof10120Responding = false; + +static void tof10120i2cInit(rangefinderDev_t *rangefinder) +{ + busWrite(rangefinder->busDev, TOF10120_I2C_REGISTRY_SENDING_METHOD_CONFIG, 0x01); + delay(100); +} + +void tof10120i2cUpdate(rangefinderDev_t *rangefinder) +{ + uint8_t buffer[2]; + uint16_t distance_mm; + + isTof10120Responding = busReadBuf(rangefinder->busDev, TOF10120_I2C_REGISTRY_RT_DISTANCE, buffer, sizeof(buffer)); + + if (!isTof10120Responding) { + return; + } + + distance_mm = (buffer[0] << 8) | buffer[1]; + + if (distance_mm >= TOF10120_I2C_MAX_RANGE_MM) { + tof10120MeasurementCm = RANGEFINDER_OUT_OF_RANGE; + return; + } + + tof10120MeasurementCm = (int32_t)roundf(distance_mm / 10); +} + +/** + * Get the distance that was measured by the last pulse, in centimeters. + */ +int32_t tof10120i2cGetDistance(rangefinderDev_t *rangefinder) +{ + UNUSED(rangefinder); + + if (isTof10120Responding) { + return tof10120MeasurementCm; + } else { + return RANGEFINDER_HARDWARE_FAILURE; + } +} + +static bool deviceDetect(rangefinderDev_t *rangefinder) +{ + busWrite(rangefinder->busDev, TOF10120_I2C_REGISTRY_SENDING_METHOD_CONFIG, 0x01); + delay(100); + + uint8_t response = 0; + + if (!busRead(rangefinder->busDev, TOF10120_I2C_REGISTRY_BUS_ADDR_CONFIG, &response) || !response) { + return false; + } + + return true; +} + +bool tof10120Detect(rangefinderDev_t *rangefinder) +{ + rangefinder->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_TOF10120_I2C, 0, OWNER_RANGEFINDER); + if (rangefinder->busDev == NULL) { + return false; + } + + if (!deviceDetect(rangefinder)) { + busDeviceDeInit(rangefinder->busDev); + return false; + } + + rangefinder->delayMs = RANGEFINDER_TOF10120_I2C_TASK_PERIOD_MS; + rangefinder->maxRangeCm = TOF10120_I2C_MAX_RANGE_CM; + rangefinder->detectionConeDeciDegrees = TOF10120_I2C_DETECTION_CONE_DECIDEGREES; + rangefinder->detectionConeExtendedDeciDegrees = TOF10120_I2C_DETECTION_CONE_EXTENDED_DECIDEGREES; + + rangefinder->init = &tof10120i2cInit; + rangefinder->update = &tof10120i2cUpdate; + rangefinder->read = &tof10120i2cGetDistance; + + return true; +} +#endif diff --git a/src/main/drivers/accgyro/accgyro_adxl345.h b/src/main/drivers/rangefinder/rangefinder_tof10120_i2c.h similarity index 58% rename from src/main/drivers/accgyro/accgyro_adxl345.h rename to src/main/drivers/rangefinder/rangefinder_tof10120_i2c.h index dcb7ea6d3a..08c96ca34a 100644 --- a/src/main/drivers/accgyro/accgyro_adxl345.h +++ b/src/main/drivers/rangefinder/rangefinder_tof10120_i2c.h @@ -1,20 +1,22 @@ /* - * This file is part of Cleanflight. + * This file is part of INAV. * - * Cleanflight is free software: you can redistribute it and/or modify + * INAV is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * - * Cleanflight is distributed in the hope that it will be useful, + * INAV is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . + * along with INAV. If not, see . */ #pragma once -bool adxl345Detect(accDev_t *acc); +#define RANGEFINDER_TOF10120_I2C_TASK_PERIOD_MS 100 + +bool tof10120Detect(rangefinderDev_t *rangefinder); diff --git a/src/main/drivers/rx_pwm.c b/src/main/drivers/rx_pwm.c deleted file mode 100644 index c915e58d61..0000000000 --- a/src/main/drivers/rx_pwm.c +++ /dev/null @@ -1,248 +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 . - */ - -#include -#include - -#include - -#if defined(USE_RX_PPM) - -#include "build/build_config.h" -#include "build/debug.h" - -#include "common/utils.h" - -#include "drivers/time.h" - -#include "drivers/nvic.h" -#include "drivers/io.h" -#include "timer.h" - -#include "pwm_output.h" -#include "pwm_mapping.h" - -#include "rx_pwm.h" - -#define PPM_CAPTURE_COUNT 16 -#define INPUT_FILTER_TICKS 10 -#define PPM_TIMER_PERIOD 0x10000 - -void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity); - -static uint16_t captures[PPM_CAPTURE_COUNT]; - -static uint8_t ppmFrameCount = 0; -static uint8_t lastPPMFrameCount = 0; -static uint8_t ppmCountDivisor = 1; - -typedef struct ppmDevice_s { - uint8_t pulseIndex; - uint32_t currentCapture; - uint32_t currentTime; - uint32_t deltaTime; - uint32_t captures[PPM_CAPTURE_COUNT]; - uint32_t largeCounter; - int8_t numChannels; - int8_t numChannelsPrevFrame; - uint8_t stableFramesSeenCount; - - bool tracking; - bool overflowed; -} ppmDevice_t; - -ppmDevice_t ppmDev; - -#define PPM_IN_MIN_SYNC_PULSE_US 2700 // microseconds -#define PPM_IN_MIN_CHANNEL_PULSE_US 750 // microseconds -#define PPM_IN_MAX_CHANNEL_PULSE_US 2250 // microseconds -#define PPM_STABLE_FRAMES_REQUIRED_COUNT 25 -#define PPM_IN_MIN_NUM_CHANNELS 4 -#define PPM_IN_MAX_NUM_CHANNELS PPM_CAPTURE_COUNT - -bool isPPMDataBeingReceived(void) -{ - return (ppmFrameCount != lastPPMFrameCount); -} - -void resetPPMDataReceivedState(void) -{ - lastPPMFrameCount = ppmFrameCount; -} - -#define MIN_CHANNELS_BEFORE_PPM_FRAME_CONSIDERED_VALID 4 - -static void ppmInit(void) -{ - ppmDev.pulseIndex = 0; - ppmDev.currentCapture = 0; - ppmDev.currentTime = 0; - ppmDev.deltaTime = 0; - ppmDev.largeCounter = 0; - ppmDev.numChannels = -1; - ppmDev.numChannelsPrevFrame = -1; - ppmDev.stableFramesSeenCount = 0; - ppmDev.tracking = false; - ppmDev.overflowed = false; -} - -static void ppmOverflowCallback(struct TCH_s * tch, uint32_t capture) -{ - UNUSED(tch); - - ppmDev.largeCounter += capture + 1; - if (capture == PPM_TIMER_PERIOD - 1) { - ppmDev.overflowed = true; - } -} - -static void ppmEdgeCallback(struct TCH_s * tch, uint32_t capture) -{ - UNUSED(tch); - - int32_t i; - - uint32_t previousTime = ppmDev.currentTime; - uint32_t previousCapture = ppmDev.currentCapture; - - /* Grab the new count */ - uint32_t currentTime = capture; - - /* Convert to 32-bit timer result */ - currentTime += ppmDev.largeCounter; - - if (capture < previousCapture) { - if (ppmDev.overflowed) { - currentTime += PPM_TIMER_PERIOD; - } - } - - // Divide to match output protocol - currentTime = currentTime / ppmCountDivisor; - - /* Capture computation */ - if (currentTime > previousTime) { - ppmDev.deltaTime = currentTime - (previousTime + (ppmDev.overflowed ? (PPM_TIMER_PERIOD / ppmCountDivisor) : 0)); - } else { - ppmDev.deltaTime = (PPM_TIMER_PERIOD / ppmCountDivisor) + currentTime - previousTime; - } - - ppmDev.overflowed = false; - - - /* Store the current measurement */ - ppmDev.currentTime = currentTime; - ppmDev.currentCapture = capture; - -#if 0 - static uint32_t deltaTimes[20]; - static uint8_t deltaIndex = 0; - - deltaIndex = (deltaIndex + 1) % 20; - deltaTimes[deltaIndex] = ppmDev.deltaTime; - UNUSED(deltaTimes); -#endif - - -#if 0 - static uint32_t captureTimes[20]; - static uint8_t captureIndex = 0; - - captureIndex = (captureIndex + 1) % 20; - captureTimes[captureIndex] = capture; - UNUSED(captureTimes); -#endif - - /* Sync pulse detection */ - if (ppmDev.deltaTime > PPM_IN_MIN_SYNC_PULSE_US) { - if (ppmDev.pulseIndex == ppmDev.numChannelsPrevFrame - && ppmDev.pulseIndex >= PPM_IN_MIN_NUM_CHANNELS - && ppmDev.pulseIndex <= PPM_IN_MAX_NUM_CHANNELS) { - /* If we see n simultaneous frames of the same - number of channels we save it as our frame size */ - if (ppmDev.stableFramesSeenCount < PPM_STABLE_FRAMES_REQUIRED_COUNT) { - ppmDev.stableFramesSeenCount++; - } else { - ppmDev.numChannels = ppmDev.pulseIndex; - } - } else { - ppmDev.stableFramesSeenCount = 0; - } - - /* Check if the last frame was well formed */ - if (ppmDev.pulseIndex == ppmDev.numChannels && ppmDev.tracking) { - /* The last frame was well formed */ - for (i = 0; i < ppmDev.numChannels; i++) { - captures[i] = ppmDev.captures[i]; - } - for (i = ppmDev.numChannels; i < PPM_IN_MAX_NUM_CHANNELS; i++) { - captures[i] = PPM_RCVR_TIMEOUT; - } - ppmFrameCount++; - } - - ppmDev.tracking = true; - ppmDev.numChannelsPrevFrame = ppmDev.pulseIndex; - ppmDev.pulseIndex = 0; - - /* We rely on the supervisor to set captureValue to invalid - if no valid frame is found otherwise we ride over it */ - } else if (ppmDev.tracking) { - /* Valid pulse duration 0.75 to 2.5 ms*/ - if (ppmDev.deltaTime > PPM_IN_MIN_CHANNEL_PULSE_US - && ppmDev.deltaTime < PPM_IN_MAX_CHANNEL_PULSE_US - && ppmDev.pulseIndex < PPM_IN_MAX_NUM_CHANNELS) { - ppmDev.captures[ppmDev.pulseIndex] = ppmDev.deltaTime; - ppmDev.pulseIndex++; - } else { - /* Not a valid pulse duration */ - ppmDev.tracking = false; - for (i = 0; i < PPM_CAPTURE_COUNT; i++) { - ppmDev.captures[i] = PPM_RCVR_TIMEOUT; - } - } - } -} - -bool ppmInConfig(const timerHardware_t *timerHardwarePtr) -{ - static timerCallbacks_t callbacks; - TCH_t * tch = timerGetTCH(timerHardwarePtr); - if (tch == NULL) { - return false; - } - - ppmInit(); - - IO_t io = IOGetByTag(timerHardwarePtr->tag); - IOInit(io, OWNER_PPMINPUT, RESOURCE_INPUT, 0); - IOConfigGPIOAF(io, timerHardwarePtr->ioMode, timerHardwarePtr->alternateFunction); - - timerConfigure(tch, (uint16_t)PPM_TIMER_PERIOD, PWM_TIMER_HZ); - timerChInitCallbacks(&callbacks, (void*)&ppmDev, &ppmEdgeCallback, &ppmOverflowCallback); - timerChConfigCallbacks(tch, &callbacks); - timerChConfigIC(tch, true, INPUT_FILTER_TICKS); - timerChCaptureEnable(tch); - - return true; -} - -uint16_t ppmRead(uint8_t channel) -{ - return captures[channel]; -} -#endif diff --git a/src/main/drivers/rx_pwm.h b/src/main/drivers/rx_pwm.h deleted file mode 100644 index a80de956fc..0000000000 --- a/src/main/drivers/rx_pwm.h +++ /dev/null @@ -1,32 +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 . - */ - -#pragma once - -#define PPM_RCVR_TIMEOUT 0 - -struct timerHardware_s; -bool ppmInConfig(const struct timerHardware_s *timerHardwarePtr); - -void pwmInConfig(const struct timerHardware_s *timerHardwarePtr, uint8_t channel); -uint16_t pwmRead(uint8_t channel); -uint16_t ppmRead(uint8_t channel); - -bool isPPMDataBeingReceived(void); -void resetPPMDataReceivedState(void); - -bool isPWMDataBeingReceived(void); diff --git a/src/main/drivers/sdcard/sdmmc_sdio_f7xx.c b/src/main/drivers/sdcard/sdmmc_sdio_f7xx.c index e03d11acef..20fe8ca8dd 100644 --- a/src/main/drivers/sdcard/sdmmc_sdio_f7xx.c +++ b/src/main/drivers/sdcard/sdmmc_sdio_f7xx.c @@ -1385,14 +1385,19 @@ bool SD_Init(void) // Initialize SDMMC1 peripheral interface with default configuration for SD card initialization MODIFY_REG(SDMMC1->CLKCR, CLKCR_CLEAR_MASK, (uint32_t) SDMMC_INIT_CLK_DIV); + delay(100); + // Identify card operating voltage if ((ErrorState = SD_PowerON()) == SD_OK) { + delay(100); // Initialize the present card and put them in idle state if ((ErrorState = SD_InitializeCard()) == SD_OK) { + delay(100); // Read CSD/CID MSD registers if ((ErrorState = SD_GetCardInfo()) == SD_OK) { // Select the Card - Send CMD7 SDMMC_SEL_DESEL_CARD ErrorState = SD_TransmitCommand((SD_CMD_SEL_DESEL_CARD | SD_CMD_RESPONSE_SHORT), SD_CardRCA, 1); + delay(100); MODIFY_REG(SDMMC1->CLKCR, CLKCR_CLEAR_MASK, (uint32_t) SDMMC_CLK_DIV); // Configure SDMMC1 peripheral interface } } @@ -1400,12 +1405,14 @@ bool SD_Init(void) // Configure SD Bus width if (ErrorState == SD_OK) { + delay(100); // Enable wide operation #ifdef SDCARD_SDIO_4BIT ErrorState = SD_WideBusOperationConfig(SD_BUS_WIDE_4B); #else ErrorState = SD_WideBusOperationConfig(SD_BUS_WIDE_1B); #endif + delay(100); } // Configure the SDCARD device diff --git a/src/main/drivers/serial_uart_stm32h7xx.c b/src/main/drivers/serial_uart_stm32h7xx.c index 0e22206bed..7f5d6da7e0 100644 --- a/src/main/drivers/serial_uart_stm32h7xx.c +++ b/src/main/drivers/serial_uart_stm32h7xx.c @@ -50,8 +50,8 @@ typedef struct uartDevice_s { #ifdef USE_UART1 #define UART_PIN_AF_UART1_PA9 GPIO_AF7_USART1 #define UART_PIN_AF_UART1_PA10 GPIO_AF7_USART1 -#define UART_PIN_AF_UART1_PB6 GPIO_AF4_USART1 -#define UART_PIN_AF_UART1_PB7 GPIO_AF4_USART1 +#define UART_PIN_AF_UART1_PB6 GPIO_AF7_USART1 +#define UART_PIN_AF_UART1_PB7 GPIO_AF7_USART1 #define UART_PIN_AF_UART1_PB14 GPIO_AF4_USART1 #define UART_PIN_AF_UART1_PB15 GPIO_AF4_USART1 @@ -470,4 +470,4 @@ void UART8_IRQHandler(void) uartPort_t *s = &(uartHardwareMap[UARTDEV_8]->port); uartIrqHandler(s); } -#endif \ No newline at end of file +#endif diff --git a/src/main/drivers/sound_beeper.c b/src/main/drivers/sound_beeper.c index 4dc37cbd16..e7ba58ac35 100644 --- a/src/main/drivers/sound_beeper.c +++ b/src/main/drivers/sound_beeper.c @@ -23,11 +23,10 @@ #include "drivers/time.h" #include "drivers/io.h" -#ifdef BEEPER_PWM #include "drivers/timer.h" #include "drivers/pwm_mapping.h" #include "drivers/pwm_output.h" -#endif +#include "fc/config.h" #include "sound_beeper.h" @@ -44,13 +43,18 @@ void systemBeep(bool onoff) { #if !defined(BEEPER) UNUSED(onoff); -#elif defined(BEEPER_PWM) - pwmWriteBeeper(onoff); - beeperState = onoff; #else - IOWrite(beeperIO, beeperInverted ? onoff : !onoff); - beeperState = onoff; + + if (beeperConfig()->pwmMode) { + pwmWriteBeeper(onoff); + beeperState = onoff; + } else { + IOWrite(beeperIO, beeperInverted ? onoff : !onoff); + beeperState = onoff; + } + #endif + } void systemBeepToggle(void) @@ -70,11 +74,11 @@ void beeperInit(const beeperDevConfig_t *config) if (beeperIO) { IOInit(beeperIO, OWNER_BEEPER, RESOURCE_OUTPUT, 0); -#if defined(BEEPER_PWM) - beeperPwmInit(config->ioTag, BEEPER_PWM_FREQUENCY); -#else - IOConfigGPIO(beeperIO, config->isOD ? IOCFG_OUT_OD : IOCFG_OUT_PP); -#endif + if (beeperConfig()->pwmMode) { + beeperPwmInit(config->ioTag, BEEPER_PWM_FREQUENCY); + } else { + IOConfigGPIO(beeperIO, config->isOD ? IOCFG_OUT_OD : IOCFG_OUT_PP); + } } systemBeep(false); diff --git a/src/main/drivers/system.h b/src/main/drivers/system.h index 2f2184613b..10c0a1e717 100644 --- a/src/main/drivers/system.h +++ b/src/main/drivers/system.h @@ -30,7 +30,8 @@ typedef enum { FAILURE_ACC_INCOMPATIBLE, FAILURE_INVALID_EEPROM_CONTENTS, FAILURE_FLASH_WRITE_FAILED, - FAILURE_GYRO_INIT_FAILED + FAILURE_GYRO_INIT_FAILED, + FAILURE_FLASH_READ_FAILED, } failureMode_e; // failure diff --git a/src/main/drivers/timer_def_stm32h7xx.h b/src/main/drivers/timer_def_stm32h7xx.h index a21132ae05..ef4d60166f 100644 --- a/src/main/drivers/timer_def_stm32h7xx.h +++ b/src/main/drivers/timer_def_stm32h7xx.h @@ -138,7 +138,7 @@ #define DEF_TIM_AF__PA1__TCH_TIM15_CH1N D(4, 15) #define DEF_TIM_AF__PA2__TCH_TIM15_CH1 D(4, 15) -#define DEF_TIM_AF__PA2__TCH_TIM15_CH2 D(4, 15) +#define DEF_TIM_AF__PA3__TCH_TIM15_CH2 D(4, 15) //PORTB #define DEF_TIM_AF__PB0__TCH_TIM1_CH2N D(1, 1) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 5edafca5d9..e420d1cc95 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -25,7 +25,7 @@ #include "platform.h" -uint8_t cliMode = 0; +bool cliMode = false; #include "blackbox/blackbox.h" @@ -60,7 +60,6 @@ uint8_t cliMode = 0; #include "drivers/io_impl.h" #include "drivers/osd_symbols.h" #include "drivers/persistent.h" -#include "drivers/rx_pwm.h" #include "drivers/sdcard/sdcard.h" #include "drivers/sensor.h" #include "drivers/serial.h" @@ -119,6 +118,9 @@ uint8_t cliMode = 0; #include "sensors/opflow.h" #include "sensors/sensors.h" #include "sensors/temperature.h" +#ifdef USE_ESC_SENSOR +#include "sensors/esc_sensor.h" +#endif #include "telemetry/frsky_d.h" #include "telemetry/telemetry.h" @@ -159,9 +161,15 @@ static const char * const featureNames[] = { "OSD", "FW_LAUNCH", "FW_AUTOTRIM", NULL }; +#ifdef USE_BLACKBOX +static const char * const blackboxIncludeFlagNames[] = { + "NAV_ACC", "NAV_POS", "NAV_PID", "MAG", "ACC", "ATTI", "RC_DATA", "RC_COMMAND", "MOTORS", NULL +}; +#endif + /* Sensor names (used in lookup tables for *_hardware settings and in status command output) */ // sync with gyroSensor_e -static const char * const gyroNames[] = { "NONE", "AUTO", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "FAKE"}; +static const char * const gyroNames[] = { "NONE", "AUTO", "MPU6050", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "ICM42605", "BMI270", "FAKE"}; // sync this with sensors_e static const char * const sensorTypeNames[] = { @@ -1387,7 +1395,7 @@ static void cliWaypoints(char *cmdline) } else if (sl_strcasecmp(cmdline, "reset") == 0) { resetWaypointList(); } else if (sl_strcasecmp(cmdline, "load") == 0) { - loadNonVolatileWaypointList(); + loadNonVolatileWaypointList(true); } else if (sl_strcasecmp(cmdline, "save") == 0) { posControl.waypointListValid = false; for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) { @@ -2501,6 +2509,94 @@ static void cliFeature(char *cmdline) } } +#ifdef USE_BLACKBOX +static void printBlackbox(uint8_t dumpMask, const blackboxConfig_t *config, const blackboxConfig_t *configDefault) +{ + + UNUSED(configDefault); + + uint32_t mask = config->includeFlags; + + for (uint8_t i = 0; ; i++) { // reenable what we want. + if (blackboxIncludeFlagNames[i] == NULL) { + break; + } + + const char *formatOn = "blackbox %s"; + const char *formatOff = "blackbox -%s"; + + if (mask & (1 << i)) { + cliDumpPrintLinef(dumpMask, false, formatOn, blackboxIncludeFlagNames[i]); + cliDefaultPrintLinef(dumpMask, false, formatOn, blackboxIncludeFlagNames[i]); + } else { + cliDumpPrintLinef(dumpMask, false, formatOff, blackboxIncludeFlagNames[i]); + cliDefaultPrintLinef(dumpMask, false, formatOff, blackboxIncludeFlagNames[i]); + } + } + +} + +static void cliBlackbox(char *cmdline) +{ + uint32_t len = strlen(cmdline); + uint32_t mask = blackboxConfig()->includeFlags; + + if (len == 0) { + cliPrint("Enabled: "); + for (uint8_t i = 0; ; i++) { + if (blackboxIncludeFlagNames[i] == NULL) { + break; + } + + if (mask & (1 << i)) + cliPrintf("%s ", blackboxIncludeFlagNames[i]); + } + cliPrintLinefeed(); + } else if (sl_strncasecmp(cmdline, "list", len) == 0) { + cliPrint("Available: "); + for (uint32_t i = 0; ; i++) { + if (blackboxIncludeFlagNames[i] == NULL) { + break; + } + + cliPrintf("%s ", blackboxIncludeFlagNames[i]); + } + cliPrintLinefeed(); + return; + } else { + bool remove = false; + if (cmdline[0] == '-') { + // remove feature + remove = true; + cmdline++; // skip over - + len--; + } + + for (uint32_t i = 0; ; i++) { + if (blackboxIncludeFlagNames[i] == NULL) { + cliPrintErrorLine("Invalid name"); + break; + } + + if (sl_strncasecmp(cmdline, blackboxIncludeFlagNames[i], len) == 0) { + + mask = 1 << i; + + if (remove) { + blackboxIncludeFlagClear(mask); + cliPrint("Disabled"); + } else { + blackboxIncludeFlagSet(mask); + cliPrint("Enabled"); + } + cliPrintLinef(" %s", blackboxIncludeFlagNames[i]); + break; + } + } + } +} +#endif + #if defined(BEEPER) || defined(USE_DSHOT) static void printBeeper(uint8_t dumpMask, const beeperConfig_t *beeperConfig, const beeperConfig_t *beeperConfigDefault) { @@ -2731,7 +2827,7 @@ static void cliExit(char *cmdline) *cliBuffer = '\0'; bufferIndex = 0; - cliMode = 0; + cliMode = false; // incase a motor was left running during motortest, clear it here mixerResetDisarmedMotors(); cliReboot(); @@ -3183,16 +3279,29 @@ static void cliStatus(char *cmdline) cliPrintLinef(" PCLK2 = %d MHz", clocks.PCLK2_Frequency / 1000000); #endif - cliPrintLinef("Sensor status: GYRO=%s, ACC=%s, MAG=%s, BARO=%s, RANGEFINDER=%s, OPFLOW=%s, GPS=%s", + cliPrintLinef("Sensor status: GYRO=%s, ACC=%s, MAG=%s, BARO=%s, RANGEFINDER=%s, OPFLOW=%s, GPS=%s, IMU2=%s", hardwareSensorStatusNames[getHwGyroStatus()], hardwareSensorStatusNames[getHwAccelerometerStatus()], hardwareSensorStatusNames[getHwCompassStatus()], hardwareSensorStatusNames[getHwBarometerStatus()], hardwareSensorStatusNames[getHwRangefinderStatus()], hardwareSensorStatusNames[getHwOpticalFlowStatus()], - hardwareSensorStatusNames[getHwGPSStatus()] + hardwareSensorStatusNames[getHwGPSStatus()], + hardwareSensorStatusNames[getHwSecondaryImuStatus()] ); +#ifdef USE_ESC_SENSOR + uint8_t motorCount = getMotorCount(); + if (STATE(ESC_SENSOR_ENABLED) && motorCount > 0) { + cliPrintLinef("ESC Temperature(s): Motor Count = %d", motorCount); + for (uint8_t i = 0; i < motorCount; i++) { + const escSensorData_t *escState = getEscTelemetry(i); //Get ESC telemetry + cliPrintf("ESC %d: %d\260C, ", i, escState->temperature); + } + cliPrintLinefeed(); + } +#endif + #ifdef USE_SDCARD cliSdInfo(NULL); #endif @@ -3489,6 +3598,11 @@ static void printConfig(const char *cmdline, bool doDiff) printBeeper(dumpMask, &beeperConfig_Copy, beeperConfig()); #endif +#ifdef USE_BLACKBOX + cliPrintHashLine("blackbox"); + printBlackbox(dumpMask, &blackboxConfig_Copy, blackboxConfig()); +#endif + cliPrintHashLine("map"); printMap(dumpMask, &rxConfig_Copy, rxConfig()); @@ -3682,6 +3796,11 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("feature", "configure features", "list\r\n" "\t<+|->[name]", cliFeature), +#ifdef USE_BLACKBOX + CLI_COMMAND_DEF("blackbox", "configure blackbox fields", + "list\r\n" + "\t<+|->[name]", cliBlackbox), +#endif #ifdef USE_FLASHFS CLI_COMMAND_DEF("flash_erase", "erase flash chip", NULL, cliFlashErase), CLI_COMMAND_DEF("flash_info", "show flash chip info", NULL, cliFlashInfo), @@ -3897,7 +4016,7 @@ void cliEnter(serialPort_t *serialPort) return; } - cliMode = 1; + cliMode = true; cliPort = serialPort; setPrintfSerialPort(cliPort); cliWriter = bufWriterInit(cliWriteBuffer, sizeof(cliWriteBuffer), (bufWrite_t)serialWriteBufShim, serialPort); diff --git a/src/main/fc/cli.h b/src/main/fc/cli.h index a0ad8bee53..8286d448f1 100644 --- a/src/main/fc/cli.h +++ b/src/main/fc/cli.h @@ -17,7 +17,7 @@ #pragma once -extern uint8_t cliMode; +extern bool cliMode; struct serialConfig_s; void cliInit(const struct serialConfig_s *serialConfig); diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 8937097a7b..e4610cddf7 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -107,7 +107,7 @@ PG_RESET_TEMPLATE(featureConfig_t, featureConfig, .enabledFeatures = DEFAULT_FEATURES | COMMON_DEFAULT_FEATURES ); -PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 3); +PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 5); PG_RESET_TEMPLATE(systemConfig_t, systemConfig, .current_profile_index = 0, @@ -123,13 +123,14 @@ PG_RESET_TEMPLATE(systemConfig_t, systemConfig, .name = SETTING_NAME_DEFAULT ); -PG_REGISTER_WITH_RESET_TEMPLATE(beeperConfig_t, beeperConfig, PG_BEEPER_CONFIG, 1); +PG_REGISTER_WITH_RESET_TEMPLATE(beeperConfig_t, beeperConfig, PG_BEEPER_CONFIG, 2); PG_RESET_TEMPLATE(beeperConfig_t, beeperConfig, .beeper_off_flags = 0, .preferred_beeper_off_flags = 0, .dshot_beeper_enabled = SETTING_DSHOT_BEEPER_ENABLED_DEFAULT, .dshot_beeper_tone = SETTING_DSHOT_BEEPER_TONE_DEFAULT, + .pwmMode = SETTING_BEEPER_PWM_MODE_DEFAULT, ); PG_REGISTER_WITH_RESET_TEMPLATE(adcChannelConfig_t, adcChannelConfig, PG_ADC_CHANNEL_CONFIG, 0); @@ -192,13 +193,6 @@ void validateAndFixConfig(void) // Disable unused features featureClear(FEATURE_UNUSED_1 | FEATURE_UNUSED_3 | FEATURE_UNUSED_4 | FEATURE_UNUSED_5 | FEATURE_UNUSED_6 | FEATURE_UNUSED_7 | FEATURE_UNUSED_8 | FEATURE_UNUSED_9 | FEATURE_UNUSED_10); -#if !defined(USE_RX_PPM) - if (rxConfig()->receiverType == RX_TYPE_PPM) { - rxConfigMutable()->receiverType = RX_TYPE_NONE; - } -#endif - - #if defined(USE_LED_STRIP) && (defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2)) if (featureConfigured(FEATURE_SOFTSERIAL) && featureConfigured(FEATURE_LED_STRIP)) { const timerHardware_t *ledTimerHardware = timerGetByTag(IO_TAG(WS2811_PIN), TIM_USE_ANY); @@ -264,9 +258,6 @@ void validateAndFixConfig(void) case PWM_TYPE_ONESHOT125: // Limited to 3900 Hz motorConfigMutable()->motorPwmRate = MIN(motorConfig()->motorPwmRate, 3900); break; - case PWM_TYPE_ONESHOT42: // 2-8 kHz - motorConfigMutable()->motorPwmRate = constrain(motorConfig()->motorPwmRate, 2000, 8000); - break; case PWM_TYPE_MULTISHOT: // 2-16 kHz motorConfigMutable()->motorPwmRate = constrain(motorConfig()->motorPwmRate, 2000, 16000); break; @@ -286,14 +277,6 @@ void validateAndFixConfig(void) case PWM_TYPE_DSHOT600: motorConfigMutable()->motorPwmRate = MIN(motorConfig()->motorPwmRate, 16000); break; - case PWM_TYPE_DSHOT1200: - motorConfigMutable()->motorPwmRate = MIN(motorConfig()->motorPwmRate, 32000); - break; -#endif -#ifdef USE_SERIALSHOT - case PWM_TYPE_SERIALSHOT: // 2-4 kHz - motorConfigMutable()->motorPwmRate = constrain(motorConfig()->motorPwmRate, 2000, 4000); - break; #endif } #endif diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 4db38b6d6a..dda257670c 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -22,7 +22,6 @@ #include "common/time.h" #include "config/parameter_group.h" #include "drivers/adc.h" -#include "drivers/rx_pwm.h" #include "fc/stats.h" #define MAX_PROFILE_COUNT 3 @@ -30,7 +29,7 @@ #define MAX_NAME_LENGTH 16 #define TASK_GYRO_LOOPTIME 250 // Task gyro always runs at 4kHz - typedef enum { +typedef enum { FEATURE_THR_VBAT_COMP = 1 << 0, FEATURE_VBAT = 1 << 1, FEATURE_TX_PROF_SEL = 1 << 2, // Profile selection by TX stick command @@ -86,6 +85,7 @@ typedef struct beeperConfig_s { uint32_t preferred_beeper_off_flags; bool dshot_beeper_enabled; uint8_t dshot_beeper_tone; + bool pwmMode; } beeperConfig_t; PG_DECLARE(beeperConfig_t, beeperConfig); diff --git a/src/main/fc/controlrate_profile.c b/src/main/fc/controlrate_profile.c index ba464a1fe6..18f5bd343b 100644 --- a/src/main/fc/controlrate_profile.c +++ b/src/main/fc/controlrate_profile.c @@ -34,7 +34,7 @@ const controlRateConfig_t *currentControlRateProfile; -PG_REGISTER_ARRAY_WITH_RESET_FN(controlRateConfig_t, MAX_CONTROL_RATE_PROFILE_COUNT, controlRateProfiles, PG_CONTROL_RATE_PROFILES, 3); +PG_REGISTER_ARRAY_WITH_RESET_FN(controlRateConfig_t, MAX_CONTROL_RATE_PROFILE_COUNT, controlRateProfiles, PG_CONTROL_RATE_PROFILES, 4); void pgResetFn_controlRateProfiles(controlRateConfig_t *instance) { @@ -66,7 +66,17 @@ void pgResetFn_controlRateProfiles(controlRateConfig_t *instance) .misc = { .fpvCamAngleDegrees = SETTING_FPV_MIX_DEGREES_DEFAULT + }, + #ifdef USE_RATE_DYNAMICS + .rateDynamics = { + .sensitivityCenter = SETTING_RATE_DYNAMICS_CENTER_SENSITIVITY_DEFAULT, + .sensitivityEnd = SETTING_RATE_DYNAMICS_END_SENSITIVITY_DEFAULT, + .correctionCenter = SETTING_RATE_DYNAMICS_CENTER_CORRECTION_DEFAULT, + .correctionEnd = SETTING_RATE_DYNAMICS_END_CORRECTION_DEFAULT, + .weightCenter = SETTING_RATE_DYNAMICS_CENTER_WEIGHT_DEFAULT, + .weightEnd = SETTING_RATE_DYNAMICS_END_WEIGHT_DEFAULT, } + #endif ); } } diff --git a/src/main/fc/controlrate_profile_config_struct.h b/src/main/fc/controlrate_profile_config_struct.h index 30e50d2a2b..308b87d314 100644 --- a/src/main/fc/controlrate_profile_config_struct.h +++ b/src/main/fc/controlrate_profile_config_struct.h @@ -49,4 +49,15 @@ typedef struct controlRateConfig_s { uint8_t fpvCamAngleDegrees; // Camera angle to treat as "forward" base axis in ACRO (Roll and Yaw sticks will command rotation considering this axis) } misc; +#ifdef USE_RATE_DYNAMICS + struct { + uint8_t sensitivityCenter; + uint8_t sensitivityEnd; + uint8_t correctionCenter; + uint8_t correctionEnd; + uint8_t weightCenter; + uint8_t weightEnd; + } rateDynamics; +#endif + } controlRateConfig_t; diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index d9b83c9ad0..f72a773398 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -87,6 +87,8 @@ FILE_COMPILE_FOR_SPEED #include "flight/servos.h" #include "flight/pid.h" #include "flight/imu.h" +#include "flight/secondary_imu.h" +#include "flight/rate_dynamics.h" #include "flight/failsafe.h" #include "flight/power_limits.h" @@ -223,7 +225,7 @@ static void updateArmingStatus(void) /* CHECK: pitch / roll sticks centered when NAV_LAUNCH_MODE enabled */ if (isNavLaunchEnabled()) { - if (areSticksDeflected()) { + if (isRollPitchStickDeflected()) { ENABLE_ARMING_FLAG(ARMING_DISABLED_ROLLPITCH_NOT_CENTERED); } else { DISABLE_ARMING_FLAG(ARMING_DISABLED_ROLLPITCH_NOT_CENTERED); @@ -373,7 +375,7 @@ static bool emergencyArmingIsEnabled(void) return emergencyArmingIsTriggered() && emergencyArmingCanOverrideArmingDisabled(); } -void annexCode(void) +void annexCode(float dT) { int32_t throttleValue; @@ -392,6 +394,19 @@ void annexCode(void) rcCommand[ROLL] = rcCommand[ROLL] * currentControlRateProfile->manual.rates[FD_ROLL] / 100L; rcCommand[PITCH] = rcCommand[PITCH] * currentControlRateProfile->manual.rates[FD_PITCH] / 100L; rcCommand[YAW] = rcCommand[YAW] * currentControlRateProfile->manual.rates[FD_YAW] / 100L; + } else { + DEBUG_SET(DEBUG_RATE_DYNAMICS, 0, rcCommand[ROLL]); + rcCommand[ROLL] = applyRateDynamics(rcCommand[ROLL], ROLL, dT); + DEBUG_SET(DEBUG_RATE_DYNAMICS, 1, rcCommand[ROLL]); + + DEBUG_SET(DEBUG_RATE_DYNAMICS, 2, rcCommand[PITCH]); + rcCommand[PITCH] = applyRateDynamics(rcCommand[PITCH], PITCH, dT); + DEBUG_SET(DEBUG_RATE_DYNAMICS, 3, rcCommand[PITCH]); + + DEBUG_SET(DEBUG_RATE_DYNAMICS, 4, rcCommand[YAW]); + rcCommand[YAW] = applyRateDynamics(rcCommand[YAW], YAW, dT); + DEBUG_SET(DEBUG_RATE_DYNAMICS, 5, rcCommand[YAW]); + } //Compute THROTTLE command @@ -574,10 +589,11 @@ void tryArm(void) //beep to indicate arming #ifdef USE_NAV - if (navigationPositionEstimateIsHealthy()) + if (navigationPositionEstimateIsHealthy()) { beeper(BEEPER_ARMING_GPS_FIX); - else + } else { beeper(BEEPER_ARMING); + } #else beeper(BEEPER_ARMING); #endif @@ -599,8 +615,9 @@ void processRx(timeUs_t currentTimeUs) // in 3D mode, we need to be able to disarm by switch at any time if (feature(FEATURE_REVERSIBLE_MOTORS)) { - if (!IS_RC_MODE_ACTIVE(BOXARM)) + if (!IS_RC_MODE_ACTIVE(BOXARM)) { disarm(DISARM_SWITCH_3D); + } } updateRSSI(currentTimeUs); @@ -859,7 +876,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs) imuUpdateAccelerometer(); imuUpdateAttitude(currentTimeUs); - annexCode(); + annexCode(dT); if (rxConfig()->rcFilterFrequency) { rcInterpolationApply(isRXDataNew); @@ -914,7 +931,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs) } #endif - mixTable(dT); + mixTable(); if (isMixerUsingServos()) { servoMixer(dT); diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 20b754d626..68c73079f1 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -63,7 +63,6 @@ #include "drivers/pwm_mapping.h" #include "drivers/pwm_output.h" #include "drivers/pwm_output.h" -#include "drivers/rx_pwm.h" #include "drivers/sensor.h" #include "drivers/serial.h" #include "drivers/serial_softserial.h" @@ -217,6 +216,13 @@ void init(void) detectBrushedESC(); #endif +#ifdef CONFIG_IN_EXTERNAL_FLASH + // Reset config to defaults. Note: Default flash config must be functional for config in external flash to work. + pgResetAll(0); + + flashDeviceInitialized = flashInit(); +#endif + initEEPROM(); ensureEEPROMContainsValidData(); readEEPROM(); @@ -270,15 +276,7 @@ void init(void) timerInit(); // timer must be initialized before any channel is allocated -#if defined(AVOID_UART2_FOR_PWM_PPM) - serialInit(feature(FEATURE_SOFTSERIAL), - (rxConfig()->receiverType == RX_TYPE_PPM) ? SERIAL_PORT_USART2 : SERIAL_PORT_NONE); -#elif defined(AVOID_UART3_FOR_PWM_PPM) - serialInit(feature(FEATURE_SOFTSERIAL), - (rxConfig()->receiverType == RX_TYPE_PPM) ? SERIAL_PORT_USART3 : SERIAL_PORT_NONE); -#else - serialInit(feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE); -#endif + serialInit(feature(FEATURE_SOFTSERIAL)); // Initialize MSP serial ports here so LOG can share a port with MSP. // XXX: Don't call mspFcInit() yet, since it initializes the boxes and needs @@ -360,24 +358,15 @@ void init(void) // Initialize buses busInit(); +#ifdef CONFIG_IN_EXTERNAL_FLASH + // busInit re-configures the SPI pins. Init flash again so it is ready to write settings + flashDeviceInitialized = flashInit(); +#endif + #ifdef USE_HARDWARE_REVISION_DETECTION updateHardwareRevision(); #endif -#if defined(USE_RANGEFINDER_HCSR04) && defined(USE_SOFTSERIAL1) -#if defined(FURYF3) || defined(OMNIBUS) || defined(SPRACINGF3MINI) - if ((rangefinderConfig()->rangefinder_hardware == RANGEFINDER_HCSR04) && feature(FEATURE_SOFTSERIAL)) { - serialRemovePort(SERIAL_PORT_SOFTSERIAL1); - } -#endif -#endif - -#if defined(USE_RANGEFINDER_HCSR04) && defined(USE_SOFTSERIAL2) && defined(SPRACINGF3) - if ((rangefinderConfig()->rangefinder_hardware == RANGEFINDER_HCSR04) && feature(FEATURE_SOFTSERIAL)) { - serialRemovePort(SERIAL_PORT_SOFTSERIAL2); - } -#endif - #ifdef USE_USB_MSC /* MSC mode will start after init, but will not allow scheduler to run, * so there is no bottleneck in reading and writing data diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index c534531bc9..bd9d793fb8 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -76,6 +76,7 @@ #include "flight/mixer.h" #include "flight/pid.h" #include "flight/servos.h" +#include "flight/secondary_imu.h" #include "config/config_eeprom.h" #include "config/feature.h" @@ -424,6 +425,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, getHwRangefinderStatus()); sbufWriteU8(dst, getHwPitotmeterStatus()); sbufWriteU8(dst, getHwOpticalFlowStatus()); + sbufWriteU8(dst, getHwSecondaryImuStatus()); break; case MSP_ACTIVEBOXES: @@ -2846,7 +2848,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) #ifdef NAV_NON_VOLATILE_WAYPOINT_STORAGE case MSP_WP_MISSION_LOAD: sbufReadU8Safe(NULL, src); // Mission ID (reserved) - if ((dataSize != 1) || (!loadNonVolatileWaypointList())) + if ((dataSize != 1) || (!loadNonVolatileWaypointList(false))) return MSP_RESULT_ERROR; break; diff --git a/src/main/fc/firmware_update_common.h b/src/main/fc/firmware_update_common.h index 27fd8799e2..ce396875ad 100644 --- a/src/main/fc/firmware_update_common.h +++ b/src/main/fc/firmware_update_common.h @@ -39,8 +39,11 @@ #define AVAILABLE_FIRMWARE_SPACE (FLASH_END - FIRMWARE_START_ADDRESS) extern uint8_t __firmware_start; // set via linker + +#if defined(CONFIG_IN_FLASH) extern uint8_t __config_start; extern uint8_t __config_end; +#endif typedef struct { uint32_t magic; diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 74952a5329..b5d4648838 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -260,9 +260,14 @@ void processRcStickPositions(throttleStatus_e throttleStatus) // Load waypoint list if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_HI) { - const bool success = loadNonVolatileWaypointList(); + const bool success = loadNonVolatileWaypointList(false); beeper(success ? BEEPER_ACTION_SUCCESS : BEEPER_ACTION_FAIL); } + + if (rcSticks == THR_LO + YAW_CE + PIT_LO + ROL_HI) { + resetWaypointList(); + beeper(BEEPER_ACTION_FAIL); // The above cannot fail, but traditionally, we play FAIL for not-loading + } #endif // Multiple configuration profiles diff --git a/src/main/fc/rc_curves.c b/src/main/fc/rc_curves.c index 7f34607d15..47da45113b 100644 --- a/src/main/fc/rc_curves.c +++ b/src/main/fc/rc_curves.c @@ -31,9 +31,6 @@ #include "rx/rx.h" - -#define PITCH_LOOKUP_LENGTH 7 -#define YAW_LOOKUP_LENGTH 7 #define THROTTLE_LOOKUP_LENGTH 11 static EXTENDED_FASTRAM int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 1a1ad14957..3066d2a9c2 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4,16 +4,16 @@ tables: - name: gyro_lpf values: ["256HZ", "188HZ", "98HZ", "42HZ", "20HZ", "10HZ"] - name: acc_hardware - values: ["NONE", "AUTO", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "FAKE"] + values: ["NONE", "AUTO", "MPU6050", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "ICM42605", "BMI270", "FAKE"] enum: accelerationSensor_e - name: rangefinder_hardware - values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UNUSED", "BENEWAKE", "VL53L1X", "US42"] + values: ["NONE", "SRF10", "VL53L0X", "MSP", "BENEWAKE", "VL53L1X", "US42", "TOF10120_I2C"] enum: rangefinderType_e - name: secondary_imu_hardware values: ["NONE", "BNO055", "BNO055_SERIAL"] enum: secondaryImuType_e - name: mag_hardware - values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "RM3100", FAKE"] + values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "RM3100", "VCM5883", "MLX90393", "FAKE"] enum: magSensor_e - name: opflow_hardware values: ["NONE", "CXOF", "MSP", "FAKE"] @@ -25,7 +25,7 @@ tables: values: ["NONE", "AUTO", "MS4525", "ADC", "VIRTUAL", "FAKE", "MSP"] enum: pitotSensor_e - name: receiver_type - values: ["NONE", "PPM", "SERIAL", "MSP", "SPI", "UNUSED"] + values: ["NONE", "SERIAL", "MSP", "SPI"] enum: rxReceiverType_e - name: serial_rx values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "SUMH", "XB-B", "XB-B-RJ01", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "SBUS_FAST", "FPORT2", "SRXL2", "GHST", "MAVLINK"] @@ -35,11 +35,11 @@ tables: - name: blackbox_device values: ["SERIAL", "SPIFLASH", "SDCARD"] - name: motor_pwm_protocol - values: ["STANDARD", "ONESHOT125", "ONESHOT42", "MULTISHOT", "BRUSHED", "DSHOT150", "DSHOT300", "DSHOT600", "DSHOT1200", "SERIALSHOT"] + values: ["STANDARD", "ONESHOT125", "MULTISHOT", "BRUSHED", "DSHOT150", "DSHOT300", "DSHOT600"] - name: servo_protocol values: ["PWM", "SERVO_DRIVER", "SBUS", "SBUS_PWM"] - name: failsafe_procedure - values: ["SET-THR", "DROP", "RTH", "NONE"] + values: ["LAND", "DROP", "RTH", "NONE"] - name: current_sensor values: ["NONE", "ADC", "VIRTUAL", "ESC"] enum: currentSensor_e @@ -64,7 +64,7 @@ tables: - name: nav_rth_alt_mode values: ["CURRENT", "EXTRA", "FIXED", "MAX", "AT_LEAST", "AT_LEAST_LINEAR_DESCENT"] - name: osd_unit - values: ["IMPERIAL", "METRIC", "UK"] + values: ["IMPERIAL", "METRIC", "METRIC_MPH", "UK", "GA"] enum: osd_unit_e - name: osd_stats_energy_unit values: ["MAH", "WH"] @@ -93,15 +93,15 @@ tables: "FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE", "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY", - "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "FW_D", "IMU2", "ALTITUDE", - "GYRO_ALPHA_BETA_GAMMA", "SMITH_PREDICTOR", "AUTOTRIM", "AUTOTUNE"] + "IRLOCK", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "IMU2", "ALTITUDE", + "SMITH_PREDICTOR", "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS"] - name: async_mode values: ["NONE", "GYRO", "ALL"] - name: aux_operator values: ["OR", "AND"] enum: modeActivationOperator_e - name: osd_crosshairs_style - values: ["DEFAULT", "AIRCRAFT", "TYPE3", "TYPE4", "TYPE5", "TYPE6", "TYPE7"] + values: ["DEFAULT", "AIRCRAFT", "TYPE3", "TYPE4", "TYPE5", "TYPE6", "TYPE7", "TYPE8"] enum: osd_crosshairs_style_e - name: osd_sidebar_scroll values: ["NONE", "ALTITUDE", "SPEED", "HOME_DISTANCE"] @@ -129,6 +129,8 @@ tables: enum: vtxLowerPowerDisarm_e - name: filter_type values: ["PT1", "BIQUAD"] + - name: filter_type_full + values: ["PT1", "BIQUAD", "PT2", "PT3"] - name: log_level values: ["ERROR", "WARNING", "INFO", "VERBOSE", "DEBUG"] - name: iterm_relax @@ -142,9 +144,6 @@ tables: - name: rssi_source values: ["NONE", "AUTO", "ADC", "CHANNEL", "PROTOCOL", "MSP"] enum: rssiSource_e - - name: dynamicFilterRangeTable - values: ["HIGH", "MEDIUM", "LOW"] - enum: dynamicFilterRange_e - name: pidTypeTable values: ["NONE", "PID", "PIFF", "AUTO"] enum: pidType_e @@ -281,16 +280,10 @@ groups: max: 10 - name: dynamic_gyro_notch_enabled description: "Enable/disable dynamic gyro notch also known as Matrix Filter" - default_value: OFF + default_value: ON field: dynamicGyroNotchEnabled condition: USE_DYNAMIC_FILTERS type: bool - - name: dynamic_gyro_notch_range - description: "Range for dynamic gyro notches. `MEDIUM` for 5\", `HIGH` for 3\" and `MEDIUM`/`LOW` for 7\" and bigger propellers" - default_value: "MEDIUM" - field: dynamicGyroNotchRange - condition: USE_DYNAMIC_FILTERS - table: dynamicFilterRangeTable - name: dynamic_gyro_notch_q description: "Q factor for dynamic notches" default_value: 120 @@ -299,8 +292,8 @@ groups: min: 1 max: 1000 - name: dynamic_gyro_notch_min_hz - description: "Minimum frequency for dynamic notches. Default value of `150` works best with 5\" multirors. Should be lowered with increased size of propellers. Values around `100` work fine on 7\" drones. 10\" can go down to `60` - `70`" - default_value: 150 + description: "Minimum frequency for dynamic notches. Default value of `150` works best with 5\" multirotors. Should be lowered with increased size of propellers. Values around `100` work fine on 7\" drones. 10\" can go down to `60` - `70`" + default_value: 50 field: dynamicGyroNotchMinHz condition: USE_DYNAMIC_FILTERS min: 30 @@ -308,32 +301,11 @@ groups: - name: gyro_to_use condition: USE_DUAL_GYRO min: 0 - max: 1 - default_value: 0 - - name: gyro_abg_alpha - description: "Alpha factor for Gyro Alpha-Beta-Gamma filter" - default_value: 0 - field: alphaBetaGammaAlpha - condition: USE_ALPHA_BETA_GAMMA_FILTER - min: 0 - max: 1 - - name: gyro_abg_boost - description: "Boost factor for Gyro Alpha-Beta-Gamma filter" - default_value: 0.35 - field: alphaBetaGammaBoost - condition: USE_ALPHA_BETA_GAMMA_FILTER - min: 0 max: 2 - - name: gyro_abg_half_life - description: "Sample half-life for Gyro Alpha-Beta-Gamma filter" - default_value: 0.5 - field: alphaBetaGammaHalfLife - condition: USE_ALPHA_BETA_GAMMA_FILTER - min: 0 - max: 10 + default_value: 0 - name: setpoint_kalman_enabled description: "Enable Kalman filter on the gyro data" - default_value: OFF + default_value: ON condition: USE_GYRO_KALMAN field: kalmanEnabled type: bool @@ -864,18 +836,6 @@ groups: field: motorPwmRate min: 50 max: 32000 - - name: motor_accel_time - description: "Minimum time for the motor(s) to accelerate from 0 to 100% throttle (ms) [0-1000]" - default_value: 0 - field: motorAccelTimeMs - min: 0 - max: 1000 - - name: motor_decel_time - description: "Minimum time for the motor(s) to deccelerate from 100 to 0% throttle (ms) [0-1000]" - default_value: 0 - field: motorDecelTimeMs - min: 0 - max: 1000 - name: motor_pwm_protocol description: "Protocol that is used to send motor updates to ESCs. Possible values - STANDARD, ONESHOT125, ONESHOT42, MULTISHOT, DSHOT150, DSHOT300, DSHOT600, DSHOT1200, BRUSHED" default_value: "ONESHOT125" @@ -914,7 +874,7 @@ groups: max: 300 - name: failsafe_procedure description: "What failsafe procedure to initiate in Stage 2. See [Failsafe documentation](Failsafe.md#failsafe_throttle)." - default_value: "SET-THR" + default_value: "LAND" table: failsafe_procedure - name: failsafe_stick_threshold description: "Threshold for stick motion to consider failsafe condition resolved. If non-zero failsafe won't clear even if RC link is restored - you have to move sticks to exit failsafe." @@ -923,17 +883,17 @@ groups: min: 0 max: 500 - name: failsafe_fw_roll_angle - description: "Amount of banking when `SET-THR` failsafe is active on a fixed-wing machine. In 1/10 deg (deci-degrees). Negative values = left roll" + description: "Amount of banking when `LAND` (or old `SET-THR`) failsafe is active on a fixed-wing machine. In 1/10 deg (deci-degrees). Negative values = left roll" default_value: -200 min: -800 max: 800 - name: failsafe_fw_pitch_angle - description: "Amount of dive/climb when `SET-THR` failsafe is active on a fixed-wing machine. In 1/10 deg (deci-degrees). Negative values = climb" + description: "Amount of dive/climb when `LAND` (or old `SET-THR`) failsafe is active on a fixed-wing machine. In 1/10 deg (deci-degrees). Negative values = climb" default_value: 100 min: -800 max: 800 - name: failsafe_fw_yaw_rate - description: "Requested yaw rate to execute when `SET-THR` failsafe is active on a fixed-wing machine. In deg/s. Negative values = left turn" + description: "Requested yaw rate to execute when `LAND` (or old `SET-THR`) failsafe is active on a fixed-wing machine. In deg/s. Negative values = left turn" default_value: -45 min: -1000 max: 1000 @@ -1150,7 +1110,7 @@ groups: field: motor.turtleModePowerFactor default_value: 55 description: "Turtle mode power factor" - condition: "USE_DSHOT" + condition: USE_DSHOT min: 0 max: 100 - name: failsafe_throttle @@ -1414,7 +1374,7 @@ groups: max: 180 - name: manual_rc_expo description: "Exposition value used for the PITCH/ROLL axes by the `MANUAL` flight mode [0-100]" - default_value: 70 + default_value: 35 field: manual.rcExpo8 min: 0 max: 100 @@ -1447,6 +1407,48 @@ groups: min: 0 max: 50 default_value: 0 + - name: rate_dynamics_center_sensitivity + field: rateDynamics.sensitivityCenter + default_value: 100 + min: 25 + max: 175 + description: "The center stick sensitivity for Rate Dynamics" + condition: USE_RATE_DYNAMICS + - name: rate_dynamics_end_sensitivity + field: rateDynamics.sensitivityEnd + default_value: 100 + min: 25 + max: 175 + description: "The end stick sensitivity for Rate Dynamics" + condition: USE_RATE_DYNAMICS + - name: rate_dynamics_center_correction + field: rateDynamics.correctionCenter + default_value: 10 + min: 10 + max: 95 + description: "The center stick correction for Rate Dynamics" + condition: USE_RATE_DYNAMICS + - name: rate_dynamics_end_correction + field: rateDynamics.correctionEnd + default_value: 10 + min: 10 + max: 95 + description: "The end stick correction for Rate Dynamics" + condition: USE_RATE_DYNAMICS + - name: rate_dynamics_center_weight + field: rateDynamics.weightCenter + default_value: 0 + min: 0 + max: 95 + description: "The center stick weight for Rate Dynamics" + condition: USE_RATE_DYNAMICS + - name: rate_dynamics_end_weight + field: rateDynamics.weightEnd + default_value: 0 + min: 0 + max: 95 + description: "The end stick weight for Rate Dynamics" + condition: USE_RATE_DYNAMICS - name: PG_SERIAL_CONFIG type: serialConfig_t @@ -1464,7 +1466,7 @@ groups: members: - name: imu_dcm_kp description: "Inertial Measurement Unit KP Gain for accelerometer measurements" - default_value: 2500 + default_value: 1000 field: dcm_kp_acc max: UINT16_MAX - name: imu_dcm_ki @@ -1474,7 +1476,7 @@ groups: max: UINT16_MAX - name: imu_dcm_kp_mag description: "Inertial Measurement Unit KP Gain for compass measurements" - default_value: 10000 + default_value: 5000 field: dcm_kp_mag max: UINT16_MAX - name: imu_dcm_ki_mag @@ -1857,26 +1859,26 @@ groups: max: 900 - name: dterm_lpf_hz description: "Dterm low pass filter cutoff frequency. Default setting is very conservative and small multirotors should use higher value between 80 and 100Hz. 80 seems like a gold spot for 7-inch builds while 100 should work best with 5-inch machines. If motors are getting too hot, lower the value" - default_value: 40 + default_value: 110 min: 0 max: 500 - name: dterm_lpf_type - description: "Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation." - default_value: "BIQUAD" + description: "Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`, `PT2`, `PT3`." + default_value: "PT2" field: dterm_lpf_type - table: filter_type + table: filter_type_full - name: dterm_lpf2_hz description: "Cutoff frequency for stage 2 D-term low pass filter" default_value: 0 min: 0 max: 500 - name: dterm_lpf2_type - description: "Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation." - default_value: "BIQUAD" + description: "Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`, `PT2`, `PT3`." + default_value: "PT1" field: dterm_lpf2_type - table: filter_type + table: filter_type_full - name: yaw_lpf_hz - description: "Yaw low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below)" + description: "Yaw P term low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below)" default_value: 0 min: 0 max: 200 @@ -2134,8 +2136,14 @@ groups: min: 1 max: 100 default_value: 15 - - name: d_boost_factor - field: dBoostFactor + - name: d_boost_min + field: dBoostMin + condition: USE_D_BOOST + min: 0 + max: 1 + default_value: 0.5 + - name: d_boost_max + field: dBoostMax condition: USE_D_BOOST min: 1 max: 3 @@ -2223,7 +2231,7 @@ groups: condition: USE_AUTOTUNE_FIXED_WING members: - name: fw_autotune_min_stick - description: "Minimum stick input [%] to consider overshoot/undershoot detection" + description: "Minimum stick input [%], after applying deadband and expo, to start recording the plane's response to stick input." default_value: 50 field: fw_min_stick min: 0 @@ -2254,7 +2262,7 @@ groups: type: uint8_t - name: fw_autotune_max_rate_deflection description: "The target percentage of maximum mixer output used for determining the rates in `AUTO` and `LIMIT`." - default_value: 90 + default_value: 80 field: fw_max_rate_deflection min: 50 max: 100 @@ -2443,8 +2451,14 @@ groups: field: general.flags.waypoint_mission_restart table: nav_wp_mission_restart - name: nav_auto_speed - description: "Maximum velocity firmware is allowed in full auto modes (RTH, WP) [cm/s] [Multirotor only]" + description: "Speed in fully autonomous modes (RTH, WP) [cm/s]. Used for WP mode when no specific WP speed set. [Multirotor only]" default_value: 300 + field: general.auto_speed + min: 10 + max: 2000 + - name: nav_max_auto_speed + description: "Maximum speed allowed in fully autonomous modes (RTH, WP) [cm/s] [Multirotor only]" + default_value: 1000 field: general.max_auto_speed min: 10 max: 2000 @@ -2455,7 +2469,7 @@ groups: min: 10 max: 2000 - name: nav_manual_speed - description: "Maximum velocity firmware is allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only]" + description: "Maximum speed allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only]" default_value: 500 field: general.max_manual_speed min: 10 @@ -3081,7 +3095,12 @@ groups: field: stats_min_voltage_unit table: osd_stats_min_voltage_unit type: uint8_t - + - name: osd_stats_page_auto_swap_time + description: "Auto swap display time interval between disarm stats pages (seconds). Reverts to manual control when Roll stick used to change pages. Disabled when set to 0." + default_value: 3 + field: stats_page_auto_swap_time + min: 0 + max: 10 - name: osd_rssi_alarm description: "Value below which to make the OSD RSSI indicator blink" default_value: 20 @@ -3305,7 +3324,6 @@ groups: description: "How many navigation waypoints are displayed, set to 0 (zero) to disable. As sample, if set to 2, and you just passed the 3rd waypoint of the mission, you'll see markers for the 4th waypoint (marked 1) and the 5th waypoint (marked 2)" default_value: 0 field: hud_wp_disp - min: 0 max: 3 - name: osd_left_sidebar_scroll @@ -3441,6 +3459,13 @@ groups: min: -36 max: 36 + - name: osd_esc_rpm_precision + description: Number of characters used to display the RPM value. + field: esc_rpm_precision + min: 3 + max: 6 + default_value: 3 + - name: PG_OSD_COMMON_CONFIG type: osdCommonConfig_t headers: ["io/osd_common.h"] @@ -3742,6 +3767,11 @@ groups: default_value: 1 field: dshot_beeper_tone type: uint8_t + - name: beeper_pwm_mode + field: pwmMode + type: bool + default_value: OFF + description: "Allows disabling PWM mode for beeper on some targets. Switch from ON to OFF if the external beeper sound is weak. Do not switch from OFF to ON without checking if the board supports PWM beeper mode" - name: PG_POWER_LIMITS_CONFIG type: powerLimitsConfig_t diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index fa1d31c34f..ee39176b5d 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -87,7 +87,9 @@ PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, typedef enum { FAILSAFE_CHANNEL_HOLD, // Hold last known good value FAILSAFE_CHANNEL_NEUTRAL, // RPY = zero, THR = zero +#if !defined(USE_NAV) FAILSAFE_CHANNEL_AUTO, // Defined by failsafe configured values +#endif } failsafeChannelBehavior_e; typedef struct { @@ -98,14 +100,24 @@ typedef struct { static const failsafeProcedureLogic_t failsafeProcedureLogic[] = { [FAILSAFE_PROCEDURE_AUTO_LANDING] = { - .bypassNavigation = true, .forceAngleMode = true, +#if defined(USE_NAV) + .bypassNavigation = false, + .channelBehavior = { + FAILSAFE_CHANNEL_NEUTRAL, // ROLL + FAILSAFE_CHANNEL_NEUTRAL, // PITCH + FAILSAFE_CHANNEL_NEUTRAL, // YAW + FAILSAFE_CHANNEL_HOLD // THROTTLE + } +#else + .bypassNavigation = true, .channelBehavior = { FAILSAFE_CHANNEL_AUTO, // ROLL FAILSAFE_CHANNEL_AUTO, // PITCH FAILSAFE_CHANNEL_AUTO, // YAW FAILSAFE_CHANNEL_AUTO // THROTTLE } +#endif }, [FAILSAFE_PROCEDURE_DROP_IT] = { @@ -216,9 +228,16 @@ bool failsafeRequiresAngleMode(void) bool failsafeRequiresMotorStop(void) { +#if defined(USE_NAV) + return failsafeState.active && + failsafeState.activeProcedure == FAILSAFE_PROCEDURE_AUTO_LANDING && + posControl.flags.estAltStatus < EST_USABLE && + currentBatteryProfile->failsafe_throttle < getThrottleIdleValue(); +#else return failsafeState.active && failsafeState.activeProcedure == FAILSAFE_PROCEDURE_AUTO_LANDING && currentBatteryProfile->failsafe_throttle < getThrottleIdleValue(); +#endif } void failsafeStartMonitoring(void) @@ -258,6 +277,7 @@ void failsafeUpdateRcCommandValues(void) void failsafeApplyControlInput(void) { +#if !defined(USE_NAV) // Prepare FAILSAFE_CHANNEL_AUTO values for rcCommand int16_t autoRcCommand[4]; if (STATE(FIXED_WING_LEGACY)) { @@ -272,7 +292,7 @@ void failsafeApplyControlInput(void) } autoRcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle; } - +#endif // Apply channel values for (int idx = 0; idx < 4; idx++) { switch (failsafeProcedureLogic[failsafeState.activeProcedure].channelBehavior[idx]) { @@ -293,10 +313,11 @@ void failsafeApplyControlInput(void) break; } break; - +#if !defined(USE_NAV) case FAILSAFE_CHANNEL_AUTO: rcCommand[idx] = autoRcCommand[idx]; break; +#endif } } } @@ -357,7 +378,7 @@ static bool failsafeCheckStickMotion(void) static failsafeProcedure_e failsafeChooseFailsafeProcedure(void) { - if (FLIGHT_MODE(NAV_WP_MODE) && !failsafeConfig()->failsafe_mission) { + if ((FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive()) && !failsafeConfig()->failsafe_mission) { return FAILSAFE_PROCEDURE_NONE; } @@ -438,8 +459,11 @@ void failsafeUpdateState(void) switch (failsafeState.activeProcedure) { case FAILSAFE_PROCEDURE_AUTO_LANDING: - // Stabilize, and set Throttle to specified level + // Use Emergency Landing if Nav defined (otherwise stabilize and set Throttle to specified level). failsafeActivate(FAILSAFE_LANDING); +#if defined(USE_NAV) + activateForcedEmergLanding(); +#endif break; case FAILSAFE_PROCEDURE_DROP_IT: @@ -513,16 +537,42 @@ void failsafeUpdateState(void) case FAILSAFE_LANDING: if (receivingRxDataAndNotFailsafeMode && sticksAreMoving) { +#if defined(USE_NAV) + abortForcedEmergLanding(); +#endif failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; reprocessState = true; - } - if (armed) { - beeperMode = BEEPER_RX_LOST_LANDING; - } - if (failsafeShouldHaveCausedLandingByNow() || !armed) { - failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_30_SECONDS; // require 30 seconds of valid rxData - failsafeState.phase = FAILSAFE_LANDED; - reprocessState = true; + } else { + if (armed) { + beeperMode = BEEPER_RX_LOST_LANDING; + } +#if defined(USE_NAV) + bool emergLanded = false; + switch (getStateOfForcedEmergLanding()) { + case EMERG_LAND_IN_PROGRESS: + break; + + case EMERG_LAND_HAS_LANDED: + emergLanded = true; + break; + + case EMERG_LAND_IDLE: + default: + // If emergency landing was somehow aborted during failsafe - fallback to FAILSAFE_PROCEDURE_DROP_IT + abortForcedEmergLanding(); + failsafeSetActiveProcedure(FAILSAFE_PROCEDURE_DROP_IT); + failsafeActivate(FAILSAFE_LANDED); + reprocessState = true; + break; + } + if (emergLanded || failsafeShouldHaveCausedLandingByNow() || !armed) { +#else + if (failsafeShouldHaveCausedLandingByNow() || !armed) { +#endif + failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_30_SECONDS; // require 30 seconds of valid rxData + failsafeState.phase = FAILSAFE_LANDED; + reprocessState = true; + } } break; diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index e2ab280ac1..be324b29f6 100644 --- a/src/main/flight/failsafe.h +++ b/src/main/flight/failsafe.h @@ -48,11 +48,12 @@ typedef struct failsafeConfig_s { PG_DECLARE(failsafeConfig_t, failsafeConfig); typedef enum { + FAILSAFE_IDLE = 0, /* Failsafe mode is not active. All other * phases indicate that the failsafe flight * mode is active. */ - FAILSAFE_IDLE = 0, + FAILSAFE_RX_LOSS_DETECTED, /* In this phase, the connection from the receiver * has been confirmed as lost and it will either * transition into FAILSAFE_RX_LOSS_RECOVERED if the @@ -61,7 +62,7 @@ typedef enum { * failsafe_procedure) or into FAILSAFE_RX_LOSS_IDLE * if failsafe_procedure is NONE. */ - FAILSAFE_RX_LOSS_DETECTED, + FAILSAFE_RX_LOSS_IDLE, /* This phase will just do nothing else than wait * until the RX connection is re-established and the * sticks are moved more than the failsafe_stick_threshold @@ -69,8 +70,8 @@ typedef enum { * Note that this phase is only used when * failsafe_procedure = NONE. */ - FAILSAFE_RX_LOSS_IDLE, #if defined(USE_NAV) + FAILSAFE_RETURN_TO_HOME, /* Failsafe is executing RTH. This phase is the first one * enabled when failsafe_procedure = RTH if an RTH is * deemed possible (RTH might not be activated if e.g. @@ -78,36 +79,36 @@ typedef enum { * sensors are not working at the moment). If RTH can't * be started, this phase will transition to FAILSAFE_LANDING. */ - FAILSAFE_RETURN_TO_HOME, #endif - /* Failsafe mode is performing a simplified landing procedure. + FAILSAFE_LANDING, + /* Pergorms NAV Emergency Landing if USE_NAV defined. + * Otherwise Failsafe mode performs a simplified landing procedure. * This is done by setting throttle and roll/pitch/yaw controls * to a pre-configured values that will allow aircraft * to reach ground in somewhat safe "controlled crash" way. * This is the first recovery phase enabled when - * failsafe_procedure = SET-THR. Once timeout expires or if a + * failsafe_procedure = LAND. Once timeout expires or if a * "controlled crash" can't be executed, this phase will * transition to FAILSAFE_LANDED. */ - FAILSAFE_LANDING, + FAILSAFE_LANDED, /* Failsafe has either detected that the model has landed and disabled * the motors or either decided to drop the model because it couldn't * perform an emergency landing. It will disarm, prevent re-arming * and transition into FAILSAFE_RX_LOSS_MONITORING immediately. This is * the first recovery phase enabled when failsafe_procedure = DROP. */ - FAILSAFE_LANDED, + FAILSAFE_RX_LOSS_MONITORING, /* This phase will wait until the RX connection is * working for some time and if and only if switch arming * is used and the switch is in the unarmed position * will allow rearming again. */ - FAILSAFE_RX_LOSS_MONITORING, + FAILSAFE_RX_LOSS_RECOVERED /* This phase indicates that the RX link has been re-established and * it will immediately transition out of failsafe mode (phase will * transition to FAILSAFE_IDLE.) */ - FAILSAFE_RX_LOSS_RECOVERED } failsafePhase_e; typedef enum { @@ -122,12 +123,20 @@ typedef enum { FAILSAFE_PROCEDURE_NONE } failsafeProcedure_e; +#if defined(USE_NAV) typedef enum { RTH_IDLE = 0, // RTH is waiting RTH_IN_PROGRESS, // RTH is active RTH_HAS_LANDED // RTH is active and has landed. } rthState_e; +typedef enum { + EMERG_LAND_IDLE = 0, // Emergency landing is waiting + EMERG_LAND_IN_PROGRESS, // Emergency landing is active + EMERG_LAND_HAS_LANDED // Emergency landing is active and has landed. +} emergLandState_e; +#endif + typedef struct failsafeState_s { int16_t events; bool monitoring; // Flag that failsafe is monitoring RC link diff --git a/src/main/flight/gyroanalyse.c b/src/main/flight/gyroanalyse.c index b85d538a15..2037b4712c 100644 --- a/src/main/flight/gyroanalyse.c +++ b/src/main/flight/gyroanalyse.c @@ -46,6 +46,14 @@ FILE_COMPILE_FOR_SPEED #include "gyroanalyse.h" +enum { + STEP_ARM_CFFT_F32, + STEP_BITREVERSAL_AND_STAGE_RFFT_F32, + STEP_MAGNITUDE_AND_FREQUENCY, + STEP_UPDATE_FILTERS_AND_HANNING, + STEP_COUNT +}; + // The FFT splits the frequency domain into an number of bins // A sampling frequency of 1000 and max frequency of 500 at a window size of 32 gives 16 frequency bins each 31.25Hz wide // Eg [0,31), [31,62), [62, 93) etc @@ -54,62 +62,48 @@ FILE_COMPILE_FOR_SPEED #define FFT_BIN_COUNT (FFT_WINDOW_SIZE / 2) // smoothing frequency for FFT centre frequency #define DYN_NOTCH_SMOOTH_FREQ_HZ 50 -// we need 4 steps for each axis -#define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * 4) + +/* + * Slow down gyro sample acquisition. This lowers the max frequency but increases the resolution. + * On default 500us looptime and denominator 1, max frequency is 1000Hz with a resolution of 31.25Hz + * On default 500us looptime and denominator 2, max frequency is 500Hz with a resolution of 15.6Hz + */ +#define FFT_SAMPLING_DENOMINATOR 2 void gyroDataAnalyseStateInit( gyroAnalyseState_t *state, uint16_t minFrequency, - uint8_t range, uint32_t targetLooptimeUs ) { - state->fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_LOW; state->minFrequency = minFrequency; - if (range == DYN_NOTCH_RANGE_HIGH) { - state->fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_HIGH; - } - else if (range == DYN_NOTCH_RANGE_MEDIUM) { - state->fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_MEDIUM; - } - - // If we get at least 3 samples then use the default FFT sample frequency - // otherwise we need to calculate a FFT sample frequency to ensure we get 3 samples (gyro loops < 4K) - const int gyroLoopRateHz = lrintf((1.0f / targetLooptimeUs) * 1e6f); - - state->fftSamplingRateHz = MIN((gyroLoopRateHz / 3), state->fftSamplingRateHz); - - state->fftResolution = (float)state->fftSamplingRateHz / FFT_WINDOW_SIZE; + state->fftSamplingRateHz = 1e6f / targetLooptimeUs / FFT_SAMPLING_DENOMINATOR; + state->maxFrequency = state->fftSamplingRateHz / 2; //max possible frequency is half the sampling rate + state->fftResolution = (float)state->maxFrequency / FFT_BIN_COUNT; state->fftStartBin = state->minFrequency / lrintf(state->fftResolution); - state->maxFrequency = state->fftSamplingRateHz / 2; //Nyquist - for (int i = 0; i < FFT_WINDOW_SIZE; i++) { state->hanningWindow[i] = (0.5f - 0.5f * cos_approx(2 * M_PIf * i / (FFT_WINDOW_SIZE - 1))); } - const uint16_t samplingFrequency = 1000000 / targetLooptimeUs; - state->maxSampleCount = samplingFrequency / state->fftSamplingRateHz; - state->maxSampleCountRcp = 1.f / state->maxSampleCount; - arm_rfft_fast_init_f32(&state->fftInstance, FFT_WINDOW_SIZE); -// recalculation of filters takes 4 calls per axis => each filter gets updated every DYN_NOTCH_CALC_TICKS calls -// at 4khz gyro loop rate this means 4khz / 4 / 3 = 333Hz => update every 3ms -// for gyro rate > 16kHz, we have update frequency of 1kHz => 1ms - const float looptime = MAX(1000000u / state->fftSamplingRateHz, targetLooptimeUs * DYN_NOTCH_CALC_TICKS); + // Frequency filter is executed every 12 cycles. 4 steps per cycle, 3 axises + const uint32_t filterUpdateUs = targetLooptimeUs * STEP_COUNT * XYZ_AXIS_COUNT; + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { // any init value state->centerFreq[axis] = state->maxFrequency; state->prevCenterFreq[axis] = state->maxFrequency; - biquadFilterInitLPF(&state->detectedFrequencyFilter[axis], DYN_NOTCH_SMOOTH_FREQ_HZ, looptime); + + biquadFilterInitLPF(&state->detectedFrequencyFilter[axis], DYN_NOTCH_SMOOTH_FREQ_HZ, filterUpdateUs); } } void gyroDataAnalysePush(gyroAnalyseState_t *state, const int axis, const float sample) { - state->oversampledGyroAccumulator[axis] += sample; + state->currentSample[axis] = sample; } static void gyroDataAnalyseUpdate(gyroAnalyseState_t *state); @@ -121,160 +115,101 @@ void gyroDataAnalyse(gyroAnalyseState_t *state) { state->filterUpdateExecute = false; //This will be changed to true only if new data is present - // samples should have been pushed by `gyroDataAnalysePush` - // if gyro sampling is > 1kHz, accumulate multiple samples - state->sampleCount++; - - // this runs at 1kHz - if (state->sampleCount == state->maxSampleCount) { - state->sampleCount = 0; + static uint8_t samplingIndex = 0; + if (samplingIndex == 0) { // calculate mean value of accumulated samples for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - float sample = state->oversampledGyroAccumulator[axis] * state->maxSampleCountRcp; - state->downsampledGyroData[axis][state->circularBufferIdx] = sample; - - state->oversampledGyroAccumulator[axis] = 0; + state->downsampledGyroData[axis][state->circularBufferIdx] = state->currentSample[axis]; } state->circularBufferIdx = (state->circularBufferIdx + 1) % FFT_WINDOW_SIZE; - - // We need DYN_NOTCH_CALC_TICKS tick to update all axis with newly sampled value - state->updateTicks = DYN_NOTCH_CALC_TICKS; } - // calculate FFT and update filters - if (state->updateTicks > 0) { - gyroDataAnalyseUpdate(state); - --state->updateTicks; - } + samplingIndex = (samplingIndex + 1) % FFT_SAMPLING_DENOMINATOR; + + gyroDataAnalyseUpdate(state); } void stage_rfft_f32(arm_rfft_fast_instance_f32 *S, float32_t *p, float32_t *pOut); -void arm_cfft_radix8by2_f32(arm_cfft_instance_f32 *S, float32_t *p1); void arm_cfft_radix8by4_f32(arm_cfft_instance_f32 *S, float32_t *p1); -void arm_radix8_butterfly_f32(float32_t *pSrc, uint16_t fftLen, const float32_t *pCoef, uint16_t twidCoefModifier); void arm_bitreversal_32(uint32_t *pSrc, const uint16_t bitRevLen, const uint16_t *pBitRevTable); +static uint8_t findPeakBinIndex(gyroAnalyseState_t *state) { + uint8_t peakBinIndex = state->fftStartBin; + float peakValue = 0; + for (int i = state->fftStartBin; i < FFT_BIN_COUNT; i++) { + if (state->fftData[i] > peakValue) { + peakValue = state->fftData[i]; + peakBinIndex = i; + } + } + return peakBinIndex; +} + +static float computeParabolaMean(gyroAnalyseState_t *state, uint8_t peakBinIndex) { + float preciseBin = peakBinIndex; + + // Height of peak bin (y1) and shoulder bins (y0, y2) + const float y0 = state->fftData[peakBinIndex - 1]; + const float y1 = state->fftData[peakBinIndex]; + const float y2 = state->fftData[peakBinIndex - 1]; + + // Estimate true peak position aka. preciseBin (fit parabola y(x) over y0, y1 and y2, solve dy/dx=0 for x) + const float denom = 2.0f * (y0 - 2 * y1 + y2); + if (denom != 0.0f) { + preciseBin += (y0 - y2) / denom; + } + + return preciseBin; +} + /* * Analyse last gyro data from the last FFT_WINDOW_SIZE milliseconds */ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state) { - enum { - STEP_ARM_CFFT_F32, - STEP_BITREVERSAL, - STEP_STAGE_RFFT_F32, - STEP_ARM_CMPLX_MAG_F32, - STEP_CALC_FREQUENCIES, - STEP_UPDATE_FILTERS, - STEP_HANNING, - STEP_COUNT - }; arm_cfft_instance_f32 *Sint = &(state->fftInstance.Sint); switch (state->updateStep) { case STEP_ARM_CFFT_F32: { - switch (FFT_BIN_COUNT) { - case 16: - // 16us - arm_cfft_radix8by2_f32(Sint, state->fftData); - break; - case 32: - // 35us - arm_cfft_radix8by4_f32(Sint, state->fftData); - break; - case 64: - // 70us - arm_radix8_butterfly_f32(state->fftData, FFT_BIN_COUNT, Sint->pTwiddle, 1); - break; - } + // Important this works only with FFT windows size of 64 elements! + arm_cfft_radix8by4_f32(Sint, state->fftData); break; } - case STEP_BITREVERSAL: + case STEP_BITREVERSAL_AND_STAGE_RFFT_F32: { - // 6us arm_bitreversal_32((uint32_t*) state->fftData, Sint->bitRevLength, Sint->pBitRevTable); - state->updateStep++; - FALLTHROUGH; - } - case STEP_STAGE_RFFT_F32: - { - // 14us - // this does not work in place => fftData AND rfftData needed stage_rfft_f32(&state->fftInstance, state->fftData, state->rfftData); break; } - case STEP_ARM_CMPLX_MAG_F32: + case STEP_MAGNITUDE_AND_FREQUENCY: { // 8us arm_cmplx_mag_f32(state->rfftData, state->fftData, FFT_BIN_COUNT); - state->updateStep++; - FALLTHROUGH; - } - case STEP_CALC_FREQUENCIES: - { - bool fftIncreased = false; - float dataMax = 0; - uint8_t binStart = 0; - uint8_t binMax = 0; - //for bins after initial decline, identify start bin and max bin - for (int i = state->fftStartBin; i < FFT_BIN_COUNT; i++) { - if (fftIncreased || (state->fftData[i] > state->fftData[i - 1])) { - if (!fftIncreased) { - binStart = i; // first up-step bin - fftIncreased = true; - } - if (state->fftData[i] > dataMax) { - dataMax = state->fftData[i]; - binMax = i; // tallest bin - } - } - } - // accumulate fftSum and fftWeightedSum from peak bin, and shoulder bins either side of peak - float cubedData = state->fftData[binMax] * state->fftData[binMax] * state->fftData[binMax]; - float fftSum = cubedData; - float fftWeightedSum = cubedData * (binMax + 1); - // accumulate upper shoulder - for (int i = binMax; i < FFT_BIN_COUNT - 1; i++) { - if (state->fftData[i] > state->fftData[i + 1]) { - cubedData = state->fftData[i] * state->fftData[i] * state->fftData[i]; - fftSum += cubedData; - fftWeightedSum += cubedData * (i + 1); - } else { - break; - } - } - // accumulate lower shoulder - for (int i = binMax; i > binStart + 1; i--) { - if (state->fftData[i] > state->fftData[i - 1]) { - cubedData = state->fftData[i] * state->fftData[i] * state->fftData[i]; - fftSum += cubedData; - fftWeightedSum += cubedData * (i + 1); - } else { - break; - } - } - // get weighted center of relevant frequency range (this way we have a better resolution than 31.25Hz) - float centerFreq = state->maxFrequency; - float fftMeanIndex = 0; - // idx was shifted by 1 to start at 1, not 0 - if (fftSum > 0) { - fftMeanIndex = (fftWeightedSum / fftSum) - 1; - // the index points at the center frequency of each bin so index 0 is actually 16.125Hz - centerFreq = fftMeanIndex * state->fftResolution; - } else { - centerFreq = state->prevCenterFreq[state->updateAxis]; - } - centerFreq = fmax(centerFreq, state->minFrequency); - centerFreq = biquadFilterApply(&state->detectedFrequencyFilter[state->updateAxis], centerFreq); + + //Find peak frequency + uint8_t peakBin = findPeakBinIndex(state); + + // Failsafe to ensure the last bin is not a peak bin + peakBin = constrain(peakBin, state->fftStartBin, FFT_BIN_COUNT - 1); + + /* + * Calculate center frequency using the parabola method + */ + float preciseBin = computeParabolaMean(state, peakBin); + float peakFrequency = preciseBin * state->fftResolution; + + peakFrequency = biquadFilterApply(&state->detectedFrequencyFilter[state->updateAxis], peakFrequency); + peakFrequency = constrainf(peakFrequency, state->minFrequency, state->maxFrequency); + state->prevCenterFreq[state->updateAxis] = state->centerFreq[state->updateAxis]; - state->centerFreq[state->updateAxis] = centerFreq; + state->centerFreq[state->updateAxis] = peakFrequency; break; } - case STEP_UPDATE_FILTERS: + case STEP_UPDATE_FILTERS_AND_HANNING: { // 7us // calculate cutoffFreq and notch Q, update notch filter =1.8+((A2-150)*0.004) @@ -287,20 +222,12 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state) state->filterUpdateFrequency = state->centerFreq[state->updateAxis]; } + //Switch to the next axis state->updateAxis = (state->updateAxis + 1) % XYZ_AXIS_COUNT; - state->updateStep++; - FALLTHROUGH; - } - case STEP_HANNING: - { - // 5us + // apply hanning window to gyro samples and store result in fftData // hanning starts and ends with 0, could be skipped for minor speed improvement - const uint8_t ringBufIdx = FFT_WINDOW_SIZE - state->circularBufferIdx; - arm_mult_f32(&state->downsampledGyroData[state->updateAxis][state->circularBufferIdx], &state->hanningWindow[0], &state->fftData[0], ringBufIdx); - if (state->circularBufferIdx > 0) { - arm_mult_f32(&state->downsampledGyroData[state->updateAxis][0], &state->hanningWindow[ringBufIdx], &state->fftData[ringBufIdx], state->circularBufferIdx); - } + arm_mult_f32(state->downsampledGyroData[state->updateAxis], state->hanningWindow, state->fftData, FFT_WINDOW_SIZE); } } diff --git a/src/main/flight/gyroanalyse.h b/src/main/flight/gyroanalyse.h index b6de10e40a..0d95be42cc 100644 --- a/src/main/flight/gyroanalyse.h +++ b/src/main/flight/gyroanalyse.h @@ -25,15 +25,15 @@ #include "arm_math.h" #include "common/filter.h" -// max for F3 targets -#define FFT_WINDOW_SIZE 32 +/* + * Current code works only with 64 window size. Changing it do a different size would require + * adapting the gyroDataAnalyseUpdate in STEP_ARM_CFFT_F32 step + */ +#define FFT_WINDOW_SIZE 64 typedef struct gyroAnalyseState_s { // accumulator for oversampled data => no aliasing and less noise - uint8_t sampleCount; - uint8_t maxSampleCount; - float maxSampleCountRcp; - float oversampledGyroAccumulator[XYZ_AXIS_COUNT]; + float currentSample[XYZ_AXIS_COUNT]; // downsampled gyro data circular buffer for frequency analysis uint8_t circularBufferIdx; @@ -69,7 +69,6 @@ STATIC_ASSERT(FFT_WINDOW_SIZE <= (uint8_t) -1, window_size_greater_than_underlyi void gyroDataAnalyseStateInit( gyroAnalyseState_t *state, uint16_t minFrequency, - uint8_t range, uint32_t targetLooptimeUs ); void gyroDataAnalysePush(gyroAnalyseState_t *gyroAnalyse, int axis, float sample); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index b92c999e3b..43d399a13f 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -102,15 +102,13 @@ PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig, #define DEFAULT_MAX_THROTTLE 1850 -PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 8); +PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 9); PG_RESET_TEMPLATE(motorConfig_t, motorConfig, .motorPwmProtocol = SETTING_MOTOR_PWM_PROTOCOL_DEFAULT, .motorPwmRate = SETTING_MOTOR_PWM_RATE_DEFAULT, .maxthrottle = SETTING_MAX_THROTTLE_DEFAULT, .mincommand = SETTING_MIN_COMMAND_DEFAULT, - .motorAccelTimeMs = SETTING_MOTOR_ACCEL_TIME_DEFAULT, - .motorDecelTimeMs = SETTING_MOTOR_DECEL_TIME_DEFAULT, .motorPoleCount = SETTING_MOTOR_POLES_DEFAULT, // Most brushless motors that we use are 14 poles ); @@ -118,9 +116,6 @@ PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTO #define CRASH_OVER_AFTER_CRASH_FLIP_STICK_MIN 0.15f -typedef void (*motorRateLimitingApplyFnPtr)(const float dT); -static EXTENDED_FASTRAM motorRateLimitingApplyFnPtr motorRateLimitingApplyFn; - int getThrottleIdleValue(void) { if (!throttleIdleValue) { @@ -201,44 +196,6 @@ void nullMotorRateLimiting(const float dT) UNUSED(dT); } -void applyMotorRateLimiting(const float dT) -{ - static float motorPrevious[MAX_SUPPORTED_MOTORS] = { 0 }; - - if (feature(FEATURE_REVERSIBLE_MOTORS)) { - // FIXME: Don't apply rate limiting in 3D mode - for (int i = 0; i < motorCount; i++) { - motorPrevious[i] = motor[i]; - } - } - else { - // Calculate max motor step - const uint16_t motorRange = motorConfig()->maxthrottle - throttleIdleValue; - const float motorMaxInc = (motorConfig()->motorAccelTimeMs == 0) ? 2000 : motorRange * dT / (motorConfig()->motorAccelTimeMs * 1e-3f); - const float motorMaxDec = (motorConfig()->motorDecelTimeMs == 0) ? 2000 : motorRange * dT / (motorConfig()->motorDecelTimeMs * 1e-3f); - - for (int i = 0; i < motorCount; i++) { - // Apply motor rate limiting - motorPrevious[i] = constrainf(motor[i], motorPrevious[i] - motorMaxDec, motorPrevious[i] + motorMaxInc); - - // Handle throttle below min_throttle (motor start/stop) - if (motorPrevious[i] < throttleIdleValue) { - if (motor[i] < throttleIdleValue) { - motorPrevious[i] = motor[i]; - } - else { - motorPrevious[i] = throttleIdleValue; - } - } - } - } - - // Update motor values - for (int i = 0; i < motorCount; i++) { - motor[i] = motorPrevious[i]; - } -} - void mixerInit(void) { computeMotorCount(); @@ -253,12 +210,6 @@ void mixerInit(void) mixerResetDisarmedMotors(); - if (motorConfig()->motorAccelTimeMs || motorConfig()->motorDecelTimeMs) { - motorRateLimitingApplyFn = applyMotorRateLimiting; - } else { - motorRateLimitingApplyFn = nullMotorRateLimiting; - } - if (mixerConfig()->motorDirectionInverted) { motorYawMultiplier = -1; } else { @@ -516,7 +467,7 @@ static int getReversibleMotorsThrottleDeadband(void) return feature(FEATURE_MOTOR_STOP) ? reversibleMotorsConfig()->neutral : directionValue; } -void FAST_CODE mixTable(const float dT) +void FAST_CODE mixTable() { #ifdef USE_DSHOT if (FLIGHT_MODE(TURTLE_MODE)) { @@ -649,9 +600,6 @@ void FAST_CODE mixTable(const float dT) motor[i] = motor_disarmed[i]; } } - - /* Apply motor acceleration/deceleration limit */ - motorRateLimitingApplyFn(dT); } int16_t getThrottlePercent(void) diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index e40842cb81..211923aa0e 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -80,8 +80,6 @@ typedef struct motorConfig_s { uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs uint16_t motorPwmRate; // The update rate of motor outputs (50-498Hz) uint8_t motorPwmProtocol; - uint16_t motorAccelTimeMs; // Time limit for motor to accelerate from 0 to 100% throttle [ms] - uint16_t motorDecelTimeMs; // Time limit for motor to decelerate from 0 to 100% throttle [ms] uint16_t digitalIdleOffsetValue; uint8_t motorPoleCount; // Magnetic poles in the motors for calculating actual RPM from eRPM provided by ESC telemetry } motorConfig_t; @@ -115,7 +113,7 @@ void writeAllMotors(int16_t mc); void mixerInit(void); void mixerUpdateStateFlags(void); void mixerResetDisarmedMotors(void); -void mixTable(const float dT); +void mixTable(void); void writeMotors(void); void processServoAutotrim(const float dT); void processServoAutotrimMode(void); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index d54f5ff443..cee8231c5f 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -68,6 +68,7 @@ FILE_COMPILE_FOR_SPEED #include "programming/logic_condition.h" typedef struct { + uint8_t axis; float kP; // Proportional gain float kI; // Integral gain float kD; // Derivative gain @@ -78,11 +79,6 @@ typedef struct { float gyroRate; float rateTarget; - // Buffer for derivative calculation -#define PID_GYRO_RATE_BUF_LENGTH 5 - float gyroRateBuf[PID_GYRO_RATE_BUF_LENGTH]; - firFilter_t gyroRateFilter; - // Rate integrator float errorGyroIf; float errorGyroIfLimit; @@ -104,13 +100,14 @@ typedef struct { #ifdef USE_D_BOOST pt1Filter_t dBoostLpf; biquadFilter_t dBoostGyroLpf; + float dBoostTargetAcceleration; #endif uint16_t pidSumLimit; filterApply4FnPtr ptermFilterApplyFn; bool itermLimitActive; bool itermFreezeActive; - biquadFilter_t rateTargetFilter; + pt3Filter_t rateTargetFilter; smithPredictor_t smithPredictor; } pidState_t; @@ -126,7 +123,11 @@ STATIC_FASTRAM bool pidGainsUpdateRequired; FASTRAM int16_t axisPID[FLIGHT_DYNAMICS_INDEX_COUNT]; #ifdef USE_BLACKBOX -int32_t axisPID_P[FLIGHT_DYNAMICS_INDEX_COUNT], axisPID_I[FLIGHT_DYNAMICS_INDEX_COUNT], axisPID_D[FLIGHT_DYNAMICS_INDEX_COUNT], axisPID_Setpoint[FLIGHT_DYNAMICS_INDEX_COUNT]; +int32_t axisPID_P[FLIGHT_DYNAMICS_INDEX_COUNT]; +int32_t axisPID_I[FLIGHT_DYNAMICS_INDEX_COUNT]; +int32_t axisPID_D[FLIGHT_DYNAMICS_INDEX_COUNT]; +int32_t axisPID_F[FLIGHT_DYNAMICS_INDEX_COUNT]; +int32_t axisPID_Setpoint[FLIGHT_DYNAMICS_INDEX_COUNT]; #endif static EXTENDED_FASTRAM pidState_t pidState[FLIGHT_DYNAMICS_INDEX_COUNT]; @@ -141,10 +142,11 @@ static EXTENDED_FASTRAM float antigravityAccelerator; #endif #define D_BOOST_GYRO_LPF_HZ 80 // Biquad lowpass input cutoff to peak D around propwash frequencies -#define D_BOOST_LPF_HZ 10 // PT1 lowpass cutoff to smooth the boost effect +#define D_BOOST_LPF_HZ 7 // PT1 lowpass cutoff to smooth the boost effect #ifdef USE_D_BOOST -static EXTENDED_FASTRAM float dBoostFactor; +static EXTENDED_FASTRAM float dBoostMin; +static EXTENDED_FASTRAM float dBoostMax; static EXTENDED_FASTRAM float dBoostMaxAtAlleceleration; #endif @@ -285,7 +287,8 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .iterm_relax = SETTING_MC_ITERM_RELAX_DEFAULT, #ifdef USE_D_BOOST - .dBoostFactor = SETTING_D_BOOST_FACTOR_DEFAULT, + .dBoostMin = SETTING_D_BOOST_MIN_DEFAULT, + .dBoostMax = SETTING_D_BOOST_MAX_DEFAULT, .dBoostMaxAtAlleceleration = SETTING_D_BOOST_MAX_AT_ACCELERATION_DEFAULT, .dBoostGyroDeltaLpfHz = SETTING_D_BOOST_GYRO_DELTA_LPF_HZ_DEFAULT, #endif @@ -310,6 +313,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, #endif ); +FUNCTION_COMPILE_FOR_SIZE bool pidInitFilters(void) { const uint32_t refreshRate = getLooptime(); @@ -318,26 +322,9 @@ bool pidInitFilters(void) return false; } - // Init other filters - if (pidProfile()->dterm_lpf_hz) { - for (int axis = 0; axis < 3; ++ axis) { - if (pidProfile()->dterm_lpf_type == FILTER_PT1) { - pt1FilterInit(&pidState[axis].dtermLpfState.pt1, pidProfile()->dterm_lpf_hz, refreshRate * 1e-6f); - } else { - biquadFilterInitLPF(&pidState[axis].dtermLpfState.biquad, pidProfile()->dterm_lpf_hz, refreshRate); - } - } - } - - // Init other filters - if (pidProfile()->dterm_lpf2_hz) { - for (int axis = 0; axis < 3; ++ axis) { - if (pidProfile()->dterm_lpf2_type == FILTER_PT1) { - pt1FilterInit(&pidState[axis].dtermLpf2State.pt1, pidProfile()->dterm_lpf2_hz, refreshRate * 1e-6f); - } else { - biquadFilterInitLPF(&pidState[axis].dtermLpf2State.biquad, pidProfile()->dterm_lpf2_hz, refreshRate); - } - } + for (int axis = 0; axis < 3; ++ axis) { + initFilter(pidProfile()->dterm_lpf_type, &pidState[axis].dtermLpfState, pidProfile()->dterm_lpf_hz, refreshRate); + initFilter(pidProfile()->dterm_lpf2_type, &pidState[axis].dtermLpf2State, pidProfile()->dterm_lpf2_hz, refreshRate); } for (int i = 0; i < XYZ_AXIS_COUNT; i++) { @@ -356,7 +343,7 @@ bool pidInitFilters(void) if (pidProfile()->controlDerivativeLpfHz) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - biquadFilterInitLPF(&pidState[axis].rateTargetFilter, pidProfile()->controlDerivativeLpfHz, getLooptime()); + pt3FilterInit(&pidState[axis].rateTargetFilter, pt3FilterGain(pidProfile()->controlDerivativeLpfHz, refreshRate * 1e-6f)); } } @@ -557,7 +544,7 @@ void updatePIDCoefficients() pidState[axis].kP = pidBank()->pid[axis].P / FP_PID_RATE_P_MULTIPLIER * axisTPA; pidState[axis].kI = pidBank()->pid[axis].I / FP_PID_RATE_I_MULTIPLIER; pidState[axis].kD = pidBank()->pid[axis].D / FP_PID_RATE_D_MULTIPLIER * axisTPA; - pidState[axis].kCD = pidBank()->pid[axis].FF / FP_PID_RATE_D_FF_MULTIPLIER * axisTPA; + pidState[axis].kCD = (pidBank()->pid[axis].FF / FP_PID_RATE_D_FF_MULTIPLIER * axisTPA) / (getLooptime() * 0.000001f); pidState[axis].kFF = 0.0f; // Tracking anti-windup requires P/I/D to be all defined which is only true for MC @@ -699,17 +686,21 @@ static float FAST_CODE applyDBoost(pidState_t *pidState, float dT) { float dBoost = 1.0f; - if (dBoostFactor > 1) { - const float dBoostGyroDelta = (pidState->gyroRate - pidState->previousRateGyro) / dT; - const float dBoostGyroAcceleration = fabsf(biquadFilterApply(&pidState->dBoostGyroLpf, dBoostGyroDelta)); - const float dBoostRateAcceleration = fabsf((pidState->rateTarget - pidState->previousRateTarget) / dT); + const float dBoostGyroDelta = (pidState->gyroRate - pidState->previousRateGyro) / dT; + const float dBoostGyroAcceleration = fabsf(biquadFilterApply(&pidState->dBoostGyroLpf, dBoostGyroDelta)); + const float dBoostRateAcceleration = fabsf((pidState->rateTarget - pidState->previousRateTarget) / dT); - const float acceleration = MAX(dBoostGyroAcceleration, dBoostRateAcceleration); - dBoost = scaleRangef(acceleration, 0.0f, dBoostMaxAtAlleceleration, 1.0f, dBoostFactor); - dBoost = pt1FilterApply4(&pidState->dBoostLpf, dBoost, D_BOOST_LPF_HZ, dT); - dBoost = constrainf(dBoost, 1.0f, dBoostFactor); + if (dBoostGyroAcceleration >= dBoostRateAcceleration) { + //Gyro is accelerating faster than setpoint, we want to smooth out + dBoost = scaleRangef(dBoostGyroAcceleration, 0.0f, dBoostMaxAtAlleceleration, 1.0f, dBoostMax); + } else { + //Setpoint is accelerating, we want to boost response + dBoost = scaleRangef(dBoostRateAcceleration, 0.0f, pidState->dBoostTargetAcceleration, 1.0f, dBoostMin); } + dBoost = pt1FilterApply4(&pidState->dBoostLpf, dBoost, D_BOOST_LPF_HZ, dT); + dBoost = constrainf(dBoost, dBoostMin, dBoostMax); + return dBoost; } #else @@ -728,7 +719,7 @@ static float dTermProcess(pidState_t *pidState, float dT) { newDTerm = 0; } else { float delta = pidState->previousRateGyro - pidState->gyroRate; - + delta = dTermLpfFilterApplyFn((filter_t *) &pidState->dtermLpfState, delta); delta = dTermLpf2FilterApplyFn((filter_t *) &pidState->dtermLpf2State, delta); @@ -760,7 +751,6 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh const float newDTerm = dTermProcess(pidState, dT); const float newFFTerm = pidState->rateTarget * pidState->kFF; - DEBUG_SET(DEBUG_FW_D, axis, newDTerm); /* * Integral should be updated only if axis Iterm is not frozen */ @@ -786,6 +776,7 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh axisPID_P[axis] = newPTerm; axisPID_I[axis] = pidState->errorGyroIf; axisPID_D[axis] = newDTerm; + axisPID_F[axis] = newFFTerm; axisPID_Setpoint[axis] = pidState->rateTarget; #endif @@ -816,19 +807,12 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid const float newDTerm = dTermProcess(pidState, dT); const float rateTargetDelta = pidState->rateTarget - pidState->previousRateTarget; - const float rateTargetDeltaFiltered = biquadFilterApply(&pidState->rateTargetFilter, rateTargetDelta); + const float rateTargetDeltaFiltered = pt3FilterApply(&pidState->rateTargetFilter, rateTargetDelta); /* * Compute Control Derivative - * CD is enabled only when ANGLE and HORIZON modes are not enabled! */ - float newCDTerm; - if (levelingEnabled) { - newCDTerm = 0.0F; - } else { - newCDTerm = rateTargetDeltaFiltered * (pidState->kCD / dT); - } - DEBUG_SET(DEBUG_CD, axis, newCDTerm); + const float newCDTerm = rateTargetDeltaFiltered * pidState->kCD; // TODO: Get feedback from mixer on available correction range for each axis const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf + newCDTerm; @@ -852,6 +836,7 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid axisPID_P[axis] = newPTerm; axisPID_I[axis] = pidState->errorGyroIf; axisPID_D[axis] = newDTerm; + axisPID_F[axis] = newCDTerm; axisPID_Setpoint[axis] = pidState->rateTarget; #endif @@ -1185,7 +1170,8 @@ void pidInit(void) motorItermWindupPoint = 1.0f - (pidProfile()->itermWindupPointPercent / 100.0f); #ifdef USE_D_BOOST - dBoostFactor = pidProfile()->dBoostFactor; + dBoostMin = pidProfile()->dBoostMin; + dBoostMax = pidProfile()->dBoostMax; dBoostMaxAtAlleceleration = pidProfile()->dBoostMaxAtAlleceleration; #endif @@ -1195,6 +1181,14 @@ void pidInit(void) #endif for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) { + + #ifdef USE_D_BOOST + // Rate * 10 * 10. First 10 is to convert stick to DPS. Second 10 is to convert target to acceleration. + // We assume, max acceleration is when pilot deflects the stick fully in 100ms + pidState[axis].dBoostTargetAcceleration = currentControlRateProfile->stabilized.rates[axis] * 10 * 10; + #endif + + pidState[axis].axis = axis; if (axis == FD_YAW) { pidState[axis].pidSumLimit = pidProfile()->pidSumLimitYaw; if (yawLpfHz) { @@ -1222,23 +1216,8 @@ void pidInit(void) usedPidControllerType = pidProfile()->pidControllerType; } - dTermLpfFilterApplyFn = nullFilterApply; - if (pidProfile()->dterm_lpf_hz) { - if (pidProfile()->dterm_lpf_type == FILTER_PT1) { - dTermLpfFilterApplyFn = (filterApplyFnPtr) pt1FilterApply; - } else { - dTermLpfFilterApplyFn = (filterApplyFnPtr) biquadFilterApply; - } - } - - dTermLpf2FilterApplyFn = nullFilterApply; - if (pidProfile()->dterm_lpf2_hz) { - if (pidProfile()->dterm_lpf2_type == FILTER_PT1) { - dTermLpf2FilterApplyFn = (filterApplyFnPtr) pt1FilterApply; - } else { - dTermLpf2FilterApplyFn = (filterApplyFnPtr) biquadFilterApply; - } - } + assignFilterApplyFn(pidProfile()->dterm_lpf_type, pidProfile()->dterm_lpf_hz, &dTermLpfFilterApplyFn); + assignFilterApplyFn(pidProfile()->dterm_lpf2_type, pidProfile()->dterm_lpf2_hz, &dTermLpf2FilterApplyFn); if (usedPidControllerType == PID_TYPE_PIFF) { pidControllerApplyFn = pidApplyFixedWingRateController; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 7e1ab17a76..8520ff5d0d 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -142,7 +142,8 @@ typedef struct pidProfile_s { uint8_t iterm_relax; // Enable iterm suppression during stick input #ifdef USE_D_BOOST - float dBoostFactor; + float dBoostMin; + float dBoostMax; float dBoostMaxAtAlleceleration; uint8_t dBoostGyroDeltaLpfHz; #endif @@ -188,7 +189,7 @@ const pidBank_t * pidBank(void); pidBank_t * pidBankMutable(void); extern int16_t axisPID[]; -extern int32_t axisPID_P[], axisPID_I[], axisPID_D[], axisPID_Setpoint[]; +extern int32_t axisPID_P[], axisPID_I[], axisPID_D[], axisPID_F[], axisPID_Setpoint[]; void pidInit(void); bool pidInitFilters(void); diff --git a/src/main/flight/pid_autotune.c b/src/main/flight/pid_autotune.c index a3cc96e811..151d23be05 100755 --- a/src/main/flight/pid_autotune.c +++ b/src/main/flight/pid_autotune.c @@ -248,13 +248,14 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa if (gainsUpdated) { // Set P-gain to 10% of FF gain (quite agressive - FIXME) tuneCurrent[axis].gainP = tuneCurrent[axis].gainFF * (pidAutotuneConfig()->fw_ff_to_p_gain / 100.0f); + tuneCurrent[axis].gainP = constrainf(tuneCurrent[axis].gainP, 1.0f, 20.0f); // Set D-gain relative to P-gain (0 for now) tuneCurrent[axis].gainD = tuneCurrent[axis].gainP * (pidAutotuneConfig()->fw_p_to_d_gain / 100.0f); // Set integrator gain to reach the same response as FF gain in 0.667 second tuneCurrent[axis].gainI = (tuneCurrent[axis].gainFF / FP_PID_RATE_FF_MULTIPLIER) * (1000.0f / pidAutotuneConfig()->fw_ff_to_i_time_constant) * FP_PID_RATE_I_MULTIPLIER; - tuneCurrent[axis].gainI = constrainf(tuneCurrent[axis].gainI, 2.0f, 50.0f); + tuneCurrent[axis].gainI = constrainf(tuneCurrent[axis].gainI, 2.0f, 30.0f); autotuneUpdateGains(tuneCurrent); switch (axis) { diff --git a/src/main/flight/rate_dynamics.c b/src/main/flight/rate_dynamics.c new file mode 100644 index 0000000000..f77fb03228 --- /dev/null +++ b/src/main/flight/rate_dynamics.c @@ -0,0 +1,88 @@ +/* + * This file is part of the INAV distribution https://github.com/iNavFlight/inav. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, version 3. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * The code in this file is a derivative work of EmuFlight distribution https://github.com/emuflight/EmuFlight/ + * + */ +#include "platform.h" + +#ifdef USE_RATE_DYNAMICS + +FILE_COMPILE_FOR_SPEED + +#include +#include "rate_dynamics.h" +#include "fc/controlrate_profile.h" +#include + +static FASTRAM float lastRcCommandData[3]; +static FASTRAM float iterm[3]; + +FAST_CODE static float calculateK(const float k, const float dT) { + if (k == 0.0f) { + return 0; + } + // scale so it feels like running at 62.5hz (16ms) regardless of the current rx rate + + // The original code is: + // const float rxRate = 1.0f / dT; + // const float rxRateFactor = (rxRate / 62.5f) * rxRate; + // const float freq = k / ((1.0f / rxRateFactor) * (1.0f - k)); + // const float RC = 1.0f / freq; + // return dT / (RC + dT); + + // This can be simplified to (while saving 128B of flash on F722): + + return k / (62.5f * dT * (1 - k) + k); +} + +FAST_CODE int applyRateDynamics(int rcCommand, const int axis, const float dT) { + if ( + currentControlRateProfile->rateDynamics.sensitivityCenter != 100 || + currentControlRateProfile->rateDynamics.sensitivityEnd != 100 || + currentControlRateProfile->rateDynamics.weightCenter > 0 || + currentControlRateProfile->rateDynamics.weightEnd > 0 + ) { + + float pterm_centerStick, pterm_endStick, pterm, iterm_centerStick, iterm_endStick, dterm_centerStick, dterm_endStick, dterm; + float rcCommandPercent; + float rcCommandError; + float inverseRcCommandPercent; + + rcCommandPercent = abs(rcCommand) / 500.0f; // make rcCommandPercent go from 0 to 1 + inverseRcCommandPercent = 1.0f - rcCommandPercent; + + pterm_centerStick = inverseRcCommandPercent * rcCommand * (currentControlRateProfile->rateDynamics.sensitivityCenter / 100.0f); // valid pterm values are between 50-150 + pterm_endStick = rcCommandPercent * rcCommand * (currentControlRateProfile->rateDynamics.sensitivityEnd / 100.0f); + pterm = pterm_centerStick + pterm_endStick; + rcCommandError = rcCommand - (pterm + iterm[axis]); + rcCommand = pterm; // add this fake pterm to the rcCommand + + iterm_centerStick = inverseRcCommandPercent * rcCommandError * calculateK(currentControlRateProfile->rateDynamics.correctionCenter / 100.0f, dT); // valid iterm values are between 0-95 + iterm_endStick = rcCommandPercent * rcCommandError * calculateK(currentControlRateProfile->rateDynamics.correctionEnd / 100.0f, dT); + iterm[axis] += iterm_centerStick + iterm_endStick; + rcCommand = rcCommand + iterm[axis]; // add the iterm to the rcCommand + + dterm_centerStick = inverseRcCommandPercent * (lastRcCommandData[axis] - rcCommand) * calculateK(currentControlRateProfile->rateDynamics.weightCenter / 100.0f, dT); // valid dterm values are between 0-95 + dterm_endStick = rcCommandPercent * (lastRcCommandData[axis] - rcCommand) * calculateK(currentControlRateProfile->rateDynamics.weightEnd / 100.0f, dT); + dterm = dterm_centerStick + dterm_endStick; + rcCommand = rcCommand + dterm; // add dterm to the rcCommand (this is real dterm) + + lastRcCommandData[axis] = rcCommand; + } + return rcCommand; +} + +#endif \ No newline at end of file diff --git a/src/main/flight/rate_dynamics.h b/src/main/flight/rate_dynamics.h new file mode 100644 index 0000000000..8ad92f54d3 --- /dev/null +++ b/src/main/flight/rate_dynamics.h @@ -0,0 +1,19 @@ +/* + * This file is part of the INAV distribution https://github.com/iNavFlight/inav. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, version 3. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * The code in this file is a derivative work of EmuFlight distribution https://github.com/emuflight/EmuFlight/ + * + */ +int applyRateDynamics(int rcCommand, const int axis, const float dT); diff --git a/src/main/flight/secondary_imu.c b/src/main/flight/secondary_imu.c index b8e5a20160..21a0f613af 100644 --- a/src/main/flight/secondary_imu.c +++ b/src/main/flight/secondary_imu.c @@ -95,10 +95,13 @@ void secondaryImuInit(void) calibrationData.radius[ACC] = secondaryImuConfig()->calibrationRadiusAcc; calibrationData.radius[MAG] = secondaryImuConfig()->calibrationRadiusMag; + requestedSensors[SENSOR_INDEX_IMU2] = secondaryImuConfig()->hardwareType; + if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055) { secondaryImuState.active = bno055Init(calibrationData, (secondaryImuConfig()->calibrationRadiusAcc && secondaryImuConfig()->calibrationRadiusMag)); if (secondaryImuState.active) { + detectedSensors[SENSOR_INDEX_IMU2] = SECONDARY_IMU_BNO055; rescheduleTask(TASK_SECONDARY_IMU, TASK_PERIOD_HZ(10)); } @@ -106,12 +109,13 @@ void secondaryImuInit(void) secondaryImuState.active = bno055SerialInit(calibrationData, (secondaryImuConfig()->calibrationRadiusAcc && secondaryImuConfig()->calibrationRadiusMag)); if (secondaryImuState.active) { + detectedSensors[SENSOR_INDEX_IMU2] = SECONDARY_IMU_BNO055_SERIAL; rescheduleTask(TASK_SECONDARY_IMU, TASK_PERIOD_HZ(50)); } } if (!secondaryImuState.active) { - secondaryImuConfigMutable()->hardwareType = SECONDARY_IMU_NONE; + detectedSensors[SENSOR_INDEX_IMU2] = SECONDARY_IMU_NONE; } } @@ -223,3 +227,33 @@ void secondaryImuFetchCalibration(void) { void secondaryImuSetMagneticDeclination(float declination) { //Incoming units are degrees secondaryImuState.magDeclination = declination * 10.0f; //Internally declination is stored in decidegrees } + +bool isSecondaryImuHealthy(void) { + return secondaryImuState.active; +} + +hardwareSensorStatus_e getHwSecondaryImuStatus(void) +{ +#ifdef USE_SECONDARY_IMU + if (detectedSensors[SENSOR_INDEX_IMU2] != SECONDARY_IMU_NONE) { + if (isSecondaryImuHealthy()) { + return HW_SENSOR_OK; + } + else { + return HW_SENSOR_UNHEALTHY; + } + } + else { + if (requestedSensors[SENSOR_INDEX_IMU2] != SECONDARY_IMU_NONE) { + // Selected but not detected + return HW_SENSOR_UNAVAILABLE; + } + else { + // Not selected and not detected + return HW_SENSOR_NONE; + } + } +#else + return HW_SENSOR_NONE; +#endif +} \ No newline at end of file diff --git a/src/main/flight/secondary_imu.h b/src/main/flight/secondary_imu.h index e7414a48c7..0471b0a7c4 100644 --- a/src/main/flight/secondary_imu.h +++ b/src/main/flight/secondary_imu.h @@ -29,6 +29,7 @@ #include "sensors/sensors.h" #include "drivers/accgyro/accgyro_bno055.h" #include "drivers/accgyro/accgyro_bno055_serial.h" +#include "sensors/diagnostics.h" typedef enum { SECONDARY_IMU_NONE = 0, @@ -66,4 +67,6 @@ void secondaryImuProcess(void); void secondaryImuInit(void); void taskSecondaryImu(timeUs_t currentTimeUs); void secondaryImuFetchCalibration(void); -void secondaryImuSetMagneticDeclination(float declination); \ No newline at end of file +void secondaryImuSetMagneticDeclination(float declination); +bool isSecondaryImuHealthy(void); +hardwareSensorStatus_e getHwSecondaryImuStatus(void); \ No newline at end of file diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 6fb74e319e..e318800903 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -105,9 +105,9 @@ int16_t servo[MAX_SUPPORTED_SERVOS]; static uint8_t servoRuleCount = 0; static servoMixer_t currentServoMixer[MAX_SERVO_RULES]; -static int servoOutputEnabled; +static bool servoOutputEnabled; -static uint8_t mixerUsesServos; +static bool mixerUsesServos; static uint8_t minServoIndex; static uint8_t maxServoIndex; @@ -151,8 +151,8 @@ void servosInit(void) // If there are servo rules after all, update variables if (servoRuleCount > 0) { - servoOutputEnabled = 1; - mixerUsesServos = 1; + servoOutputEnabled = true; + mixerUsesServos = true; } for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { diff --git a/src/main/io/asyncfatfs/asyncfatfs.c b/src/main/io/asyncfatfs/asyncfatfs.c index f4f8af6d33..90c211410d 100644 --- a/src/main/io/asyncfatfs/asyncfatfs.c +++ b/src/main/io/asyncfatfs/asyncfatfs.c @@ -2593,8 +2593,18 @@ static void afatfs_createFileContinue(afatfsFile_t *file) opState->phase = AFATFS_CREATEFILE_PHASE_FAILURE; goto doMore; } + } else if (entry->attrib & FAT_FILE_ATTRIBUTE_VOLUME_ID) { + break; } else if (strncmp(entry->filename, (char*) opState->filename, FAT_FILENAME_LENGTH) == 0) { - // We found a file with this name! + // We found a file or directory with this name! + + // Do not open file as dir or dir as file + if (((entry->attrib ^ file->attrib) & FAT_FILE_ATTRIBUTE_DIRECTORY) != 0) { + afatfs_findLast(&afatfs.currentDirectory); + opState->phase = AFATFS_CREATEFILE_PHASE_FAILURE; + goto doMore; + } + afatfs_fileLoadDirectoryEntry(file, entry); afatfs_findLast(&afatfs.currentDirectory); diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index cd7893f5e6..57cba56a61 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -157,8 +157,8 @@ static uint8_t beep_multiBeeps[MAX_MULTI_BEEPS + 2]; #define BEEPER_CONFIRMATION_BEEP_GAP_DURATION 20 -// Beeper off = 0 Beeper on = 1 -static uint8_t beeperIsOn = 0; +// Beeper off = false Beeper on = true +static bool beeperIsOn = false; // Place in current sequence static uint16_t beeperPos = 0; @@ -259,7 +259,7 @@ void beeperSilence(void) warningLedRefresh(); - beeperIsOn = 0; + beeperIsOn = false; beeperNextToggleTime = 0; beeperPos = 0; @@ -352,7 +352,7 @@ void beeperUpdate(timeUs_t currentTimeUs) } #endif - beeperIsOn = 1; + beeperIsOn = true; if (currentBeeperEntry->sequence[beeperPos] != 0) { if (!(getBeeperOffMask() & (1 << (currentBeeperEntry->mode - 1)))) BEEP_ON; @@ -368,7 +368,7 @@ void beeperUpdate(timeUs_t currentTimeUs) } } } else { - beeperIsOn = 0; + beeperIsOn = false; if (currentBeeperEntry->sequence[beeperPos] != 0) { BEEP_OFF; warningLedDisable(); diff --git a/src/main/io/esc_serialshot.c b/src/main/io/esc_serialshot.c deleted file mode 100644 index 29fe5d5232..0000000000 --- a/src/main/io/esc_serialshot.c +++ /dev/null @@ -1,116 +0,0 @@ -/* - * This file is part of INAV Project. - * - * This Source Code Form is subject to the terms of the Mozilla Public - * License, v. 2.0. If a copy of the MPL was not distributed with this file, - * You can obtain one at http://mozilla.org/MPL/2.0/. - * - * Alternatively, the contents of this file may be used under the terms - * of the GNU General Public License Version 3, as described below: - * - * This file is free software: you may copy, redistribute and/or modify - * it under the terms of the GNU General Public License as published by the - * Free Software Foundation, either version 3 of the License, or (at your - * option) any later version. - * - * This file is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General - * Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see http://www.gnu.org/licenses/. - */ - -#include -#include -#include -#include - -#include "platform.h" - -#include "build/build_config.h" -#include "build/debug.h" - -#include "common/maths.h" -#include "common/crc.h" - -#include "io/serial.h" -#include "io/esc_serialshot.h" - -#if defined(USE_SERIALSHOT) - -#define SERIALSHOT_UART_BAUD 921600 -#define SERIALSHOT_PKT_DEFAULT_HEADER (0x00) // Default header (motors 1-4, regular 4-in-1 ESC) - - -typedef struct __attribute__((packed)) { - uint8_t hdr; // Header/version marker - uint8_t motorData[6]; // 12 bit per motor - uint8_t crc; // CRC8/DVB-T of hdr & motorData -} serialShortPacket_t; - - -static serialShortPacket_t txPkt; -static uint16_t motorValues[4]; -static serialPort_t * escPort = NULL; -static serialPortConfig_t * portConfig; - -bool serialshotInitialize(void) -{ - // Avoid double initialization - if (escPort) { - return true; - } - - portConfig = findSerialPortConfig(FUNCTION_ESCSERIAL); - if (!portConfig) { - return false; - } - - escPort = openSerialPort(portConfig->identifier, FUNCTION_ESCSERIAL, NULL, NULL, SERIALSHOT_UART_BAUD, MODE_RXTX, SERIAL_NOT_INVERTED | SERIAL_UNIDIR); - if (!escPort) { - return false; - } - - return true; -} - -void serialshotUpdateMotor(int index, uint16_t value) -{ - if (index < 0 || index > 3) { - return; - } - - motorValues[index] = value; -} - -void serialshotSendUpdate(void) -{ - // Check if the port is initialized - if (!escPort) { - return; - } - - // Skip update if previous one is not yet fully sent - // This helps to avoid buffer overflow and evenyually the data corruption - if (!isSerialTransmitBufferEmpty(escPort)) { - return; - } - - txPkt.hdr = SERIALSHOT_PKT_DEFAULT_HEADER; - - txPkt.motorData[0] = motorValues[0] & 0x00FF; - txPkt.motorData[1] = motorValues[1] & 0x00FF; - txPkt.motorData[2] = motorValues[2] & 0x00FF; - txPkt.motorData[3] = motorValues[3] & 0x00FF; - txPkt.motorData[4] = (((motorValues[0] & 0xF00) >> 8) << 0) | (((motorValues[1] & 0xF00) >> 8) << 4); - txPkt.motorData[5] = (((motorValues[2] & 0xF00) >> 8) << 0) | (((motorValues[3] & 0xF00) >> 8) << 4); - - txPkt.crc = crc8_dvb_s2(0x00, txPkt.hdr); - txPkt.crc = crc8_dvb_s2_update(txPkt.crc, txPkt.motorData, sizeof(txPkt.motorData)); - - serialWriteBuf(escPort, (const uint8_t *)&txPkt, sizeof(txPkt)); -} - -#endif diff --git a/src/main/io/gps_naza.c b/src/main/io/gps_naza.c index 91e3d09fcd..9abef4c878 100644 --- a/src/main/io/gps_naza.c +++ b/src/main/io/gps_naza.c @@ -183,9 +183,9 @@ static bool NAZA_parse_gps(void) uint32_t v_acc = decodeLong(_buffernaza.nav.v_acc, mask); // mm //uint32_t test = decodeLong(_buffernaza.nav.reserved, mask); - gpsSol.velNED[0] = decodeLong(_buffernaza.nav.ned_north, mask); // cm/s - gpsSol.velNED[1] = decodeLong(_buffernaza.nav.ned_east, mask); // cm/s - gpsSol.velNED[2] = decodeLong(_buffernaza.nav.ned_down, mask); // cm/s + gpsSol.velNED[X] = decodeLong(_buffernaza.nav.ned_north, mask); // cm/s + gpsSol.velNED[Y] = decodeLong(_buffernaza.nav.ned_east, mask); // cm/s + gpsSol.velNED[Z] = decodeLong(_buffernaza.nav.ned_down, mask); // cm/s uint16_t pdop = decodeShort(_buffernaza.nav.pdop, mask); // pdop @@ -199,10 +199,10 @@ static bool NAZA_parse_gps(void) gpsSol.eph = gpsConstrainEPE(h_acc / 10); // hAcc in cm gpsSol.epv = gpsConstrainEPE(v_acc / 10); // vAcc in cm gpsSol.numSat = _buffernaza.nav.satellites; - gpsSol.groundSpeed = fast_fsqrtf(powf(gpsSol.velNED[0], 2)+powf(gpsSol.velNED[1], 2)); //cm/s + gpsSol.groundSpeed = fast_fsqrtf(powf(gpsSol.velNED[X], 2) + powf(gpsSol.velNED[Y], 2)); //cm/s // calculate gps heading from VELNE - gpsSol.groundCourse = (uint16_t) (fmodf(RADIANS_TO_DECIDEGREES(atan2_approx(gpsSol.velNED[1], gpsSol.velNED[0]))+3600.0f,3600.0f)); + gpsSol.groundCourse = (uint16_t)(fmodf(RADIANS_TO_DECIDEGREES(atan2_approx(gpsSol.velNED[Y], gpsSol.velNED[X])) + 3600.0f, 3600.0f)); gpsSol.flags.validVelNE = 1; gpsSol.flags.validVelD = 1; diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index a1d1378e5c..1dfa702bfa 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -587,9 +587,9 @@ static bool gpsParceFrameUBLOX(void) case MSG_VELNED: gpsSol.groundSpeed = _buffer.velned.speed_2d; // cm/s gpsSol.groundCourse = (uint16_t) (_buffer.velned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10 - gpsSol.velNED[0] = _buffer.velned.ned_north; - gpsSol.velNED[1] = _buffer.velned.ned_east; - gpsSol.velNED[2] = _buffer.velned.ned_down; + gpsSol.velNED[X] = _buffer.velned.ned_north; + gpsSol.velNED[Y] = _buffer.velned.ned_east; + gpsSol.velNED[Z] = _buffer.velned.ned_down; gpsSol.flags.validVelNE = 1; gpsSol.flags.validVelD = 1; _new_speed = true; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index dc3ba87a41..0ed029705e 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -145,7 +145,7 @@ FILE_COMPILE_FOR_SPEED #define OSD_CENTER_LEN(x) ((osdDisplayPort->cols - x) / 2) #define OSD_CENTER_S(s) OSD_CENTER_LEN(strlen(s)) -#define OSD_MIN_FONT_VERSION 2 +#define OSD_MIN_FONT_VERSION 3 static unsigned currentLayout = 0; static int layoutOverride = -1; @@ -156,6 +156,8 @@ static float GForce, GForceAxis[XYZ_AXIS_COUNT]; typedef struct statistic_s { uint16_t max_speed; + uint16_t max_3D_speed; + uint16_t max_air_speed; uint16_t min_voltage; // /100 int16_t max_current; int32_t max_power; @@ -317,6 +319,8 @@ static void osdLeftAlignString(char *buff) static void osdFormatDistanceSymbol(char *buff, int32_t dist, uint8_t decimals) { switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; case OSD_UNIT_IMPERIAL: if (osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(dist), FEET_PER_MILE, decimals, 3, 3)) { buff[3] = SYM_DIST_MI; @@ -325,7 +329,7 @@ static void osdFormatDistanceSymbol(char *buff, int32_t dist, uint8_t decimals) } buff[4] = '\0'; break; - case OSD_UNIT_UK: + case OSD_UNIT_METRIC_MPH: FALLTHROUGH; case OSD_UNIT_METRIC: if (osdFormatCentiNumber(buff, dist, METERS_PER_KILOMETER, decimals, 3, 3)) { @@ -335,6 +339,14 @@ static void osdFormatDistanceSymbol(char *buff, int32_t dist, uint8_t decimals) } buff[4] = '\0'; break; + case OSD_UNIT_GA: + if (osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(dist), FEET_PER_NAUTICALMILE, decimals, 3, 3)) { + buff[3] = SYM_DIST_NM; + } else { + buff[3] = SYM_DIST_FT; + } + buff[4] = '\0'; + break; } } @@ -344,32 +356,45 @@ static void osdFormatDistanceSymbol(char *buff, int32_t dist, uint8_t decimals) */ static void osdFormatDistanceStr(char *buff, int32_t dist) { - int32_t centifeet; - switch ((osd_unit_e)osdConfig()->units) { - case OSD_UNIT_IMPERIAL: - centifeet = CENTIMETERS_TO_CENTIFEET(dist); - if (abs(centifeet) < FEET_PER_MILE * 100 / 2) { - // Show feet when dist < 0.5mi - tfp_sprintf(buff, "%d%c", (int)(centifeet / 100), SYM_FT); - } else { - // Show miles when dist >= 0.5mi - tfp_sprintf(buff, "%d.%02d%c", (int)(centifeet / (100*FEET_PER_MILE)), - (abs(centifeet) % (100 * FEET_PER_MILE)) / FEET_PER_MILE, SYM_MI); + int32_t centifeet; + switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + centifeet = CENTIMETERS_TO_CENTIFEET(dist); + if (abs(centifeet) < FEET_PER_MILE * 100 / 2) { + // Show feet when dist < 0.5mi + tfp_sprintf(buff, "%d%c", (int)(centifeet / 100), SYM_FT); + } else { + // Show miles when dist >= 0.5mi + tfp_sprintf(buff, "%d.%02d%c", (int)(centifeet / (100*FEET_PER_MILE)), + (abs(centifeet) % (100 * FEET_PER_MILE)) / FEET_PER_MILE, SYM_MI); + } + break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + if (abs(dist) < METERS_PER_KILOMETER * 100) { + // Show meters when dist < 1km + tfp_sprintf(buff, "%d%c", (int)(dist / 100), SYM_M); + } else { + // Show kilometers when dist >= 1km + tfp_sprintf(buff, "%d.%02d%c", (int)(dist / (100*METERS_PER_KILOMETER)), + (abs(dist) % (100 * METERS_PER_KILOMETER)) / METERS_PER_KILOMETER, SYM_KM); + } + break; + case OSD_UNIT_GA: + centifeet = CENTIMETERS_TO_CENTIFEET(dist); + if (abs(centifeet) < 100000) { + // Show feet when dist < 1000ft + tfp_sprintf(buff, "%d%c", (int)(centifeet / 100), SYM_FT); + } else { + // Show nautical miles when dist >= 1000ft + tfp_sprintf(buff, "%d.%02d%c", (int)(centifeet / (100 * FEET_PER_NAUTICALMILE)), + (int)((abs(centifeet) % (int)(100 * FEET_PER_NAUTICALMILE)) / FEET_PER_NAUTICALMILE), SYM_NM); + } + break; } - break; - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_METRIC: - if (abs(dist) < METERS_PER_KILOMETER * 100) { - // Show meters when dist < 1km - tfp_sprintf(buff, "%d%c", (int)(dist / 100), SYM_M); - } else { - // Show kilometers when dist >= 1km - tfp_sprintf(buff, "%d.%02d%c", (int)(dist / (100*METERS_PER_KILOMETER)), - (abs(dist) % (100 * METERS_PER_KILOMETER)) / METERS_PER_KILOMETER, SYM_KM); - } - break; - } } /** @@ -381,10 +406,14 @@ static int32_t osdConvertVelocityToUnit(int32_t vel) switch ((osd_unit_e)osdConfig()->units) { case OSD_UNIT_UK: FALLTHROUGH; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; case OSD_UNIT_IMPERIAL: - return (vel * 224) / 10000; // Convert to mph + return CMSEC_TO_CENTIMPH(vel) / 100; // Convert to mph case OSD_UNIT_METRIC: - return (vel * 36) / 1000; // Convert to kmh + return CMSEC_TO_CENTIKPH(vel) / 100; // Convert to kmh + case OSD_UNIT_GA: + return CMSEC_TO_CENTIKNOTS(vel) / 100; // Convert to Knots } // Unreachable return -1; @@ -392,22 +421,49 @@ static int32_t osdConvertVelocityToUnit(int32_t vel) /** * Converts velocity into a string based on the current unit system. - * @param alt Raw velocity (i.e. as taken from gpsSol.groundSpeed in centimeters/seconds) + * @param vel Raw velocity (i.e. as taken from gpsSol.groundSpeed in centimeters/seconds) + * @param _3D is a 3D velocity + * @param _max is a maximum velocity */ -void osdFormatVelocityStr(char* buff, int32_t vel, bool _3D) +void osdFormatVelocityStr(char* buff, int32_t vel, bool _3D, bool _max) { switch ((osd_unit_e)osdConfig()->units) { case OSD_UNIT_UK: FALLTHROUGH; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; case OSD_UNIT_IMPERIAL: - tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_MPH : SYM_MPH)); + if (_max) { + tfp_sprintf(buff, "%c%3d%c", SYM_MAX, (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_MPH : SYM_MPH)); + } else { + tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_MPH : SYM_MPH)); + } break; case OSD_UNIT_METRIC: - tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_KMH : SYM_KMH)); + if (_max) { + tfp_sprintf(buff, "%c%3d%c", SYM_MAX, (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_KMH : SYM_KMH)); + } else { + tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_KMH : SYM_KMH)); + } + break; + case OSD_UNIT_GA: + if (_max) { + tfp_sprintf(buff, "%c%3d%c", SYM_MAX, (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_KT : SYM_KT)); + } else { + tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_KT : SYM_KT)); + } break; } } +/** + * Returns the average velocity. This always uses stats, so can be called as an OSD element later if wanted, to show a real time average + */ +static void osdGenerateAverageVelocityStr(char* buff) { + uint32_t cmPerSec = getTotalTravelDistance() / getFlightTime(); + osdFormatVelocityStr(buff, cmPerSec, false, false); +} + /** * Converts wind speed into a string based on the current unit system, using * always 3 digits and an additional character for the unit at the right. buff @@ -422,13 +478,19 @@ static void osdFormatWindSpeedStr(char *buff, int32_t ws, bool isValid) switch (osdConfig()->units) { case OSD_UNIT_UK: FALLTHROUGH; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; case OSD_UNIT_IMPERIAL: - centivalue = (ws * 224) / 100; + centivalue = CMSEC_TO_CENTIMPH(ws); suffix = SYM_MPH; break; + case OSD_UNIT_GA: + centivalue = CMSEC_TO_CENTIKNOTS(ws); + suffix = SYM_KT; + break; default: case OSD_UNIT_METRIC: - centivalue = (ws * 36) / 10; + centivalue = CMSEC_TO_CENTIKPH(ws); suffix = SYM_KMH; break; } @@ -459,6 +521,8 @@ void osdFormatAltitudeSymbol(char *buff, int32_t alt) switch ((osd_unit_e)osdConfig()->units) { case OSD_UNIT_UK: FALLTHROUGH; + case OSD_UNIT_GA: + FALLTHROUGH; case OSD_UNIT_IMPERIAL: if (osdFormatCentiNumber(buff + 4 - digits, CENTIMETERS_TO_CENTIFEET(alt), 1000, 0, 2, digits)) { // Scaled to kft @@ -469,6 +533,8 @@ void osdFormatAltitudeSymbol(char *buff, int32_t alt) } buff[5] = '\0'; break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; case OSD_UNIT_METRIC: // alt is alredy in cm if (osdFormatCentiNumber(buff + 4 - digits, alt, 1000, 0, 2, digits)) { @@ -491,11 +557,15 @@ static void osdFormatAltitudeStr(char *buff, int32_t alt) { int32_t value; switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_GA: + FALLTHROUGH; case OSD_UNIT_IMPERIAL: value = CENTIMETERS_TO_FEET(alt); tfp_sprintf(buff, "%d%c", (int)value, SYM_FT); break; - case OSD_UNIT_UK: + case OSD_UNIT_METRIC_MPH: FALLTHROUGH; case OSD_UNIT_METRIC: value = CENTIMETERS_TO_METERS(alt); @@ -549,6 +619,8 @@ static uint16_t osdGetCrsfLQ(void) int16_t displayedLQ; switch (osdConfig()->crsf_lq_format) { case OSD_CRSF_LQ_TYPE1: + displayedLQ = statsLQ; + break; case OSD_CRSF_LQ_TYPE2: displayedLQ = statsLQ; break; @@ -661,6 +733,9 @@ static void osdFormatCraftName(char *buff) static const char * osdArmingDisabledReasonMessage(void) { + const char *message = NULL; + char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH+1)]; + switch (isArmingDisabledReason()) { case ARMING_DISABLED_FAILSAFE_SYSTEM: // See handling of FAILSAFE_RX_LOSS_MONITORING in failsafe.c @@ -685,6 +760,7 @@ static const char * osdArmingDisabledReasonMessage(void) #if defined(USE_NAV) // Check the exact reason switch (navigationIsBlockingArming(NULL)) { + char buf[6]; case NAV_ARMING_BLOCKER_NONE: break; case NAV_ARMING_BLOCKER_MISSING_GPS_FIX: @@ -692,7 +768,9 @@ static const char * osdArmingDisabledReasonMessage(void) case NAV_ARMING_BLOCKER_NAV_IS_ALREADY_ACTIVE: return OSD_MESSAGE_STR(OSD_MSG_DISABLE_NAV_FIRST); case NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR: - return OSD_MESSAGE_STR(OSD_MSG_1ST_WP_TOO_FAR); + osdFormatDistanceSymbol(buf, distanceToFirstWP(), 0); + tfp_sprintf(messageBuf, "FIRST WP TOO FAR (%s)", buf); + return message = messageBuf; case NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR: return OSD_MESSAGE_STR(OSD_MSG_JUMP_WP_MISCONFIG); } @@ -959,17 +1037,37 @@ static void osdFormatRpm(char *buff, uint32_t rpm) { buff[0] = SYM_RPM; if (rpm) { - if (rpm >= 1000) { - osdFormatCentiNumber(buff + 1, rpm / 10, 0, 1, 1, 2); - buff[3] = 'K'; - buff[4] = '\0'; + if ( digitCount(rpm) > osdConfig()->esc_rpm_precision) { + uint8_t rpmMaxDecimals = (osdConfig()->esc_rpm_precision - 3); + osdFormatCentiNumber(buff + 1, rpm / 10, 0, rpmMaxDecimals, rpmMaxDecimals, osdConfig()->esc_rpm_precision-1); + buff[osdConfig()->esc_rpm_precision] = 'K'; + buff[osdConfig()->esc_rpm_precision+1] = '\0'; } else { - tfp_sprintf(buff + 1, "%3lu", rpm); + switch(osdConfig()->esc_rpm_precision) { + case 6: + tfp_sprintf(buff + 1, "%6lu", rpm); + break; + case 5: + tfp_sprintf(buff + 1, "%5lu", rpm); + break; + case 4: + tfp_sprintf(buff + 1, "%4lu", rpm); + break; + case 3: + default: + tfp_sprintf(buff + 1, "%3lu", rpm); + break; + } + + } } else { - strcpy(buff + 1, "---"); + uint8_t buffPos = 1; + while (buffPos <=( osdConfig()->esc_rpm_precision)) { + strcpy(buff + buffPos++, "-"); + } } } #endif @@ -1052,8 +1150,8 @@ int osdGetHeadingAngle(int angle) * in-out used to store the last position where the craft was drawn to avoid * erasing all screen on each redraw. */ -static void osdDrawMap(int referenceHeading, uint8_t referenceSym, uint8_t centerSym, - uint32_t poiDistance, int16_t poiDirection, uint8_t poiSymbol, +static void osdDrawMap(int referenceHeading, uint16_t referenceSym, uint16_t centerSym, + uint32_t poiDistance, int16_t poiDirection, uint16_t poiSymbol, uint16_t *drawn, uint32_t *usedScale) { // TODO: These need to be tested with several setups. We might @@ -1088,12 +1186,17 @@ static void osdDrawMap(int referenceHeading, uint8_t referenceSym, uint8_t cente const int scaleReductionMultiplier = MIN(midX - hMargin, midY - vMargin) / 2; switch (osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; case OSD_UNIT_IMPERIAL: initialScale = 16; // 16m ~= 0.01miles break; - case OSD_UNIT_UK: - FALLTHROUGH; + case OSD_UNIT_GA: + initialScale = 18; // 18m ~= 0.01 nautical miles + break; default: + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; case OSD_UNIT_METRIC: initialScale = 10; // 10m as initial scale break; @@ -1485,7 +1588,8 @@ static bool osdDrawSingleElement(uint8_t item) break; case OSD_BATTERY_REMAINING_PERCENT: - tfp_sprintf(buff, "%3d%%", calculateBatteryPercentage()); + osdFormatBatteryChargeSymbol(buff); + tfp_sprintf(buff + 1, "%3d%%", calculateBatteryPercentage()); osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr); break; @@ -1512,14 +1616,20 @@ static bool osdDrawSingleElement(uint8_t item) break; case OSD_GPS_SPEED: - osdFormatVelocityStr(buff, gpsSol.groundSpeed, false); + osdFormatVelocityStr(buff, gpsSol.groundSpeed, false, false); + break; + + case OSD_GPS_MAX_SPEED: + osdFormatVelocityStr(buff, stats.max_speed, false, true); break; case OSD_3D_SPEED: - { - osdFormatVelocityStr(buff, osdGet3DSpeed(), true); - break; - } + osdFormatVelocityStr(buff, osdGet3DSpeed(), true, false); + break; + + case OSD_3D_MAX_SPEED: + osdFormatVelocityStr(buff, stats.max_3D_speed, true, true); + break; case OSD_GLIDESLOPE: { @@ -1621,7 +1731,8 @@ static bool osdDrawSingleElement(uint8_t item) } else { buff[1] = buff[2] = buff[3] = '-'; } - buff[4] = '\0'; + buff[4] = SYM_DEGREES; + buff[5] = '\0'; break; } @@ -1761,10 +1872,11 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH: { - static timeUs_t updatedTimestamp = 0; /*static int32_t updatedTimeSeconds = 0;*/ - timeUs_t currentTimeUs = micros(); static int32_t timeSeconds = -1; +#if defined(USE_ADC) && defined(USE_GPS) + static timeUs_t updatedTimestamp = 0; + timeUs_t currentTimeUs = micros(); if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= MS2US(1000)) { #ifdef USE_WIND_ESTIMATOR timeSeconds = calculateRemainingFlightTimeBeforeRTH(osdConfig()->estimations_wind_compensation); @@ -1773,10 +1885,13 @@ static bool osdDrawSingleElement(uint8_t item) #endif updatedTimestamp = currentTimeUs; } +#endif if ((!ARMING_FLAG(ARMED)) || (timeSeconds == -1)) { buff[0] = SYM_FLY_M; strcpy(buff + 1, "--:--"); +#if defined(USE_ADC) && defined(USE_GPS) updatedTimestamp = 0; +#endif } else if (timeSeconds == -2) { // Wind is too strong to come back with cruise throttle buff[0] = SYM_FLY_M; @@ -1793,9 +1908,10 @@ static bool osdDrawSingleElement(uint8_t item) break; case OSD_REMAINING_DISTANCE_BEFORE_RTH:; + static int32_t distanceMeters = -1; +#if defined(USE_ADC) && defined(USE_GPS) static timeUs_t updatedTimestamp = 0; timeUs_t currentTimeUs = micros(); - static int32_t distanceMeters = -1; if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= MS2US(1000)) { #ifdef USE_WIND_ESTIMATOR distanceMeters = calculateRemainingDistanceBeforeRTH(osdConfig()->estimations_wind_compensation); @@ -1804,15 +1920,30 @@ static bool osdDrawSingleElement(uint8_t item) #endif updatedTimestamp = currentTimeUs; } +#endif buff[0] = SYM_TRIP_DIST; if ((!ARMING_FLAG(ARMED)) || (distanceMeters == -1)) { - buff[4] = SYM_DIST_M; + buff[4] = SYM_BLANK; buff[5] = '\0'; strcpy(buff + 1, "---"); } else if (distanceMeters == -2) { // Wind is too strong to come back with cruise throttle buff[1] = buff[2] = buff[3] = SYM_WIND_HORIZONTAL; - buff[4] = SYM_DIST_M; + switch ((osd_unit_e)osdConfig()->units){ + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + buff[4] = SYM_DIST_MI; + break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + buff[4] = SYM_DIST_KM; + break; + case OSD_UNIT_GA: + buff[4] = SYM_DIST_NM; + break; + } buff[5] = '\0'; TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } else { @@ -1887,9 +2018,12 @@ static bool osdDrawSingleElement(uint8_t item) vtxDeviceOsdInfo_t osdInfo; vtxCommonGetOsdInfo(vtxCommonDevice(), &osdInfo); + tfp_sprintf(buff, "%c", SYM_VTX_POWER); + displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); + tfp_sprintf(buff, "%c", osdInfo.powerIndexLetter); if (isAdjustmentFunctionSelected(ADJUSTMENT_VTX_POWER_LEVEL)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX+1, elemPosY, buff, elemAttr); return true; } @@ -1917,16 +2051,28 @@ static bool osdDrawSingleElement(uint8_t item) int16_t scaledLQ = scaleRange(constrain(statsLQ, 0, 100), 0, 100, 170, 300); switch (osdConfig()->crsf_lq_format) { case OSD_CRSF_LQ_TYPE1: - tfp_sprintf(buff+1, "%3d", rxLinkStatistics.uplinkLQ); + if (!failsafeIsReceivingRxData()) { + tfp_sprintf(buff+1, "%3d", 0); + } else { + tfp_sprintf(buff+1, "%3d", rxLinkStatistics.uplinkLQ); + } break; case OSD_CRSF_LQ_TYPE2: - tfp_sprintf(buff+1, "%d:%3d", rxLinkStatistics.rfMode, rxLinkStatistics.uplinkLQ); + if (!failsafeIsReceivingRxData()) { + tfp_sprintf(buff+1, "%s:%3d", " ", 0); + } else { + tfp_sprintf(buff+1, "%d:%3d", rxLinkStatistics.rfMode, rxLinkStatistics.uplinkLQ); + } break; case OSD_CRSF_LQ_TYPE3: - tfp_sprintf(buff+1, "%3d", rxLinkStatistics.rfMode >= 2 ? scaledLQ : rxLinkStatistics.uplinkLQ); + if (!failsafeIsReceivingRxData()) { + tfp_sprintf(buff+1, "%3d", 0); + } else { + tfp_sprintf(buff+1, "%3d", rxLinkStatistics.rfMode >= 2 ? scaledLQ : rxLinkStatistics.uplinkLQ); + } break; } - if (!failsafeIsReceivingRxData()){ + if (!failsafeIsReceivingRxData()) { TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } else if (rxLinkStatistics.uplinkLQ < osdConfig()->link_quality_alarm) { TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); @@ -1964,7 +2110,10 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_CRSF_TX_POWER: { - tfp_sprintf(buff, "%4d%c", rxLinkStatistics.uplinkTXPower, SYM_MW); + if (!failsafeIsReceivingRxData()) + tfp_sprintf(buff, "%s%c", " ", SYM_BLANK); + else + tfp_sprintf(buff, "%4d%c", rxLinkStatistics.uplinkTXPower, SYM_MW); break; } #endif @@ -2117,7 +2266,14 @@ static bool osdDrawSingleElement(uint8_t item) value = CENTIMETERS_TO_CENTIFEET(value); sym = SYM_FTS; break; + case OSD_UNIT_GA: + // Convert to centi-100feet/min + value = CENTIMETERS_TO_FEET(value * 60); + sym = SYM_100FTM; + break; default: + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; case OSD_UNIT_METRIC: // Already in cm/s sym = SYM_MS; @@ -2131,6 +2287,11 @@ static bool osdDrawSingleElement(uint8_t item) } #endif + case OSD_ACTIVE_PROFILE: + tfp_sprintf(buff, "%c%u", SYM_PROFILE, (getConfigProfile() + 1)); + displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); + break; + case OSD_ROLL_PIDS: osdDisplayFlightPIDValues(elemPosX, elemPosY, "ROL", PID_ROLL, ADJUSTMENT_ROLL_P, ADJUSTMENT_ROLL_I, ADJUSTMENT_ROLL_D, ADJUSTMENT_ROLL_FF); return true; @@ -2321,7 +2482,19 @@ static bool osdDrawSingleElement(uint8_t item) { #ifdef USE_PITOT buff[0] = SYM_AIR; - osdFormatVelocityStr(buff + 1, pitot.airSpeed, false); + osdFormatVelocityStr(buff + 1, pitot.airSpeed, false, false); + #else + return false; + #endif + break; + } + + case OSD_AIR_MAX_SPEED: + { + #ifdef USE_PITOT + buff[0] = SYM_MAX; + buff[1] = SYM_AIR; + osdFormatVelocityStr(buff + 2, stats.max_air_speed, false, false); #else return false; #endif @@ -2420,6 +2593,22 @@ static bool osdDrawSingleElement(uint8_t item) buff[5] = '\0'; } break; + case OSD_UNIT_GA: + moreThanAh = osdFormatCentiNumber(buff, value * METERS_PER_NAUTICALMILE / 10, 1000, 0, 2, 3); + if (!moreThanAh) { + tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_NM_0, SYM_MAH_NM_1); + } else { + tfp_sprintf(buff, "%s%c", buff, SYM_AH_NM); + } + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + buff[3] = SYM_MAH_NM_0; + buff[4] = SYM_MAH_NM_1; + buff[5] = '\0'; + } + break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; case OSD_UNIT_METRIC: moreThanAh = osdFormatCentiNumber(buff, value * 100, 1000, 0, 2, 3); if (!moreThanAh) { @@ -2465,6 +2654,12 @@ static bool osdDrawSingleElement(uint8_t item) osdFormatCentiNumber(buff, value * METERS_PER_MILE / 10000, 0, 2, 0, 3); buff[3] = SYM_WH_MI; break; + case OSD_UNIT_GA: + osdFormatCentiNumber(buff, value * METERS_PER_NAUTICALMILE / 10000, 0, 2, 0, 3); + buff[3] = SYM_WH_NM; + break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; case OSD_UNIT_METRIC: osdFormatCentiNumber(buff, value / 10, 0, 2, 0, 3); buff[3] = SYM_WH_KM; @@ -2635,7 +2830,8 @@ static bool osdDrawSingleElement(uint8_t item) } else { buff[1] = buff[2] = buff[3] = '-'; } - buff[4] = '\0'; + buff[4] = SYM_DEGREES; + buff[5] = '\0'; break; } @@ -2648,6 +2844,8 @@ static bool osdDrawSingleElement(uint8_t item) int maxDecimals; switch (osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; case OSD_UNIT_IMPERIAL: scaleToUnit = 100 / 1609.3440f; // scale to 0.01mi for osdFormatCentiNumber() scaleUnitDivisor = 0; @@ -2655,9 +2853,16 @@ static bool osdDrawSingleElement(uint8_t item) symScaled = SYM_MI; maxDecimals = 2; break; - case OSD_UNIT_UK: - FALLTHROUGH; + case OSD_UNIT_GA: + scaleToUnit = 100 / 1852.0010f; // scale to 0.01mi for osdFormatCentiNumber() + scaleUnitDivisor = 0; + symUnscaled = SYM_NM; + symScaled = SYM_NM; + maxDecimals = 2; + break; default: + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; case OSD_UNIT_METRIC: scaleToUnit = 100; // scale to cm for osdFormatCentiNumber() scaleUnitDivisor = 1000; // Convert to km when scale gets bigger than 999m @@ -2968,6 +3173,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .osd_home_position_arm_screen = SETTING_OSD_HOME_POSITION_ARM_SCREEN_DEFAULT, .pan_servo_index = SETTING_OSD_PAN_SERVO_INDEX_DEFAULT, .pan_servo_pwm2centideg = SETTING_OSD_PAN_SERVO_PWM2CENTIDEG_DEFAULT, + .esc_rpm_precision = SETTING_OSD_ESC_RPM_PRECISION_DEFAULT, .units = SETTING_OSD_UNITS_DEFAULT, .main_voltage_decimals = SETTING_OSD_MAIN_VOLTAGE_DECIMALS_DEFAULT, @@ -2992,7 +3198,8 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .force_grid = SETTING_OSD_FORCE_GRID_DEFAULT, .stats_energy_unit = SETTING_OSD_STATS_ENERGY_UNIT_DEFAULT, - .stats_min_voltage_unit = SETTING_OSD_STATS_MIN_VOLTAGE_UNIT_DEFAULT + .stats_min_voltage_unit = SETTING_OSD_STATS_MIN_VOLTAGE_UNIT_DEFAULT, + .stats_page_auto_swap_time = SETTING_OSD_STATS_PAGE_AUTO_SWAP_TIME_DEFAULT ); void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig) @@ -3198,14 +3405,17 @@ static void osdCompleteAsyncInitialization(void) y++; } y++; - } else { - if (!fontHasMetadata || metadata.version < OSD_MIN_FONT_VERSION) { - const char *m = "INVALID FONT"; - displayWrite(osdDisplayPort, OSD_CENTER_S(m), 3, m); - } + } else if (!fontHasMetadata) { + const char *m = "INVALID FONT"; + displayWrite(osdDisplayPort, OSD_CENTER_S(m), 3, m); y = 4; } + if (fontHasMetadata && metadata.version < OSD_MIN_FONT_VERSION) { + const char *m = "INVALID FONT VERSION"; + displayWrite(osdDisplayPort, OSD_CENTER_S(m), y++, m); + } + char string_buffer[30]; tfp_sprintf(string_buffer, "INAV VERSION: %s", FC_VERSION_STRING); displayWrite(osdDisplayPort, 5, y++, string_buffer); @@ -3220,12 +3430,24 @@ static void osdCompleteAsyncInitialization(void) #define STATS_VALUE_X_POS 24 if (statsConfig()->stats_enabled) { displayWrite(osdDisplayPort, STATS_LABEL_X_POS, ++y, "ODOMETER:"); - if (osdConfig()->units == OSD_UNIT_IMPERIAL) { - tfp_sprintf(string_buffer, "%5d", (int)(statsConfig()->stats_total_dist / METERS_PER_MILE)); - string_buffer[5] = SYM_MI; - } else { - tfp_sprintf(string_buffer, "%5d", (int)(statsConfig()->stats_total_dist / METERS_PER_KILOMETER)); - string_buffer[5] = SYM_KM; + switch (osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + tfp_sprintf(string_buffer, "%5d", (int)(statsConfig()->stats_total_dist / METERS_PER_MILE)); + string_buffer[5] = SYM_MI; + break; + default: + case OSD_UNIT_GA: + tfp_sprintf(string_buffer, "%5d", (int)(statsConfig()->stats_total_dist / METERS_PER_NAUTICALMILE)); + string_buffer[5] = SYM_NM; + break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + tfp_sprintf(string_buffer, "%5d", (int)(statsConfig()->stats_total_dist / METERS_PER_KILOMETER)); + string_buffer[5] = SYM_KM; + break; } string_buffer[6] = '\0'; displayWrite(osdDisplayPort, STATS_VALUE_X_POS-5, y, string_buffer); @@ -3245,12 +3467,24 @@ static void osdCompleteAsyncInitialization(void) displayWrite(osdDisplayPort, STATS_LABEL_X_POS, ++y, "AVG EFFICIENCY:"); if (statsConfig()->stats_total_dist) { uint32_t avg_efficiency = statsConfig()->stats_total_energy / (statsConfig()->stats_total_dist / METERS_PER_KILOMETER); // mWh/km - if (osdConfig()->units == OSD_UNIT_IMPERIAL) { - osdFormatCentiNumber(string_buffer, avg_efficiency / 10, 0, 2, 0, 3); - string_buffer[3] = SYM_WH_MI; - } else { - osdFormatCentiNumber(string_buffer, avg_efficiency / 10000 * METERS_PER_MILE, 0, 2, 0, 3); - string_buffer[3] = SYM_WH_KM; + switch (osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + osdFormatCentiNumber(string_buffer, avg_efficiency / 10, 0, 2, 0, 3); + string_buffer[3] = SYM_WH_MI; + break; + case OSD_UNIT_GA: + osdFormatCentiNumber(string_buffer, avg_efficiency / 10, 0, 2, 0, 3); + string_buffer[3] = SYM_WH_NM; + break; + default: + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + osdFormatCentiNumber(string_buffer, avg_efficiency / 10000 * METERS_PER_MILE, 0, 2, 0, 3); + string_buffer[3] = SYM_WH_KM; + break; } } else { string_buffer[0] = string_buffer[1] = string_buffer[2] = '-'; @@ -3289,6 +3523,8 @@ static void osdResetStats(void) stats.max_current = 0; stats.max_power = 0; stats.max_speed = 0; + stats.max_3D_speed = 0; + stats.max_air_speed = 0; stats.min_voltage = 5000; stats.min_rssi = 99; stats.min_lq = 300; @@ -3302,8 +3538,14 @@ static void osdUpdateStats(void) if (feature(FEATURE_GPS)) { value = osdGet3DSpeed(); - if (stats.max_speed < value) - stats.max_speed = value; + if (stats.max_3D_speed < value) + stats.max_3D_speed = value; + + if (stats.max_speed < gpsSol.groundSpeed) + stats.max_speed = gpsSol.groundSpeed; + + if (stats.max_air_speed < pitot.airSpeed) + stats.max_air_speed = pitot.airSpeed; if (stats.max_distance < GPS_distanceToHome) stats.max_distance = GPS_distanceToHome; @@ -3329,6 +3571,9 @@ static void osdUpdateStats(void) if (stats.min_lq > value) stats.min_lq = value; + if (!failsafeIsReceivingRxData()) + stats.min_lq = 0; + value = osdGetCrsfdBm(); if (stats.min_rssi_dbm > value) stats.min_rssi_dbm = value; @@ -3352,7 +3597,12 @@ static void osdShowStatsPage1(void) if (feature(FEATURE_GPS)) { displayWrite(osdDisplayPort, statNameX, top, "MAX SPEED :"); - osdFormatVelocityStr(buff, stats.max_speed, true); + osdFormatVelocityStr(buff, stats.max_3D_speed, true, false); + osdLeftAlignString(buff); + displayWrite(osdDisplayPort, statValuesX, top++, buff); + + displayWrite(osdDisplayPort, statNameX, top, "AVG SPEED :"); + osdGenerateAverageVelocityStr(buff); osdLeftAlignString(buff); displayWrite(osdDisplayPort, statValuesX, top++, buff); @@ -3371,15 +3621,20 @@ static void osdShowStatsPage1(void) switch (rxConfig()->serialrx_provider) { case SERIALRX_CRSF: - displayWrite(osdDisplayPort, statNameX, top, "MIN LQ :"); - itoa(stats.min_lq, buff, 10); + displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI % :"); + itoa(stats.min_rssi, buff, 10); strcat(buff, "%"); displayWrite(osdDisplayPort, statValuesX, top++, buff); - displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI :"); + displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI DBM :"); itoa(stats.min_rssi_dbm, buff, 10); tfp_sprintf(buff, "%s%c", buff, SYM_DBM); displayWrite(osdDisplayPort, statValuesX, top++, buff); + + displayWrite(osdDisplayPort, statNameX, top, "MIN LQ :"); + itoa(stats.min_lq, buff, 10); + strcat(buff, "%"); + displayWrite(osdDisplayPort, statValuesX, top++, buff); break; default: displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI :"); @@ -3476,6 +3731,30 @@ static void osdShowStatsPage2(void) } } break; + case OSD_UNIT_GA: + if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { + moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_NAUTICALMILE / totalDistance), 1000, 0, 2, 3); + if (!moreThanAh) { + tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_NM_0, SYM_MAH_NM_1); + } else { + tfp_sprintf(buff, "%s%c", buff, SYM_AH_NM); + } + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + buff[3] = SYM_MAH_NM_0; + buff[4] = SYM_MAH_NM_1; + buff[5] = '\0'; + } + } else { + osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_NAUTICALMILE / totalDistance), 0, 2, 0, 3); + tfp_sprintf(buff, "%s%c", buff, SYM_WH_NM); + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + } + } + break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; case OSD_UNIT_METRIC: if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000000.0f / totalDistance), 1000, 0, 2, 3); @@ -3632,9 +3911,11 @@ static void osdRefresh(timeUs_t currentTimeUs) } // detect arm/disarm + static uint8_t statsPageAutoSwapCntl = 2; if (armState != ARMING_FLAG(ARMED)) { if (ARMING_FLAG(ARMED)) { osdResetStats(); + statsPageAutoSwapCntl = 2; osdShowArmed(); // reset statistic etc uint32_t delay = ARMED_SCREEN_DISPLAY_TIME; statsPagesCheck = 0; @@ -3644,8 +3925,9 @@ static void osdRefresh(timeUs_t currentTimeUs) #endif osdSetNextRefreshIn(delay); } else { - osdShowStatsPage1(); // show first page of statistic + osdShowStatsPage1(); // show first page of statistics osdSetNextRefreshIn(STATS_SCREEN_DISPLAY_TIME); + statsPageAutoSwapCntl = osdConfig()->stats_page_auto_swap_time > 0 ? 0 : 2; // disable swapping pages when time = 0 } armState = ARMING_FLAG(ARMED); @@ -3657,6 +3939,26 @@ static void osdRefresh(timeUs_t currentTimeUs) // Clear the screen first to erase other elements which // might have been drawn while the OSD wasn't refreshing. + // auto swap stats pages when first shown + // auto swap cancelled using roll stick + if (statsPageAutoSwapCntl != 2) { + if (STATS_PAGE1 || STATS_PAGE2) { + statsPageAutoSwapCntl = 2; + } else { + if (OSD_ALTERNATING_CHOICES((osdConfig()->stats_page_auto_swap_time * 1000), 2)) { + if (statsPageAutoSwapCntl == 0) { + osdShowStatsPage1(); + statsPageAutoSwapCntl = 1; + } + } else { + if (statsPageAutoSwapCntl == 1) { + osdShowStatsPage2(); + statsPageAutoSwapCntl = 0; + } + } + } + } + if (!DELAYED_REFRESH_RESUME_COMMAND) refreshWaitForResumeCmdRelease = false; @@ -3831,85 +4133,65 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter if (buff != NULL) { const char *message = NULL; char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH+1)]; - if (ARMING_FLAG(ARMED)) { - // Aircraft is armed. We might have up to 5 - // messages to show. - const char *messages[5]; - unsigned messageCount = 0; - if (FLIGHT_MODE(FAILSAFE_MODE)) { - // In FS mode while being armed too - const char *failsafePhaseMessage = osdFailsafePhaseMessage(); - const char *failsafeInfoMessage = osdFailsafeInfoMessage(); - const char *navStateFSMessage = navigationStateMessage(); + // We might have up to 5 messages to show. + const char *messages[5]; + unsigned messageCount = 0; + const char *failsafeInfoMessage = NULL; + const char *invertedInfoMessage = NULL; - if (failsafePhaseMessage) { - messages[messageCount++] = failsafePhaseMessage; + if (ARMING_FLAG(ARMED)) { + if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) { + if (isWaypointMissionRTHActive()) { + // if RTH activated whilst WP mode selected, remind pilot to cancel WP mode to exit RTH + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_WP_RTH_CANCEL); } - if (failsafeInfoMessage) { - messages[messageCount++] = failsafeInfoMessage; - } - if (navStateFSMessage) { - messages[messageCount++] = navStateFSMessage; + if (navGetCurrentStateFlags() & NAV_AUTO_WP_DONE) { + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_WP_FINISHED); + } else if (NAV_Status.state == MW_NAV_STATE_WP_ENROUTE) { + // Countdown display for remaining Waypoints + char buf[6]; + osdFormatDistanceSymbol(buf, posControl.wpDistance, 0); + tfp_sprintf(messageBuf, "TO WP %u/%u (%s)", getGeoWaypointNumber(posControl.activeWaypointIndex), posControl.geoWaypointCount, buf); + messages[messageCount++] = messageBuf; + } else if (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED) { + // WP hold time countdown in seconds + timeMs_t currentTime = millis(); + int holdTimeRemaining = posControl.waypointList[posControl.activeWaypointIndex].p1 - (int)((currentTime - posControl.wpReachedTime)/1000); + if (holdTimeRemaining >=0) { + tfp_sprintf(messageBuf, "HOLDING WP FOR %2u S", holdTimeRemaining); + } + messages[messageCount++] = messageBuf; + } else { + const char *navStateMessage = navigationStateMessage(); + if (navStateMessage) { + messages[messageCount++] = navStateMessage; + } } #if defined(USE_SAFE_HOME) const char *safehomeMessage = divertingToSafehomeMessage(); - if (safehomeMessage) { - messages[messageCount++] = safehomeMessage; - } -#endif - if (messageCount > 0) { - message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)]; - if (message == failsafeInfoMessage) { - // failsafeInfoMessage is not useful for recovering - // a lost model, but might help avoiding a crash. - // Blink to grab user attention. - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - // We're shoing either failsafePhaseMessage or - // navStateFSMessage. Don't BLINK here since - // having this text available might be crucial - // during a lost aircraft recovery and blinking - // will cause it to be missing from some frames. + if (safehomeMessage) { + messages[messageCount++] = safehomeMessage; } - } else { - if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) { - if (isWaypointMissionRTHActive()) { - // if RTH activated whilst WP mode selected, remind pilot to cancel WP mode to exit RTH - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_WP_RTH_CANCEL); - } - - if (NAV_Status.state == MW_NAV_STATE_WP_ENROUTE) { - // Countdown display for remaining Waypoints - char buf[6]; - osdFormatDistanceSymbol(buf, posControl.wpDistance, 0); - tfp_sprintf(messageBuf, "TO WP %u/%u (%s)", getGeoWaypointNumber(posControl.activeWaypointIndex), posControl.geoWaypointCount, buf); - messages[messageCount++] = messageBuf; - } else if (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED) { - // WP hold time countdown in seconds - timeMs_t currentTime = millis(); - int holdTimeRemaining = posControl.waypointList[posControl.activeWaypointIndex].p1 - (int)((currentTime - posControl.wpReachedTime)/1000); - if (holdTimeRemaining >=0) { - tfp_sprintf(messageBuf, "HOLDING WP FOR %2u S", holdTimeRemaining); - messages[messageCount++] = messageBuf; - } - } else { - const char *navStateMessage = navigationStateMessage(); - if (navStateMessage) { - messages[messageCount++] = navStateMessage; - } - } -#if defined(USE_SAFE_HOME) - const char *safehomeMessage = divertingToSafehomeMessage(); - if (safehomeMessage) { - messages[messageCount++] = safehomeMessage; - } #endif - } else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOLAUNCH); - const char *launchStateMessage = fixedWingLaunchStateMessage(); - if (launchStateMessage) { - messages[messageCount++] = launchStateMessage; - } + if (FLIGHT_MODE(FAILSAFE_MODE)) { + // In FS mode while being armed too + const char *failsafePhaseMessage = osdFailsafePhaseMessage(); + failsafeInfoMessage = osdFailsafeInfoMessage(); + + if (failsafePhaseMessage) { + messages[messageCount++] = failsafePhaseMessage; + } + if (failsafeInfoMessage) { + messages[messageCount++] = failsafeInfoMessage; + } + } + } else { /* messages shown only when Failsafe, WP, RTH or Emergency Landing not active */ + if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) { + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOLAUNCH); + const char *launchStateMessage = fixedWingLaunchStateMessage(); + if (launchStateMessage) { + messages[messageCount++] = launchStateMessage; + } } else { if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) { // ALTHOLD might be enabled alongside ANGLE/HORIZON/ACRO @@ -3923,42 +4205,64 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter } if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) { messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTUNE); + if (FLIGHT_MODE(MANUAL_MODE)) { + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTUNE_ACRO); + } } if (FLIGHT_MODE(HEADFREE_MODE)) { messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_HEADFREE); } } - // Pick one of the available messages. Each message lasts - // a second. - if (messageCount > 0) { - message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)]; - } } } else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) { unsigned invalidIndex; + // Check if we're unable to arm for some reason if (ARMING_FLAG(ARMING_DISABLED_INVALID_SETTING) && !settingsValidate(&invalidIndex)) { - if (OSD_ALTERNATING_CHOICES(1000, 2) == 0) { + const setting_t *setting = settingGet(invalidIndex); settingGetName(setting, messageBuf); for (int ii = 0; messageBuf[ii]; ii++) { messageBuf[ii] = sl_toupper(messageBuf[ii]); } - message = messageBuf; - } else { - message = OSD_MESSAGE_STR(OSD_MSG_INVALID_SETTING); - TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr); - } + invertedInfoMessage = messageBuf; + messages[messageCount++] = invertedInfoMessage; + + invertedInfoMessage = OSD_MESSAGE_STR(OSD_MSG_INVALID_SETTING); + messages[messageCount++] = invertedInfoMessage; + } else { - if (OSD_ALTERNATING_CHOICES(1000, 2) == 0) { - message = OSD_MESSAGE_STR(OSD_MSG_UNABLE_ARM); - TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr); - } else { + + invertedInfoMessage = OSD_MESSAGE_STR(OSD_MSG_UNABLE_ARM); + messages[messageCount++] = invertedInfoMessage; + // Show the reason for not arming - message = osdArmingDisabledReasonMessage(); - } + messages[messageCount++] = osdArmingDisabledReasonMessage(); + + } + } else if (!ARMING_FLAG(ARMED)) { + if (isWaypointListValid()) { + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_WP_MISSION_LOADED); } } + + if (messageCount > 0) { + message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)]; + if (message == failsafeInfoMessage) { + // failsafeInfoMessage is not useful for recovering + // a lost model, but might help avoiding a crash. + // Blink to grab user attention. + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } else if (message == invertedInfoMessage) { + TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr); + } + // We're shoing either failsafePhaseMessage or + // navStateMessage. Don't BLINK here since + // having this text available might be crucial + // during a lost aircraft recovery and blinking + // will cause it to be missing from some frames. + } + osdFormatMessage(buff, buff_size, message, isCenteredText); } return elemAttr; diff --git a/src/main/io/osd.h b/src/main/io/osd.h index e1d22e1a7d..5e150858cb 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -53,7 +53,6 @@ #define OSD_MSG_SYS_OVERLOADED "SYSTEM OVERLOADED" #define OSD_MSG_WAITING_GPS_FIX "WAITING FOR GPS FIX" #define OSD_MSG_DISABLE_NAV_FIRST "DISABLE NAVIGATION FIRST" -#define OSD_MSG_1ST_WP_TOO_FAR "FIRST WAYPOINT IS TOO FAR" #define OSD_MSG_JUMP_WP_MISCONFIG "JUMP WAYPOINT MISCONFIGURED" #define OSD_MSG_MAG_NOT_CAL "COMPASS NOT CALIBRATED" #define OSD_MSG_ACC_NOT_CAL "ACCELEROMETER NOT CALIBRATED" @@ -84,10 +83,10 @@ #define OSD_MSG_STARTING_RTH "STARTING RTH" #define OSD_MSG_RTH_CLIMB "ADJUSTING RTH ALTITUDE" #define OSD_MSG_HEADING_HOME "EN ROUTE TO HOME" -#define OSD_MSG_HOLDING_WAYPOINT "HOLDING WAYPOINT" -#define OSD_MSG_TO_WP "TO WP" +#define OSD_MSG_WP_FINISHED "WP END>HOLDING POSITION" #define OSD_MSG_PREPARE_NEXT_WP "PREPARING FOR NEXT WAYPOINT" #define OSD_MSG_WP_RTH_CANCEL "CANCEL WP TO EXIT RTH" +#define OSD_MSG_WP_MISSION_LOADED "* MISSION LOADED *" #define OSD_MSG_EMERG_LANDING "EMERGENCY LANDING" #define OSD_MSG_LANDING "LANDING" #define OSD_MSG_LOITERING_HOME "LOITERING AROUND HOME" @@ -98,6 +97,7 @@ #define OSD_MSG_ALTITUDE_HOLD "(ALTITUDE HOLD)" #define OSD_MSG_AUTOTRIM "(AUTOTRIM)" #define OSD_MSG_AUTOTUNE "(AUTOTUNE)" +#define OSD_MSG_AUTOTUNE_ACRO "SWITCH TO ACRO" #define OSD_MSG_HEADFREE "(HEADFREE)" #define OSD_MSG_UNABLE_ARM "UNABLE TO ARM" @@ -232,15 +232,21 @@ typedef enum { OSD_PLIMIT_ACTIVE_CURRENT_LIMIT, OSD_PLIMIT_ACTIVE_POWER_LIMIT, OSD_GLIDESLOPE, + OSD_GPS_MAX_SPEED, + OSD_3D_MAX_SPEED, + OSD_AIR_MAX_SPEED, + OSD_ACTIVE_PROFILE, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; typedef enum { OSD_UNIT_IMPERIAL, OSD_UNIT_METRIC, - OSD_UNIT_UK, // Show speed in mp/h, other values in metric + OSD_UNIT_METRIC_MPH, // Old UK units, all metric except speed in mph + OSD_UNIT_UK, // Show everything in imperial, temperature in C + OSD_UNIT_GA, // General Aviation: Knots, Nautical Miles, Feet, Degrees C - OSD_UNIT_MAX = OSD_UNIT_UK, + OSD_UNIT_MAX = OSD_UNIT_GA, } osd_unit_e; typedef enum { @@ -353,6 +359,7 @@ typedef struct osdConfig_s { uint8_t units; // from osd_unit_e uint8_t stats_energy_unit; // from osd_stats_energy_unit_e uint8_t stats_min_voltage_unit; // from osd_stats_min_voltage_unit_e + uint8_t stats_page_auto_swap_time; // stats page auto swap interval time (seconds) #ifdef USE_WIND_ESTIMATOR bool estimations_wind_compensation; // use wind compensation for estimated remaining flight/distance @@ -378,6 +385,7 @@ typedef struct osdConfig_s { uint8_t crsf_lq_format; uint8_t sidebar_height; // sidebar height in rows, 0 turns off sidebars leaving only level indicator arrows uint8_t telemetry; // use telemetry on displayed pixel line 0 + uint8_t esc_rpm_precision; // Number of characters used for the RPM numbers. } osdConfig_t; @@ -410,7 +418,7 @@ int32_t osdGetAltitude(void); void osdCrosshairPosition(uint8_t *x, uint8_t *y); bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int maxDecimals, int maxScaledDecimals, int length); void osdFormatAltitudeSymbol(char *buff, int32_t alt); -void osdFormatVelocityStr(char* buff, int32_t vel, bool _3D); +void osdFormatVelocityStr(char* buff, int32_t vel, bool _3D, bool _max); // Returns a heading angle in degrees normalized to [0, 360). int osdGetHeadingAngle(int angle); diff --git a/src/main/io/osd_canvas.c b/src/main/io/osd_canvas.c index 9d7a965bde..43e3b9b481 100644 --- a/src/main/io/osd_canvas.c +++ b/src/main/io/osd_canvas.c @@ -438,7 +438,7 @@ void osdCanvasDrawArtificialHorizon(displayPort_t *display, displayCanvas_t *can void osdCanvasDrawHeadingGraph(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, int heading) { - static const uint8_t graph[] = { + static const uint16_t graph[] = { SYM_HEADING_W, SYM_HEADING_LINE, SYM_HEADING_DIVIDED_LINE, @@ -502,9 +502,13 @@ static int32_t osdCanvasSidebarGetValue(osd_sidebar_scroll_e scroll) break; case OSD_SIDEBAR_SCROLL_ALTITUDE: switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_GA: + FALLTHROUGH; case OSD_UNIT_IMPERIAL: return CENTIMETERS_TO_CENTIFEET(osdGetAltitude()); - case OSD_UNIT_UK: + case OSD_UNIT_METRIC_MPH: FALLTHROUGH; case OSD_UNIT_METRIC: return osdGetAltitude(); @@ -517,9 +521,14 @@ static int32_t osdCanvasSidebarGetValue(osd_sidebar_scroll_e scroll) switch ((osd_unit_e)osdConfig()->units) { case OSD_UNIT_UK: FALLTHROUGH; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; case OSD_UNIT_IMPERIAL: // cms/s to (mi/h) * 100 return speed * 224 / 100; + case OSD_UNIT_GA: + // cm/s to Knots * 100 + return (int)(speed * 0.019438444924406) * 100; case OSD_UNIT_METRIC: // cm/s to (km/h) * 100 return speed * 36 / 10; @@ -530,9 +539,13 @@ static int32_t osdCanvasSidebarGetValue(osd_sidebar_scroll_e scroll) case OSD_SIDEBAR_SCROLL_HOME_DISTANCE: #if defined(USE_GPS) switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_GA: + FALLTHROUGH; case OSD_UNIT_IMPERIAL: return CENTIMETERS_TO_CENTIFEET(GPS_distanceToHome * 100); - case OSD_UNIT_UK: + case OSD_UNIT_METRIC_MPH: FALLTHROUGH; case OSD_UNIT_METRIC: return GPS_distanceToHome * 100; @@ -575,13 +588,17 @@ static void osdCanvasSidebarGetUnit(osdUnit_t *unit, uint16_t *countsPerStep, os break; case OSD_SIDEBAR_SCROLL_ALTITUDE: switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_GA: + FALLTHROUGH; case OSD_UNIT_IMPERIAL: unit->symbol = SYM_ALT_FT; unit->divisor = FEET_PER_KILOFEET; unit->divided_symbol = SYM_ALT_KFT; *countsPerStep = 50; break; - case OSD_UNIT_UK: + case OSD_UNIT_METRIC_MPH: FALLTHROUGH; case OSD_UNIT_METRIC: unit->symbol = SYM_ALT_M; @@ -595,12 +612,20 @@ static void osdCanvasSidebarGetUnit(osdUnit_t *unit, uint16_t *countsPerStep, os switch ((osd_unit_e)osdConfig()->units) { case OSD_UNIT_UK: FALLTHROUGH; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; case OSD_UNIT_IMPERIAL: unit->symbol = SYM_MPH; unit->divisor = 0; unit->divided_symbol = 0; *countsPerStep = 5; break; + case OSD_UNIT_GA: + unit->symbol = SYM_KT; + unit->divisor = 0; + unit->divided_symbol = 0; + *countsPerStep = 5; + break; case OSD_UNIT_METRIC: unit->symbol = SYM_KMH; unit->divisor = 0; @@ -611,13 +636,21 @@ static void osdCanvasSidebarGetUnit(osdUnit_t *unit, uint16_t *countsPerStep, os break; case OSD_SIDEBAR_SCROLL_HOME_DISTANCE: switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; case OSD_UNIT_IMPERIAL: unit->symbol = SYM_FT; unit->divisor = FEET_PER_MILE; unit->divided_symbol = SYM_MI; *countsPerStep = 300; break; - case OSD_UNIT_UK: + case OSD_UNIT_GA: + unit->symbol = SYM_FT; + unit->divisor = (int)FEET_PER_NAUTICALMILE; + unit->divided_symbol = SYM_NM; + *countsPerStep = 300; + break; + case OSD_UNIT_METRIC_MPH: FALLTHROUGH; case OSD_UNIT_METRIC: unit->symbol = SYM_M; @@ -636,7 +669,7 @@ static bool osdCanvasDrawSidebar(uint32_t *configured, displayWidgets_t *widgets osd_sidebar_scroll_e scroll, unsigned scrollStep) { STATIC_ASSERT(OSD_SIDEBAR_SCROLL_MAX <= 3, adjust_scroll_shift); - STATIC_ASSERT(OSD_UNIT_MAX <= 3, adjust_units_shift); + STATIC_ASSERT(OSD_UNIT_MAX <= 5, adjust_units_shift); // Configuration uint32_t configuration = scrollStep << 16 | (unsigned)osdConfig()->sidebar_horizontal_offset << 8 | scroll << 6 | osdConfig()->units << 4; if (configuration != *configured) { diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index f6a0b9e4d0..840d5495a9 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -652,12 +652,14 @@ static int32_t osdConvertVelocityToUnit(int32_t vel) switch (osdConfig()->units) { case OSD_UNIT_UK: FALLTHROUGH; - + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; case OSD_UNIT_IMPERIAL: - return (vel * 224) / 10000; // Convert to mph - + return CMSEC_TO_CENTIMPH(vel) / 100; // Convert to mph + case OSD_UNIT_GA: + return CMSEC_TO_CENTIKNOTS(vel) / 100; // Convert to Knots case OSD_UNIT_METRIC: - return (vel * 36) / 1000; // Convert to kmh + return CMSEC_TO_CENTIKPH(vel) / 100; // Convert to kmh } // Unreachable @@ -692,9 +694,14 @@ void osdDJIFormatVelocityStr(char* buff) switch (osdConfig()->units) { case OSD_UNIT_UK: FALLTHROUGH; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; case OSD_UNIT_IMPERIAL: tfp_sprintf(buff, "%s %3d MPH", sourceBuf, (int)osdConvertVelocityToUnit(vel)); break; + case OSD_UNIT_GA: + tfp_sprintf(buff, "%s %3d KT", sourceBuf, (int)osdConvertVelocityToUnit(vel)); + break; case OSD_UNIT_METRIC: tfp_sprintf(buff, "%s %3d KPH", sourceBuf, (int)osdConvertVelocityToUnit(vel)); break; @@ -719,6 +726,8 @@ static void osdDJIFormatDistanceStr(char *buff, int32_t dist) int32_t centifeet; switch (osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; case OSD_UNIT_IMPERIAL: centifeet = CENTIMETERS_TO_CENTIFEET(dist); if (abs(centifeet) < FEET_PER_MILE * 100 / 2) { @@ -731,7 +740,19 @@ static void osdDJIFormatDistanceStr(char *buff, int32_t dist) (abs(centifeet) % (100 * FEET_PER_MILE)) / FEET_PER_MILE, "Mi"); } break; - case OSD_UNIT_UK: + case OSD_UNIT_GA: + centifeet = CENTIMETERS_TO_CENTIFEET(dist); + if (abs(centifeet) < FEET_PER_NAUTICALMILE * 100 / 2) { + // Show feet when dist < 0.5mi + tfp_sprintf(buff, "%d%s", (int)(centifeet / 100), "FT"); + } + else { + // Show miles when dist >= 0.5mi + tfp_sprintf(buff, "%d.%02d%s", (int)(centifeet / (100 * FEET_PER_NAUTICALMILE)), + (int)((abs(centifeet) % (int)(100 * FEET_PER_NAUTICALMILE)) / FEET_PER_NAUTICALMILE), "NM"); + } + break; + case OSD_UNIT_METRIC_MPH: FALLTHROUGH; case OSD_UNIT_METRIC: if (abs(dist) < METERS_PER_KILOMETER * 100) { @@ -986,7 +1007,7 @@ static bool djiFormatMessages(char *buff) } } else { #ifdef USE_SERIALRX_CRSF - if (rxLinkStatistics.rfMode == 0) { + if (djiOsdConfig()->rssi_source == DJI_CRSF_LQ && rxLinkStatistics.rfMode == 0) { messages[messageCount++] = "CRSF LOW RF"; } #endif @@ -1156,19 +1177,17 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms case DJI_MSP_NAME: { - const char * name = systemConfig()->name; - #if defined(USE_OSD) if (djiOsdConfig()->use_name_for_messages) { djiSerializeCraftNameOverride(dst); - } - else + } else { #endif - { - int len = strlen(name); - sbufWriteData(dst, name, MAX(len, 12)); - break; + sbufWriteData(dst, systemConfig()->name, (int)strlen(systemConfig()->name)); +#if defined(USE_OSD) } +#endif + + break; } break; diff --git a/src/main/io/osd_grid.c b/src/main/io/osd_grid.c index 2a1ee09d0c..680f8a9999 100644 --- a/src/main/io/osd_grid.c +++ b/src/main/io/osd_grid.c @@ -59,7 +59,7 @@ typedef struct osd_sidebar_s { void osdGridDrawVario(displayPort_t *display, unsigned gx, unsigned gy, float zvel) { int v = zvel / OSD_VARIO_CM_S_PER_ARROW; - uint8_t vchars[] = {SYM_BLANK, SYM_BLANK, SYM_BLANK, SYM_BLANK, SYM_BLANK}; + uint16_t vchars[] = {SYM_BLANK, SYM_BLANK, SYM_BLANK, SYM_BLANK, SYM_BLANK}; if (v >= 6) vchars[0] = SYM_VARIO_UP_2A; @@ -220,11 +220,11 @@ void osdGridDrawHeadingGraph(displayPort_t *display, unsigned gx, unsigned gy, i displayWrite(display, gx, gy, buf); } -static uint8_t osdUpdateSidebar(osd_sidebar_scroll_e scroll, osd_sidebar_t *sidebar, timeMs_t currentTimeMs) +static uint16_t osdUpdateSidebar(osd_sidebar_scroll_e scroll, osd_sidebar_t *sidebar, timeMs_t currentTimeMs) { // Scroll between SYM_AH_DECORATION_MIN and SYM_AH_DECORATION_MAX. // Zero scrolling should draw SYM_AH_DECORATION. - uint8_t decoration = SYM_AH_DECORATION; + uint16_t decoration = SYM_AH_DECORATION; int offset = 0; int steps; switch (scroll) { @@ -291,8 +291,8 @@ void osdGridDrawSidebars(displayPort_t *display) static osd_sidebar_t right; timeMs_t currentTimeMs = millis(); - uint8_t leftDecoration = osdUpdateSidebar(osdConfig()->left_sidebar_scroll, &left, currentTimeMs); - uint8_t rightDecoration = osdUpdateSidebar(osdConfig()->right_sidebar_scroll, &right, currentTimeMs); + uint16_t leftDecoration = osdUpdateSidebar(osdConfig()->left_sidebar_scroll, &left, currentTimeMs); + uint16_t rightDecoration = osdUpdateSidebar(osdConfig()->right_sidebar_scroll, &right, currentTimeMs); const int hudwidth = OSD_AH_SIDEBAR_WIDTH_POS; const int hudheight = osdConfig()->sidebar_height; diff --git a/src/main/io/osd_hud.c b/src/main/io/osd_hud.c index ed131f7496..f2283a7811 100644 --- a/src/main/io/osd_hud.c +++ b/src/main/io/osd_hud.c @@ -226,6 +226,7 @@ void osdHudDrawCrosshair(displayCanvas_t *canvas, uint8_t px, uint8_t py) SYM_AH_CH_TYPE5, SYM_AH_CH_TYPE5 + 1, SYM_AH_CH_TYPE5 + 2, SYM_AH_CH_TYPE6, SYM_AH_CH_TYPE6 + 1, SYM_AH_CH_TYPE6 + 2, SYM_AH_CH_TYPE7, SYM_AH_CH_TYPE7 + 1, SYM_AH_CH_TYPE7 + 2, + SYM_AH_CH_TYPE8, SYM_AH_CH_TYPE8 + 1, SYM_AH_CH_TYPE8 + 2, }; // Center on the screen @@ -240,6 +241,11 @@ void osdHudDrawCrosshair(displayCanvas_t *canvas, uint8_t px, uint8_t py) displayWriteChar(osdGetDisplayPort(), px, py, crh_style_all[crh_crosshair * 3 + 1]); displayWriteChar(osdGetDisplayPort(), px + 1, py, crh_style_all[crh_crosshair * 3 + 2]); + if ((crh_style_all[crh_crosshair * 3]) == SYM_AH_CH_AIRCRAFT1) { + displayWriteChar(osdGetDisplayPort(), px - 2, py, SYM_AH_CH_AIRCRAFT0); + displayWriteChar(osdGetDisplayPort(), px + 2, py, SYM_AH_CH_AIRCRAFT4); + } + if (canvas) { displayCanvasContextPop(canvas); } @@ -345,7 +351,7 @@ void osdHudDrawExtras(uint8_t poi_id) displayWrite(osdGetDisplayPort(), minX + 4, lineY, buftmp); - osdFormatVelocityStr(buftmp, radar_pois[poi_id].speed, false); + osdFormatVelocityStr(buftmp, radar_pois[poi_id].speed, false, false); displayWrite(osdGetDisplayPort(), maxX - 9, lineY, buftmp); tfp_sprintf(buftmp, "%3d%c", radar_pois[poi_id].heading, SYM_HEADING); diff --git a/src/main/io/pwmdriver_i2c.c b/src/main/io/pwmdriver_i2c.c index dc5e930457..0103a29102 100644 --- a/src/main/io/pwmdriver_i2c.c +++ b/src/main/io/pwmdriver_i2c.c @@ -7,6 +7,9 @@ #include "fc/runtime_config.h" #include "config/feature.h" +#include "platform.h" + +#ifdef USE_PWM_SERVO_DRIVER #define PWM_DRIVER_IMPLEMENTATION_COUNT 1 #define PWM_DRIVER_MAX_CYCLE 4 @@ -63,3 +66,5 @@ void pwmDriverSync(void) { cycle = 0; } } + +#endif \ No newline at end of file diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 7e4906d521..ddfde27b2c 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -441,7 +441,7 @@ void closeSerialPort(serialPort_t *serialPort) serialPortUsage->serialPort = NULL; } -void serialInit(bool softserialEnabled, serialPortIdentifier_e serialPortToDisable) +void serialInit(bool softserialEnabled) { uint8_t index; @@ -451,12 +451,6 @@ void serialInit(bool softserialEnabled, serialPortIdentifier_e serialPortToDisab for (index = 0; index < SERIAL_PORT_COUNT; index++) { serialPortUsageList[index].identifier = serialPortIdentifiers[index]; - if (serialPortToDisable != SERIAL_PORT_NONE) { - if (serialPortUsageList[index].identifier == serialPortToDisable) { - serialPortUsageList[index].identifier = SERIAL_PORT_NONE; - serialPortCount--; - } - } if (!softserialEnabled) { if (0 #ifdef USE_SOFTSERIAL1 diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 3c76e160b3..20d389048c 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -138,7 +138,7 @@ typedef void serialConsumer(uint8_t); // // configuration // -void serialInit(bool softserialEnabled, serialPortIdentifier_e serialPortToDisable); +void serialInit(bool softserialEnabled); void serialRemovePort(serialPortIdentifier_e identifier); uint8_t serialGetAvailablePortCount(void); bool serialIsPortAvailable(serialPortIdentifier_e identifier); diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index 31030573d5..6e0bf8e695 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -492,14 +492,9 @@ static void saReceiveFramer(uint8_t c) static void saSendFrame(uint8_t *buf, int len) { - switch (smartAudioSerialPort->identifier) { - case SERIAL_PORT_SOFTSERIAL1: - case SERIAL_PORT_SOFTSERIAL2: - break; - default: - serialWrite(smartAudioSerialPort, 0x00); // Generate 1st start bit - break; - } + // TBS SA definition requires that the line is low before frame is sent + // (for both soft and hard serial). It can be done by sending first 0x00 + serialWrite(smartAudioSerialPort, 0x00); for (int i = 0 ; i < len ; i++) { serialWrite(smartAudioSerialPort, buf[i]); @@ -690,7 +685,7 @@ bool vtxSmartAudioInit(void) { serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_SMARTAUDIO); if (portConfig) { - portOptions_t portOptions = SERIAL_BIDIR_NOPULL; + portOptions_t portOptions = SERIAL_STOPBITS_2 | SERIAL_BIDIR_NOPULL; portOptions = portOptions | (vtxConfig()->halfDuplex ? SERIAL_BIDIR | SERIAL_BIDIR_PP : SERIAL_UNIDIR); smartAudioSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_SMARTAUDIO, NULL, NULL, 4800, MODE_RXTX, portOptions); } diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index e31e8061fe..3c9fa88adf 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -96,7 +96,7 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0); #endif -PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 13); +PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 14); PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { @@ -122,7 +122,8 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .waypoint_radius = SETTING_NAV_WP_RADIUS_DEFAULT, // 2m diameter .waypoint_safe_distance = SETTING_NAV_WP_SAFE_DISTANCE_DEFAULT, // centimeters - first waypoint should be closer than this .waypoint_load_on_boot = SETTING_NAV_WP_LOAD_ON_BOOT_DEFAULT, // load waypoints automatically during boot - .max_auto_speed = SETTING_NAV_AUTO_SPEED_DEFAULT, // 3 m/s = 10.8 km/h + .auto_speed = SETTING_NAV_AUTO_SPEED_DEFAULT, // speed in autonomous modes (3 m/s = 10.8 km/h) + .max_auto_speed = SETTING_NAV_MAX_AUTO_SPEED_DEFAULT, // max allowed speed autonomous modes .max_auto_climb_rate = SETTING_NAV_AUTO_CLIMB_RATE_DEFAULT, // 5 m/s .max_manual_speed = SETTING_NAV_MANUAL_SPEED_DEFAULT, .max_manual_climb_rate = SETTING_NAV_MANUAL_CLIMB_RATE_DEFAULT, @@ -200,7 +201,7 @@ navigationPosControl_t posControl; navSystemStatus_t NAV_Status; EXTENDED_FASTRAM multicopterPosXyCoefficients_t multicopterPosXyCoefficients; -#if defined(NAV_BLACKBOX) +// Blackbox states int16_t navCurrentState; int16_t navActualVelocity[3]; int16_t navDesiredVelocity[3]; @@ -213,7 +214,7 @@ uint16_t navFlags; uint16_t navEPH; uint16_t navEPV; int16_t navAccNEU[3]; -#endif +//End of blackbox states static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode); static void updateDesiredRTHAltitude(void); @@ -775,12 +776,13 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_STATE_WAYPOINT_FINISHED] = { .persistentId = NAV_PERSISTENT_ID_WAYPOINT_FINISHED, .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED, - .timeoutMs = 0, + .timeoutMs = 10, .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP | NAV_AUTO_WP_DONE, .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_WP_ENROUTE, .mwError = MW_NAV_ERROR_FINISH, .onEvent = { + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_FINISHED, [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, @@ -804,7 +806,9 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS, [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_IDLE, // ALTHOLD also bails out from emergency (to IDLE, AltHold will take over from there) + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, } }, @@ -820,7 +824,9 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS, // re-process the state [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_EMERGENCY_LANDING_FINISHED, [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_IDLE, // ALTHOLD also bails out from emergency (to IDLE, AltHold will take over from there) + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, } }, @@ -1106,8 +1112,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati { navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState); - if ((posControl.flags.estHeadingStatus == EST_NONE) || (posControl.flags.estAltStatus == EST_NONE) || (posControl.flags.estPosStatus != EST_TRUSTED) || !STATE(GPS_FIX_HOME)) { + if ((posControl.flags.estHeadingStatus == EST_NONE) || (posControl.flags.estAltStatus == EST_NONE) || !STATE(GPS_FIX_HOME)) { // Heading sensor, altitude sensor and HOME fix are mandatory for RTH. If not satisfied - switch to emergency landing + // Relevant to failsafe forced RTH only. Switched RTH blocked in selectNavEventFromBoxModeInput if sensors unavailanle. // If we are in dead-reckoning mode - also fail, since coordinates may be unreliable return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } @@ -1163,8 +1170,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_CLIMB_TO_SAFE_ALT } } - /* Position sensor failure timeout - land */ - else if (checkForPositionSensorTimeout()) { + /* Position sensor failure timeout - land. Land immediately if failsafe RTH and timeout disabled (set to 0) */ + else if (checkForPositionSensorTimeout() || (!navConfig()->general.pos_failure_timeout && posControl.flags.forcedRTHActivated)) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } /* No valid POS sensor but still within valid timeout - wait */ @@ -1331,7 +1338,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(na { UNUSED(previousState); - if (!(validateRTHSanityChecker() || (posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout())) + if (posControl.flags.estHeadingStatus == EST_NONE || !(validateRTHSanityChecker() || (posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout())) return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_HOVER); @@ -1370,8 +1377,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF return NAV_FSM_EVENT_SUCCESS; } else { - if (!validateRTHSanityChecker()) { - // Continue to check for RTH sanity during landing + /* If position sensors unavailable - land immediately (wait for timeout on GPS) + * Continue to check for RTH sanity during landing */ + if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout() ||!validateRTHSanityChecker()) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } @@ -1576,8 +1584,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na UNREACHABLE(); } } - /* No pos sensor available for NAV_WAIT_FOR_GPS_TIMEOUT_MS - land */ - else if (checkForPositionSensorTimeout()) { + /* If position sensors unavailable - land immediately (wait for timeout on GPS) */ + else if (checkForPositionSensorTimeout() || (posControl.flags.estHeadingStatus == EST_NONE)) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } else { @@ -1617,6 +1625,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navi { UNUSED(previousState); + /* If position sensors unavailable - land immediately (wait for timeout on GPS) */ + if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout()) { + return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; + } + timeMs_t currentTime = millis(); if(posControl.waypointList[posControl.activeWaypointIndex].p1 <= 0) @@ -1963,12 +1976,11 @@ void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelVali posControl.flags.horizontalPositionDataNew = 0; } -#if defined(NAV_BLACKBOX) + //Update blackbox data navLatestActualPosition[X] = newX; navLatestActualPosition[Y] = newY; navActualVelocity[X] = constrain(newVelX, -32678, 32767); navActualVelocity[Y] = constrain(newVelY, -32678, 32767); -#endif } /*----------------------------------------------------------- @@ -2018,10 +2030,9 @@ void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, flo posControl.actualState.surfaceMin = -1; } -#if defined(NAV_BLACKBOX) + //Update blackbox data navLatestActualPosition[Z] = navGetCurrentActualPositionAndVelocity()->pos.z; navActualVelocity[Z] = constrain(navGetCurrentActualPositionAndVelocity()->vel.z, -32678, 32767); -#endif } /*----------------------------------------------------------- @@ -2882,13 +2893,13 @@ int getWaypointCount(void) } #ifdef NAV_NON_VOLATILE_WAYPOINT_STORAGE -bool loadNonVolatileWaypointList(void) +bool loadNonVolatileWaypointList(bool clearIfLoaded) { if (ARMING_FLAG(ARMED)) return false; - // if waypoints are already loaded, just unload them. - if (posControl.waypointCount > 0) { + // if forced and waypoints are already loaded, just unload them. + if (clearIfLoaded && posControl.waypointCount > 0) { resetWaypointList(); return false; } @@ -3016,7 +3027,7 @@ float getActiveWaypointSpeed(void) return navConfig()->general.max_manual_speed; } else { - uint16_t waypointSpeed = navConfig()->general.max_auto_speed; + uint16_t waypointSpeed = navConfig()->general.auto_speed; if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) { if (posControl.waypointCount > 0 && (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_LAND)) { @@ -3028,6 +3039,8 @@ float getActiveWaypointSpeed(void) if (wpSpecificSpeed >= 50.0f && wpSpecificSpeed <= navConfig()->general.max_auto_speed) { waypointSpeed = wpSpecificSpeed; + } else if (wpSpecificSpeed > navConfig()->general.max_auto_speed) { + waypointSpeed = navConfig()->general.max_auto_speed; } } } @@ -3079,7 +3092,7 @@ void applyWaypointNavigationAndAltitudeHold(void) { const timeUs_t currentTimeUs = micros(); -#if defined(NAV_BLACKBOX) + //Updata blackbox data navFlags = 0; if (posControl.flags.estAltStatus == EST_TRUSTED) navFlags |= (1 << 0); if (posControl.flags.estAglStatus == EST_TRUSTED) navFlags |= (1 << 1); @@ -3089,15 +3102,15 @@ void applyWaypointNavigationAndAltitudeHold(void) if (isGPSGlitchDetected()) navFlags |= (1 << 4); #endif if (posControl.flags.estHeadingStatus == EST_TRUSTED) navFlags |= (1 << 5); -#endif // Reset all navigation requests - NAV controllers will set them if necessary DISABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // No navigation when disarmed if (!ARMING_FLAG(ARMED)) { - // If we are disarmed, abort forced RTH + // If we are disarmed, abort forced RTH or Emergency Landing posControl.flags.forcedRTHActivated = false; + posControl.flags.forcedEmergLandingActivated = false; // ensure WP missions always restart from first waypoint after disarm posControl.activeWaypointIndex = 0; return; @@ -3125,8 +3138,7 @@ void applyWaypointNavigationAndAltitudeHold(void) if (posControl.flags.verticalPositionDataConsumed) posControl.flags.verticalPositionDataNew = 0; - -#if defined(NAV_BLACKBOX) + //Update blackbox data if (posControl.flags.isAdjustingPosition) navFlags |= (1 << 6); if (posControl.flags.isAdjustingAltitude) navFlags |= (1 << 7); if (posControl.flags.isAdjustingHeading) navFlags |= (1 << 8); @@ -3134,7 +3146,6 @@ void applyWaypointNavigationAndAltitudeHold(void) navTargetPosition[X] = lrintf(posControl.desiredState.pos.x); navTargetPosition[Y] = lrintf(posControl.desiredState.pos.y); navTargetPosition[Z] = lrintf(posControl.desiredState.pos.z); -#endif } /*----------------------------------------------------------- @@ -3190,6 +3201,24 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) const bool isExecutingRTH = navGetStateFlags(posControl.navState) & NAV_AUTO_RTH; checkSafeHomeState(isExecutingRTH || posControl.flags.forcedRTHActivated); + /* Emergency landing triggered by failsafe when Failsafe procedure set to Landing */ + if (posControl.flags.forcedEmergLandingActivated) { + return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; + } + + /* Keep Emergency landing mode active once triggered. Is cancelled when landing in progress if position sensors working again. + * If failsafe not active landing also cancelled if WP or RTH deselected or if Manual or Althold modes selected. + * Remains active if landing finished regardless of sensor status or flight mode selection */ + bool autonomousNavNotPossible = !(canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME)); + bool emergLandingCancel = IS_RC_MODE_ACTIVE(BOXMANUAL) || (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) && canActivateAltHold) || + !(IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVRTH)); + + if (navigationIsExecutingAnEmergencyLanding()) { + if (autonomousNavNotPossible && (!emergLandingCancel || FLIGHT_MODE(FAILSAFE_MODE))) { + return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; + } + } + // Keep canActivateWaypoint flag at FALSE if there is no mission loaded // Also block WP mission if we are executing RTH if (!isWaypointMissionValid() || isExecutingRTH) { @@ -3354,6 +3383,13 @@ bool navigationTerrainFollowingEnabled(void) return posControl.flags.isTerrainFollowEnabled; } +uint32_t distanceToFirstWP(void) +{ + fpVector3_t startingWaypointPos; + mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[0], GEO_ALT_RELATIVE); + return calculateDistanceToDestination(&startingWaypointPos); +} + navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass) { const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) || (STATE(FIXED_WING_LEGACY) && (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCRUISE))); @@ -3370,7 +3406,7 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass) // Apply extra arming safety only if pilot has any of GPS modes configured if ((isUsingNavigationModes() || failsafeMayRequireNavigationMode()) && !((posControl.flags.estPosStatus >= EST_USABLE) && STATE(GPS_FIX_HOME))) { if (navConfig()->general.flags.extra_arming_safety == NAV_EXTRA_ARMING_SAFETY_ALLOW_BYPASS && - (STATE(NAV_EXTRA_ARMING_SAFETY_BYPASSED) || rxGetChannelValue(YAW) > 1750)) { + (STATE(NAV_EXTRA_ARMING_SAFETY_BYPASSED) || checkStickPosition(YAW_HI))) { if (usedBypass) { *usedBypass = true; } @@ -3386,12 +3422,7 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass) // Don't allow arming if first waypoint is farther than configured safe distance if ((posControl.waypointCount > 0) && (navConfig()->general.waypoint_safe_distance != 0)) { - fpVector3_t startingWaypointPos; - mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[0], GEO_ALT_RELATIVE); - - const bool navWpMissionStartTooFar = calculateDistanceToDestination(&startingWaypointPos) > navConfig()->general.waypoint_safe_distance; - - if (navWpMissionStartTooFar) { + if (distanceToFirstWP() > navConfig()->general.waypoint_safe_distance && !checkStickPosition(YAW_HI)) { return NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR; } } @@ -3476,9 +3507,8 @@ void updateWaypointsAndNavigationMode(void) // Map navMode back to enabled flight modes switchNavigationFlightModes(); -#if defined(NAV_BLACKBOX) + //Update Blackbox data navCurrentState = (int16_t)posControl.navPersistentId; -#endif } /*----------------------------------------------------------- @@ -3588,6 +3618,7 @@ void navigationInit(void) posControl.flags.estAglStatus = EST_NONE; posControl.flags.forcedRTHActivated = 0; + posControl.flags.forcedEmergLandingActivated = false; posControl.waypointCount = 0; posControl.activeWaypointIndex = 0; posControl.waypointListValid = false; @@ -3613,7 +3644,7 @@ void navigationInit(void) #if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE) if (navConfig()->general.waypoint_load_on_boot) - loadNonVolatileWaypointList(); + loadNonVolatileWaypointList(true); #endif } @@ -3666,6 +3697,38 @@ rthState_e getStateOfForcedRTH(void) } } +/*----------------------------------------------------------- + * Ability to execute Emergency Landing on external event + *-----------------------------------------------------------*/ +void activateForcedEmergLanding(void) +{ + abortFixedWingLaunch(); + posControl.flags.forcedEmergLandingActivated = true; + navProcessFSMEvents(selectNavEventFromBoxModeInput()); +} + +void abortForcedEmergLanding(void) +{ + // Disable emergency landing and make sure we back out of navigation mode to IDLE + // If any navigation mode was active prior to emergency landing it will be re-enabled with next RX update + posControl.flags.forcedEmergLandingActivated = false; + navProcessFSMEvents(NAV_FSM_EVENT_SWITCH_TO_IDLE); +} + +emergLandState_e getStateOfForcedEmergLanding(void) +{ + /* If forced emergency landing activated and in EMERG state */ + if (posControl.flags.forcedEmergLandingActivated && (navGetStateFlags(posControl.navState) & NAV_CTL_EMERG)) { + if (posControl.navState == NAV_STATE_EMERGENCY_LANDING_FINISHED) { + return EMERG_LAND_HAS_LANDED; + } else { + return EMERG_LAND_IN_PROGRESS; + } + } else { + return EMERG_LAND_IDLE; + } +} + bool isWaypointMissionRTHActive(void) { return FLIGHT_MODE(NAV_RTH_MODE) && IS_RC_MODE_ACTIVE(BOXNAVWP) && !(IS_RC_MODE_ACTIVE(BOXNAVRTH) || posControl.flags.forcedRTHActivated); diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index ab00b65928..59c7909ed8 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -69,9 +69,6 @@ bool findNearestSafeHome(void); // Find nearest safehome #endif // defined(USE_SAFE_HOME) #if defined(USE_NAV) -#if defined(USE_BLACKBOX) -#define NAV_BLACKBOX -#endif #ifndef NAV_MAX_WAYPOINTS #define NAV_MAX_WAYPOINTS 15 @@ -212,7 +209,8 @@ typedef struct navConfig_s { uint16_t waypoint_radius; // if we are within this distance to a waypoint then we consider it reached (distance is in cm) uint16_t waypoint_safe_distance; // Waypoint mission sanity check distance bool waypoint_load_on_boot; // load waypoints automatically during boot - uint16_t max_auto_speed; // autonomous navigation speed cm/sec + uint16_t auto_speed; // autonomous navigation speed cm/sec + uint16_t max_auto_speed; // maximum allowed autonomous navigation speed cm/sec uint16_t max_auto_climb_rate; // max vertical speed limitation cm/sec uint16_t max_manual_speed; // manual velocity control max horizontal speed uint16_t max_manual_climb_rate; // manual velocity control max vertical speed @@ -468,7 +466,7 @@ bool isWaypointListValid(void); void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData); void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData); void resetWaypointList(void); -bool loadNonVolatileWaypointList(void); +bool loadNonVolatileWaypointList(bool); bool saveNonVolatileWaypointList(void); float getFinalRTHAltitude(void); @@ -515,12 +513,18 @@ geoAltitudeConversionMode_e waypointMissionAltConvMode(geoAltitudeDatumFlag_e da /* Distance/bearing calculation */ bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos); +uint32_t distanceToFirstWP(void); /* Failsafe-forced RTH mode */ void activateForcedRTH(void); void abortForcedRTH(void); rthState_e getStateOfForcedRTH(void); +/* Failsafe-forced Emergency Landing mode */ +void activateForcedEmergLanding(void); +void abortForcedEmergLanding(void); +emergLandState_e getStateOfForcedEmergLanding(void); + /* Getter functions which return data about the state of the navigation system */ bool navigationInAutomaticThrottleMode(void); bool navigationIsControllingThrottle(void); @@ -576,6 +580,6 @@ extern int16_t navAccNEU[3]; #define getEstimatedActualVelocity(axis) (0) #define navigationIsControllingThrottle() (0) #define navigationRTHAllowsLanding() (0) -#define navigationGetHomeHeading(0) +#define navigationGetHomeHeading() (0) #endif diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index c8300fbf87..e711877c60 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -49,6 +49,8 @@ #include "navigation/navigation.h" #include "navigation/navigation_private.h" +#include "programming/logic_condition.h" + #include "rx/rx.h" #include "sensors/battery.h" @@ -236,13 +238,28 @@ void resetFixedWingPositionController(void) static int8_t loiterDirection(void) { int8_t dir = 1; //NAV_LOITER_RIGHT - if (pidProfile()->loiter_direction == NAV_LOITER_LEFT) dir = -1; + + if (pidProfile()->loiter_direction == NAV_LOITER_LEFT) { + dir = -1; + } + if (pidProfile()->loiter_direction == NAV_LOITER_YAW) { - if (rcCommand[YAW] < -250) loiterDirYaw = 1; //RIGHT //yaw is contrariwise - if (rcCommand[YAW] > 250) loiterDirYaw = -1; //LEFT //see annexCode in fc_core.c + + if (rcCommand[YAW] < -250) { + loiterDirYaw = 1; //RIGHT //yaw is contrariwise + } + + if (rcCommand[YAW] > 250) { + loiterDirYaw = -1; //LEFT //see annexCode in fc_core.c + } + dir = loiterDirYaw; } - if (IS_RC_MODE_ACTIVE(BOXLOITERDIRCHN)) dir *= -1; + + if (IS_RC_MODE_ACTIVE(BOXLOITERDIRCHN)) { + dir *= -1; + } + return dir; } @@ -256,10 +273,12 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod) // Limit minimum forward velocity to 1 m/s float trackingDistance = trackingPeriod * MAX(posControl.actualState.velXY, 100.0f); + uint32_t navLoiterRadius = getLoiterRadius(navConfig()->fw.loiter_radius); + // If angular visibility of a waypoint is less than 30deg, don't calculate circular loiter, go straight to the target #define TAN_15DEG 0.26795f bool needToCalculateCircularLoiter = (isApproachingLastWaypoint() || isWaypointWait()) - && (distanceToActualTarget <= (navConfig()->fw.loiter_radius / TAN_15DEG)) + && (distanceToActualTarget <= (navLoiterRadius / TAN_15DEG)) && (distanceToActualTarget > 50.0f) && !FLIGHT_MODE(NAV_COURSE_HOLD_MODE); @@ -268,8 +287,8 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod) // We are closing in on a waypoint, calculate circular loiter float loiterAngle = atan2_approx(-posErrorY, -posErrorX) + DEGREES_TO_RADIANS(loiterDirection() * 45.0f); - float loiterTargetX = posControl.desiredState.pos.x + navConfig()->fw.loiter_radius * cos_approx(loiterAngle); - float loiterTargetY = posControl.desiredState.pos.y + navConfig()->fw.loiter_radius * sin_approx(loiterAngle); + float loiterTargetX = posControl.desiredState.pos.x + navLoiterRadius * cos_approx(loiterAngle); + float loiterTargetY = posControl.desiredState.pos.y + navLoiterRadius * sin_approx(loiterAngle); // We have temporary loiter target. Recalculate distance and position error posErrorX = loiterTargetX - navGetCurrentActualPositionAndVelocity()->pos.x; @@ -535,10 +554,11 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat // Manual throttle increase if (navConfig()->fw.allow_manual_thr_increase && !FLIGHT_MODE(FAILSAFE_MODE)) { - if (rcCommand[THROTTLE] < PWM_RANGE_MIN + (PWM_RANGE_MAX - PWM_RANGE_MIN) * 0.95) + if (rcCommand[THROTTLE] < PWM_RANGE_MIN + (PWM_RANGE_MAX - PWM_RANGE_MIN) * 0.95){ correctedThrottleValue += MAX(0, rcCommand[THROTTLE] - currentBatteryProfile->nav.fw.cruise_throttle); - else + } else { correctedThrottleValue = motorConfig()->maxthrottle; + } isAutoThrottleManuallyIncreased = (rcCommand[THROTTLE] > currentBatteryProfile->nav.fw.cruise_throttle); } else { isAutoThrottleManuallyIncreased = false; @@ -596,13 +616,21 @@ bool isFixedWingLandingDetected(void) /*----------------------------------------------------------- * FixedWing emergency landing *-----------------------------------------------------------*/ -void applyFixedWingEmergencyLandingController(void) +void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs) { - // FIXME: Use altitude controller if available (similar to MC code) rcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]); - rcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]); rcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]); rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle; + + if (posControl.flags.estAltStatus >= EST_USABLE) { + updateClimbRateToAltitudeController(-1.0f * navConfig()->general.emerg_descent_rate, ROC_TO_ALT_NORMAL); + applyFixedWingAltitudeAndThrottleController(currentTimeUs); + + int16_t pitchCorrection = constrain(posControl.rcAdjustment[PITCH], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle)); + rcCommand[PITCH] = -pidAngleToRcCommand(pitchCorrection, pidProfile()->max_angle_inclination[FD_PITCH]); + } else { + rcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]); + } } /*----------------------------------------------------------- @@ -625,7 +653,7 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags, applyFixedWingLaunchController(currentTimeUs); } else if (navStateFlags & NAV_CTL_EMERG) { - applyFixedWingEmergencyLandingController(); + applyFixedWingEmergencyLandingController(currentTimeUs); } else { #ifdef NAV_FW_LIMIT_MIN_FLY_VELOCITY @@ -639,24 +667,28 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags, // Motor has been stopped by user. Update target altitude and bypass navigation pitch/throttle control resetFixedWingAltitudeController(); setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); - } else + } else { applyFixedWingAltitudeAndThrottleController(currentTimeUs); + } } - if (navStateFlags & NAV_CTL_POS) + if (navStateFlags & NAV_CTL_POS) { applyFixedWingPositionController(currentTimeUs); + } } else { posControl.rcAdjustment[PITCH] = 0; posControl.rcAdjustment[ROLL] = 0; } - if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && posControl.flags.isAdjustingPosition) + if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && posControl.flags.isAdjustingPosition) { rcCommand[ROLL] = applyDeadbandRescaled(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband, -500, 500); + } //if (navStateFlags & NAV_CTL_YAW) - if ((navStateFlags & NAV_CTL_ALT) || (navStateFlags & NAV_CTL_POS)) + if ((navStateFlags & NAV_CTL_ALT) || (navStateFlags & NAV_CTL_POS)) { applyFixedWingPitchRollThrottleController(navStateFlags, currentTimeUs); + } } } diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index f4c8fc29d4..ce4d7504da 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -399,7 +399,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_ const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs); const uint16_t motorSpinUpMs = navConfig()->fw.launch_motor_spinup_time; - const uint16_t launchThrottle = currentBatteryProfile->nav.fw.launch_throttle; + const uint16_t launchThrottle = constrain(currentBatteryProfile->nav.fw.launch_throttle, getThrottleIdleValue(), motorConfig()->maxthrottle); if (elapsedTimeMs > motorSpinUpMs) { rcCommand[THROTTLE] = launchThrottle; @@ -415,7 +415,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t currentTimeUs) { - rcCommand[THROTTLE] = currentBatteryProfile->nav.fw.launch_throttle; + rcCommand[THROTTLE] = constrain(currentBatteryProfile->nav.fw.launch_throttle, getThrottleIdleValue(), motorConfig()->maxthrottle); if (isLaunchMaxAltitudeReached()) { return FW_LAUNCH_EVENT_SUCCESS; // cancel the launch and do the FW_LAUNCH_STATE_FINISH state @@ -445,7 +445,8 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t curr } else { // make a smooth transition from the launch state to the current state for throttle and the pitch angle - rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, currentBatteryProfile->nav.fw.launch_throttle, rcCommand[THROTTLE]); + const uint16_t launchThrottle = constrain(currentBatteryProfile->nav.fw.launch_throttle, getThrottleIdleValue(), motorConfig()->maxthrottle); + rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, launchThrottle, rcCommand[THROTTLE]); fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_climb_angle, rcCommand[PITCH]); } diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index b35146edfa..2f5028716f 100755 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -98,9 +98,7 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros) posControl.desiredState.vel.z = targetVel; } -#if defined(NAV_BLACKBOX) navDesiredVelocity[Z] = constrain(lrintf(posControl.desiredState.vel.z), -32678, 32767); -#endif } static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros) @@ -467,10 +465,8 @@ static void updatePositionVelocityController_MC(const float maxSpeed) posControl.desiredState.vel.x = newVelX * velHeadFactor * velExpoFactor; posControl.desiredState.vel.y = newVelY * velHeadFactor * velExpoFactor; -#if defined(NAV_BLACKBOX) navDesiredVelocity[X] = constrain(lrintf(posControl.desiredState.vel.x), -32678, 32767); navDesiredVelocity[Y] = constrain(lrintf(posControl.desiredState.vel.y), -32678, 32767); -#endif } static float computeNormalizedVelocity(const float value, const float maxValue) diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 236420c89b..e11ab12beb 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -246,8 +246,8 @@ void onNewGPSData(void) /* Use VELNED provided by GPS if available, calculate from coordinates otherwise */ float gpsScaleLonDown = constrainf(cos_approx((ABS(gpsSol.llh.lat) / 10000000.0f) * 0.0174532925f), 0.01f, 1.0f); if (positionEstimationConfig()->use_gps_velned && gpsSol.flags.validVelNE) { - posEstimator.gps.vel.x = gpsSol.velNED[0]; - posEstimator.gps.vel.y = gpsSol.velNED[1]; + posEstimator.gps.vel.x = gpsSol.velNED[X]; + posEstimator.gps.vel.y = gpsSol.velNED[Y]; } else { posEstimator.gps.vel.x = (posEstimator.gps.vel.x + (DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * (gpsSol.llh.lat - previousLat) / dT)) / 2.0f; @@ -255,7 +255,7 @@ void onNewGPSData(void) } if (positionEstimationConfig()->use_gps_velned && gpsSol.flags.validVelD) { - posEstimator.gps.vel.z = - gpsSol.velNED[2]; // NEU + posEstimator.gps.vel.z = -gpsSol.velNED[Z]; // NEU } else { posEstimator.gps.vel.z = (posEstimator.gps.vel.z + (gpsSol.llh.alt - previousAlt) / dT) / 2.0f; @@ -443,12 +443,10 @@ static void updateIMUTopic(timeUs_t currentTimeUs) posEstimator.imu.accelNEU.z = 0; } -#if defined(NAV_BLACKBOX) /* Update blackbox values */ navAccNEU[X] = posEstimator.imu.accelNEU.x; navAccNEU[Y] = posEstimator.imu.accelNEU.y; navAccNEU[Z] = posEstimator.imu.accelNEU.z; -#endif } } @@ -785,10 +783,9 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs) updateActualAltitudeAndClimbRate(false, posEstimator.est.pos.z, 0, posEstimator.est.aglAlt, 0, EST_NONE); } -#if defined(NAV_BLACKBOX) + //Update Blackbox states navEPH = posEstimator.est.eph; navEPV = posEstimator.est.epv; -#endif } } diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 8792f452d6..9a36a59c7e 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -91,7 +91,9 @@ typedef struct navigationFlags_s { bool isGCSAssistedNavigationReset; // GCS control was disabled - indicate that so code could take action accordingly bool isTerrainFollowEnabled; // Does iNav use rangefinder for terrain following (adjusting baro altitude target according to rangefinders readings) + // Failsafe actions bool forcedRTHActivated; + bool forcedEmergLandingActivated; } navigationFlags_t; typedef struct { diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 5fa6f92733..9ad94591e8 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -34,6 +34,7 @@ #include "common/utils.h" #include "rx/rx.h" #include "common/maths.h" +#include "fc/config.h" #include "fc/fc_core.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" @@ -82,10 +83,10 @@ void pgResetFn_logicConditions(logicCondition_t *instance) logicConditionState_t logicConditionStates[MAX_LOGIC_CONDITIONS]; static int logicConditionCompute( - int currentVaue, + int32_t currentVaue, logicOperation_e operation, - int operandA, - int operandB + int32_t operandA, + int32_t operandB ) { int temporaryValue; vtxDeviceCapability_t vtxDeviceCapability; @@ -176,20 +177,20 @@ static int logicConditionCompute( break; case LOGIC_CONDITION_ADD: - return constrain(operandA + operandB, INT16_MIN, INT16_MAX); + return constrain(operandA + operandB, INT32_MIN, INT32_MAX); break; case LOGIC_CONDITION_SUB: - return constrain(operandA - operandB, INT16_MIN, INT16_MAX); + return constrain(operandA - operandB, INT32_MIN, INT32_MAX); break; case LOGIC_CONDITION_MUL: - return constrain(operandA * operandB, INT16_MIN, INT16_MAX); + return constrain(operandA * operandB, INT32_MIN, INT32_MAX); break; case LOGIC_CONDITION_DIV: if (operandB != 0) { - return constrain(operandA / operandB, INT16_MIN, INT16_MAX); + return constrain(operandA / operandB, INT32_MIN, INT32_MAX); } else { return operandA; } @@ -327,12 +328,34 @@ static int logicConditionCompute( case LOGIC_CONDITION_MODULUS: if (operandB != 0) { - return constrain(operandA % operandB, INT16_MIN, INT16_MAX); + return constrain(operandA % operandB, INT32_MIN, INT32_MAX); } else { return operandA; } break; + case LOGIC_CONDITION_SET_PROFILE: + operandA--; + if ( getConfigProfile() != operandA && (operandA >= 0 && operandA < MAX_PROFILE_COUNT)) { + bool profileChanged = false; + if (setConfigProfile(operandA)) { + pidInit(); + pidInitFilters(); + schedulePidGainsUpdate(); + profileChanged = true; + } + return profileChanged; + } else { + return false; + } + break; + + case LOGIC_CONDITION_LOITER_OVERRIDE: + logicConditionValuesByType[LOGIC_CONDITION_LOITER_OVERRIDE] = constrain(operandA, 0, 100000); + LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS); + return true; + break; + default: return false; break; @@ -533,6 +556,13 @@ static int logicConditionGetFlightOperandValue(int operand) { #endif break; + case LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE: // int + return getConfigProfile() + 1; + break; + + case LOGIC_CONDITION_OPERAND_FLIGHT_LOITER_RADIUS: + return getLoiterRadius(navConfig()->fw.loiter_radius); + default: return 0; break; @@ -720,3 +750,16 @@ int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue) { return originalValue; } } + +uint32_t getLoiterRadius(uint32_t loiterRadius) { +#ifdef USE_PROGRAMMING_FRAMEWORK + if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS) && + !(FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding())) { + return constrain(logicConditionValuesByType[LOGIC_CONDITION_LOITER_OVERRIDE], loiterRadius, 100000); + } else { + return loiterRadius; + } +#else + return loiterRadius; +#endif +} diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 3b795ba43e..4e32bd0ac9 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -70,7 +70,9 @@ typedef enum { LOGIC_CONDITION_RC_CHANNEL_OVERRIDE = 38, LOGIC_CONDITION_SET_HEADING_TARGET = 39, LOGIC_CONDITION_MODULUS = 40, - LOGIC_CONDITION_LAST = 41, + LOGIC_CONDITION_LOITER_OVERRIDE = 41, + LOGIC_CONDITION_SET_PROFILE = 42, + LOGIC_CONDITION_LAST = 43, } logicOperation_e; typedef enum logicOperandType_s { @@ -120,6 +122,8 @@ typedef enum { LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_LQ, // 32 LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_SNR, // 33 LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID, // 0/1 // 34 + LOGIC_CONDITION_OPERAND_FLIGHT_LOITER_RADIUS, // 35 + LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE, //int // 36 } logicFlightOperands_e; @@ -148,6 +152,7 @@ typedef enum { LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE = (1 << 6), LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_OSD_LAYOUT = (1 << 7), LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_RC_CHANNEL = (1 << 8), + LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS = (1 << 9), } logicConditionsGlobalFlags_t; typedef enum { @@ -198,3 +203,4 @@ void logicConditionReset(void); float getThrottleScale(float globalThrottleScale); int16_t getRcCommandOverride(int16_t command[], uint8_t axis); int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue); +uint32_t getLoiterRadius(uint32_t loiterRadius); diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c index 8af07bce1c..f01dcb965e 100755 --- a/src/main/rx/crsf.c +++ b/src/main/rx/crsf.c @@ -58,8 +58,8 @@ static timeUs_t crsfFrameStartAt = 0; static uint8_t telemetryBuf[CRSF_FRAME_SIZE_MAX]; static uint8_t telemetryBufLen = 0; -// The power levels represented by uplinkTXPower above in mW (250mW added to full TX in v4.00 firmware) -const uint16_t crsfPowerStates[] = {0, 10, 25, 100, 500, 1000, 2000, 250}; +// The power levels represented by uplinkTXPower above in mW (250mW added to full TX in v4.00 firmware, 50mW added for ExpressLRS) +const uint16_t crsfPowerStates[] = {0, 10, 25, 100, 500, 1000, 2000, 250, 50}; /* * CRSF protocol @@ -232,10 +232,7 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) } crsfFrame.frame.frameLength = CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC; - // Inject link quality into channel 17. const crsfPayloadLinkStatistics_t* linkStats = (crsfPayloadLinkStatistics_t*)&crsfFrame.frame.payload; - crsfChannelData[16] = scaleRange(constrain(linkStats->uplinkLQ, 0, 100), 0, 100, 191, 1791); // will map to [1000;2000] range - lqTrackerSet(rxRuntimeConfig->lqTracker, scaleRange(constrain(linkStats->uplinkLQ, 0, 100), 0, 100, 0, RSSI_MAX_VALUE)); rxLinkStatistics.uplinkRSSI = -1* (linkStats->activeAntenna ? linkStats->uplinkRSSIAnt2 : linkStats->uplinkRSSIAnt1); rxLinkStatistics.uplinkLQ = linkStats->uplinkLQ; @@ -244,6 +241,11 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) rxLinkStatistics.uplinkTXPower = crsfPowerStates[linkStats->uplinkTXPower]; rxLinkStatistics.activeAnt = linkStats->activeAntenna; + if (rxLinkStatistics.uplinkLQ > 0) + lqTrackerSet(rxRuntimeConfig->lqTracker, scaleRange(constrain(rxLinkStatistics.uplinkRSSI, -120, -30), -120, -30, 0, RSSI_MAX_VALUE)); + else + lqTrackerSet(rxRuntimeConfig->lqTracker, 0); + // This is not RC channels frame, update channel value but don't indicate frame completion return RX_FRAME_PENDING; } diff --git a/src/main/rx/jetiexbus.c b/src/main/rx/jetiexbus.c index 14424ff952..3521ff6ab7 100644 --- a/src/main/rx/jetiexbus.c +++ b/src/main/rx/jetiexbus.c @@ -1,19 +1,24 @@ /* - * This file is part of Cleanflight. + * This file is part of Cleanflight and Betaflight. * - * 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 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 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. + * 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 Cleanflight. If not, see . + * along with this software. * + * If not, see . + */ + +/* * Authors: * Thomas Miric - marv-t * @@ -28,15 +33,12 @@ * Number of channels: 16 * * Connect as follows: - * Jeti EX Bus -> Serial RX (connect directly) - * Serial TX -> Resistor(2k4) ->Serial RX - * In jeti pdf it is different, but if the resistor breaks, the receiver continues to operate. - * + * Jeti EX Bus -> Serial TX (connect directly) */ + #include #include -#include #include "platform.h" @@ -47,8 +49,6 @@ #include "common/utils.h" -#include "drivers/serial.h" -#include "drivers/serial_uart.h" #include "drivers/time.h" #include "io/serial.h" @@ -56,176 +56,49 @@ #include "rx/rx.h" #include "rx/jetiexbus.h" -#ifdef USE_TELEMETRY -#include "sensors/sensors.h" -#include "sensors/battery.h" -#include "sensors/barometer.h" - -#include "telemetry/telemetry.h" -#include "telemetry/jetiexbus.h" -#endif // TELEMETRY - // // Serial driver for Jeti EX Bus receiver // #define JETIEXBUS_BAUDRATE 125000 // EX Bus 125000; EX Bus HS 250000 not supported -#define JETIEXBUS_OPTIONS (SERIAL_STOPBITS_1 | SERIAL_PARITY_NO | SERIAL_NOT_INVERTED) +#define JETIEXBUS_OPTIONS (SERIAL_STOPBITS_1 | SERIAL_PARITY_NO) #define JETIEXBUS_MIN_FRAME_GAP 1000 #define JETIEXBUS_CHANNEL_COUNT 16 // most Jeti TX transmit 16 channels -#define EXBUS_HEADER_LEN 6 -#define EXBUS_CRC_LEN 2 -#define EXBUS_OVERHEAD (EXBUS_HEADER_LEN + EXBUS_CRC_LEN) -#define EXBUS_MAX_CHANNEL_FRAME_SIZE (EXBUS_HEADER_LEN + JETIEXBUS_CHANNEL_COUNT*2 + EXBUS_CRC_LEN) -#define EXBUS_MAX_REQUEST_FRAME_SIZE 9 #define EXBUS_START_CHANNEL_FRAME (0x3E) #define EXBUS_START_REQUEST_FRAME (0x3D) -#define EXBUS_EX_REQUEST (0x3A) #define EXBUS_JETIBOX_REQUEST (0x3B) #define EXBUS_CHANNELDATA (0x3E03) // Frame contains Channel Data #define EXBUS_CHANNELDATA_DATA_REQUEST (0x3E01) // Frame contains Channel Data, but with a request for data #define EXBUS_TELEMETRY_REQUEST (0x3D01) // Frame is a request Frame -enum { - EXBUS_STATE_ZERO = 0, - EXBUS_STATE_IN_PROGRESS, - EXBUS_STATE_RECEIVED, - EXBUS_STATE_PROCESSED -}; - -enum { - EXBUS_TRANS_ZERO = 0, - EXBUS_TRANS_RX_READY, - EXBUS_TRANS_RX, - EXBUS_TRANS_IS_TX_COMPLETED, - EXBUS_TRANS_TX -}; - -enum exBusHeader_e { - EXBUS_HEADER_SYNC = 0, - EXBUS_HEADER_REQ, - EXBUS_HEADER_MSG_LEN, - EXBUS_HEADER_PACKET_ID, - EXBUS_HEADER_DATA_ID, - EXBUS_HEADER_SUBLEN, - EXBUS_HEADER_DATA -}; - -#ifdef USE_TELEMETRY - -#define EXTEL_DATA_MSG (0x40) -#define EXTEL_UNMASK_TYPE (0x3F) -#define EXTEL_SYNC_LEN 1 -#define EXTEL_CRC_LEN 1 -#define EXTEL_HEADER_LEN 6 -#define EXTEL_MAX_LEN 29 -#define EXTEL_OVERHEAD (EXTEL_SYNC_LEN + EXTEL_HEADER_LEN + EXTEL_CRC_LEN) -#define EXTEL_MAX_PAYLOAD (EXTEL_MAX_LEN - EXTEL_OVERHEAD) -#define EXBUS_MAX_REQUEST_BUFFER_SIZE (EXBUS_OVERHEAD + EXTEL_MAX_LEN) - -enum exTelHeader_e { - EXTEL_HEADER_SYNC = 0, - EXTEL_HEADER_TYPE_LEN, - EXTEL_HEADER_USN_LB, - EXTEL_HEADER_USN_HB, - EXTEL_HEADER_LSN_LB, - EXTEL_HEADER_LSN_HB, - EXTEL_HEADER_RES, - EXTEL_HEADER_ID, - EXTEL_HEADER_DATA -}; - -enum exDataType_e { - EX_TYPE_6b = 0, // int6_t Data type 6b (-31 ¸31) - EX_TYPE_14b = 1, // int14_t Data type 14b (-8191 ¸8191) - EX_TYPE_22b = 4, // int22_t Data type 22b (-2097151 ¸2097151) - EX_TYPE_DT = 5, // int22_t Special data type – time and date - EX_TYPE_30b = 8, // int30_t Data type 30b (-536870911 ¸536870911) - EX_TYPE_GPS = 9 // int30_t Special data type – GPS coordinates: lo/hi minute - lo/hi degree. -}; - -const uint8_t exDataTypeLen[]={ - [EX_TYPE_6b] = 1, - [EX_TYPE_14b] = 2, - [EX_TYPE_22b] = 3, - [EX_TYPE_DT] = 3, - [EX_TYPE_30b] = 4, - [EX_TYPE_GPS] = 4 -}; - -typedef struct exBusSensor_s{ - const char *label; - const char *unit; - int32_t value; - const uint8_t exDataType; - const uint8_t decimals; -} exBusSensor_t; - -#define DECIMAL_MASK(decimals) (decimals << 5) - -// list of telemetry messages -// after every 15 sensors a new header has to be inserted (e.g. "CF-Dev 1.12 S2") -exBusSensor_t jetiExSensors[] = { - { "CF-Dev 1.12 S1", "", 0, 0, 0 }, // device descripton - { "Voltage", "V", 0, EX_TYPE_14b, DECIMAL_MASK(1) }, - { "Current", "A", 0, EX_TYPE_14b, DECIMAL_MASK(2) }, - { "Altitude", "m", 0, EX_TYPE_14b, DECIMAL_MASK(1) }, - { "Capacity", "mAh", 0, EX_TYPE_22b, DECIMAL_MASK(0) }, - { "frames lost", " ", 0, EX_TYPE_22b, DECIMAL_MASK(0) }, // for debug only - { "time Diff", "us", 0, EX_TYPE_14b, DECIMAL_MASK(0) } // for debug only -}; -// after every 15 sensors increment the step by 2 (e.g. ...EX_VAL15, EX_VAL16 = 17) to skip the device description -enum exSensors_e { - EX_VOLTAGE = 1, - EX_CURRENT, - EX_ALTITUDE, - EX_CAPACITY, - EX_FRAMES_LOST, // for debug only - EX_TIME_DIFF // for debug only -}; +serialPort_t *jetiExBusPort; -#define JETI_EX_SENSOR_COUNT (ARRAYLEN(jetiExSensors)) -#endif //TELEMETRY - -static serialPort_t *jetiExBusPort; - -static uint32_t jetiTimeStampRequest = 0; +uint32_t jetiTimeStampRequest = 0; static uint8_t jetiExBusFramePosition; static uint8_t jetiExBusFrameLength; static uint8_t jetiExBusFrameState = EXBUS_STATE_ZERO; -static uint8_t jetiExBusRequestState = EXBUS_STATE_ZERO; +uint8_t jetiExBusRequestState = EXBUS_STATE_ZERO; // Use max values for ram areas static uint8_t jetiExBusChannelFrame[EXBUS_MAX_CHANNEL_FRAME_SIZE]; -static uint8_t jetiExBusRequestFrame[EXBUS_MAX_REQUEST_FRAME_SIZE]; +uint8_t jetiExBusRequestFrame[EXBUS_MAX_REQUEST_FRAME_SIZE]; static uint16_t jetiExBusChannelData[JETIEXBUS_CHANNEL_COUNT]; -#ifdef USE_TELEMETRY - -static uint8_t jetiExBusTelemetryFrame[40]; -static uint8_t jetiExBusTransceiveState = EXBUS_TRANS_RX; -static void sendJetiExBusTelemetry(uint8_t packetID); - -uint8_t calcCRC8(uint8_t *pt, uint8_t msgLen); - -#endif //TELEMETRY - - // Jeti Ex Bus CRC calculations for a frame -uint16_t calcCRC16(uint8_t *pt, uint8_t msgLen) +uint16_t jetiExBusCalcCRC16(uint8_t *pt, uint8_t msgLen) { uint16_t crc16_data = 0; uint8_t data=0; - for (uint8_t mlen = 0; mlen < msgLen; mlen++){ + for (uint8_t mlen = 0; mlen < msgLen; mlen++) { data = pt[mlen] ^ ((uint8_t)(crc16_data) & (uint8_t)(0xFF)); data ^= data << 4; crc16_data = ((((uint16_t)data << 8) | ((crc16_data & 0xFF00) >> 8)) @@ -235,31 +108,13 @@ uint16_t calcCRC16(uint8_t *pt, uint8_t msgLen) return(crc16_data); } -#ifdef USE_TELEMETRY - - -// Jeti Ex Telemetry CRC calculations for a frame -uint8_t calcCRC8(uint8_t *pt, uint8_t msgLen) -{ - uint8_t crc=0; - - for (uint8_t mlen = 0; mlen < msgLen; mlen++) { - crc ^= pt[mlen]; - crc = crc ^ (crc << 1) ^ (crc << 2) ^ (0x0e090700 >> ((crc >> 3) & 0x18)); - } - return(crc); -} - -#endif //TELEMETRY - - void jetiExBusDecodeChannelFrame(uint8_t *exBusFrame) { uint16_t value; uint8_t frameAddr; // Decode header - switch (((uint16_t)exBusFrame[EXBUS_HEADER_SYNC] << 8) | ((uint16_t)exBusFrame[EXBUS_HEADER_REQ])){ + switch (((uint16_t)exBusFrame[EXBUS_HEADER_SYNC] << 8) | ((uint16_t)exBusFrame[EXBUS_HEADER_REQ])) { case EXBUS_CHANNELDATA_DATA_REQUEST: // not yet specified case EXBUS_CHANNELDATA: @@ -274,14 +129,12 @@ void jetiExBusDecodeChannelFrame(uint8_t *exBusFrame) } } - void jetiExBusFrameReset(void) { jetiExBusFramePosition = 0; jetiExBusFrameLength = EXBUS_MAX_CHANNEL_FRAME_SIZE; } - /* supported: 0x3E 0x01 LEN Packet_ID 0x31 SUB_LEN Data_array CRC16 // Channel Data with telemetry request (2nd byte 0x01) @@ -294,31 +147,25 @@ void jetiExBusFrameReset(void) */ // Receive ISR callback -static void jetiExBusDataReceive(uint16_t c, void *rxCallbackData) +static void jetiExBusDataReceive(uint16_t c, void *data) { - UNUSED(rxCallbackData); + UNUSED(data); - timeUs_t now; static timeUs_t jetiExBusTimeLast = 0; - static timeDelta_t jetiExBusTimeInterval; - static uint8_t *jetiExBusFrame; + const timeUs_t now = microsISR(); // Check if we shall reset frame position due to time - now = micros(); - - jetiExBusTimeInterval = cmpTimeUs(now, jetiExBusTimeLast); - jetiExBusTimeLast = now; - - if (jetiExBusTimeInterval > JETIEXBUS_MIN_FRAME_GAP) { + if (cmpTimeUs(now, jetiExBusTimeLast) > JETIEXBUS_MIN_FRAME_GAP) { jetiExBusFrameReset(); jetiExBusFrameState = EXBUS_STATE_ZERO; jetiExBusRequestState = EXBUS_STATE_ZERO; } + jetiExBusTimeLast = now; // Check if we shall start a frame? if (jetiExBusFramePosition == 0) { - switch (c){ + switch (c) { case EXBUS_START_CHANNEL_FRAME: jetiExBusFrameState = EXBUS_STATE_IN_PROGRESS; jetiExBusFrame = jetiExBusChannelFrame; @@ -363,32 +210,30 @@ static void jetiExBusDataReceive(uint16_t c, void *rxCallbackData) jetiExBusFrameState = EXBUS_STATE_RECEIVED; if (jetiExBusRequestState == EXBUS_STATE_IN_PROGRESS) { jetiExBusRequestState = EXBUS_STATE_RECEIVED; - jetiTimeStampRequest = micros(); + jetiTimeStampRequest = now; } jetiExBusFrameReset(); } } - // Check if it is time to read a frame from the data... static uint8_t jetiExBusFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) { UNUSED(rxRuntimeConfig); - if (jetiExBusFrameState != EXBUS_STATE_RECEIVED) - return RX_FRAME_PENDING; + uint8_t frameStatus = RX_FRAME_PENDING; - if (calcCRC16(jetiExBusChannelFrame, jetiExBusChannelFrame[EXBUS_HEADER_MSG_LEN]) == 0) { - jetiExBusDecodeChannelFrame(jetiExBusChannelFrame); + if (jetiExBusFrameState == EXBUS_STATE_RECEIVED) { + if (jetiExBusCalcCRC16(jetiExBusChannelFrame, jetiExBusChannelFrame[EXBUS_HEADER_MSG_LEN]) == 0) { + jetiExBusDecodeChannelFrame(jetiExBusChannelFrame); + frameStatus = RX_FRAME_COMPLETE; + } jetiExBusFrameState = EXBUS_STATE_ZERO; - return RX_FRAME_COMPLETE; - } else { - jetiExBusFrameState = EXBUS_STATE_ZERO; - return RX_FRAME_PENDING; } -} + return frameStatus; +} static uint16_t jetiExBusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) { @@ -398,191 +243,6 @@ static uint16_t jetiExBusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uin return (jetiExBusChannelData[chan]); } - -#ifdef USE_TELEMETRY -/* - ----------------------------------------------- - Jeti Ex Bus Telemetry - ----------------------------------------------- -*/ - -void initJetiExBusTelemetry(void) -{ - // Init Ex Bus Frame header - jetiExBusTelemetryFrame[EXBUS_HEADER_SYNC] = 0x3B; // Startbytes - jetiExBusTelemetryFrame[EXBUS_HEADER_REQ] = 0x01; - jetiExBusTelemetryFrame[EXBUS_HEADER_DATA_ID] = 0x3A; // Ex Telemetry - - - // Init Ex Telemetry header - uint8_t *jetiExTelemetryFrame = &jetiExBusTelemetryFrame[EXBUS_HEADER_DATA]; - jetiExTelemetryFrame[EXTEL_HEADER_SYNC] = 0x9F; // Startbyte - jetiExTelemetryFrame[EXTEL_HEADER_USN_LB] = 0x1E; // Serial Number 4 Byte - jetiExTelemetryFrame[EXTEL_HEADER_USN_HB] = 0xA4; - jetiExTelemetryFrame[EXTEL_HEADER_LSN_LB] = 0x00; // increment by telemetry count (%16) > only 15 values per device possible - jetiExTelemetryFrame[EXTEL_HEADER_LSN_HB] = 0x00; - jetiExTelemetryFrame[EXTEL_HEADER_RES] = 0x00; // reserved, by default 0x00 -} - - -void createExTelemetrieTextMessage(uint8_t *exMessage, uint8_t messageID, const exBusSensor_t *sensor) -{ - uint8_t labelLength = strlen(sensor->label); - uint8_t unitLength = strlen(sensor->unit); - - exMessage[EXTEL_HEADER_TYPE_LEN] = EXTEL_OVERHEAD + labelLength + unitLength; - exMessage[EXTEL_HEADER_LSN_LB] = messageID & 0xF0; // Device ID - exMessage[EXTEL_HEADER_ID] = messageID & 0x0F; // Sensor ID (%16) - exMessage[EXTEL_HEADER_DATA] = (labelLength << 3) + unitLength; - - memcpy(&exMessage[EXTEL_HEADER_DATA + 1], sensor->label, labelLength); - memcpy(&exMessage[EXTEL_HEADER_DATA + 1 + labelLength], sensor->unit, unitLength); - - exMessage[exMessage[EXTEL_HEADER_TYPE_LEN] + EXTEL_CRC_LEN] = calcCRC8(&exMessage[EXTEL_HEADER_TYPE_LEN], exMessage[EXTEL_HEADER_TYPE_LEN]); -} - - -uint8_t createExTelemetrieValueMessage(uint8_t *exMessage, uint8_t itemStart) -{ - uint8_t item = itemStart; - uint8_t iCount; - uint8_t messageSize; - uint32_t sensorValue; - - if ((item & 0x0F) == 0) - item++; - - if (item >= JETI_EX_SENSOR_COUNT) - item = 1; - - exMessage[EXTEL_HEADER_LSN_LB] = item & 0xF0; // Device ID - uint8_t *p = &exMessage[EXTEL_HEADER_ID]; - - while (item <= (itemStart | 0x0F)) { - *p++ = ((item & 0x0F) << 4) | jetiExSensors[item].exDataType; // Sensor ID (%16) | EX Data Type - - sensorValue = jetiExSensors[item].value; - iCount = exDataTypeLen[jetiExSensors[item].exDataType]; - while (iCount > 1) { - *p++ = sensorValue; - sensorValue = sensorValue >> 8; - iCount--; - } - *p++ = (sensorValue & 0x9F) | jetiExSensors[item].decimals; - - item++; - if (item >= JETI_EX_SENSOR_COUNT) - break; - if (EXTEL_MAX_PAYLOAD <= ((p-&exMessage[EXTEL_HEADER_ID]) + exDataTypeLen[jetiExSensors[item].exDataType]) + 1) - break; - } - - messageSize = (EXTEL_HEADER_LEN + (p-&exMessage[EXTEL_HEADER_ID])); - exMessage[EXTEL_HEADER_TYPE_LEN] = EXTEL_DATA_MSG | messageSize; - exMessage[messageSize + EXTEL_CRC_LEN] = calcCRC8(&exMessage[EXTEL_HEADER_TYPE_LEN], messageSize); - - return item; // return the next item -} - - -void createExBusMessage(uint8_t *exBusMessage, uint8_t *exMessage, uint8_t packetID) -{ - uint16_t crc16; - - exBusMessage[EXBUS_HEADER_PACKET_ID] = packetID; - exBusMessage[EXBUS_HEADER_SUBLEN] = (exMessage[EXTEL_HEADER_TYPE_LEN] & EXTEL_UNMASK_TYPE) + 2; // +2: startbyte & CRC8 - exBusMessage[EXBUS_HEADER_MSG_LEN] = EXBUS_OVERHEAD + exBusMessage[EXBUS_HEADER_SUBLEN]; - - crc16 = calcCRC16(exBusMessage, exBusMessage[EXBUS_HEADER_MSG_LEN] - EXBUS_CRC_LEN); - exBusMessage[exBusMessage[EXBUS_HEADER_MSG_LEN] - 2] = crc16; - exBusMessage[exBusMessage[EXBUS_HEADER_MSG_LEN] - 1] = crc16 >> 8; -} - - -void checkJetiExBusTelemetryState(void) -{ - return; -} - - -void handleJetiExBusTelemetry(void) -{ - static uint16_t framesLost = 0; // only for debug - uint32_t timeDiff; - // Check if we shall reset frame position due to time - - if (jetiExBusRequestState == EXBUS_STATE_RECEIVED) { - - // to prevent timing issues from request to answer - max. 4ms - timeDiff = micros() - jetiTimeStampRequest; - - if (timeDiff > 3000) { // include reserved time - jetiExBusRequestState = EXBUS_STATE_ZERO; - framesLost++; - return; - } - - if ((jetiExBusRequestFrame[EXBUS_HEADER_DATA_ID] == EXBUS_EX_REQUEST) && (calcCRC16(jetiExBusRequestFrame, jetiExBusRequestFrame[EXBUS_HEADER_MSG_LEN]) == 0)) { - jetiExSensors[EX_VOLTAGE].value = getBatteryVoltage() / 10; - jetiExSensors[EX_CURRENT].value = getAmperage(); - jetiExSensors[EX_ALTITUDE].value = baro.BaroAlt; - jetiExSensors[EX_CAPACITY].value = getMAhDrawn(); - jetiExSensors[EX_FRAMES_LOST].value = framesLost; - jetiExSensors[EX_TIME_DIFF].value = timeDiff; - - // switch to TX mode - if (uartTotalRxBytesWaiting(jetiExBusPort) == 0) { - serialSetMode(jetiExBusPort, MODE_TX); - jetiExBusTransceiveState = EXBUS_TRANS_TX; - sendJetiExBusTelemetry(jetiExBusRequestFrame[EXBUS_HEADER_PACKET_ID]); - jetiExBusRequestState = EXBUS_STATE_PROCESSED; - } - } else { - jetiExBusRequestState = EXBUS_STATE_ZERO; - return; - } - } - - // check the state if transmit is ready - if (jetiExBusTransceiveState == EXBUS_TRANS_IS_TX_COMPLETED) { - if (isSerialTransmitBufferEmpty(jetiExBusPort)) { - serialSetMode(jetiExBusPort, MODE_RX); - jetiExBusTransceiveState = EXBUS_TRANS_RX; - jetiExBusRequestState = EXBUS_STATE_ZERO; - } - } -} - - -void sendJetiExBusTelemetry(uint8_t packetID) -{ - static uint8_t sensorDescriptionCounter = 0; - static uint8_t sensorValueCounter = 1; - static uint8_t requestLoop = 0; - uint8_t *jetiExTelemetryFrame = &jetiExBusTelemetryFrame[EXBUS_HEADER_DATA]; - - if (requestLoop == 100){ //every nth request send the name of a value - if (sensorDescriptionCounter == JETI_EX_SENSOR_COUNT ) - sensorDescriptionCounter = 0; - - createExTelemetrieTextMessage(jetiExTelemetryFrame, sensorDescriptionCounter, &jetiExSensors[sensorDescriptionCounter]); - createExBusMessage(jetiExBusTelemetryFrame, jetiExTelemetryFrame, packetID); - - requestLoop = 0; - sensorDescriptionCounter++; - } else { - sensorValueCounter = createExTelemetrieValueMessage(jetiExTelemetryFrame, sensorValueCounter); - createExBusMessage(jetiExBusTelemetryFrame, jetiExTelemetryFrame, packetID); - } - - for (uint8_t iCount = 0; iCount < jetiExBusTelemetryFrame[EXBUS_HEADER_MSG_LEN]; iCount++) { - serialWrite(jetiExBusPort, jetiExBusTelemetryFrame[iCount]); - } - jetiExBusTransceiveState = EXBUS_TRANS_IS_TX_COMPLETED; - requestLoop++; -} -#endif // TELEMETRY - bool jetiExBusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) { UNUSED(rxConfig); @@ -607,10 +267,8 @@ bool jetiExBusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfi NULL, JETIEXBUS_BAUDRATE, MODE_RXTX, - JETIEXBUS_OPTIONS | (tristateWithDefaultOffIsActive(rxConfig->halfDuplex) ? SERIAL_BIDIR : 0) + JETIEXBUS_OPTIONS | (rxConfig->serialrx_inverted ? SERIAL_INVERTED : 0) | SERIAL_BIDIR ); - - serialSetMode(jetiExBusPort, MODE_RX); return jetiExBusPort != NULL; } -#endif // USE_SERIALRX_JETIEXBUS +#endif // SERIAL_RX diff --git a/src/main/rx/jetiexbus.h b/src/main/rx/jetiexbus.h index 4622c7a4d4..d23999e9b0 100644 --- a/src/main/rx/jetiexbus.h +++ b/src/main/rx/jetiexbus.h @@ -17,4 +17,36 @@ #pragma once +#define EXBUS_HEADER_LEN 6 +#define EXBUS_CRC_LEN 2 +#define EXBUS_OVERHEAD (EXBUS_HEADER_LEN + EXBUS_CRC_LEN) +#define EXBUS_MAX_CHANNEL_FRAME_SIZE (EXBUS_HEADER_LEN + JETIEXBUS_CHANNEL_COUNT*2 + EXBUS_CRC_LEN) +#define EXBUS_MAX_REQUEST_FRAME_SIZE 9 + +#define EXBUS_EX_REQUEST (0x3A) + +enum exBusHeader_e { + EXBUS_HEADER_SYNC = 0, + EXBUS_HEADER_REQ, + EXBUS_HEADER_MSG_LEN, + EXBUS_HEADER_PACKET_ID, + EXBUS_HEADER_DATA_ID, + EXBUS_HEADER_SUBLEN, + EXBUS_HEADER_DATA +}; + +enum { + EXBUS_STATE_ZERO = 0, + EXBUS_STATE_IN_PROGRESS, + EXBUS_STATE_RECEIVED, + EXBUS_STATE_PROCESSED +}; + +extern uint8_t jetiExBusRequestState; +extern uint32_t jetiTimeStampRequest; +extern uint8_t jetiExBusRequestFrame[EXBUS_MAX_REQUEST_FRAME_SIZE]; +struct serialPort_s; +extern struct serialPort_s *jetiExBusPort; + +uint16_t jetiExBusCalcCRC16(uint8_t *pt, uint8_t msgLen); bool jetiExBusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); diff --git a/src/main/rx/pwm.c b/src/main/rx/pwm.c deleted file mode 100644 index fa90e3b476..0000000000 --- a/src/main/rx/pwm.c +++ /dev/null @@ -1,90 +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 . - */ - -#include -#include -#include - -#include - -#include "platform.h" - -#if defined(USE_RX_PPM) - -#include "build/debug.h" -#include "common/utils.h" - -#include "config/feature.h" - -#include "drivers/timer.h" -#include "drivers/rx_pwm.h" -#include "drivers/time.h" - -#include "fc/config.h" - -#include "rx/rx.h" -#include "rx/pwm.h" - -#define RC_PWM_50HZ_UPDATE (20 * 1000) // 50Hz update rate period - -static uint16_t channelData[MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT]; - -static uint16_t readRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channel) -{ - UNUSED(rxRuntimeConfig); - return channelData[channel]; -} - -static uint8_t ppmFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) -{ - UNUSED(rxRuntimeConfig); - - // PPM receiver counts received frames so we actually know if new data is available - if (isPPMDataBeingReceived()) { - for (int channel = 0; channel < MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT; channel++) { - channelData[channel] = ppmRead(channel); - } - - resetPPMDataReceivedState(); - return RX_FRAME_COMPLETE; - } - - return RX_FRAME_PENDING; -} - -bool rxPpmInit(rxRuntimeConfig_t *rxRuntimeConfig) -{ - const timerHardware_t * timHw = timerGetByUsageFlag(TIM_USE_PPM); - - if (timHw == NULL) { - return false; - } - - if (!ppmInConfig(timHw)) { - return false; - } - - rxRuntimeConfig->rxRefreshRate = RC_PWM_50HZ_UPDATE; - rxRuntimeConfig->requireFiltering = true; - rxRuntimeConfig->channelCount = MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT; - rxRuntimeConfig->rcReadRawFn = readRawRC; - rxRuntimeConfig->rcFrameStatusFn = ppmFrameStatus; - - return true; -} -#endif - diff --git a/src/main/rx/pwm.h b/src/main/rx/pwm.h deleted file mode 100644 index 6c2ef6ca9c..0000000000 --- a/src/main/rx/pwm.h +++ /dev/null @@ -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 . - */ - -#pragma once - -bool rxPpmInit(rxRuntimeConfig_t *rxRuntimeConfig); diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index f92097befe..fd4e783473 100755 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -37,7 +37,6 @@ #include "drivers/adc.h" -#include "drivers/rx_pwm.h" #include "drivers/rx_spi.h" #include "drivers/serial.h" #include "drivers/time.h" @@ -60,7 +59,6 @@ #include "rx/fport2.h" #include "rx/msp.h" #include "rx/msp_override.h" -#include "rx/pwm.h" #include "rx/rx_spi.h" #include "rx/sbus.h" #include "rx/spektrum.h" @@ -110,7 +108,7 @@ rxLinkStatistics_t rxLinkStatistics; rxRuntimeConfig_t rxRuntimeConfig; static uint8_t rcSampleIndex = 0; -PG_REGISTER_WITH_RESET_TEMPLATE(rxConfig_t, rxConfig, PG_RX_CONFIG, 9); +PG_REGISTER_WITH_RESET_TEMPLATE(rxConfig_t, rxConfig, PG_RX_CONFIG, 10); #ifndef RX_SPI_DEFAULT_PROTOCOL #define RX_SPI_DEFAULT_PROTOCOL 0 @@ -285,7 +283,6 @@ void rxInit(void) rxRuntimeConfig.rcReadRawFn = nullReadRawRC; rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus; rxRuntimeConfig.rxSignalTimeout = DELAY_10_HZ; - rxRuntimeConfig.requireFiltering = false; rcSampleIndex = 0; timeMs_t nowMs = millis(); @@ -317,15 +314,6 @@ void rxInit(void) } switch (rxConfig()->receiverType) { -#if defined(USE_RX_PPM) - case RX_TYPE_PPM: - if (!rxPpmInit(&rxRuntimeConfig)) { - rxConfigMutable()->receiverType = RX_TYPE_NONE; - rxRuntimeConfig.rcReadRawFn = nullReadRawRC; - rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus; - } - break; -#endif #ifdef USE_SERIAL_RX case RX_TYPE_SERIAL: @@ -477,39 +465,6 @@ bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime) return result; } -#define FILTERING_SAMPLE_COUNT 5 -static uint16_t applyChannelFiltering(uint8_t chan, uint16_t sample) -{ - static int16_t rcSamples[MAX_SUPPORTED_RC_CHANNEL_COUNT][FILTERING_SAMPLE_COUNT]; - static bool rxSamplesCollected = false; - - // Update the recent samples - rcSamples[chan][rcSampleIndex % FILTERING_SAMPLE_COUNT] = sample; - - // Until we have enough data - return unfiltered samples - if (!rxSamplesCollected) { - if (rcSampleIndex < FILTERING_SAMPLE_COUNT) { - return sample; - } - rxSamplesCollected = true; - } - - // Assuming a step transition from 1000 -> 2000 different filters will yield the following output: - // No filter: 1000, 2000, 2000, 2000, 2000 - 0 samples delay - // 3-point moving average: 1000, 1333, 1667, 2000, 2000 - 2 samples delay - // 3-point median: 1000, 1000, 2000, 2000, 2000 - 1 sample delay - // 5-point median: 1000, 1000, 1000, 2000, 2000 - 2 sample delay - - // For the same filters - noise rejection capabilities (2 out of 5 outliers - // No filter: 1000, 2000, 1000, 2000, 1000, 1000, 1000 - // 3-point MA: 1000, 1333, 1333, 1667, 1333, 1333, 1000 - noise has reduced magnitude, but spread over more samples - // 3-point median: 1000, 1000, 1000, 2000, 1000, 1000, 1000 - high density noise is not removed - // 5-point median: 1000, 1000, 1000, 1000, 1000, 1000, 1000 - only 3 out of 5 outlier noise will get through - - // Apply 5-point median filtering. This filter has the same delay as 3-point moving average, but better noise rejection - return quickMedianFilter5_16(rcSamples[chan]); -} - bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs) { int16_t rcStaging[MAX_SUPPORTED_RC_CHANNEL_COUNT]; @@ -576,14 +531,8 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs) // Update channel input value if receiver is not in failsafe mode // If receiver is in failsafe (not receiving signal or sending invalid channel values) - last good input values are retained if (rxFlightChannelsValid && rxSignalReceived) { - if (rxRuntimeConfig.requireFiltering) { - for (int channel = 0; channel < rxChannelCount; channel++) { - rcChannels[channel].data = applyChannelFiltering(channel, rcStaging[channel]); - } - } else { - for (int channel = 0; channel < rxChannelCount; channel++) { - rcChannels[channel].data = rcStaging[channel]; - } + for (int channel = 0; channel < rxChannelCount; channel++) { + rcChannels[channel].data = rcStaging[channel]; } } diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 883d8594fe..5b00281905 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -51,6 +51,8 @@ #define RSSI_MAX_VALUE 1023 +#define PPM_RCVR_TIMEOUT 0 + typedef enum { RX_FRAME_PENDING = 0, // No new data available from receiver RX_FRAME_COMPLETE = (1 << 0), // There is new data available @@ -61,11 +63,9 @@ typedef enum { typedef enum { RX_TYPE_NONE = 0, - RX_TYPE_PPM = 1, - RX_TYPE_SERIAL = 2, - RX_TYPE_MSP = 3, - RX_TYPE_SPI = 4, - RX_TYPE_UNUSED_1 = 5 + RX_TYPE_SERIAL = 1, + RX_TYPE_MSP = 2, + RX_TYPE_SPI = 3 } rxReceiverType_e; typedef enum { @@ -162,7 +162,6 @@ typedef struct rxRuntimeConfig_s { uint8_t channelCount; // number of rc channels as reported by current input driver timeUs_t rxRefreshRate; timeUs_t rxSignalTimeout; - bool requireFiltering; rcReadRawDataFnPtr rcReadRawFn; rcFrameStatusFnPtr rcFrameStatusFn; rcProcessFrameFnPtr rcProcessFrameFn; diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 1f5d1de04d..f11fdd4c90 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -45,14 +45,12 @@ FILE_COMPILE_FOR_SPEED #include "drivers/accgyro/accgyro_mpu9250.h" #include "drivers/accgyro/accgyro_lsm303dlhc.h" -#include "drivers/accgyro/accgyro_l3g4200d.h" #include "drivers/accgyro/accgyro_l3gd20.h" -#include "drivers/accgyro/accgyro_adxl345.h" -#include "drivers/accgyro/accgyro_mma845x.h" -#include "drivers/accgyro/accgyro_bma280.h" #include "drivers/accgyro/accgyro_bmi088.h" #include "drivers/accgyro/accgyro_bmi160.h" +#include "drivers/accgyro/accgyro_bmi270.h" #include "drivers/accgyro/accgyro_icm20689.h" +#include "drivers/accgyro/accgyro_icm42605.h" #include "drivers/accgyro/accgyro_fake.h" #include "drivers/sensor.h" @@ -124,19 +122,6 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse) switch (accHardwareToUse) { case ACC_AUTODETECT: FALLTHROUGH; -#ifdef USE_IMU_ADXL345 - case ACC_ADXL345: { - if (adxl345Detect(dev)) { - accHardware = ACC_ADXL345; - break; - } - /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */ - if (accHardwareToUse != ACC_AUTODETECT) { - break; - } - } - FALLTHROUGH; -#endif #ifdef USE_IMU_LSM303DLHC case ACC_LSM303DLHC: @@ -164,32 +149,6 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse) FALLTHROUGH; #endif -#ifdef USE_IMU_MMA8452 - case ACC_MMA8452: // MMA8452 - if (mma8452Detect(dev)) { - accHardware = ACC_MMA8452; - break; - } - /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */ - if (accHardwareToUse != ACC_AUTODETECT) { - break; - } - FALLTHROUGH; -#endif - -#ifdef USE_IMU_BMA280 - case ACC_BMA280: // BMA280 - if (bma280Detect(dev)) { - accHardware = ACC_BMA280; - break; - } - /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */ - if (accHardwareToUse != ACC_AUTODETECT) { - break; - } - FALLTHROUGH; -#endif - #ifdef USE_IMU_MPU6000 case ACC_MPU6000: if (mpu6000AccDetect(dev)) { @@ -268,6 +227,31 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse) FALLTHROUGH; #endif +#ifdef USE_IMU_ICM42605 + case ACC_ICM42605: + if (icm42605AccDetect(dev)) { + accHardware = ACC_ICM42605; + break; + } + /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */ + if (accHardwareToUse != ACC_AUTODETECT) { + break; + } + FALLTHROUGH; +#endif + +#ifdef USE_IMU_BMI270 + case ACC_BMI270: + if (bmi270AccDetect(dev)) { + accHardware = ACC_BMI270; + break; + } + /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */ + if (accHardwareToUse != ACC_AUTODETECT) { + break; + } + FALLTHROUGH; +#endif #ifdef USE_IMU_FAKE case ACC_FAKE: @@ -438,9 +422,9 @@ static void performAcclerationCalibration(void) } if (!calibratedPosition[positionIndex]) { - v.v[0] = accADC[0]; - v.v[1] = accADC[1]; - v.v[2] = accADC[2]; + v.v[X] = accADC[X]; + v.v[Y] = accADC[Y]; + v.v[Z] = accADC[Z]; zeroCalibrationAddValueV(&zeroCalibration, &v); @@ -475,9 +459,9 @@ static void performAcclerationCalibration(void) } if (!sensorCalibrationSolveForOffset(&calState, accTmp)) { - accTmp[0] = 0.0f; - accTmp[1] = 0.0f; - accTmp[2] = 0.0f; + accTmp[X] = 0.0f; + accTmp[Y] = 0.0f; + accTmp[Z] = 0.0f; calFailed = true; } @@ -499,9 +483,9 @@ static void performAcclerationCalibration(void) } if (!sensorCalibrationSolveForScale(&calState, accTmp)) { - accTmp[0] = 1.0f; - accTmp[1] = 1.0f; - accTmp[2] = 1.0f; + accTmp[X] = 1.0f; + accTmp[Y] = 1.0f; + accTmp[Z] = 1.0f; calFailed = true; } diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index a650b4be95..786af3963b 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -33,20 +33,19 @@ // Type of accelerometer used/detected typedef enum { - ACC_NONE = 0, - ACC_AUTODETECT = 1, - ACC_ADXL345 = 2, - ACC_MPU6050 = 3, - ACC_MMA8452 = 4, - ACC_BMA280 = 5, - ACC_LSM303DLHC = 6, - ACC_MPU6000 = 7, - ACC_MPU6500 = 8, - ACC_MPU9250 = 9, - ACC_BMI160 = 10, - ACC_ICM20689 = 11, - ACC_BMI088 = 12, - ACC_FAKE = 13, + ACC_NONE = 0, + ACC_AUTODETECT = 1, + ACC_MPU6050 = 2, + ACC_LSM303DLHC = 3, + ACC_MPU6000 = 4, + ACC_MPU6500 = 5, + ACC_MPU9250 = 6, + ACC_BMI160 = 7, + ACC_ICM20689 = 8, + ACC_BMI088 = 9, + ACC_ICM42605 = 10, + ACC_BMI270 = 11, + ACC_FAKE, ACC_MAX = ACC_FAKE } accelerationSensor_e; diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 92ea43ee6b..150d716c51 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -41,6 +41,8 @@ #include "drivers/compass/compass_mpu9250.h" #include "drivers/compass/compass_lis3mdl.h" #include "drivers/compass/compass_rm3100.h" +#include "drivers/compass/compass_vcm5883.h" +#include "drivers/compass/compass_mlx90393.h" #include "drivers/compass/compass_msp.h" #include "drivers/io.h" #include "drivers/light_led.h" @@ -62,7 +64,7 @@ mag_t mag; // mag access functions #ifdef USE_MAG -PG_REGISTER_WITH_RESET_TEMPLATE(compassConfig_t, compassConfig, PG_COMPASS_CONFIG, 4); +PG_REGISTER_WITH_RESET_TEMPLATE(compassConfig_t, compassConfig, PG_COMPASS_CONFIG, 5); PG_RESET_TEMPLATE(compassConfig_t, compassConfig, .mag_align = SETTING_ALIGN_MAG_DEFAULT, @@ -246,6 +248,32 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse) } FALLTHROUGH; + case MAG_VCM5883: +#ifdef USE_MAG_VCM5883 + if (vcm5883Detect(dev)) { + magHardware = MAG_VCM5883; + break; + } +#endif + /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */ + if (magHardwareToUse != MAG_AUTODETECT) { + break; + } + FALLTHROUGH; + + case MAG_MLX90393: +#ifdef USE_MAG_MLX90393 + if (mlx90393Detect(dev)) { + magHardware = MAG_MLX90393; + break; + } +#endif + /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */ + if (magHardwareToUse != MAG_AUTODETECT) { + break; + } + FALLTHROUGH; + case MAG_FAKE: #ifdef USE_FAKE_MAG if (fakeMagDetect(dev)) { diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index fb0e75367f..9f798f28c5 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -42,7 +42,9 @@ typedef enum { MAG_LIS3MDL = 11, MAG_MSP = 12, MAG_RM3100 = 13, - MAG_FAKE = 14, + MAG_VCM5883 = 14, + MAG_MLX90393 = 15, + MAG_FAKE = 16, MAG_MAX = MAG_FAKE } magSensor_e; diff --git a/src/main/sensors/diagnostics.c b/src/main/sensors/diagnostics.c index e00859a196..2825440b49 100644 --- a/src/main/sensors/diagnostics.c +++ b/src/main/sensors/diagnostics.c @@ -24,6 +24,7 @@ #include "sensors/rangefinder.h" #include "sensors/pitotmeter.h" #include "sensors/opflow.h" +#include "flight/secondary_imu.h" extern uint8_t requestedSensors[SENSOR_INDEX_COUNT]; extern uint8_t detectedSensors[SENSOR_INDEX_COUNT]; @@ -226,6 +227,7 @@ bool isHardwareHealthy(void) const hardwareSensorStatus_e pitotStatus = getHwPitotmeterStatus(); const hardwareSensorStatus_e gpsStatus = getHwGPSStatus(); const hardwareSensorStatus_e opflowStatus = getHwOpticalFlowStatus(); + const hardwareSensorStatus_e imu2Status = getHwSecondaryImuStatus(); // Sensor is considered failing if it's either unavailable (selected but not detected) or unhealthy (returning invalid readings) if (gyroStatus == HW_SENSOR_UNAVAILABLE || gyroStatus == HW_SENSOR_UNHEALTHY) @@ -252,5 +254,8 @@ bool isHardwareHealthy(void) if (opflowStatus == HW_SENSOR_UNAVAILABLE || opflowStatus == HW_SENSOR_UNHEALTHY) return false; + if (imu2Status == HW_SENSOR_UNAVAILABLE || opflowStatus == HW_SENSOR_UNHEALTHY) + return false; + return true; } diff --git a/src/main/sensors/diagnostics.h b/src/main/sensors/diagnostics.h index 3ee3214d2b..fd976fa010 100644 --- a/src/main/sensors/diagnostics.h +++ b/src/main/sensors/diagnostics.h @@ -5,6 +5,8 @@ * rather already known hardware status should be returned. */ +#pragma once + typedef enum { HW_SENSOR_NONE = 0, // Not selected HW_SENSOR_OK = 1, // Selected, detected and healthy (ready to be used) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index f70bdbcdf0..4ea232a977 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -47,14 +47,12 @@ FILE_COMPILE_FOR_SPEED #include "drivers/accgyro/accgyro_mpu9250.h" #include "drivers/accgyro/accgyro_lsm303dlhc.h" -#include "drivers/accgyro/accgyro_l3g4200d.h" #include "drivers/accgyro/accgyro_l3gd20.h" -#include "drivers/accgyro/accgyro_adxl345.h" -#include "drivers/accgyro/accgyro_mma845x.h" -#include "drivers/accgyro/accgyro_bma280.h" #include "drivers/accgyro/accgyro_bmi088.h" #include "drivers/accgyro/accgyro_bmi160.h" +#include "drivers/accgyro/accgyro_bmi270.h" #include "drivers/accgyro/accgyro_icm20689.h" +#include "drivers/accgyro/accgyro_icm42605.h" #include "drivers/accgyro/accgyro_fake.h" #include "drivers/io.h" @@ -105,14 +103,7 @@ EXTENDED_FASTRAM dynamicGyroNotchState_t dynamicGyroNotchState; #endif -#ifdef USE_ALPHA_BETA_GAMMA_FILTER - -STATIC_FASTRAM filterApplyFnPtr abgFilterApplyFn; -STATIC_FASTRAM alphaBetaGammaFilter_t abgFilter[XYZ_AXIS_COUNT]; - -#endif - -PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 15); +PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 0); PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_lpf = SETTING_GYRO_HARDWARE_LPF_DEFAULT, @@ -133,16 +124,10 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyroDynamicLpfMaxHz = SETTING_GYRO_DYN_LPF_MAX_HZ_DEFAULT, .gyroDynamicLpfCurveExpo = SETTING_GYRO_DYN_LPF_CURVE_EXPO_DEFAULT, #ifdef USE_DYNAMIC_FILTERS - .dynamicGyroNotchRange = SETTING_DYNAMIC_GYRO_NOTCH_RANGE_DEFAULT, .dynamicGyroNotchQ = SETTING_DYNAMIC_GYRO_NOTCH_Q_DEFAULT, .dynamicGyroNotchMinHz = SETTING_DYNAMIC_GYRO_NOTCH_MIN_HZ_DEFAULT, .dynamicGyroNotchEnabled = SETTING_DYNAMIC_GYRO_NOTCH_ENABLED_DEFAULT, #endif -#ifdef USE_ALPHA_BETA_GAMMA_FILTER - .alphaBetaGammaAlpha = SETTING_GYRO_ABG_ALPHA_DEFAULT, - .alphaBetaGammaBoost = SETTING_GYRO_ABG_BOOST_DEFAULT, - .alphaBetaGammaHalfLife = SETTING_GYRO_ABG_HALF_LIFE_DEFAULT, -#endif #ifdef USE_GYRO_KALMAN .kalman_q = SETTING_SETPOINT_KALMAN_Q_DEFAULT, .kalmanEnabled = SETTING_SETPOINT_KALMAN_ENABLED_DEFAULT, @@ -166,15 +151,6 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard FALLTHROUGH; #endif -#ifdef USE_IMU_L3G4200D - case GYRO_L3G4200D: - if (l3g4200dDetect(dev)) { - gyroHardware = GYRO_L3G4200D; - break; - } - FALLTHROUGH; -#endif - #ifdef USE_IMU_MPU3050 case GYRO_MPU3050: if (mpu3050Detect(dev)) { @@ -247,6 +223,24 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard FALLTHROUGH; #endif +#ifdef USE_IMU_ICM42605 + case GYRO_ICM42605: + if (icm42605GyroDetect(dev)) { + gyroHardware = GYRO_ICM42605; + break; + } + FALLTHROUGH; +#endif + +#ifdef USE_IMU_BMI270 + case GYRO_BMI270: + if (bmi270GyroDetect(dev)) { + gyroHardware = GYRO_BMI270; + break; + } + FALLTHROUGH; +#endif + #ifdef USE_IMU_FAKE case GYRO_FAKE: if (fakeGyroDetect(dev)) { @@ -306,23 +300,6 @@ static void gyroInitFilters(void) } } -#ifdef USE_ALPHA_BETA_GAMMA_FILTER - - abgFilterApplyFn = (filterApplyFnPtr)nullFilterApply; - - if (gyroConfig()->alphaBetaGammaAlpha > 0) { - abgFilterApplyFn = (filterApplyFnPtr)alphaBetaGammaFilterApply; - for (int axis = 0; axis < 3; axis++) { - alphaBetaGammaFilterInit( - &abgFilter[axis], - gyroConfig()->alphaBetaGammaAlpha, - gyroConfig()->alphaBetaGammaBoost, - gyroConfig()->alphaBetaGammaHalfLife, - getLooptime() * 1e-6f - ); - } - } -#endif #ifdef USE_GYRO_KALMAN if (gyroConfig()->kalmanEnabled) { gyroKalmanInitialize(gyroConfig()->kalman_q); @@ -377,7 +354,6 @@ bool gyroInit(void) gyroDataAnalyseStateInit( &gyroAnalyseState, gyroConfig()->dynamicGyroNotchMinHz, - gyroConfig()->dynamicGyroNotchRange, getLooptime() ); #endif @@ -407,26 +383,26 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVe fpVector3_t v; // Consume gyro reading - v.v[0] = dev->gyroADCRaw[0]; - v.v[1] = dev->gyroADCRaw[1]; - v.v[2] = dev->gyroADCRaw[2]; + v.v[X] = dev->gyroADCRaw[X]; + v.v[Y] = dev->gyroADCRaw[Y]; + v.v[Z] = dev->gyroADCRaw[Z]; zeroCalibrationAddValueV(gyroCalibration, &v); // Check if calibration is complete after this cycle if (zeroCalibrationIsCompleteV(gyroCalibration)) { zeroCalibrationGetZeroV(gyroCalibration, &v); - dev->gyroZero[0] = v.v[0]; - dev->gyroZero[1] = v.v[1]; - dev->gyroZero[2] = v.v[2]; + dev->gyroZero[X] = v.v[X]; + dev->gyroZero[Y] = v.v[Y]; + dev->gyroZero[Z] = v.v[Z]; - LOG_D(GYRO, "Gyro calibration complete (%d, %d, %d)", dev->gyroZero[0], dev->gyroZero[1], dev->gyroZero[2]); + LOG_D(GYRO, "Gyro calibration complete (%d, %d, %d)", dev->gyroZero[X], dev->gyroZero[Y], dev->gyroZero[Z]); schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics } else { - dev->gyroZero[0] = 0; - dev->gyroZero[1] = 0; - dev->gyroZero[2] = 0; + dev->gyroZero[X] = 0; + dev->gyroZero[Y] = 0; + dev->gyroZero[Z] = 0; } } @@ -496,12 +472,6 @@ void FAST_CODE NOINLINE gyroFilter() gyroADCf = gyroLpf2ApplyFn((filter_t *) &gyroLpf2State[axis], gyroADCf); gyroADCf = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf); -#ifdef USE_ALPHA_BETA_GAMMA_FILTER - DEBUG_SET(DEBUG_GYRO_ALPHA_BETA_GAMMA, axis, gyroADCf); - gyroADCf = abgFilterApplyFn(&abgFilter[axis], gyroADCf); - DEBUG_SET(DEBUG_GYRO_ALPHA_BETA_GAMMA, axis + 3, gyroADCf); -#endif - #ifdef USE_DYNAMIC_FILTERS if (dynamicGyroNotchState.enabled) { gyroDataAnalysePush(&gyroAnalyseState, axis, gyroADCf); diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index eed2aceb60..bacce4b458 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -28,7 +28,6 @@ typedef enum { GYRO_NONE = 0, GYRO_AUTODETECT, GYRO_MPU6050, - GYRO_L3G4200D, GYRO_MPU3050, GYRO_L3GD20, GYRO_MPU6000, @@ -37,19 +36,11 @@ typedef enum { GYRO_BMI160, GYRO_ICM20689, GYRO_BMI088, + GYRO_ICM42605, + GYRO_BMI270, GYRO_FAKE } gyroSensor_e; -typedef enum { - DYN_NOTCH_RANGE_HIGH = 0, - DYN_NOTCH_RANGE_MEDIUM, - DYN_NOTCH_RANGE_LOW -} dynamicFilterRange_e; - -#define DYN_NOTCH_RANGE_HZ_HIGH 2000 -#define DYN_NOTCH_RANGE_HZ_MEDIUM 1333 -#define DYN_NOTCH_RANGE_HZ_LOW 1000 - typedef struct gyro_s { bool initialized; uint32_t targetLooptime; @@ -77,16 +68,10 @@ typedef struct gyroConfig_s { uint16_t gyroDynamicLpfMaxHz; uint8_t gyroDynamicLpfCurveExpo; #ifdef USE_DYNAMIC_FILTERS - uint8_t dynamicGyroNotchRange; uint16_t dynamicGyroNotchQ; uint16_t dynamicGyroNotchMinHz; uint8_t dynamicGyroNotchEnabled; #endif -#ifdef USE_ALPHA_BETA_GAMMA_FILTER - float alphaBetaGammaAlpha; - float alphaBetaGammaBoost; - float alphaBetaGammaHalfLife; -#endif #ifdef USE_GYRO_KALMAN uint16_t kalman_q; uint8_t kalmanEnabled; diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 274b76eb86..6fe022daea 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -38,10 +38,11 @@ #include "sensors/rangefinder.h" #include "sensors/sensors.h" #include "sensors/temperature.h" +#include "sensors/temperature.h" +#include "flight/secondary_imu.h" -uint8_t requestedSensors[SENSOR_INDEX_COUNT] = { GYRO_AUTODETECT, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE, PITOT_NONE, OPFLOW_NONE }; -uint8_t detectedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE, PITOT_NONE, OPFLOW_NONE }; - +uint8_t requestedSensors[SENSOR_INDEX_COUNT] = { GYRO_AUTODETECT, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE, PITOT_NONE, OPFLOW_NONE, SECONDARY_IMU_NONE }; +uint8_t detectedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE, PITOT_NONE, OPFLOW_NONE, SECONDARY_IMU_NONE }; bool sensorsAutodetect(void) { diff --git a/src/main/sensors/rangefinder.c b/src/main/sensors/rangefinder.c index 8b26c586d8..ce3aadd0e9 100644 --- a/src/main/sensors/rangefinder.c +++ b/src/main/sensors/rangefinder.c @@ -36,13 +36,12 @@ #include "drivers/io.h" #include "drivers/time.h" #include "drivers/rangefinder/rangefinder.h" -#include "drivers/rangefinder/rangefinder_hcsr04.h" #include "drivers/rangefinder/rangefinder_srf10.h" -#include "drivers/rangefinder/rangefinder_hcsr04_i2c.h" #include "drivers/rangefinder/rangefinder_vl53l0x.h" #include "drivers/rangefinder/rangefinder_vl53l1x.h" #include "drivers/rangefinder/rangefinder_virtual.h" #include "drivers/rangefinder/rangefinder_us42.h" +#include "drivers/rangefinder/rangefinder_tof10120_i2c.h" #include "fc/config.h" #include "fc/runtime_config.h" @@ -64,28 +63,13 @@ rangefinder_t rangefinder; #define RANGEFINDER_DYNAMIC_FACTOR 75 #ifdef USE_RANGEFINDER -PG_REGISTER_WITH_RESET_TEMPLATE(rangefinderConfig_t, rangefinderConfig, PG_RANGEFINDER_CONFIG, 1); +PG_REGISTER_WITH_RESET_TEMPLATE(rangefinderConfig_t, rangefinderConfig, PG_RANGEFINDER_CONFIG, 3); PG_RESET_TEMPLATE(rangefinderConfig_t, rangefinderConfig, .rangefinder_hardware = SETTING_RANGEFINDER_HARDWARE_DEFAULT, .use_median_filtering = SETTING_RANGEFINDER_MEDIAN_FILTER_DEFAULT, ); -const rangefinderHardwarePins_t * rangefinderGetHardwarePins(void) -{ - static rangefinderHardwarePins_t rangefinderHardwarePins; - -#if defined(RANGEFINDER_HCSR04_TRIGGER_PIN) - rangefinderHardwarePins.triggerTag = IO_TAG(RANGEFINDER_HCSR04_TRIGGER_PIN); - rangefinderHardwarePins.echoTag = IO_TAG(RANGEFINDER_HCSR04_ECHO_PIN); -#else - // No Trig/Echo hardware rangefinder - rangefinderHardwarePins.triggerTag = IO_TAG(NONE); - rangefinderHardwarePins.echoTag = IO_TAG(NONE); -#endif - return &rangefinderHardwarePins; -} - /* * Detect which rangefinder is present */ @@ -95,18 +79,6 @@ static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwar requestedSensors[SENSOR_INDEX_RANGEFINDER] = rangefinderHardwareToUse; switch (rangefinderHardwareToUse) { - case RANGEFINDER_HCSR04: -#ifdef USE_RANGEFINDER_HCSR04 - { - const rangefinderHardwarePins_t *rangefinderHardwarePins = rangefinderGetHardwarePins(); - if (hcsr04Detect(dev, rangefinderHardwarePins)) { // FIXME: Do actual detection if HC-SR04 is plugged in - rangefinderHardware = RANGEFINDER_HCSR04; - rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_HCSR04_TASK_PERIOD_MS)); - } - } -#endif - break; - case RANGEFINDER_SRF10: #ifdef USE_RANGEFINDER_SRF10 if (srf10Detect(dev)) { @@ -116,15 +88,6 @@ static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwar #endif break; - case RANGEFINDER_HCSR04I2C: -#ifdef USE_RANGEFINDER_HCSR04_I2C - if (hcsr04i2c0Detect(dev)) { - rangefinderHardware = RANGEFINDER_HCSR04I2C; - rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_HCSR04_i2C_TASK_PERIOD_MS)); - } -#endif - break; - case RANGEFINDER_VL53L0X: #if defined(USE_RANGEFINDER_VL53L0X) if (vl53l0xDetect(dev)) { @@ -170,6 +133,15 @@ static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwar #endif break; + case RANGEFINDER_TOF10102I2C: +#ifdef USE_RANGEFINDER_TOF10120_I2C + if (tof10120Detect(dev)) { + rangefinderHardware = RANGEFINDER_TOF10102I2C; + rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_TOF10120_I2C_TASK_PERIOD_MS)); + } +#endif + break; + case RANGEFINDER_NONE: rangefinderHardware = RANGEFINDER_NONE; break; diff --git a/src/main/sensors/rangefinder.h b/src/main/sensors/rangefinder.h index 568c01ae12..e7ab9e8a61 100644 --- a/src/main/sensors/rangefinder.h +++ b/src/main/sensors/rangefinder.h @@ -23,15 +23,13 @@ typedef enum { RANGEFINDER_NONE = 0, - RANGEFINDER_HCSR04 = 1, - RANGEFINDER_SRF10 = 2, - RANGEFINDER_HCSR04I2C = 3, - RANGEFINDER_VL53L0X = 4, - RANGEFINDER_MSP = 5, - RANGEFINDER_UNUSED = 6, // Was UIB - RANGEFINDER_BENEWAKE = 7, - RANGEFINDER_VL53L1X = 8, - RANGEFINDER_US42 = 9, + RANGEFINDER_SRF10 = 1, + RANGEFINDER_VL53L0X = 2, + RANGEFINDER_MSP = 3, + RANGEFINDER_BENEWAKE = 4, + RANGEFINDER_VL53L1X = 5, + RANGEFINDER_US42 = 6, + RANGEFINDER_TOF10102I2C = 7, } rangefinderType_e; typedef struct rangefinderConfig_s { @@ -51,8 +49,6 @@ typedef struct rangefinder_s { extern rangefinder_t rangefinder; -const rangefinderHardwarePins_t * rangefinderGetHardwarePins(void); - bool rangefinderInit(void); int32_t rangefinderGetLatestAltitude(void); diff --git a/src/main/sensors/sensors.h b/src/main/sensors/sensors.h index 1a6ab0b8b2..6d8bbea298 100644 --- a/src/main/sensors/sensors.h +++ b/src/main/sensors/sensors.h @@ -25,6 +25,7 @@ typedef enum { SENSOR_INDEX_RANGEFINDER, SENSOR_INDEX_PITOT, SENSOR_INDEX_OPFLOW, + SENSOR_INDEX_IMU2, SENSOR_INDEX_COUNT } sensorIndex_e; diff --git a/src/main/target/AIKONF4/target.h b/src/main/target/AIKONF4/target.h index d0e13b8a12..28ee33cf22 100644 --- a/src/main/target/AIKONF4/target.h +++ b/src/main/target/AIKONF4/target.h @@ -146,7 +146,6 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD 0xffff -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN SPI3_NSS_PIN diff --git a/src/main/target/AIRBOTF4/target.h b/src/main/target/AIRBOTF4/target.h index 70279b57b2..4dbd8476c8 100644 --- a/src/main/target/AIRBOTF4/target.h +++ b/src/main/target/AIRBOTF4/target.h @@ -110,7 +110,6 @@ #define USE_RANGEFINDER #define RANGEFINDER_I2C_BUS BUS_I2C2 -#define USE_RANGEFINDER_HCSR04_I2C #define USE_ADC #define ADC_CHANNEL_1_PIN PC1 @@ -127,7 +126,7 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT -#define DEFAULT_RX_TYPE RX_TYPE_PPM +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define DISABLE_RX_PWM_FEATURE #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX | FEATURE_VBAT) diff --git a/src/main/target/AIRBOTF7/target.h b/src/main/target/AIRBOTF7/target.h index 4a554cdd56..460d2ae820 100644 --- a/src/main/target/AIRBOTF7/target.h +++ b/src/main/target/AIRBOTF7/target.h @@ -59,7 +59,6 @@ #endif // *************** OSD ***************************** -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PC15 diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h index d60025155c..7b5349533c 100644 --- a/src/main/target/ALIENFLIGHTF3/target.h +++ b/src/main/target/ALIENFLIGHTF3/target.h @@ -74,7 +74,6 @@ #define USE_UART2 // Receiver - RX (PA3) #define USE_UART3 // Not connected - 10/RX (PB11) 11/TX (PB10) #define SERIAL_PORT_COUNT 4 -#define AVOID_UART2_FOR_PWM_PPM #define UART1_TX_PIN PB6 #define UART1_RX_PIN PB7 diff --git a/src/main/target/ALIENFLIGHTNGF7/target.h b/src/main/target/ALIENFLIGHTNGF7/target.h index 9f48f87c37..8e727f1171 100644 --- a/src/main/target/ALIENFLIGHTNGF7/target.h +++ b/src/main/target/ALIENFLIGHTNGF7/target.h @@ -138,7 +138,6 @@ #define I2C1_SCL PB6 #define I2C1_SDA PB7 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN PB12 diff --git a/src/main/target/ANYFC/target.h b/src/main/target/ANYFC/target.h index 3d86866897..dbc23b09d7 100644 --- a/src/main/target/ANYFC/target.h +++ b/src/main/target/ANYFC/target.h @@ -116,7 +116,7 @@ #define SENSORS_SET (SENSOR_ACC|SENSOR_MAG|SENSOR_BARO) -#define DEFAULT_RX_TYPE RX_TYPE_PPM +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/ANYFCF7/target.h b/src/main/target/ANYFCF7/target.h index fbd9683c92..02d73169c7 100644 --- a/src/main/target/ANYFCF7/target.h +++ b/src/main/target/ANYFCF7/target.h @@ -65,7 +65,6 @@ #define PITOT_I2C_BUS BUS_I2C2 #define USE_RANGEFINDER -#define USE_RANGEFINDER_HCSR04_I2C #define RANGEFINDER_I2C_BUS BUS_I2C2 #define USE_VCP @@ -125,7 +124,6 @@ #define SPI4_MISO_PIN PE13 #define SPI4_MOSI_PIN PE14 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN SPI3_NSS_PIN diff --git a/src/main/target/ANYFCM7/target.h b/src/main/target/ANYFCM7/target.h index 0bb51355ec..2ba9486297 100644 --- a/src/main/target/ANYFCM7/target.h +++ b/src/main/target/ANYFCM7/target.h @@ -111,7 +111,6 @@ #define USE_FLASH_M25P16 #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN SPI3_NSS_PIN diff --git a/src/main/target/ASGARD32F4/target.h b/src/main/target/ASGARD32F4/target.h index d9018a5cd9..ad671e8065 100644 --- a/src/main/target/ASGARD32F4/target.h +++ b/src/main/target/ASGARD32F4/target.h @@ -117,7 +117,6 @@ #define SPI3_MISO_PIN PB4 #define SPI3_MOSI_PIN PB5 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN PA15 @@ -143,10 +142,6 @@ #define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS -// Disable PWM & PPM inputs -#undef USE_RX_PWM -#undef USE_RX_PPM - // Set default UARTs #define TELEMETRY_UART SERIAL_PORT_SOFTSERIAL1 #define SERIALRX_UART SERIAL_PORT_USART1 diff --git a/src/main/target/ASGARD32F7/target.h b/src/main/target/ASGARD32F7/target.h index 97e2ad210e..0ac92994f9 100644 --- a/src/main/target/ASGARD32F7/target.h +++ b/src/main/target/ASGARD32F7/target.h @@ -119,7 +119,6 @@ #define SPI3_MISO_PIN PB4 #define SPI3_MOSI_PIN PB5 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN PA15 @@ -148,10 +147,6 @@ #define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS -// Disable PWM & PPM inputs -#undef USE_RX_PWM -#undef USE_RX_PPM - // Set default UARTs #define TELEMETRY_UART SERIAL_PORT_SOFTSERIAL1 #define SERIALRX_UART SERIAL_PORT_USART1 diff --git a/src/main/target/BEEROTORF4/target.h b/src/main/target/BEEROTORF4/target.h index 9d86514f9d..9c78cf7738 100644 --- a/src/main/target/BEEROTORF4/target.h +++ b/src/main/target/BEEROTORF4/target.h @@ -57,7 +57,6 @@ #define TEMPERATURE_I2C_BUS BUS_I2C1 #define BNO055_I2C_BUS BUS_I2C1 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN SPI3_NSS_PIN diff --git a/src/main/target/BETAFLIGHTF3/target.h b/src/main/target/BETAFLIGHTF3/target.h index 1c4cc3fe68..71e469db2c 100755 --- a/src/main/target/BETAFLIGHTF3/target.h +++ b/src/main/target/BETAFLIGHTF3/target.h @@ -79,7 +79,6 @@ #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI1 #define MAX7456_CS_PIN PA1 diff --git a/src/main/target/BETAFLIGHTF4/target.h b/src/main/target/BETAFLIGHTF4/target.h index 872327b41a..e9b44bb480 100755 --- a/src/main/target/BETAFLIGHTF4/target.h +++ b/src/main/target/BETAFLIGHTF4/target.h @@ -45,7 +45,6 @@ #define BMP280_SPI_BUS BUS_SPI2 #define BMP280_CS_PIN PB3 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PB12 @@ -142,7 +141,6 @@ #define USE_RANGEFINDER #define RANGEFINDER_I2C_BUS BUS_I2C2 -#define USE_RANGEFINDER_HCSR04_I2C #define USE_ADC #define ADC_CHANNEL_1_PIN PC1 diff --git a/src/main/target/BETAFPVF722/CMakeLists.txt b/src/main/target/BETAFPVF722/CMakeLists.txt new file mode 100644 index 0000000000..bad557907d --- /dev/null +++ b/src/main/target/BETAFPVF722/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f722xe(BETAFPVF722) diff --git a/src/main/target/BETAFPVF722/target.c b/src/main/target/BETAFPVF722/target.c new file mode 100755 index 0000000000..2d02a8a71e --- /dev/null +++ b/src/main/target/BETAFPVF722/target.c @@ -0,0 +1,44 @@ +/* +* This file is part of Cleanflight. +* +* Cleanflight is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* Cleanflight is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with Cleanflight. If not, see . +*/ + +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" + +const timerHardware_t timerHardware[] = { + DEF_TIM(TIM1, CH3, PA10, TIM_USE_PPM, 0, 0), // PPM + + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR, 0, 0), // S1 D(1, 4, 5) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR, 0, 0), // S2 D(2, 3, 7) + DEF_TIM(TIM2, CH2, PA1, TIM_USE_MC_MOTOR, 0, 0), // S3 D(2, 4, 7) + DEF_TIM(TIM2, CH1, PA0, TIM_USE_MC_MOTOR, 0, 0), // S4 D(2, 7, 7) + + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR, 0, 0), // S5 DMA1_ST2 + + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR, 0, 0), // S6 DMA2_ST6 + + DEF_TIM(TIM8, CH3, PC8, TIM_USE_ANY, 0, 0), //camera + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED STRIP D(1, 5, 3) +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/BETAFPVF722/target.h b/src/main/target/BETAFPVF722/target.h new file mode 100755 index 0000000000..85c709edab --- /dev/null +++ b/src/main/target/BETAFPVF722/target.h @@ -0,0 +1,165 @@ +/* + * 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 . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "BHF7" + +#define USBD_PRODUCT_STRING "BETAFPVF722" + +#define LED0 PC15 + +#define BEEPER PC14 +#define BEEPER_INVERTED + +// *************** Gyro & ACC ********************** +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_BUS BUS_SPI1 + +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW180_DEG + +#define USE_EXTI +#define GYRO_INT_EXTI PC4 +#define USE_MPU_DATA_READY_SIGNAL + + +// *************** I2C/Baro/Mag ********************* +#define USE_I2C + +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define USE_I2C_DEVICE_2 +#define I2C_DEVICE_2_SHARES_UART3 + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_BMP085 +#define USE_BARO_DPS310 +#define USE_BARO_SPL06 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C2 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_IST8308 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL + +#define TEMPERATURE_I2C_BUS BUS_I2C2 +#define PITOT_I2C_BUS BUS_I2C2 +#define RANGEFINDER_I2C_BUS BUS_I2C2 + +// *************** OSD************************* +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + +#define USE_OSD +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI3 +#define MAX7456_CS_PIN PA15 + + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 + +#define USE_UART4 +#define UART4_RX_PIN PC11 +#define UART4_TX_PIN PC10 + +#define USE_UART5 +#define UART5_RX_PIN PD2 +#define UART5_TX_PIN PC12 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define SERIAL_PORT_COUNT 7 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 +#define ADC_CHANNEL_1_PIN PC0 +#define ADC_CHANNEL_2_PIN PC1 +#define ADC_CHANNEL_3_PIN PC2 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 + + +// *************** FLASH ***************************** +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_CS_PIN PB12 +#define M25P16_SPI_BUS BUS_SPI2 + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + + +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY| FEATURE_VBAT | FEATURE_OSD ) + + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) + +#define MAX_PWM_OUTPUT_PORTS 6 +#define TARGET_MOTOR_COUNT 4 + +// ESC-related features +#define USE_DSHOT +#define USE_ESC_SENSOR diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index 321b5e280a..d40c931411 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -131,7 +131,7 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT -#define DEFAULT_RX_TYPE RX_TYPE_PPM +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/CLRACINGF4AIR/target.h b/src/main/target/CLRACINGF4AIR/target.h index 56c3918aff..dab824e036 100644 --- a/src/main/target/CLRACINGF4AIR/target.h +++ b/src/main/target/CLRACINGF4AIR/target.h @@ -64,7 +64,6 @@ #define BMP280_SPI_BUS BUS_SPI3 #define BMP280_CS_PIN PB3 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN PA15 @@ -116,6 +115,7 @@ #define TEMPERATURE_I2C_BUS BUS_I2C2 #define BNO055_I2C_BUS BUS_I2C2 +#define MAG_I2C_BUS BUS_I2C2 #endif #define USE_ADC diff --git a/src/main/target/COLIBRI/target.h b/src/main/target/COLIBRI/target.h index c111bcb378..a99cf0cbcd 100755 --- a/src/main/target/COLIBRI/target.h +++ b/src/main/target/COLIBRI/target.h @@ -124,9 +124,6 @@ #define USE_RANGEFINDER #define RANGEFINDER_I2C_BUS BUS_I2C3 -#define USE_RANGEFINDER_HCSR04 -#define RANGEFINDER_HCSR04_TRIGGER_PIN PB8 -#define RANGEFINDER_HCSR04_ECHO_PIN PB9 #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT diff --git a/src/main/target/DALRCF405/target.h b/src/main/target/DALRCF405/target.h index c2787f71ef..f939dd1f23 100644 --- a/src/main/target/DALRCF405/target.h +++ b/src/main/target/DALRCF405/target.h @@ -66,7 +66,6 @@ #define SPI3_MISO_PIN PC11 #define SPI3_MOSI_PIN PB5 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN PA15 @@ -99,7 +98,6 @@ #define BNO055_I2C_BUS BUS_I2C1 #define USE_RANGEFINDER -#define USE_RANGEFINDER_HCSR04_I2C #define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS #define PITOT_I2C_BUS DEFAULT_I2C_BUS diff --git a/src/main/target/DALRCF722DUAL/target.h b/src/main/target/DALRCF722DUAL/target.h index ca29604c6c..ad863e2e0a 100644 --- a/src/main/target/DALRCF722DUAL/target.h +++ b/src/main/target/DALRCF722DUAL/target.h @@ -103,7 +103,6 @@ #define SPI3_MISO_PIN PB4 #define SPI3_MOSI_PIN PB5 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN SPI2_NSS_PIN @@ -139,7 +138,6 @@ #define MAX_PWM_OUTPUT_PORTS 6 #define USE_DSHOT -#define USE_SERIALSHOT #define USE_ESC_SENSOR #define TARGET_IO_PORTA 0xffff diff --git a/src/main/target/F4BY/target.h b/src/main/target/F4BY/target.h index e50a360d41..03fc7c6749 100644 --- a/src/main/target/F4BY/target.h +++ b/src/main/target/F4BY/target.h @@ -134,7 +134,7 @@ #define SENSORS_SET (SENSOR_ACC|SENSOR_MAG|SENSOR_BARO) #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_VBAT | FEATURE_BLACKBOX) -#define DEFAULT_RX_TYPE RX_TYPE_PPM +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT diff --git a/src/main/target/FALCORE/config.c b/src/main/target/FALCORE/config.c index f109d0e100..d1e6e71818 100755 --- a/src/main/target/FALCORE/config.c +++ b/src/main/target/FALCORE/config.c @@ -192,4 +192,6 @@ void targetConfiguration(void) ((controlRateConfig_t*)currentControlRateProfile)->throttle.rcExpo8 = 0; ((controlRateConfig_t*)currentControlRateProfile)->throttle.dynPID = 10; ((controlRateConfig_t*)currentControlRateProfile)->throttle.pa_breakpoint = 1600; + + beeperConfigMutable()->pwmMode = true; } diff --git a/src/main/target/FALCORE/target.h b/src/main/target/FALCORE/target.h index cdb8ba4636..094adb7631 100755 --- a/src/main/target/FALCORE/target.h +++ b/src/main/target/FALCORE/target.h @@ -25,7 +25,6 @@ #define USE_HARDWARE_PREBOOT_SETUP // FALCORE board requires some hardware to be set up before booting and detecting sensors #define BEEPER PB4 -#define BEEPER_PWM #define BEEPER_PWM_FREQUENCY 2700 #define MPU6500_CS_PIN PC0 @@ -57,7 +56,6 @@ #define USE_UART4 #define USE_UART5 #define SERIAL_PORT_COUNT 5 -#define AVOID_UART2_FOR_PWM_PPM #define UART1_TX_PIN PC4 #define UART1_RX_PIN PC5 @@ -118,9 +116,6 @@ #define BIND_PIN PA3 #define USE_RANGEFINDER -#define USE_RANGEFINDER_HCSR04 -#define RANGEFINDER_HCSR04_TRIGGER_PIN PA7 -#define RANGEFINDER_HCSR04_ECHO_PIN PA2 #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX | FEATURE_VBAT | FEATURE_GPS | FEATURE_TELEMETRY | FEATURE_LED_STRIP) #define DEFAULT_RX_TYPE RX_TYPE_SERIAL diff --git a/src/main/target/FF_F35_LIGHTNING/target.h b/src/main/target/FF_F35_LIGHTNING/target.h index a916b186d7..6fd417b2eb 100644 --- a/src/main/target/FF_F35_LIGHTNING/target.h +++ b/src/main/target/FF_F35_LIGHTNING/target.h @@ -42,13 +42,13 @@ #define USE_MAG #define USE_MAG_MPU9250 +#define MAG_I2C_BUS BUS_I2C1 #define USE_BARO #define USE_BARO_BMP280 #define BMP280_CS_PIN PC5 #define BMP280_SPI_BUS BUS_SPI3 -#define USE_OSD #define USE_MAX7456 #define MAX7456_CS_PIN PA4 #define MAX7456_SPI_BUS BUS_SPI1 diff --git a/src/main/target/FF_FORTINIF4/target.h b/src/main/target/FF_FORTINIF4/target.h index 669f4bd440..00c154dd94 100644 --- a/src/main/target/FF_FORTINIF4/target.h +++ b/src/main/target/FF_FORTINIF4/target.h @@ -58,7 +58,6 @@ /*---------------------------------*/ /*-------------OSD-----------------*/ -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN PB3 diff --git a/src/main/target/FF_PIKOF4/target.h b/src/main/target/FF_PIKOF4/target.h index 59337ad53b..244ddec53b 100644 --- a/src/main/target/FF_PIKOF4/target.h +++ b/src/main/target/FF_PIKOF4/target.h @@ -73,7 +73,6 @@ #if defined(FF_PIKOF4OSD) /*-------------OSD-----------------*/ -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI1 #define MAX7456_CS_PIN PA4 diff --git a/src/main/target/FIREWORKSV2/target.h b/src/main/target/FIREWORKSV2/target.h index a0ea4428da..ec20e387ec 100644 --- a/src/main/target/FIREWORKSV2/target.h +++ b/src/main/target/FIREWORKSV2/target.h @@ -123,7 +123,6 @@ #else #define RANGEFINDER_I2C_BUS BUS_I2C2 #endif -#define USE_RANGEFINDER_HCSR04_I2C #define USE_VCP #define VBUS_SENSING_PIN PC5 @@ -175,7 +174,6 @@ #define SPI3_MISO_PIN PC11 #define SPI3_MOSI_PIN PC12 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN PA15 diff --git a/src/main/target/FISHDRONEF4/target.h b/src/main/target/FISHDRONEF4/target.h index 45b8b5b865..2b150d3f02 100644 --- a/src/main/target/FISHDRONEF4/target.h +++ b/src/main/target/FISHDRONEF4/target.h @@ -78,7 +78,6 @@ #define SPI2_MISO_PIN PC2 #define SPI2_MOSI_PIN PC3 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN SPI2_NSS_PIN @@ -148,9 +147,6 @@ // *************** RANGEFINDER ***************************** // #define USE_RANGEFINDER -// #define USE_RANGEFINDER_HCSR04 -// #define RANGEFINDER_HCSR04_TRIGGER_PIN PB10 -// #define RANGEFINDER_HCSR04_ECHO_PIN PB11 // #define USE_RANGEFINDER_SRF10 // *************** NAV ***************************** diff --git a/src/main/target/FLYWOOF411/target.h b/src/main/target/FLYWOOF411/target.h index 34d110d7a5..22229390b0 100644 --- a/src/main/target/FLYWOOF411/target.h +++ b/src/main/target/FLYWOOF411/target.h @@ -88,7 +88,6 @@ #define USE_MAG_LIS3MDL // *************** SPI OSD ***************************** -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PB12 @@ -166,7 +165,6 @@ #define USE_DSHOT #define USE_ESC_SENSOR -#define USE_SERIALSHOT #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define TARGET_IO_PORTA 0xffff diff --git a/src/main/target/FLYWOOF745/target.h b/src/main/target/FLYWOOF745/target.h index c58690f9b8..c129a87c0b 100644 --- a/src/main/target/FLYWOOF745/target.h +++ b/src/main/target/FLYWOOF745/target.h @@ -107,7 +107,6 @@ #define SPI4_MISO_PIN PE5 #define SPI4_MOSI_PIN PE6 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 diff --git a/src/main/target/FLYWOOF7DUAL/target.h b/src/main/target/FLYWOOF7DUAL/target.h index 10b8d285bb..8cae941538 100644 --- a/src/main/target/FLYWOOF7DUAL/target.h +++ b/src/main/target/FLYWOOF7DUAL/target.h @@ -73,7 +73,6 @@ #define M25P16_SPI_BUS BUS_SPI3 /*** OSD ***/ -#define USE_OSD #define USE_MAX7456 #define MAX7456_CS_PIN PB12 #define MAX7456_SPI_BUS BUS_SPI2 diff --git a/src/main/target/FOXEERF405/target.h b/src/main/target/FOXEERF405/target.h index 2b8d4e2ac0..2b9f5ae7a0 100644 --- a/src/main/target/FOXEERF405/target.h +++ b/src/main/target/FOXEERF405/target.h @@ -72,7 +72,6 @@ #define M25P16_SPI_BUS BUS_SPI2 /*** OSD ***/ -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN PA15 @@ -147,4 +146,5 @@ #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(2)) \ No newline at end of file +#define TARGET_IO_PORTD (BIT(2)) + diff --git a/src/main/target/FOXEERF722DUAL/target.c b/src/main/target/FOXEERF722DUAL/target.c index 6f124321ab..bbce6735d4 100644 --- a/src/main/target/FOXEERF722DUAL/target.c +++ b/src/main/target/FOXEERF722DUAL/target.c @@ -29,10 +29,14 @@ BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500, DEVHW_MPU6500, MPU6500_SPI_BUS, #endif const timerHardware_t timerHardware[] = { - DEF_TIM(TIM4, CH2, PB7, TIM_USE_PPM, 0, 0), // PPM&SBUS + DEF_TIM(TIM4, CH2, PB7, TIM_USE_PPM, 0, 0), // PPM&SBUS DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S1 - D(2, 1, 6) + #if defined(FOXEERF722V2) + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S2 - D(2, 1, 6) + #else DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 - D(2, 6, 0) + #endif DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 - D(2, 7, 7) DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 - D(2, 4, 7) DEF_TIM(TIM3, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 - D(1, 4, 5) diff --git a/src/main/target/FOXEERF722DUAL/target.h b/src/main/target/FOXEERF722DUAL/target.h index b339cef7a9..f946150985 100644 --- a/src/main/target/FOXEERF722DUAL/target.h +++ b/src/main/target/FOXEERF722DUAL/target.h @@ -81,7 +81,6 @@ #define M25P16_SPI_BUS BUS_SPI2 /*** OSD ***/ -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN PC3 @@ -158,4 +157,5 @@ #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(2)) \ No newline at end of file +#define TARGET_IO_PORTD (BIT(2)) + diff --git a/src/main/target/FRSKYF3/target.h b/src/main/target/FRSKYF3/target.h index b046c399cb..304fa22214 100644 --- a/src/main/target/FRSKYF3/target.h +++ b/src/main/target/FRSKYF3/target.h @@ -56,7 +56,6 @@ #define I2C1_SDA PB7 #define USE_SPI -#define USE_OSD // include the max7456 driver #define USE_MAX7456 diff --git a/src/main/target/FRSKYF4/target.h b/src/main/target/FRSKYF4/target.h index 4d950a18a7..222153b964 100755 --- a/src/main/target/FRSKYF4/target.h +++ b/src/main/target/FRSKYF4/target.h @@ -37,7 +37,6 @@ #define BMP280_SPI_BUS BUS_SPI3 #define BMP280_CS_PIN PB3 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN PA15 @@ -92,9 +91,6 @@ #define CURRENT_METER_ADC_CHANNEL ADC_CHN_1 #define VBAT_ADC_CHANNEL ADC_CHN_2 -#undef USE_RX_PWM -#undef USE_RX_PPM - #define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_OSD | FEATURE_TELEMETRY) #define SERIALRX_PROVIDER SERIALRX_SBUS diff --git a/src/main/target/FRSKYPILOT/config.c b/src/main/target/FRSKYPILOT/config.c index 05ec1b5a85..60f29d3884 100644 --- a/src/main/target/FRSKYPILOT/config.c +++ b/src/main/target/FRSKYPILOT/config.c @@ -22,6 +22,7 @@ #include "io/serial.h" #include "rx/rx.h" +#include "fc/config.h" void targetConfiguration(void) { @@ -29,4 +30,6 @@ void targetConfiguration(void) serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART6)].functionMask = FUNCTION_FRSKY_OSD; rxConfigMutable()->serialrx_inverted = 1; + + beeperConfigMutable()->pwmMode = true; } diff --git a/src/main/target/FRSKYPILOT/target.h b/src/main/target/FRSKYPILOT/target.h index d4da84dde0..539b465461 100644 --- a/src/main/target/FRSKYPILOT/target.h +++ b/src/main/target/FRSKYPILOT/target.h @@ -29,7 +29,6 @@ #define BEEPER PA0 #define BEEPER_INVERTED -#define BEEPER_PWM #define BEEPER_PWM_FREQUENCY 4000 #define USE_SPI @@ -152,7 +151,6 @@ #define UART5_AF 1 // OSD -#define USE_OSD #define USE_UART6 #define UART6_TX_PIN PC6 #define UART6_RX_PIN PC7 diff --git a/src/main/target/FRSKY_ROVERF7/target.h b/src/main/target/FRSKY_ROVERF7/target.h index 6347cd9bd9..ef6334ca60 100755 --- a/src/main/target/FRSKY_ROVERF7/target.h +++ b/src/main/target/FRSKY_ROVERF7/target.h @@ -87,7 +87,6 @@ #define SPI3_MISO_PIN PB4 #define SPI3_MOSI_PIN PB5 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN PD2 @@ -151,4 +150,3 @@ #define MAX_PWM_OUTPUT_PORTS 5 #define USE_DSHOT #define USE_ESC_SENSOR -#define USE_SERIALSHOT diff --git a/src/main/target/FURYF3/target.h b/src/main/target/FURYF3/target.h index 9091457131..78fc898cc0 100755 --- a/src/main/target/FURYF3/target.h +++ b/src/main/target/FURYF3/target.h @@ -128,12 +128,9 @@ #define WS2811_PIN PA8 #define USE_RANGEFINDER -#define USE_RANGEFINDER_HCSR04 -#define RANGEFINDER_HCSR04_TRIGGER_PIN PB0 // RC_CH7 (PB0) - only 3.3v ( add a 1K Ohms resistor ) -#define RANGEFINDER_HCSR04_ECHO_PIN PB1 // RC_CH8 (PB1) - only 3.3v ( add a 1K Ohms resistor ) #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) -#define DEFAULT_RX_TYPE RX_TYPE_PPM +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define USE_SPEKTRUM_BIND // UART3, diff --git a/src/main/target/FURYF4OSD/target.h b/src/main/target/FURYF4OSD/target.h index d5ccf15a95..9e8283dfff 100644 --- a/src/main/target/FURYF4OSD/target.h +++ b/src/main/target/FURYF4OSD/target.h @@ -67,7 +67,6 @@ #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PB12 @@ -138,7 +137,6 @@ #define USE_RANGEFINDER #define USE_RANGEFINDER_MSP -#define USE_RANGEFINDER_HCSR04_I2C #define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS #define PITOT_I2C_BUS DEFAULT_I2C_BUS diff --git a/src/main/target/HGLRCF722/target.h b/src/main/target/HGLRCF722/target.h index 440a657c1b..456c68cb2e 100644 --- a/src/main/target/HGLRCF722/target.h +++ b/src/main/target/HGLRCF722/target.h @@ -92,7 +92,6 @@ #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PB12 @@ -176,5 +175,5 @@ #define MAX_PWM_OUTPUT_PORTS 8 #define USE_DSHOT -#define USE_SERIALSHOT -#define USE_ESC_SENSOR \ No newline at end of file +#define USE_ESC_SENSOR + diff --git a/src/main/target/IFLIGHTF4_SUCCEXD/target.h b/src/main/target/IFLIGHTF4_SUCCEXD/target.h index aee436548c..59523f3c70 100644 --- a/src/main/target/IFLIGHTF4_SUCCEXD/target.h +++ b/src/main/target/IFLIGHTF4_SUCCEXD/target.h @@ -49,7 +49,6 @@ #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PB12 @@ -116,7 +115,6 @@ #define USE_RANGEFINDER #define USE_RANGEFINDER_MSP -#define USE_RANGEFINDER_HCSR04_I2C #define RANGEFINDER_I2C_BUS BUS_I2C1 // *************** ADC ***************************** diff --git a/src/main/target/IFLIGHTF4_TWING/target.h b/src/main/target/IFLIGHTF4_TWING/target.h index ab5d0bf5f3..5a3943e11a 100644 --- a/src/main/target/IFLIGHTF4_TWING/target.h +++ b/src/main/target/IFLIGHTF4_TWING/target.h @@ -87,7 +87,6 @@ #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PB12 diff --git a/src/main/target/IFLIGHTF7_TWING/target.c b/src/main/target/IFLIGHTF7_TWING/target.c index 83090b5c94..fb1d9804fd 100644 --- a/src/main/target/IFLIGHTF7_TWING/target.c +++ b/src/main/target/IFLIGHTF7_TWING/target.c @@ -37,11 +37,11 @@ const timerHardware_t timerHardware[] = { DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR | TIM_USE_MC_SERVO, 0, 0), + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR | TIM_USE_MC_SERVO, 0, 0), - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), DEF_TIM(TIM2, CH2, PA1, TIM_USE_LED, 0, 0), }; diff --git a/src/main/target/IFLIGHTF7_TWING/target.h b/src/main/target/IFLIGHTF7_TWING/target.h index 2fb765bce7..fb74f2d530 100644 --- a/src/main/target/IFLIGHTF7_TWING/target.h +++ b/src/main/target/IFLIGHTF7_TWING/target.h @@ -100,7 +100,6 @@ #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PB12 diff --git a/src/main/target/KAKUTEF4/target.h b/src/main/target/KAKUTEF4/target.h index 2b0f908a91..7c3843b047 100755 --- a/src/main/target/KAKUTEF4/target.h +++ b/src/main/target/KAKUTEF4/target.h @@ -47,6 +47,10 @@ #define MPU6500_CS_PIN PC4 #define MPU6500_SPI_BUS BUS_SPI1 +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW270_DEG +#define MPU6000_CS_PIN PC4 +#define MPU6000_SPI_BUS BUS_SPI1 #ifdef KAKUTEF4V2 # define USE_I2C @@ -75,7 +79,6 @@ # define USE_MAG #endif -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN PB14 @@ -168,7 +171,6 @@ #define TARGET_IO_PORTD (BIT(2)) #define USE_DSHOT -#define USE_SERIALSHOT #define USE_ESC_SENSOR #ifdef KAKUTEF4V2 diff --git a/src/main/target/KAKUTEF7/target.h b/src/main/target/KAKUTEF7/target.h index 0d42c7e2d6..1beb8db7ba 100644 --- a/src/main/target/KAKUTEF7/target.h +++ b/src/main/target/KAKUTEF7/target.h @@ -104,7 +104,6 @@ #define SPI4_MISO_PIN PE5 #define SPI4_MOSI_PIN PE6 -#define USE_OSD #ifndef KAKUTEF7HDV #define USE_MAX7456 @@ -145,6 +144,7 @@ #define USE_MAG_IST8310 #define USE_MAG_IST8308 #define USE_MAG_LIS3MDL +#define USE_MAG_MLX90393 #define TEMPERATURE_I2C_BUS BUS_I2C1 @@ -179,4 +179,5 @@ #define MAX_PWM_OUTPUT_PORTS 6 -#define BNO055_I2C_BUS BUS_I2C1 \ No newline at end of file +#define BNO055_I2C_BUS BUS_I2C1 + diff --git a/src/main/target/KAKUTEF7MINIV3/target.h b/src/main/target/KAKUTEF7MINIV3/target.h index 7ecc7a3d1e..8914f81af1 100644 --- a/src/main/target/KAKUTEF7MINIV3/target.h +++ b/src/main/target/KAKUTEF7MINIV3/target.h @@ -106,7 +106,6 @@ /* * OSD */ -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN PA15 @@ -161,7 +160,6 @@ #define MAX_PWM_OUTPUT_PORTS 6 #define USE_DSHOT #define USE_ESC_SENSOR -#define USE_SERIALSHOT #define USE_RANGEFINDER #define RANGEFINDER_I2C_BUS BUS_I2C1 @@ -173,4 +171,5 @@ #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(2)) \ No newline at end of file +#define TARGET_IO_PORTD (BIT(2)) + diff --git a/src/main/target/KISSFC/target.h b/src/main/target/KISSFC/target.h index a97d2b6166..230be7f66d 100644 --- a/src/main/target/KISSFC/target.h +++ b/src/main/target/KISSFC/target.h @@ -76,8 +76,6 @@ #define MAX_PWM_OUTPUT_PORTS 12 -#define AVOID_UART2_FOR_PWM_PPM - #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff diff --git a/src/main/target/KROOZX/target.h b/src/main/target/KROOZX/target.h index 4f5eac5f7c..aa90575ecb 100755 --- a/src/main/target/KROOZX/target.h +++ b/src/main/target/KROOZX/target.h @@ -61,7 +61,6 @@ #define SDCARD_DETECT_INVERTED #define SDCARD_DETECT_PIN PC13 -#define USE_OSD #ifdef USE_MSP_DISPLAYPORT #undef USE_MSP_DISPLAYPORT #endif @@ -135,7 +134,7 @@ #define SENSORS_SET (SENSOR_ACC|SENSOR_MAG|SENSOR_BARO) -#define DEFAULT_RX_TYPE RX_TYPE_PPM +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define RX_CHANNELS_TAER #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_VBAT | FEATURE_CURRENT_METER | FEATURE_OSD) diff --git a/src/main/target/LUX_RACE/target.h b/src/main/target/LUX_RACE/target.h index 5c64316a83..45acda286a 100644 --- a/src/main/target/LUX_RACE/target.h +++ b/src/main/target/LUX_RACE/target.h @@ -94,7 +94,7 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE -#define DEFAULT_RX_TYPE RX_TYPE_PPM +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_VBAT) // Number of available PWM outputs diff --git a/src/main/target/MAMBAF405US/target.h b/src/main/target/MAMBAF405US/target.h index adb4c09f1e..d0cbc97389 100644 --- a/src/main/target/MAMBAF405US/target.h +++ b/src/main/target/MAMBAF405US/target.h @@ -57,7 +57,7 @@ #define I2C2_SDA PB9 #define DEFAULT_I2C_BUS BUS_I2C1 -#else +#else #define USE_I2C_DEVICE_2 #define I2C2_SCL PB10 // SCL pad TX3 @@ -158,7 +158,6 @@ #define VBAT_SCALE_DEFAULT 1100 // ******* OSD ******** -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PB12 @@ -187,16 +186,15 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(2)) -#define MAX_PWM_OUTPUT_PORTS 8 -#define TARGET_MOTOR_COUNT 4 +#define MAX_PWM_OUTPUT_PORTS 4 +#define TARGET_MOTOR_COUNT 4 // ESC-related features #define USE_DSHOT -#define USE_SERIALSHOT #define USE_ESC_SENSOR #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS #define PITOT_I2C_BUS DEFAULT_I2C_BUS #define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS -#define BNO055_I2C_BUS DEFAULT_I2C_BUS \ No newline at end of file +#define BNO055_I2C_BUS DEFAULT_I2C_BUS diff --git a/src/main/target/MAMBAF722/target.h b/src/main/target/MAMBAF722/target.h index 2c1cb07cf7..5f4b9035ae 100644 --- a/src/main/target/MAMBAF722/target.h +++ b/src/main/target/MAMBAF722/target.h @@ -156,7 +156,6 @@ #define VBAT_SCALE_DEFAULT 1100 // ******* OSD ******** -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN SPI2_NSS_PIN @@ -194,7 +193,6 @@ // ESC-related features #define USE_DSHOT -#define USE_SERIALSHOT #define USE_ESC_SENSOR #define USE_SERIAL_4WAY_BLHELI_INTERFACE @@ -207,4 +205,5 @@ #define BNO055_I2C_BUS DEFAULT_I2C_BUS -#endif \ No newline at end of file +#endif + diff --git a/src/main/target/MATEKF405/target.h b/src/main/target/MATEKF405/target.h index a70cc55583..fc5b7f09fe 100644 --- a/src/main/target/MATEKF405/target.h +++ b/src/main/target/MATEKF405/target.h @@ -76,7 +76,6 @@ #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PB10 @@ -166,7 +165,6 @@ #define USE_RANGEFINDER #define USE_RANGEFINDER_MSP -#define USE_RANGEFINDER_HCSR04_I2C #define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS #define PITOT_I2C_BUS DEFAULT_I2C_BUS @@ -200,7 +198,6 @@ #define USE_DSHOT #define USE_ESC_SENSOR -#define USE_SERIALSHOT #define MAX_PWM_OUTPUT_PORTS 6 diff --git a/src/main/target/MATEKF405CAN/config.c b/src/main/target/MATEKF405CAN/config.c index 34166b5512..b400704b00 100644 --- a/src/main/target/MATEKF405CAN/config.c +++ b/src/main/target/MATEKF405CAN/config.c @@ -21,6 +21,7 @@ #include "config/feature.h" #include "io/serial.h" #include "sensors/compass.h" +#include "fc/config.h" void targetConfiguration(void) @@ -28,5 +29,5 @@ void targetConfiguration(void) compassConfigMutable()->mag_align = CW0_DEG_FLIP; serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].functionMask = FUNCTION_GPS; // serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].gps_baudrateIndex = BAUD_115200; - + beeperConfigMutable()->pwmMode = true; } diff --git a/src/main/target/MATEKF405CAN/target.h b/src/main/target/MATEKF405CAN/target.h index b9dde1e434..f9fc403795 100644 --- a/src/main/target/MATEKF405CAN/target.h +++ b/src/main/target/MATEKF405CAN/target.h @@ -25,7 +25,6 @@ #define BEEPER PA8 #define BEEPER_INVERTED -#define BEEPER_PWM #define BEEPER_PWM_FREQUENCY 2500 // *************** SPI1 Gyro & ACC ******************* @@ -69,7 +68,6 @@ #define USE_MAG_LIS3MDL #define USE_RANGEFINDER -#define USE_RANGEFINDER_HCSR04_I2C #define RANGEFINDER_I2C_BUS BUS_I2C2 #define PITOT_I2C_BUS BUS_I2C2 diff --git a/src/main/target/MATEKF405SE/target.h b/src/main/target/MATEKF405SE/target.h index 75afdea322..d9e07b0384 100644 --- a/src/main/target/MATEKF405SE/target.h +++ b/src/main/target/MATEKF405SE/target.h @@ -71,7 +71,6 @@ #define USE_MAG_LIS3MDL #define USE_RANGEFINDER -#define USE_RANGEFINDER_HCSR04_I2C #define USE_RANGEFINDER_US42 #define RANGEFINDER_I2C_BUS BUS_I2C2 #define PITOT_I2C_BUS BUS_I2C2 @@ -85,7 +84,6 @@ #define SPI2_MISO_PIN PC2 #define SPI2_MOSI_PIN PC3 -#define USE_OSD #define USE_MAX7456 #define MAX7456_CS_PIN PB12 #define MAX7456_SPI_BUS BUS_SPI2 diff --git a/src/main/target/MATEKF411/target.h b/src/main/target/MATEKF411/target.h index ac6962a350..124dcbd0a2 100755 --- a/src/main/target/MATEKF411/target.h +++ b/src/main/target/MATEKF411/target.h @@ -55,7 +55,6 @@ #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PB12 @@ -161,7 +160,6 @@ #define USE_DSHOT #define USE_ESC_SENSOR -#define USE_SERIALSHOT #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define TARGET_IO_PORTA 0xffff diff --git a/src/main/target/MATEKF411SE/target.h b/src/main/target/MATEKF411SE/target.h index 2da0393f54..1a86e8bda5 100755 --- a/src/main/target/MATEKF411SE/target.h +++ b/src/main/target/MATEKF411SE/target.h @@ -50,7 +50,6 @@ #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PB12 @@ -122,7 +121,6 @@ #define USE_RANGEFINDER #define USE_RANGEFINDER_MSP -#define USE_RANGEFINDER_HCSR04_I2C #define RANGEFINDER_I2C_BUS BUS_I2C1 // *************** ADC ***************************** @@ -164,7 +162,6 @@ #define USE_DSHOT #define USE_ESC_SENSOR -#define USE_SERIALSHOT #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define TARGET_IO_PORTA 0xffff diff --git a/src/main/target/MATEKF722/target.h b/src/main/target/MATEKF722/target.h index 331ad65821..47f0d3508c 100755 --- a/src/main/target/MATEKF722/target.h +++ b/src/main/target/MATEKF722/target.h @@ -90,7 +90,6 @@ #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PB10 @@ -158,6 +157,5 @@ #define MAX_PWM_OUTPUT_PORTS 7 #define USE_DSHOT #define USE_ESC_SENSOR -#define USE_SERIALSHOT #define BNO055_I2C_BUS BUS_I2C1 diff --git a/src/main/target/MATEKF722PX/target.h b/src/main/target/MATEKF722PX/target.h index ca9dd843a6..8edf1e7755 100755 --- a/src/main/target/MATEKF722PX/target.h +++ b/src/main/target/MATEKF722PX/target.h @@ -169,7 +169,5 @@ #define TARGET_IO_PORTD 0xffff #define MAX_PWM_OUTPUT_PORTS 10 -#define USE_OSD #define USE_DSHOT -#define USE_SERIALSHOT #define USE_ESC_SENSOR diff --git a/src/main/target/MATEKF722SE/target.h b/src/main/target/MATEKF722SE/target.h index ecff68fa66..e55e379131 100644 --- a/src/main/target/MATEKF722SE/target.h +++ b/src/main/target/MATEKF722SE/target.h @@ -95,7 +95,6 @@ #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PB12 @@ -192,7 +191,7 @@ #define MAX_PWM_OUTPUT_PORTS 8 #define USE_DSHOT -#define USE_SERIALSHOT #define USE_ESC_SENSOR -#define BNO055_I2C_BUS BUS_I2C1 \ No newline at end of file +#define BNO055_I2C_BUS BUS_I2C1 + diff --git a/src/main/target/MATEKF765/CMakeLists.txt b/src/main/target/MATEKF765/CMakeLists.txt index 2461647ec0..0efbb358d6 100644 --- a/src/main/target/MATEKF765/CMakeLists.txt +++ b/src/main/target/MATEKF765/CMakeLists.txt @@ -1 +1,2 @@ target_stm32f765xi(MATEKF765) +target_stm32f765xi(MATEKF765SE) diff --git a/src/main/target/MATEKF765/config.c b/src/main/target/MATEKF765/config.c index 7e47e5bf6b..9ec186282f 100644 --- a/src/main/target/MATEKF765/config.c +++ b/src/main/target/MATEKF765/config.c @@ -20,11 +20,15 @@ #include "platform.h" #include "fc/fc_msp_box.h" - +#include "fc/config.h" #include "io/piniobox.h" void targetConfiguration(void) { pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; + + #if defined(MATEKF765SE) + beeperConfigMutable()->pwmMode = true; + #endif } diff --git a/src/main/target/MATEKF765/target.c b/src/main/target/MATEKF765/target.c index af157cad89..e4728c42ca 100644 --- a/src/main/target/MATEKF765/target.c +++ b/src/main/target/MATEKF765/target.c @@ -29,6 +29,10 @@ BUSDEV_REGISTER_SPI_TAG(busdev_imu0, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU6000_CS_PIN, MPU6000_EXTI_PIN, 0, DEVFLAGS_NONE, IMU_MPU6000_ALIGN); BUSDEV_REGISTER_SPI_TAG(busdev_imu1, DEVHW_MPU6500, MPU6500_SPI_BUS, MPU6500_CS_PIN, MPU6500_EXTI_PIN, 1, DEVFLAGS_NONE, IMU_MPU6500_ALIGN); +#if defined(MATEKF765SE) +BUSDEV_REGISTER_SPI_TAG(busdev_imu3, DEVHW_ICM42605, ICM42605_SPI_BUS, ICM42605_CS_PIN, ICM42605_EXTI_PIN, 0, DEVFLAGS_NONE, IMU_ICM42605_ALIGN); +#endif + const timerHardware_t timerHardware[] = { DEF_TIM(TIM2, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 UP(1,7), D(1,5,3) DEF_TIM(TIM2, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 UP(1,7), D(1,6,3) @@ -50,6 +54,8 @@ const timerHardware_t timerHardware[] = { DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM, 0, 0), // RX6 PPM DEF_TIM(TIM8, CH1, PC6, TIM_USE_ANY, 0, 0), // TX6 + + DEF_TIM(TIM11, CH1, PB9, TIM_USE_BEEPER, 0, 0), // BEEPER PWM for MATEKF765SE }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/MATEKF765/target.h b/src/main/target/MATEKF765/target.h index 8f5b256026..ea3414f2c6 100644 --- a/src/main/target/MATEKF765/target.h +++ b/src/main/target/MATEKF765/target.h @@ -21,8 +21,15 @@ #define USE_TARGET_IMU_HARDWARE_DESCRIPTORS #define USE_TARGET_CONFIG -#define TARGET_BOARD_IDENTIFIER "M765" -#define USBD_PRODUCT_STRING "MATEKF765" +#if defined(MATEKF765SE) + #define TARGET_BOARD_IDENTIFIER "M7SE" + #define USBD_PRODUCT_STRING "MATEKF765SE" + + #define BEEPER_PWM_FREQUENCY 2500 +#else + #define TARGET_BOARD_IDENTIFIER "M765" + #define USBD_PRODUCT_STRING "MATEKF765" +#endif #define LED0 PD10 #define LED1 PD11 @@ -56,6 +63,13 @@ #define MPU6500_CS_PIN PD7 #define MPU6500_EXTI_PIN PD4 +#if defined(MATEKF765SE) +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW0_DEG_FLIP +#define ICM42605_SPI_BUS BUS_SPI4 +#define ICM42605_CS_PIN PE11 +#define ICM42605_EXTI_PIN PC13 +#endif #define USE_EXTI #define USE_MPU_DATA_READY_SIGNAL @@ -98,18 +112,15 @@ #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PB12 // *************** SPI4 ****************************** -/* #define USE_SPI_DEVICE_4 #define SPI4_SCK_PIN PE12 #define SPI4_MISO_PIN PE13 #define SPI4_MOSI_PIN PE14 -*/ // *************** UART ***************************** #define USE_VCP @@ -128,9 +139,10 @@ #define UART3_TX_PIN PD8 #define UART3_RX_PIN PD9 -#define USE_UART4 -#define UART4_TX_PIN PD1 -#define UART4_RX_PIN PD0 +#define USE_UART5 +#define UART5_TX_PIN NONE +#define UART5_RX_PIN PB8 +#define UART5_AF 7 #define USE_UART6 #define UART6_TX_PIN PC6 @@ -149,8 +161,15 @@ #define SOFTSERIAL_1_TX_PIN PC6 //TX6 pad #define SOFTSERIAL_1_RX_PIN PC6 //TX6 pad - -#define SERIAL_PORT_COUNT 9 +#if defined(MATEKF765SE) + #define SERIAL_PORT_COUNT 9 + // PD1 and PD0 are used for CAN +#else + #define USE_UART4 + #define UART4_TX_PIN PD1 + #define UART4_RX_PIN PD0 + #define SERIAL_PORT_COUNT 10 +#endif #define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS @@ -190,7 +209,14 @@ #define WS2811_PIN PA8 #define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) -#define CURRENT_METER_SCALE 250 + +#if defined(MATEKF765SE) + #define VBAT_SCALE_DEFAULT 2100 + #define CURRENT_METER_SCALE 150 +#else + #define VBAT_SCALE_DEFAULT 1100 + #define CURRENT_METER_SCALE 250 +#endif #define USE_SERIAL_4WAY_BLHELI_INTERFACE @@ -200,7 +226,6 @@ #define TARGET_IO_PORTD 0xffff #define TARGET_IO_PORTE 0xffff -#define MAX_PWM_OUTPUT_PORTS 15 +#define MAX_PWM_OUTPUT_PORTS 16 #define USE_DSHOT #define USE_ESC_SENSOR -#define USE_SERIALSHOT diff --git a/src/main/target/MATEKH743/CMakeLists.txt b/src/main/target/MATEKH743/CMakeLists.txt index 40b0996e17..96a65ca5a4 100644 --- a/src/main/target/MATEKH743/CMakeLists.txt +++ b/src/main/target/MATEKH743/CMakeLists.txt @@ -1 +1 @@ -target_stm32h743xi(MATEKH743 SKIP_RELEASES) +target_stm32h743xi(MATEKH743) diff --git a/src/main/drivers/rangefinder/rangefinder_hcsr04.h b/src/main/target/MATEKH743/config.c similarity index 67% rename from src/main/drivers/rangefinder/rangefinder_hcsr04.h rename to src/main/target/MATEKH743/config.c index a5647d81fd..1065971614 100644 --- a/src/main/drivers/rangefinder/rangefinder_hcsr04.h +++ b/src/main/target/MATEKH743/config.c @@ -15,10 +15,18 @@ * along with Cleanflight. If not, see . */ -#pragma once +#include -#include "drivers/rangefinder/rangefinder.h" +#include "platform.h" -#define RANGEFINDER_HCSR04_TASK_PERIOD_MS 70 +#include "fc/fc_msp_box.h" +#include "fc/config.h" -bool hcsr04Detect(rangefinderDev_t *dev, const rangefinderHardwarePins_t * rangefinderHardwarePins); +#include "io/piniobox.h" + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; + beeperConfigMutable()->pwmMode = true; +} diff --git a/src/main/target/MATEKH743/target.c b/src/main/target/MATEKH743/target.c index 94b2ff7b75..e2c31e2592 100644 --- a/src/main/target/MATEKH743/target.c +++ b/src/main/target/MATEKH743/target.c @@ -26,22 +26,23 @@ #include "drivers/pinio.h" #include "drivers/sensor.h" -BUSDEV_REGISTER_SPI_TAG(busdev_imu1_6000, DEVHW_MPU6000, IMU1_SPI_BUS, IMU1_CS_PIN, IMU1_EXTI_PIN, 0, DEVFLAGS_NONE, IMU1_ALIGN); -BUSDEV_REGISTER_SPI_TAG(busdev_imu2_6500, DEVHW_MPU6500, IMU2_SPI_BUS, IMU2_CS_PIN, IMU2_EXTI_PIN, 1, DEVFLAGS_NONE, IMU2_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_imu0, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU6000_CS_PIN, MPU6000_EXTI_PIN, 0, DEVFLAGS_NONE, IMU_MPU6000_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_imu1, DEVHW_MPU6500, MPU6500_SPI_BUS, MPU6500_CS_PIN, MPU6500_EXTI_PIN, 1, DEVFLAGS_NONE, IMU_MPU6500_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_imu2, DEVHW_ICM42605, ICM42605_SPI_BUS, ICM42605_CS_PIN, ICM42605_EXTI_PIN, 2, DEVFLAGS_NONE, IMU_ICM42605_ALIGN); const timerHardware_t timerHardware[] = { - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S1 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 2), // S2 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S2 - DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 3), // S3 - DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 4), // S4 - DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 5), // S5 - DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 6), // S6 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 2), // S3 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 3), // S4 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 4), // S5 + DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 5), // S6 - DEF_TIM(TIM4, CH1, PD12, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 7), // S7 - DEF_TIM(TIM4, CH2, PD13, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 8), // S8 - DEF_TIM(TIM4, CH3, PD14, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S9 - DEF_TIM(TIM4, CH4, PD15, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S10 DMA_NONE + DEF_TIM(TIM4, CH1, PD12, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 6), // S7 + DEF_TIM(TIM4, CH2, PD13, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 7), // S8 + DEF_TIM(TIM4, CH3, PD14, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S9 + DEF_TIM(TIM4, CH4, PD15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S10 DMA_NONE DEF_TIM(TIM15, CH1, PE5, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S11 DEF_TIM(TIM15, CH2, PE6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S12 DMA_NONE diff --git a/src/main/target/MATEKH743/target.h b/src/main/target/MATEKH743/target.h index ca99c9ddcc..9234a09c5a 100644 --- a/src/main/target/MATEKH743/target.h +++ b/src/main/target/MATEKH743/target.h @@ -21,48 +21,14 @@ #define TARGET_BOARD_IDENTIFIER "H743" #define USBD_PRODUCT_STRING "MATEKH743" +#define USE_TARGET_CONFIG + #define LED0 PE3 #define LED1 PE4 #define BEEPER PA15 #define BEEPER_INVERTED - -// *************** UART ***************************** -#define USE_VCP - -#define USE_UART1 -#define UART1_TX_PIN PA9 -#define UART1_RX_PIN PA10 - -#define USE_UART2 -#define UART2_TX_PIN PD5 -#define UART2_RX_PIN PD6 - -#define USE_UART3 -#define UART3_TX_PIN PD8 -#define UART3_RX_PIN PD9 - -#define USE_UART4 -#define UART4_TX_PIN PD1 -#define UART4_RX_PIN PD0 - -#define USE_UART6 -#define UART6_TX_PIN PC6 -#define UART6_RX_PIN PC7 - -#define USE_UART7 -#define UART7_TX_PIN PE8 -#define UART7_RX_PIN PE7 - -#define USE_UART8 -#define UART8_TX_PIN PE1 -#define UART8_RX_PIN PE0 - -#define SERIAL_PORT_COUNT 8 - -#define DEFAULT_RX_TYPE RX_TYPE_SERIAL -#define SERIALRX_PROVIDER SERIALRX_SBUS -#define SERIALRX_UART SERIAL_PORT_USART6 +#define BEEPER_PWM_FREQUENCY 2500 // *************** IMU generic *********************** #define USE_DUAL_GYRO @@ -71,7 +37,7 @@ #define USE_EXTI #define USE_MPU_DATA_READY_SIGNAL -// *************** SPI1 IMU1 ************************* +// *************** SPI1 IMU0 MPU6000 **************** #define USE_SPI #define USE_SPI_DEVICE_1 #define SPI1_SCK_PIN PA5 @@ -79,12 +45,33 @@ #define SPI1_MOSI_PIN PD7 #define USE_IMU_MPU6000 + +#define IMU_MPU6000_ALIGN CW0_DEG_FLIP +#define MPU6000_SPI_BUS BUS_SPI1 +#define MPU6000_CS_PIN PC15 +#define MPU6000_EXTI_PIN PB2 + +// *************** SPI4 IMU1 ICM20602 ************** +#define USE_SPI_DEVICE_4 +#define SPI4_SCK_PIN PE12 +#define SPI4_MISO_PIN PE13 +#define SPI4_MOSI_PIN PE14 + #define USE_IMU_MPU6500 -#define IMU1_ALIGN CW90_DEG_FLIP -#define IMU1_SPI_BUS BUS_SPI1 -#define IMU1_CS_PIN PC15 -#define IMU1_EXTI_PIN NONE +#define IMU_MPU6500_ALIGN CW0_DEG_FLIP +#define MPU6500_SPI_BUS BUS_SPI4 +#define MPU6500_CS_PIN PE11 +#define MPU6500_EXTI_PIN PE15 + +// *************** SPI4 IMU2 ICM42605 ************** +#define USE_IMU_ICM42605 + +#define IMU_ICM42605_ALIGN CW90_DEG_FLIP +#define ICM42605_SPI_BUS BUS_SPI4 +#define ICM42605_CS_PIN PC13 +#define ICM42605_EXTI_PIN PC14 + // *************** SPI2 OSD *********************** #define USE_SPI_DEVICE_2 @@ -92,30 +79,20 @@ #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PB12 -// *************** SPI3 SD BLACKBOX******************* -/* -#define USE_SDCARD -#define USE_SDCARD_SDIO -#define SDCARD_SDIO_DMA DMA_TAG(2,3,4) -#define SDCARD_SDIO_4BIT -#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT -*/ +// *************** SPI3 SPARE for external RM3100 *********** +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 -// *************** SPI4 IMU2 ************************* -#define USE_SPI_DEVICE_4 -#define SPI4_SCK_PIN PE12 -#define SPI4_MISO_PIN PE13 -#define SPI4_MOSI_PIN PE14 - -#define IMU2_ALIGN CW270_DEG_FLIP -#define IMU2_SPI_BUS BUS_SPI4 -#define IMU2_CS_PIN PE11 -#define IMU2_EXTI_PIN NONE +#define USE_MAG_RM3100 +#define RM3100_CS_PIN PE2 //CS2 pad +// PD4 //CS1 pad +#define RM3100_SPI_BUS BUS_SPI3 // *************** I2C /Baro/Mag ********************* #define USE_I2C @@ -147,7 +124,55 @@ #define PITOT_I2C_BUS BUS_I2C2 #define USE_RANGEFINDER -#define RANGEFINDER_I2C_BUS BUS_I2C2 +#define RANGEFINDER_I2C_BUS BUS_I2C1 + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PD5 +#define UART2_RX_PIN PD6 + +#define USE_UART3 +#define UART3_TX_PIN PD8 +#define UART3_RX_PIN PD9 + +#define USE_UART4 +#define UART4_TX_PIN PB9 +#define UART4_RX_PIN PB8 + +#define USE_UART6 +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define USE_UART7 +#define UART7_TX_PIN PE8 +#define UART7_RX_PIN PE7 + +#define USE_UART8 +#define UART8_TX_PIN PE1 +#define UART8_RX_PIN PE0 + +#define USE_SOFTSERIAL1 +#define SOFTSERIAL_1_TX_PIN PC6 //TX6 pad +#define SOFTSERIAL_1_RX_PIN PC6 //TX6 pad + +#define SERIAL_PORT_COUNT 9 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART6 + +// *************** SDIO SD BLACKBOX******************* +//#define USE_SDCARD +//#define USE_SDCARD_SDIO +//#define SDCARD_SDIO_DMA DMA_TAG(2,3,4) +//#define SDCARD_SDIO_4BIT +//#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT // *************** ADC ***************************** #define USE_ADC @@ -168,8 +193,8 @@ // *************** PINIO *************************** #define USE_PINIO #define USE_PINIOBOX -#define PINIO1_PIN PE4 // VTX power switcher -#define PINIO2_PIN PE15 // 2xCamera switcher +#define PINIO1_PIN PD10 // VTX power switcher +#define PINIO2_PIN PD11 // 2xCamera switcher // *************** LEDSTRIP ************************ #define USE_LED_STRIP @@ -189,4 +214,4 @@ #define MAX_PWM_OUTPUT_PORTS 15 #define USE_DSHOT #define USE_ESC_SENSOR -#define USE_SERIALSHOT \ No newline at end of file + diff --git a/src/main/target/NOX/target.h b/src/main/target/NOX/target.h index 9a40ea5050..764e889bb4 100755 --- a/src/main/target/NOX/target.h +++ b/src/main/target/NOX/target.h @@ -61,7 +61,6 @@ #define BMP280_CS_PIN PA9 // *************** SPI OSD ***************************** -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PA10 diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index 3338db998e..19a5c9aaad 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -103,7 +103,6 @@ #define SDCARD_SPI_BUS BUS_SPI2 #define SDCARD_CS_PIN SPI2_NSS_PIN -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI1 #define MAX7456_CS_PIN PB1 @@ -124,7 +123,7 @@ //#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT -#define DEFAULT_RX_TYPE RX_TYPE_PPM +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_VBAT | FEATURE_CURRENT_METER | FEATURE_BLACKBOX | FEATURE_OSD) #define BUTTONS diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 4027144c8d..96c0de6244 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -213,7 +213,6 @@ #define SPI3_MISO_PIN PC11 #define SPI3_MOSI_PIN PC12 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN PA15 @@ -259,7 +258,7 @@ #define WS2811_PIN PA1 #endif -#define DEFAULT_RX_TYPE RX_TYPE_PPM +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define DISABLE_RX_PWM_FEATURE #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX | FEATURE_VBAT | FEATURE_OSD) diff --git a/src/main/target/OMNIBUSF7/target.h b/src/main/target/OMNIBUSF7/target.h index 50e10578b4..65f1fb364d 100644 --- a/src/main/target/OMNIBUSF7/target.h +++ b/src/main/target/OMNIBUSF7/target.h @@ -73,7 +73,6 @@ #define UART1_RX_PIN PA10 #define UART1_TX_PIN PA9 -//#define AVOID_UART2_FOR_PWM_PPM #define USE_UART2 #define UART2_TX_PIN PA2 //not wired #define UART2_RX_PIN PA3 @@ -126,7 +125,6 @@ #define SPI4_MISO_PIN PE5 #define SPI4_MOSI_PIN PE6 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN SPI2_NSS_PIN @@ -160,7 +158,6 @@ #define BNO055_I2C_BUS BUS_I2C2 #define USE_RANGEFINDER -#define USE_RANGEFINDER_HCSR04_I2C #define RANGEFINDER_I2C_BUS BUS_I2C2 #define SENSORS_SET (SENSOR_ACC | SENSOR_BARO) diff --git a/src/main/target/OMNIBUSF7NXT/target.h b/src/main/target/OMNIBUSF7NXT/target.h index 241322b5d6..4396302717 100644 --- a/src/main/target/OMNIBUSF7NXT/target.h +++ b/src/main/target/OMNIBUSF7NXT/target.h @@ -128,7 +128,6 @@ #define SPI3_MISO_PIN PC11 #define SPI3_MOSI_PIN PC12 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PA15 @@ -170,7 +169,6 @@ #define USE_DSHOT #define USE_ESC_SENSOR -#define USE_SERIALSHOT // Number of available PWM outputs #define MAX_PWM_OUTPUT_PORTS 6 diff --git a/src/main/target/PIXRACER/target.h b/src/main/target/PIXRACER/target.h index 96fcd68237..e700d09aa6 100755 --- a/src/main/target/PIXRACER/target.h +++ b/src/main/target/PIXRACER/target.h @@ -154,7 +154,7 @@ #define SERIALRX_PROVIDER SERIALRX_SBUS #define SERIALRX_UART SERIAL_PORT_USART6 -#define DEFAULT_RX_TYPE RX_TYPE_PPM +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/RCEXPLORERF3/target.h b/src/main/target/RCEXPLORERF3/target.h index 738ed44638..18e6719435 100644 --- a/src/main/target/RCEXPLORERF3/target.h +++ b/src/main/target/RCEXPLORERF3/target.h @@ -90,9 +90,6 @@ #define WS2811_PIN PB8 // TIM16_CH1 #define USE_RANGEFINDER -#define USE_RANGEFINDER_HCSR04 -#define RANGEFINDER_HCSR04_TRIGGER_PIN PA6 // RC_CH7 (PB0) - only 3.3v ( add a 1K Ohms resistor ) -#define RANGEFINDER_HCSR04_ECHO_PIN PB1 // RC_CH8 (PB1) - only 3.3v ( add a 1K Ohms resistor ) #define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_GPS | SENSOR_MAG) diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index f8a3c7264c..4f4befce66 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -31,7 +31,6 @@ #define USE_DSHOT #define USE_ESC_SENSOR -#define USE_SERIALSHOT // MPU6000 interrupts #define USE_EXTI diff --git a/src/main/target/RUSH_BLADE_F7/target.h b/src/main/target/RUSH_BLADE_F7/target.h index cdae1860b3..fb9c522642 100644 --- a/src/main/target/RUSH_BLADE_F7/target.h +++ b/src/main/target/RUSH_BLADE_F7/target.h @@ -141,7 +141,5 @@ #define TARGET_IO_PORTD 0xffff #define MAX_PWM_OUTPUT_PORTS 10 -#define USE_OSD #define USE_DSHOT -#define USE_SERIALSHOT #define USE_ESC_SENSOR diff --git a/src/main/target/SKYSTARSF405HD/CMakeLists.txt b/src/main/target/SKYSTARSF405HD/CMakeLists.txt new file mode 100644 index 0000000000..cc9515d9e0 --- /dev/null +++ b/src/main/target/SKYSTARSF405HD/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f405xg(SKYSTARSF405HD) diff --git a/src/main/target/SKYSTARSF405HD/config.c b/src/main/target/SKYSTARSF405HD/config.c new file mode 100644 index 0000000000..6dd129e144 --- /dev/null +++ b/src/main/target/SKYSTARSF405HD/config.c @@ -0,0 +1,29 @@ +/* + * This file is part of iNav. + * + * iNav is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * iNav is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with iNav. If not, see . + */ + +#include +#include + +#include + +#include "io/serial.h" +#include "rx/rx.h" + +void targetConfiguration(void) +{ + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART6)].functionMask = FUNCTION_DJI_HD_OSD; +} diff --git a/src/main/target/SKYSTARSF405HD/target.c b/src/main/target/SKYSTARSF405HD/target.c new file mode 100644 index 0000000000..ecaeed2621 --- /dev/null +++ b/src/main/target/SKYSTARSF405HD/target.c @@ -0,0 +1,35 @@ +/* + * This file is part of iNav. + * + * iNav is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * iNav is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with iNav. If not, see . + */ + +#include +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" + +const timerHardware_t timerHardware[] = { + DEF_TIM(TIM1, CH1, PA8, TIM_USE_PPM, 0, 0), + + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), + DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), + + DEF_TIM(TIM8, CH3, PC8, TIM_USE_LED, 0, 0), +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/SKYSTARSF405HD/target.h b/src/main/target/SKYSTARSF405HD/target.h new file mode 100644 index 0000000000..544a001487 --- /dev/null +++ b/src/main/target/SKYSTARSF405HD/target.h @@ -0,0 +1,164 @@ +/* + * 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 . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "SS4D" + +#define USBD_PRODUCT_STRING "SkystarsF405HD" + +#define LED0 PC14 // green +#define LED1 PC15 // blue + +#define BEEPER PC13 +#define BEEPER_INVERTED + +// *************** Gyro & ACC ********************** +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_BUS BUS_SPI1 + +#define USE_EXTI +#define GYRO_INT_EXTI PC4 +#define USE_MPU_DATA_READY_SIGNAL + +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW180_DEG_FLIP + +// *************** M25P256 flash ******************** +#define USE_FLASHFS +#define USE_FLASH_M25P16 + +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PB5 + +#define M25P16_SPI_BUS BUS_SPI3 +#define M25P16_CS_PIN PA15 + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +// *************** OSD & baro *********************** +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN PB12 + +#define USE_BARO +#define USE_BARO_BMP280 +#define BMP280_SPI_BUS BUS_SPI2 +#define BMP280_CS_PIN PC5 + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART5 +#define UART5_RX_PIN PD2 +#define UART5_TX_PIN PC12 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define SERIAL_PORT_COUNT 7 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART1 + +// *************** I2C **************************** +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define DEFAULT_I2C_BUS BUS_I2C1 + +#define USE_MAG +#define MAG_I2C_BUS DEFAULT_I2C_BUS +#define USE_MAG_AK8963 +#define USE_MAG_AK8975 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_IST8308 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL + +#define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS + +#define BNO055_I2C_BUS DEFAULT_I2C_BUS + +#define USE_RANGEFINDER +#define USE_RANGEFINDER_MSP +#define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS + +#define PITOT_I2C_BUS DEFAULT_I2C_BUS +#define PCA9685_I2C_BUS DEFAULT_I2C_BUS + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_CHANNEL_1_PIN PC0 +#define ADC_CHANNEL_2_PIN PC1 +#define ADC_CHANNEL_3_PIN PC2 +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY ) + +#define USE_LED_STRIP +#define WS2811_PIN PC8 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff + +#define USE_DSHOT +#define USE_ESC_SENSOR + +#define MAX_PWM_OUTPUT_PORTS 4 diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index 5052027dc4..f119da56b1 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -55,7 +55,6 @@ #define USE_UART2 // Input - RX (PA3) #define USE_UART3 // Servo out - 10/RX (PB11) 11/TX (PB10) #define SERIAL_PORT_COUNT 4 -#define AVOID_UART2_FOR_PWM_PPM #define UART1_TX_PIN PB6 #define UART1_RX_PIN PB7 @@ -92,11 +91,9 @@ #define BIND_PIN PA3 // #define USE_RANGEFINDER -// #define USE_RANGEFINDER_HCSR04 -// #define RANGEFINDER_HCSR04_TRIGGER_PIN PA2 // PWM6 (PA2) - only 3.3v ( add a 1K Ohms resistor ) -// #define RANGEFINDER_HCSR04_ECHO_PIN PB1 // PWM7 (PB1) - only 3.3v ( add a 1K Ohms resistor ) -#define DEFAULT_RX_TYPE RX_TYPE_PPM + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL // Number of available PWM outputs #define MAX_PWM_OUTPUT_PORTS 10 diff --git a/src/main/target/SPEEDYBEEF4/target.h b/src/main/target/SPEEDYBEEF4/target.h index 3920653df7..0258ac2974 100644 --- a/src/main/target/SPEEDYBEEF4/target.h +++ b/src/main/target/SPEEDYBEEF4/target.h @@ -73,7 +73,6 @@ #define USE_FLASH_M25P16 /*** OSD ***/ -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PB10 @@ -171,4 +170,5 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(2)) -#define RANGEFINDER_I2C_BUS BUS_I2C1 \ No newline at end of file +#define RANGEFINDER_I2C_BUS BUS_I2C1 + diff --git a/src/main/target/SPEEDYBEEF7/target.h b/src/main/target/SPEEDYBEEF7/target.h index 71e855af0a..559ad0924e 100644 --- a/src/main/target/SPEEDYBEEF7/target.h +++ b/src/main/target/SPEEDYBEEF7/target.h @@ -73,7 +73,6 @@ // OSD -- SPI2 #define USE_SPI_DEVICE_2 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PB12 diff --git a/src/main/target/SPEEDYBEEF7V2/CMakeLists.txt b/src/main/target/SPEEDYBEEF7V2/CMakeLists.txt new file mode 100644 index 0000000000..f16c68178d --- /dev/null +++ b/src/main/target/SPEEDYBEEF7V2/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f722xe(SPEEDYBEEF7V2) diff --git a/src/main/target/SPEEDYBEEF7V2/config.c b/src/main/target/SPEEDYBEEF7V2/config.c new file mode 100644 index 0000000000..dccd2e8a04 --- /dev/null +++ b/src/main/target/SPEEDYBEEF7V2/config.c @@ -0,0 +1,35 @@ +/* + * 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 . + */ + + +#include +#include + +#include "platform.h" + +#include "fc/fc_msp_box.h" + +#include "io/piniobox.h" +#include "io/serial.h" + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; +} diff --git a/src/main/target/SPEEDYBEEF7V2/target.c b/src/main/target/SPEEDYBEEF7V2/target.c new file mode 100644 index 0000000000..46bd5d8932 --- /dev/null +++ b/src/main/target/SPEEDYBEEF7V2/target.c @@ -0,0 +1,42 @@ +/* + * 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 . + */ + +#include + +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/bus.h" + +const timerHardware_t timerHardware[] = { + DEF_TIM(TIM4, CH2, PB7, TIM_USE_PPM, 0, 0), // PPM + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S1 + DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S2 + DEF_TIM(TIM1, CH3, PA10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S3 + DEF_TIM(TIM3, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S4 + + DEF_TIM(TIM8, CH2N, PB0, TIM_USE_LED, 0, 0), // LED + + DEF_TIM(TIM5, CH1, PA0, TIM_USE_ANY, 0, 0), // Camera Control +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); \ No newline at end of file diff --git a/src/main/target/SPEEDYBEEF7V2/target.h b/src/main/target/SPEEDYBEEF7V2/target.h new file mode 100644 index 0000000000..0e5f5d190a --- /dev/null +++ b/src/main/target/SPEEDYBEEF7V2/target.h @@ -0,0 +1,163 @@ +/* + * 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 . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "SB72" +#define USBD_PRODUCT_STRING "SpeedyBeeF7V2" +#define USE_TARGET_CONFIG + +#define LED0 PC13 + +// *************** UART ***************************** +#define USB_IO +#define USE_VCP +#define VBUS_SENSING_PIN PC15 +#define VBUS_SENSING_ENABLED + +#define USE_UART1 +#define UART1_RX_PIN PB7 +#define UART1_TX_PIN PB6 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PC11 +#define UART3_TX_PIN PC10 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART5 +#define UART5_RX_PIN PD2 +#define UART5_TX_PIN PC12 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define SERIAL_PORT_COUNT 7 + +// *************** Gyro & ACC ********************** +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW0_DEG + +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_BUS BUS_SPI1 + +#define USE_EXTI +#define GYRO_INT_EXTI PC4 +#define USE_MPU_DATA_READY_SIGNAL + +#define USE_BEEPER +#define BEEPER PB2 +#define BEEPER_INVERTED + +// *************** I2C(Baro & I2C) ************************** +#define USE_I2C + +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define USE_I2C_DEVICE_2 +#define I2C2_SCL PB10 +#define I2C2_SDA PB11 + +// Baro +#define USE_BARO +#define USE_BARO_BMP280 +#define USE_BARO_DPS310 +#define BARO_I2C_BUS BUS_I2C1 + +// Mag +#define USE_MAG +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_LIS3MDL +#define MAG_I2C_BUS BUS_I2C2 + +// *************** Flash ************************** +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_CS_PIN PA14 +#define M25P16_SPI_BUS BUS_SPI3 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +// *************** OSD ***************************** +#define USE_OSD +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN PB12 + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_CHANNEL_1_PIN PC2 +#define ADC_CHANNEL_2_PIN PC1 +#define ADC_CHANNEL_3_PIN PC3 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 + + +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX) +#define USE_DSHOT +#define USE_ESC_SENSOR +#define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define USE_SERIALSHOT + +#define SERIALRX_UART SERIAL_PORT_USART1 +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS + +#define MAX_PWM_OUTPUT_PORTS 4 + +#define CURRENT_METER_SCALE 102 + +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PA15 +#define PINIO1_FLAGS PINIO_FLAGS_INVERTED + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index bab9f28842..3d907a99b2 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -96,16 +96,12 @@ #define WS2811_PIN PA8 #define USE_RANGEFINDER -#define USE_RANGEFINDER_HCSR04 -#define RANGEFINDER_HCSR04_TRIGGER_PIN PB0 -#define RANGEFINDER_HCSR04_ECHO_PIN PB1 -#define USE_RANGEFINDER_HCSR04_I2C #define RANGEFINDER_I2C_BUS BUS_I2C1 #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX | FEATURE_VBAT) -#define DEFAULT_RX_TYPE RX_TYPE_PPM +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define USE_SPEKTRUM_BIND #define BIND_PIN PB11 // UART3 diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index a667188092..ea11fcc5ea 100755 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -123,14 +123,11 @@ #define USE_RANGEFINDER #define RANGEFINDER_I2C_BUS BUS_I2C1 -#define USE_RANGEFINDER_HCSR04 -#define RANGEFINDER_HCSR04_TRIGGER_PIN PB0 -#define RANGEFINDER_HCSR04_ECHO_PIN PB1 #define USE_RANGEFINDER_SRF10 #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT -#define DEFAULT_RX_TYPE RX_TYPE_PPM +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_TRANSPONDER | FEATURE_BLACKBOX | FEATURE_RSSI_ADC | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY) #define USE_SPEKTRUM_BIND diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h index 406ee535f0..e40dbd12c1 100644 --- a/src/main/target/SPRACINGF3MINI/target.h +++ b/src/main/target/SPRACINGF3MINI/target.h @@ -56,9 +56,6 @@ #define USE_MAG_LIS3MDL #define USE_RANGEFINDER -#define USE_RANGEFINDER_HCSR04 -#define RANGEFINDER_HCSR04_ECHO_PIN PB1 -#define RANGEFINDER_HCSR04_TRIGGER_PIN PB0 #define USB_CABLE_DETECTION @@ -131,7 +128,7 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT -#define DEFAULT_RX_TYPE RX_TYPE_PPM +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) #define BUTTONS diff --git a/src/main/target/SPRACINGF4EVO/target.h b/src/main/target/SPRACINGF4EVO/target.h index efe89add1e..8722883d01 100755 --- a/src/main/target/SPRACINGF4EVO/target.h +++ b/src/main/target/SPRACINGF4EVO/target.h @@ -143,7 +143,6 @@ // PC4 - NC - Free for ADC12_IN14 / VTX CS // PC5 - NC - Free for ADC12_IN15 / VTX Enable / OSD VSYNC -//#define USE_OSD //#define USE_MAX7456 //#define USE_OSD_OVER_MSP_DISPLAYPORT diff --git a/src/main/target/SPRACINGF7DUAL/target.h b/src/main/target/SPRACINGF7DUAL/target.h index 9bbc2a7455..c41c61732a 100644 --- a/src/main/target/SPRACINGF7DUAL/target.h +++ b/src/main/target/SPRACINGF7DUAL/target.h @@ -168,8 +168,6 @@ #define CURRENT_METER_SCALE 300 -#define USE_OSD - #define USE_LED_STRIP #define WS2811_PIN PA1 diff --git a/src/main/target/YUPIF4/config.c b/src/main/target/YUPIF4/config.c index 5ac92a8358..0e82c20e93 100644 --- a/src/main/target/YUPIF4/config.c +++ b/src/main/target/YUPIF4/config.c @@ -59,4 +59,5 @@ void targetConfiguration(void) pidProfileMutable()->bank_mc.pid[PID_LEVEL].I = 10; pidProfileMutable()->bank_mc.pid[PID_LEVEL].D = 75; + beeperConfigMutable()->pwmMode = true; } diff --git a/src/main/target/YUPIF4/target.h b/src/main/target/YUPIF4/target.h index 741f224252..69c5bc3ab1 100644 --- a/src/main/target/YUPIF4/target.h +++ b/src/main/target/YUPIF4/target.h @@ -40,7 +40,6 @@ #if defined(YUPIF4MINI) // #define BEEPER_INVERTED #else -#define BEEPER_PWM #define BEEPER_INVERTED #define BEEPER_PWM_FREQUENCY 3150 #endif @@ -74,7 +73,6 @@ #define USE_BARO_MS5611 #define USE_BARO_BMP280 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI1 #define MAX7456_CS_PIN PA14 diff --git a/src/main/target/YUPIF7/config.c b/src/main/target/YUPIF7/config.c index e61e532d56..9d5077edba 100644 --- a/src/main/target/YUPIF7/config.c +++ b/src/main/target/YUPIF7/config.c @@ -59,4 +59,5 @@ void targetConfiguration(void) pidProfileMutable()->bank_mc.pid[PID_LEVEL].I = 10; pidProfileMutable()->bank_mc.pid[PID_LEVEL].D = 75; + beeperConfigMutable()->pwmMode = true; } diff --git a/src/main/target/YUPIF7/target.h b/src/main/target/YUPIF7/target.h index a95336a738..6123af03ab 100644 --- a/src/main/target/YUPIF7/target.h +++ b/src/main/target/YUPIF7/target.h @@ -25,7 +25,6 @@ #define CAMERA_CONTROL_PIN PB7 #define BEEPER PB14 -#define BEEPER_PWM #define BEEPER_INVERTED #define BEEPER_PWM_FREQUENCY 3150 @@ -115,7 +114,6 @@ #define I2C_DEVICE (I2CDEV_1) // OSD -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI1 #define MAX7456_CS_PIN PA14 diff --git a/src/main/target/ZEEZF7/target.c b/src/main/target/ZEEZF7/target.c index b0ede6a513..e85f35b3fc 100755 --- a/src/main/target/ZEEZF7/target.c +++ b/src/main/target/ZEEZF7/target.c @@ -26,14 +26,14 @@ const timerHardware_t timerHardware[] = { #ifdef ZEEZF7V2 - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR, 0, 0), // S1 - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR, 0, 0), // S2 - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR, 0, 0), // S3 - DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR, 0, 0), // S4 - DEF_TIM(TIM3, CH3, PC8, TIM_USE_MC_MOTOR, 0, 0), // S5 - DEF_TIM(TIM3, CH2, PC7, TIM_USE_MC_MOTOR, 0, 0), // S6 - DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR, 0, 0), // S7 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR, 0, 0), // S8 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 + DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 + DEF_TIM(TIM3, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 + DEF_TIM(TIM3, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 + DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S8 #else DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR, 0, 0), // S1 DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR, 0, 0), // S2 diff --git a/src/main/target/ZEEZF7/target.h b/src/main/target/ZEEZF7/target.h index 4558922417..c01f4ede4d 100755 --- a/src/main/target/ZEEZF7/target.h +++ b/src/main/target/ZEEZF7/target.h @@ -140,7 +140,6 @@ #define SPI3_MISO_PIN PC11 #define SPI3_MOSI_PIN PB5 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN PA15 @@ -232,4 +231,3 @@ #endif #define USE_DSHOT #define USE_ESC_SENSOR -#define USE_SERIALSHOT diff --git a/src/main/target/common.h b/src/main/target/common.h index 068e6b8dde..6817ec7e37 100755 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -39,7 +39,6 @@ #define I2C2_OVERCLOCK false #define USE_I2C_PULLUP // Enable built-in pullups on all boards in case external ones are too week -#define USE_RX_PPM #define USE_SERIAL_RX #define USE_SERIALRX_SPEKTRUM // Cheap and fairly common protocol #define USE_SERIALRX_SBUS // Very common protocol @@ -79,17 +78,20 @@ #endif #if (MCU_FLASH_SIZE > 256) + +#if defined(MAG_I2C_BUS) || defined(VCM5883_I2C_BUS) +#define USE_MAG_VCM5883 +#endif + #define USE_MR_BRAKING_MODE #define USE_PITOT #define USE_PITOT_ADC -#define USE_PITOT_VIRTUAL -#define USE_ALPHA_BETA_GAMMA_FILTER #define USE_DYNAMIC_FILTERS #define USE_GYRO_KALMAN #define USE_SMITH_PREDICTOR +#define USE_RATE_DYNAMICS #define USE_EXTENDED_CMS_MENUS -#define USE_HOTT_TEXTMODE // NAZA GPS support for F4+ only #define USE_GPS_PROTO_NAZA @@ -100,8 +102,8 @@ #define USE_RANGEFINDER_BENEWAKE #define USE_RANGEFINDER_VL53L0X #define USE_RANGEFINDER_VL53L1X -#define USE_RANGEFINDER_HCSR04_I2C #define USE_RANGEFINDER_US42 +#define USE_RANGEFINDER_TOF10120_I2C // Allow default optic flow boards #define USE_OPFLOW @@ -125,8 +127,7 @@ #define DASHBOARD_ARMED_BITMAP #define USE_OLED_UG2864 -#define USE_PWM_DRIVER_PCA9685 - +#define USE_OSD #define USE_FRSKYOSD #define USE_DJI_HD_OSD #define USE_SMARTPORT_MASTER @@ -144,14 +145,10 @@ #define USE_GPS_PROTO_MTK #define USE_TELEMETRY_SIM -#define USE_TELEMETRY_HOTT #define USE_TELEMETRY_MAVLINK #define USE_MSP_OVER_TELEMETRY #define USE_SERIALRX_SRXL2 // Spektrum SRXL2 protocol -#define USE_SERIALRX_SUMD -#define USE_SERIALRX_SUMH -#define USE_SERIALRX_XBUS #define USE_SERIALRX_JETIEXBUS #define USE_SERIALRX_MAVLINK #define USE_TELEMETRY_SRXL @@ -186,20 +183,19 @@ #define USE_TELEMETRY_IBUS #define USE_TELEMETRY_SMARTPORT #define USE_TELEMETRY_CRSF +#define USE_TELEMETRY_JETIEXBUS // These are rather exotic serial protocols #define USE_RX_MSP //#define USE_MSP_RC_OVERRIDE #define USE_SERIALRX_CRSF -#define USE_PWM_SERVO_DRIVER #define USE_SERIAL_PASSTHROUGH -#define NAV_MAX_WAYPOINTS 60 +#define NAV_MAX_WAYPOINTS 120 #define USE_RCDEVICE //Enable VTX control #define USE_VTX_CONTROL #define USE_VTX_SMARTAUDIO #define USE_VTX_TRAMP -#define USE_VTX_FFPV #ifndef STM32F3 //F3 series does not have enoug RAM to support logic conditions #define USE_PROGRAMMING_FRAMEWORK @@ -216,3 +212,19 @@ #define SKIP_TASK_STATISTICS #endif + +//Designed to free space of F722 and F411 MCUs +#if (MCU_FLASH_SIZE > 512) + +#define USE_VTX_FFPV +#define USE_PITOT_VIRTUAL +#define USE_PWM_DRIVER_PCA9685 +#define USE_PWM_SERVO_DRIVER + +#define USE_SERIALRX_SUMD +#define USE_SERIALRX_SUMH +#define USE_SERIALRX_XBUS +#define USE_TELEMETRY_HOTT +#define USE_HOTT_TEXTMODE + +#endif diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c index bc32325ced..072473e596 100755 --- a/src/main/target/common_hardware.c +++ b/src/main/target/common_hardware.c @@ -73,6 +73,10 @@ BUSDEV_REGISTER_SPI(busdev_icm20689, DEVHW_ICM20689, ICM20689_SPI_BUS, ICM20689_CS_PIN, GYRO_INT_EXTI, DEVFLAGS_NONE, IMU_ICM20689_ALIGN); #endif + #if defined(USE_IMU_ICM42605) + BUSDEV_REGISTER_SPI(busdev_icm42605, DEVHW_ICM42605, ICM42605_SPI_BUS, ICM42605_CS_PIN, GYRO_INT_EXTI, DEVFLAGS_NONE, IMU_ICM42605_ALIGN); + #endif + #if defined(USE_IMU_BMI160) #if defined(BMI160_SPI_BUS) BUSDEV_REGISTER_SPI(busdev_bmi160, DEVHW_BMI160, BMI160_SPI_BUS, BMI160_CS_PIN, GYRO_INT_EXTI, DEVFLAGS_NONE, IMU_BMI160_ALIGN); @@ -89,6 +93,10 @@ BUSDEV_REGISTER_I2C(busdev_bmi088, DEVHW_BMI088, BMI088_I2C_BUS, 0x68, GYRO_INT_EXTI, DEVFLAGS_NONE, IMU_BMI088_ALIGN); #endif #endif + + #if defined(USE_IMU_BMI270) + BUSDEV_REGISTER_SPI(busdev_bmi270, DEVHW_BMI270, BMI270_SPI_BUS, BMI270_CS_PIN, GYRO_INT_EXTI, DEVFLAGS_NONE, IMU_BMI270_ALIGN); + #endif #endif @@ -244,6 +252,21 @@ BUSDEV_REGISTER_SPI(busdev_rm3100, DEVHW_RM3100, RM3100_SPI_BUS, RM3100_CS_PIN, NONE, DEVFLAGS_NONE, 0); #endif #endif + +#if defined(USE_MAG_VCM5883) + #if !defined(VCM5883_I2C_BUS) + #define VCM5883_I2C_BUS MAG_I2C_BUS + #endif + BUSDEV_REGISTER_I2C(busdev_vcm5883, DEVHW_VCM5883, VCM5883_I2C_BUS, 0x0C, NONE, DEVFLAGS_NONE, 0); +#endif + +#if defined(USE_MAG_MLX90393) + #if !defined(MLX90393_I2C_BUS) + #define MLX90393_I2C_BUS MAG_I2C_BUS + #endif + BUSDEV_REGISTER_I2C(busdev_mlx90393, DEVHW_MLX90393, MLX90393_I2C_BUS, 0x0C, NONE, DEVFLAGS_NONE, 0); +#endif + #endif @@ -291,15 +314,6 @@ #endif #endif -#if defined(USE_RANGEFINDER_HCSR04_I2C) && (defined(HCSR04_I2C_BUS) || defined(RANGEFINDER_I2C_BUS)) - #if !defined(HCSR04_I2C_BUS) - #define HCSR04_I2C_BUS RANGEFINDER_I2C_BUS - #endif - #if defined(HCSR04_I2C_BUS) - BUSDEV_REGISTER_I2C(busdev_hcsr04, DEVHW_HCSR04_I2C, HCSR04_I2C_BUS, 0x14, NONE, DEVFLAGS_NONE, 0); - #endif -#endif - #if defined(USE_RANGEFINDER_VL53L0X) #if !defined(VL53L0X_I2C_BUS) && defined(RANGEFINDER_I2C_BUS) #define VL53L0X_I2C_BUS RANGEFINDER_I2C_BUS @@ -329,6 +343,15 @@ #endif #endif +#if defined(USE_RANGEFINDER_TOF10120_I2C) && (defined(TOF10120_I2C_BUS) || defined(RANGEFINDER_I2C_BUS)) + #if !defined(TOF10120_I2C_BUS) + #define TOF10120_I2C_BUS RANGEFINDER_I2C_BUS + #endif + #if defined(TOF10120_I2C_BUS) + BUSDEV_REGISTER_I2C(busdev_tof10120, DEVHW_TOF10120_I2C, TOF10120_I2C_BUS, 0x52, NONE, DEVFLAGS_USE_RAW_REGISTERS, 0); + #endif +#endif + /** AIRSPEED SENSORS **/ #if defined(PITOT_I2C_BUS) && !defined(MS4525_I2C_BUS) diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index 1c7767dcf8..0ca2ff4c9a 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -58,7 +58,10 @@ extern uint8_t __config_end; #undef USE_SERIALRX_SUMH #undef USE_SERIALRX_XBUS #undef USE_SERIALRX_JETIEXBUS -#undef USE_PWM_SERVO_DRIVER +#endif + +#ifndef BEEPER_PWM_FREQUENCY +#define BEEPER_PWM_FREQUENCY 2500 #endif #define USE_ARM_MATH // try to use FPU functions @@ -85,3 +88,18 @@ extern uint8_t __config_end; #define FILE_COMPILE_NORMAL #define FILE_COMPILE_FOR_SPEED #endif + +#if defined(CONFIG_IN_RAM) || defined(CONFIG_IN_EXTERNAL_FLASH) +#ifndef EEPROM_SIZE +#define EEPROM_SIZE 8192 +#endif +extern uint8_t eepromData[EEPROM_SIZE]; +#define __config_start (*eepromData) +#define __config_end (*ARRAYEND(eepromData)) +#else +#ifndef CONFIG_IN_FLASH +#define CONFIG_IN_FLASH +#endif +extern uint8_t __config_start; // configured via linker script when building binaries. +extern uint8_t __config_end; +#endif diff --git a/src/main/target/link/stm32_flash_h743xi.ld b/src/main/target/link/stm32_flash_h743xi.ld index c338241bc7..7f3d857f5a 100644 --- a/src/main/target/link/stm32_flash_h743xi.ld +++ b/src/main/target/link/stm32_flash_h743xi.ld @@ -56,5 +56,6 @@ MEMORY REGION_ALIAS("STACKRAM", DTCM_RAM) REGION_ALIAS("FASTRAM", DTCM_RAM) +REGION_ALIAS("SLOWRAM", D2_RAM) INCLUDE "stm32_flash_h7_split.ld" \ No newline at end of file diff --git a/src/main/target/sanity_check.h b/src/main/target/sanity_check.h index 8b78b89d4d..2fa8379c7f 100644 --- a/src/main/target/sanity_check.h +++ b/src/main/target/sanity_check.h @@ -45,7 +45,7 @@ #error "Replace USE_GYRO_xxx and USE_ACC_xxx with USE_IMU_xxx" #endif -#if defined (USE_ACC_ADXL345) || defined (USE_ACC_LSM303DLHC) || defined (USE_ACC_MMA8452) || defined (USE_ACC_BMA280) || defined (USE_ACC_BMI160) +#if defined (USE_ACC_LSM303DLHC) || defined (USE_ACC_BMI160) #error "Replace USE_GYRO_xxx and USE_ACC_xxx with USE_IMU_xxx" #endif @@ -74,10 +74,6 @@ #error "Replace GYRO_ICM20689_ALIGN and ACC_ICM20689_ALIGN with IMU_ICM20689_ALIGN" #endif -#if defined (GYRO_L3G4200D_ALIGN) -#error "Replace GYRO_L3G4200D_ALIGN with IMU_L3G4200D_ALIGN" -#endif - #if defined (GYRO_MPU3050_ALIGN) #error "Replace GYRO_MPU3050_ALIGN with IMU_MPU3050_ALIGN" #endif @@ -86,18 +82,6 @@ #error "Replace GYRO_L3GD20_ALIGN with IMU_L3GD20_ALIGN" #endif -#if defined (ACC_BMA280_ALIGN) -#error "Replace ACC_BMA280_ALIGN with IMU_BMA280_ALIGN" -#endif - -#if defined (ACC_MMA8452_ALIGN) -#error "Replace ACC_MMA8452_ALIGN with IMU_MMA8452_ALIGN" -#endif - -#if defined (ACC_ADXL345_ALIGN) -#error "Replace ACC_ADXL345_ALIGN with IMU_ADXL345_ALIGN" -#endif - #if defined (ACC_LSM303DLHC_ALIGN) #error "Replace ACC_LSM303DLHC_ALIGN with IMU_LSM303DLHC_ALIGN" #endif diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 439ca8f2d0..079abf95d2 100755 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -332,9 +332,9 @@ static void crsfFrameFlightMode(sbuf_t *dst) } else if (FLIGHT_MODE(NAV_POSHOLD_MODE)) { flightMode = "HOLD"; } else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) { - flightMode = "3CRS"; + flightMode = "CRUZ"; } else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { - flightMode = "CRS"; + flightMode = "CRSH"; } else if (FLIGHT_MODE(NAV_ALTHOLD_MODE)) { flightMode = "AH"; } else if (FLIGHT_MODE(NAV_WP_MODE)) { diff --git a/src/main/telemetry/frsky_d.c b/src/main/telemetry/frsky_d.c index b551f6b9d1..0bc65d3457 100644 --- a/src/main/telemetry/frsky_d.c +++ b/src/main/telemetry/frsky_d.c @@ -193,10 +193,10 @@ static void sendGpsAltitude(void) static void sendThrottleOrBatterySizeAsRpm(void) { - uint16_t throttleForRPM = rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER; sendDataHead(ID_RPM); if (ARMING_FLAG(ARMED)) { const throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC); + uint16_t throttleForRPM = rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER; if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)) throttleForRPM = 0; serialize16(throttleForRPM); @@ -265,11 +265,7 @@ static void GPStoDDDMM_MMMM(int32_t mwiigps, gpsCoordinateDDDMMmmmm_t *result) absgps = (absgps - deg * GPS_DEGREES_DIVIDER) * 60; // absgps = Minutes left * 10^7 min = absgps / GPS_DEGREES_DIVIDER; // minutes left - if (telemetryConfig()->frsky_coordinate_format == FRSKY_FORMAT_DMS) { - result->dddmm = deg * 100 + min; - } else { - result->dddmm = deg * 60 + min; - } + result->dddmm = deg * ((FRSKY_FORMAT_DMS == telemetryConfig()->frsky_coordinate_format) ? (100) : (60)) + min; result->mmmm = (absgps - min * GPS_DEGREES_DIVIDER) / 1000; } diff --git a/src/main/telemetry/ghst.c b/src/main/telemetry/ghst.c index c2aca3ac51..85f6f85b96 100644 --- a/src/main/telemetry/ghst.c +++ b/src/main/telemetry/ghst.c @@ -147,10 +147,12 @@ void ghstFrameGpsSecondaryTelemetry(sbuf_t *dst) sbufWriteU16(dst, GPS_directionToHome); uint8_t gpsFlags = 0; - if(STATE(GPS_FIX)) + if (STATE(GPS_FIX)) { gpsFlags |= GPS_FLAGS_FIX; - if(STATE(GPS_FIX_HOME)) + } + if (STATE(GPS_FIX_HOME)) { gpsFlags |= GPS_FLAGS_FIX_HOME; + } sbufWriteU8(dst, gpsFlags); } diff --git a/src/main/telemetry/jetiexbus.c b/src/main/telemetry/jetiexbus.c new file mode 100644 index 0000000000..170c12d5fe --- /dev/null +++ b/src/main/telemetry/jetiexbus.c @@ -0,0 +1,594 @@ +/* + * 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 . + */ + +#include +#include +#include + +#include "platform.h" + +#if defined(USE_TELEMETRY_JETIEXBUS) + +#include "build/build_config.h" +#include "build/debug.h" +#include "fc/runtime_config.h" +#include "fc/config.h" +#include "config/feature.h" + +#include "common/utils.h" +#include "common/bitarray.h" + +#include "drivers/serial.h" +#include "drivers/serial_uart.h" +#include "drivers/time.h" + +#include "flight/imu.h" + +#include "io/serial.h" +#include "io/gps.h" + +#include "rx/rx.h" +#include "rx/jetiexbus.h" + +#include "sensors/battery.h" +#include "sensors/sensors.h" +#include "sensors/acceleration.h" + +#include "telemetry/jetiexbus.h" +#include "telemetry/telemetry.h" + +#include "navigation/navigation.h" + +#ifdef USE_ESC_SENSOR +#include "sensors/esc_sensor.h" +#include "flight/mixer.h" +#endif + +#define EXTEL_DATA_MSG (0x40) +#define EXTEL_UNMASK_TYPE (0x3F) +#define EXTEL_SYNC_LEN 1 +#define EXTEL_CRC_LEN 1 +#define EXTEL_HEADER_LEN 6 +#define EXTEL_MAX_LEN 26 +#define EXTEL_OVERHEAD (EXTEL_SYNC_LEN + EXTEL_HEADER_LEN + EXTEL_CRC_LEN) +#define EXTEL_MAX_PAYLOAD (EXTEL_MAX_LEN - EXTEL_OVERHEAD) +#define EXBUS_MAX_REQUEST_BUFFER_SIZE (EXBUS_OVERHEAD + EXTEL_MAX_LEN) + +enum exTelHeader_e { + EXTEL_HEADER_SYNC = 0, + EXTEL_HEADER_TYPE_LEN, + EXTEL_HEADER_USN_LB, + EXTEL_HEADER_USN_HB, + EXTEL_HEADER_LSN_LB, + EXTEL_HEADER_LSN_HB, + EXTEL_HEADER_RES, + EXTEL_HEADER_ID, + EXTEL_HEADER_DATA +}; + +enum { + EXBUS_TRANS_ZERO = 0, + EXBUS_TRANS_RX_READY, + EXBUS_TRANS_RX, + EXBUS_TRANS_IS_TX_COMPLETED, + EXBUS_TRANS_TX +}; + +enum exDataType_e { + EX_TYPE_6b = 0, // int6_t Data type 6b (-31 ¸31) + EX_TYPE_14b = 1, // int14_t Data type 14b (-8191 ¸8191) + EX_TYPE_22b = 4, // int22_t Data type 22b (-2097151 ¸2097151) + EX_TYPE_DT = 5, // int22_t Special data type – time and date + EX_TYPE_30b = 8, // int30_t Data type 30b (-536870911 ¸536870911) + EX_TYPE_GPS = 9, // int30_t Special data type – GPS coordinates: lo/hi minute - lo/hi degree. + EX_TYPE_DES = 255 // only for devicedescription +}; + +const uint8_t exDataTypeLen[] = { + [EX_TYPE_6b] = 1, + [EX_TYPE_14b] = 2, + [EX_TYPE_22b] = 3, + [EX_TYPE_DT] = 3, + [EX_TYPE_30b] = 4, + [EX_TYPE_GPS] = 4 +}; + +typedef struct exBusSensor_s { + const char *label; + const char *unit; + const uint8_t exDataType; + const uint8_t decimals; +} exBusSensor_t; + +#define DECIMAL_MASK(decimals) (decimals << 5) + +// list of telemetry messages +// after every 15 sensors a new header has to be inserted (e.g. "BF D2") +const exBusSensor_t jetiExSensors[] = { + {"INAV D1", "", EX_TYPE_DES, 0 }, // device descripton + {"Voltage", "V", EX_TYPE_22b, DECIMAL_MASK(2)}, + {"Current", "A", EX_TYPE_22b, DECIMAL_MASK(2)}, + {"Altitude", "m", EX_TYPE_22b, DECIMAL_MASK(2)}, + {"Capacity", "mAh", EX_TYPE_22b, DECIMAL_MASK(0)}, + {"Power", "W", EX_TYPE_22b, DECIMAL_MASK(0)}, + {"Roll angle", "\xB0", EX_TYPE_22b, DECIMAL_MASK(1)}, + {"Pitch angle", "\xB0", EX_TYPE_22b, DECIMAL_MASK(1)}, + {"Heading", "\xB0", EX_TYPE_22b, DECIMAL_MASK(1)}, + {"Vario", "m/s", EX_TYPE_22b, DECIMAL_MASK(2)}, + {"GPS Sats", "", EX_TYPE_22b, DECIMAL_MASK(0)}, + {"GPS Long", "", EX_TYPE_GPS, DECIMAL_MASK(0)}, + {"GPS Lat", "", EX_TYPE_GPS, DECIMAL_MASK(0)}, + {"GPS Speed", "m/s", EX_TYPE_22b, DECIMAL_MASK(2)}, + {"GPS H-Distance", "m", EX_TYPE_22b, DECIMAL_MASK(0)}, + {"GPS H-Direction", "\xB0", EX_TYPE_22b, DECIMAL_MASK(1)}, + {"INAV D2", "", EX_TYPE_DES, 0 }, // device descripton + {"GPS Heading", "\xB0", EX_TYPE_22b, DECIMAL_MASK(1)}, + {"GPS Altitude", "m", EX_TYPE_22b, DECIMAL_MASK(2)}, + {"G-Force X", "", EX_TYPE_22b, DECIMAL_MASK(3)}, + {"G-Force Y", "", EX_TYPE_22b, DECIMAL_MASK(3)}, + {"G-Force Z", "", EX_TYPE_22b, DECIMAL_MASK(3)}, + {"RPM", "", EX_TYPE_22b, DECIMAL_MASK(0)}, + {"Trip Distance", "m", EX_TYPE_22b, DECIMAL_MASK(0)} +}; + +// after every 15 sensors increment the step by 2 (e.g. ...EX_VAL15, EX_VAL16 = 17) to skip the device description +enum exSensors_e { + EX_VOLTAGE = 1, + EX_CURRENT, + EX_ALTITUDE, + EX_CAPACITY, + EX_POWER, + EX_ROLL_ANGLE, + EX_PITCH_ANGLE, + EX_HEADING, + EX_VARIO, + EX_GPS_SATS, + EX_GPS_LONG, + EX_GPS_LAT, + EX_GPS_SPEED, + EX_GPS_DISTANCE_TO_HOME, + EX_GPS_DIRECTION_TO_HOME, + EX_GPS_HEADING = 17, + EX_GPS_ALTITUDE, + EX_GFORCE_X, + EX_GFORCE_Y, + EX_GFORCE_Z, + EX_RPM, + EX_TRIP_DISTANCE, +}; + +union{ + int32_t vInt; + uint16_t vWord[2]; + char vBytes[4]; +} exGps; + + +#define JETI_EX_SENSOR_COUNT (ARRAYLEN(jetiExSensors)) + +static uint8_t jetiExBusTelemetryFrame[40]; +static uint8_t jetiExBusTransceiveState = EXBUS_TRANS_RX; +static uint8_t firstActiveSensor = 0; +static uint32_t exSensorEnabled = 0; + +static uint8_t sendJetiExBusTelemetry(uint8_t packetID, uint8_t item); +static uint8_t getNextActiveSensor(uint8_t currentSensor); + +// Jeti Ex Telemetry CRC calculations for a frame +uint8_t calcCRC8(uint8_t *pt, uint8_t msgLen) +{ + uint8_t crc=0; + for (uint8_t mlen = 0; mlen < msgLen; mlen++) { + crc ^= pt[mlen]; + crc = crc ^ (crc << 1) ^ (crc << 2) ^ (0x0e090700 >> ((crc >> 3) & 0x18)); + } + return(crc); +} + +void enableGpsTelemetry(bool enable) +{ + if (enable) { + bitArraySet(&exSensorEnabled, EX_GPS_SATS); + bitArraySet(&exSensorEnabled, EX_GPS_LONG); + bitArraySet(&exSensorEnabled, EX_GPS_LAT); + bitArraySet(&exSensorEnabled, EX_GPS_SPEED); + bitArraySet(&exSensorEnabled, EX_GPS_DISTANCE_TO_HOME); + bitArraySet(&exSensorEnabled, EX_GPS_DIRECTION_TO_HOME); + bitArraySet(&exSensorEnabled, EX_GPS_HEADING); + bitArraySet(&exSensorEnabled, EX_GPS_ALTITUDE); + bitArraySet(&exSensorEnabled, EX_TRIP_DISTANCE); + } else { + bitArrayClr(&exSensorEnabled, EX_GPS_SATS); + bitArrayClr(&exSensorEnabled, EX_GPS_LONG); + bitArrayClr(&exSensorEnabled, EX_GPS_LAT); + bitArrayClr(&exSensorEnabled, EX_GPS_SPEED); + bitArrayClr(&exSensorEnabled, EX_GPS_DISTANCE_TO_HOME); + bitArrayClr(&exSensorEnabled, EX_GPS_DIRECTION_TO_HOME); + bitArrayClr(&exSensorEnabled, EX_GPS_HEADING); + bitArrayClr(&exSensorEnabled, EX_GPS_ALTITUDE); + bitArrayClr(&exSensorEnabled, EX_TRIP_DISTANCE); + } +} + +/* + * ----------------------------------------------- + * Jeti Ex Bus Telemetry + * ----------------------------------------------- + */ +void initJetiExBusTelemetry(void) +{ + // Init Ex Bus Frame header + jetiExBusTelemetryFrame[EXBUS_HEADER_SYNC] = 0x3B; // Startbytes + jetiExBusTelemetryFrame[EXBUS_HEADER_REQ] = 0x01; + jetiExBusTelemetryFrame[EXBUS_HEADER_DATA_ID] = 0x3A; // Ex Telemetry + + // Init Ex Telemetry header + uint8_t *jetiExTelemetryFrame = &jetiExBusTelemetryFrame[EXBUS_HEADER_DATA]; + + jetiExTelemetryFrame[EXTEL_HEADER_SYNC] = 0x9F; // Startbyte + jetiExTelemetryFrame[EXTEL_HEADER_USN_LB] = 0x1E; // Serial Number 4 Byte + jetiExTelemetryFrame[EXTEL_HEADER_USN_HB] = 0xA4; + jetiExTelemetryFrame[EXTEL_HEADER_LSN_LB] = 0x00; // increment by telemetry count (%16) > only 15 values per device possible + jetiExTelemetryFrame[EXTEL_HEADER_LSN_HB] = 0x00; + jetiExTelemetryFrame[EXTEL_HEADER_RES] = 0x00; // reserved, by default 0x00 + + // Check which sensors are available + if (isBatteryVoltageConfigured()) { + bitArraySet(&exSensorEnabled, EX_VOLTAGE); + } + if (isAmperageConfigured()) { + bitArraySet(&exSensorEnabled, EX_CURRENT); + } + if (isBatteryVoltageConfigured() && isAmperageConfigured()) { + bitArraySet(&exSensorEnabled, EX_POWER); + bitArraySet(&exSensorEnabled, EX_CAPACITY); + } +#ifdef USE_NAV + if (sensors(SENSOR_BARO)) { + bitArraySet(&exSensorEnabled, EX_ALTITUDE); + bitArraySet(&exSensorEnabled, EX_VARIO); + } +#endif + if (sensors(SENSOR_ACC)) { + bitArraySet(&exSensorEnabled, EX_ROLL_ANGLE); + bitArraySet(&exSensorEnabled, EX_PITCH_ANGLE); + bitArraySet(&exSensorEnabled, EX_GFORCE_X); + bitArraySet(&exSensorEnabled, EX_GFORCE_Y); + bitArraySet(&exSensorEnabled, EX_GFORCE_Z); + } + if (sensors(SENSOR_MAG)) { + bitArraySet(&exSensorEnabled, EX_HEADING); + } + + enableGpsTelemetry(feature(FEATURE_GPS)); + +#ifdef USE_ESC_SENSOR + if (STATE(ESC_SENSOR_ENABLED) && getMotorCount() > 0) { + bitArraySet(&exSensorEnabled, EX_RPM); + } +#endif + + firstActiveSensor = getNextActiveSensor(0); // find the first active sensor +} + +void createExTelemetryTextMessage(uint8_t *exMessage, uint8_t messageID, const exBusSensor_t *sensor) +{ + uint8_t labelLength = strlen(sensor->label); + uint8_t unitLength = strlen(sensor->unit); + + exMessage[EXTEL_HEADER_TYPE_LEN] = EXTEL_OVERHEAD + labelLength + unitLength; + exMessage[EXTEL_HEADER_LSN_LB] = messageID & 0xF0; // Device ID + exMessage[EXTEL_HEADER_ID] = messageID & 0x0F; // Sensor ID (%16) + exMessage[EXTEL_HEADER_DATA] = (labelLength << 3) + unitLength; + + memcpy(&exMessage[EXTEL_HEADER_DATA + 1], sensor->label, labelLength); + memcpy(&exMessage[EXTEL_HEADER_DATA + 1 + labelLength], sensor->unit, unitLength); + + exMessage[exMessage[EXTEL_HEADER_TYPE_LEN] + EXTEL_CRC_LEN] = calcCRC8(&exMessage[EXTEL_HEADER_TYPE_LEN], exMessage[EXTEL_HEADER_TYPE_LEN]); +} + +uint32_t calcGpsDDMMmmm(int32_t value, bool isLong) +{ + uint32_t absValue = ABS(value); + uint16_t deg16 = absValue / GPS_DEGREES_DIVIDER; + uint16_t min16 = (absValue - deg16 * GPS_DEGREES_DIVIDER) * 6 / 1000; + + exGps.vInt = 0; + exGps.vWord[0] = min16; + exGps.vWord[1] = deg16; + exGps.vWord[1] |= isLong ? 0x2000 : 0; + exGps.vWord[1] |= (value < 0) ? 0x4000 : 0; + + return exGps.vInt; +} + + +int32_t getSensorValue(uint8_t sensor) +{ + +#ifdef USE_ESC_SENSOR + escSensorData_t * escSensor; +#endif + + switch (sensor) { + case EX_VOLTAGE: + return telemetryConfig()->report_cell_voltage ? getBatteryAverageCellVoltage() : getBatteryVoltage(); + break; + + case EX_CURRENT: + return getAmperage(); + break; + + case EX_ALTITUDE: + return getEstimatedActualPosition(Z); + break; + + case EX_CAPACITY: + return getMAhDrawn(); + break; + + case EX_POWER: + return (getBatteryVoltage() * getAmperage() / 10000); + break; + + case EX_ROLL_ANGLE: + return attitude.values.roll; + break; + + case EX_PITCH_ANGLE: + return attitude.values.pitch; + break; + + case EX_HEADING: + return attitude.values.yaw; + break; + +#ifdef USE_NAV + case EX_VARIO: + return getEstimatedActualVelocity(Z); + break; +#endif + +#ifdef USE_GPS + case EX_GPS_SATS: + return gpsSol.numSat; + break; + + case EX_GPS_LONG: + return calcGpsDDMMmmm(gpsSol.llh.lon, true); + break; + + case EX_GPS_LAT: + return calcGpsDDMMmmm(gpsSol.llh.lat, false); + break; + + case EX_GPS_SPEED: + return gpsSol.groundSpeed; + break; + + case EX_GPS_DISTANCE_TO_HOME: + return GPS_distanceToHome; + break; + + case EX_GPS_DIRECTION_TO_HOME: + return GPS_directionToHome; + break; + + case EX_GPS_HEADING: + return gpsSol.groundCourse; + break; + + case EX_GPS_ALTITUDE: + return getEstimatedActualPosition(Z); + break; +#endif + + case EX_GFORCE_X: + return acc.accADCf[X] * 1000; + break; + + case EX_GFORCE_Y: + return acc.accADCf[Y] * 1000; + break; + + case EX_GFORCE_Z: + return acc.accADCf[Z] * 1000; + break; + +#ifdef USE_ESC_SENSOR + case EX_RPM: + escSensor = escSensorGetData(); + if (escSensor && escSensor->dataAge <= ESC_DATA_MAX_AGE) { + return escSensor->rpm; + } else { + return 0; + } + break; +#endif + + case EX_TRIP_DISTANCE: + return getTotalTravelDistance() / 100; + + default: + return -1; + } +} + +uint8_t getNextActiveSensor(uint8_t currentSensor) +{ + while( ++currentSensor < JETI_EX_SENSOR_COUNT) { + if (bitArrayGet(&exSensorEnabled, currentSensor)) { + break; + } + } + if (currentSensor == JETI_EX_SENSOR_COUNT ) { + currentSensor = firstActiveSensor; + } + return currentSensor; +} + +uint8_t createExTelemetryValueMessage(uint8_t *exMessage, uint8_t item) +{ + uint8_t startItem = item; + uint8_t sensorItemMaxGroup = (item & 0xF0) + 0x10; + uint8_t iCount; + uint8_t messageSize; + uint32_t sensorValue; + + exMessage[EXTEL_HEADER_LSN_LB] = item & 0xF0; // Device ID + uint8_t *p = &exMessage[EXTEL_HEADER_ID]; + + while (item < sensorItemMaxGroup) { + *p++ = ((item & 0x0F) << 4) | jetiExSensors[item].exDataType; // Sensor ID (%16) | EX Data Type + + sensorValue = getSensorValue(item); + iCount = exDataTypeLen[jetiExSensors[item].exDataType]; + + while (iCount > 1) { + *p++ = sensorValue; + sensorValue = sensorValue >> 8; + iCount--; + } + if (jetiExSensors[item].exDataType != EX_TYPE_GPS) { + *p++ = (sensorValue & 0x9F) | jetiExSensors[item].decimals; + } else { + *p++ = sensorValue; + } + + item = getNextActiveSensor(item); + + if (startItem >= item) { + break; + } + + if ((p - &exMessage[EXTEL_HEADER_ID]) + exDataTypeLen[jetiExSensors[item].exDataType] + 1 >= EXTEL_MAX_PAYLOAD) { + break; + } + } + messageSize = (EXTEL_HEADER_LEN + (p-&exMessage[EXTEL_HEADER_ID])); + exMessage[EXTEL_HEADER_TYPE_LEN] = EXTEL_DATA_MSG | messageSize; + exMessage[messageSize + EXTEL_CRC_LEN] = calcCRC8(&exMessage[EXTEL_HEADER_TYPE_LEN], messageSize); + + return item; // return the next item +} + +void createExBusMessage(uint8_t *exBusMessage, uint8_t *exMessage, uint8_t packetID) +{ + uint16_t crc16; + + exBusMessage[EXBUS_HEADER_PACKET_ID] = packetID; + exBusMessage[EXBUS_HEADER_SUBLEN] = (exMessage[EXTEL_HEADER_TYPE_LEN] & EXTEL_UNMASK_TYPE) + 2; // +2: startbyte & CRC8 + exBusMessage[EXBUS_HEADER_MSG_LEN] = EXBUS_OVERHEAD + exBusMessage[EXBUS_HEADER_SUBLEN]; + + crc16 = jetiExBusCalcCRC16(exBusMessage, exBusMessage[EXBUS_HEADER_MSG_LEN] - EXBUS_CRC_LEN); + exBusMessage[exBusMessage[EXBUS_HEADER_MSG_LEN] - 2] = crc16; + exBusMessage[exBusMessage[EXBUS_HEADER_MSG_LEN] - 1] = crc16 >> 8; +} + +void checkJetiExBusTelemetryState(void) +{ + return; +} + +void handleJetiExBusTelemetry(void) +{ + static uint16_t framesLost = 0; // only for debug + static uint8_t item = 0; + uint32_t timeDiff; + + // Check if we shall reset frame position due to time + if (jetiExBusRequestState == EXBUS_STATE_RECEIVED) { + + // to prevent timing issues from request to answer - max. 4ms + timeDiff = micros() - jetiTimeStampRequest; + + if (timeDiff > 3000) { // include reserved time + jetiExBusRequestState = EXBUS_STATE_ZERO; + framesLost++; + return; + } + + if ((jetiExBusRequestFrame[EXBUS_HEADER_DATA_ID] == EXBUS_EX_REQUEST) && (jetiExBusCalcCRC16(jetiExBusRequestFrame, jetiExBusRequestFrame[EXBUS_HEADER_MSG_LEN]) == 0)) { + if (serialRxBytesWaiting(jetiExBusPort) == 0) { + jetiExBusTransceiveState = EXBUS_TRANS_TX; + item = sendJetiExBusTelemetry(jetiExBusRequestFrame[EXBUS_HEADER_PACKET_ID], item); + jetiExBusRequestState = EXBUS_STATE_PROCESSED; + return; + } + } else { + jetiExBusRequestState = EXBUS_STATE_ZERO; + return; + } + } + + // check the state if transmit is ready + if (jetiExBusTransceiveState == EXBUS_TRANS_IS_TX_COMPLETED) { + if (isSerialTransmitBufferEmpty(jetiExBusPort)) { + jetiExBusTransceiveState = EXBUS_TRANS_RX; + jetiExBusRequestState = EXBUS_STATE_ZERO; + } + } +} + +uint8_t sendJetiExBusTelemetry(uint8_t packetID, uint8_t item) +{ + static uint8_t sensorDescriptionCounter = 0xFF; + static uint8_t requestLoop = 0xFF; + static bool allSensorsActive = true; + uint8_t *jetiExTelemetryFrame = &jetiExBusTelemetryFrame[EXBUS_HEADER_DATA]; + + if (requestLoop) { + while( ++sensorDescriptionCounter < JETI_EX_SENSOR_COUNT) { + if (bitArrayGet(&exSensorEnabled, sensorDescriptionCounter) || (jetiExSensors[sensorDescriptionCounter].exDataType == EX_TYPE_DES)) { + break; + } + } + if (sensorDescriptionCounter == JETI_EX_SENSOR_COUNT ) { + sensorDescriptionCounter = 0; + } + + createExTelemetryTextMessage(jetiExTelemetryFrame, sensorDescriptionCounter, &jetiExSensors[sensorDescriptionCounter]); + createExBusMessage(jetiExBusTelemetryFrame, jetiExTelemetryFrame, packetID); + requestLoop--; + if (requestLoop == 0) { + item = firstActiveSensor; + if (feature(FEATURE_GPS)) { + enableGpsTelemetry(false); + allSensorsActive = false; + } + } + } else { + item = createExTelemetryValueMessage(jetiExTelemetryFrame, item); + createExBusMessage(jetiExBusTelemetryFrame, jetiExTelemetryFrame, packetID); + + if (!allSensorsActive) { + if (sensors(SENSOR_GPS)) { + enableGpsTelemetry(true); + allSensorsActive = true; + } + } + } + + serialWriteBuf(jetiExBusPort, jetiExBusTelemetryFrame, jetiExBusTelemetryFrame[EXBUS_HEADER_MSG_LEN]); + jetiExBusTransceiveState = EXBUS_TRANS_IS_TX_COMPLETED; + + return item; +} +#endif diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 2eb3723950..c97c73076b 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -664,7 +664,7 @@ void mavlinkSendHUDAndHeartbeat(void) } #endif - + int16_t thr = rxGetChannelValue(THROTTLE); if (navigationIsControllingThrottle()) { thr = rcCommand[THROTTLE]; @@ -777,7 +777,7 @@ void mavlinkSendBatteryTemperatureStatusText(void) if (cell < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN) { batteryVoltages[cell] = getBatteryAverageCellVoltage() * 10; } else { - batteryVoltagesExt[cell] = getBatteryAverageCellVoltage() * 10; + batteryVoltagesExt[cell-MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN] = getBatteryAverageCellVoltage() * 10; } } } diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index dd980d242f..766dea1901 100755 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -62,7 +62,7 @@ FILE_COMPILE_FOR_SPEED #include "telemetry/frsky.h" #include "telemetry/msp_shared.h" -// these data identifiers are obtained from https://github.com/opentx/opentx/blob/master/radio/src/telemetry/frsky_hub.h +// these data identifiers are obtained from https://github.com/opentx/opentx/blob/2.3/radio/src/telemetry/frsky.h enum { FSSP_DATAID_SPEED = 0x0830 , @@ -91,7 +91,8 @@ enum FSSP_DATAID_GPS_ALT = 0x0820 , FSSP_DATAID_ASPD = 0x0A00 , FSSP_DATAID_A3 = 0x0900 , - FSSP_DATAID_A4 = 0x0910 + FSSP_DATAID_A4 = 0x0910 , + FSSP_DATAID_AZIMUTH = 0x0460 }; const uint16_t frSkyDataIdTable[] = { @@ -123,6 +124,7 @@ const uint16_t frSkyDataIdTable[] = { FSSP_DATAID_ASPD , // FSSP_DATAID_A3 , FSSP_DATAID_A4 , + FSSP_DATAID_AZIMUTH , 0 }; @@ -525,6 +527,20 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear *clearToSend = false; } break; + case FSSP_DATAID_AZIMUTH : + if (smartPortShouldSendGPSData()) { + int16_t h = GPS_directionToHome; + if (h < 0) { + h += 360; + } + if(h >= 180) + h = h - 180; + else + h = h + 180; + smartPortSendPackage(id, h *10); // given in 10*deg + *clearToSend = false; + } + break; #endif case FSSP_DATAID_A4 : if (isBatteryVoltageConfigured()) { diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index add40a3b0f..00a5f182ae 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -117,7 +117,7 @@ void telemetryInit(void) initMAVLinkTelemetry(); #endif -#if defined(TELEMETRY_JETIEXBUS) +#if defined(USE_TELEMETRY_JETIEXBUS) initJetiExBusTelemetry(); #endif @@ -187,7 +187,7 @@ void telemetryCheckState(void) checkMAVLinkTelemetryState(); #endif -#if defined(TELEMETRY_JETIEXBUS) +#if defined(USE_TELEMETRY_JETIEXBUS) checkJetiExBusTelemetryState(); #endif @@ -235,7 +235,7 @@ void telemetryProcess(timeUs_t currentTimeUs) handleMAVLinkTelemetry(currentTimeUs); #endif -#if defined(TELEMETRY_JETIEXBUS) +#if defined(USE_TELEMETRY_JETIEXBUS) handleJetiExBusTelemetry(); #endif diff --git a/src/test/unit/io_serial_unittest.cc.txt b/src/test/unit/io_serial_unittest.cc.txt index e063c250e9..336c46d960 100644 --- a/src/test/unit/io_serial_unittest.cc.txt +++ b/src/test/unit/io_serial_unittest.cc.txt @@ -34,7 +34,7 @@ extern "C" { #include "gtest/gtest.h" //uint32_t testFeatureMask = 0; -uint8_t cliMode = 0; +bool cliMode = false; TEST(IoSerialTest, TestFindPortConfig) { diff --git a/src/test/unit/sonar_unittest.cc.txt b/src/test/unit/sonar_unittest.cc.txt deleted file mode 100644 index dc60b0949b..0000000000 --- a/src/test/unit/sonar_unittest.cc.txt +++ /dev/null @@ -1,107 +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 . - */ - -#include -#include - -extern "C" { - #include "build_config.h" - #include "drivers/rangefinder_hcsr04.h" - #include "sensors/sonar.h" - extern int32_t measurement; - extern int16_t sonarMaxTiltDeciDegrees; - void sonarInit(const sonarHardware_t *sonarHardware); -} - -#include "unittest_macros.h" -#include "gtest/gtest.h" - -#define DECIDEGREES_TO_RADIANS(angle) ((angle / 10.0f) * 0.0174532925f) - -TEST(SonarUnittest, TestConstants) -{ - sonarInit(0); - // SONAR_OUT_OF_RANGE must be negative - EXPECT_LE(SONAR_OUT_OF_RANGE, 0); - // Check against gross errors in max range constants - EXPECT_GT(HCSR04_MAX_RANGE_CM, 100); -} - -TEST(SonarUnittest, TestSonarInit) -{ - sonarInit(0); - // Check against gross errors in max range values - EXPECT_GE(sonarCfAltCm, 100); - EXPECT_LE(sonarCfAltCm, HCSR04_MAX_RANGE_CM); - // Check reasonable values for maximum tilt - EXPECT_GE(sonarMaxTiltDeciDegrees, 0); - EXPECT_LE(sonarMaxTiltDeciDegrees, 450); -} - -TEST(SonarUnittest, TestDistance) -{ - // Check sonar pulse time converted correctly to cm - const int echoMicroSecondsPerCm = 59; - measurement = 0; - EXPECT_EQ(hcsr04_get_distance(), 0); - - measurement = echoMicroSecondsPerCm; - EXPECT_EQ(hcsr04_get_distance(), 1); - - measurement = 10 * echoMicroSecondsPerCm; - EXPECT_EQ(hcsr04_get_distance(), 10); - - measurement = HCSR04_MAX_RANGE_CM * echoMicroSecondsPerCm; - EXPECT_EQ(hcsr04_get_distance(), HCSR04_MAX_RANGE_CM); -} - -TEST(SonarUnittest, TestAltitude) -{ - sonarInit(0); - // Check distance not modified if no tilt - EXPECT_EQ(sonarCalculateAltitude(0, cosf(DECIDEGREES_TO_RADIANS(0))), 0); - EXPECT_EQ(sonarGetLatestAltitude(), 0); - EXPECT_EQ(sonarCalculateAltitude(100, cosf(DECIDEGREES_TO_RADIANS(0))), 100); - EXPECT_EQ(sonarGetLatestAltitude(), 100); - - // Check that out of range is returned if tilt is too large - EXPECT_EQ(sonarCalculateAltitude(0, cosf(DECIDEGREES_TO_RADIANS(sonarMaxTiltDeciDegrees+1))), SONAR_OUT_OF_RANGE); - EXPECT_EQ(sonarGetLatestAltitude(), SONAR_OUT_OF_RANGE); - - // Check distance at various roll angles - // distance 400, 5 degrees of roll - EXPECT_EQ(sonarCalculateAltitude(400, cosf(DECIDEGREES_TO_RADIANS(50))), 398); - EXPECT_EQ(sonarGetLatestAltitude(), 398); - // distance 400, 10 degrees of roll - EXPECT_EQ(sonarCalculateAltitude(400, cosf(DECIDEGREES_TO_RADIANS(100))), 393); - EXPECT_EQ(sonarGetLatestAltitude(), 393); - // distance 400, 15 degrees of roll, this corresponds to HC-SR04 specified max detection angle - EXPECT_EQ(sonarCalculateAltitude(400, cosf(DECIDEGREES_TO_RADIANS(150))), 386); - EXPECT_EQ(sonarGetLatestAltitude(), 386); - // distance 400, 20 degrees of roll - EXPECT_EQ(sonarCalculateAltitude(400, cosf(DECIDEGREES_TO_RADIANS(200))), 375); - EXPECT_EQ(sonarGetLatestAltitude(), 375); - // distance 400, 22.4 degrees of roll, this corresponds to HC-SR04 effective max detection angle - EXPECT_EQ(sonarCalculateAltitude(400, cosf(DECIDEGREES_TO_RADIANS(224))), 369); - EXPECT_EQ(sonarGetLatestAltitude(), 369); -} - -// STUBS -extern "C" { -void sensorsSet(uint32_t mask) {UNUSED(mask);} -} - diff --git a/support/buildserver/upload.php b/support/buildserver/upload.php deleted file mode 100755 index ef9fd306ab..0000000000 --- a/support/buildserver/upload.php +++ /dev/null @@ -1,57 +0,0 @@ - diff --git a/support/flash.bat b/support/flash.bat deleted file mode 100644 index 0e0f8000b7..0000000000 --- a/support/flash.bat +++ /dev/null @@ -1,26 +0,0 @@ -@@echo OFF - -:: User configuration - -:: set your port number. ie: for COM6 , port is 6 -set PORT=2 -:: location of stmflashloader.exe, this is default -set FLASH_LOADER="C:\Program Files (x86)\STMicroelectronics\Software\Flash Loader Demonstrator\STMFlashLoader.exe" -:: path to firmware -set FIRMWARE="D:\baseflight.hex" - -:: ---------------------------------------------- - -mode COM%PORT% BAUD=115200 PARITY=N DATA=8 STOP=1 XON=OFF DTR=OFF RTS=OFF -echo/|set /p ="R" > COM%PORT%: - -TIMEOUT /T 3 - -cd %LOADER_PATH% - -%FLASH_LOADER% ^ - -c --pn %PORT% --br 115200 --db 8 ^ - -i STM32_Med-density_128K ^ - -e --all ^ - -d --fn %FIRMWARE% ^ - -r --a 0x8000000 diff --git a/support/stmloader/Makefile b/support/stmloader/Makefile deleted file mode 100644 index 936fa697a3..0000000000 --- a/support/stmloader/Makefile +++ /dev/null @@ -1,14 +0,0 @@ -CC = $(CROSS_COMPILE)gcc -AR = $(CROSS_COMPILE)ar -export CC -export AR - -all: - $(CC) -g -o stmloader -I./ \ - loader.c \ - serial.c \ - stmbootloader.c \ - -Wall - -clean: - rm -f stmloader; rm -rf stmloader.dSYM diff --git a/support/stmloader/loader.c b/support/stmloader/loader.c deleted file mode 100644 index 8a060fa258..0000000000 --- a/support/stmloader/loader.c +++ /dev/null @@ -1,122 +0,0 @@ -/* - This file is part of AutoQuad. - - AutoQuad 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. - - AutoQuad 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 AutoQuad. If not, see . - - Copyright © 2011 Bill Nesbitt -*/ - -#include "serial.h" -#include "stmbootloader.h" -#include -#include -#include -#include -#include -#include -#include - -#define DEFAULT_PORT "/dev/ttyUSB0" -#define DEFAULT_BAUD 115200 -#define FIRMWARE_FILENAME "STM32.hex" - -serialStruct_t *s; - -char port[256]; -unsigned int baud; -unsigned char overrideParity; -unsigned char noSendR; -char firmFile[256]; - -void loaderUsage(void) { - fprintf(stderr, "usage: loader <-h> <-p device_file> <-b baud_rate> <-f firmware_file> <-o> <-n>\n"); -} - -unsigned int loaderOptions(int argc, char **argv) { - int ch; - - strncpy(port, DEFAULT_PORT, sizeof(port)); - baud = DEFAULT_BAUD; - overrideParity = 0; - noSendR = 0; - strncpy(firmFile, FIRMWARE_FILENAME, sizeof(firmFile)); - - /* options descriptor */ - static struct option longopts[] = { - { "help", required_argument, NULL, 'h' }, - { "port", required_argument, NULL, 'p' }, - { "baud", required_argument, NULL, 's' }, - { "firm_file", required_argument, NULL, 'f' }, - { "override_parity", no_argument, NULL, 'o' }, - { "no_send_r", no_argument, NULL, 'n' }, - { NULL, 0, NULL, 0 } - }; - - while ((ch = getopt_long(argc, argv, "hp:b:f:o:n", longopts, NULL)) != -1) - switch (ch) { - case 'h': - loaderUsage(); - exit(0); - break; - case 'p': - strncpy(port, optarg, sizeof(port)); - break; - case 'b': - baud = atoi(optarg); - break; - case 'f': - strncpy(firmFile, optarg, sizeof(firmFile)); - break; - case 'o': - overrideParity = 1; - break; - case 'n': - noSendR = 1; - break; - default: - loaderUsage(); - return 0; - } - argc -= optind; - argv += optind; - - return 1; -} - -int main(int argc, char **argv) { - FILE *fw; - - // init - if (!loaderOptions(argc, argv)) { - fprintf(stderr, "Init failed, aborting\n"); - return 1; - } - - s = initSerial(port, baud, 0); - if (!s) { - fprintf(stderr, "Cannot open serial port '%s', aborting.\n", port); - return 1; - } - - fw = fopen(firmFile, "r"); - if (!fw) { - fprintf(stderr, "Cannot open firmware file '%s', aborting.\n", firmFile); - return 1; - } - else { - printf("Upgrading STM on port %s from %s...\n", port, firmFile); - stmLoader(s, fw, overrideParity, noSendR); - } - - return 0; -} diff --git a/support/stmloader/serial.c b/support/stmloader/serial.c deleted file mode 100644 index b11fa4923d..0000000000 --- a/support/stmloader/serial.c +++ /dev/null @@ -1,179 +0,0 @@ -/* - This file is part of AutoQuad. - - AutoQuad 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. - - AutoQuad 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 AutoQuad. If not, see . - - Copyright © 2011 Bill Nesbitt -*/ - -#ifndef _GNU_SOURCE -#define _GNU_SOURCE -#endif - -#include "serial.h" -#include -#include -#include -#include -#include -#include -#include - -serialStruct_t *initSerial(const char *port, unsigned int baud, char ctsRts) { - serialStruct_t *s; - struct termios options; - unsigned int brate; - - s = (serialStruct_t *)calloc(1, sizeof(serialStruct_t)); - - s->fd = open(port, O_RDWR | O_NOCTTY | O_NDELAY); - - if (s->fd == -1) { - free(s); - return 0; - } - - fcntl(s->fd, F_SETFL, 0); // clear all flags on descriptor, enable direct I/O - -// bzero(&options, sizeof(options)); -// memset(&options, 0, sizeof(options)); - tcgetattr(s->fd, &options); - -#ifdef B921600 - switch (baud) { - case 9600: - brate = B9600; - break; - case 19200: - brate = B19200; - break; - case 38400: - brate = B38400; - break; - case 57600: - brate = B57600; - break; - case 115200: - brate = B115200; - break; - case 230400: - brate = B230400; - break; - case 460800: - brate = B460800; - break; - case 921600: - brate = B921600; - break; - default: - brate = B115200; - break; - } - options.c_cflag = brate; -#else // APPLE - cfsetispeed(&options, baud); - cfsetospeed(&options, baud); -#endif - - options.c_cflag |= CRTSCTS | CS8 | CLOCAL | CREAD; - options.c_iflag = IGNPAR; - options.c_iflag &= (~(IXON|IXOFF|IXANY)); // turn off software flow control - options.c_oflag = 0; - - /* set input mode (non-canonical, no echo,...) */ - options.c_lflag = 0; - - options.c_cc[VTIME] = 0; /* inter-character timer unused */ - options.c_cc[VMIN] = 1; /* blocking read until 1 chars received */ - -#ifdef CCTS_OFLOW - options.c_cflag |= CCTS_OFLOW; -#endif - - if (!ctsRts) - options.c_cflag &= ~(CRTSCTS); // turn off hardware flow control - - // set the new port options - tcsetattr(s->fd, TCSANOW, &options); - - return s; -} - -void serialFree(serialStruct_t *s) { - if (s) { - if (s->fd) - close(s->fd); - free (s); - } -} - -void serialNoParity(serialStruct_t *s) { - struct termios options; - - tcgetattr(s->fd, &options); // read serial port options - - options.c_cflag &= ~(PARENB | CSTOPB); - - tcsetattr(s->fd, TCSANOW, &options); -} - -void serialEvenParity(serialStruct_t *s) { - struct termios options; - - tcgetattr(s->fd, &options); // read serial port options - - options.c_cflag |= (PARENB); - options.c_cflag &= ~(PARODD | CSTOPB); - - tcsetattr(s->fd, TCSANOW, &options); -} - -void serialWriteChar(serialStruct_t *s, unsigned char c) { - char ret; - - ret = write(s->fd, &c, 1); -} - -void serialWrite(serialStruct_t *s, const char *str, unsigned int len) { - char ret; - - ret = write(s->fd, str, len); -} - -unsigned char serialAvailable(serialStruct_t *s) { - fd_set fdSet; - struct timeval timeout; - - FD_ZERO(&fdSet); - FD_SET(s->fd, &fdSet); - memset((char *)&timeout, 0, sizeof(timeout)); - - if (select(s->fd+1, &fdSet, 0, 0, &timeout) == 1) - return 1; - else - return 0; -} - -void serialFlush(serialStruct_t *s) { - while (serialAvailable(s)) - serialRead(s); -} - -unsigned char serialRead(serialStruct_t *s) { - char ret; - unsigned char c; - - ret = read(s->fd, &c, 1); - - return c; -} diff --git a/support/stmloader/serial.h b/support/stmloader/serial.h deleted file mode 100644 index 90beedc781..0000000000 --- a/support/stmloader/serial.h +++ /dev/null @@ -1,38 +0,0 @@ -/* - This file is part of AutoQuad. - - AutoQuad 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. - - AutoQuad 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 AutoQuad. If not, see . - - Copyright © 2011 Bill Nesbitt -*/ - -#ifndef _serial_h -#define _serial_h - -#define INPUT_BUFFER_SIZE 1024 - -typedef struct { - int fd; -} serialStruct_t; - -extern serialStruct_t *initSerial(const char *port, unsigned int baud, char ctsRts); -extern void serialWrite(serialStruct_t *s, const char *str, unsigned int len); -extern void serialWriteChar(serialStruct_t *s, unsigned char c); -extern unsigned char serialAvailable(serialStruct_t *s); -extern void serialFlush(serialStruct_t *s); -extern unsigned char serialRead(serialStruct_t *s); -extern void serialEvenParity(serialStruct_t *s); -extern void serialNoParity(serialStruct_t *s); -extern void serialFree(serialStruct_t *s); - -#endif diff --git a/support/stmloader/stmbootloader.c b/support/stmloader/stmbootloader.c deleted file mode 100644 index 9a0550b8da..0000000000 --- a/support/stmloader/stmbootloader.c +++ /dev/null @@ -1,343 +0,0 @@ -/* - This file is part of AutoQuad. - - AutoQuad 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. - - AutoQuad 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 AutoQuad. If not, see . - - Copyright © 2011 Bill Nesbitt -*/ - -#ifndef _GNU_SOURCE -#define _GNU_SOURCE -#endif - -#include "serial.h" -#include -#include -#include -#include - -#define STM_RETRIES_SHORT 1000 -#define STM_RETRIES_LONG 50000 - -unsigned char getResults[11]; - -unsigned char stmHexToChar(const char *hex) { - char hex1, hex2; - unsigned char nibble1, nibble2; - - // force data to upper case - hex1 = toupper(hex[0]); - hex2 = toupper(hex[1]); - - if (hex1 < 65) - nibble1 = hex1 - 48; - else - nibble1 = hex1 - 55; - - if (hex2 < 65) - nibble2 = hex2 - 48; - else - nibble2 = hex2 - 55; - - return (nibble1 << 4 | nibble2); -} - - -unsigned char stmWaitAck(serialStruct_t *s, int retries) { - unsigned char c; - unsigned int i; - - for (i = 0; i < retries; i++) { - if (serialAvailable(s)) { - c = serialRead(s); - if (c == 0x79) { -// putchar('+'); fflush(stdout); - return 1; - } - if (c == 0x1f) { - putchar('-'); fflush(stdout); - return 0; - } - else { - printf("?%x?", c); fflush(stdout); - return 0; - } - } - usleep(500); - } - - return 0; -} - -unsigned char stmWrite(serialStruct_t *s, const char *hex) { - unsigned char c; - unsigned char ck; - unsigned char i; - - ck = 0; - i = 0; - while (*hex) { - c = stmHexToChar(hex); - serialWrite(s, (char *)&c, 1); - ck ^= c; - hex += 2; - i++; - } - if (i == 1) - ck = 0xff ^ c; - - // send checksum - serialWrite(s, (char *)&ck, 1); - - return stmWaitAck(s, STM_RETRIES_LONG); -} - -void stmWriteCommand(serialStruct_t *s, char *msb, char *lsb, char *len, char *data) { - char startAddress[9]; - char lenPlusData[128]; - char c; - - strncpy(startAddress, msb, sizeof(startAddress)); - strcat(startAddress, lsb); - - sprintf(lenPlusData, "%02x%s", stmHexToChar(len) - 1, data); - - write: - // send WRITE MEMORY command - do { - c = getResults[5]; - serialWrite(s, &c, 1); - c = 0xff ^ c; - serialWrite(s, &c, 1); - } while (!stmWaitAck(s, STM_RETRIES_LONG)); - - // send address - if (!stmWrite(s, startAddress)) { - putchar('A'); - goto write; - } - - // send len + data - if (!stmWrite(s, lenPlusData)) { - putchar('D'); - goto write; - } - - putchar('='); fflush(stdout); -} - -char *stmHexLoader(serialStruct_t *s, FILE *fp) { - char hexByteCount[3], hexAddressLSB[5], hexRecordType[3], hexData[128]; - char addressMSB[5]; - static char addressJump[9]; - -// bzero(addressJump, sizeof(addressJump)); -// bzero(addressMSB, sizeof(addressMSB)); - memset(addressJump, 0, sizeof(addressJump)); - memset(addressMSB, 0, sizeof(addressMSB)); - - while (fscanf(fp, ":%2s%4s%2s%s\n", hexByteCount, hexAddressLSB, hexRecordType, hexData) != EOF) { - unsigned int byteCount, addressLSB, recordType; - - recordType = stmHexToChar(hexRecordType); - hexData[stmHexToChar(hexByteCount) * 2] = 0; // terminate at CHKSUM - -// printf("Record Type: %d\n", recordType); - switch (recordType) { - case 0x00: - stmWriteCommand(s, addressMSB, hexAddressLSB, hexByteCount, hexData); - break; - case 0x01: - // EOF - return addressJump; - break; - case 0x04: - // MSB of destination 32 bit address - strncpy(addressMSB, hexData, 4); - break; - case 0x05: - // 32 bit address to run after load - strncpy(addressJump, hexData, 8); - break; - } - } - - return 0; -} - -void stmLoader(serialStruct_t *s, FILE *fp, unsigned char overrideParity, unsigned char noSendR) { - char c; - unsigned char b1, b2, b3; - unsigned char i, n; - char *jumpAddress; - - // turn on parity generation - if (!overrideParity) - serialEvenParity(s); - - if(!noSendR) { - top: - printf("Sending R to place Baseflight in bootloader, press a key to continue"); - serialFlush(s); - c = 'R'; - serialWrite(s, &c, 1); - getchar(); - printf("\n"); - } - - serialFlush(s); - - printf("Poking the MCU to check whether bootloader is alive..."); - - // poke the MCU - do { - printf("p"); fflush(stdout); - c = 0x7f; - serialWrite(s, &c, 1); - } while (!stmWaitAck(s, STM_RETRIES_SHORT)); - printf("STM bootloader alive...\n"); - - // send GET command - do { - c = 0x00; - serialWrite(s, &c, 1); - c = 0xff; - serialWrite(s, &c, 1); - } while (!stmWaitAck(s, STM_RETRIES_LONG)); - - b1 = serialRead(s); // number of bytes - b2 = serialRead(s); // bootloader version - - for (i = 0; i < b1; i++) - getResults[i] = serialRead(s); - - stmWaitAck(s, STM_RETRIES_LONG); - printf("Received commands.\n"); - - - // send GET VERSION command - do { - c = getResults[1]; - serialWrite(s, &c, 1); - c = 0xff ^ c; - serialWrite(s, &c, 1); - } while (!stmWaitAck(s, STM_RETRIES_LONG)); - b1 = serialRead(s); - b2 = serialRead(s); - b3 = serialRead(s); - stmWaitAck(s, STM_RETRIES_LONG); - printf("STM Bootloader version: %d.%d\n", (b1 & 0xf0) >> 4, (b1 & 0x0f)); - - // send GET ID command - do { - c = getResults[2]; - serialWrite(s, &c, 1); - c = 0xff ^ c; - serialWrite(s, &c, 1); - } while (!stmWaitAck(s, STM_RETRIES_LONG)); - n = serialRead(s); - printf("STM Device ID: 0x"); - for (i = 0; i <= n; i++) { - b1 = serialRead(s); - printf("%02x", b1); - } - stmWaitAck(s, STM_RETRIES_LONG); - printf("\n"); - -/* - flash_size: - // read Flash size - c = getResults[3]; - serialWrite(s, &c, 1); - c = 0xff ^ c; - serialWrite(s, &c, 1); - - // if read not allowed, unprotect (which also erases) - if (!stmWaitAck(s, STM_RETRIES_LONG)) { - // unprotect command - do { - c = getResults[10]; - serialWrite(s, &c, 1); - c = 0xff ^ c; - serialWrite(s, &c, 1); - } while (!stmWaitAck(s, STM_RETRIES_LONG)); - - // wait for results - if (stmWaitAck(s, STM_RETRIES_LONG)) - goto top; - } - - // send address - if (!stmWrite(s, "1FFFF7E0")) - goto flash_size; - - // send # bytes (N-1 = 1) - if (!stmWrite(s, "01")) - goto flash_size; - - b1 = serialRead(s); - b2 = serialRead(s); - printf("STM Flash Size: %dKB\n", b2<<8 | b1); -*/ - - // erase flash - erase_flash: - printf("Global flash erase [command 0x%x]...", getResults[6]); fflush(stdout); - do { - c = getResults[6]; - serialWrite(s, &c, 1); - c = 0xff ^ c; - serialWrite(s, &c, 1); - } while (!stmWaitAck(s, STM_RETRIES_LONG)); - - // global erase - if (getResults[6] == 0x44) { - // mass erase - if (!stmWrite(s, "FFFF")) - goto erase_flash; - } - else { - c = 0xff; - serialWrite(s, &c, 1); - c = 0x00; - serialWrite(s, &c, 1); - - if (!stmWaitAck(s, STM_RETRIES_LONG)) - goto erase_flash; - } - - printf("Done.\n"); - - // upload hex file - printf("Flashing device...\n"); - jumpAddress = stmHexLoader(s, fp); - if (jumpAddress) { - printf("\nFlash complete, executing.\n"); - - go: - // send GO command - do { - c = getResults[4]; - serialWrite(s, &c, 1); - c = 0xff ^ c; - serialWrite(s, &c, 1); - } while (!stmWaitAck(s, STM_RETRIES_LONG)); - - // send address - if (!stmWrite(s, jumpAddress)) - goto go; - } - else { - printf("\nFlash complete.\n"); - } -} diff --git a/support/stmloader/stmbootloader.h b/support/stmloader/stmbootloader.h deleted file mode 100644 index 9c7a5de5ad..0000000000 --- a/support/stmloader/stmbootloader.h +++ /dev/null @@ -1,27 +0,0 @@ -/* - This file is part of AutoQuad. - - AutoQuad 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. - - AutoQuad 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 AutoQuad. If not, see . - - Copyright © 2011 Bill Nesbitt -*/ - -#ifndef _stmbootloader_h -#define _stmbootloader_h - -#include -#include "serial.h" - -extern void stmLoader(serialStruct_t *s, FILE *fp, unsigned char overrideParity, unsigned char noSendR); - -#endif