Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 01:05:21 +03:00
Code Issues Wiki Activity

Merge branch 'master' of https://github.com/RomanLut/inav into submit-gps-fix-estimation

Browse source
This commit is contained in:
Roman Lut 2023-08-04 00:57:19 +02:00
commit 2b9a5a6a8d
178 changed files with 5846 additions and 2036 deletions
Download patch file Download diff file Expand all files Collapse all files

48
.github/workflows/ci.yml vendored
View file

@ -38,9 +38,9 @@ jobs:
- name: Build targets (${{ matrix.id }})
run: mkdir -p build && cd build && cmake -DWARNINGS_AS_ERRORS=ON -DCI_JOB_INDEX=${{ matrix.id }} -DCI_JOB_COUNT=${{ strategy.job-total }} -DBUILD_SUFFIX=${{ env.BUILD_SUFFIX }} -G Ninja .. && ninja ci
- name: Upload artifacts
uses: actions/upload-artifact@v2-preview
uses: actions/upload-artifact@v3
with:
name: ${{ env.BUILD_NAME }}.zip
name: ${{ env.BUILD_NAME }}
path: ./build/*.hex
build-SITL-Linux:
@ -68,9 +68,45 @@ jobs:
- name: Build SITL
run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja
- name: Upload artifacts
uses: actions/upload-artifact@v2-preview
uses: actions/upload-artifact@v3
with:
name: ${{ env.BUILD_NAME }}_SITL.zip
name: ${{ env.BUILD_NAME }}_SITL
path: ./build_SITL/*_SITL
build-SITL-Mac:
runs-on: macos-latest
steps:
- uses: actions/checkout@v3
- name: Install dependencies
run: |
brew install cmake ninja ruby
- name: Setup environment
env:
ACTIONS_ALLOW_UNSECURE_COMMANDS: true
run: |
# This is the hash of the commit for the PR
# when the action is triggered by PR, empty otherwise
COMMIT_ID=${{ github.event.pull_request.head.sha }}
# This is the hash of the commit when triggered by push
# but the hash of refs/pull/<n>/merge, which is different
# from the hash of the latest commit in the PR, that's
# why we try github.event.pull_request.head.sha first
COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }')
echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
- name: Build SITL
run: |
mkdir -p build_SITL && cd build_SITL
cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja ..
ninja
- name: Upload artifacts
uses: actions/upload-artifact@v3
with:
name: ${{ env.BUILD_NAME }}_SITL-MacOS
path: ./build_SITL/*_SITL
build-SITL-Windows:
@ -103,9 +139,9 @@ jobs:
- name: Build SITL
run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja
- name: Upload artifacts
uses: actions/upload-artifact@v2-preview
uses: actions/upload-artifact@v3
with:
name: ${{ env.BUILD_NAME }}_SITL-WIN.zip
name: ${{ env.BUILD_NAME }}_SITL-WIN
path: ./build_SITL/*.exe

4
CMakeLists.txt
View file

@ -14,10 +14,10 @@ option(SITL "SITL build for host system" OFF)
set(TOOLCHAIN_OPTIONS none arm-none-eabi host)
if (SITL)
set(TOOLCHAIN "host" CACHE STRING "Toolchain to use. Available: ${TOOLCHAIN_OPTIONS}")
if (${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
if (CMAKE_HOST_APPLE)
set(MACOSX TRUE)
endif()
set(TOOLCHAIN "host" CACHE STRING "Toolchain to use. Available: ${TOOLCHAIN_OPTIONS}")
else()
set(TOOLCHAIN "arm-none-eabi" CACHE STRING "Toolchain to use. Available: ${TOOLCHAIN_OPTIONS}")
endif()

2
cmake/at32.cmake
View file

@ -15,6 +15,8 @@ set(CMSIS_DSP_DIR "${MAIN_LIB_DIR}/main/CMSIS/DSP")
set(CMSIS_DSP_INCLUDE_DIR "${CMSIS_DSP_DIR}/Include")
set(CMSIS_DSP_SRC
BasicMathFunctions/arm_scale_f32.c
BasicMathFunctions/arm_sub_f32.c
BasicMathFunctions/arm_mult_f32.c
TransformFunctions/arm_rfft_fast_f32.c
TransformFunctions/arm_cfft_f32.c

12
cmake/host.cmake
View file

@ -16,7 +16,11 @@ set(CMAKE_CXX_COMPILER "g++${TOOL_EXECUTABLE_SUFFIX}" CACHE INTERNAL "c++ compil
set(CMAKE_OBJCOPY "objcopy${TOOL_EXECUTABLE_SUFFIX}" CACHE INTERNAL "objcopy tool")
set(CMAKE_OBJDUMP "objdump${TOOL_EXECUTABLE_SUFFIX}" CACHE INTERNAL "objdump tool")
set(CMAKE_SIZE "size${TOOL_EXECUTABLE_SUFFIX}" CACHE INTERNAL "size tool")
set(CMAKE_DEBUGGER "gdb${TOOL_EXECUTABLE_SUFFIX}" CACHE INTERNAL "debugger")
if(CMAKE_HOST_APPLE)
set(CMAKE_DEBUGGER "lldb${TOOL_EXECUTABLE_SUFFIX}" CACHE INTERNAL "debugger")
else()
set(CMAKE_DEBUGGER "gdb${TOOL_EXECUTABLE_SUFFIX}" CACHE INTERNAL "debugger")
endif()
set(CMAKE_CPPFILT "c++filt${TOOL_EXECUTABLE_SUFFIX}" CACHE INTERNAL "c++filt")
set(CMAKE_BUILD_TYPE ${CMAKE_BUILD_TYPE} CACHE STRING "Build Type" FORCE)
@ -24,7 +28,11 @@ set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS ${CMAKE_CONFIGURATION_TYPES
set(debug_options "-Og -O0 -g")
set(release_options "-Os -DNDEBUG")
set(relwithdebinfo_options "-ggdb3 ${release_options}")
if(CMAKE_HOST_APPLE)
set(relwithdebinfo_options "-g ${release_options}")
else()
set(relwithdebinfo_options "-ggdb3 ${release_options}")
endif()
set(CMAKE_C_FLAGS_DEBUG ${debug_options} CACHE INTERNAL "c compiler flags debug")
set(CMAKE_CXX_FLAGS_DEBUG ${debug_options} CACHE INTERNAL "c++ compiler flags debug")

2
cmake/main.cmake
View file

@ -83,7 +83,7 @@ function(setup_executable exe name)
set_target_properties(${exe} PROPERTIES
RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/bin"
)
if(IS_RELEASE_BUILD)
if(IS_RELEASE_BUILD AND NOT (CMAKE_HOST_APPLE AND SITL))
set_target_properties(${exe} PROPERTIES
INTERPROCEDURAL_OPTIMIZATION ON
)

8
cmake/sitl.cmake
View file

@ -26,7 +26,7 @@ main_sources(SITL_SRC
)
if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
if(CMAKE_HOST_APPLE)
set(MACOSX ON)
endif()
@ -59,6 +59,12 @@ if(NOT MACOSX)
-Wno-error=maybe-uninitialized
-fsingle-precision-constant
)
if (CMAKE_COMPILER_IS_GNUCC AND NOT CMAKE_C_COMPILER_VERSION VERSION_LESS 12.0)
set(SITL_LINK_OPTIONS ${SITL_LINK_OPTIONS} "-Wl,--no-warn-rwx-segments")
endif()
else()
set(SITL_COMPILE_OPTIONS ${SITL_COMPILE_OPTIONS}
)
endif()
set(SITL_DEFINITIONS

2
cmake/stm32.cmake
View file

@ -16,6 +16,8 @@ set(CMSIS_DSP_DIR "${MAIN_LIB_DIR}/main/CMSIS/DSP")
set(CMSIS_DSP_INCLUDE_DIR "${CMSIS_DSP_DIR}/Include")
set(CMSIS_DSP_SRC
BasicMathFunctions/arm_scale_f32.c
BasicMathFunctions/arm_sub_f32.c
BasicMathFunctions/arm_mult_f32.c
TransformFunctions/arm_rfft_fast_f32.c
TransformFunctions/arm_cfft_f32.c

2
cmake/stm32h7.cmake
View file

@ -149,6 +149,8 @@ main_sources(STM32H7_SRC
drivers/bus_i2c_hal.c
drivers/dma_stm32h7xx.c
drivers/bus_spi_hal_ll.c
drivers/bus_quadspi.c
drivers/bus_quadspi_hal.c
drivers/memprot.h
drivers/memprot_hal.c
drivers/memprot_stm32h7xx.c

32
docs/Buzzer.md
View file

@ -62,12 +62,14 @@ beeper list
giving:
```
Available: RUNTIME_CALIBRATION HW_FAILURE RX_LOST RX_LOST_LANDING DISARMING ARMING ARMING_GPS_FIX BAT_CRIT_LOW BAT_LOW GPS_STATUS RX_SET ACTION_SUCCESS ACTION_FAIL READY_BEEP MULTI_BEEPS DISARM_REPEAT ARMED SYSTEM_INIT ON_USB LAUNCH_MODE CAM_CONNECTION_OPEN CAM_CONNECTION_CLOSED ALL PREFERED
Available: RUNTIME_CALIBRATION HW_FAILURE RX_LOST RX_LOST_LANDING DISARMING ARMING ARMING_GPS_FIX BAT_CRIT_LOW
BAT_LOW GPS_STATUS RX_SET ACTION_SUCCESS ACTION_FAIL READY_BEEP MULTI_BEEPS DISARM_REPEAT ARMED SYSTEM_INIT
ON_USB LAUNCH_MODE CAM_CONNECTION_OPEN CAM_CONNECTION_CLOSED ALL PREFERED
```
The `beeper` command syntax follows that of the `feature` command; a minus (`-`) in front of a name disables that function.
So to disable the beeper / buzzer when connected to USB (may enhance domestic harmony)
So to disable the beeper / buzzer when powered by USB (may enhance domestic harmony):
```
beeper -ON_USB
@ -78,17 +80,39 @@ Now the `beeper` command will show:
```
# beeper
Disabled: ON_USB
```
*Note: SYSTEM_INIT sequence is not affected by ON_USB setting and will still be played on USB connection. Disable both ON_USB and SYSTEM_INIT to disable buzzer completely when FC is powered from USB.*
*Note: ON_USB setting requires present and configured battery voltage metter.*
To disable all features use:
```
beeper -ALL
```
To store current set to preferences use (preferences also require ```save```):
```
beeper PREFERED
```
To restore set from preferences use:
```
beeper -PREFERED
As with other CLI commands, the `save` command is needed to save the new settings.
## Types of buzzer supported
The buzzers are enabled/disabled by simply enabling or disabling a GPIO output pin on the board.
Most FCs require ACTIVE buzzers. Active buzzers are enabled/disabled by simply enabling or disabling a GPIO output pin on the board.
This means the buzzer must be able to generate its own tone simply by having power applied to it.
Buzzers that need an analog or PWM signal do not work and will make clicking noises or no sound at all.
Passive buzzers that need an analog or PWM signal do not work and will make clicking noises or no sound at all.
Passive buzzers are supported on FCs which are designed to work with passive buzzers only (so far there is no available, except rare cases like Matek F765-WSE where passive buzzer is preinstalled).
Examples of a known-working buzzers.

4
docs/Controls.md
View file

@ -44,6 +44,10 @@ The stick positions are combined to activate different functions:
| Bypass Nav Arm disable | LOW | HIGH | CENTER | CENTER |
| Save setting | LOW | LOW | LOW | HIGH |
| Enter OSD Menu (CMS) | CENTER | LOW | HIGH | CENTER |
| Enter Camera OSD(RuncamDevice)| RIGHT | CENTER | CENTER | CENTER |
| Exit Camera OSD (RuncamDevice)| LEFT | CENTER | CENTER | CENTER |
| Confirm - Camera OSD | RIGHT | CENTER | CENTER | CENTER |
| Navigation - Camera OSD | CENTER | 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)

14
docs/Navigation.md
View file

@ -4,11 +4,14 @@ The navigation system in INAV is responsible for assisting the pilot allowing al
## NAV ALTHOLD mode - altitude hold
Altitude hold requires a valid source of altitude - barometer, GPS or rangefinder. The best source is chosen automatically. GPS is available as an altitude source for airplanes only.
Altitude hold requires a valid source of altitude - barometer, GPS or rangefinder. The best source is chosen automatically.
In this mode THROTTLE stick controls climb rate (vertical velocity). When pilot moves stick up - quad goes up, pilot moves stick down - quad descends, you keep stick at neutral position - quad hovers.
By default, GPS is available as an altitude source for airplanes only. Multirotor requires barometer, unless *inav_use_gps_no_baro* is enabled.
### CLI parameters affecting ALTHOLD mode:
* *nav_use_midthr_for_althold* - when set to "0", firmware will remember where your throttle stick was when ALTHOLD was activated - this will be considered neutral position. When set to "1" - 50% throttle will be considered neutral position.
* *inav_use_gps_no_baro* - Multirotor only: enable althold based on GPS only, without baromemer installed. Default is OFF.
### Related PIDs
PIDs affecting altitude hold: ALT & VEL
@ -22,12 +25,13 @@ Throttle tilt compensation attempts to maintain constant vertical thrust when co
## NAV POSHOLD mode - position hold
Position hold requires GPS, accelerometer and compass sensors. Flight modes that require a compass (POSHOLD, RTH) are locked until compass is properly calibrated.
Position hold requires GPS, accelerometer and compass sensors. Multirotor requires barometer, unless *inav_use_gps_no_baro* is enabled. Flight modes that require a compass (POSHOLD, RTH) are locked until compass is properly calibrated.
When activated, this mode will attempt to keep copter where it is (based on GPS coordinates). From INAV 2.0, POSHOLD is a full 3D position hold. Heading hold in this mode is assumed and activated automatically.
### CLI parameters affecting POSHOLD mode:
* *nav_user_control_mode* - can be set to "0" (GPS_ATTI) or "1" (GPS_CRUISE), controls how firmware will respond to roll/pitch stick movement. When in GPS_ATTI mode, right stick controls attitude, when it is released, new position is recorded and held. When in GPS_CRUISE mode right stick controls velocity and firmware calculates required attitude on its own.
* *inav_use_gps_no_baro* - Multirotor only: enable althold based on GPS only, without baromemer installed. Default is OFF.
### Related PIDs
PIDs affecting position hold: POS, POSR
PID meaning:
@ -38,9 +42,9 @@ PID meaning:
Home for RTH is the position where vehicle was first armed. This position may be offset by the CLI settings `nav_rth_home_offset_distance` and `nav_rth_home_offset_direction`. This position may also be overridden with Safehomes. RTH requires accelerometer, compass and GPS sensors.
If barometer is NOT present, RTH will fly directly to home, altitude control here is up to pilot.
RTH requires barometer for multirotor, unless unless *inav_use_gps_no_baro* is enabled.
If barometer is present, RTH will maintain altitude during the return. When home is reached, a copter will attempt automated landing. An airplane will either loiter around the home position, or attempt an automated landing, depending on your settings.
RTH will maintain altitude during the return. When home is reached, a copter will attempt automated landing. An airplane will either loiter around the home position, or attempt an automated landing, depending on your settings.
When deciding what altitude to maintain, RTH has 6 different modes of operation (controlled by *nav_rth_alt_mode* and *nav_rth_altitude* cli variables):
* 0 (NAV_RTH_NO_ALT) - keep current altitude during whole RTH sequence (*nav_rth_altitude* is ignored)
* 1 (NAV_RTH_EXTRA_ALT) - climb to current altitude plus extra margin prior to heading home (*nav_rth_altitude* defines the extra altitude (cm))

167
docs/OSD.md Normal file
View file

@ -0,0 +1,167 @@
# On Screen Display
The On Screen Display, or OSD, is a feature that overlays flight data over the video image. This can be done on the flight controller, using the analogue MAX7456 chip. Digital systems take the OSD data, via MSP DisplayPort, send it to the video receiver; which combines the data with the image. You can specify what elements are displayed, and their locations on the image. Most systems are character based, and use the MAX7456 analogue setup, or MSP DisplayPort. However, there are some different systems which are also supported. Such as the canvas based FrSKY PixelOSD on analogue. Canvas OSDs draw shapes on the image. Whereas character based OSDs use font characters to display the data.
## Features and Limitations
Not all OSDs are created equally. This table shows the differences between the different systems available.
| OSD System | Character grid | Character | Canvas | MSP DisplayPort | All elements supported |
|---------------|-------------------|-----------|-----------|-------------------|---------------------------|
| Analogue PAL | 30 x 16 | X | | | YES |
| Analogue NTSC | 30 x 13 | X | | | YES |
| PixelOSD | As PAL or NTSC | | X | | YES |
| DJI OSD | 30 x 16 | X | | | NO - BF Characters only |
| DJI WTFOS | 60 x 22 | X | | X | YES |
| HDZero | 50 x 18 | X | | X | YES |
| Avatar | 53 x 20 | X | | X | YES |
| DJI O3 | 53 x 20 (HD) | X | | X (partial) | NO - BF Characters only |
## OSD Elements
Here are the OSD Elements provided by INAV.
| ID | Element | Added |
|-------|-----------------------------------------------|-------|
| 0 | OSD_RSSI_VALUE | 1.0.0 |
| 1 | OSD_MAIN_BATT_VOLTAGE | 1.0.0 |
| 2 | OSD_CROSSHAIRS | 1.0.0 |
| 3 | OSD_ARTIFICIAL_HORIZON | 1.0.0 |
| 4 | OSD_HORIZON_SIDEBARS | 1.0.0 |
| 5 | OSD_ONTIME | 1.0.0 |
| 6 | OSD_FLYTIME | 1.0.0 |
| 7 | OSD_FLYMODE | 1.0.0 |
| 8 | OSD_CRAFT_NAME | 1.0.0 |
| 9 | OSD_THROTTLE_POS | 1.0.0 |
| 10 | OSD_VTX_CHANNEL | 1.0.0 |
| 11 | OSD_CURRENT_DRAW | 1.0.0 |
| 12 | OSD_MAH_DRAWN | 1.0.0 |
| 13 | OSD_GPS_SPEED | 1.0.0 |
| 14 | OSD_GPS_SATS | 1.0.0 |
| 15 | OSD_ALTITUDE | 1.0.0 |
| 16 | OSD_ROLL_PIDS | 1.6.0 |
| 17 | OSD_PITCH_PIDS | 1.6.0 |
| 18 | OSD_YAW_PIDS | 1.6.0 |
| 19 | OSD_POWER | 1.6.0 |
| 20 | OSD_GPS_LON | 1.6.0 |
| 21 | OSD_GPS_LAT | 1.6.0 |
| 22 | OSD_HOME_DIR | 1.6.0 |
| 23 | OSD_HOME_DIST | 1.6.0 |
| 24 | OSD_HEADING | 1.6.0 |
| 25 | OSD_VARIO | 1.6.0 |
| 26 | OSD_VARIO_NUM | 1.6.0 |
| 27 | OSD_AIR_SPEED | 1.7.3 |
| 28 | OSD_ONTIME_FLYTIME | 1.8.0 |
| 29 | OSD_RTC_TIME | 1.8.0 |
| 30 | OSD_MESSAGES | 1.8.0 |
| 31 | OSD_GPS_HDOP | 1.8.0 |
| 32 | OSD_MAIN_BATT_CELL_VOLTAGE | 1.8.0 |
| 33 | OSD_SCALED_THROTTLE_POS | 1.8.0 |
| 34 | OSD_HEADING_GRAPH | 1.8.0 |
| 35 | OSD_EFFICIENCY_MAH_PER_KM | 1.9.0 |
| 36 | OSD_WH_DRAWN | 1.9.0 |
| 37 | OSD_BATTERY_REMAINING_CAPACITY | 1.9.0 |
| 38 | OSD_BATTERY_REMAINING_PERCENT | 1.9.0 |
| 39 | OSD_EFFICIENCY_WH_PER_KM | 1.9.0 |
| 40 | OSD_TRIP_DIST | 1.9.1 |
| 41 | OSD_ATTITUDE_PITCH | 2.0.0 |
| 42 | OSD_ATTITUDE_ROLL | 2.0.0 |
| 43 | OSD_MAP_NORTH | 2.0.0 |
| 44 | OSD_MAP_TAKEOFF | 2.0.0 |
| 45 | OSD_RADAR | 2.0.0 |
| 46 | OSD_WIND_SPEED_HORIZONTAL | 2.0.0 |
| 47 | OSD_WIND_SPEED_VERTICAL | 2.0.0 |
| 48 | OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH | 2.0.0 |
| 49 | OSD_REMAINING_DISTANCE_BEFORE_RTH | 2.0.0 |
| 50 | OSD_HOME_HEADING_ERROR | 2.0.0 |
| 51 | OSD_COURSE_HOLD_ERROR | 2.0.0 |
| 52 | OSD_COURSE_HOLD_ADJUSTMENT | 2.0.0 |
| 53 | OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE | 2.0.0 |
| 54 | OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE | 2.0.0 |
| 55 | OSD_POWER_SUPPLY_IMPEDANCE | 2.0.0 |
| 56 | OSD_LEVEL_PIDS | 2.0.0 |
| 57 | OSD_POS_XY_PIDS | 2.0.0 |
| 58 | OSD_POS_Z_PIDS | 2.0.0 |
| 59 | OSD_VEL_XY_PIDS | 2.0.0 |
| 60 | OSD_VEL_Z_PIDS | 2.0.0 |
| 61 | OSD_HEADING_P | 2.0.0 |
| 62 | OSD_BOARD_ALIGN_ROLL | 2.0.0 |
| 63 | OSD_BOARD_ALIGN_PITCH | 2.0.0 |
| 64 | OSD_RC_EXPO | 2.0.0 |
| 65 | OSD_RC_YAW_EXPO | 2.0.0 |
| 66 | OSD_THROTTLE_EXPO | 2.0.0 |
| 67 | OSD_PITCH_RATE | 2.0.0 |
| 68 | OSD_ROLL_RATE | 2.0.0 |
| 69 | OSD_YAW_RATE | 2.0.0 |
| 70 | OSD_MANUAL_RC_EXPO | 2.0.0 |
| 71 | OSD_MANUAL_RC_YAW_EXPO | 2.0.0 |
| 72 | OSD_MANUAL_PITCH_RATE | 2.0.0 |
| 73 | OSD_MANUAL_ROLL_RATE | 2.0.0 |
| 74 | OSD_MANUAL_YAW_RATE | 2.0.0 |
| 75 | OSD_NAV_FW_CRUISE_THR | 2.0.0 |
| 76 | OSD_NAV_FW_PITCH2THR | 2.0.0 |
| 77 | OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE | 2.0.0 |
| 78 | OSD_DEBUG | 2.0.0 |
| 79 | OSD_FW_ALT_PID_OUTPUTS | 2.0.0 |
| 80 | OSD_FW_POS_PID_OUTPUTS | 2.0.0 |
| 81 | OSD_MC_VEL_X_PID_OUTPUTS | 2.0.0 |
| 82 | OSD_MC_VEL_Y_PID_OUTPUTS | 2.0.0 |
| 83 | OSD_MC_VEL_Z_PID_OUTPUTS | 2.0.0 |
| 84 | OSD_MC_POS_XYZ_P_OUTPUTS | 2.0.0 |
| 85 | OSD_3D_SPEED | 2.1.0 |
| 86 | OSD_IMU_TEMPERATURE | 2.1.0 |
| 87 | OSD_BARO_TEMPERATURE | 2.1.0 |
| 88 | OSD_TEMP_SENSOR_0_TEMPERATURE | 2.1.0 |
| 89 | OSD_TEMP_SENSOR_1_TEMPERATURE | 2.1.0 |
| 90 | OSD_TEMP_SENSOR_2_TEMPERATURE | 2.1.0 |
| 91 | OSD_TEMP_SENSOR_3_TEMPERATURE | 2.1.0 |
| 92 | OSD_TEMP_SENSOR_4_TEMPERATURE | 2.1.0 |
| 93 | OSD_TEMP_SENSOR_5_TEMPERATURE | 2.1.0 |
| 94 | OSD_TEMP_SENSOR_6_TEMPERATURE | 2.1.0 |
| 95 | OSD_TEMP_SENSOR_7_TEMPERATURE | 2.1.0 |
| 96 | OSD_ALTITUDE_MSL | 2.1.0 |
| 97 | OSD_PLUS_CODE | 2.1.0 |
| 98 | OSD_MAP_SCALE | 2.2.0 |
| 99 | OSD_MAP_REFERENCE | 2.2.0 |
| 100 | OSD_GFORCE | 2.2.0 |
| 101 | OSD_GFORCE_X | 2.2.0 |
| 102 | OSD_GFORCE_Y | 2.2.0 |
| 103 | OSD_GFORCE_Z | 2.2.0 |
| 104 | OSD_RC_SOURCE | 2.2.0 |
| 105 | OSD_VTX_POWER | 2.2.0 |
| 106 | OSD_ESC_RPM | 2.3.0 |
| 107 | OSD_ESC_TEMPERATURE | 2.5.0 |
| 108 | OSD_AZIMUTH | 2.6.0 |
| 109 | OSD_CRSF_RSSI_DBM | 2.6.0 |
| 110 | OSD_CRSF_LQ | 2.6.0 |
| 111 | OSD_CRSF_SNR_DB | 2.6.0 |
| 112 | OSD_CRSF_TX_POWER | 2.6.0 |
| 113 | OSD_GVAR_0 | 2.6.0 |
| 114 | OSD_GVAR_1 | 2.6.0 |
| 115 | OSD_GVAR_2 | 2.6.0 |
| 116 | OSD_GVAR_3 | 2.6.0 |
| 117 | OSD_TPA | 2.6.0 |
| 118 | OSD_NAV_FW_CONTROL_SMOOTHNESS | 2.6.0 |
| 119 | OSD_VERSION | 3.0.0 |
| 120 | OSD_RANGEFINDER | 3.0.0 |
| 121 | OSD_PLIMIT_REMAINING_BURST_TIME | 3.0.0 |
| 122 | OSD_PLIMIT_ACTIVE_CURRENT_LIMIT | 3.0.0 |
| 123 | OSD_PLIMIT_ACTIVE_POWER_LIMIT | 3.0.0 |
| 124 | OSD_GLIDESLOPE | 3.0.1 |
| 125 | OSD_GPS_MAX_SPEED | 4.0.0 |
| 126 | OSD_3D_MAX_SPEED | 4.0.0 |
| 127 | OSD_AIR_MAX_SPEED | 4.0.0 |
| 128 | OSD_ACTIVE_PROFILE | 4.0.0 |
| 129 | OSD_MISSION | 4.0.0 |
| 130 | OSD_SWITCH_INDICATOR_0 | 5.0.0 |
| 131 | OSD_SWITCH_INDICATOR_1 | 5.0.0 |
| 132 | OSD_SWITCH_INDICATOR_2 | 5.0.0 |
| 133 | OSD_SWITCH_INDICATOR_3 | 5.0.0 |
| 134 | OSD_TPA_TIME_CONSTANT | 5.0.0 |
| 135 | OSD_FW_LEVEL_TRIM | 5.0.0 |
| 136 | OSD_GLIDE_TIME_REMAINING | 6.0.0 |
| 137 | OSD_GLIDE_RANGE | 6.0.0 |
| 138 | OSD_CLIMB_EFFICIENCY | 6.0.0 |
| 139 | OSD_NAV_WP_MULTI_MISSION_INDEX | 6.0.0 |
| 140 | OSD_GROUND_COURSE | 6.0.0 |
| 141 | OSD_CROSS_TRACK_ERROR | 6.0.0 |
| 142 | OSD_PILOT_NAME | 6.0.0 |
| 143 | OSD_PAN_SERVO_CENTRED | 6.0.0 |

40
docs/Settings.md
View file

@ -1432,6 +1432,26 @@ Which SBAS mode to be used
---
### gps_ublox_nav_hz
Navigation update rate for UBLOX7 receivers. Some receivers may limit the maximum number of satellites tracked when set to a higher rate or even stop sending navigation updates if the value is too high. Some M10 devices can do up to 25Hz. 10 is a safe value for M8 and newer.
| Default | Min | Max |
| --- | --- | --- |
| 10 | 5 | 200 |
---
### gps_ublox_use_beidou
Enable use of Beidou satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires gps hardware support [OFF/ON].
| Default | Min | Max |
| --- | --- | --- |
| OFF | OFF | ON |
---
### gps_ublox_use_galileo
Enable use of Galileo satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires M8N and Ublox firmware 3.x (or later) [OFF/ON].
@ -1442,6 +1462,16 @@ Enable use of Galileo satellites. This is at the expense of other regional const
---
### gps_ublox_use_glonass
Enable use of Glonass satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires gps haardware support [OFF/ON].
| Default | Min | Max |
| --- | --- | --- |
| OFF | OFF | ON |
---
### ground_test_mode
For developer ground test use. Disables motors, sets heading status = Trusted on FW.
@ -3202,6 +3232,16 @@ Defines at what altitude the descent velocity should start to be `nav_land_minal
---
### nav_landing_bump_detection
Allows immediate landing detection based on G bump at touchdown when set to ON. Requires a barometer and currently only works for multirotors.
| Default | Min | Max |
| --- | --- | --- |
| OFF | OFF | ON |
---
### nav_manual_climb_rate
Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s]

BIN
docs/assets/images/StickPositions.png
View file

Binary file not shown.
Side by side Swipe Overlay

Before

Width:  |  Height:  |  Size: 718 KiB

After

Width:  |  Height:  |  Size: 774 KiB

Before After
Before After

1042
docs/assets/images/StickPositions.svg
View file

File diff suppressed because it is too large Load diff
Side by side

Before

Width:  |  Height:  |  Size: 54 KiB

BIN
docs/assets/images/StickPositions_trans.png
View file

Binary file not shown.
Side by side

Before

Width:  |  Height:  |  Size: 819 KiB

53
docs/development/How to test a pull request.md Normal file
View file

@ -0,0 +1,53 @@
# Introduction
While many of the instructions here are somewhat generic and will likely work for other projects, the goal of these instructions is to assist a non-developer INAV user to acquire firmware that includes a pull request so he can flash it on his supported fc.
Building the pull request manually or using custom/unofficial targets is not the focus of this document.
# Why should you test a pull request?
- You want to volunteer time and resources helping improving INAV for everyone by catching issues before they are introduced in the next release of INAV!
- You reported or are affected by a bug that has been addressed by this pull request and want to help test the fix
- You are interested in testing a new feature implemented by this pull request
# Why should you not test a pull request?
- Pull requests are beta code and may have bugs; bugs may cause you to crash and damage your model
- Upgrading from the stable version of INAV may require changes to your config that are not yet fully documented
# Before you proceed
- Read the comments on the pull request you want to test. It may provide useful context and information on known issues, required configuration changes or what branch of the inav-configurator is required.
- Make sure the pull request has passed all checks, otherwise you may not have pre-compiled firmware images.
- Make a diff all backup of your existing INAV configuration.
- Take notes of what INAV target you are using.
- You will need a recent version of INAV Configurator from master, or even a specific branch. If you don't need a specific branch, [inav-configurator-next](http://seyrsnys.myzen.co.uk/inav-configurator-next/) usually has recent unofficial pre-built versions of INAV Configurator. If your pull requests refers to an inav-configruator pull request, you are likely to need a specific branch of the configurator. In that case you can try to build it from source by following the build [``Instructions``](https://github.com/iNavFlight/inav-configurator#building-and-running-inav-configurator-locally-for-development) or follow instructions on how to do any needed configuration changes using the CLI.
# Finding the pull request
This is easy, but you will need to be logged in to your GitHub account.
Navigate to the INAV github project and click on [``Pull Requests``](https://github.com/iNavFlight/inav/pulls).
You can just scroll through the list to find a pull request you are interested in, or use the filter bar by typing the name of the pull request, or the number, if you know it.
![Search results](assets/pr_testing/pr_search_result.png)
Once you find the one you are looking for, go ahead an open it!
Click on the ``Checks`` tab
Click on ``Build firmware``, it should take you to the ``Actions`` tab.
![Search results](assets/pr_testing/build_firmware.png)
You should see a summary with a column saying ``Artifacts`` and a number. Click on the number to be taken to the list of artifacts.
![Search results](assets/pr_testing/actions_summary.png)
On the ``Artifacts`` list, there should be an artifact without SITL in its name.
![Search results](assets/pr_testing/artifact_listing.png)
Click on it to download the zip file containing pre-compiled firmware images for all INAV official targets. Extract all files and select the firmware for your target using the configurator by clicking on ``Load Firmware [Local]`` button. Don't forget to use the ``Full chip erase`` option, as there are no guarantees the firmware will be compatible with your existing settings.
# I have flashed the new firmware, what should I do next?
- You should configure your model, either manually from scratch, or by loading your diff file. Keep in mind that loading a diff file may not always work, as there may have been some other changes in INAV that require attention. But even if you start from scratch, there are usually many sections that are safe to copy over from your diff.
- Try to reproduce the bug reported or play around with the new feature.
- Once you are done testing, don't forget to report your results on the pull request. Both positive results and issues are valid and welcome feedback.

BIN
docs/development/assets/pr_testing/actions_summary.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 12 KiB

BIN
docs/development/assets/pr_testing/artifact_listing.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 71 KiB

BIN
docs/development/assets/pr_testing/build_firmware.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 32 KiB

BIN
docs/development/assets/pr_testing/pr_search_result.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 26 KiB

3
lib/main/MAVLink/mavlink_helpers.h
View file

@ -574,8 +574,6 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg,
mavlink_message_t* r_message,
mavlink_status_t* r_mavlink_status)
{
int bufferIndex = 0;
status->msg_received = MAVLINK_FRAMING_INCOMPLETE;
switch (status->parse_state)
@ -801,7 +799,6 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg,
break;
}
bufferIndex++;
// If a message has been sucessfully decoded, check index
if (status->msg_received == MAVLINK_FRAMING_OK)
{

22
readme.md
View file

@ -2,10 +2,6 @@
![INAV](http://static.rcgroups.net/forums/attachments/6/1/0/3/7/6/a9088858-102-inav.png)
# INAV 6 Horizon Hawk feature freeze
> INAV 6 feature freeze will happen on 29th of January 2023. No new features for INAV 6 will be accepted after that date.
# INAV Community
* [INAV Discord Server](https://discord.gg/peg2hhbYwN)
@ -18,13 +14,13 @@
* On Screen Display (OSD) - both character and pixel style
* DJI OSD integration: all elements, system messages and warnings
* Outstanding performance out of the box
* Position Hold, Altitude Hold, Return To Home and Missions
* Excellent support for fixed wing UAVs: airplanes, flying wings
* Position Hold, Altitude Hold, Return To Home and Waypoint Missions
* Excellent support for fixed wing UAVs: airplanes, flying wings
* 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
* Blackbox flight recorder logging
* Telemetry: SmartPort, FPort, MAVlink, LTM
* Telemetry: SmartPort, FPort, MAVlink, LTM, CRSF
* Multi-color RGB LED Strip support
* Advanced gyro filtering
* Logic Conditions, Global Functions and Global Variables: you can program INAV with a GUI
@ -36,15 +32,19 @@ For a list of features, changes and some discussion please review consult the re
### INAV Configurator
Official tool for INAV can be downloaded [here](https://github.com/iNavFlight/inav-configurator/releases). It can be run on Windows, MacOS and Linux machines and standalone application.
Official tool for INAV can be downloaded [here](https://github.com/iNavFlight/inav-configurator/releases). It can be run on Windows, MacOS and Linux machines and standalone application.
### INAV Blackbox Explorer
Tool for Blackbox logs analysis is available [here](https://github.com/iNavFlight/blackbox-log-viewer/releases)
### Telemetry screen for OpenTX
### INAV Blackbox Tools
Users of OpenTX radios (Taranis, Horus, Jumper, Radiomaster, Nirvana) can use INAV OpenTX Telemetry Widget screen. Software and installation instruction are available here: [https://github.com/iNavFlight/OpenTX-Telemetry-Widget](https://github.com/iNavFlight/OpenTX-Telemetry-Widget)
Command line tools (`blackbox_decode`, `blackbox_render`) for Blackbox log conversion and analysis [here](https://github.com/iNavFlight/blackbox-tools).
### Telemetry screen for EdgeTX and OpenTX
Users of EdgeTX and OpenTX radios (Taranis, Horus, Jumper, Radiomaster, Nirvana) can use INAV OpenTX Telemetry Widget screen. Software and installation instruction are available here: [https://github.com/iNavFlight/OpenTX-Telemetry-Widget](https://github.com/iNavFlight/OpenTX-Telemetry-Widget)
### INAV magnetometer alignment helper
@ -81,7 +81,7 @@ Contributions are welcome and encouraged. You can contribute in many ways:
* Telling us your ideas and suggestions.
* Buying your hardware from this [link](https://inavflight.com/shop/u/bg/)
A good place to start is Telegram channel or Facebook group. Drop in, say hi.
A good place to start is the Discord channel, Telegram channel or Facebook group. Drop in, say hi.
Github issue tracker is a good place to search for existing issues or report a new bug/feature request:

3
src/main/CMakeLists.txt
View file

@ -499,6 +499,7 @@ main_sources(COMMON_SRC
io/gps.c
io/gps.h
io/gps_ublox.c
io/gps_ublox_utils.c
io/gps_nmea.c
io/gps_msp.c
io/gps_fake.c
@ -531,6 +532,8 @@ main_sources(COMMON_SRC
io/vtx_ffpv24g.h
io/vtx_control.c
io/vtx_control.h
io/vtx_msp.c
io/vtx_msp.h
navigation/navigation.c
navigation/navigation.h

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

@ -1326,12 +1326,19 @@ static void loadSlowState(blackboxSlowState_t *slow)
#endif
bool valid_temp;
valid_temp = getIMUTemperature(&slow->imuTemperature);
if (!valid_temp) slow->imuTemperature = TEMPERATURE_INVALID_VALUE;
int16_t newTemp = 0;
valid_temp = getIMUTemperature(&newTemp);
if (valid_temp)
slow->imuTemperature = newTemp;
else
slow->imuTemperature = TEMPERATURE_INVALID_VALUE;
#ifdef USE_BARO
valid_temp = getBaroTemperature(&slow->baroTemperature);
if (!valid_temp) slow->baroTemperature = TEMPERATURE_INVALID_VALUE;
valid_temp = getBaroTemperature(&newTemp);
if (valid_temp)
slow->baroTemperature = newTemp;
else
slow->baroTemperature = TEMPERATURE_INVALID_VALUE;
#endif
#ifdef USE_TEMPERATURE_SENSOR

7
src/main/cms/cms_menu_osd.c
View file

@ -330,13 +330,6 @@ static const OSD_Entry menuOsdElemsEntries[] =
OSD_BACK_AND_END_ENTRY,
};
#if defined(USE_GPS) && defined(USE_BARO) && defined(USE_PITOT) && defined(USE_TEMPERATURE_SENSOR) && defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
// All CMS OSD elements should be enabled in this case. The menu has 2 extra
// elements (label, back+end), but there's an OSD element that we intentionally
// don't show here (OSD_DEBUG).
_Static_assert(ARRAYLEN(menuOsdElemsEntries) - 2 + 1 == OSD_ITEM_COUNT, "missing OSD elements in CMS");
#endif
const CMS_Menu menuOsdElements = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "MENUOSDE",

2
src/main/cms/cms_types.h
View file

@ -192,7 +192,7 @@ typedef struct OSD_SETTING_s {
const uint8_t step;
} __attribute__((packed)) OSD_SETTING_t;
#define OSD_SETTING_ENTRY_STEP_TYPE(name, setting, step, type) { name, NULL, &(const OSD_SETTING_t){ setting, step }, OME_Setting, type }
#define OSD_SETTING_ENTRY_STEP_TYPE(name, setting, step, type) { name, { NULL }, &(const OSD_SETTING_t){ setting, step }, OME_Setting, type }
#define OSD_SETTING_ENTRY_TYPE(name, setting, type) OSD_SETTING_ENTRY_STEP_TYPE(name, setting, 0, type)
#define OSD_SETTING_ENTRY_STEP(name, setting, step) OSD_SETTING_ENTRY_STEP_TYPE(name, setting, step, 0)
#define OSD_SETTING_ENTRY(name, setting) OSD_SETTING_ENTRY_STEP(name, setting, 0)

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

@ -18,9 +18,9 @@
#pragma once
#define FEET_PER_MILE 5280
#define FEET_PER_NAUTICALMILE 6076.118
#define FEET_PER_NAUTICALMILE 6076.118f
#define FEET_PER_KILOFEET 1000 // Used for altitude
#define METERS_PER_KILOMETER 1000
#define METERS_PER_MILE 1609.344
#define METERS_PER_FOOT 3.28084
#define METERS_PER_NAUTICALMILE 1852.001
#define METERS_PER_MILE 1609.344f
#define METERS_PER_FOOT 3.28084f
#define METERS_PER_NAUTICALMILE 1852.001f

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

@ -380,36 +380,36 @@ void sensorCalibrationResetState(sensorCalibrationState_t * state)
}
}
void sensorCalibrationPushSampleForOffsetCalculation(sensorCalibrationState_t * state, int32_t sample[3])
void sensorCalibrationPushSampleForOffsetCalculation(sensorCalibrationState_t * state, float sample[3])
{
state->XtX[0][0] += (float)sample[0] * sample[0];
state->XtX[0][1] += (float)sample[0] * sample[1];
state->XtX[0][2] += (float)sample[0] * sample[2];
state->XtX[0][3] += (float)sample[0];
state->XtX[0][0] += sample[0] * sample[0];
state->XtX[0][1] += sample[0] * sample[1];
state->XtX[0][2] += sample[0] * sample[2];
state->XtX[0][3] += sample[0];
state->XtX[1][0] += (float)sample[1] * sample[0];
state->XtX[1][1] += (float)sample[1] * sample[1];
state->XtX[1][2] += (float)sample[1] * sample[2];
state->XtX[1][3] += (float)sample[1];
state->XtX[1][0] += sample[1] * sample[0];
state->XtX[1][1] += sample[1] * sample[1];
state->XtX[1][2] += sample[1] * sample[2];
state->XtX[1][3] += sample[1];
state->XtX[2][0] += (float)sample[2] * sample[0];
state->XtX[2][1] += (float)sample[2] * sample[1];
state->XtX[2][2] += (float)sample[2] * sample[2];
state->XtX[2][3] += (float)sample[2];
state->XtX[2][0] += sample[2] * sample[0];
state->XtX[2][1] += sample[2] * sample[1];
state->XtX[2][2] += sample[2] * sample[2];
state->XtX[2][3] += sample[2];
state->XtX[3][0] += (float)sample[0];
state->XtX[3][1] += (float)sample[1];
state->XtX[3][2] += (float)sample[2];
state->XtX[3][0] += sample[0];
state->XtX[3][1] += sample[1];
state->XtX[3][2] += sample[2];
state->XtX[3][3] += 1;
float squareSum = ((float)sample[0] * sample[0]) + ((float)sample[1] * sample[1]) + ((float)sample[2] * sample[2]);
float squareSum = (sample[0] * sample[0]) + (sample[1] * sample[1]) + (sample[2] * sample[2]);
state->XtY[0] += sample[0] * squareSum;
state->XtY[1] += sample[1] * squareSum;
state->XtY[2] += sample[2] * squareSum;
state->XtY[3] += squareSum;
}
void sensorCalibrationPushSampleForScaleCalculation(sensorCalibrationState_t * state, int axis, int32_t sample[3], int target)
void sensorCalibrationPushSampleForScaleCalculation(sensorCalibrationState_t * state, int axis, float sample[3], int target)
{
for (int i = 0; i < 3; i++) {
float scaledSample = (float)sample[i] / (float)target;
@ -521,7 +521,7 @@ float bellCurve(const float x, const float curveWidth)
return powf(M_Ef, -sq(x) / (2.0f * sq(curveWidth)));
}
float fast_fsqrtf(const double value) {
float fast_fsqrtf(const float value) {
float ret = 0.0f;
#ifdef USE_ARM_MATH
arm_sqrt_f32(value, &ret);
@ -538,11 +538,57 @@ float fast_fsqrtf(const double value) {
// function to calculate the normalization (pythagoras) of a 2-dimensional vector
float NOINLINE calc_length_pythagorean_2D(const float firstElement, const float secondElement)
{
return fast_fsqrtf(sq(firstElement) + sq(secondElement));
return fast_fsqrtf(sq(firstElement) + sq(secondElement));
}
// function to calculate the normalization (pythagoras) of a 3-dimensional vector
float NOINLINE calc_length_pythagorean_3D(const float firstElement, const float secondElement, const float thirdElement)
{
return fast_fsqrtf(sq(firstElement) + sq(secondElement) + sq(thirdElement));
}
return fast_fsqrtf(sq(firstElement) + sq(secondElement) + sq(thirdElement));
}
#ifdef SITL_BUILD
/**
* @brief Floating-point vector subtraction, equivalent of CMSIS arm_sub_f32.
*/
void arm_sub_f32(
float * pSrcA,
float * pSrcB,
float * pDst,
uint32_t blockSize)
{
for (uint32_t i = 0; i < blockSize; i++) {
pDst[i] = pSrcA[i] - pSrcB[i];
}
}
/**
* @brief Floating-point vector scaling, equivalent of CMSIS arm_scale_f32.
*/
void arm_scale_f32(
float * pSrc,
float scale,
float * pDst,
uint32_t blockSize)
{
for (uint32_t i = 0; i < blockSize; i++) {
pDst[i] = pSrc[i] * scale;
}
}
/**
* @brief Floating-point vector multiplication, equivalent of CMSIS arm_mult_f32.
*/
void arm_mult_f32(
float * pSrcA,
float * pSrcB,
float * pDst,
uint32_t blockSize)
{
for (uint32_t i = 0; i < blockSize; i++) {
pDst[i] = pSrcA[i] * pSrcB[i];
}
}
#endif

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

@ -36,13 +36,13 @@
#define RAD (M_PIf / 180.0f)
#define DEGREES_TO_CENTIDEGREES(angle) ((angle) * 100)
#define CENTIDEGREES_TO_DEGREES(angle) ((angle) / 100)
#define CENTIDEGREES_TO_DEGREES(angle) ((angle) / 100.0f)
#define CENTIDEGREES_TO_DECIDEGREES(angle) ((angle) / 10)
#define CENTIDEGREES_TO_DECIDEGREES(angle) ((angle) / 10.0f)
#define DECIDEGREES_TO_CENTIDEGREES(angle) ((angle) * 10)
#define DEGREES_TO_DECIDEGREES(angle) ((angle) * 10)
#define DECIDEGREES_TO_DEGREES(angle) ((angle) / 10)
#define DECIDEGREES_TO_DEGREES(angle) ((angle) / 10.0f)
#define DEGREES_PER_DEKADEGREE 10
#define DEGREES_TO_DEKADEGREES(angle) ((angle) / DEGREES_PER_DEKADEGREE)
@ -56,15 +56,15 @@
#define RADIANS_TO_CENTIDEGREES(angle) (((angle) * 100.0f) / RAD)
#define CENTIDEGREES_TO_RADIANS(angle) (((angle) / 100.0f) * RAD)
#define CENTIMETERS_TO_CENTIFEET(cm) (cm / 0.3048)
#define CENTIMETERS_TO_FEET(cm) (cm / 30.48)
#define CENTIMETERS_TO_METERS(cm) (cm / 100)
#define CENTIMETERS_TO_CENTIFEET(cm) (cm / 0.3048f)
#define CENTIMETERS_TO_FEET(cm) (cm / 30.48f)
#define CENTIMETERS_TO_METERS(cm) (cm / 100.0f)
#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)
#define CMSEC_TO_CENTIMPH(cms) (cms * 2.2369363f)
#define CMSEC_TO_CENTIKPH(cms) (cms * 3.6f)
#define CMSEC_TO_CENTIKNOTS(cms) (cms * 1.943845f)
#define C_TO_KELVIN(temp) (temp + 273.15f)
@ -137,8 +137,8 @@ typedef struct {
} sensorCalibrationState_t;
void sensorCalibrationResetState(sensorCalibrationState_t * state);
void sensorCalibrationPushSampleForOffsetCalculation(sensorCalibrationState_t * state, int32_t sample[3]);
void sensorCalibrationPushSampleForScaleCalculation(sensorCalibrationState_t * state, int axis, int32_t sample[3], int target);
void sensorCalibrationPushSampleForOffsetCalculation(sensorCalibrationState_t * state, float sample[3]);
void sensorCalibrationPushSampleForScaleCalculation(sensorCalibrationState_t * state, int axis, float sample[3], int target);
bool sensorCalibrationSolveForOffset(sensorCalibrationState_t * state, float result[3]);
bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float result[3]);
@ -188,6 +188,23 @@ float acos_approx(float x);
void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count);
float bellCurve(const float x, const float curveWidth);
float fast_fsqrtf(const double value);
float fast_fsqrtf(const float value);
float calc_length_pythagorean_2D(const float firstElement, const float secondElement);
float calc_length_pythagorean_3D(const float firstElement, const float secondElement, const float thirdElement);
/*
* The most significat byte is placed at the lowest address
* in other words, the most significant byte is "first", on even indexes
*/
#define int16_val_big_endian(v, idx) ((int16_t)(((uint8_t)v[2 * idx] << 8) | v[2 * idx + 1]))
/*
* The most significat byte is placed at the highest address
* in other words, the most significant byte is "last", on odd indexes
*/
#define int16_val_little_endian(v, idx) ((int16_t)(((uint8_t)v[2 * idx + 1] << 8) | v[2 * idx]))
#ifdef SITL_BUILD
void arm_sub_f32(float * pSrcA, float * pSrcB, float * pDst, uint32_t blockSize);
void arm_scale_f32(float * pSrc, float scale, float * pDst, uint32_t blockSize);
void arm_mult_f32(float * pSrcA, float * pSrcB, float * pDst, uint32_t blockSize);
#endif

2
src/main/common/memory.c
View file

@ -52,7 +52,7 @@ void * memAllocate(size_t wantedSize, resourceOwner_e owner)
retPointer = &dynHeap[dynHeapFreeWord];
dynHeapFreeWord += wantedWords;
dynHeapUsage[owner] += wantedWords * sizeof(uint32_t);
LOG_DEBUG(SYSTEM, "Memory allocated. Free memory = %d", memGetAvailableBytes());
LOG_DEBUG(SYSTEM, "Memory allocated. Free memory = %ld", (unsigned long)memGetAvailableBytes());
}
else {
// OOM

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

@ -58,7 +58,7 @@ static inline void quaternionToAxisAngle(fpAxisAngle_t * result, const fpQuatern
a.angle -= 2.0f * M_PIf;
}
const float sinVal = sqrt(1.0f - q->q0 * q->q0);
const float sinVal = sqrtf(1.0f - q->q0 * q->q0);
// Axis is only valid when rotation is large enough sin(0.0057 deg) = 0.0001
if (sinVal > 1e-4f) {

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

@ -90,10 +90,9 @@ static inline int32_t cmp32(uint32_t a, uint32_t b) { return a-b; }
// using memcpy_fn will force memcpy function call, instead of inlining it. In most cases function call takes fewer instructions
// than inlined version (inlining is cheaper for very small moves < 8 bytes / 2 store instructions)
#ifdef UNIT_TEST
// Call memcpy when building unittest - this is easier that asm symbol name mangling (symbols start with _underscore on win32)
#if defined(UNIT_TEST) || defined(SITL_BUILD)
#include <string.h>
static inline void memcpy_fn(void *destination, const void *source, size_t num) { memcpy(destination, source, num); };
#define memcpy_fn(destination, source, num) memcpy(destination, source, num)
#else
void *memcpy_fn(void *destination, const void *source, size_t num) asm("memcpy");
#endif

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

@ -44,8 +44,8 @@ typedef struct gyroDev_s {
sensorGyroInterruptStatusFuncPtr intStatusFn;
sensorGyroUpdateFuncPtr updateFn;
float scale; // scalefactor
int16_t gyroADCRaw[XYZ_AXIS_COUNT];
int16_t gyroZero[XYZ_AXIS_COUNT];
float gyroADCRaw[XYZ_AXIS_COUNT];
float gyroZero[XYZ_AXIS_COUNT];
uint8_t imuSensorToUse;
uint8_t lpf; // Configuration value: Hardware LPF setting
uint32_t requestedSampleIntervalUs; // Requested sample interval
@ -59,7 +59,7 @@ typedef struct accDev_s {
sensorAccInitFuncPtr initFn; // initialize function
sensorAccReadFuncPtr readFn; // read 3 axis data function
uint16_t acc_1G;
int16_t ADCRaw[XYZ_AXIS_COUNT];
float ADCRaw[XYZ_AXIS_COUNT];
uint8_t imuSensorToUse;
sensor_align_e accAlign;
} accDev_t;

12
src/main/drivers/accgyro/accgyro_bmi088.c
View file

@ -136,9 +136,9 @@ static bool bmi088GyroRead(gyroDev_t *gyro)
uint8_t gyroRaw[6];
if (busReadBuf(gyro->busDev, REGG_RATE_X_LSB, gyroRaw, 6)) {
gyro->gyroADCRaw[X] = (int16_t)((gyroRaw[1] << 8) | gyroRaw[0]);
gyro->gyroADCRaw[Y] = (int16_t)((gyroRaw[3] << 8) | gyroRaw[2]);
gyro->gyroADCRaw[Z] = (int16_t)((gyroRaw[5] << 8) | gyroRaw[4]);
gyro->gyroADCRaw[X] = (float) int16_val_little_endian(gyroRaw, 0);
gyro->gyroADCRaw[Y] = (float) int16_val_little_endian(gyroRaw, 1);
gyro->gyroADCRaw[Z] = (float) int16_val_little_endian(gyroRaw, 2);
return true;
}
@ -151,9 +151,9 @@ static bool bmi088AccRead(accDev_t *acc)
if (busReadBuf(acc->busDev, REGA_STATUS, buffer, 2) && (buffer[1] & 0x80) && busReadBuf(acc->busDev, REGA_X_LSB, buffer, 7)) {
// first byte is discarded, see datasheet
acc->ADCRaw[X] = (int32_t)(((int16_t)(buffer[2] << 8) | buffer[1]) * 3 / 4);
acc->ADCRaw[Y] = (int32_t)(((int16_t)(buffer[4] << 8) | buffer[3]) * 3 / 4);
acc->ADCRaw[Z] = (int32_t)(((int16_t)(buffer[6] << 8) | buffer[5]) * 3 / 4);
acc->ADCRaw[X] = (float)(((int16_t)(buffer[2] << 8) | buffer[1]) * 3 / 4);
acc->ADCRaw[Y] = (float)(((int16_t)(buffer[4] << 8) | buffer[3]) * 3 / 4);
acc->ADCRaw[Z] = (float)(((int16_t)(buffer[6] << 8) | buffer[5]) * 3 / 4);
return true;
}

12
src/main/drivers/accgyro/accgyro_bmi160.c
View file

@ -180,9 +180,9 @@ bool bmi160GyroReadScratchpad(gyroDev_t *gyro)
ctx->lastReadStatus = busReadBuf(gyro->busDev, BMI160_REG_GYR_DATA_X_LSB, ctx->gyroRaw, 6 + 6);
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]);
gyro->gyroADCRaw[X] = (float) int16_val_little_endian(ctx->gyroRaw, 0);
gyro->gyroADCRaw[Y] = (float) int16_val_little_endian(ctx->gyroRaw, 1);
gyro->gyroADCRaw[Z] = (float) int16_val_little_endian(ctx->gyroRaw, 2);
return true;
}
@ -195,9 +195,9 @@ bool bmi160AccReadScratchpad(accDev_t *acc)
bmi160ContextData_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]);
acc->ADCRaw[X] = (float) int16_val_little_endian(ctx->gyroRaw, 0);
acc->ADCRaw[Y] = (float) int16_val_little_endian(ctx->gyroRaw, 1);
acc->ADCRaw[Z] = (float) int16_val_little_endian(ctx->gyroRaw, 2);
return true;
}

12
src/main/drivers/accgyro/accgyro_bmi270.c
View file

@ -254,9 +254,9 @@ static bool bmi270yroReadScratchpad(gyroDev_t *gyro)
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]);
gyro->gyroADCRaw[X] = (float) int16_val_little_endian(ctx->gyroRaw, 0);
gyro->gyroADCRaw[Y] = (float) int16_val_little_endian(ctx->gyroRaw, 1);
gyro->gyroADCRaw[Z] = (float) int16_val_little_endian(ctx->gyroRaw, 2);
return true;
}
@ -269,9 +269,9 @@ 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]);
acc->ADCRaw[X] = (float) int16_val_little_endian(ctx->accRaw, 0);
acc->ADCRaw[Y] = (float) int16_val_little_endian(ctx->accRaw, 1);
acc->ADCRaw[Z] = (float) int16_val_little_endian(ctx->accRaw, 2);
return true;
}

2
src/main/drivers/accgyro/accgyro_fake.c
View file

@ -32,7 +32,7 @@
#ifdef USE_IMU_FAKE
static int16_t fakeGyroADC[XYZ_AXIS_COUNT];
static float fakeGyroADC[XYZ_AXIS_COUNT];
static void fakeGyroInit(gyroDev_t *gyro)
{

12
src/main/drivers/accgyro/accgyro_icm42605.c
View file

@ -98,9 +98,9 @@ static bool icm42605AccRead(accDev_t *acc)
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]);
acc->ADCRaw[X] = (float) int16_val_big_endian(data, 0);
acc->ADCRaw[Y] = (float) int16_val_big_endian(data, 1);
acc->ADCRaw[Z] = (float) int16_val_big_endian(data, 2);
return true;
}
@ -231,9 +231,9 @@ static bool icm42605GyroRead(gyroDev_t *gyro)
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]);
gyro->gyroADCRaw[X] = (float) int16_val_big_endian(data, 0);
gyro->gyroADCRaw[Y] = (float) int16_val_big_endian(data, 1);
gyro->gyroADCRaw[Z] = (float) int16_val_big_endian(data, 2);
return true;
}

12
src/main/drivers/accgyro/accgyro_lsm6dxx.c
View file

@ -184,9 +184,9 @@ static bool lsm6dxxAccRead(accDev_t *acc)
if (!ack) {
return false;
}
acc->ADCRaw[X] = (int16_t)((data[1] << 8) | data[0]);
acc->ADCRaw[Y] = (int16_t)((data[3] << 8) | data[2]);
acc->ADCRaw[Z] = (int16_t)((data[5] << 8) | data[4]);
acc->ADCRaw[X] = (float) int16_val_little_endian(data, 0);
acc->ADCRaw[Y] = (float) int16_val_little_endian(data, 1);
acc->ADCRaw[Z] = (float) int16_val_little_endian(data, 2);
return true;
}
@ -197,9 +197,9 @@ static bool lsm6dxxGyroRead(gyroDev_t *gyro)
if (!ack) {
return false;
}
gyro->gyroADCRaw[X] = (int16_t)((data[1] << 8) | data[0]);
gyro->gyroADCRaw[Y] = (int16_t)((data[3] << 8) | data[2]);
gyro->gyroADCRaw[Z] = (int16_t)((data[5] << 8) | data[4]);
gyro->gyroADCRaw[X] = (float) int16_val_little_endian(data, 0);
gyro->gyroADCRaw[Y] = (float) int16_val_little_endian(data, 1);
gyro->gyroADCRaw[Z] = (float) int16_val_little_endian(data, 2);
return true;
}

22
src/main/drivers/accgyro/accgyro_mpu.c
View file

@ -43,8 +43,6 @@
// Check busDevice scratchpad memory size
STATIC_ASSERT(sizeof(mpuContextData_t) < BUS_SCRATCHPAD_MEMORY_SIZE, busDevice_scratchpad_memory_too_small);
#define int16_val(v, idx) ((int16_t)(((uint8_t)v[2 * idx] << 8) | v[2 * idx + 1]))
static const gyroFilterAndRateConfig_t mpuGyroConfigs[] = {
{ GYRO_LPF_256HZ, 8000, { MPU_DLPF_256HZ, 0 } },
{ GYRO_LPF_256HZ, 4000, { MPU_DLPF_256HZ, 1 } },
@ -83,9 +81,9 @@ bool mpuGyroRead(gyroDev_t *gyro)
return false;
}
gyro->gyroADCRaw[X] = int16_val(data, 0);
gyro->gyroADCRaw[Y] = int16_val(data, 1);
gyro->gyroADCRaw[Z] = int16_val(data, 2);
gyro->gyroADCRaw[X] = (float) int16_val_big_endian(data, 0);
gyro->gyroADCRaw[Y] = (float) int16_val_big_endian(data, 1);
gyro->gyroADCRaw[Z] = (float) int16_val_big_endian(data, 2);
return true;
}
@ -102,9 +100,9 @@ bool mpuGyroReadScratchpad(gyroDev_t *gyro)
mpuContextData_t * ctx = busDeviceGetScratchpadMemory(busDev);
if (mpuUpdateSensorContext(busDev, ctx)) {
gyro->gyroADCRaw[X] = int16_val(ctx->gyroRaw, 0);
gyro->gyroADCRaw[Y] = int16_val(ctx->gyroRaw, 1);
gyro->gyroADCRaw[Z] = int16_val(ctx->gyroRaw, 2);
gyro->gyroADCRaw[X] = (float) int16_val_big_endian(ctx->gyroRaw, 0);
gyro->gyroADCRaw[Y] = (float) int16_val_big_endian(ctx->gyroRaw, 1);
gyro->gyroADCRaw[Z] = (float) int16_val_big_endian(ctx->gyroRaw, 2);
return true;
}
@ -116,9 +114,9 @@ bool mpuAccReadScratchpad(accDev_t *acc)
mpuContextData_t * ctx = busDeviceGetScratchpadMemory(acc->busDev);
if (ctx->lastReadStatus) {
acc->ADCRaw[X] = int16_val(ctx->accRaw, 0);
acc->ADCRaw[Y] = int16_val(ctx->accRaw, 1);
acc->ADCRaw[Z] = int16_val(ctx->accRaw, 2);
acc->ADCRaw[X] = (float) int16_val_big_endian(ctx->accRaw, 0);
acc->ADCRaw[Y] = (float) int16_val_big_endian(ctx->accRaw, 1);
acc->ADCRaw[Z] = (float) int16_val_big_endian(ctx->accRaw, 2);
return true;
}
@ -131,7 +129,7 @@ bool mpuTemperatureReadScratchpad(gyroDev_t *gyro, int16_t * data)
if (ctx->lastReadStatus) {
// Convert to degC*10: degC = raw / 340 + 36.53
*data = int16_val(ctx->tempRaw, 0) / 34 + 365;
*data = int16_val_big_endian(ctx->tempRaw, 0) / 34 + 365;
return true;
}

2
src/main/drivers/bus_i2c_hal.c
View file

@ -85,7 +85,7 @@ static i2cDevice_t i2cHardwareMap[I2CDEV_COUNT] = {
{ .dev = I2C2, .scl = IO_TAG(I2C2_SCL), .sda = IO_TAG(I2C2_SDA), .rcc = RCC_APB1L(I2C2), .speed = I2C_SPEED_400KHZ, .ev_irq = I2C2_EV_IRQn, .er_irq = I2C2_ER_IRQn, .af = GPIO_AF4_I2C2 },
{ .dev = I2C3, .scl = IO_TAG(I2C3_SCL), .sda = IO_TAG(I2C3_SDA), .rcc = RCC_APB1L(I2C3), .speed = I2C_SPEED_400KHZ, .ev_irq = I2C3_EV_IRQn, .er_irq = I2C3_ER_IRQn, .af = GPIO_AF4_I2C3 },
#if defined(USE_I2C_DEVICE_4)
{ .dev = I2C4, .scl = IO_TAG(I2C4_SCL), .sda = IO_TAG(I2C4_SDA), .rcc = RCC_APB1L(I2C4), .speed = I2C_SPEED_400KHZ, .ev_irq = I2C4_EV_IRQn, .er_irq = I2C4_ER_IRQn, .af = GPIO_AF4_I2C4 }
{ .dev = I2C4, .scl = IO_TAG(I2C4_SCL), .sda = IO_TAG(I2C4_SDA), .rcc = RCC_APB4(I2C4), .speed = I2C_SPEED_400KHZ, .ev_irq = I2C4_EV_IRQn, .er_irq = I2C4_ER_IRQn, .af = GPIO_AF4_I2C4 }
#endif
#endif
};

310
src/main/drivers/bus_quadspi.c Normal file
View file

@ -0,0 +1,310 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* Cleanflight and Betaflight are distributed in the hope that they
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*
* Author: Dominic Clifton
*/
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include "platform.h"
#ifdef USE_QUADSPI
#include "drivers/bus_quadspi.h"
#include "drivers/bus_quadspi_impl.h"
#include "drivers/exti.h"
#include "drivers/io.h"
#include "drivers/rcc.h"
static const quadSpiConfig_t quadSpiConfig[] = {
#ifdef USE_QUADSPI_DEVICE_1
{ .device = QUADSPIDEV_1, .clk = IO_TAG(QUADSPI1_SCK_PIN),
.bk1CS = IO_TAG(QUADSPI1_BK1_CS_PIN),
.bk1IO0 = IO_TAG(QUADSPI1_BK1_IO0_PIN), .bk1IO1 = IO_TAG(QUADSPI1_BK1_IO1_PIN), .bk1IO2 = IO_TAG(QUADSPI1_BK1_IO2_PIN), .bk1IO3 = IO_TAG(QUADSPI1_BK1_IO3_PIN),
.bk2CS = IO_TAG(QUADSPI1_BK2_CS_PIN),
.bk2IO0 = IO_TAG(QUADSPI1_BK2_IO0_PIN), .bk2IO1 = IO_TAG(QUADSPI1_BK2_IO1_PIN), .bk2IO2 = IO_TAG(QUADSPI1_BK2_IO2_PIN), .bk2IO3 = IO_TAG(QUADSPI1_BK2_IO3_PIN),
.mode = QUADSPI1_MODE, .csFlags = QUADSPI1_CS_FLAGS
}
#endif
};
quadSpiDevice_t quadSpiDevice[QUADSPIDEV_COUNT] = { 0 };
QUADSPIDevice quadSpiDeviceByInstance(QUADSPI_TypeDef *instance)
{
#ifdef USE_QUADSPI_DEVICE_1
if (instance == QUADSPI) {
return QUADSPIDEV_1;
}
#endif
return QUADSPIINVALID;
}
QUADSPI_TypeDef *quadSpiInstanceByDevice(QUADSPIDevice device)
{
if (device == QUADSPIINVALID || device >= QUADSPIDEV_COUNT) {
return NULL;
}
return quadSpiDevice[device].dev;
}
bool quadSpiInit(QUADSPIDevice device)
{
switch (device) {
case QUADSPIINVALID:
return false;
case QUADSPIDEV_1:
#ifdef USE_QUADSPI_DEVICE_1
quadSpiInitDevice(device);
return true;
#else
break;
#endif
}
return false;
}
uint32_t quadSpiTimeoutUserCallback(QUADSPI_TypeDef *instance)
{
QUADSPIDevice device = quadSpiDeviceByInstance(instance);
if (device == QUADSPIINVALID) {
return -1;
}
quadSpiDevice[device].errorCount++;
return quadSpiDevice[device].errorCount;
}
uint16_t quadSpiGetErrorCounter(QUADSPI_TypeDef *instance)
{
QUADSPIDevice device = quadSpiDeviceByInstance(instance);
if (device == QUADSPIINVALID) {
return 0;
}
return quadSpiDevice[device].errorCount;
}
void quadSpiResetErrorCounter(QUADSPI_TypeDef *instance)
{
QUADSPIDevice device = quadSpiDeviceByInstance(instance);
if (device != QUADSPIINVALID) {
quadSpiDevice[device].errorCount = 0;
}
}
const quadSpiHardware_t quadSpiHardware[] = {
#ifdef STM32H7
{
.device = QUADSPIDEV_1,
.reg = QUADSPI,
.clkPins = {
{ DEFIO_TAG_E(PB2), GPIO_AF9_QUADSPI },
},
.bk1IO0Pins = {
{ DEFIO_TAG_E(PC9), GPIO_AF9_QUADSPI },
{ DEFIO_TAG_E(PD11), GPIO_AF9_QUADSPI },
{ DEFIO_TAG_E(PF8), GPIO_AF10_QUADSPI },
},
.bk1IO1Pins = {
{ DEFIO_TAG_E(PC10), GPIO_AF9_QUADSPI },
{ DEFIO_TAG_E(PD12), GPIO_AF9_QUADSPI },
{ DEFIO_TAG_E(PF9), GPIO_AF10_QUADSPI },
},
.bk1IO2Pins = {
{ DEFIO_TAG_E(PE2), GPIO_AF9_QUADSPI },
{ DEFIO_TAG_E(PF7), GPIO_AF9_QUADSPI },
},
.bk1IO3Pins = {
{ DEFIO_TAG_E(PA1), GPIO_AF9_QUADSPI },
{ DEFIO_TAG_E(PD13), GPIO_AF9_QUADSPI },
{ DEFIO_TAG_E(PF6), GPIO_AF9_QUADSPI },
},
.bk1CSPins = {
{ DEFIO_TAG_E(PB6), GPIO_AF10_QUADSPI },
{ DEFIO_TAG_E(PB10), GPIO_AF9_QUADSPI },
{ DEFIO_TAG_E(PG6), GPIO_AF10_QUADSPI },
},
.bk2IO0Pins = {
{ DEFIO_TAG_E(PE7), GPIO_AF10_QUADSPI },
//{ DEFIO_TAG_E(PH7), GPIO_AF9_QUADSPI }, // FIXME regenerate io_def_generated with support for GPIO 'H'
},
.bk2IO1Pins = {
{ DEFIO_TAG_E(PE8), GPIO_AF10_QUADSPI },
//{ DEFIO_TAG_E(PH3), GPIO_AF9_QUADSPI }, // FIXME regenerate io_def_generated with support for GPIO 'H'
},
.bk2IO2Pins = {
{ DEFIO_TAG_E(PE9), GPIO_AF10_QUADSPI },
{ DEFIO_TAG_E(PG9), GPIO_AF9_QUADSPI },
},
.bk2IO3Pins = {
{ DEFIO_TAG_E(PE10), GPIO_AF10_QUADSPI },
{ DEFIO_TAG_E(PG14), GPIO_AF9_QUADSPI },
},
.bk2CSPins = {
{ DEFIO_TAG_E(PC11), GPIO_AF9_QUADSPI },
},
.rcc = RCC_AHB3(QSPI),
},
#endif
};
const quadSpiConfig_t * getQuadSpiConfig(QUADSPIDevice device)
{
const quadSpiConfig_t * pConfig = NULL;
for (size_t configIndex = 0; configIndex < ARRAYLEN(quadSpiConfig); configIndex++) {
if (quadSpiConfig[configIndex].device == device) {
pConfig = &quadSpiConfig[configIndex];
break;
}
}
return pConfig;
}
void quadSpiPinConfigure(QUADSPIDevice device)
{
if (device == QUADSPIINVALID) {
return;
}
const quadSpiConfig_t * pConfig = getQuadSpiConfig(device);
if (pConfig == NULL) {
return;
}
for (size_t hwindex = 0; hwindex < ARRAYLEN(quadSpiHardware); hwindex++) {
const quadSpiHardware_t *hw = &quadSpiHardware[hwindex];
if (hw->device != device) {
continue;
}
quadSpiDevice_t *pDev = &quadSpiDevice[device];
for (int pindex = 0; pindex < MAX_QUADSPI_PIN_SEL; pindex++) {
if (pConfig->clk == hw->clkPins[pindex].pin) {
pDev->clk = hw->clkPins[pindex].pin;
}
//
// BK1
//
if (pConfig->bk1IO0 == hw->bk1IO0Pins[pindex].pin) {
pDev->bk1IO0 = hw->bk1IO0Pins[pindex].pin;
pDev->bk1IO0AF = hw->bk1IO0Pins[pindex].af;
}
if (pConfig->bk1IO1 == hw->bk1IO1Pins[pindex].pin) {
pDev->bk1IO1 = hw->bk1IO1Pins[pindex].pin;
pDev->bk1IO1AF = hw->bk1IO1Pins[pindex].af;
}
if (pConfig->bk1IO2 == hw->bk1IO2Pins[pindex].pin) {
pDev->bk1IO2 = hw->bk1IO2Pins[pindex].pin;
pDev->bk1IO2AF = hw->bk1IO2Pins[pindex].af;
}
if (pConfig->bk1IO3 == hw->bk1IO3Pins[pindex].pin) {
pDev->bk1IO3 = hw->bk1IO3Pins[pindex].pin;
pDev->bk1IO3AF = hw->bk1IO3Pins[pindex].af;
}
if (pConfig->bk1CS == hw->bk1CSPins[pindex].pin) {
pDev->bk1CS = hw->bk1CSPins[pindex].pin;
pDev->bk1CSAF = hw->bk1CSPins[pindex].af;
}
//
// BK2
//
if (pConfig->bk2IO0 == hw->bk2IO0Pins[pindex].pin) {
pDev->bk2IO0 = hw->bk2IO0Pins[pindex].pin;
pDev->bk2IO0AF = hw->bk2IO0Pins[pindex].af;
}
if (pConfig->bk2IO1 == hw->bk2IO1Pins[pindex].pin) {
pDev->bk2IO1 = hw->bk2IO1Pins[pindex].pin;
pDev->bk2IO1AF = hw->bk2IO1Pins[pindex].af;
}
if (pConfig->bk2IO1 == hw->bk2IO2Pins[pindex].pin) {
pDev->bk2IO2 = hw->bk2IO2Pins[pindex].pin;
pDev->bk2IO2AF = hw->bk2IO2Pins[pindex].af;
}
if (pConfig->bk2IO2 == hw->bk2IO3Pins[pindex].pin) {
pDev->bk2IO3 = hw->bk2IO3Pins[pindex].pin;
pDev->bk2IO3AF = hw->bk2IO3Pins[pindex].af;
}
if (pConfig->bk2IO3 == hw->bk2CSPins[pindex].pin) {
pDev->bk2CS = hw->bk2CSPins[pindex].pin;
pDev->bk2CSAF = hw->bk2CSPins[pindex].af;
}
}
if ((pConfig->csFlags & QUADSPI_BK1_CS_MASK) == QUADSPI_BK1_CS_SOFTWARE) {
pDev->bk1CS = pConfig->bk1CS;
}
if ((pConfig->csFlags & QUADSPI_BK2_CS_MASK) == QUADSPI_BK2_CS_SOFTWARE) {
pDev->bk2CS = pConfig->bk2CS;
}
bool haveResources = true;
// clock pins
haveResources = haveResources && pDev->clk;
// data pins
bool needBK1 = (pConfig->mode == QUADSPI_MODE_DUAL_FLASH) || (pConfig->mode == QUADSPI_MODE_BK1_ONLY);
if (needBK1) {
bool haveBK1Resources = pDev->bk1IO0 && pDev->bk1IO1 && pDev->bk1IO2 && pDev->bk1IO3 && pDev->bk1CS;
haveResources = haveResources && haveBK1Resources;
}
bool needBK2 = (pConfig->mode == QUADSPI_MODE_DUAL_FLASH) || (pConfig->mode == QUADSPI_MODE_BK2_ONLY);
if (needBK2) {
bool haveBK2Resources = pDev->bk2IO0 && pDev->bk2IO1 && pDev->bk2IO2 && pDev->bk2IO3;
haveResources = haveResources && haveBK2Resources;
}
// cs pins
if (needBK1) {
haveResources = haveResources && pDev->bk1CS;
}
bool needBK2CS =
(pConfig->mode == QUADSPI_MODE_DUAL_FLASH && (pConfig->csFlags & QUADSPI_CS_MODE_MASK) == QUADSPI_CS_MODE_SEPARATE) ||
(pConfig->mode == QUADSPI_MODE_BK2_ONLY);
if (needBK2CS) {
haveResources = haveResources && pDev->bk2CS;
}
if (haveResources) {
pDev->dev = hw->reg;
pDev->rcc = hw->rcc;
}
// copy mode and flags
pDev->mode = pConfig->mode;
pDev->csFlags = pConfig->csFlags;
}
}
#endif

158
src/main/drivers/bus_quadspi.h Normal file
View file

@ -0,0 +1,158 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* Cleanflight and Betaflight are distributed in the hope that they
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*
* Author: Dominic Clifton
*/
#pragma once
#include "drivers/io_types.h"
#include "drivers/rcc_types.h"
/*
* Quad SPI supports 1/2/4 wire modes
*
* 1LINE is like SPI MISO/MOSI using D0 (MOSI)/D1(MISO).
* 2LINE uses D0, D1 (bidirectional).
* 4LINE uses D0..D3 (bidirectional)
*
* See ST Micros' AN4760 "Quad-SPI (QSPI) interface on STM32 microcontrollers"
*/
#ifdef USE_QUADSPI
#if !defined(STM32H7)
#error Quad SPI unsupported on this MCU
#endif
#define QUADSPI_IO_AF_BK_IO_CFG IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_NOPULL)
#define QUADSPI_IO_AF_CLK_CFG IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_NOPULL)
#define QUADSPI_IO_AF_BK_CS_CFG IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLUP)
#define QUADSPI_IO_BK_CS_CFG IO_CONFIG(GPIO_MODE_OUTPUT_PP, GPIO_SPEED_FREQ_HIGH, GPIO_PULLUP)
typedef enum {
/* QSPI freq = CLK /(1 + ClockPrescaler) = 200 MHz/(1+x) */
QUADSPI_CLOCK_INITIALISATION = 255, // 0.78125 Mhz
QUADSPI_CLOCK_SLOW = 19, // 10.00000 Mhz
QUADSPI_CLOCK_STANDARD = 9, // 20.00000 MHz
QUADSPI_CLOCK_FAST = 3, // 50.00000 MHz
QUADSPI_CLOCK_ULTRAFAST = 1 //100.00000 MHz
} QUADSPIClockDivider_e;
typedef enum QUADSPIDevice {
QUADSPIINVALID = -1,
QUADSPIDEV_1 = 0,
} QUADSPIDevice;
#define QUADSPIDEV_COUNT 1
// Macros to convert between CLI bus number and SPIDevice.
#define QUADSPI_CFG_TO_DEV(x) ((x) - 1)
#define QUADSPI_DEV_TO_CFG(x) ((x) + 1)
typedef enum {
QUADSPI_MODE_BK1_ONLY = 0,
QUADSPI_MODE_BK2_ONLY,
QUADSPI_MODE_DUAL_FLASH,
} quadSpiMode_e;
//
// QUADSPI1_CS_FLAGS
//
#define QUADSPI_BK1_CS_MASK ((1 << 1) | (1 << 0))
#define QUADSPI_BK1_CS_NONE ((0 << 1) | (0 << 0))
#define QUADSPI_BK1_CS_HARDWARE ((0 << 1) | (1 << 0)) // pin must support QSPI Alternate function for BK1_NCS
#define QUADSPI_BK1_CS_SOFTWARE ((1 << 1) | (0 << 0)) // use any GPIO pin for BK1 CS
#define QUADSPI_BK2_CS_MASK ((1 << 3) | (1 << 2))
#define QUADSPI_BK2_CS_NONE ((0 << 3) | (0 << 2))
#define QUADSPI_BK2_CS_HARDWARE ((0 << 3) | (1 << 2)) // pin must support QSPI Alternate function for BK2_NCS
#define QUADSPI_BK2_CS_SOFTWARE ((1 << 3) | (0 << 2)) // use any GPIO pin for BK2 CS
#define QUADSPI_CS_MODE_MASK (1 << 4)
#define QUADSPI_CS_MODE_SEPARATE (0 << 4)
#define QUADSPI_CS_MODE_LINKED (1 << 4)
// H7 QSPI notes:
// Hardware supports BK1 and BK2 connected to both flash chips when using DUAL FLASH mode.
// Hardware does NOT support using BK1_NCS for single flash chip on BK2.
// It's possible to use BK1_NCS for single chip on BK2 using software CS via QUADSPI_BK2_CS_SOFTWARE
void quadSpiPreInit(void);
bool quadSpiInit(QUADSPIDevice device);
void quadSpiSetDivisor(QUADSPI_TypeDef *instance, uint16_t divisor);
bool quadSpiTransmit1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, const uint8_t *out, int length);
bool quadSpiReceive1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint8_t *in, int length);
bool quadSpiReceive4LINES(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint8_t *in, int length);
bool quadSpiInstructionWithData1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, const uint8_t *out, int length);
bool quadSpiReceiveWithAddress1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, uint8_t *in, int length);
bool quadSpiReceiveWithAddress4LINES(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, uint8_t *in, int length);
bool quadSpiTransmitWithAddress1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, const uint8_t *out, int length);
bool quadSpiTransmitWithAddress4LINES(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, const uint8_t *out, int length);
bool quadSpiInstructionWithAddress1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize);
//bool quadSpiIsBusBusy(SPI_TypeDef *instance);
uint16_t quadSpiGetErrorCounter(QUADSPI_TypeDef *instance);
void quadSpiResetErrorCounter(QUADSPI_TypeDef *instance);
QUADSPIDevice quadSpiDeviceByInstance(QUADSPI_TypeDef *instance);
QUADSPI_TypeDef *quadSpiInstanceByDevice(QUADSPIDevice device);
//
// Config
//
typedef struct quadSpiConfig_s {
QUADSPIDevice device;
ioTag_t clk;
// Note: Either or both CS pin may be used in DUAL_FLASH mode, any unused pins should be IO_NONE
ioTag_t bk1IO0;
ioTag_t bk1IO1;
ioTag_t bk1IO2;
ioTag_t bk1IO3;
ioTag_t bk1CS;
ioTag_t bk2IO0;
ioTag_t bk2IO1;
ioTag_t bk2IO2;
ioTag_t bk2IO3;
ioTag_t bk2CS;
uint8_t mode;
// CS pins can be under software control, useful when using BK1CS as the CS pin for BK2 in non-DUAL-FLASH mode.
uint8_t csFlags;
} quadSpiConfig_t;
const quadSpiConfig_t * getQuadSpiConfig(QUADSPIDevice device);
void quadSpiPinConfigure(QUADSPIDevice device);
#endif

575
src/main/drivers/bus_quadspi_hal.c Normal file
View file

@ -0,0 +1,575 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* Cleanflight and Betaflight are distributed in the hope that they
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*
* Author: Dominic Clifton
*/
#include <stdbool.h>
#include <stdint.h>
#include "platform.h"
#ifdef USE_QUADSPI
#include "bus_quadspi.h"
#include "bus_quadspi_impl.h"
#include "dma.h"
#include "io.h"
#include "io_impl.h"
#include "nvic.h"
#include "rcc.h"
static void Error_Handler(void) { while (1) { } }
void quadSpiInitDevice(QUADSPIDevice device)
{
quadSpiDevice_t *quadSpi = &(quadSpiDevice[device]);
// Enable QUADSPI clock
RCC_ClockCmd(quadSpi->rcc, ENABLE);
//RCC_ResetCmd(quadSpi->rcc, ENABLE);
IOInit(IOGetByTag(quadSpi->clk), OWNER_QUADSPI, RESOURCE_QUADSPI_CLK, RESOURCE_INDEX(device));
IOInit(IOGetByTag(quadSpi->bk1IO0), OWNER_QUADSPI, RESOURCE_QUADSPI_BK1IO0, RESOURCE_INDEX(device));
IOInit(IOGetByTag(quadSpi->bk1IO1), OWNER_QUADSPI, RESOURCE_QUADSPI_BK1IO1, RESOURCE_INDEX(device));
IOInit(IOGetByTag(quadSpi->bk1IO2), OWNER_QUADSPI, RESOURCE_QUADSPI_BK1IO2, RESOURCE_INDEX(device));
IOInit(IOGetByTag(quadSpi->bk1IO3), OWNER_QUADSPI, RESOURCE_QUADSPI_BK1IO3, RESOURCE_INDEX(device));
IOInit(IOGetByTag(quadSpi->bk1CS), OWNER_QUADSPI, RESOURCE_QUADSPI_BK1CS, RESOURCE_INDEX(device));
IOInit(IOGetByTag(quadSpi->bk2IO0), OWNER_QUADSPI, RESOURCE_QUADSPI_BK2IO0, RESOURCE_INDEX(device));
IOInit(IOGetByTag(quadSpi->bk2IO1), OWNER_QUADSPI, RESOURCE_QUADSPI_BK2IO1, RESOURCE_INDEX(device));
IOInit(IOGetByTag(quadSpi->bk2IO2), OWNER_QUADSPI, RESOURCE_QUADSPI_BK2IO2, RESOURCE_INDEX(device));
IOInit(IOGetByTag(quadSpi->bk2IO3), OWNER_QUADSPI, RESOURCE_QUADSPI_BK2IO3, RESOURCE_INDEX(device));
IOInit(IOGetByTag(quadSpi->bk2CS), OWNER_QUADSPI, RESOURCE_QUADSPI_BK2CS, RESOURCE_INDEX(device));
#if defined(STM32H7)
// clock is only on AF9
// IO and CS lines are on AF9 and AF10
IOConfigGPIOAF(IOGetByTag(quadSpi->clk), QUADSPI_IO_AF_CLK_CFG, GPIO_AF9_QUADSPI);
IOConfigGPIOAF(IOGetByTag(quadSpi->bk1IO0), QUADSPI_IO_AF_BK_IO_CFG, quadSpi->bk1IO0AF);
IOConfigGPIOAF(IOGetByTag(quadSpi->bk1IO1), QUADSPI_IO_AF_BK_IO_CFG, quadSpi->bk1IO1AF);
IOConfigGPIOAF(IOGetByTag(quadSpi->bk1IO2), QUADSPI_IO_AF_BK_IO_CFG, quadSpi->bk1IO2AF);
IOConfigGPIOAF(IOGetByTag(quadSpi->bk1IO3), QUADSPI_IO_AF_BK_IO_CFG, quadSpi->bk1IO3AF);
IOConfigGPIOAF(IOGetByTag(quadSpi->bk2IO0), QUADSPI_IO_AF_BK_IO_CFG, quadSpi->bk2IO0AF);
IOConfigGPIOAF(IOGetByTag(quadSpi->bk2IO1), QUADSPI_IO_AF_BK_IO_CFG, quadSpi->bk2IO1AF);
IOConfigGPIOAF(IOGetByTag(quadSpi->bk2IO2), QUADSPI_IO_AF_BK_IO_CFG, quadSpi->bk2IO2AF);
IOConfigGPIOAF(IOGetByTag(quadSpi->bk2IO3), QUADSPI_IO_AF_BK_IO_CFG, quadSpi->bk2IO3AF);
if ((quadSpi->csFlags & QUADSPI_BK1_CS_MASK) == QUADSPI_BK1_CS_HARDWARE) {
IOConfigGPIOAF(IOGetByTag(quadSpi->bk1CS), QUADSPI_IO_AF_BK_CS_CFG, quadSpi->bk1CSAF);
} else {
IOConfigGPIO(IOGetByTag(quadSpi->bk1CS), QUADSPI_IO_BK_CS_CFG);
}
if ((quadSpi->csFlags & QUADSPI_BK2_CS_MASK) == QUADSPI_BK2_CS_HARDWARE) {
IOConfigGPIOAF(IOGetByTag(quadSpi->bk2CS), QUADSPI_IO_AF_BK_CS_CFG, quadSpi->bk2CSAF);
} else {
IOConfigGPIO(IOGetByTag(quadSpi->bk2CS), QUADSPI_IO_BK_CS_CFG);
}
#endif
quadSpi->hquadSpi.Instance = quadSpi->dev;
// DeInit QUADSPI hardware
HAL_QSPI_DeInit(&quadSpi->hquadSpi);
quadSpi->hquadSpi.Init.ClockPrescaler = QUADSPI_CLOCK_INITIALISATION;
quadSpi->hquadSpi.Init.FifoThreshold = 1;
quadSpi->hquadSpi.Init.SampleShifting = QSPI_SAMPLE_SHIFTING_NONE;
quadSpi->hquadSpi.Init.FlashSize = 23; // address bits + 1
quadSpi->hquadSpi.Init.ChipSelectHighTime = QSPI_CS_HIGH_TIME_1_CYCLE;
quadSpi->hquadSpi.Init.ClockMode = QSPI_CLOCK_MODE_0;
switch (quadSpi->mode) {
case QUADSPI_MODE_BK1_ONLY:
quadSpi->hquadSpi.Init.FlashID = QSPI_FLASH_ID_1;
quadSpi->hquadSpi.Init.DualFlash = QSPI_DUALFLASH_DISABLE;
break;
case QUADSPI_MODE_BK2_ONLY:
quadSpi->hquadSpi.Init.FlashID = QSPI_FLASH_ID_2;
quadSpi->hquadSpi.Init.DualFlash = QSPI_DUALFLASH_DISABLE;
break;
case QUADSPI_MODE_DUAL_FLASH:
quadSpi->hquadSpi.Init.DualFlash = QSPI_DUALFLASH_ENABLE;
break;
}
// Init QUADSPI hardware
if (HAL_QSPI_Init(&quadSpi->hquadSpi) != HAL_OK)
{
Error_Handler();
}
}
static const uint32_t quadSpi_addressSizeMap[] = {
QSPI_ADDRESS_8_BITS,
QSPI_ADDRESS_16_BITS,
QSPI_ADDRESS_24_BITS,
QSPI_ADDRESS_32_BITS
};
static uint32_t quadSpi_addressSizeFromValue(uint8_t addressSize)
{
return quadSpi_addressSizeMap[((addressSize + 1) / 8) - 1]; // rounds to nearest QSPI_ADDRESS_* value that will hold the address.
}
/**
* Return true if the bus is currently in the middle of a transmission.
*/
bool quadSpiIsBusBusy(QUADSPI_TypeDef *instance)
{
QUADSPIDevice device = quadSpiDeviceByInstance(instance);
if(quadSpiDevice[device].hquadSpi.State == HAL_QSPI_STATE_BUSY)
return true;
else
return false;
}
#define QUADSPI_DEFAULT_TIMEOUT 10
void quadSpiSelectDevice(QUADSPI_TypeDef *instance)
{
QUADSPIDevice device = quadSpiDeviceByInstance(instance);
quadSpiDevice_t *quadSpi = &(quadSpiDevice[device]);
IO_t bk1CS = IOGetByTag(quadSpi->bk1CS);
IO_t bk2CS = IOGetByTag(quadSpi->bk2CS);
switch(quadSpi->mode) {
case QUADSPI_MODE_DUAL_FLASH:
if ((quadSpi->csFlags & QUADSPI_BK1_CS_MASK) == QUADSPI_BK1_CS_SOFTWARE) {
IOLo(bk1CS);
}
if ((quadSpi->csFlags & QUADSPI_BK2_CS_MASK) == QUADSPI_BK2_CS_SOFTWARE) {
IOLo(bk2CS);
}
break;
case QUADSPI_MODE_BK1_ONLY:
if ((quadSpi->csFlags & QUADSPI_BK1_CS_MASK) == QUADSPI_BK1_CS_SOFTWARE) {
IOLo(bk1CS);
}
break;
case QUADSPI_MODE_BK2_ONLY:
if ((quadSpi->csFlags & QUADSPI_BK2_CS_MASK) == QUADSPI_BK2_CS_SOFTWARE) {
IOLo(bk2CS);
}
break;
}
}
void quadSpiDeselectDevice(QUADSPI_TypeDef *instance)
{
QUADSPIDevice device = quadSpiDeviceByInstance(instance);
quadSpiDevice_t *quadSpi = &(quadSpiDevice[device]);
IO_t bk1CS = IOGetByTag(quadSpi->bk1CS);
IO_t bk2CS = IOGetByTag(quadSpi->bk2CS);
switch(quadSpi->mode) {
case QUADSPI_MODE_DUAL_FLASH:
if ((quadSpi->csFlags & QUADSPI_BK1_CS_MASK) == QUADSPI_BK1_CS_SOFTWARE) {
IOHi(bk1CS);
}
if ((quadSpi->csFlags & QUADSPI_BK2_CS_MASK) == QUADSPI_BK2_CS_SOFTWARE) {
IOHi(bk2CS);
}
break;
case QUADSPI_MODE_BK1_ONLY:
if ((quadSpi->csFlags & QUADSPI_BK1_CS_MASK) == QUADSPI_BK1_CS_SOFTWARE) {
IOHi(bk1CS);
}
break;
case QUADSPI_MODE_BK2_ONLY:
if ((quadSpi->csFlags & QUADSPI_BK2_CS_MASK) == QUADSPI_BK2_CS_SOFTWARE) {
IOHi(bk2CS);
}
break;
}
}
bool quadSpiTransmit1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, const uint8_t *out, int length)
{
QUADSPIDevice device = quadSpiDeviceByInstance(instance);
HAL_StatusTypeDef status;
QSPI_CommandTypeDef cmd;
cmd.InstructionMode = QSPI_INSTRUCTION_1_LINE;
cmd.AddressMode = QSPI_ADDRESS_NONE;
cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
cmd.DataMode = QSPI_DATA_NONE;
cmd.DummyCycles = dummyCycles;
cmd.DdrMode = QSPI_DDR_MODE_DISABLE;
cmd.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
cmd.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
cmd.Instruction = instruction;
cmd.NbData = length;
if (out) {
cmd.DataMode = QSPI_DATA_1_LINE;
}
quadSpiSelectDevice(instance);
status = HAL_QSPI_Command(&quadSpiDevice[device].hquadSpi, &cmd, QUADSPI_DEFAULT_TIMEOUT);
bool timeout = (status != HAL_OK);
if (!timeout) {
if (out && length > 0) {
status = HAL_QSPI_Transmit(&quadSpiDevice[device].hquadSpi, (uint8_t *)out, QUADSPI_DEFAULT_TIMEOUT);
timeout = (status != HAL_OK);
}
}
quadSpiDeselectDevice(instance);
if (timeout) {
quadSpiTimeoutUserCallback(instance);
return false;
}
return true;
}
bool quadSpiReceive1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint8_t *in, int length)
{
QUADSPIDevice device = quadSpiDeviceByInstance(instance);
HAL_StatusTypeDef status;
QSPI_CommandTypeDef cmd;
cmd.InstructionMode = QSPI_INSTRUCTION_1_LINE;
cmd.AddressMode = QSPI_ADDRESS_NONE;
cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
cmd.DataMode = QSPI_DATA_1_LINE;
cmd.DummyCycles = dummyCycles;
cmd.DdrMode = QSPI_DDR_MODE_DISABLE;
cmd.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
cmd.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
cmd.Instruction = instruction;
cmd.NbData = length;
quadSpiSelectDevice(instance);
status = HAL_QSPI_Command(&quadSpiDevice[device].hquadSpi, &cmd, QUADSPI_DEFAULT_TIMEOUT);
bool timeout = (status != HAL_OK);
if (!timeout) {
status = HAL_QSPI_Receive(&quadSpiDevice[device].hquadSpi, in, QUADSPI_DEFAULT_TIMEOUT);
timeout = (status != HAL_OK);
}
quadSpiDeselectDevice(instance);
if (timeout) {
quadSpiTimeoutUserCallback(instance);
return false;
}
return true;
}
bool quadSpiReceive4LINES(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint8_t *in, int length)
{
QUADSPIDevice device = quadSpiDeviceByInstance(instance);
HAL_StatusTypeDef status;
QSPI_CommandTypeDef cmd;
cmd.InstructionMode = QSPI_INSTRUCTION_4_LINES;
cmd.AddressMode = QSPI_ADDRESS_NONE;
cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
cmd.DataMode = QSPI_DATA_4_LINES;
cmd.DummyCycles = dummyCycles;
cmd.DdrMode = QSPI_DDR_MODE_DISABLE;
cmd.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
cmd.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
cmd.Instruction = instruction;
cmd.NbData = length;
quadSpiSelectDevice(instance);
status = HAL_QSPI_Command(&quadSpiDevice[device].hquadSpi, &cmd, QUADSPI_DEFAULT_TIMEOUT);
bool timeout = (status != HAL_OK);
if (!timeout) {
status = HAL_QSPI_Receive(&quadSpiDevice[device].hquadSpi, in, QUADSPI_DEFAULT_TIMEOUT);
timeout = (status != HAL_OK);
}
quadSpiDeselectDevice(instance);
if (timeout) {
quadSpiTimeoutUserCallback(instance);
return false;
}
return true;
}
bool quadSpiReceiveWithAddress1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, uint8_t *in, int length)
{
QUADSPIDevice device = quadSpiDeviceByInstance(instance);
HAL_StatusTypeDef status;
QSPI_CommandTypeDef cmd;
cmd.InstructionMode = QSPI_INSTRUCTION_1_LINE;
cmd.AddressMode = QSPI_ADDRESS_1_LINE;
cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
cmd.DataMode = QSPI_DATA_1_LINE;
cmd.DummyCycles = dummyCycles;
cmd.DdrMode = QSPI_DDR_MODE_DISABLE;
cmd.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
cmd.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
cmd.Instruction = instruction;
cmd.Address = address;
cmd.AddressSize = quadSpi_addressSizeFromValue(addressSize);
cmd.NbData = length;
quadSpiSelectDevice(instance);
status = HAL_QSPI_Command(&quadSpiDevice[device].hquadSpi, &cmd, QUADSPI_DEFAULT_TIMEOUT);
bool timeout = (status != HAL_OK);
if (!timeout) {
status = HAL_QSPI_Receive(&quadSpiDevice[device].hquadSpi, in, QUADSPI_DEFAULT_TIMEOUT);
timeout = (status != HAL_OK);
}
quadSpiDeselectDevice(instance);
if (timeout) {
quadSpiTimeoutUserCallback(instance);
return false;
}
return true;
}
bool quadSpiReceiveWithAddress4LINES(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, uint8_t *in, int length)
{
QUADSPIDevice device = quadSpiDeviceByInstance(instance);
HAL_StatusTypeDef status;
QSPI_CommandTypeDef cmd;
cmd.InstructionMode = QSPI_INSTRUCTION_1_LINE;
cmd.AddressMode = QSPI_ADDRESS_1_LINE;
cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
cmd.DataMode = QSPI_DATA_4_LINES;
cmd.DummyCycles = dummyCycles;
cmd.DdrMode = QSPI_DDR_MODE_DISABLE;
cmd.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
cmd.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
cmd.Instruction = instruction;
cmd.Address = address;
cmd.AddressSize = quadSpi_addressSizeFromValue(addressSize);
cmd.NbData = length;
quadSpiSelectDevice(instance);
status = HAL_QSPI_Command(&quadSpiDevice[device].hquadSpi, &cmd, QUADSPI_DEFAULT_TIMEOUT);
bool timeout = (status != HAL_OK);
if (!timeout) {
status = HAL_QSPI_Receive(&quadSpiDevice[device].hquadSpi, in, QUADSPI_DEFAULT_TIMEOUT);
timeout = (status != HAL_OK);
}
quadSpiDeselectDevice(instance);
if (timeout) {
quadSpiTimeoutUserCallback(instance);
return false;
}
return true;
}
bool quadSpiTransmitWithAddress1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, const uint8_t *out, int length)
{
QUADSPIDevice device = quadSpiDeviceByInstance(instance);
HAL_StatusTypeDef status;
QSPI_CommandTypeDef cmd;
cmd.InstructionMode = QSPI_INSTRUCTION_1_LINE;
cmd.AddressMode = QSPI_ADDRESS_1_LINE;
cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
cmd.DataMode = QSPI_DATA_1_LINE;
cmd.DummyCycles = dummyCycles;
cmd.DdrMode = QSPI_DDR_MODE_DISABLE;
cmd.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
cmd.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
cmd.Instruction = instruction;
cmd.Address = address;
cmd.AddressSize = quadSpi_addressSizeFromValue(addressSize);
cmd.NbData = length;
quadSpiSelectDevice(instance);
status = HAL_QSPI_Command(&quadSpiDevice[device].hquadSpi, &cmd, QUADSPI_DEFAULT_TIMEOUT);
bool timeout = (status != HAL_OK);
if (!timeout) {
status = HAL_QSPI_Transmit(&quadSpiDevice[device].hquadSpi, (uint8_t *)out, QUADSPI_DEFAULT_TIMEOUT);
timeout = (status != HAL_OK);
}
quadSpiDeselectDevice(instance);
if (timeout) {
quadSpiTimeoutUserCallback(instance);
return false;
}
return true;
}
bool quadSpiTransmitWithAddress4LINES(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, const uint8_t *out, int length)
{
QUADSPIDevice device = quadSpiDeviceByInstance(instance);
HAL_StatusTypeDef status;
QSPI_CommandTypeDef cmd;
cmd.InstructionMode = QSPI_INSTRUCTION_1_LINE;
cmd.AddressMode = QSPI_ADDRESS_1_LINE;
cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
cmd.DataMode = QSPI_DATA_4_LINES;
cmd.DummyCycles = dummyCycles;
cmd.DdrMode = QSPI_DDR_MODE_DISABLE;
cmd.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
cmd.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
cmd.Instruction = instruction;
cmd.Address = address;
cmd.AddressSize = quadSpi_addressSizeFromValue(addressSize);
cmd.NbData = length;
quadSpiSelectDevice(instance);
status = HAL_QSPI_Command(&quadSpiDevice[device].hquadSpi, &cmd, QUADSPI_DEFAULT_TIMEOUT);
bool timeout = (status != HAL_OK);
if (!timeout) {
status = HAL_QSPI_Transmit(&quadSpiDevice[device].hquadSpi, (uint8_t *)out, QUADSPI_DEFAULT_TIMEOUT);
timeout = (status != HAL_OK);
}
quadSpiDeselectDevice(instance);
if (timeout) {
quadSpiTimeoutUserCallback(instance);
return false;
}
return true;
}
bool quadSpiInstructionWithAddress1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize)
{
QUADSPIDevice device = quadSpiDeviceByInstance(instance);
HAL_StatusTypeDef status;
QSPI_CommandTypeDef cmd;
cmd.InstructionMode = QSPI_INSTRUCTION_1_LINE;
cmd.AddressMode = QSPI_ADDRESS_1_LINE;
cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
cmd.DataMode = QSPI_DATA_NONE;
cmd.DummyCycles = dummyCycles;
cmd.DdrMode = QSPI_DDR_MODE_DISABLE;
cmd.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
cmd.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
cmd.Instruction = instruction;
cmd.Address = address;
cmd.AddressSize = quadSpi_addressSizeFromValue(addressSize);
cmd.NbData = 0;
quadSpiSelectDevice(instance);
status = HAL_QSPI_Command(&quadSpiDevice[device].hquadSpi, &cmd, QUADSPI_DEFAULT_TIMEOUT);
bool timeout = (status != HAL_OK);
quadSpiDeselectDevice(instance);
if (timeout) {
quadSpiTimeoutUserCallback(instance);
return false;
}
return true;
}
bool quadSpiInstructionWithData1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, const uint8_t *out, int length)
{
QUADSPIDevice device = quadSpiDeviceByInstance(instance);
HAL_StatusTypeDef status;
QSPI_CommandTypeDef cmd;
cmd.InstructionMode = QSPI_INSTRUCTION_1_LINE;
cmd.AddressMode = QSPI_ADDRESS_NONE;
cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
cmd.DataMode = QSPI_DATA_1_LINE;
cmd.DummyCycles = dummyCycles;
cmd.DdrMode = QSPI_DDR_MODE_DISABLE;
cmd.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
cmd.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
cmd.Instruction = instruction;
cmd.NbData = length;
quadSpiSelectDevice(instance);
status = HAL_QSPI_Command(&quadSpiDevice[device].hquadSpi, &cmd, QUADSPI_DEFAULT_TIMEOUT);
bool timeout =(status != HAL_OK);
if (!timeout) {
status = HAL_QSPI_Transmit(&quadSpiDevice[device].hquadSpi, (uint8_t *)out, QUADSPI_DEFAULT_TIMEOUT);
timeout = (status != HAL_OK);
}
quadSpiDeselectDevice(instance);
if (timeout) {
quadSpiTimeoutUserCallback(instance);
return false;
}
return true;
}
void quadSpiSetDivisor(QUADSPI_TypeDef *instance, uint16_t divisor)
{
QUADSPIDevice device = quadSpiDeviceByInstance(instance);
if (HAL_QSPI_DeInit(&quadSpiDevice[device].hquadSpi) != HAL_OK)
{
Error_Handler();
}
quadSpiDevice_t *quadSpi = &(quadSpiDevice[device]);
quadSpi->hquadSpi.Init.ClockPrescaler = divisor;
HAL_QSPI_Init(&quadSpi->hquadSpi);
}
#endif

94
src/main/drivers/bus_quadspi_impl.h Normal file
View file

@ -0,0 +1,94 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* Cleanflight and Betaflight are distributed in the hope that they
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*
* Author: Dominic Clifton
*/
#pragma once
typedef struct quadSpiPinDef_s {
ioTag_t pin;
#if defined(STM32H7)
uint8_t af;
#endif
} quadSpiPinDef_t;
#if defined(STM32H7)
#define MAX_QUADSPI_PIN_SEL 3
#endif
typedef struct quadSpiHardware_s {
QUADSPIDevice device;
QUADSPI_TypeDef *reg;
quadSpiPinDef_t clkPins[MAX_QUADSPI_PIN_SEL];
quadSpiPinDef_t bk1IO0Pins[MAX_QUADSPI_PIN_SEL];
quadSpiPinDef_t bk1IO1Pins[MAX_QUADSPI_PIN_SEL];
quadSpiPinDef_t bk1IO2Pins[MAX_QUADSPI_PIN_SEL];
quadSpiPinDef_t bk1IO3Pins[MAX_QUADSPI_PIN_SEL];
quadSpiPinDef_t bk1CSPins[MAX_QUADSPI_PIN_SEL];
quadSpiPinDef_t bk2IO0Pins[MAX_QUADSPI_PIN_SEL];
quadSpiPinDef_t bk2IO1Pins[MAX_QUADSPI_PIN_SEL];
quadSpiPinDef_t bk2IO2Pins[MAX_QUADSPI_PIN_SEL];
quadSpiPinDef_t bk2IO3Pins[MAX_QUADSPI_PIN_SEL];
quadSpiPinDef_t bk2CSPins[MAX_QUADSPI_PIN_SEL];
rccPeriphTag_t rcc;
} quadSpiHardware_t;
extern const quadSpiHardware_t quadSpiHardware[];
typedef struct QUADSPIDevice_s {
QUADSPI_TypeDef *dev;
ioTag_t clk;
ioTag_t bk1IO0;
ioTag_t bk1IO1;
ioTag_t bk1IO2;
ioTag_t bk1IO3;
ioTag_t bk1CS;
ioTag_t bk2IO0;
ioTag_t bk2IO1;
ioTag_t bk2IO2;
ioTag_t bk2IO3;
ioTag_t bk2CS;
#if defined(STM32H7)
uint8_t bk1IO0AF;
uint8_t bk1IO1AF;
uint8_t bk1IO2AF;
uint8_t bk1IO3AF;
uint8_t bk1CSAF;
uint8_t bk2IO0AF;
uint8_t bk2IO1AF;
uint8_t bk2IO2AF;
uint8_t bk2IO3AF;
uint8_t bk2CSAF;
#endif
uint8_t mode;
uint8_t csFlags;
rccPeriphTag_t rcc;
volatile uint16_t errorCount;
#if defined(USE_HAL_DRIVER)
QSPI_HandleTypeDef hquadSpi;
#endif
} quadSpiDevice_t;
extern quadSpiDevice_t quadSpiDevice[QUADSPIDEV_COUNT];
void quadSpiInitDevice(QUADSPIDevice device);
uint32_t quadSpiTimeoutUserCallback(QUADSPI_TypeDef *instance);

8
src/main/drivers/compass/compass_msp.c
View file

@ -45,7 +45,7 @@
#define MSP_MAG_TIMEOUT_MS 250 // Less than 4Hz updates is considered a failure
static int32_t mspMagData[XYZ_AXIS_COUNT];
static float mspMagData[XYZ_AXIS_COUNT];
static timeMs_t mspMagLastUpdateMs;
static bool mspMagInit(magDev_t *magDev)
@ -62,9 +62,9 @@ void mspMagReceiveNewData(uint8_t * bufferPtr)
{
const mspSensorCompassDataMessage_t * pkt = (const mspSensorCompassDataMessage_t *)bufferPtr;
mspMagData[X] = pkt->magX;
mspMagData[Y] = pkt->magY;
mspMagData[Z] = pkt->magZ;
mspMagData[X] = (float) pkt->magX;
mspMagData[Y] = (float) pkt->magY;
mspMagData[Z] = (float) pkt->magZ;
applySensorAlignment(mspMagData, mspMagData, CW90_DEG_FLIP);

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

@ -218,7 +218,7 @@ static void flashConfigurePartitions(void)
#endif
}
flashPartition_t *flashPartitionFindByType(uint8_t type)
flashPartition_t *flashPartitionFindByType(flashPartitionType_e type)
{
for (int index = 0; index < FLASH_MAX_PARTITIONS; index++) {
flashPartition_t *candidate = &flashPartitionTable.partitions[index];

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

@ -27,13 +27,20 @@
#include "drivers/bus.h"
#include "drivers/time.h"
#if defined(USE_QUADSPI)
#include "drivers/bus_quadspi.h"
#include "drivers/bus_quadspi_impl.h"
#endif
#define M25P16_INSTRUCTION_RDID 0x9F
#define M25P16_INSTRUCTION_READ_BYTES 0x03
#define M25P16_INSTRUCTION_QUAD_READ 0x6B
#define M25P16_INSTRUCTION_READ_STATUS_REG 0x05
#define M25P16_INSTRUCTION_WRITE_STATUS_REG 0x01
#define M25P16_INSTRUCTION_WRITE_ENABLE 0x06
#define M25P16_INSTRUCTION_WRITE_DISABLE 0x04
#define M25P16_INSTRUCTION_PAGE_PROGRAM 0x02
#define M25P16_INSTRUCTION_QPAGE_PROGRAM 0x32
#define M25P16_INSTRUCTION_SECTOR_ERASE 0xD8
#define M25P16_INSTRUCTION_BULK_ERASE 0xC7
@ -42,6 +49,8 @@
#define W25Q256_INSTRUCTION_ENTER_4BYTE_ADDRESS_MODE 0xB7
#define M25P16_FAST_READ_DUMMY_CYCLES 8
struct {
uint32_t jedecID;
flashSector_t sectors;
@ -118,7 +127,12 @@ struct {
static flashGeometry_t geometry = {.pageSize = M25P16_PAGESIZE};
#if !defined(M25P16_QUADSPI_DEVICE)
static busDevice_t * busDev = NULL;
#else
static QUADSPI_TypeDef * qspi = NULL;
#endif /* !defined(M25P16_QUADSPI_DEVICE) */
static bool isLargeFlash = false;
static uint32_t timeoutAt = 0;
@ -134,7 +148,11 @@ static bool couldBeBusy = false;
*/
static void m25p16_performOneByteCommand(uint8_t command)
{
#if !defined(M25P16_QUADSPI_DEVICE)
busTransfer(busDev, NULL, &command, 1);
#else
quadSpiTransmit1LINE(qspi, command, 0, NULL, 0);
#endif /* !defined(M25P16_QUADSPI_DEVICE) */
}
/**
@ -151,12 +169,18 @@ static void m25p16_writeEnable(void)
static uint8_t m25p16_readStatus(void)
{
uint8_t status;
#if !defined(M25P16_QUADSPI_DEVICE)
uint8_t command[2] = { M25P16_INSTRUCTION_READ_STATUS_REG, 0 };
uint8_t in[2];
busTransfer(busDev, in, command, sizeof(command));
status = in[1];
#else
quadSpiReceive1LINE(qspi, M25P16_INSTRUCTION_READ_STATUS_REG, 0, &status, 1);
#endif /* !defined(M25P16_QUADSPI_DEVICE) */
return in[1];
return status;
}
bool m25p16_isReady(void)
@ -201,7 +225,6 @@ bool m25p16_waitForReady(uint32_t timeoutMillis)
*/
static bool m25p16_readIdentification(void)
{
uint8_t out[] = { M25P16_INSTRUCTION_RDID, 0, 0, 0 };
uint8_t in[4];
uint32_t chipID;
@ -212,7 +235,16 @@ static bool m25p16_readIdentification(void)
*/
in[1] = 0;
#if !defined(M25P16_QUADSPI_DEVICE)
uint8_t out[] = { M25P16_INSTRUCTION_RDID, 0, 0, 0 };
busTransfer(busDev, in, out, sizeof(out));
#else
bool status = quadSpiReceive1LINE(qspi, M25P16_INSTRUCTION_RDID, 0, &in[1], sizeof(in) - 1);
if (!status) {
return false;
}
#endif /* !defined(M25P16_QUADSPI_DEVICE) */
// Manufacturer, memory type, and capacity
chipID = (in[1] << 16) | (in[2] << 8) | (in[3]);
@ -256,6 +288,9 @@ static bool m25p16_readIdentification(void)
*/
bool m25p16_init(int flashNumToUse)
{
bool detected;
#if !defined(M25P16_QUADSPI_DEVICE)
// SPI Mode
busDev = busDeviceInit(BUSTYPE_SPI, DEVHW_M25P16, flashNumToUse, OWNER_FLASH);
if (busDev == NULL) {
return false;
@ -264,8 +299,25 @@ bool m25p16_init(int flashNumToUse)
#ifndef M25P16_SPI_SHARED
busSetSpeed(busDev, BUS_SPEED_FAST);
#endif
#else
// QUADSPI Mode
UNUSED(flashNumToUse);
quadSpiPinConfigure(M25P16_QUADSPI_DEVICE);
quadSpiInitDevice(M25P16_QUADSPI_DEVICE);
return m25p16_readIdentification();
qspi = quadSpiInstanceByDevice(M25P16_QUADSPI_DEVICE);
quadSpiSetDivisor(qspi, QUADSPI_CLOCK_INITIALISATION);
#endif /* M25P16_QUADSPI_DEVICE */
detected = m25p16_readIdentification();
#if defined(M25P16_QUADSPI_DEVICE)
if (detected) {
quadSpiSetDivisor(qspi, QUADSPI_CLOCK_ULTRAFAST);
}
#endif
return detected;
}
void m25p16_setCommandAddress(uint8_t *buf, uint32_t address, bool useLongAddress)
@ -284,17 +336,20 @@ void m25p16_setCommandAddress(uint8_t *buf, uint32_t address, bool useLongAddres
*/
void m25p16_eraseSector(uint32_t address)
{
uint8_t out[5] = { M25P16_INSTRUCTION_SECTOR_ERASE };
m25p16_setCommandAddress(&out[1], address, isLargeFlash);
if (!m25p16_waitForReady(0)) {
return;
}
m25p16_writeEnable();
#if !defined(M25P16_QUADSPI_DEVICE)
uint8_t out[5] = { M25P16_INSTRUCTION_SECTOR_ERASE };
m25p16_setCommandAddress(&out[1], address, isLargeFlash);
busTransfer(busDev, NULL, out, isLargeFlash ? 5 : 4);
#else
quadSpiInstructionWithAddress1LINE(qspi, M25P16_INSTRUCTION_SECTOR_ERASE, 0, address, isLargeFlash ? 32 : 24);
#endif /* !defined(M25P16_QUADSPI_DEVICE) */
m25p16_setTimeout(SECTOR_ERASE_TIMEOUT_MILLIS);
}
@ -329,6 +384,14 @@ void m25p16_eraseCompletely(void)
*/
uint32_t m25p16_pageProgram(uint32_t address, const uint8_t *data, int length)
{
if (!m25p16_waitForReady(0)) {
// return same address to indicate timeout
return address;
}
m25p16_writeEnable();
#if !defined(M25P16_QUADSPI_DEVICE)
uint8_t command[5] = { M25P16_INSTRUCTION_PAGE_PROGRAM };
busTransferDescriptor_t txn[2] = {
@ -338,14 +401,10 @@ uint32_t m25p16_pageProgram(uint32_t address, const uint8_t *data, int length)
m25p16_setCommandAddress(&command[1], address, isLargeFlash);
if (!m25p16_waitForReady(0)) {
// return same address to indicate timeout
return address;
}
m25p16_writeEnable();
busTransferMultiple(busDev, txn, 2);
#else
quadSpiTransmitWithAddress4LINES(qspi, M25P16_INSTRUCTION_QPAGE_PROGRAM, 0, address, isLargeFlash ? 32 : 24, data, length);
#endif /* !defined(M25P16_QUADSPI_DEVICE) */
m25p16_setTimeout(DEFAULT_TIMEOUT_MILLIS);
@ -362,6 +421,11 @@ uint32_t m25p16_pageProgram(uint32_t address, const uint8_t *data, int length)
*/
int m25p16_readBytes(uint32_t address, uint8_t *buffer, int length)
{
if (!m25p16_waitForReady(0)) {
return 0;
}
#if !defined(M25P16_QUADSPI_DEVICE)
uint8_t command[5] = { M25P16_INSTRUCTION_READ_BYTES };
busTransferDescriptor_t txn[2] = {
@ -371,11 +435,11 @@ int m25p16_readBytes(uint32_t address, uint8_t *buffer, int length)
m25p16_setCommandAddress(&command[1], address, isLargeFlash);
if (!m25p16_waitForReady(0)) {
return 0;
}
busTransferMultiple(busDev, txn, 2);
#else
quadSpiReceiveWithAddress4LINES(qspi, M25P16_INSTRUCTION_QUAD_READ, M25P16_FAST_READ_DUMMY_CYCLES,
address, isLargeFlash ? 32 : 24, buffer, length);
#endif /* !defined(M25P16_QUADSPI_DEVICE) */
return length;
}

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

@ -31,7 +31,7 @@ struct opflowDev_s;
typedef struct opflowData_s {
timeDelta_t deltaTime; // Integration timeframe of motionX/Y
int32_t flowRateRaw[3]; // Flow rotation in raw sensor uints (per deltaTime interval). Use dummy 3-rd axis (always zero) for compatibility with alignment functions
float flowRateRaw[3]; // Flow rotation in raw sensor uints (per deltaTime interval). Use dummy 3-rd axis (always zero) for compatibility with alignment functions
int16_t quality;
} opflowData_t;

2
src/main/drivers/rangefinder/rangefinder_vl53l0x.c
View file

@ -367,7 +367,7 @@ uint16_t encodeTimeout(uint16_t timeout_mclks)
// Defaults to 0.25 MCPS as initialized by the ST API and this library.
bool setSignalRateLimit(busDevice_t * busDev, float limit_Mcps)
{
if (limit_Mcps < 0 || limit_Mcps > 511.99) {
if (limit_Mcps < 0 || limit_Mcps > 511.99f) {
return false;
}

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

@ -19,7 +19,7 @@
const char * const ownerNames[OWNER_TOTAL_COUNT] = {
"FREE", "PWM/IO", "MOTOR", "SERVO", "SOFTSERIAL", "ADC", "SERIAL", "TIMER",
"RANGEFINDER", "SYSTEM", "SPI", "I2C", "SDCARD", "FLASH", "USB", "BEEPER", "OSD",
"RANGEFINDER", "SYSTEM", "SPI", "QUADSPI", "I2C", "SDCARD", "FLASH", "USB", "BEEPER", "OSD",
"BARO", "MPU", "INVERTER", "LED STRIP", "LED", "RECEIVER", "TRANSMITTER",
"VTX", "SPI_PREINIT", "COMPASS", "TEMPERATURE", "1-WIRE", "AIRSPEED", "OLED DISPLAY",
"PINIO", "IRLOCK"
@ -33,6 +33,8 @@ const char * const resourceNames[RESOURCE_TOTAL_COUNT] = {
"EXTI",
"SCL", "SDA",
"SCK", "MOSI", "MISO", "CS",
"CLK","BK1IO0", "BK1IO1", "BK1IO2", "BK1IO3", "BK1CS",
"BK2IO0", "BK2IO1", "BK2IO2", "BK2IO3", "BK2CS",
"CH1", "CH2", "CH3", "CH4",
"CE"
};

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

@ -31,6 +31,7 @@ typedef enum {
OWNER_RANGEFINDER,
OWNER_SYSTEM,
OWNER_SPI,
OWNER_QUADSPI,
OWNER_I2C,
OWNER_SDCARD,
OWNER_FLASH,
@ -68,6 +69,10 @@ typedef enum {
RESOURCE_EXTI,
RESOURCE_I2C_SCL, RESOURCE_I2C_SDA,
RESOURCE_SPI_SCK, RESOURCE_SPI_MOSI, RESOURCE_SPI_MISO, RESOURCE_SPI_CS,
RESOURCE_QUADSPI_CLK, RESOURCE_QUADSPI_BK1IO0, RESOURCE_QUADSPI_BK1IO1,
RESOURCE_QUADSPI_BK1IO2, RESOURCE_QUADSPI_BK1IO3, RESOURCE_QUADSPI_BK1CS,
RESOURCE_QUADSPI_BK2IO0, RESOURCE_QUADSPI_BK2IO1, RESOURCE_QUADSPI_BK2IO2,
RESOURCE_QUADSPI_BK2IO3, RESOURCE_QUADSPI_BK2CS,
RESOURCE_ADC_CH1, RESOURCE_ADC_CH2, RESOURCE_ADC_CH3, RESOURCE_ADC_CH4,
RESOURCE_RX_CE,
RESOURCE_TOTAL_COUNT

18
src/main/drivers/sdcard/sdcard_spi.c
View file

@ -90,10 +90,6 @@ static void sdcardSpi_reset(void)
return;
}
if (sdcard.state >= SDCARD_STATE_READY) {
busSetSpeed(sdcard.dev, BUS_SPEED_INITIALIZATION);
}
sdcard.failureCount++;
if (sdcard.failureCount >= SDCARD_MAX_CONSECUTIVE_FAILURES) {
sdcard.state = SDCARD_STATE_NOT_PRESENT;
@ -474,6 +470,8 @@ static bool sdcardSpi_poll(void)
sdcard.state = SDCARD_STATE_NOT_PRESENT;
}
}
busSetSpeed(sdcard.dev, BUS_SPEED_STANDARD);
break;
case SDCARD_STATE_CARD_INIT_IN_PROGRESS:
@ -511,6 +509,8 @@ static bool sdcardSpi_poll(void)
}
}
}
busSetSpeed(sdcard.dev, BUS_SPEED_STANDARD);
break;
case SDCARD_STATE_INITIALIZATION_RECEIVE_CID:
if (sdcardSpi_receiveCID()) {
@ -532,6 +532,8 @@ static bool sdcardSpi_poll(void)
sdcard.state = SDCARD_STATE_READY;
goto doMore;
} // else keep waiting for the CID to arrive
busSetSpeed(sdcard.dev, BUS_SPEED_STANDARD);
break;
case SDCARD_STATE_SENDING_WRITE:
// Have we finished sending the write yet?
@ -851,16 +853,18 @@ void sdcardSpi_init(void)
busSetSpeed(sdcard.dev, BUS_SPEED_INITIALIZATION);
// SDCard wants 1ms minimum delay after power is applied to it
delay(1000);
delay(1);
// Transmit at least 74 dummy clock cycles with CS high so the SD card can start up
busDeselectDevice(sdcard.dev);
busTransfer(sdcard.dev, NULL, NULL, SDCARD_INIT_NUM_DUMMY_BYTES);
IOHi(sdcard.dev->busdev.spi.csnPin);
SPI_TypeDef * instance = spiInstanceByDevice(sdcard.dev->busdev.spi.spiBus);
spiTransfer(instance, NULL, NULL, SDCARD_INIT_NUM_DUMMY_BYTES);
// Wait for that transmission to finish before we enable the SDCard, so it receives the required number of cycles:
int time = 100000;
while (busIsBusy(sdcard.dev)) {
if (time-- == 0) {
busSetSpeed(sdcard.dev, BUS_SPEED_STANDARD);
busDeviceDeInit(sdcard.dev);
sdcard.dev = NULL;
sdcard.state = SDCARD_STATE_NOT_PRESENT;

26
src/main/drivers/sdcard/sdmmc_sdio_h7xx.c
View file

@ -127,14 +127,22 @@ void sdioPinConfigure(void)
sdioHardware = &sdioPinHardware[SDCARD_SDIO_DEVICE];
sdioPin[SDIO_PIN_CK] = sdioHardware[SDCARD_SDIO_DEVICE].sdioPinCK[0];
sdioPin[SDIO_PIN_CMD] = sdioHardware[SDCARD_SDIO_DEVICE].sdioPinCMD[0];
sdioPin[SDIO_PIN_D0] = sdioHardware[SDCARD_SDIO_DEVICE].sdioPinD0[0];
#ifdef SDCARD_SDIO2_CK_ALT
sdioPin[SDIO_PIN_CK] = sdioHardware->sdioPinCK[1];
#else
sdioPin[SDIO_PIN_CK] = sdioHardware->sdioPinCK[0];
#endif
#ifdef SDCARD_SDIO2_CMD_ALT
sdioPin[SDIO_PIN_CMD] = sdioHardware->sdioPinCMD[1];
#else
sdioPin[SDIO_PIN_CMD] = sdioHardware->sdioPinCMD[0];
#endif
sdioPin[SDIO_PIN_D0] = sdioHardware->sdioPinD0[0];
#ifdef SDCARD_SDIO_4BIT
sdioPin[SDIO_PIN_D1] = sdioHardware[SDCARD_SDIO_DEVICE].sdioPinD1[0];
sdioPin[SDIO_PIN_D2] = sdioHardware[SDCARD_SDIO_DEVICE].sdioPinD2[0];
sdioPin[SDIO_PIN_D3] = sdioHardware[SDCARD_SDIO_DEVICE].sdioPinD3[0];
sdioPin[SDIO_PIN_D1] = sdioHardware->sdioPinD1[0];
sdioPin[SDIO_PIN_D2] = sdioHardware->sdioPinD2[0];
sdioPin[SDIO_PIN_D3] = sdioHardware->sdioPinD3[0];
#endif
}
@ -254,7 +262,11 @@ bool SD_Init(void)
hsd1.Init.BusWide = SDMMC_BUS_WIDE_1B; // FIXME untested
#endif
hsd1.Init.HardwareFlowControl = SDMMC_HARDWARE_FLOW_CONTROL_ENABLE;
hsd1.Init.ClockDiv = 1; // 200Mhz / (2 * 1 ) = 100Mhz, used for "UltraHigh speed SD card" only, see HAL_SD_ConfigWideBusOperation, SDMMC_HSpeed_CLK_DIV, SDMMC_NSpeed_CLK_DIV
#ifdef SDCARD_SDIO_NORMAL_SPEED
hsd1.Init.ClockDiv = SDMMC_NSpeed_CLK_DIV;
#else
hsd1.Init.ClockDiv = 1; // 200Mhz / (2 * 1 ) = 100Mhz, used for "UltraHigh speed SD card" only, see HAL_SD_ConfigWideBusOperation, SDMMC_HSpeed_CLK_DIV, SDMMC_NSpeed_CLK_DIV
#endif
status = HAL_SD_Init(&hsd1); // Will call HAL_SD_MspInit

108
src/main/drivers/serial_tcp.c
View file

@ -37,6 +37,7 @@
#include <netdb.h>
#include <arpa/inet.h>
#include <errno.h>
#include <netinet/tcp.h>
#include "common/utils.h"
@ -46,66 +47,6 @@
static const struct serialPortVTable tcpVTable[];
static tcpPort_t tcpPorts[SERIAL_PORT_COUNT];
static int lookup_address (char *name, int port, int type, struct sockaddr *addr, socklen_t* len )
{
struct addrinfo *servinfo, *p;
struct addrinfo hints = {.ai_family = AF_UNSPEC, .ai_socktype = type, .ai_flags = AI_V4MAPPED|AI_ADDRCONFIG};
if (name == NULL) {
hints.ai_flags |= AI_PASSIVE;
}
/*
This nonsense is to uniformly deliver the same sa_family regardless of whether
name is NULL or non-NULL
Otherwise, at least on Linux, we get
- V6,V4 for the non-null case and
- V4,V6 for the null case, regardless of gai.conf
Which may confuse consumers.
FreeBSD and Windows behave consistently, giving V6 for Ipv6 enabled stacks in all cases
unless a quad dotted address is specificied
*/
struct addrinfo *p4 = NULL;
struct addrinfo *p6 = NULL;
int result;
char aport[16];
snprintf(aport, sizeof(aport), "%d", port);
if ((result = getaddrinfo(name, aport, &hints, &servinfo)) != 0) {
fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(result));
return result;
} else {
for(p = servinfo; p != NULL; p = p->ai_next) {
if(p->ai_family == AF_INET6)
p6 = p;
else if(p->ai_family == AF_INET)
p4 = p;
}
if (p6 != NULL)
p = p6;
else if (p4 != NULL)
p = p4;
else
return -1;
memcpy(addr, p->ai_addr, p->ai_addrlen);
*len = p->ai_addrlen;
freeaddrinfo(servinfo);
}
return 0;
}
static char *tcpGetAddressString(struct sockaddr *addr)
{
static char straddr[INET6_ADDRSTRLEN];
void *ipaddr;
if (addr->sa_family == AF_INET6) {
struct sockaddr_in6 * ip = (struct sockaddr_in6*)addr;
ipaddr = &ip->sin6_addr;
} else {
struct sockaddr_in * ip = (struct sockaddr_in*)addr;
ipaddr = &ip->sin_addr;
}
const char *res = inet_ntop(addr->sa_family, ipaddr, straddr, sizeof straddr);
return (char *)res;
}
static void *tcpReceiveThread(void* arg)
{
tcpPort_t *port = (tcpPort_t*)arg;
@ -125,8 +66,8 @@ static tcpPort_t *tcpReConfigure(tcpPort_t *port, uint32_t id)
}
uint16_t tcpPort = BASE_IP_ADDRESS + id - 1;
if (lookup_address(NULL, tcpPort, SOCK_STREAM, (struct sockaddr*)&port->sockAddress, &sockaddrlen) != 0) {
return NULL;
if (lookupAddress(NULL, tcpPort, SOCK_STREAM, (struct sockaddr*)&port->sockAddress, &sockaddrlen) != 0) {
return NULL;
}
port->socketFd = socket(((struct sockaddr*)&port->sockAddress)->sa_family, SOCK_STREAM, IPPROTO_TCP);
@ -138,15 +79,16 @@ static tcpPort_t *tcpReConfigure(tcpPort_t *port, uint32_t id)
#ifdef __CYGWIN__
// Sadly necesary to enforce dual-stack behaviour on Windows networking ,,,
if (((struct sockaddr*)&port->sockAddress)->sa_family == AF_INET6) {
int v6only=0;
err = setsockopt(port->socketFd, IPPROTO_IPV6, IPV6_V6ONLY, &v6only, sizeof(v6only));
if (err != 0) {
fprintf(stderr,"[SOCKET] setting V6ONLY=false: %s\n", strerror(errno));
}
int v6only=0;
err = setsockopt(port->socketFd, IPPROTO_IPV6, IPV6_V6ONLY, &v6only, sizeof(v6only));
if (err != 0) {
fprintf(stderr,"[SOCKET] setting V6ONLY=false: %s\n", strerror(errno));
}
}
#endif
int one = 1;
err = setsockopt(port->socketFd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one));
err = setsockopt(port->socketFd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
err = fcntl(port->socketFd, F_SETFL, fcntl(port->socketFd, F_GETFL, 0) | O_NONBLOCK);
@ -168,16 +110,17 @@ static tcpPort_t *tcpReConfigure(tcpPort_t *port, uint32_t id)
fprintf(stderr, "[SOCKET] Unable to listen.\n");
return NULL;
}
fprintf(stderr, "[SOCKET] Bind TCP %s port %d to UART%d\n",
tcpGetAddressString((struct sockaddr*)&port->sockAddress), tcpPort, id);
char addrbuf[IPADDRESS_PRINT_BUFLEN];
char *addrptr = prettyPrintAddress((struct sockaddr *)&port->sockAddress, addrbuf, IPADDRESS_PRINT_BUFLEN);
if (addrptr != NULL) {
fprintf(stderr, "[SOCKET] Bind TCP %s to UART%d\n", addrptr, id);
}
return port;
}
int tcpReceive(tcpPort_t *port)
{
char addrbuf[IPADDRESS_PRINT_BUFLEN];
if (!port->isClientConnected) {
fd_set fds;
@ -190,14 +133,17 @@ int tcpReceive(tcpPort_t *port)
return -1;
}
socklen_t addrLen = sizeof(struct sockaddr_storage);
socklen_t addrLen = sizeof(struct sockaddr_storage);
port->clientSocketFd = accept(port->socketFd,(struct sockaddr*)&port->clientAddress, &addrLen);
if (port->clientSocketFd < 1) {
fprintf(stderr, "[SOCKET] Can't accept connection.\n");
return -1;
}
fprintf(stderr, "[SOCKET] %s connected to UART%d\n", tcpGetAddressString((struct sockaddr *)&port->clientAddress), port->id);
char *addrptr = prettyPrintAddress((struct sockaddr *)&port->clientAddress, addrbuf, IPADDRESS_PRINT_BUFLEN);
if (addrptr != NULL) {
fprintf(stderr, "[SOCKET] %s connected to UART%d\n", addrptr, port->id);
}
port->isClientConnected = true;
}
@ -206,12 +152,14 @@ int tcpReceive(tcpPort_t *port)
// recv() under cygwin does not recognise the closed connection under certain circumstances, but returns ECONNRESET as an error.
if (port->isClientConnected && (recvSize == 0 || ( recvSize == -1 && errno == ECONNRESET))) {
fprintf(stderr, "[SOCKET] %s disconnected from UART%d\n", tcpGetAddressString((struct sockaddr *)&port->clientAddress), port->id);
close(port->clientSocketFd);
memset(&port->clientAddress, 0, sizeof(port->clientAddress));
port->isClientConnected = false;
return 0;
char *addrptr = prettyPrintAddress((struct sockaddr *)&port->clientAddress, addrbuf, IPADDRESS_PRINT_BUFLEN);
if (addrptr != NULL) {
fprintf(stderr, "[SOCKET] %s disconnected from UART%d\n", addrptr, port->id);
}
close(port->clientSocketFd);
memset(&port->clientAddress, 0, sizeof(port->clientAddress));
port->isClientConnected = false;
return 0;
}
for (ssize_t i = 0; i < recvSize; i++) {

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

@ -24,6 +24,7 @@
#include "platform.h"
#include "build/debug.h"
#include "common/log.h"
#include "vtx_common.h"
@ -73,8 +74,9 @@ void vtxCommonSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t ch
if (!vtxDevice)
return;
if ((band > vtxDevice->capability.bandCount) || (channel > vtxDevice->capability.channelCount))
if ((band > vtxDevice->capability.bandCount) || (channel > vtxDevice->capability.channelCount)) {
return;
}
if (vtxDevice->vTable->setBandAndChannel) {
vtxDevice->vTable->setBandAndChannel(vtxDevice, band, channel);

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

@ -36,7 +36,7 @@
#define VTX_SETTINGS_MIN_FREQUENCY_MHZ 0 //min freq (in MHz) for 'vtx_freq' setting
#define VTX_SETTINGS_MAX_FREQUENCY_MHZ 5999 //max freq (in MHz) for 'vtx_freq' setting
#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP)
#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) || defined(USE_VTX_MSP)
#define VTX_SETTINGS_POWER_COUNT 5
#define VTX_SETTINGS_DEFAULT_POWER 1
@ -63,6 +63,7 @@ typedef enum {
VTXDEV_SMARTAUDIO = 3,
VTXDEV_TRAMP = 4,
VTXDEV_FFPV = 5,
VTXDEV_MSP = 6,
VTXDEV_UNKNOWN = 0xFF,
} vtxDevType_e;

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

@ -89,6 +89,7 @@ bool cliMode = false;
#include "io/beeper.h"
#include "io/flashfs.h"
#include "io/gps.h"
#include "io/gps_ublox.h"
#include "io/ledstrip.h"
#include "io/osd.h"
#include "io/serial.h"
@ -3468,6 +3469,23 @@ static void cliStatus(char *cmdline)
cliPrintLinefeed();
#endif
if (featureConfigured(FEATURE_GPS) && (gpsConfig()->provider == GPS_UBLOX || gpsConfig()->provider == GPS_UBLOX7PLUS)) {
cliPrint("GPS: ");
cliPrintf("HW Version: %s Proto: %d.%02d Baud: %d", getGpsHwVersion(), getGpsProtoMajorVersion(), getGpsProtoMinorVersion(), getGpsBaudrate());
cliPrintLinefeed();
//cliPrintLinef(" GNSS Capabilities: %d", gpsUbloxCapLastUpdate());
cliPrintLinef(" GNSS Capabilities:");
cliPrintLine(" GNSS Provider active/default");
cliPrintLine(" GPS 1/1");
if(gpsUbloxHasGalileo())
cliPrintLinef(" Galileo %d/%d", gpsUbloxGalileoEnabled(), gpsUbloxGalileoDefault());
if(gpsUbloxHasBeidou())
cliPrintLinef(" BeiDou %d/%d", gpsUbloxBeidouEnabled(), gpsUbloxBeidouDefault());
if(gpsUbloxHasGlonass())
cliPrintLinef(" Glonass %d/%d", gpsUbloxGlonassEnabled(), gpsUbloxGlonassDefault());
cliPrintLinef(" Max concurrent: %d", gpsUbloxMaxGnss());
}
// If we are blocked by PWM init - provide more information
if (getPwmInitError() != PWM_INIT_ERROR_NONE) {
cliPrintLinef("PWM output init error: %s", getPwmInitErrorMessage());

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

@ -469,11 +469,11 @@ void setConfigBatteryProfileAndWriteEEPROM(uint8_t profileIndex)
beeperConfirmationBeeps(profileIndex + 1);
}
void setGyroCalibration(int16_t getGyroZero[XYZ_AXIS_COUNT])
void setGyroCalibration(float getGyroZero[XYZ_AXIS_COUNT])
{
gyroConfigMutable()->gyro_zero_cal[X] = getGyroZero[X];
gyroConfigMutable()->gyro_zero_cal[Y] = getGyroZero[Y];
gyroConfigMutable()->gyro_zero_cal[Z] = getGyroZero[Z];
gyroConfigMutable()->gyro_zero_cal[X] = (int16_t) getGyroZero[X];
gyroConfigMutable()->gyro_zero_cal[Y] = (int16_t) getGyroZero[Y];
gyroConfigMutable()->gyro_zero_cal[Z] = (int16_t) getGyroZero[Z];
}
void setGravityCalibration(float getGravity)

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

@ -138,7 +138,7 @@ uint8_t getConfigBatteryProfile(void);
bool setConfigBatteryProfile(uint8_t profileIndex);
void setConfigBatteryProfileAndWriteEEPROM(uint8_t profileIndex);
void setGyroCalibration(int16_t getGyroZero[XYZ_AXIS_COUNT]);
void setGyroCalibration(float getGyroZero[XYZ_AXIS_COUNT]);
void setGravityCalibration(float getGravity);
bool canSoftwareSerialBeUsed(void);

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

@ -983,12 +983,12 @@ void taskUpdateRxMain(timeUs_t currentTimeUs)
}
// returns seconds
float getFlightTime()
float getFlightTime(void)
{
return US2S(flightTime);
}
float getArmTime()
float getArmTime(void)
{
return US2S(armTime);
}

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

@ -119,6 +119,7 @@
#include "io/vtx_control.h"
#include "io/vtx_smartaudio.h"
#include "io/vtx_tramp.h"
#include "io/vtx_msp.h"
#include "io/vtx_ffpv24g.h"
#include "io/piniobox.h"
@ -174,7 +175,7 @@ void flashLedsAndBeep(void)
LED1_TOGGLE;
LED0_TOGGLE;
delay(25);
if (!(getPreferredBeeperOffMask() & (1 << (BEEPER_SYSTEM_INIT - 1))))
if (!(getBeeperOffMask() & (1 << (BEEPER_SYSTEM_INIT - 1))))
BEEP_ON;
delay(25);
BEEP_OFF;
@ -665,6 +666,10 @@ void init(void)
vtxFuriousFPVInit();
#endif
#ifdef USE_VTX_MSP
vtxMspInit();
#endif
#endif // USE_VTX_CONTROL
// Now that everything has powered up the voltage and cell count be determined.

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

@ -2482,12 +2482,24 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
if (newFrequency <= VTXCOMMON_MSP_BANDCHAN_CHKVAL) { //value is band and channel
const uint8_t newBand = (newFrequency / 8) + 1;
const uint8_t newChannel = (newFrequency % 8) + 1;
if(vtxSettingsConfig()->band != newBand || vtxSettingsConfig()->channel != newChannel) {
vtxCommonSetBandAndChannel(vtxDevice, newBand, newChannel);
}
vtxSettingsConfigMutable()->band = newBand;
vtxSettingsConfigMutable()->channel = newChannel;
}
if (sbufBytesRemaining(src) > 1) {
vtxSettingsConfigMutable()->power = sbufReadU8(src);
uint8_t newPower = sbufReadU8(src);
uint8_t currentPower = 0;
vtxCommonGetPowerIndex(vtxDevice, &currentPower);
if (newPower != currentPower) {
vtxCommonSetPowerByIndex(vtxDevice, newPower);
vtxSettingsConfigMutable()->power = newPower;
}
// Delegate pitmode to vtx directly
const uint8_t newPitmode = sbufReadU8(src);
uint8_t currentPitmode = 0;
@ -3484,7 +3496,9 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
DISABLE_ARMING_FLAG(SIMULATOR_MODE_HITL);
#ifdef USE_BARO
if ( requestedSensors[SENSOR_INDEX_BARO] != BARO_NONE ) {
baroStartCalibration();
}
#endif
#ifdef USE_MAG
DISABLE_STATE(COMPASS_CALIBRATED);
@ -3495,10 +3509,15 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
disarm(DISARM_SWITCH); // Disarm to prevent motor output!!!
}
} else if (!areSensorsCalibrating()) {
} else {
if (!ARMING_FLAG(SIMULATOR_MODE_HITL)) { // Just once
#ifdef USE_BARO
baroStartCalibration();
if ( requestedSensors[SENSOR_INDEX_BARO] != BARO_NONE ) {
sensorsSet(SENSOR_BARO);
setTaskEnabled(TASK_BARO, true);
DISABLE_ARMING_FLAG(ARMING_DISABLED_HARDWARE_FAILURE);
baroStartCalibration();
}
#endif
#ifdef USE_MAG

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

@ -64,6 +64,7 @@
#include "io/rcdevice_cam.h"
#include "io/smartport_master.h"
#include "io/vtx.h"
#include "io/vtx_msp.h"
#include "io/osd_dji_hd.h"
#include "io/displayport_msp_osd.h"
#include "io/servo_sbus.h"
@ -109,6 +110,9 @@ void taskHandleSerial(timeUs_t currentTimeUs)
#ifdef USE_MSP_OSD
// Capture MSP Displayport messages to determine if VTX is connected
mspOsdSerialProcess(mspFcProcessCommand);
#ifdef USE_VTX_MSP
mspVtxSerialProcess(mspFcProcessCommand);
#endif
#endif
}

4
src/main/fc/runtime_config.c
View file

@ -179,7 +179,7 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void)
#ifdef USE_SIMULATOR
simulatorData_t simulatorData = {
flags: 0,
debugIndex: 0
.flags = 0,
.debugIndex = 0
};
#endif

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

@ -1542,12 +1542,30 @@ groups:
default_value: OFF
field: ubloxUseGalileo
type: bool
- name: gps_ublox_use_beidou
description: "Enable use of Beidou satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires gps hardware support [OFF/ON]."
default_value: OFF
field: ubloxUseBeidou
type: bool
- name: gps_ublox_use_glonass
description: "Enable use of Glonass satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires gps haardware support [OFF/ON]."
default_value: OFF
field: ubloxUseGlonass
type: bool
- name: gps_min_sats
description: "Minimum number of GPS satellites in view to acquire GPS_FIX and consider GPS position valid. Some GPS receivers appeared to be very inaccurate with low satellite count."
default_value: 6
field: gpsMinSats
min: 5
max: 10
- name: gps_ublox_nav_hz
description: "Navigation update rate for UBLOX7 receivers. Some receivers may limit the maximum number of satellites tracked when set to a higher rate or even stop sending navigation updates if the value is too high. Some M10 devices can do up to 25Hz. 10 is a safe value for M8 and newer."
default_value: 10
field: ubloxNavHz
type: uint8_t
min: 5
max: 200
- name: PG_RC_CONTROLS_CONFIG
type: rcControlsConfig_t
@ -2297,6 +2315,11 @@ groups:
field: general.land_detect_sensitivity
min: 1
max: 15
- name: nav_landing_bump_detection
description: "Allows immediate landing detection based on G bump at touchdown when set to ON. Requires a barometer and currently only works for multirotors."
default_value: OFF
field: general.flags.landing_bump_detection
type: bool
- name: nav_use_midthr_for_althold
description: "If set to OFF, the FC remembers your throttle stick position when enabling ALTHOLD and treats it as a neutral midpoint for holding altitude"
default_value: OFF

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

@ -36,7 +36,7 @@ static uint32_t arm_distance_cm;
static uint32_t arm_mWhDrawn;
static uint32_t flyingEnergy; // energy drawn during flying up to last disarm (ARMED) mWh
uint32_t getFlyingEnergy() {
uint32_t getFlyingEnergy(void) {
return flyingEnergy;
}
#endif

4
src/main/flight/imu.c
View file

@ -277,7 +277,7 @@ static void imuResetOrientationQuaternion(const fpVector3_t * accBF)
static bool imuValidateQuaternion(const fpQuaternion_t * quat)
{
const float check = fabs(quat->q0) + fabs(quat->q1) + fabs(quat->q2) + fabs(quat->q3);
const float check = fabsf(quat->q0) + fabsf(quat->q1) + fabsf(quat->q2) + fabsf(quat->q3);
if (!isnan(check) && !isinf(check)) {
return true;
@ -480,7 +480,7 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
const float thetaMagnitudeSq = vectorNormSquared(&vTheta);
// If calculated rotation is zero - don't update quaternion
if (thetaMagnitudeSq >= 1e-20) {
if (thetaMagnitudeSq >= 1e-20f) {
// Calculate quaternion delta:
// Theta is a axis/angle rotation. Direction of a vector is axis, magnitude is angle/2.
// Proper quaternion from axis/angle involves computing sin/cos, but the formula becomes numerically unstable as Theta approaches zero.

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

@ -461,7 +461,7 @@ static int getReversibleMotorsThrottleDeadband(void)
return feature(FEATURE_MOTOR_STOP) ? reversibleMotorsConfig()->neutral : directionValue;
}
void FAST_CODE mixTable()
void FAST_CODE mixTable(void)
{
#ifdef USE_DSHOT
if (FLIGHT_MODE(TURTLE_MODE)) {

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

@ -154,7 +154,7 @@ static EXTENDED_FASTRAM float iTermAntigravityGain;
#endif
static EXTENDED_FASTRAM uint8_t usedPidControllerType;
typedef void (*pidControllerFnPtr)(pidState_t *pidState, flight_dynamics_index_t axis, float dT);
typedef void (*pidControllerFnPtr)(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv);
static EXTENDED_FASTRAM pidControllerFnPtr pidControllerApplyFn;
static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn;
static EXTENDED_FASTRAM bool levelingEnabled = false;
@ -477,7 +477,7 @@ void schedulePidGainsUpdate(void)
pidGainsUpdateRequired = true;
}
void updatePIDCoefficients()
void updatePIDCoefficients(void)
{
STATIC_FASTRAM uint16_t prevThrottle = 0;
@ -625,7 +625,7 @@ static void pidLevel(const float angleTarget, pidState_t *pidState, flight_dynam
}
}
float angleRateTarget = constrainf(angleErrorDeg * (pidBank()->pid[PID_LEVEL].P / FP_PID_LEVEL_P_MULTIPLIER), -currentControlRateProfile->stabilized.rates[axis] * 10.0f, currentControlRateProfile->stabilized.rates[axis] * 10.0f);
float angleRateTarget = constrainf(angleErrorDeg * (pidBank()->pid[PID_LEVEL].P * FP_PID_LEVEL_P_MULTIPLIER), -currentControlRateProfile->stabilized.rates[axis] * 10.0f, currentControlRateProfile->stabilized.rates[axis] * 10.0f);
// Apply simple LPF to angleRateTarget to make response less jerky
// Ideas behind this:
@ -681,13 +681,13 @@ static float pTermProcess(pidState_t *pidState, float rateError, float dT) {
}
#ifdef USE_D_BOOST
static float FAST_CODE applyDBoost(pidState_t *pidState, float currentRateTarget, float dT) {
static float FAST_CODE applyDBoost(pidState_t *pidState, float currentRateTarget, float dT, float dT_inv) {
float dBoost = 1.0f;
const float dBoostGyroDelta = (pidState->gyroRate - pidState->previousRateGyro) / dT;
const float dBoostGyroDelta = (pidState->gyroRate - pidState->previousRateGyro) * dT_inv;
const float dBoostGyroAcceleration = fabsf(biquadFilterApply(&pidState->dBoostGyroLpf, dBoostGyroDelta));
const float dBoostRateAcceleration = fabsf((currentRateTarget - pidState->previousRateTarget) / dT);
const float dBoostRateAcceleration = fabsf((currentRateTarget - pidState->previousRateTarget) * dT_inv);
if (dBoostGyroAcceleration >= dBoostRateAcceleration) {
//Gyro is accelerating faster than setpoint, we want to smooth out
@ -710,7 +710,7 @@ static float applyDBoost(pidState_t *pidState, float dT) {
}
#endif
static float dTermProcess(pidState_t *pidState, float currentRateTarget, float dT) {
static float dTermProcess(pidState_t *pidState, float currentRateTarget, float dT, float dT_inv) {
// Calculate new D-term
float newDTerm = 0;
if (pidState->kD == 0) {
@ -722,7 +722,7 @@ static float dTermProcess(pidState_t *pidState, float currentRateTarget, float d
delta = dTermLpfFilterApplyFn((filter_t *) &pidState->dtermLpfState, delta);
// Calculate derivative
newDTerm = delta * (pidState->kD / dT) * applyDBoost(pidState, currentRateTarget, dT);
newDTerm = delta * (pidState->kD * dT_inv) * applyDBoost(pidState, currentRateTarget, dT, dT_inv);
}
return(newDTerm);
}
@ -736,19 +736,20 @@ static void applyItermLimiting(pidState_t *pidState) {
}
}
static void nullRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT) {
static void nullRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv) {
UNUSED(pidState);
UNUSED(axis);
UNUSED(dT);
UNUSED(dT_inv);
}
static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv)
{
const float rateTarget = getFlightAxisRateOverride(axis, pidState->rateTarget);
const float rateError = rateTarget - pidState->gyroRate;
const float newPTerm = pTermProcess(pidState, rateError, dT);
const float newDTerm = dTermProcess(pidState, rateTarget, dT);
const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv);
const float newFFTerm = rateTarget * pidState->kFF;
/*
@ -806,14 +807,14 @@ static float FAST_CODE applyItermRelax(const int axis, float currentPidSetpoint,
return itermErrorRate;
}
static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv)
{
const float rateTarget = getFlightAxisRateOverride(axis, pidState->rateTarget);
const float rateError = rateTarget - pidState->gyroRate;
const float newPTerm = pTermProcess(pidState, rateError, dT);
const float newDTerm = dTermProcess(pidState, rateTarget, dT);
const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv);
const float rateTargetDelta = rateTarget - pidState->previousRateTarget;
const float rateTargetDeltaFiltered = pt3FilterApply(&pidState->rateTargetFilter, rateTargetDelta);
@ -864,7 +865,7 @@ void resetHeadingHoldTarget(int16_t heading)
pt1FilterReset(&headingHoldRateFilter, 0.0f);
}
int16_t getHeadingHoldTarget() {
int16_t getHeadingHoldTarget(void) {
return headingHoldTarget;
}
@ -1062,6 +1063,9 @@ void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis
void FAST_CODE pidController(float dT)
{
const float dT_inv = 1.0f / dT;
if (!pidFiltersConfigured) {
return;
}
@ -1135,7 +1139,7 @@ void FAST_CODE pidController(float dT)
}
// Prevent strong Iterm accumulation during stick inputs
antiWindupScaler = constrainf((1.0f - getMotorMixRange()) / motorItermWindupPoint, 0.0f, 1.0f);
antiWindupScaler = constrainf((1.0f - getMotorMixRange()) * motorItermWindupPoint, 0.0f, 1.0f);
for (int axis = 0; axis < 3; axis++) {
// Apply setpoint rate of change limits
@ -1145,7 +1149,7 @@ void FAST_CODE pidController(float dT)
checkItermLimitingActive(&pidState[axis]);
checkItermFreezingActive(&pidState[axis], axis);
pidControllerApplyFn(&pidState[axis], axis, dT);
pidControllerApplyFn(&pidState[axis], axis, dT, dT_inv);
}
}
@ -1177,7 +1181,7 @@ void pidInit(void)
itermRelax = pidProfile()->iterm_relax;
yawLpfHz = pidProfile()->yaw_lpf_hz;
motorItermWindupPoint = 1.0f - (pidProfile()->itermWindupPointPercent / 100.0f);
motorItermWindupPoint = 1.0f / (1.0f - (pidProfile()->itermWindupPointPercent / 100.0f));
#ifdef USE_D_BOOST
dBoostMin = pidProfile()->dBoostMin;

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

@ -47,7 +47,7 @@ FP-PID has been rescaled to match LuxFloat (and MWRewrite) from Cleanflight 1.13
#define FP_PID_RATE_I_MULTIPLIER 4.0f
#define FP_PID_RATE_D_MULTIPLIER 1905.0f
#define FP_PID_RATE_D_FF_MULTIPLIER 7270.0f
#define FP_PID_LEVEL_P_MULTIPLIER 6.56f // Level P gain units is [1/sec] and angle error is [deg] => [deg/s]
#define FP_PID_LEVEL_P_MULTIPLIER 1.0f / 6.56f // Level P gain units is [1/sec] and angle error is [deg] => [deg/s]
#define FP_PID_YAWHOLD_P_MULTIPLIER 80.0f
#define MC_ITERM_RELAX_SETPOINT_THRESHOLD 40.0f

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

@ -187,7 +187,7 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa
// Use different max rate in ANLGE mode
if (FLIGHT_MODE(ANGLE_MODE)) {
float maxDesiredRateInAngleMode = DECIDEGREES_TO_DEGREES(pidProfile()->max_angle_inclination[axis] * 1.0f) * pidBank()->pid[PID_LEVEL].P / FP_PID_LEVEL_P_MULTIPLIER;
float maxDesiredRateInAngleMode = DECIDEGREES_TO_DEGREES(pidProfile()->max_angle_inclination[axis] * 1.0f) * pidBank()->pid[PID_LEVEL].P * FP_PID_LEVEL_P_MULTIPLIER;
maxDesiredRate = MIN(maxRateSetting, maxDesiredRateInAngleMode);
}

18
src/main/flight/power_limits.c
View file

@ -162,14 +162,14 @@ void powerLimiterApply(int16_t *throttleCommand) {
int32_t overCurrent = current - activeCurrentLimit;
if (lastCallTimestamp) {
currentThrAttnIntegrator = constrainf(currentThrAttnIntegrator + overCurrent * powerLimitsConfig()->piI * callTimeDelta * 2e-7, 0, PWM_RANGE_MAX - PWM_RANGE_MIN);
currentThrAttnIntegrator = constrainf(currentThrAttnIntegrator + overCurrent * powerLimitsConfig()->piI * callTimeDelta * 2e-7f, 0, PWM_RANGE_MAX - PWM_RANGE_MIN);
}
float currentThrAttnProportional = MAX(0, overCurrent) * powerLimitsConfig()->piP * 1e-3;
float currentThrAttnProportional = MAX(0, overCurrent) * powerLimitsConfig()->piP * 1e-3f;
uint16_t currentThrAttn = lrintf(pt1FilterApply3(&currentThrAttnFilter, currentThrAttnProportional + currentThrAttnIntegrator, callTimeDelta * 1e-6));
uint16_t currentThrAttn = lrintf(pt1FilterApply3(&currentThrAttnFilter, currentThrAttnProportional + currentThrAttnIntegrator, callTimeDelta * 1e-6f));
throttleBase = wasLimitingCurrent ? lrintf(pt1FilterApply3(&currentThrLimitingBaseFilter, *throttleCommand, callTimeDelta * 1e-6)) : *throttleCommand;
throttleBase = wasLimitingCurrent ? lrintf(pt1FilterApply3(&currentThrLimitingBaseFilter, *throttleCommand, callTimeDelta * 1e-6f)) : *throttleCommand;
uint16_t currentThrAttned = MAX(PWM_RANGE_MIN, (int16_t)throttleBase - currentThrAttn);
if (activeCurrentLimit && currentThrAttned < *throttleCommand) {
@ -191,12 +191,12 @@ void powerLimiterApply(int16_t *throttleCommand) {
int32_t overPower = power - activePowerLimit;
if (lastCallTimestamp) {
powerThrAttnIntegrator = constrainf(powerThrAttnIntegrator + overPower * powerLimitsConfig()->piI * callTimeDelta / voltage * 2e-5, 0, PWM_RANGE_MAX - PWM_RANGE_MIN);
powerThrAttnIntegrator = constrainf(powerThrAttnIntegrator + overPower * powerLimitsConfig()->piI * callTimeDelta / voltage * 2e-5f, 0, PWM_RANGE_MAX - PWM_RANGE_MIN);
}
float powerThrAttnProportional = MAX(0, overPower) * powerLimitsConfig()->piP / voltage * 1e-1;
float powerThrAttnProportional = MAX(0, overPower) * powerLimitsConfig()->piP / voltage * 1e-1f;
uint16_t powerThrAttn = lrintf(pt1FilterApply3(&powerThrAttnFilter, powerThrAttnProportional + powerThrAttnIntegrator, callTimeDelta * 1e-6));
uint16_t powerThrAttn = lrintf(pt1FilterApply3(&powerThrAttnFilter, powerThrAttnProportional + powerThrAttnIntegrator, callTimeDelta * 1e-6f));
throttleBase = wasLimitingPower ? lrintf(pt1FilterApply3(&powerThrLimitingBaseFilter, *throttleCommand, callTimeDelta * 1e-6)) : *throttleCommand;
uint16_t powerThrAttned = MAX(PWM_RANGE_MIN, (int16_t)throttleBase - powerThrAttn);
@ -244,11 +244,11 @@ bool powerLimiterIsLimitingPower(void) {
// returns seconds
float powerLimiterGetRemainingBurstTime(void) {
uint16_t currentBurstOverContinuous = currentBatteryProfile->powerLimits.burstCurrent - currentBatteryProfile->powerLimits.continuousCurrent;
float remainingCurrentBurstTime = burstCurrentReserve / currentBurstOverContinuous / 1e7;
float remainingCurrentBurstTime = burstCurrentReserve / currentBurstOverContinuous / 1e7f;
#ifdef USE_ADC
uint16_t powerBurstOverContinuous = currentBatteryProfile->powerLimits.burstPower - currentBatteryProfile->powerLimits.continuousPower;
float remainingPowerBurstTime = burstPowerReserve / powerBurstOverContinuous / 1e7;
float remainingPowerBurstTime = burstPowerReserve / powerBurstOverContinuous / 1e7f;
if (!currentBatteryProfile->powerLimits.continuousCurrent) {
return remainingPowerBurstTime;

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

@ -150,7 +150,7 @@ static float calculateRemainingEnergyBeforeRTH(bool takeWindIntoAccount) {
float RTH_heading; // degrees
#ifdef USE_WIND_ESTIMATOR
uint16_t windHeading; // centidegrees
uint16_t windHeading = 0; // centidegrees
const float horizontalWindSpeed = takeWindIntoAccount ? getEstimatedHorizontalWindSpeed(&windHeading) / 100 : 0; // m/s
const float windHeadingDegrees = CENTIDEGREES_TO_DEGREES((float)windHeading);
const float verticalWindSpeed = -getEstimatedWindSpeed(Z) / 100; //from NED to NEU

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

@ -54,7 +54,7 @@ float applySmithPredictor(uint8_t axis, smithPredictor_t *predictor, float sampl
}
void smithPredictorInit(smithPredictor_t *predictor, float delay, float strength, uint16_t filterLpfHz, uint32_t looptime) {
if (delay > 0.1) {
if (delay > 0.1f) {
predictor->enabled = true;
predictor->samples = (delay * 1000) / looptime;
predictor->idx = 0;

2
src/main/io/beeper.c
View file

@ -218,7 +218,7 @@ static const beeperTableEntry_t *currentBeeperEntry = NULL;
*/
void beeper(beeperMode_e mode)
{
if (mode == BEEPER_SILENCE || ((getBeeperOffMask() & (1 << (BEEPER_USB-1))) && (feature(FEATURE_VBAT) && (getBatteryCellCount() < 2))) || IS_RC_MODE_ACTIVE(BOXBEEPERMUTE)) {
if (mode == BEEPER_SILENCE || ((getBeeperOffMask() & (1 << (BEEPER_USB-1))) && (feature(FEATURE_VBAT) && (getBatteryState() == BATTERY_NOT_PRESENT))) || IS_RC_MODE_ACTIVE(BOXBEEPERMUTE)) {
beeperSilence();
return;
}

2
src/main/io/dashboard.c
View file

@ -400,7 +400,7 @@ static void showStatusPage(void)
#ifdef USE_MAG
if (sensors(SENSOR_MAG)) {
tfp_sprintf(lineBuffer, "HDG: %d", DECIDEGREES_TO_DEGREES(attitude.values.yaw));
tfp_sprintf(lineBuffer, "HDG: %d", (int)DECIDEGREES_TO_DEGREES(attitude.values.yaw));
padHalfLineBuffer();
i2c_OLED_set_line(rowIndex);
i2c_OLED_send_string(lineBuffer);

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

@ -515,4 +515,13 @@ void mspOsdSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn)
}
}
mspPort_t *getMspOsdPort()
{
if (mspPort.port) {
return &mspPort;
}
return NULL;
}
#endif // USE_MSP_OSD

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

@ -31,6 +31,7 @@ typedef struct displayPort_s displayPort_t;
displayPort_t *mspOsdDisplayPortInit(const videoSystem_e videoSystem);
void mspOsdSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn);
mspPort_t *getMspOsdPort(void);
// MSP displayport V2 attribute byte bit functions
#define DISPLAYPORT_MSP_ATTR_FONTPAGE 0 // Select bank of 256 characters as per displayPortSeverity_e

2
src/main/io/frsky_osd.c
View file

@ -365,7 +365,7 @@ static void frskyOSDUpdateReceiveBuffer(void)
// Full uvarint decoded. Check against buffer size.
if (state.recvBuffer.expected > sizeof(state.recvBuffer.data)) {
FRSKY_OSD_ERROR("Can't handle payload of size %u with a buffer of size %u",
state.recvBuffer.expected, sizeof(state.recvBuffer.data));
state.recvBuffer.expected, (unsigned int)sizeof(state.recvBuffer.data));
frskyOSDResetReceiveBuffer();
break;
}

67
src/main/io/gps.c
View file

@ -55,6 +55,7 @@
#include "io/serial.h"
#include "io/gps.h"
#include "io/gps_private.h"
#include "io/gps_ublox.h"
#include "navigation/navigation.h"
#include "navigation/navigation_private.h"
@ -91,7 +92,7 @@ gpsSolutionData_t gpsSolDRV; //filled by driver
gpsSolutionData_t gpsSol; //used in the rest of the code
// Map gpsBaudRate_e index to baudRate_e
baudRate_e gpsToSerialBaudRate[GPS_BAUDRATE_COUNT] = { BAUD_115200, BAUD_57600, BAUD_38400, BAUD_19200, BAUD_9600, BAUD_230400 };
baudRate_e gpsToSerialBaudRate[GPS_BAUDRATE_COUNT] = { BAUD_115200, BAUD_57600, BAUD_38400, BAUD_19200, BAUD_9600, BAUD_230400, BAUD_460800, BAUD_921600 };
static gpsProviderDescriptor_t gpsProviders[GPS_PROVIDER_COUNT] = {
/* NMEA GPS */
@ -130,7 +131,7 @@ static gpsProviderDescriptor_t gpsProviders[GPS_PROVIDER_COUNT] = {
};
PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 2);
PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 3);
PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig,
.provider = SETTING_GPS_PROVIDER_DEFAULT,
@ -139,9 +140,69 @@ PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig,
.autoBaud = SETTING_GPS_AUTO_BAUD_DEFAULT,
.dynModel = SETTING_GPS_DYN_MODEL_DEFAULT,
.gpsMinSats = SETTING_GPS_MIN_SATS_DEFAULT,
.ubloxUseGalileo = SETTING_GPS_UBLOX_USE_GALILEO_DEFAULT
.ubloxUseGalileo = SETTING_GPS_UBLOX_USE_GALILEO_DEFAULT,
.ubloxUseBeidou = SETTING_GPS_UBLOX_USE_BEIDOU_DEFAULT,
.ubloxUseGlonass = SETTING_GPS_UBLOX_USE_GLONASS_DEFAULT,
.ubloxNavHz = SETTING_GPS_UBLOX_NAV_HZ_DEFAULT
);
int getGpsBaudrate(void)
{
switch(gpsState.baudrateIndex)
{
case GPS_BAUDRATE_115200:
return 115200;
case GPS_BAUDRATE_57600:
return 57600;
case GPS_BAUDRATE_38400:
return 38400;
case GPS_BAUDRATE_19200:
return 19200;
case GPS_BAUDRATE_9600:
return 9600;
case GPS_BAUDRATE_230400:
return 230400;
case GPS_BAUDRATE_460800:
return 460800;
case GPS_BAUDRATE_921600:
return 921600;
default:
return 0;
}
}
const char *getGpsHwVersion(void)
{
switch(gpsState.hwVersion)
{
case UBX_HW_VERSION_UBLOX5:
return "UBLOX5";
case UBX_HW_VERSION_UBLOX6:
return "UBLOX6";
case UBX_HW_VERSION_UBLOX7:
return "UBLOX7";
case UBX_HW_VERSION_UBLOX8:
return "UBLOX8";
case UBX_HW_VERSION_UBLOX9:
return "UBLOX9";
case UBX_HW_VERSION_UBLOX10:
return "UBLOX10";
default:
return "Unknown";
}
}
uint8_t getGpsProtoMajorVersion(void)
{
return gpsState.swVersionMajor;
}
uint8_t getGpsProtoMinorVersion(void)
{
return gpsState.swVersionMinor;
}
void gpsSetState(gpsState_e state)
{
gpsState.state = state;

11
src/main/io/gps.h
View file

@ -59,6 +59,8 @@ typedef enum {
GPS_BAUDRATE_19200,
GPS_BAUDRATE_9600,
GPS_BAUDRATE_230400,
GPS_BAUDRATE_460800,
GPS_BAUDRATE_921600,
GPS_BAUDRATE_COUNT
} gpsBaudRate_e;
@ -93,7 +95,10 @@ typedef struct gpsConfig_s {
gpsAutoBaud_e autoBaud;
gpsDynModel_e dynModel;
bool ubloxUseGalileo;
bool ubloxUseBeidou;
bool ubloxUseGlonass;
uint8_t gpsMinSats;
uint8_t ubloxNavHz;
} gpsConfig_t;
PG_DECLARE(gpsConfig_t, gpsConfig);
@ -163,6 +168,12 @@ struct serialPort_s;
void gpsEnablePassthrough(struct serialPort_s *gpsPassthroughPort);
void mspGPSReceiveNewData(const uint8_t * bufferPtr);
const char *getGpsHwVersion(void);
uint8_t getGpsProtoMajorVersion(void);
uint8_t getGpsProtoMinorVersion(void);
int getGpsBaudrate(void);
#if defined(USE_GPS_FAKE)
void gpsFakeSet(
gpsFixType_e fixType,

4
src/main/io/gps_private.h
View file

@ -43,6 +43,8 @@ typedef struct {
serialPort_t * gpsPort; // Serial GPS only
uint32_t hwVersion;
uint8_t swVersionMajor;
uint8_t swVersionMinor;
gpsState_e state;
gpsBaudRate_e baudrateIndex;
@ -54,6 +56,8 @@ typedef struct {
timeMs_t lastMessageMs;
timeMs_t timeoutMs;
timeMs_t baseTimeoutMs;
timeMs_t lastCapaPoolMs;
timeMs_t lastCapaUpdMs;
} gpsReceiverData_t;
extern gpsReceiverData_t gpsState;

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

@ -42,6 +42,7 @@
#include "fc/config.h"
#include "fc/runtime_config.h"
#include "fc/settings.h"
#include "io/serial.h"
#include "io/gps.h"
@ -49,31 +50,9 @@
#include "scheduler/protothreads.h"
#define GPS_CFG_CMD_TIMEOUT_MS 200
#define GPS_VERSION_RETRY_TIMES 2
#define MAX_UBLOX_PAYLOAD_SIZE 256
#define UBLOX_BUFFER_SIZE MAX_UBLOX_PAYLOAD_SIZE
#define UBLOX_SBAS_MESSAGE_LENGTH 16
#include "gps_ublox.h"
#include "gps_ublox_utils.h"
#define UBX_DYNMODEL_PEDESTRIAN 3
#define UBX_DYNMODEL_AIR_1G 6
#define UBX_DYNMODEL_AIR_4G 8
#define UBX_FIXMODE_2D_ONLY 1
#define UBX_FIXMODE_3D_ONLY 2
#define UBX_FIXMODE_AUTO 3
#define UBX_VALID_GPS_DATE(valid) (valid & 1 << 0)
#define UBX_VALID_GPS_TIME(valid) (valid & 1 << 1)
#define UBX_VALID_GPS_DATE_TIME(valid) (UBX_VALID_GPS_DATE(valid) && UBX_VALID_GPS_TIME(valid))
#define UBX_HW_VERSION_UNKNOWN 0
#define UBX_HW_VERSION_UBLOX5 500
#define UBX_HW_VERSION_UBLOX6 600
#define UBX_HW_VERSION_UBLOX7 700
#define UBX_HW_VERSION_UBLOX8 800
#define UBX_HW_VERSION_UBLOX9 900
#define UBX_HW_VERSION_UBLOX10 1000
// SBAS_AUTO, SBAS_EGNOS, SBAS_WAAS, SBAS_MSAS, SBAS_GAGAN, SBAS_NONE
// note PRNs last upadted 2020-12-18
@ -97,255 +76,10 @@ static const char * baudInitDataNMEA[GPS_BAUDRATE_COUNT] = {
"$PUBX,41,1,0003,0001,19200,0*23\r\n", // GPS_BAUDRATE_19200
"$PUBX,41,1,0003,0001,9600,0*16\r\n", // GPS_BAUDRATE_9600
"$PUBX,41,1,0003,0001,230400,0*1C\r\n", // GPS_BAUDRATE_230400
"$PUBX,41,1,0003,0001,460800,0*1C\r\n", // GPS_BAUDRATE_460800
"$PUBX,41,1,0003,0001,921600,0*1C\r\n" // GPS_BAUDRATE_921600
};
// payload types
typedef struct {
uint8_t mode;
uint8_t usage;
uint8_t maxSBAS;
uint8_t scanmode2;
uint32_t scanmode1;
} ubx_sbas;
typedef struct {
uint8_t class;
uint8_t id;
uint8_t rate;
} ubx_msg;
typedef struct {
uint16_t meas;
uint16_t nav;
uint16_t time;
} ubx_rate;
typedef struct {
uint8_t gnssId;
uint8_t resTrkCh;
uint8_t maxTrkCh;
uint8_t reserved1;
// flags
uint8_t enabled;
uint8_t undefined0;
uint8_t sigCfgMask;
uint8_t undefined1;
} ubx_gnss_element_t;
typedef struct {
uint8_t msgVer;
uint8_t numTrkChHw;
uint8_t numTrkChUse;
uint8_t numConfigBlocks;
ubx_gnss_element_t config[0];
} ubx_gnss_msg_t;
#define MAX_GNSS 7
#define MAX_GNSS_SIZE_BYTES (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t)*MAX_GNSS)
typedef union {
uint8_t bytes[MAX_GNSS_SIZE_BYTES]; // placeholder
ubx_sbas sbas;
ubx_msg msg;
ubx_rate rate;
ubx_gnss_msg_t gnss;
} ubx_payload;
// UBX support
typedef struct {
uint8_t preamble1;
uint8_t preamble2;
uint8_t msg_class;
uint8_t msg_id;
uint16_t length;
} ubx_header;
typedef struct {
ubx_header header;
ubx_payload payload;
} __attribute__((packed)) ubx_message;
typedef struct {
char swVersion[30]; // Zero-terminated Software Version String
char hwVersion[10]; // Zero-terminated Hardware Version String
} ubx_mon_ver;
typedef struct {
uint32_t time; // GPS msToW
int32_t longitude;
int32_t latitude;
int32_t altitude_ellipsoid;
int32_t altitude_msl;
uint32_t horizontal_accuracy;
uint32_t vertical_accuracy;
} ubx_nav_posllh;
typedef struct {
uint32_t time; // GPS msToW
uint8_t fix_type;
uint8_t fix_status;
uint8_t differential_status;
uint8_t res;
uint32_t time_to_first_fix;
uint32_t uptime; // milliseconds
} ubx_nav_status;
typedef struct {
uint32_t time;
int32_t time_nsec;
int16_t week;
uint8_t fix_type;
uint8_t fix_status;
int32_t ecef_x;
int32_t ecef_y;
int32_t ecef_z;
uint32_t position_accuracy_3d;
int32_t ecef_x_velocity;
int32_t ecef_y_velocity;
int32_t ecef_z_velocity;
uint32_t speed_accuracy;
uint16_t position_DOP;
uint8_t res;
uint8_t satellites;
uint32_t res2;
} ubx_nav_solution;
typedef struct {
uint32_t time; // GPS msToW
int32_t ned_north;
int32_t ned_east;
int32_t ned_down;
uint32_t speed_3d;
uint32_t speed_2d;
int32_t heading_2d;
uint32_t speed_accuracy;
uint32_t heading_accuracy;
} ubx_nav_velned;
typedef struct {
uint8_t chn; // Channel number, 255 for SVx not assigned to channel
uint8_t svid; // Satellite ID
uint8_t flags; // Bitmask
uint8_t quality; // Bitfield
uint8_t cno; // Carrier to Noise Ratio (Signal Strength) // dbHz, 0-55.
uint8_t elev; // Elevation in integer degrees
int16_t azim; // Azimuth in integer degrees
int32_t prRes; // Pseudo range residual in centimetres
} ubx_nav_svinfo_channel;
typedef struct {
uint32_t time; // GPS Millisecond time of week
uint8_t numCh; // Number of channels
uint8_t globalFlags; // Bitmask, Chip hardware generation 0:Antaris, 1:u-blox 5, 2:u-blox 6
uint16_t reserved2; // Reserved
ubx_nav_svinfo_channel channel[16]; // 16 satellites * 12 byte
} ubx_nav_svinfo;
typedef struct {
uint32_t time; // GPS msToW
uint32_t tAcc;
int32_t nano;
uint16_t year;
uint8_t month;
uint8_t day;
uint8_t hour;
uint8_t min;
uint8_t sec;
uint8_t valid;
} ubx_nav_timeutc;
typedef struct {
uint32_t time; // GPS msToW
uint16_t year;
uint8_t month;
uint8_t day;
uint8_t hour;
uint8_t min;
uint8_t sec;
uint8_t valid;
uint32_t tAcc;
int32_t nano;
uint8_t fix_type;
uint8_t fix_status;
uint8_t reserved1;
uint8_t satellites;
int32_t longitude;
int32_t latitude;
int32_t altitude_ellipsoid;
int32_t altitude_msl;
uint32_t horizontal_accuracy;
uint32_t vertical_accuracy;
int32_t ned_north;
int32_t ned_east;
int32_t ned_down;
int32_t speed_2d;
int32_t heading_2d;
uint32_t speed_accuracy;
uint32_t heading_accuracy;
uint16_t position_DOP;
uint16_t reserved2;
uint16_t reserved3;
} ubx_nav_pvt;
typedef struct {
uint8_t class;
uint8_t msg;
} ubx_ack_ack;
enum {
PREAMBLE1 = 0xB5,
PREAMBLE2 = 0x62,
CLASS_NAV = 0x01,
CLASS_ACK = 0x05,
CLASS_CFG = 0x06,
CLASS_MON = 0x0A,
MSG_CLASS_UBX = 0x01,
MSG_CLASS_NMEA = 0xF0,
MSG_VER = 0x04,
MSG_ACK_NACK = 0x00,
MSG_ACK_ACK = 0x01,
MSG_NMEA_GGA = 0x0,
MSG_NMEA_GLL = 0x1,
MSG_NMEA_GSA = 0x2,
MSG_NMEA_GSV = 0x3,
MSG_NMEA_RMC = 0x4,
MSG_NMEA_VGS = 0x5,
MSG_POSLLH = 0x2,
MSG_STATUS = 0x3,
MSG_SOL = 0x6,
MSG_PVT = 0x7,
MSG_VELNED = 0x12,
MSG_TIMEUTC = 0x21,
MSG_SVINFO = 0x30,
MSG_NAV_SAT = 0x35,
MSG_NAV_SIG = 0x35,
MSG_CFG_PRT = 0x00,
MSG_CFG_RATE = 0x08,
MSG_CFG_SET_RATE = 0x01,
MSG_CFG_NAV_SETTINGS = 0x24,
MSG_CFG_SBAS = 0x16,
MSG_CFG_GNSS = 0x3e
} ubx_protocol_bytes;
enum {
FIX_NONE = 0,
FIX_DEAD_RECKONING = 1,
FIX_2D = 2,
FIX_3D = 3,
FIX_GPS_DEAD_RECKONING = 4,
FIX_TIME = 5
} ubs_nav_fix_type;
enum {
NAV_STATUS_FIX_VALID = 1
} ubx_nav_status_bits;
enum {
UBX_ACK_WAITING = 0,
UBX_ACK_GOT_ACK = 1,
UBX_ACK_GOT_NAK = 2
} ubx_ack_state;
// Packet checksum accumulators
static uint8_t _ck_a;
static uint8_t _ck_b;
@ -369,7 +103,12 @@ static bool _new_position;
static bool _new_speed;
// Need this to determine if Galileo capable only
static bool capGalileo;
static struct {
uint8_t supported;
int capMaxGnss;
uint8_t defaultGnss;
uint8_t enabledGnss;
} ubx_capabilities = { };
// Example packet sizes from UBlox u-center from a Glonass capable GPS receiver.
//15:17:55 R -> UBX NAV-STATUS, Size 24, 'Navigation Status'
@ -401,16 +140,63 @@ static union {
ubx_mon_ver ver;
ubx_nav_timeutc timeutc;
ubx_ack_ack ack;
ubx_mon_gnss gnss;
uint8_t bytes[UBLOX_BUFFER_SIZE];
} _buffer;
void _update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b)
bool gpsUbloxHasGalileo(void)
{
while (len--) {
*ck_a += *data;
*ck_b += *ck_a;
data++;
return (ubx_capabilities.supported & UBX_MON_GNSS_GALILEO_MASK);
}
bool gpsUbloxHasBeidou(void)
{
return ubx_capabilities.supported & UBX_MON_GNSS_BEIDOU_MASK;
}
bool gpsUbloxHasGlonass(void)
{
return ubx_capabilities.supported & UBX_MON_GNSS_GLONASS_MASK;
}
bool gpsUbloxGalileoDefault(void)
{
return ubx_capabilities.defaultGnss & UBX_MON_GNSS_GALILEO_MASK;
}
bool gpsUbloxBeidouDefault(void)
{
return ubx_capabilities.defaultGnss & UBX_MON_GNSS_BEIDOU_MASK;
}
bool gpsUbloxGlonassDefault(void)
{
return ubx_capabilities.defaultGnss & UBX_MON_GNSS_GLONASS_MASK;
}
bool gpsUbloxGalileoEnabled(void)
{
return ubx_capabilities.enabledGnss & UBX_MON_GNSS_GALILEO_MASK;
}
bool gpsUbloxBeidouEnabled(void)
{
return ubx_capabilities.enabledGnss & UBX_MON_GNSS_BEIDOU_MASK;
}
bool gpsUbloxGlonassEnabled(void)
{
return ubx_capabilities.enabledGnss & UBX_MON_GNSS_GLONASS_MASK;
}
uint8_t gpsUbloxMaxGnss(void)
{
return ubx_capabilities.capMaxGnss;
}
timeMs_t gpsUbloxCapLastUpdate(void)
{
return gpsState.lastCapaUpdMs;
}
static uint8_t gpsMapFixType(bool fixValid, uint8_t ubloxFixType)
@ -427,7 +213,7 @@ static void sendConfigMessageUBLOX(void)
uint8_t ck_a=0, ck_b=0;
send_buffer.message.header.preamble1=PREAMBLE1;
send_buffer.message.header.preamble2=PREAMBLE2;
_update_checksum(&send_buffer.bytes[2], send_buffer.message.header.length+4, &ck_a, &ck_b);
ublox_update_checksum(&send_buffer.bytes[2], send_buffer.message.header.length+4, &ck_a, &ck_b);
send_buffer.bytes[send_buffer.message.header.length+6] = ck_a;
send_buffer.bytes[send_buffer.message.header.length+7] = ck_b;
serialWriteBuf(gpsState.gpsPort, send_buffer.bytes, send_buffer.message.header.length+8);
@ -445,6 +231,15 @@ static void pollVersion(void)
sendConfigMessageUBLOX();
}
static void pollGnssCapabilities(void)
{
send_buffer.message.header.msg_class = CLASS_MON;
send_buffer.message.header.msg_id = MSG_MON_GNSS;
send_buffer.message.header.length = 0;
sendConfigMessageUBLOX();
}
static const uint8_t default_payload[] = {
0xFF, 0xFF, 0x03, 0x03, 0x00, // CFG-NAV5 - Set engine settings (original MWII code)
0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00, // Collected by resetting a GPS unit to defaults. Changing mode to Pedistrian and
@ -454,7 +249,26 @@ static const uint8_t default_payload[] = {
#define GNSSID_SBAS 1
#define GNSSID_GALILEO 2
#define GNSSID_BEIDOU 3
#define GNSSID_GZSS 5
#define GNSSID_GLONASS 6
// M10 ublox protocol info:
// https://content.u-blox.com/sites/default/files/u-blox-M10-SPG-5.10_InterfaceDescription_UBX-21035062.pdf
static void ubloxSendSetCfgBytes(ubx_config_data8_payload_t *kvPairs, uint8_t count)
{
ubx_config_data8_t cfg = {};
ubloxCfgFillBytes(&cfg, kvPairs, count);
serialWriteBuf(gpsState.gpsPort, (uint8_t *)&cfg, cfg.header.length+8);
_ack_waiting_msg = cfg.header.msg_id;
_ack_state = UBX_ACK_WAITING;
}
// Info on protocol used by M8-M9, check UBX-CFG-GNSS for gnss configuration
// https://content.u-blox.com/sites/default/files/products/documents/u-blox8-M8_ReceiverDescrProtSpec_UBX-13003221.pdf
// https://content.u-blox.com/sites/default/files/documents/u-blox-F9-HPG-1.32_InterfaceDescription_UBX-22008968.pdf
static int configureGNSS_SBAS(ubx_gnss_element_t * gnss_block)
{
gnss_block->gnssId = GNSSID_SBAS;
@ -473,13 +287,17 @@ static int configureGNSS_SBAS(ubx_gnss_element_t * gnss_block)
static int configureGNSS_GALILEO(ubx_gnss_element_t * gnss_block)
{
if (!capGalileo) {
if (!gpsUbloxHasGalileo()) {
return 0;
}
gnss_block->gnssId = GNSSID_GALILEO;
gnss_block->maxTrkCh = 8;
gnss_block->sigCfgMask = 1;
// sigCfgMask
// 0x01 = Galileo E1 (not supported for protocol versions less than 18.00)
// 0x10 = Galileo E5a // off by default
// 0x20 = Galileo E5b // off by default
gnss_block->sigCfgMask = 0x01;
if (gpsState.gpsConfig->ubloxUseGalileo) {
gnss_block->enabled = 1;
gnss_block->resTrkCh = 4;
@ -491,15 +309,109 @@ static int configureGNSS_GALILEO(ubx_gnss_element_t * gnss_block)
return 1;
}
static int configureGNSS_BEIDOU(ubx_gnss_element_t * gnss_block)
{
if (!gpsUbloxHasBeidou()) {
return 0;
}
gnss_block->gnssId = GNSSID_BEIDOU;
gnss_block->maxTrkCh = 8;
// sigCfgMask
// 0x01 = BeiDou B1I
// 0x10 = BeiDou B2I // off by default
// 0x80 = BeiDou B2A // off by default
gnss_block->sigCfgMask = 0x01;
if (gpsState.gpsConfig->ubloxUseBeidou) {
gnss_block->enabled = 1;
gnss_block->resTrkCh = 4;
} else {
gnss_block->enabled = 0;
gnss_block->resTrkCh = 0;
}
return 1;
}
/*
static int configureGNSS_GZSS(ubx_gnss_element_t * gnss_block)
{
if (!ubx_capabilities.capGzss) {
return 0;
}
gnss_block->gnssId = GNSSID_GZSS;
gnss_block->maxTrkCh = 8;
// L1C = 0x01
// L1S = 0x04
// L2C = 0x10
gnss_block->sigCfgMask = 0x01 | 0x04;
gnss_block->enabled = 1;
gnss_block->resTrkCh = 4;
return 1;
}
*/
static int configureGNSS_GLONASS(ubx_gnss_element_t * gnss_block)
{
if(!gpsUbloxHasGlonass()) {
return 0;
}
gnss_block->gnssId = GNSSID_GLONASS;
gnss_block->maxTrkCh = 8;
// 0x01 = GLONASS L1
// 0x10 = GLONASS L2 // off by default
gnss_block->sigCfgMask = 0x01;
if (gpsState.gpsConfig->ubloxUseGlonass) {
gnss_block->enabled = 1;
gnss_block->resTrkCh = 4;
} else {
gnss_block->enabled = 0;
gnss_block->resTrkCh = 0;
}
return 1;
}
static void configureGNSS10(void)
{
ubx_config_data8_payload_t gnssConfigValues[] = {
// SBAS
{UBLOX_CFG_SIGNAL_SBAS_ENA, 1},
{UBLOX_CFG_SIGNAL_SBAS_L1CA_ENA, 1},
// Galileo
{UBLOX_CFG_SIGNAL_GAL_ENA, gpsState.gpsConfig->ubloxUseGalileo},
{UBLOX_CFG_SIGNAL_GAL_E1_ENA, gpsState.gpsConfig->ubloxUseGalileo},
// Beidou
{UBLOX_CFG_SIGNAL_BDS_ENA, gpsState.gpsConfig->ubloxUseBeidou},
{UBLOX_CFG_SIGNAL_BDS_B1_ENA, gpsState.gpsConfig->ubloxUseBeidou},
{UBLOX_CFG_SIGNAL_BDS_B1C_ENA, 0},
// Should be enabled with GPS
{UBLOX_CFG_QZSS_ENA, 1},
{UBLOX_CFG_QZSS_L1CA_ENA, 1},
{UBLOX_CFG_QZSS_L1S_ENA, 1},
// Glonass
{UBLOX_CFG_GLO_ENA, gpsState.gpsConfig->ubloxUseGlonass},
{UBLOX_CFG_GLO_L1_ENA, gpsState.gpsConfig->ubloxUseGlonass}
};
ubloxSendSetCfgBytes(gnssConfigValues, 12);
}
static void configureGNSS(void)
{
int blocksUsed = 0;
send_buffer.message.header.msg_class = CLASS_CFG;
send_buffer.message.header.msg_id = MSG_CFG_GNSS;
send_buffer.message.header.msg_id = MSG_CFG_GNSS; // message deprecated in protocol > 23.01, should use UBX-CFG-VALSET/UBX-CFG-VALGET
send_buffer.message.payload.gnss.msgVer = 0;
send_buffer.message.payload.gnss.numTrkChHw = 0; // read only, so unset
send_buffer.message.payload.gnss.numTrkChUse = 32;
send_buffer.message.payload.gnss.numTrkChUse = 0xFF; // If set to 0xFF will use hardware max
/* SBAS, always generated */
blocksUsed += configureGNSS_SBAS(&send_buffer.message.payload.gnss.config[blocksUsed]);
@ -507,6 +419,12 @@ static void configureGNSS(void)
/* Galileo */
blocksUsed += configureGNSS_GALILEO(&send_buffer.message.payload.gnss.config[blocksUsed]);
/* BeiDou */
blocksUsed += configureGNSS_BEIDOU(&send_buffer.message.payload.gnss.config[blocksUsed]);
/* GLONASS */
blocksUsed += configureGNSS_GLONASS(&send_buffer.message.payload.gnss.config[blocksUsed]);
send_buffer.message.payload.gnss.numConfigBlocks = blocksUsed;
send_buffer.message.header.length = (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t)* blocksUsed);
sendConfigMessageUBLOX();
@ -523,12 +441,12 @@ static void configureNAV5(uint8_t dynModel, uint8_t fixMode)
sendConfigMessageUBLOX();
}
static void configureMSG(uint8_t class, uint8_t id, uint8_t rate)
static void configureMSG(uint8_t msg_class, uint8_t id, uint8_t rate)
{
send_buffer.message.header.msg_class = CLASS_CFG;
send_buffer.message.header.msg_id = MSG_CFG_SET_RATE;
send_buffer.message.header.length = 3;
send_buffer.message.payload.msg.class = class;
send_buffer.message.payload.msg.msg_class = msg_class;
send_buffer.message.payload.msg.id = id;
send_buffer.message.payload.msg.rate = rate;
sendConfigMessageUBLOX();
@ -565,6 +483,18 @@ static void configureSBAS(void)
sendConfigMessageUBLOX();
}
static void gpsDecodeProtocolVersion(const char *proto, size_t bufferLength)
{
if (bufferLength > 13 && (!strncmp(proto, "PROTVER=", 8) || !strncmp(proto, "PROTVER ", 8))) {
proto+=8;
float ver = atof(proto);
gpsState.swVersionMajor = (uint8_t)ver;
gpsState.swVersionMinor = (uint8_t)((ver - gpsState.swVersionMajor) * 100.0f);
}
}
static uint32_t gpsDecodeHardwareVersion(const char * szBuf, unsigned nBufSize)
{
// ublox_5 hwVersion 00040005
@ -600,7 +530,7 @@ static uint32_t gpsDecodeHardwareVersion(const char * szBuf, unsigned nBufSize)
return UBX_HW_VERSION_UNKNOWN;
}
static bool gpsParceFrameUBLOX(void)
static bool gpsParseFrameUBLOX(void)
{
switch (_msg_id) {
case MSG_POSLLH:
@ -690,18 +620,51 @@ static bool gpsParceFrameUBLOX(void)
case MSG_VER:
if (_class == CLASS_MON) {
gpsState.hwVersion = gpsDecodeHardwareVersion(_buffer.ver.hwVersion, sizeof(_buffer.ver.hwVersion));
if ((gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8) && (_buffer.ver.swVersion[9] > '2')) {
if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8) {
if (_buffer.ver.swVersion[9] > '2' || true) {
// check extensions;
// after hw + sw vers; each is 30 bytes
bool found = false;
for (int j = 40; j < _payload_length && !found; j += 30)
{
// Example content: GPS;GAL;BDS;GLO
if (strnstr((const char *)(_buffer.bytes + j), "GAL", 30))
{
ubx_capabilities.supported |= UBX_MON_GNSS_GALILEO_MASK;
found = true;
}
if (strnstr((const char *)(_buffer.bytes + j), "BDS", 30))
{
ubx_capabilities.supported |= UBX_MON_GNSS_BEIDOU_MASK;
found = true;
}
if (strnstr((const char *)(_buffer.bytes + j), "GLO", 30))
{
ubx_capabilities.supported |= UBX_MON_GNSS_GLONASS_MASK;
found = true;
}
}
}
for(int j = 40; j < _payload_length; j += 30) {
if (strnstr((const char *)(_buffer.bytes+j), "GAL", 30)) {
capGalileo = true;
if (strnstr((const char *)(_buffer.bytes + j), "PROTVER", 30)) {
gpsDecodeProtocolVersion((const char *)(_buffer.bytes + j), 30);
break;
}
}
}
}
break;
case MSG_MON_GNSS:
if(_class == CLASS_MON) {
if (_buffer.gnss.version == 0) {
ubx_capabilities.supported = _buffer.gnss.supported;
ubx_capabilities.defaultGnss = _buffer.gnss.defaultGnss;
ubx_capabilities.enabledGnss = _buffer.gnss.enabled;
ubx_capabilities.capMaxGnss = _buffer.gnss.maxConcurrent;
gpsState.lastCapaUpdMs = millis();
}
}
break;
case MSG_ACK_ACK:
if ((_ack_state == UBX_ACK_WAITING) && (_buffer.ack.msg == _ack_waiting_msg)) {
_ack_state = UBX_ACK_GOT_ACK;
@ -808,7 +771,7 @@ static bool gpsNewFrameUBLOX(uint8_t data)
break;
}
if (gpsParceFrameUBLOX()) {
if (gpsParseFrameUBLOX()) {
parsed = true;
}
}
@ -816,6 +779,11 @@ static bool gpsNewFrameUBLOX(uint8_t data)
return parsed;
}
static uint16_t hz2rate(uint8_t hz)
{
return 1000 / hz;
}
STATIC_PROTOTHREAD(gpsConfigure)
{
ptBegin(gpsConfigure);
@ -885,11 +853,19 @@ STATIC_PROTOTHREAD(gpsConfigure)
configureMSG(MSG_CLASS_UBX, MSG_NAV_SIG, 0);
ptWait(_ack_state == UBX_ACK_GOT_ACK);
// u-Blox 9 receivers such as M9N can do 10Hz as well, but the number of used satellites will be restricted to 16.
// Not mentioned in the datasheet
configureRATE(200);
if ((gpsState.gpsConfig->provider == GPS_UBLOX7PLUS) && (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX7)) {
configureRATE(hz2rate(gpsState.gpsConfig->ubloxNavHz)); // default 10Hz
} else {
configureRATE(hz2rate(5)); // 5Hz
gpsConfigMutable()->ubloxNavHz = SETTING_GPS_UBLOX_NAV_HZ_DEFAULT;
}
ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK);
if(_ack_state == UBX_ACK_GOT_NAK) { // Fallback to safe 5Hz in case of error
configureRATE(hz2rate(5)); // 5Hz
ptWait(_ack_state == UBX_ACK_GOT_ACK);
}
}
else {
// u-Blox 5/6/7/8 or unknown
// u-Blox 7-8 support PVT
@ -916,13 +892,18 @@ STATIC_PROTOTHREAD(gpsConfigure)
ptWait(_ack_state == UBX_ACK_GOT_ACK);
if ((gpsState.gpsConfig->provider == GPS_UBLOX7PLUS) && (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX7)) {
configureRATE(100); // 10Hz
configureRATE(hz2rate(gpsState.gpsConfig->ubloxNavHz)); // default 10Hz
}
else {
configureRATE(200); // 5Hz
configureRATE(hz2rate(5)); // 5Hz
}
ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK);
if(_ack_state == UBX_ACK_GOT_NAK) { // Fallback to safe 5Hz in case of error
configureRATE(hz2rate(5)); // 5Hz
ptWait(_ack_state == UBX_ACK_GOT_ACK);
}
}
// u-Blox 5/6 doesn't support PVT, use legacy config
// UNKNOWN also falls here, use as a last resort
else {
@ -962,10 +943,20 @@ STATIC_PROTOTHREAD(gpsConfigure)
ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS);
// Configure GNSS for M8N and later
if (gpsState.hwVersion >= 80000) {
if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8) {
gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT);
if(gpsState.hwVersion >= UBX_HW_VERSION_UBLOX10 || (gpsState.swVersionMajor>=23 && gpsState.swVersionMinor >= 1)) {
configureGNSS10();
} else {
configureGNSS();
}
ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS);
if(_ack_state == UBX_ACK_GOT_NAK) {
gpsConfigMutable()->ubloxUseGalileo = SETTING_GPS_UBLOX_USE_GALILEO_DEFAULT;
gpsConfigMutable()->ubloxUseBeidou = SETTING_GPS_UBLOX_USE_BEIDOU_DEFAULT;
gpsConfigMutable()->ubloxUseGlonass = SETTING_GPS_UBLOX_USE_GLONASS_DEFAULT;
}
}
ptEnd(0);
@ -1028,8 +1019,6 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread)
serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.baudrateIndex]]);
}
// Configure GPS module if enabled
if (gpsState.gpsConfig->autoConfig) {
// Reset protocol timeout
gpsSetProtocolTimeout(MAX(GPS_TIMEOUT, ((GPS_VERSION_RETRY_TIMES + 3) * GPS_CFG_CMD_TIMEOUT_MS)));
@ -1043,6 +1032,16 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread)
ptWaitTimeout((gpsState.hwVersion != UBX_HW_VERSION_UNKNOWN), GPS_CFG_CMD_TIMEOUT_MS);
} while(gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN);
gpsState.autoConfigStep = 0;
ubx_capabilities.supported = ubx_capabilities.enabledGnss = ubx_capabilities.defaultGnss = 0;
do {
pollGnssCapabilities();
gpsState.autoConfigStep++;
ptWaitTimeout((ubx_capabilities.capMaxGnss != 0), GPS_CFG_CMD_TIMEOUT_MS);
} while (gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && ubx_capabilities.capMaxGnss == 0);
// Configure GPS module if enabled
if (gpsState.gpsConfig->autoConfig) {
// Configure GPS
ptSpawn(gpsConfigure);
}
@ -1054,6 +1053,21 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread)
while (1) {
ptSemaphoreWait(semNewDataReady);
gpsProcessNewSolutionData(false);
if ((gpsState.gpsConfig->provider == GPS_UBLOX || gpsState.gpsConfig->provider == GPS_UBLOX7PLUS)) {
if ((millis() - gpsState.lastCapaPoolMs) > GPS_CAPA_INTERVAL) {
gpsState.lastCapaPoolMs = millis();
if (gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN)
{
pollVersion();
ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS);
}
pollGnssCapabilities();
ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS);
}
}
}
ptEnd(0);

429
src/main/io/gps_ublox.h Normal file
View file

@ -0,0 +1,429 @@
/*
* This file is part of INAV
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <stdint.h>
#include <stdbool.h>
#include "common/time.h"
#ifdef __cplusplus
extern "C" {
#endif
#define GPS_CFG_CMD_TIMEOUT_MS 500
#define GPS_VERSION_RETRY_TIMES 3
#define MAX_UBLOX_PAYLOAD_SIZE 256
#define UBLOX_BUFFER_SIZE MAX_UBLOX_PAYLOAD_SIZE
#define UBLOX_SBAS_MESSAGE_LENGTH 16
#define GPS_CAPA_INTERVAL 5000
#define UBX_DYNMODEL_PEDESTRIAN 3
#define UBX_DYNMODEL_AIR_1G 6
#define UBX_DYNMODEL_AIR_4G 8
#define UBX_FIXMODE_2D_ONLY 1
#define UBX_FIXMODE_3D_ONLY 2
#define UBX_FIXMODE_AUTO 3
#define UBX_VALID_GPS_DATE(valid) (valid & 1 << 0)
#define UBX_VALID_GPS_TIME(valid) (valid & 1 << 1)
#define UBX_VALID_GPS_DATE_TIME(valid) (UBX_VALID_GPS_DATE(valid) && UBX_VALID_GPS_TIME(valid))
#define UBX_HW_VERSION_UNKNOWN 0
#define UBX_HW_VERSION_UBLOX5 500
#define UBX_HW_VERSION_UBLOX6 600
#define UBX_HW_VERSION_UBLOX7 700
#define UBX_HW_VERSION_UBLOX8 800
#define UBX_HW_VERSION_UBLOX9 900
#define UBX_HW_VERSION_UBLOX10 1000
#define UBLOX_CFG_SIGNAL_SBAS_ENA 0x10310020 // U1
#define UBLOX_CFG_SIGNAL_SBAS_L1CA_ENA 0x10310005 // U1
#define UBLOX_CFG_SIGNAL_GAL_ENA 0x10310021 // U1
#define UBLOX_CFG_SIGNAL_GAL_E1_ENA 0x10310007 // U1
#define UBLOX_CFG_SIGNAL_BDS_ENA 0x10310022 // U1
#define UBLOX_CFG_SIGNAL_BDS_B1_ENA 0x1031000d // U1
#define UBLOX_CFG_SIGNAL_BDS_B1C_ENA 0x1031000f // U1 default off
#define UBLOX_CFG_QZSS_ENA 0x10310024 // U1
#define UBLOX_CFG_QZSS_L1CA_ENA 0x10310012 // U1
#define UBLOX_CFG_QZSS_L1S_ENA 0x10310014 // U1
#define UBLOX_CFG_GLO_ENA 0x10310025 // U1 default off - may conflict with other constelations
#define UBLOX_CFG_GLO_L1_ENA 0x10310018 // U1 default off
#define UBLOX_CFG_SBAS_PRNSCANMASK 0x50360006 // 0 = auto // X8
#define UBLOX_SBAS_ALL 0x0000000000000000 //Enable search for all SBAS PRNs
#define UBLOX_SBAS_PRN120 0x0000000000000001 //Enable search for SBAS PRN120
#define UBLOX_SBAS_PRN121 0x0000000000000002 //Enable search for SBAS PRN121
#define UBLOX_SBAS_PRN122 0x0000000000000004 //Enable search for SBAS PRN122
#define UBLOX_SBAS_PRN123 0x0000000000000008 //Enable search for SBAS PRN123
#define UBLOX_SBAS_PRN124 0x0000000000000010 //Enable search for SBAS PRN124
#define UBLOX_SBAS_PRN125 0x0000000000000020 //Enable search for SBAS PRN125
#define UBLOX_SBAS_PRN126 0x0000000000000040 //Enable search for SBAS PRN126
#define UBLOX_SBAS_PRN127 0x0000000000000080 //Enable search for SBAS PRN127
#define UBLOX_SBAS_PRN128 0x0000000000000100 //Enable search for SBAS PRN128
#define UBLOX_SBAS_PRN129 0x0000000000000200 //Enable search for SBAS PRN129
#define UBLOX_SBAS_PRN130 0x0000000000000400 //Enable search for SBAS PRN130
#define UBLOX_SBAS_PRN131 0x0000000000000800 //Enable search for SBAS PRN131
#define UBLOX_SBAS_PRN132 0x0000000000001000 //Enable search for SBAS PRN132
#define UBLOX_SBAS_PRN133 0x0000000000002000 //Enable search for SBAS PRN133
#define UBLOX_SBAS_PRN134 0x0000000000004000 //Enable search for SBAS PRN134
#define UBLOX_SBAS_PRN135 0x0000000000008000 //Enable search for SBAS PRN135
#define UBLOX_SBAS_PRN136 0x0000000000010000 //Enable search for SBAS PRN136
#define UBLOX_SBAS_PRN137 0x0000000000020000 //Enable search for SBAS PRN137
#define UBLOX_SBAS_PRN138 0x0000000000040000 //Enable search for SBAS PRN138
#define UBLOX_SBAS_PRN139 0x0000000000080000 //Enable search for SBAS PRN139
#define UBLOX_SBAS_PRN140 0x0000000000100000 //Enable search for SBAS PRN140
#define UBLOX_SBAS_PRN141 0x0000000000200000 //Enable search for SBAS PRN141
#define UBLOX_SBAS_PRN142 0x0000000000400000 //Enable search for SBAS PRN142
#define UBLOX_SBAS_PRN143 0x0000000000800000 //Enable search for SBAS PRN143
#define UBLOX_SBAS_PRN144 0x0000000001000000 //Enable search for SBAS PRN144
#define UBLOX_SBAS_PRN145 0x0000000002000000 //Enable search for SBAS PRN145
#define UBLOX_SBAS_PRN146 0x0000000004000000 //Enable search for SBAS PRN146
#define UBLOX_SBAS_PRN147 0x0000000008000000 //Enable search for SBAS PRN147
#define UBLOX_SBAS_PRN148 0x0000000010000000 //Enable search for SBAS PRN148
#define UBLOX_SBAS_PRN149 0x0000000020000000 //Enable search for SBAS PRN149
#define UBLOX_SBAS_PRN150 0x0000000040000000 //Enable search for SBAS PRN150
#define UBLOX_SBAS_PRN151 0x0000000080000000 //Enable search for SBAS PRN151
#define UBLOX_SBAS_PRN152 0x0000000100000000 //Enable search for SBAS PRN152
#define UBLOX_SBAS_PRN153 0x0000000200000000 //Enable search for SBAS PRN153
#define UBLOX_SBAS_PRN154 0x0000000400000000 //Enable search for SBAS PRN154
#define UBLOX_SBAS_PRN155 0x0000000800000000 //Enable search for SBAS PRN155
#define UBLOX_SBAS_PRN156 0x0000001000000000 //Enable search for SBAS PRN156
#define UBLOX_SBAS_PRN157 0x0000002000000000 //Enable search for SBAS PRN157
#define UBLOX_SBAS_PRN158 0x0000004000000000 //Enable search for SBAS PRN158
// payload types
typedef struct {
uint8_t mode;
uint8_t usage;
uint8_t maxSBAS;
uint8_t scanmode2;
uint32_t scanmode1;
} ubx_sbas;
typedef struct {
uint8_t msg_class;
uint8_t id;
uint8_t rate;
} ubx_msg;
typedef struct {
uint16_t meas;
uint16_t nav;
uint16_t time;
} ubx_rate;
typedef struct {
uint8_t gnssId;
uint8_t resTrkCh;
uint8_t maxTrkCh;
uint8_t reserved1;
// flags
uint8_t enabled;
uint8_t undefined0;
uint8_t sigCfgMask;
uint8_t undefined1;
} ubx_gnss_element_t;
typedef struct {
uint8_t msgVer;
uint8_t numTrkChHw;
uint8_t numTrkChUse;
uint8_t numConfigBlocks;
ubx_gnss_element_t config[0];
} ubx_gnss_msg_t;
typedef struct {
uint8_t version;
uint8_t layers;
uint8_t reserved;
} __attribute__((packed)) ubx_config_data_header_v0_t;
typedef struct {
uint8_t version;
uint8_t layers;
uint8_t transaction;
uint8_t reserved;
} __attribute__((packed)) ubx_config_data_header_v1_t;
#define MAX_GNSS 7
#define MAX_GNSS_SIZE_BYTES (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t)*MAX_GNSS)
typedef union {
uint8_t bytes[MAX_GNSS_SIZE_BYTES]; // placeholder
ubx_sbas sbas;
ubx_msg msg;
ubx_rate rate;
ubx_gnss_msg_t gnss;
} ubx_payload;
// UBX support
typedef struct {
uint8_t preamble1;
uint8_t preamble2;
uint8_t msg_class;
uint8_t msg_id;
uint16_t length;
} ubx_header;
typedef struct {
uint32_t key;
uint8_t value;
} __attribute__((packed)) ubx_config_data8_payload_t;
#define MAX_CONFIG_SET_VAL_VALUES 32
typedef struct {
ubx_header header;
ubx_config_data_header_v1_t configHeader;
union {
ubx_config_data8_payload_t payload[0];
uint8_t buffer[(MAX_CONFIG_SET_VAL_VALUES * sizeof(ubx_config_data8_payload_t)) + 2]; // 12 key/value pairs + 2 checksum bytes
} data;
} __attribute__((packed)) ubx_config_data8_t;
typedef struct {
ubx_header header;
ubx_payload payload;
} __attribute__((packed)) ubx_message;
typedef struct {
char swVersion[30]; // Zero-terminated Software Version String
char hwVersion[10]; // Zero-terminated Hardware Version String
} ubx_mon_ver;
typedef struct {
uint32_t time; // GPS msToW
int32_t longitude;
int32_t latitude;
int32_t altitude_ellipsoid;
int32_t altitude_msl;
uint32_t horizontal_accuracy;
uint32_t vertical_accuracy;
} ubx_nav_posllh;
typedef struct {
uint32_t time; // GPS msToW
uint8_t fix_type;
uint8_t fix_status;
uint8_t differential_status;
uint8_t res;
uint32_t time_to_first_fix;
uint32_t uptime; // milliseconds
} ubx_nav_status;
typedef struct {
uint32_t time;
int32_t time_nsec;
int16_t week;
uint8_t fix_type;
uint8_t fix_status;
int32_t ecef_x;
int32_t ecef_y;
int32_t ecef_z;
uint32_t position_accuracy_3d;
int32_t ecef_x_velocity;
int32_t ecef_y_velocity;
int32_t ecef_z_velocity;
uint32_t speed_accuracy;
uint16_t position_DOP;
uint8_t res;
uint8_t satellites;
uint32_t res2;
} ubx_nav_solution;
typedef struct {
uint32_t time; // GPS msToW
int32_t ned_north;
int32_t ned_east;
int32_t ned_down;
uint32_t speed_3d;
uint32_t speed_2d;
int32_t heading_2d;
uint32_t speed_accuracy;
uint32_t heading_accuracy;
} ubx_nav_velned;
typedef struct {
uint8_t chn; // Channel number, 255 for SVx not assigned to channel
uint8_t svid; // Satellite ID
uint8_t flags; // Bitmask
uint8_t quality; // Bitfield
uint8_t cno; // Carrier to Noise Ratio (Signal Strength) // dbHz, 0-55.
uint8_t elev; // Elevation in integer degrees
int16_t azim; // Azimuth in integer degrees
int32_t prRes; // Pseudo range residual in centimetres
} ubx_nav_svinfo_channel;
typedef struct {
uint32_t time; // GPS Millisecond time of week
uint8_t numCh; // Number of channels
uint8_t globalFlags; // Bitmask, Chip hardware generation 0:Antaris, 1:u-blox 5, 2:u-blox 6
uint16_t reserved2; // Reserved
ubx_nav_svinfo_channel channel[16]; // 16 satellites * 12 byte
} ubx_nav_svinfo;
typedef struct {
uint32_t time; // GPS msToW
uint32_t tAcc;
int32_t nano;
uint16_t year;
uint8_t month;
uint8_t day;
uint8_t hour;
uint8_t min;
uint8_t sec;
uint8_t valid;
} ubx_nav_timeutc;
typedef struct {
uint32_t time; // GPS msToW
uint16_t year;
uint8_t month;
uint8_t day;
uint8_t hour;
uint8_t min;
uint8_t sec;
uint8_t valid;
uint32_t tAcc;
int32_t nano;
uint8_t fix_type;
uint8_t fix_status;
uint8_t reserved1;
uint8_t satellites;
int32_t longitude;
int32_t latitude;
int32_t altitude_ellipsoid;
int32_t altitude_msl;
uint32_t horizontal_accuracy;
uint32_t vertical_accuracy;
int32_t ned_north;
int32_t ned_east;
int32_t ned_down;
int32_t speed_2d;
int32_t heading_2d;
uint32_t speed_accuracy;
uint32_t heading_accuracy;
uint16_t position_DOP;
uint16_t reserved2;
uint16_t reserved3;
} ubx_nav_pvt;
#define UBX_MON_GNSS_GPS_MASK (1 << 0)
#define UBX_MON_GNSS_GLONASS_MASK (1 << 1)
#define UBX_MON_GNSS_BEIDOU_MASK (1 << 2)
#define UBX_MON_GNSS_GALILEO_MASK (1 << 3)
typedef struct {
uint8_t version;
uint8_t supported; // bitfield for GNSS types: 0:GPS, 1:Glonass, 2:Beidou, 3:Galileo
uint8_t defaultGnss; // bitfield for GNSS types: 0:GPS, 1:Glonass, 2:Beidou, 3:Galileo
uint8_t enabled; // bitfield for GNSS types: 0:GPS, 1:Glonass, 2:Beidou, 3:Galileo
uint8_t maxConcurrent;
uint8_t reserverd1;
uint8_t reserverd2;
uint8_t reserverd3;
} ubx_mon_gnss;
typedef struct {
uint8_t msg_class;
uint8_t msg;
} ubx_ack_ack;
typedef enum {
UBX_ACK_WAITING = 0,
UBX_ACK_GOT_ACK = 1,
UBX_ACK_GOT_NAK = 2
} ubx_ack_state_t;
typedef enum {
PREAMBLE1 = 0xB5,
PREAMBLE2 = 0x62,
CLASS_NAV = 0x01,
CLASS_ACK = 0x05,
CLASS_CFG = 0x06,
CLASS_MON = 0x0A,
MSG_CLASS_UBX = 0x01,
MSG_CLASS_NMEA = 0xF0,
MSG_VER = 0x04,
MSG_ACK_NACK = 0x00,
MSG_ACK_ACK = 0x01,
MSG_NMEA_GGA = 0x0,
MSG_NMEA_GLL = 0x1,
MSG_NMEA_GSA = 0x2,
MSG_NMEA_GSV = 0x3,
MSG_NMEA_RMC = 0x4,
MSG_NMEA_VGS = 0x5,
MSG_POSLLH = 0x2,
MSG_STATUS = 0x3,
MSG_SOL = 0x6,
MSG_PVT = 0x7,
MSG_VELNED = 0x12,
MSG_TIMEUTC = 0x21,
MSG_SVINFO = 0x30,
MSG_NAV_SAT = 0x35,
MSG_NAV_SIG = 0x35,
MSG_CFG_PRT = 0x00,
MSG_CFG_RATE = 0x08,
MSG_CFG_SET_RATE = 0x01,
MSG_CFG_NAV_SETTINGS = 0x24,
MSG_CFG_SBAS = 0x16,
MSG_CFG_GNSS = 0x3e,
MSG_MON_GNSS = 0x28
} ubx_protocol_bytes_t;
typedef enum {
FIX_NONE = 0,
FIX_DEAD_RECKONING = 1,
FIX_2D = 2,
FIX_3D = 3,
FIX_GPS_DEAD_RECKONING = 4,
FIX_TIME = 5
} ubs_nav_fix_type_t;
typedef enum {
NAV_STATUS_FIX_VALID = 1
} ubx_nav_status_bits_t;
uint8_t gpsUbloxMaxGnss(void);
timeMs_t gpsUbloxCapLastUpdate(void);
bool gpsUbloxHasGalileo(void);
bool gpsUbloxHasBeidou(void);
bool gpsUbloxHasGlonass(void);
bool gpsUbloxGalileoDefault(void);
bool gpsUbloxBeidouDefault(void);
bool gpsUbloxGlonassDefault(void);
bool gpsUbloxGalileoEnabled(void);
bool gpsUbloxBeidouEnabled(void);
bool gpsUbloxGlonassEnabled(void);
#ifdef __cplusplus
}
#endif

62
src/main/io/gps_ublox_utils.c Normal file
View file

@ -0,0 +1,62 @@
/*
* This file is part of INAV
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdint.h>
#include "gps_ublox_utils.h"
void ublox_update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b)
{
*ck_a = *ck_b = 0;
while (len--) {
*ck_a += *data;
*ck_b += *ck_a;
data++;
}
}
int ubloxCfgFillBytes(ubx_config_data8_t *cfg, ubx_config_data8_payload_t *kvPairs, uint8_t count)
{
if (count > MAX_CONFIG_SET_VAL_VALUES)
count = MAX_CONFIG_SET_VAL_VALUES;
cfg->header.preamble1 = 0xb5;
cfg->header.preamble2 = 0x62;
cfg->header.msg_class = 0x06;
cfg->header.msg_id = 0x8A;
cfg->header.length = sizeof(ubx_config_data_header_v1_t) + ((sizeof(ubx_config_data8_payload_t) * count));
cfg->configHeader.layers = 0x1;
cfg->configHeader.transaction = 0;
cfg->configHeader.reserved = 0;
cfg->configHeader.version = 1;
for (int i = 0; i < count; ++i) {
cfg->data.payload[i].key = kvPairs[i].key; //htonl(kvPairs[i].key);
cfg->data.payload[i].value = kvPairs[i].value;
}
uint8_t *buf = (uint8_t *)cfg;
uint8_t ck_a, ck_b;
ublox_update_checksum(buf + 2, cfg->header.length + 4, &ck_a, &ck_b);
buf[cfg->header.length + 6] = ck_a;
buf[cfg->header.length + 7] = ck_b;
return count;
}

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

@ -0,0 +1,34 @@
/*
* This file is part of INAV
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <stdint.h>
#include "gps_ublox.h"
#ifdef __cplusplus
extern "C" {
#endif
int ubloxCfgFillBytes(ubx_config_data8_t *cfg, ubx_config_data8_payload_t *kvPairs, uint8_t count);
void ublox_update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b);
#ifdef __cplusplus
}
#endif

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

@ -207,11 +207,11 @@ static bool osdDisplayHasCanvas;
PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 8);
PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 1);
void osdStartedSaveProcess() {
void osdStartedSaveProcess(void) {
savingSettings = true;
}
void osdShowEEPROMSavedNotification() {
void osdShowEEPROMSavedNotification(void) {
savingSettings = false;
notify_settings_saved = millis() + 5000;
}
@ -329,7 +329,7 @@ static void osdFormatDistanceSymbol(char *buff, int32_t dist, uint8_t decimals)
buff[sym_index + 1] = '\0';
break;
case OSD_UNIT_GA:
if (osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(dist), FEET_PER_NAUTICALMILE, decimals, 3, digits)) {
if (osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(dist), (uint32_t)FEET_PER_NAUTICALMILE, decimals, 3, digits)) {
buff[sym_index] = symbol_nm;
} else {
buff[sym_index] = symbol_ft;
@ -532,6 +532,7 @@ static void osdFormatWindSpeedStr(char *buff, int32_t ws, bool isValid)
*/
void osdFormatAltitudeSymbol(char *buff, int32_t alt)
{
uint8_t totalDigits = 4U;
uint8_t digits = 4U;
uint8_t symbolIndex = 4U;
uint8_t symbolKFt = SYM_ALT_KFT;
@ -543,6 +544,7 @@ void osdFormatAltitudeSymbol(char *buff, int32_t alt)
#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values
if (isBfCompatibleVideoSystem(osdConfig())) {
totalDigits++;
digits++;
symbolIndex++;
symbolKFt = SYM_ALT_FT;
@ -555,7 +557,7 @@ void osdFormatAltitudeSymbol(char *buff, int32_t alt)
case OSD_UNIT_GA:
FALLTHROUGH;
case OSD_UNIT_IMPERIAL:
if (osdFormatCentiNumber(buff + 4 - digits, CENTIMETERS_TO_CENTIFEET(alt), 1000, 0, 2, digits)) {
if (osdFormatCentiNumber(buff + totalDigits - digits, CENTIMETERS_TO_CENTIFEET(alt), 1000, 0, 2, digits)) {
// Scaled to kft
buff[symbolIndex++] = symbolKFt;
} else {
@ -568,7 +570,7 @@ void osdFormatAltitudeSymbol(char *buff, int32_t alt)
FALLTHROUGH;
case OSD_UNIT_METRIC:
// alt is alredy in cm
if (osdFormatCentiNumber(buff + 4 - digits, alt, 1000, 0, 2, digits)) {
if (osdFormatCentiNumber(buff + totalDigits - digits, alt, 1000, 0, 2, digits)) {
// Scaled to km
buff[symbolIndex++] = SYM_ALT_KM;
} else {
@ -1092,7 +1094,7 @@ void osdCrosshairPosition(uint8_t *x, uint8_t *y)
* Check if this OSD layout is using scaled or unscaled throttle.
* If both are used, it will default to scaled.
*/
bool osdUsingScaledThrottle()
bool osdUsingScaledThrottle(void)
{
bool usingScaledThrottle = OSD_VISIBLE(osdLayoutsConfig()->item_pos[currentLayout][OSD_SCALED_THROTTLE_POS]);
bool usingRCThrottle = OSD_VISIBLE(osdLayoutsConfig()->item_pos[currentLayout][OSD_THROTTLE_POS]);
@ -1209,7 +1211,7 @@ uint16_t osdGetRemainingGlideTime(void) {
value = 0;
}
return (uint16_t)round(value);
return (uint16_t)roundf(value);
}
static bool osdIsHeadingValid(void)
@ -1434,7 +1436,7 @@ static void osdDisplayTelemetry(void)
trk_bearing %= 360;
int32_t alt = CENTIMETERS_TO_METERS(osdGetAltitude());
float at = atan2(alt, GPS_distanceToHome);
trk_elevation = (float)at * 57.2957795; // 57.2957795 = 1 rad
trk_elevation = at * 57.2957795f; // 57.2957795 = 1 rad
trk_elevation += 37; // because elevation in telemetry should be from -37 to 90
if (trk_elevation < 0) {
trk_elevation = 0;
@ -1751,7 +1753,7 @@ static bool osdDrawSingleElement(uint8_t item)
else if (!batteryWasFullWhenPluggedIn())
tfp_sprintf(buff, " NF");
else if (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH)
tfp_sprintf(buff, "%4lu", getBatteryRemainingCapacity());
tfp_sprintf(buff, "%4lu", (unsigned long)getBatteryRemainingCapacity());
else // currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH
osdFormatCentiNumber(buff + 1, getBatteryRemainingCapacity() / 10, 0, 2, 0, 3);
@ -3097,9 +3099,9 @@ static bool osdDrawSingleElement(uint8_t item)
buff,
"[%u]=%8ld [%u]=%8ld",
bufferIndex,
constrain(debug[bufferIndex], -9999999, 99999999),
(long)constrain(debug[bufferIndex], -9999999, 99999999),
bufferIndex+1,
constrain(debug[bufferIndex+1], -9999999, 99999999)
(long)constrain(debug[bufferIndex+1], -9999999, 99999999)
);
displayWrite(osdDisplayPort, elemPosX, elemPosY, buff);
}
@ -4239,12 +4241,19 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
bool efficiencyValid = totalDistance >= 10000;
if (feature(FEATURE_GPS)) {
displayWrite(osdDisplayPort, statNameX, top, "AVG EFFICIENCY :");
uint8_t digits = 3U; // Total number of digits (including decimal point)
#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values
if (isBfCompatibleVideoSystem(osdConfig())) {
// Add one digit so no switch to scaled decimal occurs above 99
digits = 4U;
}
#endif
switch (osdConfig()->units) {
case OSD_UNIT_UK:
FALLTHROUGH;
case OSD_UNIT_IMPERIAL:
if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) {
moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_MILE / totalDistance), 1000, 0, 2, 3);
moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_MILE / totalDistance), 1000, 0, 2, digits);
if (!moreThanAh) {
tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_MI_0, SYM_MAH_MI_1);
} else {
@ -4257,7 +4266,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
buff[5] = '\0';
}
} else {
osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_MILE / totalDistance), 0, 2, 0, 3);
osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_MILE / totalDistance), 0, 2, 0, digits);
tfp_sprintf(buff, "%s%c", buff, SYM_WH_MI);
if (!efficiencyValid) {
buff[0] = buff[1] = buff[2] = '-';
@ -4266,7 +4275,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
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);
moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_NAUTICALMILE / totalDistance), 1000, 0, 2, digits);
if (!moreThanAh) {
tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_NM_0, SYM_MAH_NM_1);
} else {
@ -4279,7 +4288,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
buff[5] = '\0';
}
} else {
osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_NAUTICALMILE / totalDistance), 0, 2, 0, 3);
osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_NAUTICALMILE / totalDistance), 0, 2, 0, digits);
tfp_sprintf(buff, "%s%c", buff, SYM_WH_NM);
if (!efficiencyValid) {
buff[0] = buff[1] = buff[2] = '-';
@ -4290,7 +4299,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
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);
moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000000.0f / totalDistance), 1000, 0, 2, digits);
if (!moreThanAh) {
tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_KM_0, SYM_MAH_KM_1);
} else {
@ -4303,7 +4312,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
buff[5] = '\0';
}
} else {
osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10000.0f / totalDistance), 0, 2, 0, 3);
osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10000.0f / totalDistance), 0, 2, 0, digits);
tfp_sprintf(buff, "%s%c", buff, SYM_WH_KM);
if (!efficiencyValid) {
buff[0] = buff[1] = buff[2] = '-';

2
src/main/io/osd_canvas.c
View file

@ -279,7 +279,7 @@ static void osdDrawArtificialHorizonShapes(displayCanvas_t *canvas, float pitchA
itoa(absLevel, buf, 10);
int pos = level * pixelsPerDegreeLevel;
int charY = 9 - pos * 2;
int cx = (absLevel >= 100 ? -1.5f : -1.0) * canvas->gridElementWidth;
int cx = (absLevel >= 100 ? -1.5f : -1.0f) * canvas->gridElementWidth;
int px = cx + (pitchOffset + pos) * sx * 2;
int py = -charY - (pitchOffset + pos) * (1 - sy) * 2;
displayCanvasDrawString(canvas, px, py, buf, 0);

2
src/main/io/osd_dji_hd.c
View file

@ -721,7 +721,7 @@ static void osdDJIFormatThrottlePosition(char *buff, bool autoThr )
thr = rcCommand[THROTTLE];
}
tfp_sprintf(buff, "%3ld%s", (constrain(thr, PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN), "%THR");
tfp_sprintf(buff, "%3ld%s", (unsigned long)((constrain(thr, PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN)), "%THR");
}
/**

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

@ -265,9 +265,9 @@ void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitu
case OSD_UNIT_GA:
{
if (poiType == 1) {
osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), FEET_PER_NAUTICALMILE, 0, 4, 4);
osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), (uint32_t)FEET_PER_NAUTICALMILE, 0, 4, 4);
} else {
osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), FEET_PER_NAUTICALMILE, 0, 3, 3);
osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), (uint32_t)FEET_PER_NAUTICALMILE, 0, 3, 3);
}
}
break;

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

@ -59,6 +59,8 @@ typedef enum {
FUNCTION_MSP_OSD = (1 << 25), // 33554432
} serialPortFunction_e;
#define FUNCTION_VTX_MSP FUNCTION_MSP_OSD
typedef enum {
BAUD_AUTO = 0,
BAUD_1200,

3
src/main/io/vtx_control.c
View file

@ -18,8 +18,10 @@
// Get target build configuration
#include "platform.h"
#include "build/debug.h"
#include "common/maths.h"
#include "common/log.h"
#include "config/config_eeprom.h"
#include "config/parameter_group.h"
@ -37,7 +39,6 @@
#include "io/osd.h"
#include "io/vtx_control.h"
#if defined(USE_VTX_CONTROL)
PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 4);

701
src/main/io/vtx_msp.c Normal file
View file

@ -0,0 +1,701 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* Cleanflight and Betaflight are distributed in the hope that they
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
/* Created by phobos- */
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <math.h>
#include <ctype.h>
#include <string.h>
#include "platform.h"
#if defined(USE_VTX_MSP) && defined(USE_VTX_CONTROL) && defined(USE_VTX_COMMON)
#include "build/debug.h"
//#include "cms/cms_menu_vtx_msp.h"
#include "common/crc.h"
#include "common/log.h"
#include "config/feature.h"
#include "drivers/vtx_common.h"
//#include "drivers/vtx_table.h"
#include "fc/runtime_config.h"
#include "flight/failsafe.h"
#include "io/serial.h"
#include "io/vtx_msp.h"
#include "io/vtx_control.h"
#include "io/vtx_string.h"
#include "io/vtx_smartaudio.h"
#include "io/vtx.h"
#include "io/displayport_msp_osd.h"
#include "msp/msp_protocol.h"
#include "msp/msp_serial.h"
#include "msp/msp.h"
//#include "pg/vtx_table.h"
#include "fc/settings.h"
#include "rx/crsf.h"
//#include "rx/crsf_protocol.h"
#include "rx/rx.h"
#include "telemetry/msp_shared.h"
//static uint16_t mspConfFreq = 0;
static uint8_t mspConfBand = SETTING_VTX_BAND_DEFAULT;
static uint8_t mspConfChannel = SETTING_VTX_CHANNEL_DEFAULT;
//static uint16_t mspConfPower = 0;
static uint16_t mspConfPowerIndex = SETTING_VTX_POWER_DEFAULT;
static uint8_t mspConfPitMode = 0;
static bool mspVtxConfigChanged = false;
static timeUs_t mspVtxLastTimeUs = 0;
static bool prevLowPowerDisarmedState = false;
static const vtxVTable_t mspVTable; // forward
static vtxDevice_t vtxMsp = {
.vTable = &mspVTable,
.capability.bandCount = VTX_MSP_TABLE_MAX_BANDS,
.capability.channelCount = VTX_MSP_TABLE_MAX_CHANNELS,
.capability.powerCount = VTX_MSP_TABLE_MAX_POWER_LEVELS,
.capability.bandNames = (char **)vtx58BandNames,
.capability.channelNames = (char **)vtx58ChannelNames,
.capability.powerNames = (char**)saPowerNames
};
STATIC_UNIT_TESTED mspVtxStatus_e mspVtxStatus = MSP_VTX_STATUS_OFFLINE;
static uint8_t mspVtxPortIdentifier = 255;
#define MSP_VTX_REQUEST_PERIOD_US (200 * 1000) // 200ms
static bool isCrsfPortConfig(const serialPortConfig_t *portConfig)
{
return portConfig->functionMask & FUNCTION_RX_SERIAL && portConfig->functionMask & FUNCTION_VTX_MSP && rxConfig()->serialrx_provider == SERIALRX_CRSF;
}
static bool isLowPowerDisarmed(void)
{
return (!ARMING_FLAG(ARMED) && !failsafeIsActive() &&
(vtxSettingsConfig()->lowPowerDisarm == VTX_LOW_POWER_DISARM_ALWAYS ||
(vtxSettingsConfig()->lowPowerDisarm == VTX_LOW_POWER_DISARM_UNTIL_FIRST_ARM && !ARMING_FLAG(WAS_EVER_ARMED))));
}
bool isVtxConfigValid(const vtxConfig_t *cfg)
{
LOG_DEBUG(VTX, "msp isVtxConfigValid\r\n");
for (int i = 0; i < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; ++i) {
if (cfg->vtxChannelActivationConditions[i].band ||
(cfg->vtxChannelActivationConditions[i].range.startStep && cfg->vtxChannelActivationConditions[i].range.endStep) ||
cfg->vtxChannelActivationConditions[i].auxChannelIndex ||
cfg->vtxChannelActivationConditions[i].channel) {
return true;
}
}
LOG_DEBUG(VTX, "msp Invalid Config!\r\n");
return false;
}
void setMspVtxDeviceStatusReady(const int descriptor)
{
LOG_DEBUG(VTX, "msp setMspVtxDeviceStatusReady\r\n");
UNUSED(descriptor);
mspVtxStatus = MSP_VTX_STATUS_READY;
mspVtxConfigChanged = true;
}
void prepareMspFrame(uint8_t *mspFrame)
{
LOG_DEBUG(VTX, "msp PrepareMspFrame\r\n");
/*
HDZERO parsing
fc_band_rx = msp_rx_buf[1];
fc_channel_rx = msp_rx_buf[2];
fc_pwr_rx = msp_rx_buf[3];
fc_pit_rx = msp_rx_buf[4];
fc_lp_rx = msp_rx_buf[8];
*/
uint8_t pitmode = 0;
vtxCommonGetPitMode(&vtxMsp, &pitmode);
mspFrame[0] = VTXDEV_MSP,
mspFrame[1] = vtxSettingsConfig()->band;
mspFrame[2] = vtxSettingsConfig()->channel;
mspFrame[3] = isLowPowerDisarmed() ? 1 : vtxSettingsConfig()->power; // index based
mspFrame[4] = pitmode;
mspFrame[5] = 0; // Freq_L
mspFrame[6] = 0; // Freq_H
mspFrame[7] = (mspVtxStatus == MSP_VTX_STATUS_READY) ? 1 : 0;
mspFrame[8] = isLowPowerDisarmed();
mspFrame[9] = 0; // Pitmode freq Low
mspFrame[10] = 0; // pitmod freq High
mspFrame[11] = 0; // 1 if using vtx table
mspFrame[12] = 0; // vtx table bands or 0
mspFrame[13] = 0; // vtx table channels or 0
mspFrame[14] = 0; // vtx table power levels or 0
}
static void mspCrsfPush(const uint8_t mspCommand, const uint8_t *mspFrame, const uint8_t mspFrameSize)
{
LOG_DEBUG(VTX, "msp CrsfPush\r\n");
#ifndef USE_TELEMETRY_CRSF
UNUSED(mspCommand);
UNUSED(mspFrame);
UNUSED(mspFrameSize);
#else
sbuf_t crsfPayloadBuf;
sbuf_t *dst = &crsfPayloadBuf;
uint8_t mspHeader[6] = {0x50, 0, mspCommand & 0xFF, (mspCommand >> 8) & 0xFF, mspFrameSize & 0xFF, (mspFrameSize >> 8) & 0xFF }; // MSP V2 over CRSF header
uint8_t mspChecksum;
mspChecksum = crc8_dvb_s2_update(0, &mspHeader[1], 5); // first character is not checksummable
mspChecksum = crc8_dvb_s2_update(mspChecksum, mspFrame, mspFrameSize);
uint8_t fullMspFrameSize = mspFrameSize + sizeof(mspHeader) + 1; // add 1 for msp checksum
uint8_t crsfFrameSize = CRSF_FRAME_LENGTH_EXT_TYPE_CRC + CRSF_FRAME_LENGTH_TYPE_CRC + fullMspFrameSize;
uint8_t crsfFrame[crsfFrameSize];
dst->ptr = crsfFrame;
dst->end = ARRAYEND(crsfFrame);
sbufWriteU8(dst, CRSF_SYNC_BYTE);
sbufWriteU8(dst, fullMspFrameSize + CRSF_FRAME_LENGTH_EXT_TYPE_CRC); // size of CRSF frame (everything except sync and size itself)
sbufWriteU8(dst, CRSF_FRAMETYPE_MSP_RESP); // CRSF type
sbufWriteU8(dst, CRSF_ADDRESS_CRSF_RECEIVER); // response destination is the receiver the vtx connection
sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER); // origin is always this device
sbufWriteData(dst, mspHeader, sizeof(mspHeader));
sbufWriteData(dst, mspFrame, mspFrameSize);
sbufWriteU8(dst, mspChecksum);
crc8_dvb_s2_sbuf_append(dst, &crsfFrame[2]); // start at byte 2, since CRC does not include device address and frame length
sbufSwitchToReader(dst, crsfFrame);
crsfRxSendTelemetryData(); //give the FC a chance to send outstanding telemetry
crsfRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst));
crsfRxSendTelemetryData();
#endif
}
static uint16_t packetCounter = 0;
static bool isVtxConfigChanged(void)
{
if(mspVtxStatus == MSP_VTX_STATUS_READY) {
if (mspVtxConfigChanged == true)
return true;
if (isLowPowerDisarmed() != prevLowPowerDisarmedState) {
LOG_DEBUG(VTX, "msp vtx config changed (lower power disarm 2)\r\n");
mspVtxConfigChanged = true;
prevLowPowerDisarmedState = isLowPowerDisarmed();
}
if (mspConfPowerIndex != vtxSettingsConfig()->power) {
LOG_DEBUG(VTX, "msp vtx config changed (power 2)\r\n");
mspVtxConfigChanged = true;
}
if (mspConfBand != vtxSettingsConfig()->band || mspConfChannel != vtxSettingsConfig()->channel) {
LOG_DEBUG(VTX, "msp vtx config changed (band and channel 2)\r\n");
mspVtxConfigChanged = true;
}
return mspVtxConfigChanged;
}
return false;
}
static void vtxMspProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
{
UNUSED(vtxDevice);
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_MSP_OSD);
uint8_t frame[15];
switch (mspVtxStatus) {
case MSP_VTX_STATUS_OFFLINE:
LOG_DEBUG(VTX, "msp MspProcess: OFFLINE\r\n");
// wait for MSP communication from the VTX
#ifdef USE_CMS
//mspCmsUpdateStatusString();
#endif
break;
case MSP_VTX_STATUS_READY:
LOG_DEBUG(VTX, "msp MspProcess: READY\r\n");
// send an update if stuff has changed with 200ms period
if ((isVtxConfigChanged()) && cmp32(currentTimeUs, mspVtxLastTimeUs) >= MSP_VTX_REQUEST_PERIOD_US) {
LOG_DEBUG(VTX, "msp-vtx: vtxInfo Changed\r\n");
prepareMspFrame(frame);
if (isCrsfPortConfig(portConfig)) {
mspCrsfPush(MSP_VTX_CONFIG, frame, sizeof(frame));
} else {
mspPort_t *port = getMspOsdPort();
if(port != NULL && port->port) {
LOG_DEBUG(VTX, "msp-vtx: mspSerialPushPort\r\n");
int sent = mspSerialPushPort(MSP_VTX_CONFIG, frame, sizeof(frame), port, MSP_V2_NATIVE);
if (sent <= 0) {
break;
}
}
}
packetCounter++;
mspVtxLastTimeUs = currentTimeUs;
mspVtxConfigChanged = false;
#ifdef USE_CMS
//mspCmsUpdateStatusString();
#endif
}
break;
default:
mspVtxStatus = MSP_VTX_STATUS_OFFLINE;
break;
}
#if 0
DEBUG_SET(DEBUG_VTX_MSP, 0, packetCounter);
DEBUG_SET(DEBUG_VTX_MSP, 1, isCrsfPortConfig(portConfig));
DEBUG_SET(DEBUG_VTX_MSP, 2, isLowPowerDisarmed());
#if defined(USE_MSP_OVER_TELEMETRY)
DEBUG_SET(DEBUG_VTX_MSP, 3, isCrsfPortConfig(portConfig) ? getMspTelemetryDescriptor() : getMspSerialPortDescriptor(mspVtxPortIdentifier));
#else
DEBUG_SET(DEBUG_VTX_MSP, 3, getMspSerialPortDescriptor(mspVtxPortIdentifier));
#endif
#endif
}
static vtxDevType_e vtxMspGetDeviceType(const vtxDevice_t *vtxDevice)
{
//LOG_DEBUG(VTX, "msp GetDeviceType\r\n");
UNUSED(vtxDevice);
return VTXDEV_MSP;
}
static bool vtxMspIsReady(const vtxDevice_t *vtxDevice)
{
//LOG_DEBUG(VTX, "msp vtxIsReady: %s\r\n", (vtxDevice != NULL && mspVtxStatus == MSP_VTX_STATUS_READY) ? "Y": "N");
return vtxDevice != NULL && mspVtxStatus == MSP_VTX_STATUS_READY;
}
static void vtxMspSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel)
{
LOG_DEBUG(VTX, "msp SetBandAndChannel\r\n");
UNUSED(vtxDevice);
if (band != mspConfBand || channel != mspConfChannel) {
LOG_DEBUG(VTX, "msp vtx config changed (band and channel)\r\n");
mspVtxConfigChanged = true;
}
mspConfBand = band;
mspConfChannel = channel;
}
static void vtxMspSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index)
{
LOG_DEBUG(VTX, "msp SetPowerByIndex\r\n");
UNUSED(vtxDevice);
if (index > 0 && (index < VTX_MSP_TABLE_MAX_POWER_LEVELS))
{
if (index != mspConfPowerIndex)
{
LOG_DEBUG(VTX, "msp vtx config changed (power by index)\r\n");
mspVtxConfigChanged = true;
}
mspConfPowerIndex = index;
}
}
static void vtxMspSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff)
{
LOG_DEBUG(VTX, "msp SetPitMode\r\n");
UNUSED(vtxDevice);
if (onoff != mspConfPitMode) {
LOG_DEBUG(VTX, "msp vtx config changed (pitmode)\r\n");
mspVtxConfigChanged = true;
}
mspConfPitMode = onoff;
}
#if 0
static void vtxMspSetFreq(vtxDevice_t *vtxDevice, uint16_t freq)
{
UNUSED(vtxDevice);
if (freq != mspConfFreq) {
mspVtxConfigChanged = true;
}
mspConfFreq = freq;
}
#endif
static bool vtxMspGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel)
{
if (!vtxMspIsReady(vtxDevice)) {
return false;
}
*pBand = vtxSettingsConfig()->band;
*pChannel = vtxSettingsConfig()->channel;
//LOG_DEBUG(VTX, "msp GetBandAndChannel: %02x:%02x\r\n", vtxSettingsConfig()->band, vtxSettingsConfig()->channel);
return true;
}
static bool vtxMspGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex)
{
if (!vtxMspIsReady(vtxDevice)) {
return false;
}
uint8_t power = isLowPowerDisarmed() ? 1 : vtxSettingsConfig()->power;
// Special case, power not set
if (power > VTX_MSP_TABLE_MAX_POWER_LEVELS) {
*pIndex = 0;
//LOG_DEBUG(VTX, "msp GetPowerIndex: %u\r\n", *pIndex);
return true;
}
*pIndex = power;
//LOG_DEBUG(VTX, "msp GetPowerIndex: %u\r\n", *pIndex);
return true;
}
static bool vtxMspGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq)
{
LOG_DEBUG(VTX, "msp GetFreq\r\n");
if (!vtxMspIsReady(vtxDevice)) {
return false;
}
*pFreq = 5800;
return true;
}
static bool vtxMspGetPower(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16_t *pPowerMw)
{
LOG_DEBUG(VTX, "msp GetPower\r\n");
uint8_t powerIndex;
if (!vtxMspGetPowerIndex(vtxDevice, &powerIndex)) {
return false;
}
*pIndex = powerIndex;
*pPowerMw = *pIndex;
return true;
}
static bool vtxMspGetOsdInfo(const vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t * pOsdInfo)
{
LOG_DEBUG(VTX, "msp GetOsdInfo\r\n");
uint8_t powerIndex;
uint16_t powerMw;
uint16_t freq;
uint8_t band, channel;
if (!vtxMspGetBandAndChannel(vtxDevice, &band, &channel)) {
return false;
}
if (!vtxMspGetFreq(vtxDevice, &freq)) {
return false;
}
if (!vtxMspGetPower(vtxDevice, &powerIndex, &powerMw)) {
return false;
}
pOsdInfo->band = band;
pOsdInfo->channel = channel;
pOsdInfo->frequency = freq;
pOsdInfo->powerIndex = powerIndex;
pOsdInfo->powerMilliwatt = powerMw;
pOsdInfo->bandLetter = vtx58BandNames[band][0];
pOsdInfo->bandName = vtx58BandNames[band];
pOsdInfo->channelName = vtx58ChannelNames[channel];
pOsdInfo->powerIndexLetter = '0' + powerIndex;
return true;
}
bool vtxMspInit(void)
{
LOG_DEBUG(VTX, "msp %s\r\n", __FUNCTION__);
// don't bother setting up this device if we don't have MSP vtx enabled
// Port is shared with MSP_OSD
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_MSP_OSD);
if (!portConfig) {
return false;
}
mspVtxPortIdentifier = portConfig->identifier;
// XXX Effect of USE_VTX_COMMON should be reviewed, as following call to vtxInit will do nothing if vtxCommonSetDevice is not called.
#if defined(USE_VTX_COMMON)
vtxCommonSetDevice(&vtxMsp);
#endif
mspConfBand = vtxSettingsConfig()->band;
mspConfChannel = vtxSettingsConfig()->channel;
mspConfPowerIndex = isLowPowerDisarmed() ? 1 : vtxSettingsConfig()->power; // index based
vtxCommonGetPitMode(&vtxMsp, &mspConfPitMode);
vtxInit();
return true;
}
static const vtxVTable_t mspVTable = {
.process = vtxMspProcess,
.getDeviceType = vtxMspGetDeviceType,
.isReady = vtxMspIsReady,
.setBandAndChannel = vtxMspSetBandAndChannel,
.setPowerByIndex = vtxMspSetPowerByIndex,
.setPitMode = vtxMspSetPitMode,
//.setFrequency = vtxMspSetFreq,
.getBandAndChannel = vtxMspGetBandAndChannel,
.getPowerIndex = vtxMspGetPowerIndex,
.getFrequency = vtxMspGetFreq,
//.getStatus = vtxMspGetStatus,
.getPower = vtxMspGetPower,
//.serializeCustomDeviceStatus = NULL,
.getOsdInfo = vtxMspGetOsdInfo,
};
static mspResult_e mspVtxProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn)
{
//LOG_DEBUG(VTX, "msp VTX_MSP_PROCESS\r\n");
UNUSED(mspPostProcessFn);
sbuf_t *dst = &reply->buf;
sbuf_t *src = &cmd->buf;
const unsigned int dataSize = sbufBytesRemaining(src);
UNUSED(dst);
UNUSED(src);
// Start initializing the reply message
reply->cmd = cmd->cmd;
reply->result = MSP_RESULT_ACK;
vtxDevice_t *vtxDevice = vtxCommonDevice();
if (!vtxDevice || vtxCommonGetDeviceType(vtxDevice) != VTXDEV_MSP) {
LOG_DEBUG(VTX, "msp wrong vtx\r\n");
return MSP_RESULT_ERROR;
}
switch (cmd->cmd)
{
case MSP_VTXTABLE_BAND:
{
LOG_DEBUG(VTX, "msp MSP_VTXTABLE_BAND\r\n");
uint8_t deviceType = vtxCommonGetDeviceType(vtxDevice);
if (deviceType == VTXDEV_MSP)
{
/*
char bandName[MSP_VTX_TABLE_BAND_NAME_LENGTH + 1];
memset(bandName, 0, MSP_VTX_TABLE_BAND_NAME_LENGTH + 1);
uint16_t frequencies[MSP_VTX_TABLE_MAX_CHANNELS];
const uint8_t band = sbufReadU8(src);
const uint8_t bandNameLength = sbufReadU8(src);
for (int i = 0; i < bandNameLength; i++) {
const char nameChar = sbufReadU8(src);
if (i < MSP_VTX_TABLE_BAND_NAME_LENGTH) {
bandName[i] = toupper(nameChar);
}
}
const char bandLetter = toupper(sbufReadU8(src));
const bool isFactoryBand = (bool)sbufReadU8(src);
const uint8_t channelCount = sbufReadU8(src);
for (int i = 0; i < channelCount; i++)
{
const uint16_t frequency = sbufReadU16(src);
if (i < vtxTableConfig()->channels)
{
frequencies[i] = frequency;
}
}
*/
setMspVtxDeviceStatusReady(1);
}
break;
}
case MSP_VTXTABLE_POWERLEVEL:
{
LOG_DEBUG(VTX, "msp MSP_VTXTABLE_POWERLEVEL\r\n");
/*
char powerLevelLabel[VTX_TABLE_POWER_LABEL_LENGTH + 1];
memset(powerLevelLabel, 0, VTX_TABLE_POWER_LABEL_LENGTH + 1);
const uint8_t powerLevel = sbufReadU8(src);
const uint16_t powerValue = sbufReadU16(src);
const uint8_t powerLevelLabelLength = sbufReadU8(src);
for (int i = 0; i < powerLevelLabelLength; i++)
{
const char labelChar = sbufReadU8(src);
if (i < VTX_TABLE_POWER_LABEL_LENGTH)
{
powerLevelLabel[i] = toupper(labelChar);
}
}
*/
setMspVtxDeviceStatusReady(1);
}
break;
case MSP_VTX_CONFIG:
{
LOG_DEBUG(VTX, "msp MSP_VTX_CONFIG received\r\n");
LOG_DEBUG(VTX, "msp MSP_VTX_CONFIG VTXDEV_MSP\r\n");
uint8_t pitmode = 0;
vtxCommonGetPitMode(vtxDevice, &pitmode);
// VTXDEV_MSP,
sbufWriteU8(dst, VTXDEV_MSP);
// band;
sbufWriteU8(dst, vtxSettingsConfig()->band);
// channel;
sbufWriteU8(dst, vtxSettingsConfig()->channel);
// power; // index based
sbufWriteU8(dst, vtxSettingsConfig()->power);
// pit mode;
// Freq_L
sbufWriteU8(dst, 0);
// Freq_H
sbufWriteU8(dst, 0);
// vtx status
sbufWriteU8(dst, 1);
// lowPowerDisarm
sbufWriteU8(dst, vtxSettingsConfig()->lowPowerDisarm);
// Pitmode freq Low
sbufWriteU8(dst, 0);
// pitmod freq High
sbufWriteU8(dst, 0);
// 1 if using vtx table
sbufWriteU8(dst, 0);
// vtx table bands or 0
sbufWriteU8(dst, 0);
// vtx table channels or 0
sbufWriteU8(dst, 0);
setMspVtxDeviceStatusReady(1);
break;
}
case MSP_SET_VTX_CONFIG:
LOG_DEBUG(VTX, "msp MSP_SET_VTX_CONFIG\r\n");
if (dataSize == 15)
{
if (vtxCommonGetDeviceType(vtxDevice) != VTXDEV_UNKNOWN)
{
for (int i = 0; i < 15; ++i)
{
uint8_t data = sbufReadU8(src);
switch (i)
{
case 1:
vtxSettingsConfigMutable()->band = data;
break;
case 2:
vtxSettingsConfigMutable()->channel = data;
break;
case 3:
vtxSettingsConfigMutable()->power = data;
break;
case 4:
vtxCommonSetPitMode(vtxDevice, data);
break;
case 7:
// vtx ready
break;
case 8:
vtxSettingsConfigMutable()->lowPowerDisarm = data;
break;
}
}
}
setMspVtxDeviceStatusReady(1);
break;
}
LOG_DEBUG(VTX, "msp MSP_RESULT_ERROR\r\n");
return MSP_RESULT_ERROR;
default:
// debug[1]++;
// debug[2] = cmd->cmd;
if(cmd->cmd != MSP_STATUS && cmd->cmd != MSP_STATUS_EX && cmd->cmd != MSP_RC) {
LOG_DEBUG(VTX, "msp default: %02x\r\n", cmd->cmd);
}
reply->result = MSP_RESULT_ERROR;
break;
}
// Process DONT_REPLY flag
if (cmd->flags & MSP_FLAG_DONT_REPLY)
{
reply->result = MSP_RESULT_NO_REPLY;
}
return reply->result;
}
void mspVtxSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn)
{
UNUSED(mspProcessCommandFn);
// Check if VTX is ready
/*
if (mspVtxStatus != MSP_VTX_STATUS_READY) {
LOG_DEBUG(VTX, "msp VTX NOT READY, skipping\r\n");
return;
}
*/
mspPort_t *port = getMspOsdPort();
if(port) {
mspSerialProcessOnePort(port, MSP_SKIP_NON_MSP_DATA, mspVtxProcessMspCommand);
}
}
#endif

53
src/main/io/vtx_msp.h Normal file
View file

@ -0,0 +1,53 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* Cleanflight and Betaflight are distributed in the hope that they
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <stdint.h>
#include "build/build_config.h"
#include "msp/msp_protocol.h"
#include "msp/msp_serial.h"
typedef enum {
// Offline - device hasn't responded yet
MSP_VTX_STATUS_OFFLINE = 0,
MSP_VTX_STATUS_READY,
} mspVtxStatus_e;
typedef struct mspPowerTable_s {
int mW; // rfpower
int16_t dbi; // valueV1
} mspPowerTable_t;
#define VTX_MSP_TABLE_MAX_BANDS 5 // default freq table has 5 bands
#define VTX_MSP_TABLE_MAX_CHANNELS 8 // and eight channels
#define VTX_MSP_TABLE_MAX_POWER_LEVELS 5 //max of VTX_TRAMP_POWER_COUNT, VTX_SMARTAUDIO_POWER_COUNT and VTX_RTC6705_POWER_COUNT
#define VTX_MSP_TABLE_CHANNEL_NAME_LENGTH 1
#define VTX_MSP_TABLE_BAND_NAME_LENGTH 8
#define VTX_MSP_TABLE_POWER_LABEL_LENGTH 3
bool vtxMspInit(void);
void setMspVtxDeviceStatusReady(const int descriptor);
void prepareMspFrame(uint8_t *mspFrame);
void mspVtxSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn);

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

@ -217,6 +217,9 @@
#define MSP_SPECIAL_PARAMETERS 98 // Temporary betaflight parameters before cleanup and keep CF compatibility
#define MSP_SET_SPECIAL_PARAMETERS 99 // Temporary betaflight parameters before cleanup and keep CF compatibility
#define MSP_VTXTABLE_BAND 137 //out message vtxTable band/channel data - needed by msp vtx
#define MSP_VTXTABLE_POWERLEVEL 138 //out message vtxTable powerLevel data - neede by msp vtx
//
// OSD specific
//

9
src/main/msp/msp_serial.c
View file

@ -530,7 +530,7 @@ int mspSerialPushPort(uint16_t cmd, const uint8_t *data, int datalen, mspPort_t
return mspSerialEncode(mspPort, &push, version);
}
int mspSerialPush(uint8_t cmd, const uint8_t *data, int datalen)
int mspSerialPushVersion(uint8_t cmd, const uint8_t *data, int datalen, mspVersion_e version)
{
int ret = 0;
@ -545,11 +545,16 @@ int mspSerialPush(uint8_t cmd, const uint8_t *data, int datalen)
continue;
}
ret = mspSerialPushPort(cmd, data, datalen, mspPort, MSP_V1);
ret = mspSerialPushPort(cmd, data, datalen, mspPort, version);
}
return ret; // return the number of bytes written
}
int mspSerialPush(uint8_t cmd, const uint8_t *data, int datalen)
{
return mspSerialPushVersion(cmd, data, datalen, MSP_V1);
}
uint32_t mspSerialTxBytesFree(serialPort_t *port)
{
return serialTxBytesFree(port);

1
src/main/msp/msp_serial.h
View file

@ -107,5 +107,6 @@ void mspSerialAllocatePorts(void);
void mspSerialReleasePortIfAllocated(struct serialPort_s *serialPort);
int mspSerialPushPort(uint16_t cmd, const uint8_t *data, int datalen, mspPort_t *mspPort, mspVersion_e version);
int mspSerialPush(uint8_t cmd, const uint8_t *data, int datalen);
int mspSerialPushVersion(uint8_t cmd, const uint8_t *data, int datalen, mspVersion_e version);
uint32_t mspSerialTxBytesFree(serialPort_t *port);
mspPort_t * mspSerialPortFind(const struct serialPort_s *serialPort);

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

@ -101,7 +101,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, 2);
#endif
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 3);
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 4);
PG_RESET_TEMPLATE(navConfig_t, navConfig,
.general = {
@ -125,6 +125,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.soaring_motor_stop = SETTING_NAV_FW_SOARING_MOTOR_STOP_DEFAULT, // stops motor when Saoring mode enabled
.rth_trackback_mode = SETTING_NAV_RTH_TRACKBACK_MODE_DEFAULT, // RTH trackback useage mode
.rth_use_linear_descent = SETTING_NAV_RTH_USE_LINEAR_DESCENT_DEFAULT, // Use linear descent during RTH
.landing_bump_detection = SETTING_NAV_LANDING_BUMP_DETECTION_DEFAULT, // Detect landing based on touchdown G bump
},
// General navigation parameters
@ -1247,7 +1248,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
}
const uint8_t rthClimbMarginPercent = STATE(FIXED_WING_LEGACY) ? FW_RTH_CLIMB_MARGIN_PERCENT : MR_RTH_CLIMB_MARGIN_PERCENT;
const float rthAltitudeMargin = MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (rthClimbMarginPercent/100.0) * fabsf(posControl.rthState.rthInitialAltitude - posControl.rthState.homePosition.pos.z));
const float rthAltitudeMargin = MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (rthClimbMarginPercent/100.0f) * fabsf(posControl.rthState.rthInitialAltitude - posControl.rthState.homePosition.pos.z));
// If we reached desired initial RTH altitude or we don't want to climb first
if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.rthState.rthInitialAltitude) > -rthAltitudeMargin) || (navConfig()->general.flags.rth_climb_first == RTH_CLIMB_OFF) || rthAltControlStickOverrideCheck(ROLL) || rthClimbStageActiveAndComplete()) {
@ -1722,6 +1723,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navi
timeMs_t currentTime = millis();
if (posControl.waypointList[posControl.activeWaypointIndex].p1 <= 0 ||
posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT ||
(posControl.wpReachedTime != 0 && currentTime - posControl.wpReachedTime >= (timeMs_t)posControl.waypointList[posControl.activeWaypointIndex].p1*1000L)) {
return NAV_FSM_EVENT_SUCCESS;
}
@ -1877,7 +1879,7 @@ static navigationFSMState_t navSetNewFSMState(navigationFSMState_t newState)
static void navProcessFSMEvents(navigationFSMEvent_t injectedEvent)
{
const timeMs_t currentMillis = millis();
navigationFSMState_t previousState;
navigationFSMState_t previousState = NAV_STATE_UNDEFINED;
static timeMs_t lastStateProcessTime = 0;
/* Process new injected event if event defined,
@ -2635,7 +2637,7 @@ static bool rthAltControlStickOverrideCheck(unsigned axis)
* transiton in to turn.
* Limited to fixed wing only.
* --------------------------------------------------- */
bool rthClimbStageActiveAndComplete() {
bool rthClimbStageActiveAndComplete(void) {
if ((STATE(FIXED_WING_LEGACY) || STATE(AIRPLANE)) && (navConfig()->general.rth_climb_first_stage_altitude > 0)) {
if (posControl.actualState.abs.pos.z >= posControl.rthState.rthClimbStageAltitude) {
return true;
@ -3229,7 +3231,7 @@ void loadSelectedMultiMission(uint8_t missionIndex)
posControl.geoWaypointCount = 0;
for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) {
if ((missionCount == missionIndex)) {
if (missionCount == missionIndex) {
/* store details of selected mission: start wp index, mission wp count, geo wp count */
if (!(posControl.waypointList[i].action == NAV_WP_ACTION_SET_POI ||
posControl.waypointList[i].action == NAV_WP_ACTION_SET_HEAD ||
@ -4397,7 +4399,7 @@ int32_t navigationGetHomeHeading(void)
}
// returns m/s
float calculateAverageSpeed() {
float calculateAverageSpeed(void) {
float flightTime = getFlightTime();
if (flightTime == 0.0f) return 0;
return (float)getTotalTravelDistance() / (flightTime * 100);

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