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

Merge branch 'master' into MrD_update-arming-screen-for-better-space-utilisation

Browse source
This commit is contained in:
Darren Lines 2024-08-10 16:21:34 +03:00
commit 5f8c942fba
232 changed files with 114991 additions and 2486 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -2,17 +2,26 @@ name: Build firmware
# Don't enable CI on push, just on PR. If you
# are working on the main repo and want to trigger
# a CI build submit a draft PR.
on: pull_request
on:
pull_request:
paths:
- 'src/**'
- '.github/**'
- 'cmake/**'
- 'lib/**'
- 'docs/Settings.md'
- 'CMakeLists.txt'
- '*.sh'
jobs:
build:
runs-on: ubuntu-latest
strategy:
matrix:
id: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9]
id: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16]
steps:
- uses: actions/checkout@v3
- uses: actions/checkout@v4
- name: Install dependencies
run: sudo apt-get update && sudo apt-get -y install ninja-build
- name: Setup environment
@ -31,22 +40,64 @@ jobs:
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
- uses: actions/cache@v1
- uses: actions/cache@v4
with:
path: downloads
key: ${{ runner.os }}-downloads-${{ hashFiles('CMakeLists.txt') }}-${{ hashFiles('**/cmake/*')}}
- 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
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 }} -DMAIN_COMPILE_OPTIONS=-pipe -G Ninja .. && ninja -j4 ci
- name: Upload artifacts
uses: actions/upload-artifact@v3
uses: actions/upload-artifact@v4
with:
name: matrix-${{ env.BUILD_NAME }}.${{ matrix.id }}
path: ./build/*.hex
retention-days: 1
upload-artifacts:
runs-on: ubuntu-latest
needs: [build]
steps:
- uses: actions/checkout@v4
- 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: Download artifacts
uses: actions/download-artifact@v4
with:
pattern: matrix-inav-*
merge-multiple: true
path: binaries
- name: Build target list
run: |
ls -1 binaries/*.hex | cut -d/ -f2 > targets.txt
- name: Upload firmware images
uses: actions/upload-artifact@v4
with:
name: ${{ env.BUILD_NAME }}
path: ./build/*.hex
path: binaries/*.hex
- name: Upload firmware images
uses: actions/upload-artifact@v4
with:
name: targets
path: targets.txt
build-SITL-Linux:
runs-on: ubuntu-latest
steps:
- uses: actions/checkout@v3
- uses: actions/checkout@v4
- name: Install dependencies
run: sudo apt-get update && sudo apt-get -y install ninja-build
- name: Setup environment
@ -66,17 +117,17 @@ jobs:
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
run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja -j4
- name: Upload artifacts
uses: actions/upload-artifact@v3
uses: actions/upload-artifact@v4
with:
name: ${{ env.BUILD_NAME }}_SITL
name: ${{ env.BUILD_NAME }}_SITL-Linux
path: ./build_SITL/*_SITL
build-SITL-Mac:
runs-on: macos-latest
steps:
- uses: actions/checkout@v3
- uses: actions/checkout@v4
- name: Install dependencies
run: |
brew install cmake ninja ruby
@ -101,10 +152,10 @@ jobs:
run: |
mkdir -p build_SITL && cd build_SITL
cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -DCMAKE_OSX_ARCHITECTURES="arm64;x86_64" -G Ninja ..
ninja
ninja -j3
- name: Upload artifacts
uses: actions/upload-artifact@v3
uses: actions/upload-artifact@v4
with:
name: ${{ env.BUILD_NAME }}_SITL-MacOS
path: ./build_SITL/*_SITL
@ -115,7 +166,7 @@ jobs:
run:
shell: C:\tools\cygwin\bin\bash.exe -o igncr '{0}'
steps:
- uses: actions/checkout@v3
- uses: actions/checkout@v4
- name: Setup Cygwin
uses: egor-tensin/setup-cygwin@v4
with:
@ -137,19 +188,19 @@ jobs:
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
run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja -j4
- name: Upload artifacts
uses: actions/upload-artifact@v3
uses: actions/upload-artifact@v4
with:
name: ${{ env.BUILD_NAME }}_SITL-WIN
path: ./build_SITL/*.exe
test:
needs: [build]
#needs: [build]
runs-on: ubuntu-latest
steps:
- uses: actions/checkout@v3
- uses: actions/checkout@v4
- name: Install dependencies
run: sudo apt-get update && sudo apt-get -y install ninja-build
- name: Run Tests

243
.github/workflows/dev-builds.yml vendored Normal file
View file

@ -0,0 +1,243 @@
name: Build pre-release
# Don't enable CI on push, just on PR. If you
# are working on the main repo and want to trigger
# a CI build submit a draft PR.
on:
push:
branches:
- master
paths:
- 'src/**'
- '.github/**'
- 'cmake/**'
- 'lib/**'
- 'docs/Settings.md'
- 'CMakeLists.txt'
- '*.sh'
jobs:
build:
runs-on: ubuntu-latest
strategy:
matrix:
id: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16]
steps:
- uses: actions/checkout@v4
- name: Install dependencies
run: sudo apt-get update && sudo apt-get -y install ninja-build
- name: Setup environment
env:
ACTIONS_ALLOW_UNSECURE_COMMANDS: true
run: |
# This is the hash of the commit for the PR
# when the action is triggered by PR, empty otherwise
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=dev-$(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
- uses: actions/cache@v4
with:
path: downloads
key: ${{ runner.os }}-downloads-${{ hashFiles('CMakeLists.txt') }}-${{ hashFiles('**/cmake/*')}}
- 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 }} -DVERSION_TYPE=dev -G Ninja .. && ninja ci
- name: Upload artifacts
uses: actions/upload-artifact@v4
with:
name: ${{ env.BUILD_NAME }}.${{ matrix.id }}
path: ./build/*.hex
build-SITL-Linux:
runs-on: ubuntu-latest
steps:
- uses: actions/checkout@v4
- name: Install dependencies
run: sudo apt-get update && sudo apt-get -y install ninja-build
- name: Setup environment
env:
ACTIONS_ALLOW_UNSECURE_COMMANDS: true
run: |
# This is the hash of the commit for the PR
# when the action is triggered by PR, empty otherwise
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=dev-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/[ \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 -DVERSION_TYPE=dev .. && ninja
- name: Upload artifacts
uses: actions/upload-artifact@v4
with:
name: sitl-${{ env.BUILD_NAME }}-Linux
path: ./build_SITL/*_SITL
build-SITL-Mac:
runs-on: macos-latest
steps:
- uses: actions/checkout@v4
- 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=dev-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/[ \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 -DCMAKE_OSX_ARCHITECTURES="arm64;x86_64" -DVERSION_TYPE=dev -G Ninja ..
ninja
- name: Upload artifacts
uses: actions/upload-artifact@v4
with:
name: sitl-${{ env.BUILD_NAME }}-MacOS
path: ./build_SITL/*_SITL
build-SITL-Windows:
runs-on: windows-latest
defaults:
run:
shell: C:\tools\cygwin\bin\bash.exe -o igncr '{0}'
steps:
- uses: actions/checkout@v4
- name: Setup Cygwin
uses: egor-tensin/setup-cygwin@v4
with:
packages: cmake ruby ninja gcc-g++
- 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=dev-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/[ \t\)]+/, "", $2); print $2 }')
#echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
#echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
#echo "VERSION_TAG=-$(date '+%Y%m%d')" >> $GITHUB_ENV
echo "version=${VERSION}" >> $GITHUB_OUTPUT
- name: Build SITL
run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -DVERSION_TYPE=dev -G Ninja .. && ninja
- name: Upload artifacts
uses: actions/upload-artifact@v4
with:
name: sitl-${{ env.BUILD_NAME }}-WIN
path: ./build_SITL/*.exe
test:
runs-on: ubuntu-latest
steps:
- uses: actions/checkout@v4
- name: Install dependencies
run: sudo apt-get update && sudo apt-get -y install ninja-build
- name: Run Tests
run: mkdir -p build && cd build && cmake -DTOOLCHAIN=none -G Ninja .. && ninja check
release:
needs: [build, build-SITL-Linux, build-SITL-Mac, build-SITL-Windows, test]
runs-on: ubuntu-latest
steps:
- uses: actions/checkout@v4
- name: Get version
id: version
run: |
VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/[ \t\)]+/, "", $2); print $2 }')
echo "version=${VERSION}" >> $GITHUB_OUTPUT
- name: Get current date
id: date
run: echo "today=$(date '+%Y%m%d')" >> $GITHUB_OUTPUT
- name: download artifacts
uses: actions/download-artifact@v4
with:
path: hexes
pattern: inav-*
merge-multiple: true
- name: download sitl linux
uses: actions/download-artifact@v4
with:
path: resources/sitl/linux
pattern: sitl-*-Linux
merge-multiple: true
- name: download sitl windows
uses: actions/download-artifact@v4
with:
path: resources/sitl/windows
pattern: sitl-*-WIN
merge-multiple: true
- name: download sitl mac
uses: actions/download-artifact@v4
with:
path: resources/sitl/macos
pattern: sitl-*-MacOS
merge-multiple: true
- name: Consolidate sitl files
run: |
zip -r -9 sitl-resources.zip resources/
- name: Upload release artifacts
uses: softprops/action-gh-release@v2
with:
name: inav-${{ steps.version.outputs.version }}-dev-${{ steps.date.outputs.today }}-${{ github.run_number }}-${{ github.sha }}
tag_name: v${{ steps.version.outputs.version }}-${{ steps.date.outputs.today }}.${{ github.run_number }}
# To create release on a different repo, we need a token setup
token: ${{ secrets.NIGHTLY_TOKEN }}
repository: iNavFlight/inav-nightly
prerelease: true
draft: false
#generate_release_notes: true
make_latest: false
files: |
hexes/*.hex
sitl-resources.zip
body: |
${{ steps.notes.outputs.notes }}
### Flashing
These are nightly builds and configuration settings can be added and removed often. Flashing with Full chip erase is strongly recommended to avoid issues.
Firmware related issues should be opened in the iNavflight/inav repository, not in inav-nightly.
### Repository:
${{ github.repository }} ([link](${{ github.event.repository.html_url }}))
### Branch:
${{ github.ref_name }} ([link](${{ github.event.repository.html_url }}/tree/${{ github.ref_name }}))
### Latest changeset:
${{ github.event.head_commit.id }} ([link](${{ github.event.head_commit.url }}))
### Changes:
${{ github.event.head_commit.message }}

2
.github/workflows/docs.yml vendored
View file

@ -14,7 +14,7 @@ jobs:
runs-on: ubuntu-latest
steps:
- uses: actions/checkout@v3
- uses: actions/checkout@v4
- name: Install dependencies
run: sudo apt-get update && sudo apt-get -y install python3-yaml
- name: Check that Settings.md is up to date

25
.github/workflows/non-code-change.yaml vendored Normal file
View file

@ -0,0 +1,25 @@
name: Build firmware
# Don't enable CI on push, just on PR. If you
# are working on the main repo and want to trigger
# a CI build submit a draft PR.
on:
pull_request:
paths-ignore:
- 'src/**'
- '.github/**'
- 'cmake/**'
- 'lib/**'
- 'docs/Settings.md'
- 'CMakeLists.txt'
- '*.sh'
jobs:
test:
#needs: [build]
runs-on: ubuntu-latest
steps:
- uses: actions/checkout@v4
- name: Install dependencies
run: sudo apt-get update && sudo apt-get -y install ninja-build
- name: Run Tests
run: mkdir -p build && cd build && cmake -DTOOLCHAIN=none -G Ninja .. && ninja check

9
.gitignore vendored
View file

@ -1,10 +1,12 @@
*.o
.DS_Store
*~
*.swp
*.uvopt
*.dep
*.bak
*.uvgui.*
*.ubx
.project
.settings
.cproject
@ -16,6 +18,8 @@ startup_stm32f10x_md_gcc.s
cov-int*
/build/
/build_SITL/
/[hs]itl/
/ninja/
/obj/
/patches/
/tools/
@ -36,4 +40,9 @@ make/local.mk
launch.json
.vscode/tasks.json
.vscode/c_cpp_properties.json
/cmake-build-debug/
# Assitnow token and files for test script
tokens.yaml
*.ubx

1
.vimrc
View file

@ -5,5 +5,6 @@ set expandtab
set bs=2
set sw=4
set ts=4
syn on

34
.vscode/c_cpp_properties.json vendored
View file

@ -3,24 +3,19 @@
{
"name": "Linux",
"includePath": [
"${workspaceRoot}/src/main/**",
"${workspaceRoot}/lib/main/**",
"/usr/include/**"
"${workspaceRoot}",
"${workspaceRoot}/src/main/**"
],
"browse": {
"limitSymbolsToIncludedHeaders": false,
"path": [
"${workspaceRoot}/src/main/**",
"${workspaceRoot}/lib/main/**"
"${workspaceRoot}/**"
]
},
"intelliSenseMode": "linux-gcc-arm",
"intelliSenseMode": "msvc-x64",
"cStandard": "c11",
"cppStandard": "c++17",
"defines": [
"MCU_FLASH_SIZE 512",
"USE_NAV",
"NAV_FIXED_WING_LANDING",
"USE_OSD",
"USE_GYRO_NOTCH_1",
"USE_GYRO_NOTCH_2",
@ -34,30 +29,13 @@
"USE_RPM_FILTER",
"USE_GLOBAL_FUNCTIONS",
"USE_DYNAMIC_FILTERS",
"USE_IMU_BNO055",
"USE_SECONDARY_IMU",
"USE_DSHOT",
"FLASH_SIZE 480",
"USE_I2C_IO_EXPANDER",
"USE_PCF8574",
"USE_ESC_SENSOR",
"USE_PROGRAMMING_FRAMEWORK",
"USE_SERIALRX_GHST",
"USE_TELEMETRY_GHST",
"USE_CMS",
"USE_DJI_HD_OSD",
"USE_GYRO_KALMAN",
"USE_RANGEFINDER",
"USE_RATE_DYNAMICS",
"USE_SMITH_PREDICTOR",
"USE_ALPHA_BETA_GAMMA_FILTER",
"USE_MAG_VCM5883",
"USE_TELEMETRY_JETIEXBUS",
"USE_NAV",
"USE_SDCARD_SDIO",
"USE_SDCARD",
"USE_Q_TUNE",
"USE_GYRO_FFT_FILTER"
"USE_ADAPTIVE_FILTER",
"MCU_FLASH_SIZE 1024",
],
"configurationProvider": "ms-vscode.cmake-tools"
}

1
CMakeLists.txt
View file

@ -79,6 +79,7 @@ set(COMMON_COMPILE_DEFINITIONS
FC_VERSION_MAJOR=${CMAKE_PROJECT_VERSION_MAJOR}
FC_VERSION_MINOR=${CMAKE_PROJECT_VERSION_MINOR}
FC_VERSION_PATCH_LEVEL=${CMAKE_PROJECT_VERSION_PATCH}
FC_VERSION_TYPE="${VERSION_TYPE}"
)
if (NOT SITL)

9
cmake/at32.cmake
View file

@ -18,6 +18,7 @@ set(CMSIS_DSP_SRC
BasicMathFunctions/arm_scale_f32.c
BasicMathFunctions/arm_sub_f32.c
BasicMathFunctions/arm_mult_f32.c
BasicMathFunctions/arm_offset_f32.c
TransformFunctions/arm_rfft_fast_f32.c
TransformFunctions/arm_cfft_f32.c
TransformFunctions/arm_rfft_fast_init_f32.c
@ -26,6 +27,9 @@ set(CMSIS_DSP_SRC
CommonTables/arm_common_tables.c
ComplexMathFunctions/arm_cmplx_mag_f32.c
StatisticsFunctions/arm_max_f32.c
StatisticsFunctions/arm_rms_f32.c
StatisticsFunctions/arm_std_f32.c
StatisticsFunctions/arm_mean_f32.c
)
list(TRANSFORM CMSIS_DSP_SRC PREPEND "${CMSIS_DSP_DIR}/Source/")
@ -326,6 +330,11 @@ function(target_at32)
math(EXPR hse_value "${hse_mhz} * 1000000")
list(APPEND target_definitions "HSE_VALUE=${hse_value}")
if (MSP_UART)
list(APPEND target_definitions "MSP_UART=${MSP_UART}")
endif()
if(args_COMPILE_DEFINITIONS)
list(APPEND target_definitions ${args_COMPILE_DEFINITIONS})
endif()

4
cmake/sitl.cmake
View file

@ -99,6 +99,10 @@ function (target_sitl name)
math(EXPR hse_value "${hse_mhz} * 1000000")
list(APPEND target_definitions "HSE_VALUE=${hse_value}")
if (MSP_UART)
list(APPEND target_definitions "MSP_UART=${MSP_UART}")
endif()
string(TOLOWER ${PROJECT_NAME} lowercase_project_name)
set(binary_name ${lowercase_project_name}_${FIRMWARE_VERSION}_${name})
if(DEFINED BUILD_SUFFIX AND NOT "" STREQUAL "${BUILD_SUFFIX}")

9
cmake/stm32.cmake
View file

@ -19,6 +19,7 @@ set(CMSIS_DSP_SRC
BasicMathFunctions/arm_scale_f32.c
BasicMathFunctions/arm_sub_f32.c
BasicMathFunctions/arm_mult_f32.c
BasicMathFunctions/arm_offset_f32.c
TransformFunctions/arm_rfft_fast_f32.c
TransformFunctions/arm_cfft_f32.c
TransformFunctions/arm_rfft_fast_init_f32.c
@ -27,6 +28,9 @@ set(CMSIS_DSP_SRC
CommonTables/arm_common_tables.c
ComplexMathFunctions/arm_cmplx_mag_f32.c
StatisticsFunctions/arm_max_f32.c
StatisticsFunctions/arm_rms_f32.c
StatisticsFunctions/arm_std_f32.c
StatisticsFunctions/arm_mean_f32.c
)
list(TRANSFORM CMSIS_DSP_SRC PREPEND "${CMSIS_DSP_DIR}/Source/")
@ -333,6 +337,11 @@ function(target_stm32)
math(EXPR hse_value "${hse_mhz} * 1000000")
list(APPEND target_definitions "HSE_VALUE=${hse_value}")
if (MSP_UART)
list(APPEND target_definitions "MSP_UART=${MSP_UART}")
endif()
if(args_COMPILE_DEFINITIONS)
list(APPEND target_definitions ${args_COMPILE_DEFINITIONS})
endif()

1
cmake/stm32h7.cmake
View file

@ -216,6 +216,7 @@ macro(define_target_stm32h7 subfamily size)
COMPILE_DEFINITIONS ${definitions}
LINKER_SCRIPT stm32_flash_h7${subfamily}x${size}
${${func_ARGV}}
SVD STM32H7${subfamily}
)
endfunction()
endmacro()

105015
dev/svd/STM32H743.svd Normal file
View file

File diff suppressed because it is too large Load diff

3
docs/Battery.md
View file

@ -201,7 +201,6 @@ Up to 3 battery profiles are supported. You can select the battery profile from
- `vbat_max_cell_voltage`
- `vbat_warning_cell_voltage`
- `vbat_min_cell_voltage`
- `battery_capacity_unit`
- `battery_capacity`
- `battery_capacity_warning`
- `battery_capacity_critical`
@ -253,7 +252,6 @@ feature BAT_PROF_AUTOSWITCH
battery_profile 1
set bat_cells = 3
set battery_capacity_unit = MAH
set battery_capacity = 2200
set battery_capacity_warning = 440
set battery_capacity_critical = 220
@ -262,7 +260,6 @@ set battery_capacity_critical = 220
battery_profile 2
set bat_cells = 4
set battery_capacity_unit = MAH
set battery_capacity = 1500
set battery_capacity_warning = 300
set battery_capacity_critical = 150

50
docs/Betaflight 4.3 compatible OSD.md
View file

@ -1,50 +0,0 @@
# Betaflight 4.3 compatible MSP DisplayPort OSD (DJI O3 "Canvas Mode")
INAV 6.0 includes a special mode for MSP DisplayPort that supports incomplete implementations of MSP DisplayPort that only support BetaFlight, like the DJI O3 Air Unit. INAV 6.1 expands this to include HD canvas sizes from BetaFlight 4.4.
Different flight controllers have different OSD symbols and elements and require different fonts. BetaFlight's font is a single page and supports a maximum of 256 glyphs, INAV's font is currently 2 pages and supports up to 512 different glyphs.
While there is some overlap between the glyphs in BetaFlight and INAV, it is not possible to perform a 1 to 1 mapping for all the them. In cases where there is no suitable glyph in the BetaFlight font, a question mark `?` will be displayed.
This mode can be enabled by selecting BF43COMPAT or BFHDCOMPAT as video format in the OSD tab of the configurator or by typing the following command on the CLI:
`set osd_video_system = BF43COMPAT`
or
`set osd_video_system = BFHDCOMPAT`
## Limitations
* Canvas size needs to be manually changed to HD on the Display menu in DJI's goggles (you may need a firmware update) and set as BFHDCOMPAT in the OSD tab of the configurator.
* Unsupported Glyphs show up as `?`
## FAQ
### I see a lot of `?` on my OSD.
That is expected, when your INAV OSD widgets use glyphs that don't have a suitable mapping in BetaFlight's font.
### Does it work with the G2 and Original Air Unit/Vista?
Yes.
### Is this a replacement for WTFOS?
Not exactly. WTFOS is a full implementation of MSP-Displayport for rooted Air Unit/Vista/Googles V2 and actually works much better than BetaFlight compatibility mode, being able to display all INAV's glyphs.
### Can INAV fix DJI's product?
No. OSD renderinng happens on the googles/air unit side of things. Please ask DJI to fix their incomplete MSP DisplayPort implemenation. You can probably request it in [DJI's forum](https://forum.dji.com/forum.php?mod=forumdisplay&fid=129&filter=typeid&typeid=767).
### BetaFlight X.Y now has more symbols, can you update INAV?
Maybe. If a future version of BetaFlight includes more Glyphs that can be mapped into INAV it is fairly simple to add the mapping, but the problem with DJI's implemenation persists. Even if we update the mapping, if DJI does not update the fonts on their side the problem will persist.
### Can you replace glyph `X` with text `x description`?
While it might technically be possible to replace some glyphs with text in multiple cells, it will introduce a lot of complexity in the OSD rendering and configuration for something we hope is a temporary workaround.
### Does DJI support Canvas Mode?
Actually, no. What DJI calls Canvas Mode is actually MSP DisplayPort and is a character based OSD.

79
docs/Broken USB recovery.md Normal file
View file

@ -0,0 +1,79 @@
# Broken USB recovery
It is possible to flash INAV without USB over UART 1 or 3.
## Prerequisites:
- USB/UART adapter (FT232, CP2102, etc.)
- STM32 Cube Programmer (https://www.st.com/en/development-tools/stm32cubeprog.html)
To gain access to the FC via Configurator, MSP must be activated on a UART as standard. Some FCs already have this enabled by default, if not a custom firmware must be created.
The following targets have MSP activated on a UART by default:
| Target | Standard MSP Port |
|-----------| ----------- |
| AOCODARCF4V3 | UART5 |
| ATOMRCF405NAVI_DELUXE | UART1 |
| FF_F35_LIGHTNING | UART1 |
| FLYCOLORF7V2 | UART4 |
| GEPRCF405_BT_HD | UART5* |
| GEPRCF722_BT_HD | UART4* |
| IFLIGHT_BLITZ_F7_AIO | UART1 |
| JHEMCUF405WING | UART6 |
| JHEMCUH743HD | UART4 |
| KAKUTEH7 | UART1 and UART2* |
| KAKUTEH7WING | UART6 |
| MAMBAF405_2022A | UART4 |
| MAMBAF405US | UART4 |
| MAMBAF722 | UART4 |
| MAMBAF722 APP | UART4*|
| MAMBAF722WING | UART4 |
| MAMBAF722_X8 | UART4 |
| MAMBAH743 | UART4* |
| MATEKF405SE | UART1 |
| NEUTRONRCH743BT | UART3* |
| SDMODELH7V1 | UART1 and UART2 |
| SKYSTARSH743HD | UART4 |
| SPEEDYBEEF4 | UART5* |
| SPEEDYBEEF405MINI | UART4* |
| SPEEDYBEEF405V3 | UART4* |
| SPEEDYBEEF405V4 | UART4* |
| SPEEDYBEEF405WING | UART6 |
| SPEEDYBEEF7 | UART6 |
| SPRACINGF4EVO | UART1 |
| TMOTORF7V2 | UART5 |
(*) No Pads/Pins, Port is used interally (Bluetooth)
## Custom firmware:
If the FC does not have MSP activated on a UART by default or does not have a connector for it, a custom firmware must be built.
The following procedure describes the process under Windows 10/11:
Please read [Building in Windows 2010 or 11 with Linux Subsystem](https://github.com/iNavFlight/inav/blob/master/docs/development/Building%20in%20Windows%2010%20or%2011%20with%20Linux%20Subsystem.md)
and follow the instructions up to "Building with Make".
In the step 'prepare build environment' add the option `-DMSP_UART=SERIAL_PORT_USARTX` to `cmake`
Replace the X in SERIAL_PORT_USARTX with the number of UART/serial port on which MSP is to be activated.
Example:
For UART 2: `cmake -DMSP_UART=SERIAL_PORT_USART2 ..`
For UART 3: `cmake -DMSP_UART=SERIAL_PORT_USART3 ..`
etc.
Build the firmware as described in the document above (`make [YOUR_TARGET]`).
## Flashing via Uart:
1. Disconnect ALL peripherals and the USB Cable from the FC. To power the FC use a battery or use the 5V provided from the USB/Serial Converter.
2. Connect UART 1 or 3 (other UARTS will not work) and GND to the USB/Serial converter (RX -> TX, TX -> RX)
3. Keep the boot/dfu button pressed
4. Switch on the FC / supply with power
5. Start STM32 CubeProgrammer and go to "Erasing & Programming", second option in the menu.
6. Select UART (blue dropdown field) and select the COM port of the USB/Serial adapter and press "Connect". The corresponding processor should now be displayed below.
7. Click on "Full flash erase". This is also necessary if you are flashing the same firmware version that was previously on the FC, otherwise MSP may not be activated on the UART.
8. Under "Download" load the previously created firmware (`INAV_X.X.X_[Your Target].hex`) or the standard firmware if UART is already activated there. The option "Verify programming" is optional but recommended. Make sure that "Skip flash erase while programming" is NOT activated.
9. Click "Start Programming"
After the process is completed, switch the FC off and on again and then the Configurator can connect to the FC via USB/serial adapter and the previously configured UART.

6
docs/Cli.md
View file

@ -71,7 +71,7 @@ While connected to the CLI, all Logical Switches are temporarily disabled (5.1.0
| `batch` | Start or end a batch of commands |
| `battery_profile` | Change battery profile |
| `beeper` | Show/set beeper (buzzer) [usage](Buzzer.md) |
| `bind_rx` | Initiate binding for RX SPI or SRXL2 |
| `bind_rx` | Initiate binding for SRXL2 or CRSF receivers |
| `blackbox` | Configure blackbox fields |
| `bootlog` | Show boot events |
| `color` | Configure colors |
@ -107,7 +107,7 @@ While connected to the CLI, all Logical Switches are temporarily disabled (5.1.0
| `save` | Save and reboot |
| `sd_info` | Sdcard info |
| `serial` | Configure serial ports. [Usage](Serial.md) |
| `serialpassthrough` | Passthrough serial data to port, with `<id> <baud> <mode>`, where `id` is the zero based port index, `baud` is a standard baud rate, and mode is `rx`, `tx`, or both (`rxtx`) |
| `serialpassthrough` | Passthrough serial data to port, with `<id> <baud> <mode> <options>`, where `id` is the zero based port index, `baud` is a standard baud rate, mode is `rx`, `tx`, or both (`rxtx`), and options is a short string like `8N1` or `8E2` |
| `servo` | Configure servos |
| `set` | Change setting with name=value or blank or * for list |
| `smix` | Custom servo mixer |
@ -157,6 +157,8 @@ A shorter form is also supported to enable and disable a single function using `
| TELEMETRY_SMARTPORT_MASTER | 23 | 8388608 |
| UNUSED | 24 | 16777216 |
| MSP_DISPLAYPORT | 25 | 33554432 |
| GIMBAL_SERIAL | 26 | 67108864 |
| HEADTRACKER_SERIAL | 27 | 134217728 |
Thus, to enable MSP and LTM on a port, one would use the function **value** of 17 (1 << 0)+(1<<4), aka 1+16, aka 17.

49
docs/Profiles.md → docs/Control Profiles.md
View file

@ -1,56 +1,59 @@
# Profiles
# Control Profiles
A profile is a set of configuration settings.
Currently three profiles are supported. The default profile is profile `1`.
Currently, INAV gives you three control profiles. The default control profile is `1`.
## Changing profiles
## Changing contorl profiles
### Stick Commands
Profiles can be selected using a GUI or the following stick combinations:
Control profiles can be selected using a GUI or the following stick combinations:
| Profile | Throttle | Yaw | Pitch | Roll |
| ------- | -------- | ----- | ------ | ------ |
| 1 | Down | Left | Middle | Left |
| 2 | Down | Left | Up | Middle |
| 3 | Down | Left | Middle | Right |
| Profile # | Throttle | Yaw | Pitch | Roll |
| -------- | -------- | ----- | ------ | ------ |
| 1 | Down | Left | Middle | Left |
| 2 | Down | Left | Up | Middle |
| 3 | Down | Left | Middle | Right |
### CLI
The CLI `profile` command can also be used to change profiles:
The CLI `control_profile` command can also be used to change control profiles:
```
profile <index>
control_profile <index>
```
### Programming (4.0.0 onwards)
You can change profiles using the programming frame work. This allows a lot of flexability in how you change profiles.
You can change control profiles using the programming frame work. This allows a lot of flexability in how you change profiles.
For example, using a simple switch on channel 15.
[![For example, using a simple switch](https://i.imgur.com/SS9CaaOl.png)](https://i.imgur.com/SS9CaaO.png)
Or using the speed to change profiles. In this example:
- when lower than 25 cm/s (basically not flying), profiles are not effected.
- Below 2682 cm/s (60 mph | 97 Km/h) use Profile 1
- Above 5364 cm/s (120 mph | 193 Km/h) use Profile 3
- Between 2683 and 5364 cm/s, use Profile 2
Or using the speed to change control profiles. In this example:
- when lower than 25 cm/s (basically not flying), control profiles are not effected.
- Below 2682 cm/s (60 mph | 97 Km/h) use control profile 1
- Above 5364 cm/s (120 mph | 193 Km/h) use control profile 3
- Between 2683 and 5364 cm/s, use control profile 2
[![Using speed to change profiles](https://i.imgur.com/WjkuhhWl.png)](https://i.imgur.com/WjkuhhW.png)
#### Configurator use with profile changing logic.
> [!NOTE]
> From INAV 8.0, the programming framework operator is **Set Control Profile** and the **Flight** Operand is **Active Control Profile**. Pre-INAV 8.0, they were **Set Profile** and **Active Profile** respectively.
If you have logic conditions that change the profiles. You may find that if you manually change the profile using the drop down boxes in the top right of Configurator; that they switch back to a different profile. This is because the logic conditions are still running in the background. If this is the case, the simplest solutuion is to temporarily disable the switches that trigger the `set profile` operations. Remember to re-enable these switches after you have made your changes.
#### Configurator use with control profile changing logic.
If you have logic conditions that change the profiles. You may find that if you manually change the control profile; using the drop down boxes in the top right of Configurator. That they switch back to a different control profile. This is because the logic conditions are still running in the background. If this is the case, the simplest solutuion is to temporarily disable the switches that trigger the `Set Control Profile` operations. Remember to re-enable these switches after you have made your changes.
[![Disabled SET PROFILE switches](https://i.imgur.com/AeH9ll7l.png)](https://i.imgur.com/AeH9ll7.png)
## Profile Contents
The values contained within a profile can be seen by using the CLI `dump profile` command.
The values contained within a control profile can be seen by using the CLI `dump control_profile` command.
e.g
```
# dump profile
# dump control_profile
# profile
profile 1
# control_profile
control_profile 1
set mc_p_pitch = 40
set mc_i_pitch = 30

6
docs/Controls.md
View file

@ -23,9 +23,9 @@ The stick positions are combined to activate different functions:
| Function | Throttle | Yaw | Pitch | Roll |
| ----------------------------- | -------- | ------- | ------ | ------ |
| Profile 1 | LOW | LOW | CENTER | LOW |
| Profile 2 | LOW | LOW | HIGH | CENTER |
| Profile 3 | LOW | LOW | CENTER | HIGH |
| Control Profile 1 | LOW | LOW | CENTER | LOW |
| Control Profile 2 | LOW | LOW | HIGH | CENTER |
| Control Profile 3 | LOW | LOW | CENTER | HIGH |
| Battery profile 1 | HIGH | LOW | CENTER | LOW |
| Battery profile 2 | HIGH | LOW | HIGH | CENTER |
| Battery profile 3 | HIGH | LOW | CENTER | HIGH |

50
docs/DJI compatible OSD.md Normal file
View file

@ -0,0 +1,50 @@
# DJI compatible MSP DisplayPort OSD (DJI O3 "Canvas Mode")
INAV 6.0 includes a special mode for MSP DisplayPort that supports DJI's incomplete implementations of MSP DisplayPort. This can be found on products like the DJI O3 Air Unit. INAV 6.1 expands this to include HD canvas sizes from BetaFlight 4.4.
Different flight controller firmware have different OSD symbols and elements and require different fonts. BetaFlight's font is a single page and supports a maximum of 256 glyphs, INAV's font is currently 2 pages and supports up to 512 different glyphs. DJI's font is single page and based, but not the same as, BetaFlight's font.
While there is some overlap between the glyphs in DJI and INAV, it is not possible to perform a 1 to 1 mapping for all the them. In cases where there is no suitable glyph in the DJI font, a question mark `?` will be displayed.
This mode can be enabled by selecting DJI43COMPAT or DJIHDCOMPAT as video format in the OSD tab of the configurator or by typing the following command on the CLI:
`set osd_video_system = DJI43COMPAT`
or
`set osd_video_system = DJIHDCOMPAT`
## Limitations
* Canvas size needs to be manually changed to HD on the Display menu in DJI's goggles (you may need a firmware update) and set as DJIHDCOMPAT in the OSD tab of the configurator.
* Unsupported Glyphs show up as `?`
## FAQ
### I see a lot of `?` on my OSD.
That is expected. When your INAV OSD widgets use glyphs that don't have a suitable mapping in DJI's font.
### Does it work with the G2 and Original Air Unit/Vista?
Yes.
### Is this a replacement for WTFOS?
Not exactly. WTFOS is a full implementation of MSP-Displayport for rooted Air Unit/Vista/Googles V2 and actually works much better than DJI compatibility mode. It can use all of INAV's OSD elements as intended. If you have the option of WTFOS or DJI compatability mode. WTFOS is the best option.
### Can INAV fix DJI's product?
No. OSD renderinng happens on the googles/air unit side of things. Please ask DJI to fix their incomplete MSP DisplayPort implemenation. You can probably request it in [DJI's forum](https://forum.dji.com/forum.php?mod=forumdisplay&fid=129&filter=typeid&typeid=767). To see what you're missing out on with O3. Check out what WTFOS did with the original system. Not only could the pilots upload the fonts of their choosing (who doesn't want a cool SneakyFPV font on their OSD). But there were no problems supporting and firmware. Plus, there was even an option to save the OSD to a file and overlay that over your DVR video. If you're reading this far. Please recommend to DJI that they fix their product, to at least what was possible with WTFOS.
### DJI's font now has more symbols, can you update INAV?
Maybe. If a future version of DJI's font includes more Glyphs that can be mapped into INAV. It is fairly simple to add the mapping. However, the best solution would be full support of MSP DisplayPort by DJI. Then there will never be an issue with missing icons. As the latest INAV font would be able to be uploaded on to the goggles.
### Can you replace glyph `X` with text `x description`?
While it might technically be possible to replace some glyphs with text in multiple cells, it will introduce a lot of complexity in the OSD rendering and configuration for something we hope is a temporary workaround.
### Does DJI support Canvas Mode?
Actually, no. What DJI calls Canvas Mode is actually MSP DisplayPort and is a character based OSD. Currently, the only true implementaion of Canvas Mode is with FrSKY PixelOSD. This was found on some F722 flight controllers from Matek.

9
docs/OSD.md
View file

@ -2,6 +2,15 @@
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.
General OSD information is in this document. Other documents cover specific OSD-related topics:
* [Custom OSD Messages](https://github.com/iNavFlight/inav/wiki/OSD-custom-messages)
* [OSD Hud and ESP32 radars](https://github.com/iNavFlight/inav/wiki/OSD-Hud-and-ESP32-radars)
* [OSD Joystick](https://github.com/iNavFlight/inav/blob/master/docs/OSD%20Joystick.md)
* [DJI compatible OSD.md](https://github.com/iNavFlight/inav/blob/master/docs/DJI%20compatible%20OSD.md)
* [Pixel OSD FAQ](https://github.com/iNavFlight/inav/wiki/Pixel-OSD-FAQs)
## Features and Limitations
Not all OSDs are created equally. This table shows the differences between the different systems available.

241
docs/Programming Framework.md
View file

@ -1,10 +1,10 @@
# INAV Programming Framework
INAV Programming Framework (IPF) is a mechanism that allows you to to create
INAV Programming Framework (IPF) is a mechanism that allows you to to create
custom functionality in INAV. You can choose for certain actions to be done,
based on custom conditions you select.
Logic conditions can be based on things such as RC channel values, switches, altitude,
Logic conditions can be based on things such as RC channel values, switches, altitude,
distance, timers, etc. The conditions you create can also make use of other conditions
you've entered previously.
The results can be used in:
@ -44,119 +44,124 @@ IPF can be edited using INAV Configurator user interface, or via CLI. To use COn
| Operation ID | Name | Notes |
|---------------|-------------------------------|-------|
| 0 | TRUE | Always evaluates as true |
| 1 | EQUAL | Evaluates `false` if `false` or `0` |
| 2 | GREATER_THAN | `true` if `Operand A` is a higher value than `Operand B` |
| 3 | LOWER_THAN | `true` if `Operand A` is a lower value than `Operand B` |
| 4 | LOW | `true` if `<1333` |
| 5 | MID | `true` if `>=1333 and <=1666` |
| 6 | HIGH | `true` if `>1666` |
| 0 | True | Always evaluates as true |
| 1 | Equal (A=B) | Evaluates `false` if `false` or `0` |
| 2 | Greater Than (A>B) | `true` if `Operand A` is a higher value than `Operand B` |
| 3 | Lower Than (A<B) | `true` if `Operand A` is a lower value than `Operand B` |
| 4 | Low | `true` if `<1333` |
| 5 | Mid | `true` if `>=1333 and <=1666` |
| 6 | High | `true` if `>1666` |
| 7 | AND | `true` if `Operand A` and `Operand B` are the same value or both `true` |
| 8 | OR | `true` if `Operand A` and/or `OperandB` is `true` |
| 9 | XOR | `true` if `Operand A` or `Operand B` is `true`, but not both |
| 10 | NAND | `false` if `Operand A` and `Operand B` are both `true`|
| 11 | NOR | `true` if `Operand A` and `Operand B` are both `false` |
| 12 | NOT | The boolean opposite to `Operand A` |
| 13 | Sticky | `Operand A` is the activation operator, `Operand B` is the deactivation operator. After the activation is `true`, the operator will return `true` until Operand B is evaluated as `true`|
| 14 | Basic: Add | Add `Operand A` to `Operand B` and returns the result |
| 15 | Basic: Subtract | Substract `Operand B` from `Operand A` and returns the result |
| 16 | Basic: Multiply | Multiply `Operand A` by `Operand B` and returns the result |
| 17 | Basic: Divide | Divide `Operand A` by `Operand B` and returns the result |
| 12 | NOT | The boolean opposite to `Operand A` |
| 13 | Sticky | `Operand A` is the activation operator, `Operand B` is the deactivation operator. After the activation is `true`, the operator will return `true` until Operand B is evaluated as `true`|
| 14 | Basic: Add | Add `Operand A` to `Operand B` and returns the result |
| 15 | Basic: Subtract | Substract `Operand B` from `Operand A` and returns the result |
| 16 | Basic: Multiply | Multiply `Operand A` by `Operand B` and returns the result |
| 17 | Basic: Divide | Divide `Operand A` by `Operand B` and returns the result |
| 18 | Set GVAR | Store value from `Operand B` into the Global Variable addressed by
`Operand A`. Bear in mind, that operand `Global Variable` means: Value stored in Global Variable of an index! To store in GVAR 1 use `Value 1` not `Global Variable 1` |
| 19 | Increase GVAR | Increase the GVAR indexed by `Operand A` (use `Value 1` for Global Variable 1) with value from `Operand B` |
| 20 | Decrease GVAR | Decrease the GVAR indexed by `Operand A` (use `Value 1` for Global Variable 1) with value from `Operand B` |
| 19 | Increase GVAR | Increase the GVAR indexed by `Operand A` (use `Value 1` for Global Variable 1) with value from `Operand B` |
| 20 | Decrease GVAR | Decrease the GVAR indexed by `Operand A` (use `Value 1` for Global Variable 1) with value from `Operand B` |
| 21 | Set IO Port | Set I2C IO Expander pin `Operand A` to value of `Operand B`. `Operand A` accepts values `0-7` and `Operand B` accepts `0` and `1` |
| 22 | OVERRIDE_ARMING_SAFETY | Allows the craft to arm on any angle even without GPS fix. WARNING: This bypasses all safety checks, even that the throttle is low, so use with caution. If you only want to check for certain conditions, such as arm without GPS fix. You will need to add logic conditions to check the throttle is low. |
| 23 | OVERRIDE_THROTTLE_SCALE | Override throttle scale to the value defined by operand. Operand type `0` and value `50` means throttle will be scaled by 50%. |
| 24 | SWAP_ROLL_YAW | basically, when activated, yaw stick will control roll and roll stick will control yaw. Required for tail-sitters VTOL during vertical-horizonral transition when body frame changes |
| 25 | SET_VTX_POWER_LEVEL | Sets VTX power level. Accepted values are `0-3` for SmartAudio and `0-4` for Tramp protocol |
| 26 | INVERT_ROLL | Inverts ROLL axis input for PID/PIFF controller |
| 27 | INVERT_PITCH | Inverts PITCH axis input for PID/PIFF controller |
| 28 | INVERT_YAW | Inverts YAW axis input for PID/PIFF controller |
| 29 | OVERRIDE_THROTTLE | Override throttle value that is fed to the motors by mixer. Operand is scaled in us. `1000` means throttle cut, `1500` means half throttle |
| 30 | SET_VTX_BAND | Sets VTX band. Accepted values are `1-5` |
| 31 | SET_VTX_CHANNEL | Sets VTX channel. Accepted values are `1-8` |
| 32 | SET_OSD_LAYOUT | Sets OSD layout. Accepted values are `0-3` |
| 33 | Trigonometry: Sine | Computes SIN of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` |
| 34 | Trigonometry: Cosine | Computes COS of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` |
| 35 | Trigonometry: Tangent | Computes TAN of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` |
| 36 | MAP_INPUT | Scales `Operand A` from [`0` : `Operand B`] to [`0` : `1000`]. Note: input will be constrained and then scaled |
| 37 | MAP_OUTPUT | Scales `Operand A` from [`0` : `1000`] to [`0` : `Operand B`]. Note: input will be constrained and then scaled |
| 38 | RC_CHANNEL_OVERRIDE | Overrides channel set by `Operand A` to value of `Operand B`. Note operand A should normally be set as a "Value", NOT as "Get RC Channel"|
| 39 | SET_HEADING_TARGET | Sets heading-hold target to `Operand A`, in degrees. Value wraps-around. |
| 40 | Modulo | Modulo. Divide `Operand A` by `Operand B` and returns the remainder |
| 41 | LOITER_RADIUS_OVERRIDE | Sets the loiter radius to `Operand A` [`0` : `100000`] in cm. If the value is lower than the loiter radius set in the **Advanced Tuning**, that will be used. |
| 42 | SET_PROFILE | Sets the active config profile (PIDFF/Rates/Filters/etc) to `Operand A`. `Operand A` must be a valid profile number, currently from 1 to 3. If not, the profile will not change |
| 43 | Use Lowest Value | Finds the lowest value of `Operand A` and `Operand B` |
| 44 | Use Highest Value | Finds the highest value of `Operand A` and `Operand B` |
| 45 | FLIGHT_AXIS_ANGLE_OVERRIDE | Sets the target attitude angle for axis. In other words, when active, it enforces Angle mode (Heading Hold for Yaw) on this axis (Angle mode does not have to be active). `Operand A` defines the axis: `0` - Roll, `1` - Pitch, `2` - Yaw. `Operand B` defines the angle in degrees |
| 46 | FLIGHT_AXIS_RATE_OVERRIDE | Sets the target rate (rotation speed) for axis. `Operand A` defines the axis: `0` - Roll, `1` - Pitch, `2` - Yaw. `Operand B` defines the rate in degrees per second |
| 47 | EDGE | Momentarily true when triggered by `Operand A`. `Operand A` is the activation operator [`boolean`], `Operand B` _(Optional)_ is the time for the edge to stay active [ms]. After activation, operator will return `true` until the time in Operand B is reached. If a pure momentary edge is wanted. Just leave `Operand B` as the default `Value: 0` setting. |
| 48 | DELAY | Delays activation after being triggered. This will return `true` when `Operand A` _is_ true, and the delay time in `Operand B` [ms] has been exceeded. |
| 49 | TIMER | A simple on - off timer. `true` for the duration of `Operand A` [ms]. Then `false` for the duration of `Operand B` [ms]. |
| 50 | DELTA | This returns `true` when the value of `Operand A` has changed by the value of `Operand B` or greater within 100ms. |
| 51 | APPROX_EQUAL | `true` if `Operand B` is within 1% of `Operand A`. |
| 52 | LED_PIN_PWM | Value `Operand A` from [`0` : `100`] starts PWM generation on LED Pin. See [LED pin PWM](LED%20pin%20PWM.md). Any other value stops PWM generation (stop to allow ws2812 LEDs updates in shared modes)|
| 22 | Override Arming Safety | Allows the craft to arm on any angle even without GPS fix. WARNING: This bypasses all safety checks, even that the throttle is low, so use with caution. If you only want to check for certain conditions, such as arm without GPS fix. You will need to add logic conditions to check the throttle is low. |
| 23 | Override Throttle Scale | Override throttle scale to the value defined by operand. Operand type `0` and value `50` means throttle will be scaled by 50%. |
| 24 | Swap Roll & Yaw | basically, when activated, yaw stick will control roll and roll stick will control yaw. Required for tail-sitters VTOL during vertical-horizonral transition when body frame changes |
| 25 | Set VTx Power Level | Sets VTX power level. Accepted values are `0-3` for SmartAudio and `0-4` for Tramp protocol |
| 26 | Invert Roll | Inverts ROLL axis input for PID/PIFF controller |
| 27 | Invert Pitch | Inverts PITCH axis input for PID/PIFF controller |
| 28 | Invert Yaw | Inverts YAW axis input for PID/PIFF controller |
| 29 | Override Throttlw | Override throttle value that is fed to the motors by mixer. Operand is scaled in us. `1000` means throttle cut, `1500` means half throttle |
| 30 | Set VTx Band | Sets VTX band. Accepted values are `1-5` |
| 31 | Set VTx Channel | Sets VTX channel. Accepted values are `1-8` |
| 32 | Set OSD Layout | Sets OSD layout. Accepted values are `0-3` |
| 33 | Trigonometry: Sine | Computes SIN of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` |
| 34 | Trigonometry: Cosine | Computes COS of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` |
| 35 | Trigonometry: Tangent | Computes TAN of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` |
| 36 | Map Input | Scales `Operand A` from [`0` : `Operand B`] to [`0` : `1000`]. Note: input will be constrained and then scaled |
| 37 | Map Output | Scales `Operand A` from [`0` : `1000`] to [`0` : `Operand B`]. Note: input will be constrained and then scaled |
| 38 | Override RC Channel | Overrides channel set by `Operand A` to value of `Operand B`. Note operand A should normally be set as a "Value", NOT as "Get RC Channel"|
| 39 | Set Heading Target | Sets heading-hold target to `Operand A`, in centidegrees. Value wraps-around. |
| 40 | Modulo | Modulo. Divide `Operand A` by `Operand B` and returns the remainder |
| 41 | Override Loiter Radius | Sets the loiter radius to `Operand A` [`0` : `100000`] in cm. Must be larger than the loiter radius set in the **Advanced Tuning**. |
| 42 | Set Control Profile | Sets the active config profile (PIDFF/Rates/Filters/etc) to `Operand A`. `Operand A` must be a valid profile number, currently from 1 to 3. If not, the profile will not change |
| 43 | Use Lowest Value | Finds the lowest value of `Operand A` and `Operand B` |
| 44 | Use Highest Value | Finds the highest value of `Operand A` and `Operand B` |
| 45 | Flight Axis Angle Override | Sets the target attitude angle for axis. In other words, when active, it enforces Angle mode (Heading Hold for Yaw) on this axis (Angle mode does not have to be active). `Operand A` defines the axis: `0` - Roll, `1` - Pitch, `2` - Yaw. `Operand B` defines the angle in degrees |
| 46 | Flight Axis Rate Override | Sets the target rate (rotation speed) for axis. `Operand A` defines the axis: `0` - Roll, `1` - Pitch, `2` - Yaw. `Operand B` defines the rate in degrees per second |
| 47 | Edge | Momentarily true when triggered by `Operand A`. `Operand A` is the activation operator [`boolean`], `Operand B` _(Optional)_ is the time for the edge to stay active [ms]. After activation, operator will return `true` until the time in Operand B is reached. If a pure momentary edge is wanted. Just leave `Operand B` as the default `Value: 0` setting. |
| 48 | Delay | Delays activation after being triggered. This will return `true` when `Operand A` _is_ true, and the delay time in `Operand B` [ms] has been exceeded. |
| 49 | Timer | A simple on - off timer. `true` for the duration of `Operand A` [ms]. Then `false` for the duration of `Operand B` [ms]. |
| 50 | Delta (|A| >= B) | This returns `true` when the value of `Operand A` has changed by the value of `Operand B` or greater within 100ms. |
| 51 | Approx Equals (A ~ B) | `true` if `Operand B` is within 1% of `Operand A`. |
| 52 | LED Pin PWM | Value `Operand A` from [`0` : `100`] starts PWM generation on LED Pin. See [LED pin PWM](LED%20pin%20PWM.md). Any other value stops PWM generation (stop to allow ws2812 LEDs updates in shared modes). |
| 53 | Disable GPS Sensor Fix | Disables the GNSS sensor fix. For testing GNSS failure. |
| 54 | Mag calibration | Trigger a magnetometer calibration. |
### Operands
| Operand Type | Name | Notes |
|---------------|-----------------------|-------|
| 0 | VALUE | Value derived from `value` field |
| 1 | GET_RC_CHANNEL | `value` points to RC channel number, indexed from 1 |
| 2 | FLIGHT | `value` points to flight parameter table |
| 3 | FLIGHT_MODE | `value` points to flight modes table |
| 4 | LC | `value` points to other logic condition ID |
| 5 | GVAR | Value stored in Global Variable indexed by `value`. `GVAR 1` means: value in GVAR 1 |
| 5 | PID | Output of a Programming PID indexed by `value`. `PID 1` means: value in PID 1 |
| 0 | Value | Value derived from `value` field |
| 1 | Get RC Channel | `value` points to RC channel number, indexed from 1 |
| 2 | Flight | `value` points to **Flight** Parameters table |
| 3 | Flight Mode | `value` points to **Flight_Mode** table |
| 4 | Logic Condition | `value` points to other logic condition ID |
| 5 | Get Global Variable | Value stored in Global Variable indexed by `value`. `GVAR 1` means: value in GVAR 1 |
| 5 | Programming PID | Output of a Programming PID indexed by `value`. `PID 1` means: value in PID 1 |
| 6 | Waypoints | `value` points to the **Waypoint** parameter table |
#### FLIGHT
#### Flight Parameters
| Operand Value | Name | Notes |
|---------------|-------------------------------|-------|
| 0 | ARM_TIMER | in `seconds` |
| 1 | HOME_DISTANCE | in `meters` |
| 2 | TRIP_DISTANCE | in `meters` |
| 3 | RSSI | |
| 4 | VBAT | in `Volts * 100`, eg. `12.1V` is `1210` |
| 5 | CELL_VOLTAGE | in `Volts * 100`, eg. `12.1V` is `1210` |
| 6 | CURRENT | in `Amps * 100`, eg. `9A` is `900` |
| 7 | MAH_DRAWN | in `mAh` |
| 8 | GPS_SATS | |
| 9 | GROUD_SPEED | in `cm/s` |
| 10 | 3D_SPEED | in `cm/s` |
| 11 | AIR_SPEED | in `cm/s` |
| 12 | ALTITUDE | in `cm` |
| 13 | VERTICAL_SPEED | in `cm/s` |
| 14 | TROTTLE_POS | in `%` |
| 15 | ATTITUDE_ROLL | in `degrees` |
| 16 | ATTITUDE_PITCH | in `degrees` |
| 17 | IS_ARMED | boolean `0`/`1` |
| 18 | IS_AUTOLAUNCH | boolean `0`/`1` |
| 19 | IS_ALTITUDE_CONTROL | boolean `0`/`1` |
| 20 | IS_POSITION_CONTROL | boolean `0`/`1` |
| 21 | IS_EMERGENCY_LANDING | boolean `0`/`1` |
| 22 | IS_RTH | boolean `0`/`1` |
| 23 | IS_LANDING | boolean `0`/`1` |
| 24 | IS_FAILSAFE | boolean `0`/`1` |
| 25 | STABILIZED_ROLL | Roll PID controller output `[-500:500]` |
| 26 | STABILIZED_PITCH | Pitch PID controller output `[-500:500]` |
| 27 | STABILIZED_YAW | Yaw PID controller output `[-500:500]` |
| 28 | 3D HOME_DISTANCE | in `meters`, calculated from HOME_DISTANCE and ALTITUDE using Pythagorean theorem |
| 29 | CROSSFIRE LQ | Crossfire Link quality as returned by the CRSF protocol |
| 30 | CROSSFIRE SNR | Crossfire SNR as returned by the CRSF protocol |
| 31 | GPS_VALID | boolean `0`/`1`. True when the GPS has a valid 3D Fix |
| 32 | LOITER_RADIUS | The current loiter radius in cm. |
| 33 | ACTIVE_PROFILE | integer for the active config profile `[1..MAX_PROFILE_COUNT]` |
| 34 | BATT_CELLS | Number of battery cells detected |
| 35 | AGL_STATUS | boolean `1` when AGL can be trusted, `0` when AGL estimate can not be trusted |
| 36 | AGL | integer Above The Groud Altitude in `cm` |
| 37 | RANGEFINDER_RAW | integer raw distance provided by the rangefinder in `cm` |
| 38 | ACTIVE_MIXER_PROFILE | Which mixers are currently active (for vtol etc) |
| 39 | MIXER_TRANSITION_ACTIVE | Currently switching between mixers (quad to plane etc) |
| 40 | ATTITUDE_YAW | current heading (yaw) in `degrees` |
| 41 | FW Land Sate | integer `1` - `5`, indicates the status of the FW landing, 0 Idle, 1 Downwind, 2 Base Leg, 3 Final Approach, 4 Glide, 5 Flare |
| Operand Value | Name | Notes |
|---------------|---------------------------------------|-------|
| 0 | ARM Timer [s] | Time since armed in `seconds` |
| 1 | Home Distance [m] | distance from home in `meters` |
| 2 | Trip distance [m] | Trip distance in `meters` |
| 3 | RSSI | |
| 4 | Vbat [centi-Volt] [1V = 100] | VBAT Voltage in `Volts * 100`, eg. `12.1V` is `1210` |
| 5 | Cell voltage [centi-Volt] [1V = 100] | Average cell voltage in `Volts * 100`, eg. `12.1V` is `1210` |
| 6 | Current [centi-Amp] [1A = 100] | Current in `Amps * 100`, eg. `9A` is `900` |
| 7 | Current drawn [mAh] | Total used current in `mAh` |
| 8 | GPS Sats | |
| 9 | Ground speed [cm/s] | Ground speed in `cm/s` |
| 10 | 3D speed [cm/s] | 3D speed in `cm/s` |
| 11 | Air speed [cm/s] | Air speed in `cm/s` |
| 12 | Altitude [cm] | Altitude in `cm` |
| 13 | Vertical speed [cm/s] | Vertical speed in `cm/s` |
| 14 | Throttle position [%] | Throttle position in `%` |
| 15 | Roll [deg] | Roll attitude in `degrees` |
| 16 | Pitch [deg] | Pitch attitude in `degrees` |
| 17 | Is Armed | Is the system armed? boolean `0`/`1` |
| 18 | Is Autolaunch | Is auto launch active? boolean `0`/`1` |
| 19 | Is Controlling Altitude | Is altitude being controlled? boolean `0`/`1` |
| 20 | Is Controlling Position | Is the position being controlled? boolean `0`/`1` |
| 21 | Is Emergency Landing | Is the aircraft emergency landing? boolean `0`/`1` |
| 22 | Is RTH | Is RTH active? boolean `0`/`1` |
| 23 | Is Landing | Is the aircaft automatically landing? boolean `0`/`1` |
| 24 | Is Failsafe | Is the flight controller in a failsafe? boolean `0`/`1` |
| 25 | Stabilized Roll | Roll PID controller output `[-500:500]` |
| 26 | Stabilized Pitch | Pitch PID controller output `[-500:500]` |
| 27 | Stabilized Yaw | Yaw PID controller output `[-500:500]` |
| 28 | 3D home distance [m] | 3D distance to home in `meters`. Calculated from Home distance and Altitude using Pythagorean theorem |
| 29 | CRSF LQ | Link quality as returned by the CRSF protocol |
| 30 | CRSF SNR | SNR as returned by the CRSF protocol |
| 31 | GPS Valid Fix | Boolean `0`/`1`. True when the GPS has a valid 3D Fix |
| 32 | Loiter Radius [cm] | The current loiter radius in cm. |
| 33 | Active Control Profile | Integer for the active config profile `[1..MAX_PROFILE_COUNT]` |
| 34 | Battery cells | Number of battery cells detected |
| 35 | AGL status [0/1] | Boolean `1` when AGL can be trusted, `0` when AGL estimate can not be trusted |
| 36 | AGL [cm] | Integer altitude above The Groud Altitude in `cm` |
| 37 | Rangefinder [cm] | Integer raw distance provided by the rangefinder in `cm` |
| 38 | Active Mixer Profile | Which mixer is currently active (for vtol etc) |
| 39 | Mixer Transition Active | Boolean `0`/`1`. Are we currently switching between mixers (quad to plane etc) |
| 40 | Yaw [deg] | Current heading (yaw) in `degrees` |
| 41 | FW Land Sate | Integer `1` - `5`, indicates the status of the FW landing, 0 Idle, 1 Downwind, 2 Base Leg, 3 Final Approach, 4 Glide, 5 Flare |
| 42 | Current battery profile | The active battery profile. Integer `[1..MAX_PROFILE_COUNT]` |
| 43 | Flown Loiter Radius [m] | The actual loiter radius flown by a fixed wing during hold modes, in `meters` |
#### FLIGHT_MODE
@ -164,22 +169,22 @@ The flight mode operands return `true` when the mode is active. These are modes
| Operand Value | Name | Notes |
|---------------|-------------------|-------|
| 0 | FAILSAFE | `true` when a **Failsafe** state has been triggered. |
| 1 | MANUAL | `true` when you are in the **Manual** flight mode. |
| 0 | Failsafe | `true` when a **Failsafe** state has been triggered. |
| 1 | Manual | `true` when you are in the **Manual** flight mode. |
| 2 | RTH | `true` when you are in the **Return to Home** flight mode. |
| 3 | POSHOLD | `true` when you are in the **Position Hold** or **Loiter** flight modes. |
| 4 | CRUISE | `true` when you are in the **Cruise** flight mode. |
| 5 | ALTHOLD | `true` when you the **Altitude Hold** flight mode modifier is active. |
| 6 | ANGLE | `true` when you are in the **Angle** flight mode. |
| 7 | HORIZON | `true` when you are in the **Horizon** flight mode. |
| 8 | AIR | `true` when you the **Airmode** flight mode modifier is active. |
| 9 | USER1 | `true` when the **USER 1** mode is active. |
| 10 | USER2 | `true` when the **USER 2** mode is active. |
| 11 | COURSE_HOLD | `true` when you are in the **Course Hold** flight mode. |
| 12 | USER3 | `true` when the **USER 3** mode is active. |
| 13 | USER4 | `true` when the **USER 4** mode is active. |
| 14 | ACRO | `true` when you are in the **Acro** flight mode. |
| 15 | WAYPOINT_MISSION | `true` when you are in the **WP Mission** flight mode. |
| 3 | Position Hold | `true` when you are in the **Position Hold** or **Loiter** flight modes. |
| 4 | Cruise | `true` when you are in the **Cruise** flight mode. |
| 5 | Altitude Hold | `true` when you the **Altitude Hold** flight mode modifier is active. |
| 6 | Angle | `true` when you are in the **Angle** flight mode. |
| 7 | Horizon | `true` when you are in the **Horizon** flight mode. |
| 8 | Air | `true` when you the **Airmode** flight mode modifier is active. |
| 9 | USER 1 | `true` when the **USER 1** mode is active. |
| 10 | USER 2 | `true` when the **USER 2** mode is active. |
| 11 | Course Hold | `true` when you are in the **Course Hold** flight mode. |
| 12 | USER 3 | `true` when the **USER 3** mode is active. |
| 13 | USER 4 | `true` when the **USER 4** mode is active. |
| 14 | Acro | `true` when you are in the **Acro** flight mode. |
| 15 | Waypoint Mission | `true` when you are in the **WP Mission** flight mode. |
#### WAYPOINTS
@ -212,7 +217,7 @@ The flight mode operands return `true` when the mode is active. These are modes
| JUMP | 6 |
| SET_HEAD | 7 |
| LAND | 8 |
### Flags
All flags are reseted on ARM and DISARM event.
@ -329,11 +334,9 @@ Steps:
## Common issues / questions about IPF
One common mistake involves setting RC channel values. To override (set) the
One common mistake involves setting RC channel values. To override (set) the
value of a specific RC channel, choose "Override RC value", then for operand A
choose *value* and enter the channel number. Choosing "get RC value" is a common mistake,
which does something other than what you probably want.
![screenshot of override an RC channel with a value](./assets/images/ipf_set_get_rc_channel.png)

2
docs/Rx.md
View file

@ -205,7 +205,7 @@ Note:
## SIM (SITL) Joystick
Enables the use of a joystick in the INAV SITL with a flight simulator. See the [SITL documentation](SITL/SITL).
Enables the use of a joystick in the INAV SITL with a flight simulator. See the [SITL documentation](SITL/SITL.md).
## Configuration

25
docs/SBUS2_Telemetry.md Normal file
View file

@ -0,0 +1,25 @@
# Futaba SBUS2 Telemetry
Basic experimental support for SBUS2 telemetry has been added to INAV 8.0.0. Currently it is limited to F7 and H7 mcus only. The main reason it is limited to those MCUs is due to the requirement for an inverted UART signal and the SBUS pads in F405 usually are not bi-directional.
The basic sensors have been tested with a Futaba T16IZ running software version 6.0E.
An alternative to using INAV's SBUS2 support is to use SBS-01ML MAVlink Telemetry Drone Sensor instead. (not tested and not supported with older futaba radios, including my 16IZ).
# Wiring
The SBUS2 signal should be connected to the TX PIN, not the RX PIN, like on a traditional SBUS setup.
# Sensor mapping
The following fixed sensor mapping is used:
| Slot | Sensort Type | Info |
| --- | --- | --- |
| 1 | Voltage | Pack voltage and cell voltage |
| 3 | Current | Capacity = used mAh |
| 6 | rpm sensor | motor rpm. Need to set geat ratio to 1.0 |
| 7 | Temperature | ESC Temperature |
| 8 | GPS | |
| 16 | Temperature | IMU Temperature |
| 17 | Temperature | Baro Temperature |
| 18-25 | Temperature | Temperature sensor 0-7 |

56
docs/Serial Gimbal.md Normal file
View file

@ -0,0 +1,56 @@
# Serial Gimbal
INAV 8.0 introduces support for serial Gimbals. Currently, it is compatible with the protocol used by the Walksnail GM series gimbals.
While these gimbals also support PWM as input, using the Serial protocol gives it more flexibility and saves up to 4 PWM channels. The downside of the Serial protocol vs PWM input is that you don't have access to the full power of INAV's mixers. The main advantage is that you gain easy control of gimbal functions using INAV's modes.
# Axis Input
The Serial Gimbal supports 2 differents inputs.
## PWM Channels
This is the simplest way to control the Gimbal, as you can use your radio mixer and sliders to Control the gimbal by assigning RC channels to functions in the ```Configuration``` tab. You can control all 3 gimbal axis and unlike the raw PWM input, gimbal modes are controlled by INAV modes and you can control roll channel as well, instead of wiring 4 servo outputs. If an rc channel is set to 0, that input will be ignore and will be equivalent to a centered RC channel. So, if you setup the serial gimbal and don't assign any rc channels, it will stay centered, with default sensitivity and will obey the Gimbal MODES setup in the Modes tab.
## Headtracker Input
Headtracker input is only used when you have a Headtracker device configured and the ```Gimbal Headtracker``` mode is active.
A Headtracker device is a device that transmits headtracker information by a side channel, instead of relying on your rc link.
In head tracker mode, the Serial Gimbal will ignore the axis rc channel inputs and replace it with the inputs coming from the Headtracker device.
# Gimbal Modes
## No Gimbal mode selected
Like ACRO is the default mode for flight modes, the Gimbal will default to ```FPV Mode``` or ```Follow Mode``` when no mode is selected. The gimbal will try to stablized the footage and will follow the aircraft pitch, roll and yaw movements and use user inputs to point the camera where the user wants.
## Gimbal Center
This locks the gimbal camera to the center position and ignores any user input. Useful to reset the camera if you loose orientation.
## Gimbal Headtracker
Switches inputs to the configured Headtracker device. If no device is configured it will behave like Gimbal Center.
## Gimbal Level Tilt
This mode locks the camera tilt (pitch axis) and keeps it level with the horizon. Pitching the aircraft up and down, will move the camera so it stays pointing at the horizon. It can be combined with ```Gimbal Level Roll```.
## Gimbal Level Roll
This mode locks the camera roll and keeps it level with the horizon. Rolling the aircraft will move the camera so it stays level with the horizon. It can be combined with ```Gimbal Level Tilt```.
# Advanced settings
The gimbal also supports some advanced settings not exposed in the configurator.
## Gimbal Trim
You can set a trim setting for the gimbal, the idea is that it will shift the notion of center of the gimbal, like a trim and let you setup a fixed camera up tilt, like you would have in a traditional fpv quad setup.
```
gimbal_pan_trim = 0
Allowed range: -500 - 500
gimbal_tilt_trim = 0
Allowed range: -500 - 500
gimbal_roll_trim = 0
Allowed range: -500 - 500
```
## Gimbal and Headtracker on a single uart
As INAV does not process any inputs from the Walksnail Gimbal, it is possible to share the uart with the Walksnail Headtracking output by connect the fc TX to the gimbal and RX to receive the headtracker input.
```
gimbal_serial_single_uart = OFF
Allowed values: OFF, ON
```

320
docs/Settings.md
View file

@ -1302,13 +1302,33 @@ Fixed-wing rate stabilisation I-gain for YAW
---
### fw_iterm_limit_stick_position
### fw_iterm_lock_engage_threshold
Iterm is not allowed to grow when stick position is above threshold. This solves the problem of bounceback or followthrough when full stick deflection is applied on poorely tuned fixed wings. In other words, stabilization is partialy disabled when pilot is actively controlling the aircraft and active when sticks are not touched. `0` mean stick is in center position, `1` means it is fully deflected to either side
Defines error rate (in percents of max rate) when Iterm Lock is engaged when sticks are release. Iterm Lock will stay active until error drops below this number
| Default | Min | Max |
| --- | --- | --- |
| 0.5 | 0 | 1 |
| 10 | 5 | 25 |
---
### fw_iterm_lock_rate_threshold
Defines rate percentage when full P I and D attenuation should happen. 100 disables Iterm Lock for P and D term
| Default | Min | Max |
| --- | --- | --- |
| 40 | 10 | 100 |
---
### fw_iterm_lock_time_max_ms
Defines max time in milliseconds for how long ITerm Lock will shut down Iterm after sticks are release
| Default | Min | Max |
| --- | --- | --- |
| 500 | 100 | 1000 |
---
@ -1442,6 +1462,86 @@ Yaw Iterm is frozen when bank angle is above this threshold [degrees]. This solv
---
### gimbal_pan_channel
Gimbal pan rc channel index. 0 is no channel.
| Default | Min | Max |
| --- | --- | --- |
| 0 | 0 | 32 |
---
### gimbal_pan_trim
Trim gimbal pan center position.
| Default | Min | Max |
| --- | --- | --- |
| 0 | -500 | 500 |
---
### gimbal_roll_channel
Gimbal roll rc channel index. 0 is no channel.
| Default | Min | Max |
| --- | --- | --- |
| 0 | 0 | 32 |
---
### gimbal_roll_trim
Trim gimbal roll center position.
| Default | Min | Max |
| --- | --- | --- |
| 0 | -500 | 500 |
---
### gimbal_sensitivity
Gimbal sensitivity is similar to gain and will affect how quickly the gimbal will react.
| Default | Min | Max |
| --- | --- | --- |
| 0 | -16 | 15 |
---
### gimbal_serial_single_uart
Gimbal serial and headtracker device share same UART. FC RX goes to headtracker device, FC TX goes to gimbal.
| Default | Min | Max |
| --- | --- | --- |
| OFF | OFF | ON |
---
### gimbal_tilt_channel
Gimbal tilt rc channel index. 0 is no channel.
| Default | Min | Max |
| --- | --- | --- |
| 0 | 0 | 32 |
---
### gimbal_tilt_trim
Trim gimbal tilt center position.
| Default | Min | Max |
| --- | --- | --- |
| 0 | -500 | 500 |
---
### gps_auto_baud
Automatic configuration of GPS baudrate(The specified baudrate in configured in ports will be used) when used with UBLOX GPS
@ -1494,7 +1594,7 @@ Minimum number of GPS satellites in view to acquire GPS_FIX and consider GPS pos
### gps_provider
Which GPS protocol to be used, note that UBLOX is 5Hz and UBLOX7 is 10Hz (M8N).
Which GPS protocol to be used.
| Default | Min | Max |
| --- | --- | --- |
@ -1514,7 +1614,7 @@ 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.
Navigation update rate for UBLOX 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 |
| --- | --- | --- |
@ -1562,6 +1662,76 @@ For developer ground test use. Disables motors, sets heading status = Trusted on
---
### gyro_adaptive_filter_hpf_hz
High pass filter cutoff frequency
| Default | Min | Max |
| --- | --- | --- |
| 10 | 1 | 50 |
---
### gyro_adaptive_filter_integrator_threshold_high
High threshold for adaptive filter integrator
| Default | Min | Max |
| --- | --- | --- |
| 4 | 1 | 10 |
---
### gyro_adaptive_filter_integrator_threshold_low
Low threshold for adaptive filter integrator
| Default | Min | Max |
| --- | --- | --- |
| -2 | -10 | 0 |
---
### gyro_adaptive_filter_max_hz
Maximum frequency for adaptive filter
| Default | Min | Max |
| --- | --- | --- |
| 150 | 0 | 505 |
---
### gyro_adaptive_filter_min_hz
Minimum frequency for adaptive filter
| Default | Min | Max |
| --- | --- | --- |
| 50 | 0 | 250 |
---
### gyro_adaptive_filter_std_lpf_hz
Standard deviation low pass filter cutoff frequency
| Default | Min | Max |
| --- | --- | --- |
| 2 | 0 | 10 |
---
### gyro_adaptive_filter_target
Target value for adaptive filter
| Default | Min | Max |
| --- | --- | --- |
| 3.5 | 1 | 6 |
---
### gyro_anti_aliasing_lpf_hz
Gyro processing anti-aliasing filter cutoff frequency. In normal operation this filter setting should never be changed. In Hz
@ -1602,6 +1772,36 @@ Minimum frequency of the gyro Dynamic LPF
---
### gyro_filter_mode
Specifies the type of the software LPF of the gyro signals.
| Default | Min | Max |
| --- | --- | --- |
| STATIC | | |
---
### gyro_lulu_enabled
Enable/disable gyro LULU filter
| Default | Min | Max |
| --- | --- | --- |
| OFF | OFF | ON |
---
### gyro_lulu_sample_count
Gyro lulu sample count, in number of samples.
| Default | Min | Max |
| --- | --- | --- |
| 3 | 1 | 15 |
---
### gyro_main_lpf_hz
Software based gyro main lowpass filter. Value is cutoff frequency (Hz)
@ -1622,16 +1822,6 @@ On multi-gyro targets, allows to choose which gyro to use. 0 = first gyro, 1 = s
---
### gyro_use_dyn_lpf
Use Dynamic LPF instead of static gyro stage1 LPF. Dynamic Gyro LPF updates gyro LPF based on the throttle position.
| Default | Min | Max |
| --- | --- | --- |
| OFF | OFF | ON |
---
### gyro_zero_x
Calculated gyro zero calibration of axis X
@ -1682,6 +1872,46 @@ This setting limits yaw rotation rate that HEADING_HOLD controller can request f
---
### headtracker_pan_ratio
Head pan movement vs camera movement ratio
| Default | Min | Max |
| --- | --- | --- |
| 1 | 0 | 5 |
---
### headtracker_roll_ratio
Head roll movement vs camera movement ratio
| Default | Min | Max |
| --- | --- | --- |
| 1 | 0 | 5 |
---
### headtracker_tilt_ratio
Head tilt movement vs camera movement ratio
| Default | Min | Max |
| --- | --- | --- |
| 1 | 0 | 5 |
---
### headtracker_type
Type of headtrackr dervice
| Default | Min | Max |
| --- | --- | --- |
| NONE | | |
---
### hott_alarm_sound_interval
Battery alarm delay in seconds for Hott telemetry
@ -1812,16 +2042,6 @@ Allows to chose when the home position is reset. Can help prevent resetting home
---
### inav_use_gps_no_baro
Defines if INAV should use only use GPS data for altitude estimation when barometer is not available. If set to ON, INAV will allow GPS assisted modes and RTH even when there is no barometer installed.
| Default | Min | Max |
| --- | --- | --- |
| ON | OFF | ON |
---
### inav_w_acc_bias
Weight for accelerometer drift estimation
@ -2118,7 +2338,7 @@ This is the main loop time (in us). Changing this affects PID effect with some P
| Default | Min | Max |
| --- | --- | --- |
| 1000 | | 9000 |
| 500 | | 9000 |
---
@ -2298,7 +2518,7 @@ Rate of the extra1 message for MAVLink telemetry
| Default | Min | Max |
| --- | --- | --- |
| 10 | 0 | 255 |
| 2 | 0 | 255 |
---
@ -2338,7 +2558,7 @@ Rate of the RC channels message for MAVLink telemetry
| Default | Min | Max |
| --- | --- | --- |
| 5 | 0 | 255 |
| 1 | 0 | 255 |
---
@ -2358,7 +2578,7 @@ Maximum inclination in level (angle) mode (PITCH axis). 100=10°
| Default | Min | Max |
| --- | --- | --- |
| 300 | 100 | 900 |
| 450 | 100 | 900 |
---
@ -2368,7 +2588,7 @@ Maximum inclination in level (angle) mode (ROLL axis). 100=10°
| Default | Min | Max |
| --- | --- | --- |
| 300 | 100 | 900 |
| 450 | 100 | 900 |
---
@ -2718,7 +2938,7 @@ Speed in fully autonomous modes (RTH, WP) [cm/s]. Used for WP mode when no speci
| Default | Min | Max |
| --- | --- | --- |
| 300 | 10 | 2000 |
| 500 | 10 | 2000 |
---
@ -2948,7 +3168,7 @@ Forward acceleration threshold for bungee launch or throw launch [cm/s/s], 1G =
| Default | Min | Max |
| --- | --- | --- |
| 1863 | 1500 | 20000 |
| 1863 | 1350 | 20000 |
---
@ -3394,7 +3614,7 @@ 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.
Allows immediate landing detection based on G bump at touchdown when set to ON. Requires a barometer and GPS and currently only works for multirotors (Note: will work during Failsafe without need for a GPS).
| Default | Min | Max |
| --- | --- | --- |
@ -3408,7 +3628,7 @@ Maximum speed allowed when processing pilot input for POSHOLD/CRUISE control mod
| Default | Min | Max |
| --- | --- | --- |
| 500 | 10 | 2000 |
| 750 | 10 | 2000 |
---
@ -3468,7 +3688,7 @@ Maximum banking angle (deg) that multicopter navigation is allowed to set. Machi
| Default | Min | Max |
| --- | --- | --- |
| 30 | 15 | 45 |
| 35 | 15 | 45 |
---
@ -3572,6 +3792,16 @@ Multicopter hover throttle hint for altitude controller. Should be set to approx
---
### nav_mc_inverted_crash_detection
Setting a value > 0 enables inverted crash detection for multirotors. It will auto disarm in situations where the multirotor has crashed inverted on the ground and can't be manually disarmed due to loss of control or for some other reason. When enabled this setting defines the additional number of seconds before disarm beyond a minimum fixed time delay of 3s. Requires a barometer to work.
| Default | Min | Max |
| --- | --- | --- |
| 0 | 0 | 15 |
---
### nav_mc_manual_climb_rate
Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s]
@ -4412,6 +4642,16 @@ Value under which the OSD axis g force indicators will blink (g)
---
### osd_highlight_djis_missing_font_symbols
Show question marks where there is no symbol in the DJI font to represent the INAV OSD element's symbol. When off, blank spaces will be used. Only relevent for DJICOMPAT modes.
| Default | Min | Max |
| --- | --- | --- |
| ON | OFF | ON |
---
### osd_home_position_arm_screen
Should home position coordinates be displayed on the arming screen.
@ -4752,6 +4992,16 @@ Number of leading digits removed from plus code. Removing 2, 4 and 6 digits requ
---
### osd_radar_peers_display_time
Time in seconds to display next peer
| Default | Min | Max |
| --- | --- | --- |
| 3 | 1 | 10 |
---
### osd_right_sidebar_scroll
Scroll type for the right sidebar

18
docs/Telemetry.md
View file

@ -390,3 +390,21 @@ In configurator set IBUS telemetry and RX on this same port, enable telemetry fe
Warning:
Schematic above work also for connect telemetry only, but not work for connect rx only - will stop FC.
## Futaba SBUS2 telemetry
SBUS2 telemetry requires a single connection from the TX pin of a bidirectional serial port to the SBUS2 pin on a Futaba T-FHSS or FASSTest telemetry receiver. (tested T16IZ radio and R7108SB and R3204SB receivers)
It shares 1 line for both TX and RX, the rx pin cannot be used for other serial port stuff.
It runs at a fixed baud rate of 100000, so it needs a hardware uart capable of inverted signals. It is not available on F4 mcus.
```
_______
/ \ /-------------\
| STM32 |-->UART TX-->[Bi-directional @ 100000 baud]-->| Futaba RX |
| uC |- UART RX--x[not connected] | SBUS2 port |
\_______/ \-------------/
```
For more information and sensor slot numbering, refer to [SBUS2 Documentation](SBUS2_telemetry.md)

7
docs/USB Flashing.md
View file

@ -49,14 +49,19 @@ With the board connected and in bootloader mode (reset it by sending the charact
* Choose Options > List All Devices
* Select `STM32 BOOTLOADER` in the device list
* Choose `WinUSB (v6.x.x.x)` in the right hand box
![Zadig Driver Procedure](assets/images/zadig-dfu.png)
* Click Replace Driver
* Restart the Configurator (make sure it is completely closed, logout and login if unsure)
* Now the DFU device should be seen by Configurator
## While Using USB-C cables
* If you are using a device with only USB-C ports such as a Mac-OS device, you will need a dongle.
* A USB-C to USB-C cable is identical on both ends and thus requires extra hardware to let them be auto detected as devices instead of hosts.
* Using a USB-A to C cable or dongle is usually the easiest way to get a working connection but an USB-OTG adapter also works.
## Using `dfu-util`
`dfu-util` is a command line tool to flash ARM devices via DFU. It is available via the package manager on most Linux systems or from [source forge](http://sourceforge.net/p/dfu-util).

25
readme.md
View file

@ -6,6 +6,22 @@
> INAV 7 is the last INAV official release available for F411 based flight controllers. The next milestone, INAV 8 will not be available for F411 boards.
# ICM426xx IMUs PSA
> The filtering settings for the ICM426xx has changed to match what is used by Ardupilot and Betaflight in INAV 7.1. When upgrading from older versions you may need to recalibrate the Accelerometer and if you are not using INAV's default tune you may also want to check if the tune is still good.
# M7, M6 and older UBLOX GPS units PSA
> INAV 8.0 will mark those GPS as deprecated and INAV 9.0.0 will require UBLOX units with Protocol version 15.00 or newer. This means that you need a GPS unit based on UBLOX M8 or newer.
> If you want to check the protocol version of your unit, it is displayed in INAV's 7.0.0+ status cli command.
> INAV 8.0.0 will warn you if your GPS is too old.
> ```GPS: HW Version: Unknown Proto: 0.00 Baud: 115200 (UBLOX Proto >= 15.0 required)```
> M8, M9 and M10 GPS are the most common units in use today, are readly available and have similar capabilities.
>Mantaining and testing GPS changes across this many UBLOX versions is a challenge and takes a lot of time. Removing the support for older devices will simplify code.
![INAV](http://static.rcgroups.net/forums/attachments/6/1/0/3/7/6/a9088858-102-inav.png)
# PosHold, Navigation and RTH without compass PSA
@ -51,7 +67,6 @@ Fly safe, fly smart with INAV 7.1 and a compass by your side!
* SmartAudio and IRC Tramp VTX support
* Telemetry: SmartPort, FPort, MAVlink, LTM, CRSF
* Multi-color RGB LED Strip support
* On Screen Display (OSD) - both character and pixel style
* And many more!
For a list of features, changes and some discussion please review consult the releases [page](https://github.com/iNavFlight/inav/releases) and the documentation.
@ -119,5 +134,13 @@ Before creating new issues please check to see if there is an existing one, sear
Please refer to the development section in the [docs/development](https://github.com/iNavFlight/inav/tree/master/docs/development) folder.
Nightly builds are available for testing on the following links:
https://github.com/iNavFlight/inav-nightly/releases
https://github.com/iNavFlight/inav-configurator-nightly/releases
## INAV Releases
https://github.com/iNavFlight/inav/releases

20
src/main/CMakeLists.txt
View file

@ -31,6 +31,8 @@ main_sources(COMMON_SRC
common/gps_conversion.h
common/log.c
common/log.h
common/lulu.c
common/lulu.h
common/maths.c
common/maths.h
common/memory.c
@ -177,6 +179,10 @@ main_sources(COMMON_SRC
drivers/flash_m25p16.h
drivers/flash_w25n01g.c
drivers/flash_w25n01g.h
drivers/gimbal_common.h
drivers/gimbal_common.c
drivers/headtracker_common.h
drivers/headtracker_common.c
drivers/io.c
drivers/io.h
drivers/io_pcf8574.c
@ -346,6 +352,8 @@ main_sources(COMMON_SRC
flight/dynamic_lpf.h
flight/ez_tune.c
flight/ez_tune.h
flight/adaptive_filter.c
flight/adaptive_filter.h
io/adsb.c
io/beeper.c
@ -354,6 +362,10 @@ main_sources(COMMON_SRC
io/servo_sbus.h
io/frsky_osd.c
io/frsky_osd.h
io/gimbal_serial.c
io/gimbal_serial.h
io/headtracker_msp.c
io/headtracker_msp.h
io/osd_dji_hd.c
io/osd_dji_hd.h
io/lights.c
@ -500,8 +512,8 @@ main_sources(COMMON_SRC
io/displayport_max7456.h
io/displayport_msp.c
io/displayport_msp.h
io/displayport_msp_bf_compat.c
io/displayport_msp_bf_compat.h
io/displayport_msp_dji_compat.c
io/displayport_msp_dji_compat.h
io/displayport_oled.c
io/displayport_oled.h
io/displayport_msp_osd.c
@ -600,6 +612,10 @@ main_sources(COMMON_SRC
telemetry/mavlink.h
telemetry/msp_shared.c
telemetry/msp_shared.h
telemetry/sbus2.c
telemetry/sbus2.h
telemetry/sbus2_sensors.c
telemetry/sbus2_sensors.h
telemetry/smartport.c
telemetry/smartport.h
telemetry/sim.c

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

@ -99,7 +99,7 @@
#define BLACKBOX_INVERTED_CARD_DETECTION 0
#endif
PG_REGISTER_WITH_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 2);
PG_REGISTER_WITH_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 3);
PG_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig,
.device = DEFAULT_BLACKBOX_DEVICE,
@ -108,7 +108,8 @@ PG_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig,
.invertedCardDetection = BLACKBOX_INVERTED_CARD_DETECTION,
.includeFlags = BLACKBOX_FEATURE_NAV_PID | BLACKBOX_FEATURE_NAV_POS |
BLACKBOX_FEATURE_MAG | BLACKBOX_FEATURE_ACC | BLACKBOX_FEATURE_ATTITUDE |
BLACKBOX_FEATURE_RC_DATA | BLACKBOX_FEATURE_RC_COMMAND | BLACKBOX_FEATURE_MOTORS,
BLACKBOX_FEATURE_RC_DATA | BLACKBOX_FEATURE_RC_COMMAND |
BLACKBOX_FEATURE_MOTORS | BLACKBOX_FEATURE_SERVOS,
);
void blackboxIncludeFlagSet(uint32_t mask)
@ -334,22 +335,43 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
{"motor", 7, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_8)},
/* servos */
{"servo", 0, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
{"servo", 1, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
{"servo", 2, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
{"servo", 3, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
{"servo", 4, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
{"servo", 5, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
{"servo", 6, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
{"servo", 7, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
{"servo", 8, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
{"servo", 9, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
{"servo", 10, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
{"servo", 11, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
{"servo", 12, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
{"servo", 13, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
{"servo", 14, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
{"servo", 15, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
{"servo", 0, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_1)},
{"servo", 1, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_2)},
{"servo", 2, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_3)},
{"servo", 3, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_4)},
{"servo", 4, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_5)},
{"servo", 5, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_6)},
{"servo", 6, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_7)},
{"servo", 7, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_8)},
{"servo", 8, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_9)},
{"servo", 9, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_10)},
{"servo", 10, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_11)},
{"servo", 11, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_12)},
{"servo", 12, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_13)},
{"servo", 13, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_14)},
{"servo", 14, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_15)},
{"servo", 15, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_16)},
{"servo", 16, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_17)},
{"servo", 17, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_18)},
{"servo", 18, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_18)},
{"servo", 19, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_19)},
{"servo", 20, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_20)},
{"servo", 21, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_21)},
{"servo", 22, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_22)},
{"servo", 23, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_23)},
{"servo", 24, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_24)},
{"servo", 25, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_25)},
/*
{"servo", 26, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_26)},
{"servo", 27, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_27)},
{"servo", 27, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_28)},
{"servo", 28, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_29)},
{"servo", 29, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_30)},
{"servo", 30, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_31)},
{"servo", 31, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_32)},
{"servo", 32, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_33)},
{"servo", 33, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_34)},
*/
{"navState", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"navFlags", -1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
@ -402,12 +424,21 @@ static const blackboxSimpleFieldDefinition_t blackboxGpsHFields[] = {
// Rarely-updated fields
static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = {
/* "flightModeFlags" renamed internally to more correct ref of rcModeFlags, since it logs rc boxmode selections,
* but name kept for external compatibility reasons.
* "activeFlightModeFlags" logs actual active flight modes rather than rc boxmodes.
* 'active' should at least distinguish it from the existing "flightModeFlags" */
{"activeWpNumber", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
{"flightModeFlags", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
{"flightModeFlags2", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
{"activeFlightModeFlags", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
{"stateFlags", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
{"failsafePhase", -1, UNSIGNED, PREDICT(0), ENCODING(TAG2_3S32)},
{"rxSignalReceived", -1, UNSIGNED, PREDICT(0), ENCODING(TAG2_3S32)},
{"rxFlightChannelsValid", -1, UNSIGNED, PREDICT(0), ENCODING(TAG2_3S32)},
{"rxUpdateRate", -1, UNSIGNED, PREDICT(PREVIOUS), ENCODING(UNSIGNED_VB)},
{"hwHealthStatus", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
{"powerSupplyImpedance", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
@ -436,8 +467,6 @@ static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = {
{"escRPM", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
{"escTemperature", -1, SIGNED, PREDICT(PREVIOUS), ENCODING(SIGNED_VB)},
#endif
{"rxUpdateRate", -1, UNSIGNED, PREDICT(PREVIOUS), ENCODING(UNSIGNED_VB)},
{"activeWpNumber", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
};
typedef enum BlackboxState {
@ -533,7 +562,9 @@ typedef struct blackboxGpsState_s {
// This data is updated really infrequently:
typedef struct blackboxSlowState_s {
uint32_t flightModeFlags; // extend this data size (from uint16_t)
uint32_t rcModeFlags;
uint32_t rcModeFlags2;
uint32_t activeFlightModeFlags;
uint32_t stateFlags;
uint8_t failsafePhase;
bool rxSignalReceived;
@ -566,7 +597,7 @@ extern boxBitmask_t rcModeActivationMask;
static BlackboxState blackboxState = BLACKBOX_STATE_DISABLED;
static uint32_t blackboxLastArmingBeep = 0;
static uint32_t blackboxLastFlightModeFlags = 0;
static uint32_t blackboxLastRcModeFlags = 0;
static struct {
uint32_t headerIndex;
@ -643,7 +674,45 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
return (getMotorCount() >= condition - FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1 + 1) && blackboxIncludeFlag(BLACKBOX_FEATURE_MOTORS);
case FLIGHT_LOG_FIELD_CONDITION_SERVOS:
return isMixerUsingServos();
return blackboxIncludeFlag(BLACKBOX_FEATURE_SERVOS) && isMixerUsingServos();
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_1:
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_2:
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_3:
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_4:
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_5:
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_6:
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_7:
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_8:
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_9:
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_10:
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_11:
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_12:
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_13:
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_14:
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_15:
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_16:
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_17:
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_18:
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_19:
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_20:
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_21:
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_22:
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_23:
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_24:
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_25:
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_26:
/*
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_27:
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_28:
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_29:
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_30:
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_31:
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_32:
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_33:
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_34:
*/
return ((FlightLogFieldCondition)MIN(getServoCount(), 26) >= condition - FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_1 + 1) && blackboxIncludeFlag(BLACKBOX_FEATURE_SERVOS);
case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0:
case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1:
@ -944,7 +1013,8 @@ static void writeIntraframe(void)
}
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_SERVOS)) {
for (int x = 0; x < MAX_SUPPORTED_SERVOS; x++) {
const int servoCount = getServoCount();
for (int x = 0; x < servoCount; x++) {
//Assume that servos spends most of its time around the center
blackboxWriteSignedVB(blackboxCurrent->servo[x] - 1500);
}
@ -1203,7 +1273,7 @@ static void writeInterframe(void)
}
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_SERVOS)) {
blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, servo), MAX_SUPPORTED_SERVOS);
blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, servo), getServoCount());
}
blackboxWriteSignedVB(blackboxCurrent->navState - blackboxLast->navState);
@ -1260,7 +1330,10 @@ static void writeSlowFrame(void)
blackboxWrite('S');
blackboxWriteUnsignedVB(slowHistory.flightModeFlags);
blackboxWriteUnsignedVB(slowHistory.activeWpNumber);
blackboxWriteUnsignedVB(slowHistory.rcModeFlags);
blackboxWriteUnsignedVB(slowHistory.rcModeFlags2);
blackboxWriteUnsignedVB(slowHistory.activeFlightModeFlags);
blackboxWriteUnsignedVB(slowHistory.stateFlags);
/*
@ -1271,6 +1344,8 @@ static void writeSlowFrame(void)
values[2] = slowHistory.rxFlightChannelsValid ? 1 : 0;
blackboxWriteTag2_3S32(values);
blackboxWriteUnsignedVB(slowHistory.rxUpdateRate);
blackboxWriteUnsignedVB(slowHistory.hwHealthStatus);
blackboxWriteUnsignedVB(slowHistory.powerSupplyImpedance);
@ -1296,8 +1371,6 @@ static void writeSlowFrame(void)
blackboxWriteUnsignedVB(slowHistory.escRPM);
blackboxWriteSignedVB(slowHistory.escTemperature);
#endif
blackboxWriteUnsignedVB(slowHistory.rxUpdateRate);
blackboxWriteUnsignedVB(slowHistory.activeWpNumber);
blackboxSlowFrameIterationTimer = 0;
}
@ -1307,27 +1380,21 @@ static void writeSlowFrame(void)
*/
static void loadSlowState(blackboxSlowState_t *slow)
{
memcpy(&slow->flightModeFlags, &rcModeActivationMask, sizeof(slow->flightModeFlags)); //was flightModeFlags;
slow->activeWpNumber = getActiveWpNumber();
slow->rcModeFlags = rcModeActivationMask.bits[0]; // first 32 bits of boxId_e
slow->rcModeFlags2 = rcModeActivationMask.bits[1]; // remaining bits of boxId_e
// Also log Nav auto enabled flight modes rather than just those selected by boxmode
if (FLIGHT_MODE(ANGLE_MODE)) {
slow->flightModeFlags |= (1 << BOXANGLE);
}
if (FLIGHT_MODE(NAV_ALTHOLD_MODE)) {
slow->flightModeFlags |= (1 << BOXNAVALTHOLD);
}
if (FLIGHT_MODE(NAV_RTH_MODE)) {
slow->flightModeFlags |= (1 << BOXNAVRTH);
}
if (navigationGetHeadingControlState() == NAV_HEADING_CONTROL_AUTO) {
slow->flightModeFlags |= (1 << BOXHEADINGHOLD);
}
if (navigationRequiresTurnAssistance()) {
slow->flightModeFlags |= (1 << BOXTURNASSIST);
slow->rcModeFlags |= (1 << BOXHEADINGHOLD);
}
slow->activeFlightModeFlags = flightModeFlags;
slow->stateFlags = stateFlags;
slow->failsafePhase = failsafePhase();
slow->rxSignalReceived = rxIsReceivingSignal();
slow->rxFlightChannelsValid = rxAreFlightChannelsValid();
slow->rxUpdateRate = getRcUpdateFrequency();
slow->hwHealthStatus = (getHwGyroStatus() << 2 * 0) | // Pack hardware health status into a bit field.
(getHwAccelerometerStatus() << 2 * 1) | // Use raw hardwareSensorStatus_e values and pack them using 2 bits per value
(getHwCompassStatus() << 2 * 2) | // Report GYRO in 2 lowest bits, then ACC, COMPASS, BARO, GPS, RANGEFINDER and PITOT
@ -1379,9 +1446,6 @@ static void loadSlowState(blackboxSlowState_t *slow)
slow->escRPM = escSensor->rpm;
slow->escTemperature = escSensor->temperature;
#endif
slow->rxUpdateRate = getRcUpdateFrequency();
slow->activeWpNumber = getActiveWpNumber();
}
/**
@ -1439,6 +1503,9 @@ static void blackboxValidateConfig(void)
#endif
#ifdef USE_SDCARD
case BLACKBOX_DEVICE_SDCARD:
#endif
#if defined(SITL_BUILD)
case BLACKBOX_DEVICE_FILE:
#endif
case BLACKBOX_DEVICE_SERIAL:
// Device supported, leave the setting alone
@ -1498,7 +1565,7 @@ void blackboxStart(void)
* it finally plays the beep for this arming event.
*/
blackboxLastArmingBeep = getArmingBeepTimeMicros();
memcpy(&blackboxLastFlightModeFlags, &rcModeActivationMask, sizeof(blackboxLastFlightModeFlags)); // record startup status
memcpy(&blackboxLastRcModeFlags, &rcModeActivationMask, sizeof(blackboxLastRcModeFlags)); // record startup status
blackboxSetState(BLACKBOX_STATE_PREPARE_LOG_FILE);
}
@ -1674,7 +1741,8 @@ static void loadMainState(timeUs_t currentTimeUs)
blackboxCurrent->rssi = getRSSI();
for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
const int servoCount = getServoCount();
for (int i = 0; i < servoCount; i++) {
blackboxCurrent->servo[i] = servo[i];
}
@ -2023,10 +2091,10 @@ static void blackboxCheckAndLogArmingBeep(void)
static void blackboxCheckAndLogFlightMode(void)
{
// Use != so that we can still detect a change if the counter wraps
if (memcmp(&rcModeActivationMask, &blackboxLastFlightModeFlags, sizeof(blackboxLastFlightModeFlags))) {
if (memcmp(&rcModeActivationMask, &blackboxLastRcModeFlags, sizeof(blackboxLastRcModeFlags))) {
flightLogEvent_flightMode_t eventData; // Add new data for current flight mode flags
eventData.lastFlags = blackboxLastFlightModeFlags;
memcpy(&blackboxLastFlightModeFlags, &rcModeActivationMask, sizeof(blackboxLastFlightModeFlags));
eventData.lastFlags = blackboxLastRcModeFlags;
memcpy(&blackboxLastRcModeFlags, &rcModeActivationMask, sizeof(blackboxLastRcModeFlags));
memcpy(&eventData.flags, &rcModeActivationMask, sizeof(eventData.flags));
blackboxLogEvent(FLIGHT_LOG_EVENT_FLIGHTMODE, (flightLogEventData_t *)&eventData);
}

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

@ -35,6 +35,7 @@ typedef enum {
BLACKBOX_FEATURE_GYRO_PEAKS_ROLL = 1 << 10,
BLACKBOX_FEATURE_GYRO_PEAKS_PITCH = 1 << 11,
BLACKBOX_FEATURE_GYRO_PEAKS_YAW = 1 << 12,
BLACKBOX_FEATURE_SERVOS = 1 << 13,
} blackboxFeatureMask_e;
typedef struct blackboxConfig_s {
uint16_t rate_num;
@ -55,4 +56,4 @@ void blackboxFinish(void);
bool blackboxMayEditConfig(void);
void blackboxIncludeFlagSet(uint32_t mask);
void blackboxIncludeFlagClear(uint32_t mask);
bool blackboxIncludeFlag(uint32_t mask);
bool blackboxIncludeFlag(uint32_t mask);

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

@ -33,7 +33,44 @@ typedef enum FlightLogFieldCondition {
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_6,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_7,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_8,
FLIGHT_LOG_FIELD_CONDITION_SERVOS,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_1,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_2,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_3,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_4,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_5,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_6,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_7,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_8,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_9,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_10,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_11,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_12,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_13,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_14,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_15,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_16,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_17,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_18,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_19,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_20,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_21,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_22,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_23,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_24,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_25,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_26,
/*
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_27,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_28,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_29,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_30,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_31,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_32,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_33,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_34,
*/
FLIGHT_LOG_FIELD_CONDITION_MAG,
FLIGHT_LOG_FIELD_CONDITION_BARO,

75
src/main/blackbox/blackbox_io.c
View file

@ -19,6 +19,11 @@
#include <stdarg.h>
#include <string.h>
#if defined(SITL_BUILD)
#include <stdio.h>
#include <time.h>
#endif
#include "platform.h"
#ifdef USE_BLACKBOX
@ -81,6 +86,12 @@ static struct {
#endif
#if defined(SITL_BUILD)
static struct {
FILE *file_handler;
} blackboxFile;
#endif
#ifndef UNIT_TEST
void blackboxOpen(void)
{
@ -103,6 +114,11 @@ void blackboxWrite(uint8_t value)
case BLACKBOX_DEVICE_SDCARD:
afatfs_fputc(blackboxSDCard.logFile, value);
break;
#endif
#if defined(SITL_BUILD)
case BLACKBOX_DEVICE_FILE:
fputc(value, blackboxFile.file_handler);
break;
#endif
case BLACKBOX_DEVICE_SERIAL:
default:
@ -133,6 +149,13 @@ int blackboxPrint(const char *s)
break;
#endif
#if defined(SITL_BUILD)
case BLACKBOX_DEVICE_FILE:
length = strlen(s);
fputs(s, blackboxFile.file_handler);
break;
#endif
case BLACKBOX_DEVICE_SERIAL:
default:
pos = (uint8_t*) s;
@ -196,6 +219,12 @@ bool blackboxDeviceFlushForce(void)
return afatfs_flush();
#endif
#if defined(SITL_BUILD)
case BLACKBOX_DEVICE_FILE:
fflush(blackboxFile.file_handler);
return true;
#endif
default:
return false;
}
@ -271,6 +300,26 @@ bool blackboxDeviceOpen(void)
return true;
break;
#endif
#if defined(SITL_BUILD)
case BLACKBOX_DEVICE_FILE:
{
const time_t now = time(NULL);
const struct tm *t = localtime(&now);
char filename[32];
strftime(filename, sizeof(filename), "%Y_%m_%d_%H%M%S.TXT", t);
blackboxFile.file_handler = fopen(filename, "wb");
if (blackboxFile.file_handler == NULL) {
fprintf(stderr, "[BlackBox] Failed to create log file\n");
return false;
}
fprintf(stderr, "[BlackBox] Created %s\n", filename);
}
blackboxMaxHeaderBytesPerIteration = BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION;
return true;
break;
#endif
default:
return false;
}
@ -302,6 +351,12 @@ void blackboxDeviceClose(void)
// Some flash device, e.g., NAND devices, require explicit close to flush internally buffered data.
flashfsClose();
break;
#endif
#if defined(SITL_BUILD)
case BLACKBOX_DEVICE_FILE:
fclose(blackboxFile.file_handler);
blackboxFile.file_handler = NULL;
break;
#endif
default:
;
@ -506,6 +561,11 @@ bool isBlackboxDeviceFull(void)
return afatfs_isFull();
#endif
#if defined (SITL_BUILD)
case BLACKBOX_DEVICE_FILE:
return false;
#endif
default:
return false;
}
@ -523,6 +583,10 @@ bool isBlackboxDeviceWorking(void)
#ifdef USE_FLASHFS
case BLACKBOX_DEVICE_FLASH:
return flashfsIsReady();
#endif
#if defined(SITL_BUILD)
case BLACKBOX_DEVICE_FILE:
return blackboxFile.file_handler != NULL;
#endif
default:
return false;
@ -562,6 +626,11 @@ void blackboxReplenishHeaderBudget(void)
case BLACKBOX_DEVICE_SDCARD:
freeSpace = afatfs_getFreeBufferSpace();
break;
#endif
#if defined(SITL_BUILD)
case BLACKBOX_DEVICE_FILE:
freeSpace = BLACKBOX_MAX_ACCUMULATED_HEADER_BUDGET;
break;
#endif
default:
freeSpace = 0;
@ -631,6 +700,12 @@ blackboxBufferReserveStatus_e blackboxDeviceReserveBufferSpace(int32_t bytes)
return BLACKBOX_RESERVE_TEMPORARY_FAILURE;
#endif
#if defined(SITL_BUILD)
case BLACKBOX_DEVICE_FILE:
// Assume that all writes will fit in the file's buffers
return BLACKBOX_RESERVE_TEMPORARY_FAILURE;
#endif
default:
return BLACKBOX_RESERVE_PERMANENT_FAILURE;
}

3
src/main/blackbox/blackbox_io.h
View file

@ -31,6 +31,9 @@ typedef enum BlackboxDevice {
#ifdef USE_SDCARD
BLACKBOX_DEVICE_SDCARD = 2,
#endif
#if defined(SITL_BUILD)
BLACKBOX_DEVICE_FILE = 3,
#endif
BLACKBOX_DEVICE_END
} BlackboxDevice;

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

@ -21,6 +21,8 @@
#include <stdint.h>
#include <stdbool.h>
#include "platform.h"
#define DEBUG32_VALUE_COUNT 8
extern int32_t debug[DEBUG32_VALUE_COUNT];
extern uint8_t debugMode;
@ -72,5 +74,16 @@ typedef enum {
DEBUG_RATE_DYNAMICS,
DEBUG_LANDING,
DEBUG_POS_EST,
DEBUG_COUNT
DEBUG_ADAPTIVE_FILTER,
DEBUG_HEADTRACKING,
DEBUG_GPS,
DEBUG_LULU,
DEBUG_SBUS2,
DEBUG_COUNT // also update debugModeNames in cli.c
} debugType_e;
#ifdef SITL_BUILD
#define SD(X) (X)
#else
#define SD(X)
#endif

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

@ -18,8 +18,12 @@
#define STR_HELPER(x) #x
#define STR(x) STR_HELPER(x)
#define FC_VERSION_STRING STR(FC_VERSION_MAJOR) "." STR(FC_VERSION_MINOR) "." STR(FC_VERSION_PATCH_LEVEL)
#ifndef FC_VERSION_TYPE
#define FC_VERSION_TYPE ""
#endif
#define FC_FIRMWARE_NAME "INAV"
#define MW_VERSION 231
extern const char* const compilerVersion;

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

@ -237,6 +237,7 @@ static const OSD_Entry menuOsdElemsEntries[] =
OSD_ELEMENT_ENTRY("RADAR", OSD_RADAR),
OSD_ELEMENT_ENTRY("MAP SCALE", OSD_MAP_SCALE),
OSD_ELEMENT_ENTRY("MAP REFERENCE", OSD_MAP_REFERENCE),
OSD_ELEMENT_ENTRY("FORMATION FLIGHT", OSD_FORMATION_FLIGHT),
#endif
OSD_ELEMENT_ENTRY("EXPO", OSD_RC_EXPO),
OSD_ELEMENT_ENTRY("YAW EXPO", OSD_RC_YAW_EXPO),

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

@ -23,10 +23,10 @@
#include "platform.h"
#include "common/filter.h"
#include "common/lulu.h"
#include "common/maths.h"
#include "common/utils.h"
#include "common/time.h"
// NULL filter
float nullFilterApply(void *filter, float input)
{
@ -326,6 +326,8 @@ void initFilter(const uint8_t filterType, filter_t *filter, const float cutoffFr
pt2FilterInit(&filter->pt2, pt2FilterGain(cutoffFrequency, dT));
} if (filterType == FILTER_PT3) {
pt3FilterInit(&filter->pt3, pt3FilterGain(cutoffFrequency, dT));
} if (filterType == FILTER_LULU) {
luluFilterInit(&filter->lulu, cutoffFrequency);
} else {
biquadFilterInitLPF(&filter->biquad, cutoffFrequency, refreshRate);
}
@ -341,8 +343,10 @@ void assignFilterApplyFn(uint8_t filterType, float cutoffFrequency, filterApplyF
*applyFn = (filterApplyFnPtr) pt2FilterApply;
} if (filterType == FILTER_PT3) {
*applyFn = (filterApplyFnPtr) pt3FilterApply;
} if (filterType == FILTER_LULU) {
*applyFn = (filterApplyFnPtr) luluFilterApply;
} else {
*applyFn = (filterApplyFnPtr) biquadFilterApply;
}
}
}
}

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

@ -17,6 +17,8 @@
#pragma once
#include "lulu.h"
typedef struct rateLimitFilter_s {
float state;
} rateLimitFilter_t;
@ -50,13 +52,15 @@ typedef union {
pt1Filter_t pt1;
pt2Filter_t pt2;
pt3Filter_t pt3;
luluFilter_t lulu;
} filter_t;
typedef enum {
FILTER_PT1 = 0,
FILTER_BIQUAD,
FILTER_PT2,
FILTER_PT3
FILTER_PT3,
FILTER_LULU
} filterType_e;
typedef enum {
@ -134,4 +138,4 @@ void alphaBetaGammaFilterInit(alphaBetaGammaFilter_t *filter, float alpha, float
float alphaBetaGammaFilterApply(alphaBetaGammaFilter_t *filter, float input);
void initFilter(uint8_t filterType, filter_t *filter, float cutoffFrequency, uint32_t refreshRate);
void assignFilterApplyFn(uint8_t filterType, float cutoffFrequency, filterApplyFnPtr *applyFn);
void assignFilterApplyFn(uint8_t filterType, float cutoffFrequency, filterApplyFnPtr *applyFn);

4
src/main/common/log.c
View file

@ -20,11 +20,12 @@
#include <stdarg.h>
#include <ctype.h>
#if defined(SEMIHOSTING)
#if defined(SEMIHOSTING) || defined(SITL_BUILD)
#include <stdio.h>
#endif
#include "build/version.h"
#include "build/debug.h"
#include "drivers/serial.h"
#include "drivers/time.h"
@ -125,6 +126,7 @@ static void logPrint(const char *buf, size_t size)
fputc(buf[ii], stdout);
}
#endif
SD(printf("%s\n", buf));
if (logPort) {
// Send data via UART (if configured & connected - a safeguard against zombie VCP)
if (serialIsConnected(logPort)) {

179
src/main/common/lulu.c Normal file
View file

@ -0,0 +1,179 @@
#include "lulu.h"
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <math.h>
#include "platform.h"
#include "common/filter.h"
#include "common/maths.h"
#include "common/utils.h"
#ifdef __ARM_ACLE
#include <arm_acle.h>
#endif /* __ARM_ACLE */
#include <fenv.h>
void luluFilterInit(luluFilter_t *filter, int N)
{
filter->N = constrain(N, 1, 15);
filter->windowSize = filter->N * 2 + 1;
filter->windowBufIndex = 0;
memset(filter->luluInterim, 0, sizeof(float) * (filter->windowSize));
memset(filter->luluInterimB, 0, sizeof(float) * (filter->windowSize));
}
FAST_CODE float fixRoad(float *series, float *seriesB, int index, int filterN, int windowSize)
{
float curVal = 0;
float curValB = 0;
for (int N = 1; N <= filterN; N++)
{
int indexNeg = (index + windowSize - 2 * N) % windowSize;
int curIndex = (indexNeg + 1) % windowSize;
float prevVal = series[indexNeg];
float prevValB = seriesB[indexNeg];
int indexPos = (curIndex + N) % windowSize;
for (int i = windowSize - 2 * N; i < windowSize - N; i++)
{
if (indexPos >= windowSize)
{
indexPos = 0;
}
if (curIndex >= windowSize)
{
curIndex = 0;
}
// curIndex = (2 - 1) % 3 = 1
curVal = series[curIndex];
curValB = seriesB[curIndex];
float nextVal = series[indexPos];
float nextValB = seriesB[indexPos];
// onbump (s, 1, 1, 3)
// if(onBump(series, curIndex, N, windowSize))
if (prevVal < curVal && curVal > nextVal)
{
float maxValue = MAX(prevVal, nextVal);
series[curIndex] = maxValue;
int k = curIndex;
for (int j = 1; j < N; j++)
{
if (++k >= windowSize)
{
k = 0;
}
series[k] = maxValue;
}
}
if (prevValB < curValB && curValB > nextValB)
{
float maxValue = MAX(prevValB, nextValB);
curVal = maxValue;
seriesB[curIndex] = maxValue;
int k = curIndex;
for (int j = 1; j < N; j++)
{
if (++k >= windowSize)
{
k = 0;
}
seriesB[k] = maxValue;
}
}
prevVal = curVal;
prevValB = curValB;
curIndex++;
indexPos++;
}
curIndex = (indexNeg + 1) % windowSize;
prevVal = series[indexNeg];
prevValB = seriesB[indexNeg];
indexPos = (curIndex + N) % windowSize;
for (int i = windowSize - 2 * N; i < windowSize - N; i++)
{
if (indexPos >= windowSize)
{
indexPos = 0;
}
if (curIndex >= windowSize)
{
curIndex = 0;
}
// curIndex = (2 - 1) % 3 = 1
curVal = series[curIndex];
curValB = seriesB[curIndex];
float nextVal = series[indexPos];
float nextValB = seriesB[indexPos];
if (prevVal > curVal && curVal < nextVal)
{
float minValue = MIN(prevVal, nextVal);
curVal = minValue;
series[curIndex] = minValue;
int k = curIndex;
for (int j = 1; j < N; j++)
{
if (++k >= windowSize)
{
k = 0;
}
series[k] = minValue;
}
}
if (prevValB > curValB && curValB < nextValB)
{
float minValue = MIN(prevValB, nextValB);
curValB = minValue;
seriesB[curIndex] = minValue;
int k = curIndex;
for (int j = 1; j < N; j++)
{
if (++k >= windowSize)
{
k = 0;
}
seriesB[k] = minValue;
}
}
prevVal = curVal;
prevValB = curValB;
curIndex++;
indexPos++;
}
}
return (curVal - curValB) / 2;
}
FAST_CODE float luluFilterPartialApply(luluFilter_t *filter, float input)
{
// This is the value N of the LULU filter.
int filterN = filter->N;
// This is the total window size for the rolling buffer
int filterWindow = filter->windowSize;
int windowIndex = filter->windowBufIndex;
float inputVal = input;
int newIndex = (windowIndex + 1) % filterWindow;
filter->windowBufIndex = newIndex;
filter->luluInterim[windowIndex] = inputVal;
filter->luluInterimB[windowIndex] = -inputVal;
return fixRoad(filter->luluInterim, filter->luluInterimB, windowIndex, filterN, filterWindow);
}
FAST_CODE float luluFilterApply(luluFilter_t *filter, float input)
{
// This is the UL filter
float resultA = luluFilterPartialApply(filter, input);
// We use the median interpretation of this filter to remove bias in the output
return resultA;
}

14
src/main/common/lulu.h Normal file
View file

@ -0,0 +1,14 @@
#pragma once
// Max N = 15
typedef struct
{
int windowSize;
int windowBufIndex;
int N;
float luluInterim[32] __attribute__((aligned(128)));
float luluInterimB[32];
} luluFilter_t;
void luluFilterInit(luluFilter_t *filter, int N);
float luluFilterApply(luluFilter_t *filter, float input);

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

@ -123,6 +123,15 @@ int32_t wrap_18000(int32_t angle)
return angle;
}
int16_t wrap_180(int16_t angle)
{
if (angle > 180)
angle -= 360;
if (angle < -180)
angle += 360;
return angle;
}
int32_t wrap_36000(int32_t angle)
{
if (angle >= 36000)
@ -516,9 +525,25 @@ bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float resu
return sensorCalibrationValidateResult(result);
}
float gaussian(const float x, const float mu, const float sigma) {
return exp(-pow((double)(x - mu), 2) / (2 * pow((double)sigma, 2)));
}
float bellCurve(const float x, const float curveWidth)
{
return powf(M_Ef, -sq(x) / (2.0f * sq(curveWidth)));
return gaussian(x, 0.0f, curveWidth);
}
/**
* @brief Calculate the attenuation of a value using a Gaussian function.
* Retuns 1 for input 0 and ~0 for input width.
* @param input The input value.
* @param width The width of the Gaussian function.
* @return The attenuation of the input value.
*/
float attenuation(const float input, const float width) {
const float sigma = width / 2.35482f; // Approximately width / sqrt(2 * ln(2))
return gaussian(input, 0.0f, sigma);
}
float fast_fsqrtf(const float value) {

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

@ -73,6 +73,10 @@
#define CMSEC_TO_CENTIKPH(cms) (cms * 3.6f)
#define CMSEC_TO_CENTIKNOTS(cms) (cms * 1.943845f)
#define CMSEC_TO_MPH(cms) (CMSEC_TO_CENTIMPH(cms) / 100.0f)
#define CMSEC_TO_KPH(cms) (CMSEC_TO_CENTIKPH(cms) / 100.0f)
#define CMSEC_TO_KNOTS(cms) (CMSEC_TO_CENTIKNOTS(cms) / 100.0f)
#define C_TO_KELVIN(temp) (temp + 273.15f)
// Standard Sea Level values
@ -166,6 +170,7 @@ int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax);
float scaleRangef(float x, float srcMin, float srcMax, float destMin, float destMax);
int32_t wrap_18000(int32_t angle);
int16_t wrap_180(int16_t angle);
int32_t wrap_36000(int32_t angle);
int32_t quickMedianFilter3(int32_t * v);
@ -195,6 +200,8 @@ 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 attenuation(const float input, const float width);
float gaussian(const float x, const float mu, const float sigma);
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);

1
src/main/common/printf.c
View file

@ -162,6 +162,7 @@ int tfp_nformat(void *putp, int size, void (*putf) (void *, char), const char *f
written += putchw(putp, end, putf, w, lz, bf);
break;
}
case 'i':
case 'd':{
#ifdef REQUIRE_PRINTF_LONG_SUPPORT
if (lng)

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

@ -126,7 +126,10 @@
#define PG_FW_AUTOLAND_CONFIG 1036
#define PG_FW_AUTOLAND_APPROACH_CONFIG 1037
#define PG_OSD_CUSTOM_ELEMENTS_CONFIG 1038
#define PG_INAV_END PG_OSD_CUSTOM_ELEMENTS_CONFIG
#define PG_GIMBAL_CONFIG 1039
#define PG_GIMBAL_SERIAL_CONFIG 1040
#define PG_HEADTRACKER_CONFIG 1041
#define PG_INAV_END PG_HEADTRACKER_CONFIG
// OSD configuration (subject to change)
//#define PG_OSD_FONT_CONFIG 2047

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

@ -214,6 +214,7 @@ bool bmi088AccDetect(accDev_t *acc)
acc->initFn = bmi088AccInit;
acc->readFn = bmi088AccRead;
acc->accAlign = acc->busDev->param;
return true;
}

139
src/main/drivers/gimbal_common.c Normal file
View file

@ -0,0 +1,139 @@
/*
* 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 INAV. If not, see <http://www.gnu.org/licenses/>.
*/
#include "platform.h"
#ifdef USE_SERIAL_GIMBAL
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <build/debug.h>
#include <config/parameter_group_ids.h>
#include "common/time.h"
#include "fc/cli.h"
#include "drivers/gimbal_common.h"
#include "rx/rx.h"
#include "settings_generated.h"
PG_REGISTER_WITH_RESET_TEMPLATE(gimbalConfig_t, gimbalConfig, PG_GIMBAL_CONFIG, 1);
PG_RESET_TEMPLATE(gimbalConfig_t, gimbalConfig,
.panChannel = SETTING_GIMBAL_PAN_CHANNEL_DEFAULT,
.tiltChannel = SETTING_GIMBAL_TILT_CHANNEL_DEFAULT,
.rollChannel = SETTING_GIMBAL_ROLL_CHANNEL_DEFAULT,
.sensitivity = SETTING_GIMBAL_SENSITIVITY_DEFAULT,
.panTrim = SETTING_GIMBAL_PAN_TRIM_DEFAULT,
.tiltTrim = SETTING_GIMBAL_TILT_TRIM_DEFAULT,
.rollTrim = SETTING_GIMBAL_ROLL_TRIM_DEFAULT
);
static gimbalDevice_t *commonGimbalDevice = NULL;
void gimbalCommonInit(void)
{
}
void gimbalCommonSetDevice(gimbalDevice_t *gimbalDevice)
{
SD(fprintf(stderr, "[GIMBAL]: device added %p\n", gimbalDevice));
commonGimbalDevice = gimbalDevice;
}
gimbalDevice_t *gimbalCommonDevice(void)
{
return commonGimbalDevice;
}
void gimbalCommonProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTimeUs)
{
if (gimbalDevice && gimbalDevice->vTable->process && gimbalCommonIsReady(gimbalDevice)) {
gimbalDevice->vTable->process(gimbalDevice, currentTimeUs);
}
}
gimbalDevType_e gimbalCommonGetDeviceType(gimbalDevice_t *gimbalDevice)
{
if (!gimbalDevice || !gimbalDevice->vTable->getDeviceType) {
return GIMBAL_DEV_UNKNOWN;
}
return gimbalDevice->vTable->getDeviceType(gimbalDevice);
}
bool gimbalCommonIsReady(gimbalDevice_t *gimbalDevice)
{
if (gimbalDevice && gimbalDevice->vTable->isReady) {
return gimbalDevice->vTable->isReady(gimbalDevice);
}
return false;
}
#ifdef GIMBAL_UNIT_TEST
void taskUpdateGimbal(timeUs_t currentTimeUs)
{
}
#else
void taskUpdateGimbal(timeUs_t currentTimeUs)
{
if (cliMode) {
return;
}
gimbalDevice_t *gimbalDevice = gimbalCommonDevice();
if(gimbalDevice) {
gimbalCommonProcess(gimbalDevice, currentTimeUs);
}
}
#endif
// TODO: check if any gimbal types are enabled
bool gimbalCommonIsEnabled(void)
{
return true;
}
bool gimbalCommonHtrkIsEnabled(void)
{
const gimbalDevice_t *dev = gimbalCommonDevice();
if(dev && dev->vTable->hasHeadTracker) {
bool ret = dev->vTable->hasHeadTracker(dev);
return ret;
}
return false;
}
int16_t gimbalCommonGetPanPwm(const gimbalDevice_t *gimbalDevice)
{
if (gimbalDevice && gimbalDevice->vTable->getGimbalPanPWM) {
return gimbalDevice->vTable->getGimbalPanPWM(gimbalDevice);
}
return gimbalDevice ? gimbalDevice->currentPanPWM : PWM_RANGE_MIDDLE + gimbalConfig()->panTrim;
}
#endif

104
src/main/drivers/gimbal_common.h Normal file
View file

@ -0,0 +1,104 @@
/*
* 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 INAV. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "platform.h"
#ifdef USE_SERIAL_GIMBAL
#include <stdint.h>
#include "config/feature.h"
#include "common/time.h"
#ifdef __cplusplus
extern "C" {
#endif
typedef enum {
GIMBAL_DEV_UNSUPPORTED = 0,
GIMBAL_DEV_SERIAL,
GIMBAL_DEV_UNKNOWN=0xFF
} gimbalDevType_e;
struct gimbalVTable_s;
typedef struct gimbalDevice_s {
const struct gimbalVTable_s *vTable;
int16_t currentPanPWM;
} gimbalDevice_t;
// {set,get}BandAndChannel: band and channel are 1 origin
// {set,get}PowerByIndex: 0 = Power OFF, 1 = device dependent
// {set,get}PitMode: 0 = OFF, 1 = ON
typedef struct gimbalVTable_s {
void (*process)(gimbalDevice_t *gimbalDevice, timeUs_t currentTimeUs);
gimbalDevType_e (*getDeviceType)(const gimbalDevice_t *gimbalDevice);
bool (*isReady)(const gimbalDevice_t *gimbalDevice);
bool (*hasHeadTracker)(const gimbalDevice_t *gimbalDevice);
int16_t (*getGimbalPanPWM)(const gimbalDevice_t *gimbalDevice);
} gimbalVTable_t;
typedef struct gimbalConfig_s {
uint8_t panChannel;
uint8_t tiltChannel;
uint8_t rollChannel;
uint8_t sensitivity;
uint16_t panTrim;
uint16_t tiltTrim;
uint16_t rollTrim;
} gimbalConfig_t;
PG_DECLARE(gimbalConfig_t, gimbalConfig);
typedef enum {
GIMBAL_MODE_FOLLOW = 0,
GIMBAL_MODE_TILT_LOCK = (1<<0),
GIMBAL_MODE_ROLL_LOCK = (1<<1),
GIMBAL_MODE_PAN_LOCK = (1<<2),
} gimbal_htk_mode_e;
#define GIMBAL_MODE_DEFAULT GIMBAL_MODE_FOLLOW
#define GIMBAL_MODE_TILT_ROLL_LOCK (GIMBAL_MODE_TILT_LOCK | GIMBAL_MODE_ROLL_LOCK)
#define GIMBAL_MODE_PAN_TILT_ROLL_LOCK (GIMBAL_MODE_TILT_LOCK | GIMBAL_MODE_ROLL_LOCK | GIMBAL_MODE_PAN_LOCK)
void gimbalCommonInit(void);
void gimbalCommonSetDevice(gimbalDevice_t *gimbalDevice);
gimbalDevice_t *gimbalCommonDevice(void);
// VTable functions
void gimbalCommonProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTimeUs);
gimbalDevType_e gimbalCommonGetDeviceType(gimbalDevice_t *gimbalDevice);
bool gimbalCommonIsReady(gimbalDevice_t *gimbalDevice);
void taskUpdateGimbal(timeUs_t currentTimeUs);
bool gimbalCommonIsEnabled(void);
bool gimbalCommonHtrkIsEnabled(void);
int16_t gimbalCommonGetPanPwm(const gimbalDevice_t *gimbalDevice);
#ifdef __cplusplus
}
#endif
#endif

193
src/main/drivers/headtracker_common.c Normal file
View file

@ -0,0 +1,193 @@
/*
* 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 INAV. If not, see <http://www.gnu.org/licenses/>.
*/
#include "platform.h"
#ifdef USE_HEADTRACKER
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <build/debug.h>
#include <config/parameter_group_ids.h>
#include "settings_generated.h"
#include "common/time.h"
#include "common/maths.h"
#include "drivers/time.h"
#include "fc/cli.h"
#include "rx/rx.h"
#include "drivers/headtracker_common.h"
PG_REGISTER_WITH_RESET_TEMPLATE(headTrackerConfig_t, headTrackerConfig, PG_HEADTRACKER_CONFIG, 1);
PG_RESET_TEMPLATE(headTrackerConfig_t, headTrackerConfig,
.devType = SETTING_HEADTRACKER_TYPE_DEFAULT,
.pan_ratio = SETTING_HEADTRACKER_PAN_RATIO_DEFAULT,
.tilt_ratio = SETTING_HEADTRACKER_TILT_RATIO_DEFAULT,
.roll_ratio = SETTING_HEADTRACKER_ROLL_RATIO_DEFAULT,
);
static headTrackerDevice_t *commonHeadTrackerDevice = NULL;
void headTrackerCommonInit(void)
{
}
void headTrackerCommonSetDevice(headTrackerDevice_t *headTrackerDevice)
{
SD(fprintf(stderr, "[headTracker]: device added %p\n", headTrackerDevice));
commonHeadTrackerDevice = headTrackerDevice;
}
headTrackerDevice_t *headTrackerCommonDevice(void)
{
return commonHeadTrackerDevice;
}
void headTrackerCommonProcess(headTrackerDevice_t *headTrackerDevice, timeUs_t currentTimeUs)
{
if (headTrackerDevice && headTrackerDevice->vTable->process && headTrackerCommonIsReady(headTrackerDevice)) {
headTrackerDevice->vTable->process(headTrackerDevice, currentTimeUs);
}
}
headTrackerDevType_e headTrackerCommonGetDeviceType(const headTrackerDevice_t *headTrackerDevice)
{
if (!headTrackerDevice || !headTrackerDevice->vTable->getDeviceType) {
return HEADTRACKER_UNKNOWN;
}
return headTrackerDevice->vTable->getDeviceType(headTrackerDevice);
}
bool headTrackerCommonIsReady(const headTrackerDevice_t *headTrackerDevice)
{
if (headTrackerDevice && headTrackerDevice->vTable->isReady) {
return headTrackerDevice->vTable->isReady(headTrackerDevice);
}
return false;
}
int headTrackerCommonGetPan(const headTrackerDevice_t *headTrackerDevice)
{
if(headTrackerDevice && headTrackerDevice->vTable && headTrackerDevice->vTable->getPan) {
return headTrackerDevice->vTable->getPan(headTrackerDevice);
}
return constrain((headTrackerDevice->pan * headTrackerConfig()->pan_ratio) + 0.5f, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX);
}
int headTrackerCommonGetTilt(const headTrackerDevice_t *headTrackerDevice)
{
if(headTrackerDevice && headTrackerDevice->vTable && headTrackerDevice->vTable->getTilt) {
return headTrackerDevice->vTable->getTilt(headTrackerDevice);
}
return constrain((headTrackerDevice->tilt * headTrackerConfig()->tilt_ratio) + 0.5f, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX);
}
int headTrackerCommonGetRoll(const headTrackerDevice_t *headTrackerDevice)
{
if(headTrackerDevice && headTrackerDevice->vTable && headTrackerDevice->vTable->getRoll) {
return headTrackerDevice->vTable->getRollPWM(headTrackerDevice);
}
return constrain((headTrackerDevice->roll * headTrackerConfig()->roll_ratio) + 0.5f, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX);
}
int headTracker2PWM(int value)
{
return constrain(scaleRange(value, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX, PWM_RANGE_MIN, PWM_RANGE_MAX), PWM_RANGE_MIN, PWM_RANGE_MAX);
}
int headTrackerCommonGetPanPWM(const headTrackerDevice_t *headTrackerDevice)
{
if(headTrackerDevice && headTrackerDevice->vTable && headTrackerDevice->vTable->getPanPWM) {
return headTrackerDevice->vTable->getPanPWM(headTrackerDevice);
}
return headTracker2PWM(headTrackerCommonGetPan(headTrackerDevice));
}
int headTrackerCommonGetTiltPWM(const headTrackerDevice_t *headTrackerDevice)
{
if(headTrackerDevice && headTrackerDevice->vTable && headTrackerDevice->vTable->getTiltPWM) {
return headTrackerDevice->vTable->getTiltPWM(headTrackerDevice);
}
return headTracker2PWM(headTrackerCommonGetTilt(headTrackerDevice));
}
int headTrackerCommonGetRollPWM(const headTrackerDevice_t *headTrackerDevice)
{
if(headTrackerDevice && headTrackerDevice->vTable && headTrackerDevice->vTable->getRollPWM) {
return headTrackerDevice->vTable->getRollPWM(headTrackerDevice);
}
return headTracker2PWM(headTrackerCommonGetRoll(headTrackerDevice));
}
#ifdef headTracker_UNIT_TEST
void taskUpdateHeadTracker(timeUs_t currentTimeUs)
{
}
#else
void taskUpdateHeadTracker(timeUs_t currentTimeUs)
{
if (cliMode) {
return;
}
headTrackerDevice_t *headTrackerDevice = headTrackerCommonDevice();
if(headTrackerDevice) {
headTrackerCommonProcess(headTrackerDevice, currentTimeUs);
}
}
// TODO: check if any headTracker types are enabled
bool headTrackerCommonIsEnabled(void)
{
if (commonHeadTrackerDevice && headTrackerCommonIsReady(commonHeadTrackerDevice)) {
return true;
}
return false;
}
bool headTrackerCommonIsValid(const headTrackerDevice_t *dev) {
if(dev && dev->vTable && dev->vTable->isValid) {
return dev->vTable->isValid(dev);
}
return micros() < dev->expires;
}
#endif
#endif

115
src/main/drivers/headtracker_common.h Normal file
View file

@ -0,0 +1,115 @@
/*
* 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 INAV. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "platform.h"
#define MAX_HEADTRACKER_DATA_AGE_US HZ2US(25)
#define HEADTRACKER_RANGE_MIN -2048
#define HEADTRACKER_RANGE_MAX 2047
#ifdef USE_HEADTRACKER
#include <stdint.h>
#include "common/time.h"
#include "drivers/time.h"
#include "config/feature.h"
#ifdef __cplusplus
extern "C" {
#endif
typedef enum {
HEADTRACKER_NONE = 0,
HEADTRACKER_SERIAL = 1,
HEADTRACKER_MSP = 2,
HEADTRACKER_UNKNOWN = 0xFF
} headTrackerDevType_e;
struct headTrackerVTable_s;
typedef struct headTrackerDevice_s {
const struct headTrackerVTable_s *vTable;
int pan;
int tilt;
int roll;
timeUs_t expires;
} headTrackerDevice_t;
// {set,get}BandAndChannel: band and channel are 1 origin
// {set,get}PowerByIndex: 0 = Power OFF, 1 = device dependent
// {set,get}PitMode: 0 = OFF, 1 = ON
typedef struct headTrackerVTable_s {
void (*process)(headTrackerDevice_t *headTrackerDevice, timeUs_t currentTimeUs);
headTrackerDevType_e (*getDeviceType)(const headTrackerDevice_t *headTrackerDevice);
bool (*isReady)(const headTrackerDevice_t *headTrackerDevice);
bool (*isValid)(const headTrackerDevice_t *headTrackerDevice);
int (*getPanPWM)(const headTrackerDevice_t *headTrackerDevice);
int (*getTiltPWM)(const headTrackerDevice_t *headTrackerDevice);
int (*getRollPWM)(const headTrackerDevice_t *headTrackerDevice);
int (*getPan)(const headTrackerDevice_t *headTrackerDevice);
int (*getTilt)(const headTrackerDevice_t *headTrackerDevice);
int (*getRoll)(const headTrackerDevice_t *headTrackerDevice);
} headTrackerVTable_t;
typedef struct headTrackerConfig_s {
headTrackerDevType_e devType;
float pan_ratio;
float tilt_ratio;
float roll_ratio;
} headTrackerConfig_t;
PG_DECLARE(headTrackerConfig_t, headTrackerConfig);
void headTrackerCommonInit(void);
void headTrackerCommonSetDevice(headTrackerDevice_t *headTrackerDevice);
headTrackerDevice_t *headTrackerCommonDevice(void);
// VTable functions
void headTrackerCommonProcess(headTrackerDevice_t *headTrackerDevice, timeUs_t currentTimeUs);
headTrackerDevType_e headTrackerCommonGetDeviceType(const headTrackerDevice_t *headTrackerDevice);
bool headTrackerCommonIsReady(const headTrackerDevice_t *headtrackerDevice);
bool headTrackerCommonIsValid(const headTrackerDevice_t *headtrackerDevice);
// Scaled value, constrained to PWM_RANGE_MIN~PWM_RANGE_MAX
int headTrackerCommonGetPanPWM(const headTrackerDevice_t *headTrackerDevice);
int headTrackerCommonGetTiltPWM(const headTrackerDevice_t *headTrackerDevice);
int headTrackerCommonGetRollPWM(const headTrackerDevice_t *headTrackerDevice);
// Scaled value, constrained to -2048~2047
int headTrackerCommonGetPan(const headTrackerDevice_t *headTrackerDevice);
int headTrackerCommonGetTilt(const headTrackerDevice_t *headTrackerDevice);
int headTrackerCommonGetRoll(const headTrackerDevice_t *headTrackerDevice);
void taskUpdateHeadTracker(timeUs_t currentTimeUs);
bool headtrackerCommonIsEnabled(void);
#ifdef __cplusplus
}
#endif
#endif

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

@ -124,6 +124,10 @@ void ws2811LedStripInit(void)
{
const timerHardware_t * timHw = timerGetByTag(IO_TAG(WS2811_PIN), TIM_USE_ANY);
if (!(timHw->usageFlags & TIM_USE_LED)) { // Check if it has not been reassigned
timHw = timerGetByUsageFlag(TIM_USE_LED); // Get first pin marked as LED
}
if (timHw == NULL) {
return;
}
@ -133,14 +137,14 @@ void ws2811LedStripInit(void)
return;
}
ws2811IO = IOGetByTag(IO_TAG(WS2811_PIN));
ws2811IO = IOGetByTag(timHw->tag); //IOGetByTag(IO_TAG(WS2811_PIN));
IOInit(ws2811IO, OWNER_LED_STRIP, RESOURCE_OUTPUT, 0);
IOConfigGPIOAF(ws2811IO, IOCFG_AF_PP_FAST, timHw->alternateFunction);
if ( ledPinConfig()->led_pin_pwm_mode == LED_PIN_PWM_MODE_LOW ) {
if (ledPinConfig()->led_pin_pwm_mode == LED_PIN_PWM_MODE_LOW) {
ledConfigurePWM();
*timerCCR(ws2811TCH) = 0;
} else if ( ledPinConfig()->led_pin_pwm_mode == LED_PIN_PWM_MODE_HIGH ) {
} else if (ledPinConfig()->led_pin_pwm_mode == LED_PIN_PWM_MODE_HIGH) {
ledConfigurePWM();
*timerCCR(ws2811TCH) = 100;
} else {

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

@ -49,8 +49,8 @@ typedef enum {
VIDEO_SYSTEM_HDZERO,
VIDEO_SYSTEM_DJIWTF,
VIDEO_SYSTEM_AVATAR,
VIDEO_SYSTEM_BFCOMPAT,
VIDEO_SYSTEM_BFCOMPAT_HD
VIDEO_SYSTEM_DJICOMPAT,
VIDEO_SYSTEM_DJICOMPAT_HD
} videoSystem_e;
typedef enum {

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

@ -44,9 +44,9 @@
#define SYM_DBM 0x13 // 019 dBm
#define SYM_SNR 0x14 // 020 SNR
#define SYM_AH_DIRECTION_UP 0x15 // 021 Arrow up AHI
#define SYM_AH_DIRECTION_DOWN 0x16 // 022 Arrow down AHI
#define SYM_DIRECTION 0x17 // 023 to 030, directional little arrows
#define SYM_AH_DECORATION_UP 0x15 // 021 Arrow up AHI
#define SYM_AH_DECORATION_DOWN 0x16 // 022 Arrow down AHI
#define SYM_DECORATION 0x17 // 023 to 030, directional little arrows
#define SYM_VOLT 0x1F // 031 VOLTS
#define SYM_MAH 0x99 // 153 mAh

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

@ -42,18 +42,20 @@
#include "sensors/rangefinder.h"
#include "io/serial.h"
#include "io/servo_sbus.h"
enum {
MAP_TO_NONE,
MAP_TO_MOTOR_OUTPUT,
MAP_TO_SERVO_OUTPUT,
MAP_TO_LED_OUTPUT
};
typedef struct {
int maxTimMotorCount;
int maxTimServoCount;
const timerHardware_t * timMotors[MAX_PWM_OUTPUT_PORTS];
const timerHardware_t * timServos[MAX_PWM_OUTPUT_PORTS];
const timerHardware_t * timMotors[MAX_PWM_OUTPUTS];
const timerHardware_t * timServos[MAX_PWM_OUTPUTS];
} timMotorServoHardware_t;
static pwmInitError_e pwmInitError = PWM_INIT_ERROR_NONE;
@ -167,10 +169,16 @@ static bool checkPwmTimerConflicts(const timerHardware_t *timHw)
#if defined(USE_LED_STRIP)
if (feature(FEATURE_LED_STRIP)) {
const timerHardware_t * ledTimHw = timerGetByTag(IO_TAG(WS2811_PIN), TIM_USE_ANY);
if (ledTimHw != NULL && timHw->tim == ledTimHw->tim) {
return true;
for (int i = 0; i < timerHardwareCount; i++) {
if (timHw->tim == timerHardware[i].tim && timerHardware[i].usageFlags & TIM_USE_LED) {
return true;
}
}
//const timerHardware_t * ledTimHw = timerGetByTag(IO_TAG(WS2811_PIN), TIM_USE_ANY);
//if (ledTimHw != NULL && timHw->tim == ledTimHw->tim) {
// return true;
//}
}
#endif
@ -213,16 +221,16 @@ static bool checkPwmTimerConflicts(const timerHardware_t *timHw)
static void timerHardwareOverride(timerHardware_t * timer) {
switch (timerOverrides(timer2id(timer->tim))->outputMode) {
case OUTPUT_MODE_MOTORS:
if (TIM_IS_SERVO(timer->usageFlags)) {
timer->usageFlags &= ~TIM_USE_SERVO;
timer->usageFlags |= TIM_USE_MOTOR;
}
timer->usageFlags &= ~(TIM_USE_SERVO|TIM_USE_LED);
timer->usageFlags |= TIM_USE_MOTOR;
break;
case OUTPUT_MODE_SERVOS:
if (TIM_IS_MOTOR(timer->usageFlags)) {
timer->usageFlags &= ~TIM_USE_MOTOR;
timer->usageFlags |= TIM_USE_SERVO;
}
timer->usageFlags &= ~(TIM_USE_MOTOR|TIM_USE_LED);
timer->usageFlags |= TIM_USE_SERVO;
break;
case OUTPUT_MODE_LED:
timer->usageFlags &= ~(TIM_USE_MOTOR|TIM_USE_SERVO);
timer->usageFlags |= TIM_USE_LED;
break;
}
}
@ -335,6 +343,8 @@ void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerU
type = MAP_TO_SERVO_OUTPUT;
} else if (TIM_IS_MOTOR(timHw->usageFlags) && !pwmHasServoOnTimer(timOutputs, timHw->tim)) {
type = MAP_TO_MOTOR_OUTPUT;
} else if (TIM_IS_LED(timHw->usageFlags) && !pwmHasMotorOnTimer(timOutputs, timHw->tim) && !pwmHasServoOnTimer(timOutputs, timHw->tim)) {
type = MAP_TO_LED_OUTPUT;
}
switch(type) {
@ -348,6 +358,10 @@ void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerU
timOutputs->timServos[timOutputs->maxTimServoCount++] = timHw;
pwmClaimTimer(timHw->tim, timHw->usageFlags);
break;
case MAP_TO_LED_OUTPUT:
timHw->usageFlags &= TIM_USE_LED;
pwmClaimTimer(timHw->tim, timHw->usageFlags);
break;
default:
break;
}
@ -429,15 +443,21 @@ static void pwmInitServos(timMotorServoHardware_t * timOutputs)
return;
}
// If mixer requests more servos than we have timer outputs - throw an error
if (servoCount > timOutputs->maxTimServoCount) {
uint16_t maxServos = timOutputs->maxTimServoCount;
if (servoConfig()->servo_protocol == SERVO_TYPE_SBUS_PWM) {
maxServos = MAX(SERVO_SBUS_MAX_SERVOS, timOutputs->maxTimServoCount);
}
if (servoCount > maxServos) {
pwmInitError = PWM_INIT_ERROR_NOT_ENOUGH_SERVO_OUTPUTS;
LOG_ERROR(PWM, "Too many servos. Mixer requested %d, timer outputs %d", servoCount, timOutputs->maxTimServoCount);
return;
}
// Configure individual servo outputs
for (int idx = 0; idx < servoCount; idx++) {
for (int idx = 0; idx < MIN(servoCount, timOutputs->maxTimServoCount); idx++) {
const timerHardware_t *timHw = timOutputs->timServos[idx];
if (!pwmServoConfig(timHw, idx, servoConfig()->servoPwmRate, servoConfig()->servoCenterPulse, feature(FEATURE_PWM_OUTPUT_ENABLE))) {

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

@ -21,16 +21,16 @@
#include "flight/mixer.h"
#include "flight/mixer_profile.h"
#include "flight/servos.h"
#include "common/maths.h"
#if defined(TARGET_MOTOR_COUNT)
#define MAX_MOTORS TARGET_MOTOR_COUNT
#define MAX_SERVOS 16
#else
#define MAX_MOTORS 12
#define MAX_SERVOS 16
#endif
#define MAX_SERVOS 18
#define PWM_TIMER_HZ 1000000
#define PULSE_1MS (1000) // 1ms pulse width
@ -53,6 +53,11 @@ typedef enum {
SERVO_TYPE_SBUS_PWM
} servoProtocolType_e;
typedef enum {
PIN_LABEL_NONE = 0,
PIN_LABEL_LED
} pinLabel_e;
typedef enum {
PWM_INIT_ERROR_NONE = 0,
PWM_INIT_ERROR_TOO_MANY_MOTORS,

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

@ -98,7 +98,7 @@ typedef struct {
bool requestTelemetry;
} pwmOutputMotor_t;
static DMA_RAM pwmOutputPort_t pwmOutputPorts[MAX_PWM_OUTPUT_PORTS];
static DMA_RAM pwmOutputPort_t pwmOutputPorts[MAX_PWM_OUTPUTS];
static pwmOutputMotor_t motors[MAX_MOTORS];
static motorPwmProtocolTypes_e initMotorProtocol;
@ -142,7 +142,7 @@ static void pwmOutConfigTimer(pwmOutputPort_t * p, TCH_t * tch, uint32_t hz, uin
static pwmOutputPort_t *pwmOutAllocatePort(void)
{
if (allocatedOutputPortCount >= MAX_PWM_OUTPUT_PORTS) {
if (allocatedOutputPortCount >= MAX_PWM_OUTPUTS) {
LOG_ERROR(PWM, "Attempt to allocate PWM output beyond MAX_PWM_OUTPUT_PORTS");
return NULL;
}
@ -638,7 +638,7 @@ ioTag_t pwmGetMotorPinTag(int motorIndex)
static void pwmServoWriteStandard(uint8_t index, uint16_t value)
{
if (servos[index]) {
if (index < MAX_SERVOS && servos[index]) {
*servos[index]->ccr = value;
}
}

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

@ -20,6 +20,12 @@
#include "drivers/io_types.h"
#include "drivers/time.h"
#if defined(WS2811_PIN)
#define MAX_PWM_OUTPUTS (MAX_PWM_OUTPUT_PORTS + 1)
#else
#define MAX_PWM_OUTPUTS (MAX_PWM_OUTPUT_PORTS)
#endif
typedef enum {
DSHOT_CMD_SPIN_DIRECTION_NORMAL = 20,
DSHOT_CMD_SPIN_DIRECTION_REVERSED = 21,

5
src/main/drivers/serial.c
View file

@ -86,6 +86,11 @@ void serialSetMode(serialPort_t *instance, portMode_t mode)
instance->vTable->setMode(instance, mode);
}
void serialSetOptions(serialPort_t *instance, portOptions_t options)
{
instance->vTable->setOptions(instance, options);
}
void serialWriteBufShim(void *instance, const uint8_t *data, int count)
{
serialWriteBuf((serialPort_t *)instance, data, count);

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

@ -95,6 +95,8 @@ struct serialPortVTable {
void (*setMode)(serialPort_t *instance, portMode_t mode);
void (*setOptions)(serialPort_t *instance, portOptions_t options);
void (*writeBuf)(serialPort_t *instance, const void *data, int count);
bool (*isConnected)(const serialPort_t *instance);
@ -113,6 +115,7 @@ void serialWriteBuf(serialPort_t *instance, const uint8_t *data, int count);
uint8_t serialRead(serialPort_t *instance);
void serialSetBaudRate(serialPort_t *instance, uint32_t baudRate);
void serialSetMode(serialPort_t *instance, portMode_t mode);
void serialSetOptions(serialPort_t *instance, portOptions_t options);
bool isSerialTransmitBufferEmpty(const serialPort_t *instance);
void serialPrint(serialPort_t *instance, const char *str);
uint32_t serialGetBaudRate(serialPort_t *instance);

6
src/main/drivers/serial_softserial.c
View file

@ -623,6 +623,11 @@ void softSerialSetMode(serialPort_t *instance, portMode_t mode)
instance->mode = mode;
}
void softSerialSetOptions(serialPort_t *instance, portOptions_t options)
{
instance->options = options;
}
bool isSoftSerialTransmitBufferEmpty(const serialPort_t *instance)
{
return instance->txBufferHead == instance->txBufferTail;
@ -636,6 +641,7 @@ static const struct serialPortVTable softSerialVTable = {
.serialSetBaudRate = softSerialSetBaudRate,
.isSerialTransmitBufferEmpty = isSoftSerialTransmitBufferEmpty,
.setMode = softSerialSetMode,
.setOptions = softSerialSetOptions,
.isConnected = NULL,
.writeBuf = NULL,
.beginWrite = NULL,

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

@ -317,6 +317,12 @@ void tcpSetMode(serialPort_t *instance, portMode_t mode)
UNUSED(mode);
}
void tcpSetOptions(serialPort_t *instance, portOptions_t options)
{
UNUSED(instance);
UNUSED(options);
}
static const struct serialPortVTable tcpVTable[] = {
{
.serialWrite = tcpWrite,
@ -326,6 +332,7 @@ static const struct serialPortVTable tcpVTable[] = {
.serialSetBaudRate = tcpSetBaudRate,
.isSerialTransmitBufferEmpty = isTcpTransmitBufferEmpty,
.setMode = tcpSetMode,
.setOptions = tcpSetOptions,
.isConnected = tcpIsConnected,
.writeBuf = tcpWritBuf,
.beginWrite = NULL,

8
src/main/drivers/serial_uart.c
View file

@ -175,6 +175,13 @@ void uartSetMode(serialPort_t *instance, portMode_t mode)
uartReconfigure(uartPort);
}
void uartSetOptions(serialPort_t *instance, portOptions_t options)
{
uartPort_t *uartPort = (uartPort_t *)instance;
uartPort->port.options = options;
uartReconfigure(uartPort);
}
uint32_t uartTotalRxBytesWaiting(const serialPort_t *instance)
{
const uartPort_t *s = (const uartPort_t*)instance;
@ -255,6 +262,7 @@ const struct serialPortVTable uartVTable[] = {
.serialSetBaudRate = uartSetBaudRate,
.isSerialTransmitBufferEmpty = isUartTransmitBufferEmpty,
.setMode = uartSetMode,
.setOptions = uartSetOptions,
.isConnected = NULL,
.writeBuf = NULL,
.beginWrite = NULL,

8
src/main/drivers/serial_uart_hal.c
View file

@ -185,6 +185,13 @@ void uartSetMode(serialPort_t *instance, portMode_t mode)
uartReconfigure(uartPort);
}
void uartSetOptions(serialPort_t *instance, portOptions_t options)
{
uartPort_t *uartPort = (uartPort_t *)instance;
uartPort->port.options = options;
uartReconfigure(uartPort);
}
uint32_t uartTotalRxBytesWaiting(const serialPort_t *instance)
{
uartPort_t *s = (uartPort_t*)instance;
@ -266,6 +273,7 @@ const struct serialPortVTable uartVTable[] = {
.serialSetBaudRate = uartSetBaudRate,
.isSerialTransmitBufferEmpty = isUartTransmitBufferEmpty,
.setMode = uartSetMode,
.setOptions = uartSetOptions,
.isConnected = NULL,
.writeBuf = NULL,
.beginWrite = NULL,

8
src/main/drivers/serial_uart_hal_at32f43x.c
View file

@ -178,6 +178,13 @@ void uartSetMode(serialPort_t *instance, portMode_t mode)
uartReconfigure(uartPort);
}
void uartSetOptions(serialPort_t *instance, portOptions_t options)
{
uartPort_t *uartPort = (uartPort_t *)instance;
uartPort->port.options = options;
uartReconfigure(uartPort);
}
uint32_t uartTotalRxBytesWaiting(const serialPort_t *instance)
{
const uartPort_t *s = (const uartPort_t*)instance;
@ -260,6 +267,7 @@ const struct serialPortVTable uartVTable[] = {
.serialSetBaudRate = uartSetBaudRate,
.isSerialTransmitBufferEmpty = isUartTransmitBufferEmpty,
.setMode = uartSetMode,
.setOptions = uartSetOptions,
.isConnected = NULL,
.writeBuf = NULL,
.beginWrite = NULL,

9
src/main/drivers/serial_usb_vcp.c
View file

@ -67,6 +67,14 @@ static void usbVcpSetMode(serialPort_t *instance, portMode_t mode)
// TODO implement
}
static void usbVcpSetOptions(serialPort_t *instance, portOptions_t options)
{
UNUSED(instance);
UNUSED(options);
// TODO implement
}
static bool isUsbVcpTransmitBufferEmpty(const serialPort_t *instance)
{
UNUSED(instance);
@ -184,6 +192,7 @@ static const struct serialPortVTable usbVTable[] = {
.serialSetBaudRate = usbVcpSetBaudRate,
.isSerialTransmitBufferEmpty = isUsbVcpTransmitBufferEmpty,
.setMode = usbVcpSetMode,
.setOptions = usbVcpSetOptions,
.isConnected = usbVcpIsConnected,
.writeBuf = usbVcpWriteBuf,
.beginWrite = usbVcpBeginWrite,

7
src/main/drivers/serial_usb_vcp_at32f43x.c
View file

@ -308,6 +308,12 @@ static void usbVcpSetMode(serialPort_t *instance, portMode_t mode)
UNUSED(mode);
}
static void usbVcpSetOptions(serialPort_t *instance, portOptions_t options)
{
UNUSED(instance);
UNUSED(options);
}
static bool isUsbVcpTransmitBufferEmpty(const serialPort_t *instance)
{
UNUSED(instance);
@ -434,6 +440,7 @@ static const struct serialPortVTable usbVTable[] = {
.serialSetBaudRate = usbVcpSetBaudRate,
.isSerialTransmitBufferEmpty = isUsbVcpTransmitBufferEmpty,
.setMode = usbVcpSetMode,
.setOptions = usbVcpSetOptions,
.isConnected = usbVcpIsConnected,
.writeBuf = usbVcpWriteBuf,
.beginWrite = usbVcpBeginWrite,

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

@ -113,9 +113,9 @@ typedef enum {
TIM_USE_MOTOR = (1 << 2), // Motor output
TIM_USE_SERVO = (1 << 3), // Servo output
TIM_USE_MC_CHNFW = (1 << 4), // Deprecated and not used after removal of CHANNEL_FORWARDING feature
//TIM_USE_FW_MOTOR = (1 << 5), // We no longer differentiate mc from fw on pwm allocation
//TIM_USE_FW_SERVO = (1 << 6),
TIM_USE_LED = (1 << 24),
//TIM_USE_FW_MOTOR = (1 << 5), // We no longer differentiate mc from fw on pwm allocation
//TIM_USE_FW_SERVO = (1 << 6),
TIM_USE_LED = (1 << 24), // Remapping needs to be in the lower 8 bits.
TIM_USE_BEEPER = (1 << 25),
} timerUsageFlag_e;
@ -123,6 +123,7 @@ typedef enum {
#define TIM_IS_MOTOR(flags) ((flags) & TIM_USE_MOTOR)
#define TIM_IS_SERVO(flags) ((flags) & TIM_USE_SERVO)
#define TIM_IS_LED(flags) ((flags) & TIM_USE_LED)
#define TIM_IS_MOTOR_ONLY(flags) (TIM_IS_MOTOR(flags) && !TIM_IS_SERVO(flags))
#define TIM_IS_SERVO_ONLY(flags) (!TIM_IS_MOTOR(flags) && TIM_IS_SERVO(flags))

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

@ -34,7 +34,7 @@
#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) || defined(USE_VTX_MSP)
#define VTX_SETTINGS_POWER_COUNT 5
#define VTX_SETTINGS_POWER_COUNT 8
#define VTX_SETTINGS_DEFAULT_POWER 1
#define VTX_SETTINGS_MIN_POWER 1
#define VTX_SETTINGS_MIN_USER_FREQ 5000

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

@ -104,6 +104,7 @@ bool cliMode = false;
#include "rx/rx.h"
#include "rx/spektrum.h"
#include "rx/srxl2.h"
#include "rx/crsf.h"
#include "scheduler/scheduler.h"
@ -163,6 +164,7 @@ static const char * outputModeNames[] = {
"AUTO",
"MOTORS",
"SERVOS",
"LED",
NULL
};
@ -181,13 +183,46 @@ static const char * const blackboxIncludeFlagNames[] = {
"PEAKS_R",
"PEAKS_P",
"PEAKS_Y",
"SERVOS",
NULL
};
#endif
/* Sensor names (used in lookup tables for *_hardware settings and in status command output) */
static const char *debugModeNames[DEBUG_COUNT] = {
"NONE",
"AGL",
"FLOW_RAW",
"FLOW",
"ALWAYS",
"SAG_COMP_VOLTAGE",
"VIBE",
"CRUISE",
"REM_FLIGHT_TIME",
"SMARTAUDIO",
"ACC",
"NAV_YAW",
"PCF8574",
"DYN_GYRO_LPF",
"AUTOLEVEL",
"ALTITUDE",
"AUTOTRIM",
"AUTOTUNE",
"RATE_DYNAMICS",
"LANDING",
"POS_EST",
"ADAPTIVE_FILTER",
"HEADTRACKER",
"GPS",
"LULU",
"SBUS2"
};
/* Sensor names (used in lookup tables for *_hardware settings and in status
command output) */
// sync with gyroSensor_e
static const char * const gyroNames[] = { "NONE", "AUTO", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "ICM42605", "BMI270","LSM6DXX", "FAKE"};
static const char *const gyroNames[] = {
"NONE", "AUTO", "MPU6000", "MPU6500", "MPU9250", "BMI160",
"ICM20689", "BMI088", "ICM42605", "BMI270", "LSM6DXX", "FAKE"};
// sync this with sensors_e
static const char * const sensorTypeNames[] = {
@ -251,6 +286,7 @@ static void cliPrintLine(const char *str)
cliPrintLinefeed();
}
static void cliPrintError(const char *str)
{
cliPrint("### ERROR: ");
@ -292,7 +328,7 @@ static void cliPutp(void *p, char ch)
typedef enum {
DUMP_MASTER = (1 << 0),
DUMP_PROFILE = (1 << 1),
DUMP_CONTROL_PROFILE = (1 << 1),
DUMP_BATTERY_PROFILE = (1 << 2),
DUMP_MIXER_PROFILE = (1 << 3),
DUMP_ALL = (1 << 4),
@ -878,6 +914,42 @@ static void cliSerial(char *cmdline)
}
#ifdef USE_SERIAL_PASSTHROUGH
portOptions_t constructPortOptions(char *options) {
if (strlen(options) != 3 || options[0] != '8') {
// Invalid format
return -1;
}
portOptions_t result = 0;
switch (options[1]) {
case 'N':
result |= SERIAL_PARITY_NO;
break;
case 'E':
result |= SERIAL_PARITY_EVEN;
break;
default:
// Invalid format
return -1;
}
switch (options[2]) {
case '1':
result |= SERIAL_STOPBITS_1;
break;
case '2':
result |= SERIAL_STOPBITS_2;
break;
default:
// Invalid format
return -1;
}
return result;
}
static void cliSerialPassthrough(char *cmdline)
{
char * saveptr;
@ -890,6 +962,7 @@ static void cliSerialPassthrough(char *cmdline)
int id = -1;
uint32_t baud = 0;
unsigned mode = 0;
portOptions_t options = SERIAL_NOT_INVERTED;
char* tok = strtok_r(cmdline, " ", &saveptr);
int index = 0;
@ -907,6 +980,9 @@ static void cliSerialPassthrough(char *cmdline)
if (strstr(tok, "tx") || strstr(tok, "TX"))
mode |= MODE_TX;
break;
case 3:
options |= constructPortOptions(tok);
break;
}
index++;
tok = strtok_r(NULL, " ", &saveptr);
@ -924,7 +1000,7 @@ static void cliSerialPassthrough(char *cmdline)
passThroughPort = openSerialPort(id, FUNCTION_NONE, NULL, NULL,
baud, mode,
SERIAL_NOT_INVERTED);
options);
if (!passThroughPort) {
tfp_printf("Port %d could not be opened.\r\n", id);
return;
@ -940,6 +1016,11 @@ static void cliSerialPassthrough(char *cmdline)
passThroughPort->mode, mode);
serialSetMode(passThroughPort, mode);
}
if (options && passThroughPort->options != options) {
tfp_printf("Adjusting options from %d to %d.\r\n",
passThroughPort->options, options);
serialSetOptions(passThroughPort, options);
}
// If this port has a rx callback associated we need to remove it now.
// Otherwise no data will be pushed in the serial port buffer!
if (passThroughPort->rxCallback) {
@ -1052,7 +1133,7 @@ static void cliAdjustmentRange(char *cmdline)
}
static void printMotorMix(uint8_t dumpMask, const motorMixer_t *primaryMotorMixer, const motorMixer_t *defaultprimaryMotorMixer)
{
{
const char *format = "mmix %d %s %s %s %s";
char buf0[FTOA_BUFFER_SIZE];
char buf1[FTOA_BUFFER_SIZE];
@ -1316,7 +1397,7 @@ static void cliTempSensor(char *cmdline)
#endif
#ifdef USE_FW_AUTOLAND
static void printFwAutolandApproach(uint8_t dumpMask, const navFwAutolandApproach_t *navFwAutolandApproach, const navFwAutolandApproach_t *defaultFwAutolandApproach)
static void printFwAutolandApproach(uint8_t dumpMask, const navFwAutolandApproach_t *navFwAutolandApproach, const navFwAutolandApproach_t *defaultFwAutolandApproach)
{
const char *format = "fwapproach %u %d %d %u %d %d %u";
for (uint8_t i = 0; i < MAX_FW_LAND_APPOACH_SETTINGS; i++) {
@ -1363,7 +1444,7 @@ static void cliFwAutolandApproach(char * cmdline)
if ((ptr = nextArg(ptr))) {
landDirection = fastA2I(ptr);
if (landDirection != 0 && landDirection != 1) {
cliShowParseError();
return;
@ -1393,7 +1474,7 @@ static void cliFwAutolandApproach(char * cmdline)
validArgumentCount++;
}
if ((ptr = nextArg(ptr))) {
isSeaLevelRef = fastA2I(ptr);
validArgumentCount++;
@ -1807,7 +1888,7 @@ static void cliLedPinPWM(char *cmdline)
if (isEmpty(cmdline)) {
ledPinStopPWM();
cliPrintLine("PWM stopped");
} else {
} else {
i = fastA2I(cmdline);
ledPinStartPWM(i);
cliPrintLinef("PWM started: %d%%",i);
@ -2821,6 +2902,8 @@ static void cliTimerOutputMode(char *cmdline)
mode = OUTPUT_MODE_MOTORS;
} else if(!sl_strcasecmp("SERVOS", tok)) {
mode = OUTPUT_MODE_SERVOS;
} else if(!sl_strcasecmp("LED", tok)) {
mode = OUTPUT_MODE_LED;
} else {
cliShowParseError();
return;
@ -3221,6 +3304,12 @@ void cliRxBind(char *cmdline){
srxl2Bind();
cliPrint("Binding SRXL2 receiver...");
break;
#endif
#if defined(USE_SERIALRX_CRSF)
case SERIALRX_CRSF:
crsfBind();
cliPrint("Binding CRSF receiver...");
break;
#endif
}
}
@ -3333,30 +3422,30 @@ static void cliPlaySound(char *cmdline)
beeper(beeperModeForTableIndex(i));
}
static void cliProfile(char *cmdline)
static void cliControlProfile(char *cmdline)
{
// CLI profile index is 1-based
if (isEmpty(cmdline)) {
cliPrintLinef("profile %d", getConfigProfile() + 1);
cliPrintLinef("control_profile %d", getConfigProfile() + 1);
return;
} else {
const int i = fastA2I(cmdline) - 1;
if (i >= 0 && i < MAX_PROFILE_COUNT) {
setConfigProfileAndWriteEEPROM(i);
cliProfile("");
cliControlProfile("");
}
}
}
static void cliDumpProfile(uint8_t profileIndex, uint8_t dumpMask)
static void cliDumpControlProfile(uint8_t profileIndex, uint8_t dumpMask)
{
if (profileIndex >= MAX_PROFILE_COUNT) {
// Faulty values
return;
}
setConfigProfile(profileIndex);
cliPrintHashLine("profile");
cliPrintLinef("profile %d\r\n", getConfigProfile() + 1);
cliPrintHashLine("control_profile");
cliPrintLinef("control_profile %d\r\n", getConfigProfile() + 1);
dumpAllValues(PROFILE_VALUE, dumpMask);
dumpAllValues(CONTROL_RATE_VALUE, dumpMask);
dumpAllValues(EZ_TUNE_VALUE, dumpMask);
@ -3659,13 +3748,14 @@ static void cliStatus(char *cmdline)
char buf[MAX(FORMATTED_DATE_TIME_BUFSIZE, SETTING_MAX_NAME_LENGTH)];
dateTime_t dt;
cliPrintLinef("%s/%s %s %s / %s (%s)",
cliPrintLinef("%s/%s %s %s / %s (%s) %s",
FC_FIRMWARE_NAME,
targetName,
FC_VERSION_STRING,
buildDate,
buildTime,
shortGitRevision
shortGitRevision,
FC_VERSION_TYPE
);
cliPrintLinef("GCC-%s",
compilerVersion
@ -3695,8 +3785,8 @@ static void cliStatus(char *cmdline)
#if defined(AT32F43x)
cliPrintLine("AT32 system clocks:");
crm_clocks_freq_type clocks;
crm_clocks_freq_get(&clocks);
crm_clocks_freq_get(&clocks);
cliPrintLinef(" SYSCLK = %d MHz", clocks.sclk_freq / 1000000);
cliPrintLinef(" ABH = %d MHz", clocks.ahb_freq / 1000000);
cliPrintLinef(" ABP1 = %d MHz", clocks.apb1_freq / 1000000);
@ -3804,6 +3894,24 @@ static void cliStatus(char *cmdline)
cliPrintErrorLinef("Invalid setting: %s", buf);
}
}
#if defined(USE_OSD)
if (armingFlags & ARMING_DISABLED_NAVIGATION_UNSAFE) {
navArmingBlocker_e reason = navigationIsBlockingArming(NULL);
if (reason & NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR)
cliPrintLinef(" %s", OSD_MSG_JUMP_WP_MISCONFIG);
if (reason & NAV_ARMING_BLOCKER_MISSING_GPS_FIX) {
cliPrintLinef(" %s", OSD_MSG_WAITING_GPS_FIX);
} else {
if (reason & NAV_ARMING_BLOCKER_NAV_IS_ALREADY_ACTIVE)
cliPrintLinef(" %s", OSD_MSG_DISABLE_NAV_FIRST);
if (reason & NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR)
cliPrintLinef(" FIRST WP TOO FAR");
}
}
#endif
#else
cliPrintLinef("Arming disabled flags: 0x%lx", armingFlags & ARMING_DISABLED_ALL_FLAGS);
#endif
@ -3847,10 +3955,17 @@ static void cliStatus(char *cmdline)
cliPrintLinefeed();
#endif
if (featureConfigured(FEATURE_GPS) && (gpsConfig()->provider == GPS_UBLOX || gpsConfig()->provider == GPS_UBLOX7PLUS)) {
if (featureConfigured(FEATURE_GPS) && isGpsUblox()) {
cliPrint("GPS: ");
cliPrintf("HW Version: %s Proto: %d.%02d Baud: %d", getGpsHwVersion(), getGpsProtoMajorVersion(), getGpsProtoMinorVersion(), getGpsBaudrate());
if(ubloxVersionLT(15, 0)) {
cliPrintf(" (UBLOX Proto >= 15.0 required)");
}
cliPrintLinefeed();
cliPrintLinef(" SATS: %i", gpsSol.numSat);
cliPrintLinef(" HDOP: %f", (double)(gpsSol.hdop / (float)HDOP_SCALE));
cliPrintLinef(" EPH : %f m", (double)(gpsSol.eph / 100.0f));
cliPrintLinef(" EPV : %f m", (double)(gpsSol.epv / 100.0f));
//cliPrintLinef(" GNSS Capabilities: %d", gpsUbloxCapLastUpdate());
cliPrintLinef(" GNSS Capabilities:");
cliPrintLine(" GNSS Provider active/default");
@ -3903,13 +4018,14 @@ static void cliVersion(char *cmdline)
{
UNUSED(cmdline);
cliPrintLinef("# %s/%s %s %s / %s (%s)",
cliPrintLinef("# %s/%s %s %s / %s (%s) %s",
FC_FIRMWARE_NAME,
targetName,
FC_VERSION_STRING,
buildDate,
buildTime,
shortGitRevision
shortGitRevision,
FC_VERSION_TYPE
);
cliPrintLinef("# GCC-%s",
compilerVersion
@ -3978,12 +4094,12 @@ static void printConfig(const char *cmdline, bool doDiff)
const char *options;
if ((options = checkCommand(cmdline, "master"))) {
dumpMask = DUMP_MASTER; // only
} else if ((options = checkCommand(cmdline, "profile"))) {
dumpMask = DUMP_PROFILE; // only
} else if ((options = checkCommand(cmdline, "battery_profile"))) {
dumpMask = DUMP_BATTERY_PROFILE; // only
} else if ((options = checkCommand(cmdline, "control_profile"))) {
dumpMask = DUMP_CONTROL_PROFILE; // only
} else if ((options = checkCommand(cmdline, "mixer_profile"))) {
dumpMask = DUMP_MIXER_PROFILE; // only
} else if ((options = checkCommand(cmdline, "battery_profile"))) {
dumpMask = DUMP_BATTERY_PROFILE; // only
} else if ((options = checkCommand(cmdline, "all"))) {
dumpMask = DUMP_ALL; // all profiles and rates
} else {
@ -3994,16 +4110,16 @@ static void printConfig(const char *cmdline, bool doDiff)
dumpMask = dumpMask | DO_DIFF;
}
const int currentProfileIndexSave = getConfigProfile();
const int currentBatteryProfileIndexSave = getConfigBatteryProfile();
const int currentControlProfileIndexSave = getConfigProfile();
const int currentMixerProfileIndexSave = getConfigMixerProfile();
const int currentBatteryProfileIndexSave = getConfigBatteryProfile();
backupConfigs();
// reset all configs to defaults to do differencing
resetConfigs();
// restore the profile indices, since they should not be reset for proper comparison
setConfigProfile(currentProfileIndexSave);
setConfigBatteryProfile(currentBatteryProfileIndexSave);
setConfigProfile(currentControlProfileIndexSave);
setConfigMixerProfile(currentMixerProfileIndexSave);
setConfigBatteryProfile(currentBatteryProfileIndexSave);
if (checkCommand(options, "showdefaults")) {
dumpMask = dumpMask | SHOW_DEFAULTS; // add default values as comments for changed values
@ -4125,25 +4241,25 @@ static void printConfig(const char *cmdline, bool doDiff)
if (dumpMask & DUMP_ALL) {
// dump all profiles
const int currentProfileIndexSave = getConfigProfile();
const int currentBatteryProfileIndexSave = getConfigBatteryProfile();
const int currentControlProfileIndexSave = getConfigProfile();
const int currentMixerProfileIndexSave = getConfigMixerProfile();
const int currentBatteryProfileIndexSave = getConfigBatteryProfile();
for (int ii = 0; ii < MAX_PROFILE_COUNT; ++ii) {
cliDumpControlProfile(ii, dumpMask);
}
for (int ii = 0; ii < MAX_MIXER_PROFILE_COUNT; ++ii) {
cliDumpMixerProfile(ii, dumpMask);
}
for (int ii = 0; ii < MAX_PROFILE_COUNT; ++ii) {
cliDumpProfile(ii, dumpMask);
}
for (int ii = 0; ii < MAX_BATTERY_PROFILE_COUNT; ++ii) {
cliDumpBatteryProfile(ii, dumpMask);
}
setConfigProfile(currentProfileIndexSave);
setConfigBatteryProfile(currentBatteryProfileIndexSave);
setConfigProfile(currentControlProfileIndexSave);
setConfigMixerProfile(currentMixerProfileIndexSave);
setConfigBatteryProfile(currentBatteryProfileIndexSave);
cliPrintHashLine("restore original profile selection");
cliPrintLinef("control_profile %d", currentControlProfileIndexSave + 1);
cliPrintLinef("mixer_profile %d", currentMixerProfileIndexSave + 1);
cliPrintLinef("profile %d", currentProfileIndexSave + 1);
cliPrintLinef("battery_profile %d", currentBatteryProfileIndexSave + 1);
#ifdef USE_CLI_BATCH
@ -4151,18 +4267,19 @@ static void printConfig(const char *cmdline, bool doDiff)
#endif
} else {
// dump just the current profiles
cliDumpControlProfile(getConfigProfile(), dumpMask);
cliDumpMixerProfile(getConfigMixerProfile(), dumpMask);
cliDumpProfile(getConfigProfile(), dumpMask);
cliDumpBatteryProfile(getConfigBatteryProfile(), dumpMask);
}
}
if (dumpMask & DUMP_CONTROL_PROFILE) {
cliDumpControlProfile(getConfigProfile(), dumpMask);
}
if (dumpMask & DUMP_MIXER_PROFILE) {
cliDumpMixerProfile(getConfigMixerProfile(), dumpMask);
}
if (dumpMask & DUMP_PROFILE) {
cliDumpProfile(getConfigProfile(), dumpMask);
}
if (dumpMask & DUMP_BATTERY_PROFILE) {
cliDumpBatteryProfile(getConfigBatteryProfile(), dumpMask);
@ -4246,6 +4363,136 @@ typedef struct {
}
#endif
static void cliCmdDebug(char *arg)
{
UNUSED(arg);
if (debugMode != DEBUG_NONE) {
cliPrintLinef("Debug fields: [%s (%i)]", debugMode < DEBUG_COUNT ? debugModeNames[debugMode] : "unknown", debugMode);
for (int i = 0; i < DEBUG32_VALUE_COUNT; i++) {
cliPrintLinef("debug[%d] = %d", i, debug[i]);
}
} else {
cliPrintLine("Debug mode is disabled");
}
}
#if defined(USE_GPS) && defined(USE_GPS_PROTO_UBLOX)
static const char* _ubloxGetSigId(uint8_t gnssId, uint8_t sigId)
{
if(gnssId == 0) {
switch(sigId) {
case 0: return "GPS L1C/A";
case 3: return "GPS L2 CL";
case 4: return "GPS L2 CM";
case 6: return "GPS L5 I";
case 7: return "GPS L5 Q";
default: return "GPS Unknown";
}
} else if(gnssId == 1) {
switch(sigId) {
case 0: return "SBAS L1C/A";
default: return "SBAS Unknown";
}
} else if(gnssId == 2) {
switch(sigId) {
case 0: return "Galileo E1 C";
case 1: return "Galileo E1 B";
case 3: return "Galileo E5 al";
case 4: return "Galileo E5 aQ";
case 5: return "Galileo E5 bl";
case 6: return "Galileo E5 bQ";
default: return "Galileo Unknown";
}
} else if(gnssId == 3) {
switch(sigId) {
case 0: return "BeiDou B1I D1";
case 1: return "BeiDou B1I D2";
case 2: return "BeiDou B2I D1";
case 3: return "BeiDou B2I D2";
case 5: return "BeiDou B1C";
case 7: return "BeiDou B2a";
default: return "BeiDou Unknown";
}
} else if(gnssId == 5) {
switch(sigId) {
case 0: return "QZSS L1C/A";
case 1: return "QZSS L1S";
case 4: return "QZSS L2 CM";
case 5: return "QZSS L2 CL";
case 8: return "QZSS L5 I";
case 9: return "QZSS L5 Q";
default: return "QZSS Unknown";
}
} else if(gnssId == 6) {
switch(sigId) {
case 0: return "GLONASS L1 OF";
case 2: return "GLONASS L2 OF";
default: return "GLONASS Unknown";
}
}
return "Unknown GNSS/SigId";
}
static const char *_ubloxGetQuality(uint8_t quality)
{
switch(quality) {
case UBLOX_SIG_QUALITY_NOSIGNAL: return "No signal";
case UBLOX_SIG_QUALITY_SEARCHING: return "Searching signal...";
case UBLOX_SIG_QUALITY_ACQUIRED: return "Signal acquired";
case UBLOX_SIG_QUALITY_UNUSABLE: return "Signal detected but unusable";
case UBLOX_SIG_QUALITY_CODE_LOCK_TIME_SYNC: return "Code locked and time sync";
case UBLOX_SIG_QUALITY_CODE_CARRIER_LOCK_TIME_SYNC:
case UBLOX_SIG_QUALITY_CODE_CARRIER_LOCK_TIME_SYNC2:
case UBLOX_SIG_QUALITY_CODE_CARRIER_LOCK_TIME_SYNC3:
return "Code and carrier locked and time sync";
default: return "Unknown";
}
}
static void cliUbloxPrintSatelites(char *arg)
{
UNUSED(arg);
if(!isGpsUblox() /*|| !(gpsState.flags.sig || gpsState.flags.sat)*/) {
cliPrint("GPS is not UBLOX or does not report satelites.");
return;
}
cliPrintLine("UBLOX Satelites");
for(int i = 0; i < UBLOX_MAX_SIGNALS; ++i)
{
const ubx_nav_sig_info *sat = gpsGetUbloxSatelite(i);
if(sat == NULL) {
continue;
}
cliPrintLinef("satelite[%d]: %d:%d", i+1, sat->gnssId, sat->svId);
cliPrintLinef("sigId: %d (%s)", sat->sigId, _ubloxGetSigId(sat->gnssId, sat->sigId));
cliPrintLinef("signal strength: %i dbHz", sat->cno);
cliPrintLinef("quality: %i (%s)", sat->quality, _ubloxGetQuality(sat->quality));
//cliPrintLinef("Correlation: %i", sat->corrSource);
//cliPrintLinef("Iono model: %i", sat->ionoModel);
cliPrintLinef("signal flags: 0x%02X", sat->sigFlags);
switch(sat->sigFlags & UBLOX_SIG_HEALTH_MASK) {
case UBLOX_SIG_HEALTH_HEALTHY:
cliPrintLine("signal: Healthy");
break;
case UBLOX_SIG_HEALTH_UNHEALTHY:
cliPrintLine("signal: Unhealthy");
break;
case UBLOX_SIG_HEALTH_UNKNOWN:
default:
cliPrintLinef("signal: Unknown (0x%X)", sat->sigFlags & UBLOX_SIG_HEALTH_MASK);
break;
}
cliPrintLinefeed();
}
}
#endif
static void cliHelp(char *cmdline);
// should be sorted a..z for bsearch()
@ -4276,9 +4523,9 @@ const clicmd_t cmdTable[] = {
CLI_COMMAND_DEF("defaults", "reset to defaults and reboot", NULL, cliDefaults),
CLI_COMMAND_DEF("dfu", "DFU mode on reboot", NULL, cliDfu),
CLI_COMMAND_DEF("diff", "list configuration changes from default",
"[master|battery_profile|profile|rates|all] {showdefaults}", cliDiff),
"[master|battery_profile|control_profile|mixer_profile|rates|all] {showdefaults}", cliDiff),
CLI_COMMAND_DEF("dump", "dump configuration",
"[master|battery_profile|profile|rates|all] {showdefaults}", cliDump),
"[master|battery_profile|control_profile|mixer_profile|rates|all] {showdefaults}", cliDump),
#ifdef USE_RX_ELERES
CLI_COMMAND_DEF("eleres_bind", NULL, NULL, cliEleresBind),
#endif // USE_RX_ELERES
@ -4305,6 +4552,7 @@ const clicmd_t cmdTable[] = {
CLI_COMMAND_DEF("get", "get variable value", "[name]", cliGet),
#ifdef USE_GPS
CLI_COMMAND_DEF("gpspassthrough", "passthrough gps to serial", NULL, cliGpsPassthrough),
CLI_COMMAND_DEF("gpssats", "show GPS satellites", NULL, cliUbloxPrintSatelites),
#endif
CLI_COMMAND_DEF("help", NULL, NULL, cliHelp),
#ifdef USE_LED_STRIP
@ -4319,12 +4567,9 @@ const clicmd_t cmdTable[] = {
CLI_COMMAND_DEF("msc", "switch into msc mode", NULL, cliMsc),
#endif
CLI_COMMAND_DEF("play_sound", NULL, "[<index>]\r\n", cliPlaySound),
CLI_COMMAND_DEF("profile", "change profile",
"[<index>]", cliProfile),
CLI_COMMAND_DEF("battery_profile", "change battery profile",
"[<index>]", cliBatteryProfile),
CLI_COMMAND_DEF("mixer_profile", "change mixer profile",
"[<index>]", cliMixerProfile),
CLI_COMMAND_DEF("control_profile", "change control profile", "[<index>]", cliControlProfile),
CLI_COMMAND_DEF("mixer_profile", "change mixer profile", "[<index>]", cliMixerProfile),
CLI_COMMAND_DEF("battery_profile", "change battery profile", "[<index>]", cliBatteryProfile),
CLI_COMMAND_DEF("resource", "view currently used resources", NULL, cliResource),
CLI_COMMAND_DEF("rxrange", "configure rx channel ranges", NULL, cliRxRange),
#if defined(USE_SAFE_HOME)
@ -4333,7 +4578,7 @@ const clicmd_t cmdTable[] = {
CLI_COMMAND_DEF("save", "save and reboot", NULL, cliSave),
CLI_COMMAND_DEF("serial", "configure serial ports", NULL, cliSerial),
#ifdef USE_SERIAL_PASSTHROUGH
CLI_COMMAND_DEF("serialpassthrough", "passthrough serial data to port", "<id> [baud] [mode] : passthrough to serial", cliSerialPassthrough),
CLI_COMMAND_DEF("serialpassthrough", "passthrough serial data to port", "<id> [baud] [mode] [options]: passthrough to serial", cliSerialPassthrough),
#endif
CLI_COMMAND_DEF("servo", "configure servos", NULL, cliServo),
#ifdef USE_PROGRAMMING_FRAMEWORK
@ -4360,6 +4605,7 @@ const clicmd_t cmdTable[] = {
#ifdef USE_SDCARD
CLI_COMMAND_DEF("sd_info", "sdcard info", NULL, cliSdInfo),
#endif
CLI_COMMAND_DEF("showdebug", "Show debug fields.", NULL, cliCmdDebug),
CLI_COMMAND_DEF("status", "show status", NULL, cliStatus),
CLI_COMMAND_DEF("tasks", "show task stats", NULL, cliTasks),
#ifdef USE_TEMPERATURE_SENSOR

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

@ -190,6 +190,18 @@ uint32_t getGyroLooptime(void)
void validateAndFixConfig(void)
{
#ifdef USE_ADAPTIVE_FILTER
// gyroConfig()->adaptiveFilterMinHz has to be at least 5 units lower than gyroConfig()->gyro_main_lpf_hz
if (gyroConfig()->adaptiveFilterMinHz + 5 > gyroConfig()->gyro_main_lpf_hz) {
gyroConfigMutable()->adaptiveFilterMinHz = gyroConfig()->gyro_main_lpf_hz - 5;
}
//gyroConfig()->adaptiveFilterMaxHz has to be at least 5 units higher than gyroConfig()->gyro_main_lpf_hz
if (gyroConfig()->adaptiveFilterMaxHz - 5 < gyroConfig()->gyro_main_lpf_hz) {
gyroConfigMutable()->adaptiveFilterMaxHz = gyroConfig()->gyro_main_lpf_hz + 5;
}
#endif
if (accelerometerConfig()->acc_notch_cutoff >= accelerometerConfig()->acc_notch_hz) {
accelerometerConfigMutable()->acc_notch_hz = 0;
}
@ -284,6 +296,14 @@ void createDefaultConfig(void)
featureSet(FEATURE_AIRMODE);
targetConfiguration();
#ifdef MSP_UART
int port = findSerialPortIndexByIdentifier(MSP_UART);
if (port) {
serialConfigMutable()->portConfigs[port].functionMask = FUNCTION_MSP;
serialConfigMutable()->portConfigs[port].msp_baudrateIndex = BAUD_115200;
}
#endif
}
void resetConfigs(void)

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

@ -704,7 +704,7 @@ void processRx(timeUs_t currentTimeUs)
}
/* Turn assistant mode */
if (IS_RC_MODE_ACTIVE(BOXTURNASSIST)) {
if (IS_RC_MODE_ACTIVE(BOXTURNASSIST) || navigationRequiresTurnAssistance()) {
ENABLE_FLIGHT_MODE(TURN_ASSISTANT);
} else {
DISABLE_FLIGHT_MODE(TURN_ASSISTANT);
@ -880,7 +880,9 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
cycleTime = getTaskDeltaTime(TASK_SELF);
dT = (float)cycleTime * 0.000001f;
if (ARMING_FLAG(ARMED) && (!STATE(FIXED_WING_LEGACY) || !isNavLaunchEnabled() || (isNavLaunchEnabled() && fixedWingLaunchStatus() >= FW_LAUNCH_DETECTED))) {
bool fwLaunchIsActive = STATE(AIRPLANE) && isNavLaunchEnabled() && armTime == 0;
if (ARMING_FLAG(ARMED) && (!STATE(AIRPLANE) || !fwLaunchIsActive || fixedWingLaunchStatus() >= FW_LAUNCH_DETECTED)) {
flightTime += cycleTime;
armTime += cycleTime;
updateAccExtremes();
@ -900,7 +902,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
}
#if defined(SITL_BUILD)
if (lockMainPID()) {
if (ARMING_FLAG(SIMULATOR_MODE_HITL) || lockMainPID()) {
#endif
gyroFilter();

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

@ -53,6 +53,8 @@
#include "drivers/exti.h"
#include "drivers/io.h"
#include "drivers/flash.h"
#include "drivers/gimbal_common.h"
#include "drivers/headtracker_common.h"
#include "drivers/light_led.h"
#include "drivers/nvic.h"
#include "drivers/osd.h"
@ -107,6 +109,8 @@
#include "io/displayport_msp_osd.h"
#include "io/displayport_srxl.h"
#include "io/flashfs.h"
#include "io/gimbal_serial.h"
#include "io/headtracker_msp.h"
#include "io/gps.h"
#include "io/ledstrip.h"
#include "io/osd.h"
@ -687,6 +691,23 @@ void init(void)
initDShotCommands();
#endif
#ifdef USE_SERIAL_GIMBAL
gimbalCommonInit();
// Needs to be called before gimbalSerialHeadTrackerInit
gimbalSerialInit();
#endif
#ifdef USE_HEADTRACKER
headTrackerCommonInit();
#ifdef USE_HEADTRACKER_SERIAL
// Needs to be called after gimbalSerialInit
gimbalSerialHeadTrackerInit();
#endif
#ifdef USE_HEADTRACKER_MSP
mspHeadTrackerInit();
#endif
#endif
// Latch active features AGAIN since some may be modified by init().
latchActiveFeatures();
motorControlEnable = true;

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

@ -18,6 +18,7 @@
#include <ctype.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
@ -87,6 +88,7 @@
#include "io/asyncfatfs/asyncfatfs.h"
#include "io/flashfs.h"
#include "io/gps.h"
#include "io/gps_ublox.h"
#include "io/opflow.h"
#include "io/rangefinder.h"
#include "io/ledstrip.h"
@ -96,6 +98,7 @@
#include "io/vtx.h"
#include "io/vtx_string.h"
#include "io/gps_private.h" //for MSP_SIMULATOR
#include "io/headtracker_msp.h"
#include "io/osd/custom_elements.h"
@ -109,6 +112,8 @@
#include "rx/rx.h"
#include "rx/msp.h"
#include "rx/srxl2.h"
#include "rx/crsf.h"
#include "scheduler/scheduler.h"
@ -856,7 +861,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU32(dst, currentBatteryProfile->capacity.value);
sbufWriteU32(dst, currentBatteryProfile->capacity.warning);
sbufWriteU32(dst, currentBatteryProfile->capacity.critical);
sbufWriteU8(dst, currentBatteryProfile->capacity.unit);
sbufWriteU8(dst, batteryMetersConfig()->capacity_unit);
break;
case MSP2_INAV_MISC2:
@ -895,7 +900,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU32(dst, currentBatteryProfile->capacity.value);
sbufWriteU32(dst, currentBatteryProfile->capacity.warning);
sbufWriteU32(dst, currentBatteryProfile->capacity.critical);
sbufWriteU8(dst, currentBatteryProfile->capacity.unit);
sbufWriteU8(dst, batteryMetersConfig()->capacity_unit);
break;
#ifdef USE_GPS
@ -1029,6 +1034,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
case MSP_MIXER:
sbufWriteU8(dst, 3); // mixerMode no longer supported, send 3 (QuadX) as fallback
break;
case MSP_RX_CONFIG:
sbufWriteU8(dst, rxConfig()->serialrx_provider);
@ -1575,6 +1581,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
}
break;
// Obsolete, replaced by MSP2_INAV_OUTPUT_MAPPING_EXT2
case MSP2_INAV_OUTPUT_MAPPING_EXT:
for (uint8_t i = 0; i < timerHardwareCount; ++i)
if (!(timerHardware[i].usageFlags & (TIM_USE_PPM | TIM_USE_PWM))) {
@ -1583,9 +1590,35 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
#else
sbufWriteU8(dst, timer2id(timerHardware[i].tim));
#endif
// usageFlags is u32, cuts out the higher 24bits
sbufWriteU8(dst, timerHardware[i].usageFlags);
}
break;
case MSP2_INAV_OUTPUT_MAPPING_EXT2:
{
#if !defined(SITL_BUILD) && defined(WS2811_PIN)
ioTag_t led_tag = IO_TAG(WS2811_PIN);
#endif
for (uint8_t i = 0; i < timerHardwareCount; ++i)
if (!(timerHardware[i].usageFlags & (TIM_USE_PPM | TIM_USE_PWM))) {
#if defined(SITL_BUILD)
sbufWriteU8(dst, i);
#else
sbufWriteU8(dst, timer2id(timerHardware[i].tim));
#endif
sbufWriteU32(dst, timerHardware[i].usageFlags);
#if defined(SITL_BUILD) || !defined(WS2811_PIN)
sbufWriteU8(dst, 0);
#else
// Extra label to help identify repurposed PINs.
// Eventually, we can try to add more labels for PPM pins, etc.
sbufWriteU8(dst, timerHardware[i].tag == led_tag ? PIN_LABEL_LED : PIN_LABEL_NONE);
#endif
}
}
break;
case MSP2_INAV_MC_BRAKING:
#ifdef USE_MR_BRAKING_MODE
@ -2054,13 +2087,13 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
currentBatteryProfileMutable->capacity.value = sbufReadU32(src);
currentBatteryProfileMutable->capacity.warning = sbufReadU32(src);
currentBatteryProfileMutable->capacity.critical = sbufReadU32(src);
currentBatteryProfileMutable->capacity.unit = sbufReadU8(src);
batteryMetersConfigMutable()->capacity_unit = sbufReadU8(src);
if ((batteryMetersConfig()->voltageSource != BAT_VOLTAGE_RAW) && (batteryMetersConfig()->voltageSource != BAT_VOLTAGE_SAG_COMP)) {
batteryMetersConfigMutable()->voltageSource = BAT_VOLTAGE_RAW;
return MSP_RESULT_ERROR;
}
if ((currentBatteryProfile->capacity.unit != BAT_CAPACITY_UNIT_MAH) && (currentBatteryProfile->capacity.unit != BAT_CAPACITY_UNIT_MWH)) {
currentBatteryProfileMutable->capacity.unit = BAT_CAPACITY_UNIT_MAH;
if ((batteryMetersConfig()->capacity_unit != BAT_CAPACITY_UNIT_MAH) && (batteryMetersConfig()->capacity_unit != BAT_CAPACITY_UNIT_MWH)) {
batteryMetersConfigMutable()->capacity_unit = BAT_CAPACITY_UNIT_MAH;
return MSP_RESULT_ERROR;
}
} else
@ -2093,13 +2126,13 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
currentBatteryProfileMutable->capacity.value = sbufReadU32(src);
currentBatteryProfileMutable->capacity.warning = sbufReadU32(src);
currentBatteryProfileMutable->capacity.critical = sbufReadU32(src);
currentBatteryProfileMutable->capacity.unit = sbufReadU8(src);
batteryMetersConfigMutable()->capacity_unit = sbufReadU8(src);
if ((batteryMetersConfig()->voltageSource != BAT_VOLTAGE_RAW) && (batteryMetersConfig()->voltageSource != BAT_VOLTAGE_SAG_COMP)) {
batteryMetersConfigMutable()->voltageSource = BAT_VOLTAGE_RAW;
return MSP_RESULT_ERROR;
}
if ((currentBatteryProfile->capacity.unit != BAT_CAPACITY_UNIT_MAH) && (currentBatteryProfile->capacity.unit != BAT_CAPACITY_UNIT_MWH)) {
currentBatteryProfileMutable->capacity.unit = BAT_CAPACITY_UNIT_MAH;
if ((batteryMetersConfig()->capacity_unit != BAT_CAPACITY_UNIT_MAH) && (batteryMetersConfig()->capacity_unit != BAT_CAPACITY_UNIT_MWH)) {
batteryMetersConfigMutable()->capacity_unit = BAT_CAPACITY_UNIT_MAH;
return MSP_RESULT_ERROR;
}
} else
@ -2859,7 +2892,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
} else
return MSP_RESULT_ERROR;
break;
case MSP_SET_FAILSAFE_CONFIG:
if (dataSize == 20) {
failsafeConfigMutable()->failsafe_delay = sbufReadU8(src);
@ -3256,6 +3289,15 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
}
break;
#endif
case MSP2_INAV_GPS_UBLOX_COMMAND:
if(dataSize < 8 || !isGpsUblox()) {
SD(fprintf(stderr, "[GPS] Not ublox!\n"));
return MSP_RESULT_ERROR;
}
SD(fprintf(stderr, "[GPS] Sending ubx command: %i!\n", dataSize));
gpsUbloxSendCommand(src->ptr, dataSize, 0);
break;
#ifdef USE_EZ_TUNE
@ -3324,6 +3366,26 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
break;
case MSP2_BETAFLIGHT_BIND:
if (rxConfig()->receiverType == RX_TYPE_SERIAL) {
switch (rxConfig()->serialrx_provider) {
default:
return MSP_RESULT_ERROR;
#if defined(USE_SERIALRX_SRXL2)
case SERIALRX_SRXL2:
srxl2Bind();
break;
#endif
#if defined(USE_SERIALRX_CRSF)
case SERIALRX_CRSF:
crsfBind();
break;
#endif
}
} else {
return MSP_RESULT_ERROR;
}
break;
default:
return MSP_RESULT_ERROR;
@ -4021,7 +4083,8 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
static mspResult_e mspProcessSensorCommand(uint16_t cmdMSP, sbuf_t *src)
{
UNUSED(src);
int dataSize = sbufBytesRemaining(src);
UNUSED(dataSize);
switch (cmdMSP) {
#if defined(USE_RANGEFINDER_MSP)
@ -4059,6 +4122,12 @@ static mspResult_e mspProcessSensorCommand(uint16_t cmdMSP, sbuf_t *src)
mspPitotmeterReceiveNewData(sbufPtr(src));
break;
#endif
#if (defined(USE_HEADTRACKER) && defined(USE_HEADTRACKER_MSP))
case MSP2_SENSOR_HEADTRACKER:
mspHeadTrackerReceiverNewData(sbufPtr(src), dataSize);
break;
#endif
}
return MSP_RESULT_NO_REPLY;
@ -4076,6 +4145,7 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro
// initialize reply by default
reply->cmd = cmd->cmd;
SD(fprintf(stderr, "[MSP] CommandId: 0x%04x bytes: %i!\n", cmdMSP, sbufBytesRemaining(src)));
if (MSP2_IS_SENSOR_MESSAGE(cmdMSP)) {
ret = mspProcessSensorCommand(cmdMSP, src);
} else if (mspFcProcessOutCommand(cmdMSP, dst, mspPostProcessFn)) {

37
src/main/fc/fc_msp_box.c
View file

@ -43,6 +43,9 @@
#include "telemetry/telemetry.h"
#include "drivers/gimbal_common.h"
#include "drivers/headtracker_common.h"
#define BOX_SUFFIX ';'
#define BOX_SUFFIX_LEN 1
@ -102,6 +105,10 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ .boxId = BOXMIXERPROFILE, .boxName = "MIXER PROFILE 2", .permanentId = 62 },
{ .boxId = BOXMIXERTRANSITION, .boxName = "MIXER TRANSITION", .permanentId = 63 },
{ .boxId = BOXANGLEHOLD, .boxName = "ANGLE HOLD", .permanentId = 64 },
{ .boxId = BOXGIMBALTLOCK, .boxName = "GIMBAL LEVEL TILT", .permanentId = 65 },
{ .boxId = BOXGIMBALRLOCK, .boxName = "GIMBAL LEVEL ROLL", .permanentId = 66 },
{ .boxId = BOXGIMBALCENTER, .boxName = "GIMBAL CENTER", .permanentId = 67 },
{ .boxId = BOXGIMBALHTRK, .boxName = "GIMBAL HEADTRACKER", .permanentId = 68 },
{ .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL, .permanentId = 0xFF }
};
@ -215,7 +222,7 @@ void initActiveBoxIds(void)
bool navReadyAltControl = getHwBarometerStatus() != HW_SENSOR_NONE;
#ifdef USE_GPS
navReadyAltControl = navReadyAltControl || (feature(FEATURE_GPS) && (STATE(AIRPLANE) || positionEstimationConfig()->use_gps_no_baro));
navReadyAltControl = navReadyAltControl || feature(FEATURE_GPS);
const bool navFlowDeadReckoning = sensors(SENSOR_OPFLOW) && sensors(SENSOR_ACC) && positionEstimationConfig()->allow_dead_reckoning;
bool navReadyPosControl = sensors(SENSOR_ACC) && feature(FEATURE_GPS);
@ -358,6 +365,19 @@ void initActiveBoxIds(void)
ADD_ACTIVE_BOX(BOXMIXERPROFILE);
ADD_ACTIVE_BOX(BOXMIXERTRANSITION);
#endif
#ifdef USE_SERIAL_GIMBAL
if (gimbalCommonIsEnabled()) {
ADD_ACTIVE_BOX(BOXGIMBALTLOCK);
ADD_ACTIVE_BOX(BOXGIMBALRLOCK);
ADD_ACTIVE_BOX(BOXGIMBALCENTER);
}
#endif
#ifdef USE_HEADTRACKER
if(headTrackerConfig()->devType != HEADTRACKER_NONE) {
ADD_ACTIVE_BOX(BOXGIMBALHTRK);
}
#endif
}
#define IS_ENABLED(mask) ((mask) == 0 ? 0 : 1)
@ -431,6 +451,21 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION)), BOXMIXERTRANSITION);
#endif
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXANGLEHOLD)), BOXANGLEHOLD);
#ifdef USE_SERIAL_GIMBAL
if(IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)) {
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)), BOXGIMBALCENTER);
#ifdef USE_HEADTRACKER
} else if (headTrackerCommonIsReady(headTrackerCommonDevice()) && IS_RC_MODE_ACTIVE(BOXGIMBALHTRK)) {
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALHTRK)), BOXGIMBALHTRK);
#endif
} else {
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALTLOCK) && !IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)), BOXGIMBALTLOCK);
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALRLOCK) && !IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)), BOXGIMBALRLOCK);
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALHTRK) && !IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)), BOXGIMBALRLOCK);
}
#endif
memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t));
for (uint32_t i = 0; i < activeBoxIdCount; i++) {
if (activeBoxes[activeBoxIds[i]]) {

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

@ -34,6 +34,8 @@
#include "drivers/serial.h"
#include "drivers/stack_check.h"
#include "drivers/pwm_mapping.h"
#include "drivers/gimbal_common.h"
#include "drivers/headtracker_common.h"
#include "fc/cli.h"
#include "fc/config.h"
@ -51,6 +53,7 @@
#include "flight/rpm_filter.h"
#include "flight/servos.h"
#include "flight/wind_estimator.h"
#include "flight/adaptive_filter.h"
#include "navigation/navigation.h"
@ -90,6 +93,7 @@
#include "sensors/opflow.h"
#include "telemetry/telemetry.h"
#include "telemetry/sbus2.h"
#include "config/feature.h"
@ -426,6 +430,26 @@ void fcTasksInit(void)
setTaskEnabled(TASK_SMARTPORT_MASTER, true);
#endif
#ifdef USE_SERIAL_GIMBAL
setTaskEnabled(TASK_GIMBAL, true);
#endif
#ifdef USE_HEADTRACKER
setTaskEnabled(TASK_HEADTRACKER, true);
#endif
#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SBUS2)
setTaskEnabled(TASK_TELEMETRY_SBUS2,feature(FEATURE_TELEMETRY) && rxConfig()->receiverType == RX_TYPE_SERIAL && rxConfig()->serialrx_provider == SERIALRX_SBUS2);
#endif
#ifdef USE_ADAPTIVE_FILTER
setTaskEnabled(TASK_ADAPTIVE_FILTER, (
gyroConfig()->gyroFilterMode == GYRO_FILTER_MODE_ADAPTIVE &&
gyroConfig()->adaptiveFilterMinHz > 0 &&
gyroConfig()->adaptiveFilterMaxHz > 0
));
#endif
#if defined(SITL_BUILD)
serialProxyStart();
#endif
@ -680,4 +704,40 @@ cfTask_t cfTasks[TASK_COUNT] = {
.desiredPeriod = TASK_PERIOD_HZ(TASK_AUX_RATE_HZ), // 100Hz @10ms
.staticPriority = TASK_PRIORITY_HIGH,
},
#ifdef USE_ADAPTIVE_FILTER
[TASK_ADAPTIVE_FILTER] = {
.taskName = "ADAPTIVE_FILTER",
.taskFunc = adaptiveFilterTask,
.desiredPeriod = TASK_PERIOD_HZ(ADAPTIVE_FILTER_RATE_HZ), // 100Hz @10ms
.staticPriority = TASK_PRIORITY_LOW,
},
#endif
#ifdef USE_SERIAL_GIMBAL
[TASK_GIMBAL] = {
.taskName = "GIMBAL",
.taskFunc = taskUpdateGimbal,
.desiredPeriod = TASK_PERIOD_HZ(50),
.staticPriority = TASK_PRIORITY_MEDIUM,
},
#endif
#ifdef USE_HEADTRACKER
[TASK_HEADTRACKER] = {
.taskName = "HEADTRACKER",
.taskFunc = taskUpdateHeadTracker,
.desiredPeriod = TASK_PERIOD_HZ(50),
.staticPriority = TASK_PRIORITY_MEDIUM,
},
#endif
#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SBUS2)
[TASK_TELEMETRY_SBUS2] = {
.taskName = "SBUS2 TLM",
.taskFunc = taskSendSbus2Telemetry,
.desiredPeriod = TASK_PERIOD_US(125), // 8kHz 2ms dead time + 650us window / sensor.
.staticPriority = TASK_PRIORITY_LOW, // timing is critical. Ideally, should be a timer interrupt triggered by sbus packet
},
#endif
};

44
src/main/fc/rc_controls.h
View file

@ -26,18 +26,38 @@ typedef enum rc_alias {
PITCH,
YAW,
THROTTLE,
AUX1,
AUX2,
AUX3,
AUX4,
AUX5,
AUX6,
AUX7,
AUX8,
AUX9,
AUX10,
AUX11,
AUX12
AUX1, // 5
AUX2, // 6
AUX3, // 7
AUX4, // 8
AUX5, // 9
AUX6, // 10
AUX7, // 11
AUX8, // 12
AUX9, // 13
AUX10, // 14
AUX11, // 15
AUX12, // 16
AUX13, // 17
AUX14, // 18
#ifdef USE_34CHANNELS
AUX15, // 19
AUX16, // 20
AUX17, // 21
AUX18, // 22
AUX19, // 23
AUX20, // 24
AUX21, // 25
AUX22, // 26
AUX23, // 27
AUX24, // 28
AUX25, // 29
AUX26, // 30
AUX27, // 31
AUX28, // 32
AUX29, // 33
AUX30, // 34
#endif
} rc_alias_e;
typedef enum {

16
src/main/fc/rc_modes.c
View file

@ -114,7 +114,6 @@ void processAirmode(void) {
} else if (STATE(MULTIROTOR)) {
processAirmodeMultirotor();
}
}
bool isUsingNavigationModes(void)
@ -122,6 +121,21 @@ bool isUsingNavigationModes(void)
return isUsingNAVModes;
}
bool isFwAutoModeActive(boxId_e mode)
{
/* Sets activation priority of fixed wing auto tune/trim modes: Autotune -> Autotrim -> Autolevel */
if (mode == BOXAUTOTUNE) {
return IS_RC_MODE_ACTIVE(BOXAUTOTUNE);
} else if (mode == BOXAUTOTRIM) {
return IS_RC_MODE_ACTIVE(BOXAUTOTRIM) && !IS_RC_MODE_ACTIVE(BOXAUTOTUNE);
} else if (mode == BOXAUTOLEVEL) {
return IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) && !IS_RC_MODE_ACTIVE(BOXAUTOTUNE) && !IS_RC_MODE_ACTIVE(BOXAUTOTRIM);
}
return false;
}
bool IS_RC_MODE_ACTIVE(boxId_e boxId)
{
return bitArrayGet(rcModeActivationMask.bits, boxId);

9
src/main/fc/rc_modes.h
View file

@ -78,9 +78,13 @@ typedef enum {
BOXCHANGEMISSION = 50,
BOXBEEPERMUTE = 51,
BOXMULTIFUNCTION = 52,
BOXMIXERPROFILE = 53,
BOXMIXERTRANSITION = 54,
BOXMIXERPROFILE = 53,
BOXMIXERTRANSITION = 54,
BOXANGLEHOLD = 55,
BOXGIMBALTLOCK = 56,
BOXGIMBALRLOCK = 57,
BOXGIMBALCENTER = 58,
BOXGIMBALHTRK = 59,
CHECKBOX_ITEM_COUNT
} boxId_e;
@ -140,3 +144,4 @@ bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range);
void updateActivatedModes(void);
void updateUsedModeActivationConditionFlags(void);
bool isFwAutoModeActive(boxId_e mode);

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

@ -23,9 +23,9 @@ tables:
values: ["NONE", "SERIAL", "MSP", "SIM (SITL)"]
enum: rxReceiverType_e
- name: serial_rx
values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "SBUS_FAST", "FPORT2", "SRXL2", "GHST", "MAVLINK", "FBUS"]
values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "SBUS_FAST", "FPORT2", "SRXL2", "GHST", "MAVLINK", "FBUS", "SBUS2"]
- name: blackbox_device
values: ["SERIAL", "SPIFLASH", "SDCARD"]
values: ["SERIAL", "SPIFLASH", "SDCARD", "FILE"]
- name: motor_pwm_protocol
values: ["STANDARD", "ONESHOT125", "MULTISHOT", "BRUSHED", "DSHOT150", "DSHOT300", "DSHOT600"]
- name: servo_protocol
@ -42,13 +42,13 @@ tables:
values: ["VELNED", "TURNRATE","ADAPTIVE"]
enum: imu_inertia_comp_method_e
- name: gps_provider
values: ["UBLOX", "UBLOX7", "MSP", "FAKE"]
values: ["UBLOX", "MSP", "FAKE"]
enum: gpsProvider_e
- name: gps_sbas_mode
values: ["AUTO", "EGNOS", "WAAS", "MSAS", "GAGAN", "SPAN", "NONE"]
enum: sbasMode_e
- name: gps_dyn_model
values: ["PEDESTRIAN","AUTOMOTIVE", "AIR_1G", "AIR_2G", "AIR_4G"]
values: ["PEDESTRIAN","AUTOMOTIVE", "AIR_1G", "AIR_2G", "AIR_4G", "SEA", "MOWER"]
enum: gpsDynModel_e
- name: reset_type
values: ["NEVER", "FIRST_ARM", "EACH_ARM"]
@ -83,7 +83,8 @@ tables:
values: ["NONE", "AGL", "FLOW_RAW", "FLOW", "ALWAYS", "SAG_COMP_VOLTAGE",
"VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC",
"NAV_YAW", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "ALTITUDE",
"AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST"]
"AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST",
"ADAPTIVE_FILTER", "HEADTRACKER", "GPS", "LULU", "SBUS2"]
- name: aux_operator
values: ["OR", "AND"]
enum: modeActivationOperator_e
@ -119,7 +120,7 @@ tables:
- name: filter_type
values: ["PT1", "BIQUAD"]
- name: filter_type_full
values: ["PT1", "BIQUAD", "PT2", "PT3"]
values: ["PT1", "BIQUAD", "PT2", "PT3", "LULU"]
- name: log_level
values: ["ERROR", "WARNING", "INFO", "VERBOSE", "DEBUG"]
- name: iterm_relax
@ -191,6 +192,12 @@ tables:
- name: led_pin_pwm_mode
values: ["SHARED_LOW", "SHARED_HIGH", "LOW", "HIGH"]
enum: led_pin_pwm_mode_e
- name: gyro_filter_mode
values: ["OFF", "STATIC", "DYNAMIC", "ADAPTIVE"]
enum: gyroFilterType_e
- name: headtracker_dev_type
values: ["NONE", "SERIAL", "MSP"]
enum: headTrackerDevType_e
constants:
RPYL_PID_MIN: 0
@ -213,24 +220,35 @@ groups:
members:
- name: looptime
description: "This is the main loop time (in us). Changing this affects PID effect with some PID controllers (see PID section for details). A very conservative value of 3500us/285Hz should work for everyone. Setting it to zero does not limit loop time, so it will go as fast as possible."
default_value: 1000
default_value: 500
max: 9000
- name: gyro_anti_aliasing_lpf_hz
description: "Gyro processing anti-aliasing filter cutoff frequency. In normal operation this filter setting should never be changed. In Hz"
default_value: 250
field: gyro_anti_aliasing_lpf_hz
max: 1000
- name: gyro_lulu_enabled
description: "Enable/disable gyro LULU filter"
default_value: OFF
field: gyroLuluEnabled
type: bool
- name: gyro_lulu_sample_count
description: "Gyro lulu sample count, in number of samples."
default_value: 3
field: gyroLuluSampleCount
min: 1
max: 15
- name: gyro_main_lpf_hz
description: "Software based gyro main lowpass filter. Value is cutoff frequency (Hz)"
default_value: 60
field: gyro_main_lpf_hz
min: 0
max: 500
- name: gyro_use_dyn_lpf
description: "Use Dynamic LPF instead of static gyro stage1 LPF. Dynamic Gyro LPF updates gyro LPF based on the throttle position."
default_value: OFF
field: useDynamicLpf
type: bool
- name: gyro_filter_mode
description: "Specifies the type of the software LPF of the gyro signals."
default_value: "STATIC"
field: gyroFilterMode
table: gyro_filter_mode
- name: gyro_dyn_lpf_min_hz
description: "Minimum frequency of the gyro Dynamic LPF"
default_value: 200
@ -330,6 +348,55 @@ groups:
field: gravity_cmss_cal
min: 0
max: 2000
- name: gyro_adaptive_filter_target
description: "Target value for adaptive filter"
default_value: 3.5
field: adaptiveFilterTarget
min: 1
max: 6
condition: USE_ADAPTIVE_FILTER
- name: gyro_adaptive_filter_min_hz
description: "Minimum frequency for adaptive filter"
default_value: 50
field: adaptiveFilterMinHz
min: 0
max: 250
condition: USE_ADAPTIVE_FILTER
- name: gyro_adaptive_filter_max_hz
description: "Maximum frequency for adaptive filter"
default_value: 150
field: adaptiveFilterMaxHz
min: 0
max: 505
condition: USE_ADAPTIVE_FILTER
- name: gyro_adaptive_filter_std_lpf_hz
description: "Standard deviation low pass filter cutoff frequency"
default_value: 2
field: adaptiveFilterStdLpfHz
min: 0
max: 10
condition: USE_ADAPTIVE_FILTER
- name: gyro_adaptive_filter_hpf_hz
description: "High pass filter cutoff frequency"
default_value: 10
field: adaptiveFilterHpfHz
min: 1
max: 50
condition: USE_ADAPTIVE_FILTER
- name: gyro_adaptive_filter_integrator_threshold_high
description: "High threshold for adaptive filter integrator"
default_value: 4
field: adaptiveFilterIntegratorThresholdHigh
min: 1
max: 10
condition: USE_ADAPTIVE_FILTER
- name: gyro_adaptive_filter_integrator_threshold_low
description: "Low threshold for adaptive filter integrator"
default_value: -2
field: adaptiveFilterIntegratorThresholdLow
min: -10
max: 0
condition: USE_ADAPTIVE_FILTER
- name: PG_ADC_CHANNEL_CONFIG
type: adcChannelConfig_t
@ -890,6 +957,12 @@ groups:
condition: USE_ADC
min: 0
max: 65535
- name: battery_capacity_unit
description: "Unit used for `battery_capacity`, `battery_capacity_warning` and `battery_capacity_critical` [MAH/MWH] (milliAmpere hour / milliWatt hour)."
default_value: "MAH"
field: capacity_unit
table: bat_capacity_unit
type: uint8_t
- name: current_meter_scale
description: "This sets the output voltage to current scaling for the current sensor in 0.1 mV/A steps. 400 is 40mV/A such as the ACS756 sensor outputs. 183 is the setting for the uberdistro with a 0.25mOhm shunt."
default_value: :target
@ -996,12 +1069,6 @@ groups:
field: capacity.critical
min: 0
max: 4294967295
- name: battery_capacity_unit
description: "Unit used for `battery_capacity`, `battery_capacity_warning` and `battery_capacity_critical` [MAH/MWH] (milliAmpere hour / milliWatt hour)."
default_value: "MAH"
field: capacity.unit
table: bat_capacity_unit
type: uint8_t
- name: controlrate_profile
description: "Control rate profile to switch to when the battery profile is selected, 0 to disable and keep the currently selected control rate profile"
default_value: 0
@ -1599,7 +1666,7 @@ groups:
condition: USE_GPS
members:
- name: gps_provider
description: "Which GPS protocol to be used, note that UBLOX is 5Hz and UBLOX7 is 10Hz (M8N)."
description: "Which GPS protocol to be used."
default_value: "UBLOX"
field: provider
table: gps_provider
@ -1654,7 +1721,7 @@ groups:
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."
description: "Navigation update rate for UBLOX 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
@ -1891,13 +1958,13 @@ groups:
max: RPYL_PID_MAX
- name: max_angle_inclination_rll
description: "Maximum inclination in level (angle) mode (ROLL axis). 100=10°"
default_value: 300
default_value: 450
field: max_angle_inclination[FD_ROLL]
min: 100
max: 900
- name: max_angle_inclination_pit
description: "Maximum inclination in level (angle) mode (PITCH axis). 100=10°"
default_value: 300
default_value: 450
field: max_angle_inclination[FD_PITCH]
min: 100
max: 900
@ -1934,12 +2001,6 @@ groups:
field: fixedWingCoordinatedPitchGain
min: 0
max: 2
- name: fw_iterm_limit_stick_position
description: "Iterm is not allowed to grow when stick position is above threshold. This solves the problem of bounceback or followthrough when full stick deflection is applied on poorely tuned fixed wings. In other words, stabilization is partialy disabled when pilot is actively controlling the aircraft and active when sticks are not touched. `0` mean stick is in center position, `1` means it is fully deflected to either side"
default_value: 0.5
field: fixedWingItermLimitOnStickPosition
min: 0
max: 1
- name: fw_yaw_iterm_freeze_bank_angle
description: "Yaw Iterm is frozen when bank angle is above this threshold [degrees]. This solves the problem of the rudder counteracting turns by partially disabling yaw stabilization when making banked turns. Setting to 0 (the default) disables this feature. Only applies when autopilot is not active and TURN ASSIST is disabled."
default_value: 0
@ -2233,6 +2294,24 @@ groups:
field: fixedWingLevelTrimGain
min: 0
max: 20
- name: fw_iterm_lock_time_max_ms
description: Defines max time in milliseconds for how long ITerm Lock will shut down Iterm after sticks are release
default_value: 500
field: fwItermLockTimeMaxMs
min: 100
max: 1000
- name: fw_iterm_lock_rate_threshold
description: Defines rate percentage when full P I and D attenuation should happen. 100 disables Iterm Lock for P and D term
field: fwItermLockRateLimit
default_value: 40
min: 10
max: 100
- name: fw_iterm_lock_engage_threshold
description: Defines error rate (in percents of max rate) when Iterm Lock is engaged when sticks are release. Iterm Lock will stay active until error drops below this number
default_value: 10
min: 5
max: 25
field: fwItermLockEngageThreshold
- name: PG_PID_AUTOTUNE_CONFIG
type: pidAutotuneConfig_t
@ -2271,11 +2350,6 @@ groups:
field: gravity_calibration_tolerance
min: 0
max: 255
- name: inav_use_gps_no_baro
description: "Defines if INAV should use only use GPS data for altitude estimation when barometer is not available. If set to ON, INAV will allow GPS assisted modes and RTH even when there is no barometer installed."
field: use_gps_no_baro
type: bool
default_value: ON
- name: inav_allow_dead_reckoning
description: "Defines if INAV will dead-reckon over short GPS outages. May also be useful for indoors OPFLOW navigation"
default_value: OFF
@ -2404,10 +2478,16 @@ groups:
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."
description: "Allows immediate landing detection based on G bump at touchdown when set to ON. Requires a barometer and GPS and currently only works for multirotors (Note: will work during Failsafe without need for a GPS)."
default_value: OFF
field: general.flags.landing_bump_detection
type: bool
- name: nav_mc_inverted_crash_detection
description: "Setting a value > 0 enables inverted crash detection for multirotors. It will auto disarm in situations where the multirotor has crashed inverted on the ground and can't be manually disarmed due to loss of control or for some other reason. When enabled this setting defines the additional number of seconds before disarm beyond a minimum fixed time delay of 3s. Requires a barometer to work."
default_value: 0
field: mc.inverted_crash_detection
min: 0
max: 15
- name: nav_mc_althold_throttle
description: "If set to STICK the FC remembers the throttle stick position when enabling ALTHOLD and treats it as the neutral midpoint for holding altitude. If set to MID_STICK or HOVER the neutral midpoint is set to the mid stick position or the hover throttle position respectively."
default_value: "STICK"
@ -2483,7 +2563,7 @@ groups:
table: nav_fw_wp_turn_smoothing
- name: nav_auto_speed
description: "Speed in fully autonomous modes (RTH, WP) [cm/s]. Used for WP mode when no specific WP speed set. [Multirotor only]"
default_value: 300
default_value: 500
field: general.auto_speed
min: 10
max: 2000
@ -2501,7 +2581,7 @@ groups:
max: 2000
- name: nav_manual_speed
description: "Maximum speed allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only]"
default_value: 500
default_value: 750
field: general.max_manual_speed
min: 10
max: 2000
@ -2677,7 +2757,7 @@ groups:
max: 120
- name: nav_mc_bank_angle
description: "Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude"
default_value: 30
default_value: 35
field: mc.max_bank_angle
min: 15
max: 45
@ -2859,7 +2939,7 @@ groups:
description: "Forward acceleration threshold for bungee launch or throw launch [cm/s/s], 1G = 981 cm/s/s"
default_value: 1863
field: fw.launch_accel_thresh
min: 1500
min: 1350
max: 20000
- name: nav_fw_launch_max_angle
description: "Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg]"
@ -3075,7 +3155,7 @@ groups:
type: uint8_t
min: 0
max: 255
default_value: 5
default_value: 1
- name: mavlink_pos_rate
description: "Rate of the position message for MAVLink telemetry"
field: mavlink.position_rate
@ -3089,7 +3169,7 @@ groups:
type: uint8_t
min: 0
max: 255
default_value: 10
default_value: 2
- name: mavlink_extra2_rate
description: "Rate of the extra2 message for MAVLink telemetry"
field: mavlink.extra2_rate
@ -3418,6 +3498,12 @@ groups:
min: 1
max: 10
default_value: 3
- name: osd_radar_peers_display_time
description: "Time in seconds to display next peer "
field: radar_peers_display_time
min: 1
max: 10
default_value: 3
- name: osd_hud_wp_disp
description: "How many navigation waypoints are displayed, set to 0 (zero) to disable. As sample, if set to 2, and you just passed the 3rd waypoint of the mission, you'll see markers for the 4th waypoint (marked 1) and the 5th waypoint (marked 2)"
default_value: 0
@ -3453,7 +3539,6 @@ groups:
min: 8
max: 11
default_value: 9
- name: osd_adsb_distance_warning
description: "Distance in meters of ADSB aircraft that is displayed"
default_value: 20000
@ -3478,7 +3563,6 @@ groups:
min: 0
max: 64000
type: uint16_t
- name: osd_estimations_wind_compensation
description: "Use wind estimation for remaining flight time/distance estimation"
default_value: ON
@ -3491,12 +3575,10 @@ groups:
condition: USE_WIND_ESTIMATOR
field: estimations_wind_mps
type: bool
- name: osd_failsafe_switch_layout
description: "If enabled the OSD automatically switches to the first layout during failsafe"
default_value: OFF
type: bool
- name: osd_plus_code_digits
description: "Numer of plus code digits before shortening with `osd_plus_code_short`. Precision at the equator: 10=13.9x13.9m; 11=2.8x3.5m; 12=56x87cm; 13=11x22cm."
field: plus_code_digits
@ -3508,213 +3590,186 @@ groups:
field: plus_code_short
default_value: "0"
table: osd_plus_code_short
- name: osd_ahi_style
description: "Sets OSD Artificial Horizon style \"DEFAULT\" or \"LINE\" for the FrSky Graphical OSD."
field: ahi_style
default_value: "DEFAULT"
table: osd_ahi_style
type: uint8_t
- name: osd_force_grid
field: force_grid
type: bool
default_value: OFF
description: Force OSD to work in grid mode even if the OSD device supports pixel level access (mainly used for development)
- name: osd_ahi_bordered
field: ahi_bordered
type: bool
description: Shows a border/corners around the AHI region (pixel OSD only)
default_value: OFF
- name: osd_ahi_width
field: ahi_width
max: 255
description: AHI width in pixels (pixel OSD only)
default_value: 132
- name: osd_ahi_height
field: ahi_height
max: 255
description: AHI height in pixels (pixel OSD only)
default_value: 162
- name: osd_ahi_vertical_offset
field: ahi_vertical_offset
min: -128
max: 127
description: AHI vertical offset from center (pixel OSD only)
default_value: -18
- name: osd_sidebar_horizontal_offset
field: sidebar_horizontal_offset
min: -128
max: 127
default_value: 0
description: Sidebar horizontal offset from default position. Positive values move the sidebars closer to the edges.
- name: osd_left_sidebar_scroll_step
field: left_sidebar_scroll_step
max: 255
default_value: 0
description: How many units each sidebar step represents. 0 means the default value for the scroll type.
- name: osd_right_sidebar_scroll_step
field: right_sidebar_scroll_step
max: 255
default_value: 0
description: Same as left_sidebar_scroll_step, but for the right sidebar
- name: osd_sidebar_height
field: sidebar_height
min: 0
max: 5
default_value: 3
description: Height of sidebars in rows. 0 leaves only the level indicator arrows (Not for pixel OSD)
- name: osd_ahi_pitch_interval
field: ahi_pitch_interval
min: 0
max: 30
default_value: 0
description: Draws AHI at increments of the set pitch interval over the full pitch range. AHI line is drawn with ends offset when pitch first exceeds interval with offset increasing with increasing pitch. Offset direction changes between climb and dive. Set to 0 to disable (Not for pixel OSD)
- name: osd_home_position_arm_screen
type: bool
default_value: ON
description: Should home position coordinates be displayed on the arming screen.
- name: osd_pan_servo_index
description: Index of the pan servo, used to adjust osd home heading direction based on camera pan. Note that this feature does not work with continiously rotating servos.
field: pan_servo_index
min: 0
max: 16
default_value: 0
- name: osd_pan_servo_pwm2centideg
description: Centidegrees of pan servo rotation us PWM signal. A servo with 180 degrees of rotation from 1000 to 2000 us PWM typically needs `18` for this setting. Change sign to inverse direction.
field: pan_servo_pwm2centideg
default_value: 0
min: -36
max: 36
- name: osd_pan_servo_offcentre_warning
description: Degrees either side of the pan servo centre; where it is assumed camera is wanted to be facing forwards, but isn't at 0. If in this range and not 0 for longer than 10 seconds, the pan servo offset OSD element will blink. 0 means the warning is disabled.
field: pan_servo_offcentre_warning
min: 0
max: 45
default_value: 10
- name: osd_pan_servo_indicator_show_degrees
description: Show the degress of offset from centre on the pan servo OSD display element.
field: pan_servo_indicator_show_degrees
type: bool
default_value: OFF
- name: osd_esc_rpm_precision
description: Number of characters used to display the RPM value.
field: esc_rpm_precision
min: 3
max: 6
default_value: 3
- name: osd_mah_precision
description: Number of digits used for mAh precision. Currently used by mAh Used and Battery Remaining Capacity
field: mAh_precision
min: 4
max: 6
default_value: 4
- name: osd_use_pilot_logo
description: Use custom pilot logo with/instead of the INAV logo. The pilot logo must be characters 473 to 511
field: use_pilot_logo
type: bool
default_value: OFF
- name: osd_inav_to_pilot_logo_spacing
description: The space between the INAV and pilot logos, if `osd_use_pilot_logo` is `ON`. This number may be adjusted so that it fits the odd/even col width displays. For example, if using an odd column width display, such as Walksnail, and this is set to 4. 1 will be added so that the logos are equally spaced from the centre of the screen.
field: inav_to_pilot_logo_spacing
min: 0
max: 20
default_value: 8
- name: osd_arm_screen_display_time
description: Amount of time to display the arm screen [ms]
field: arm_screen_display_time
min: 1000
max: 5000
default_value: 1500
- name: osd_switch_indicator_zero_name
description: "Character to use for OSD switch incicator 0."
field: osd_switch_indicator0_name
type: string
max: 5
default_value: "FLAP"
- name: osd_switch_indicator_one_name
description: "Character to use for OSD switch incicator 1."
field: osd_switch_indicator1_name
type: string
max: 5
default_value: "GEAR"
- name: osd_switch_indicator_two_name
description: "Character to use for OSD switch incicator 2."
field: osd_switch_indicator2_name
type: string
max: 5
default_value: "CAM"
- name: osd_switch_indicator_three_name
description: "Character to use for OSD switch incicator 3."
field: osd_switch_indicator3_name
type: string
max: 5
default_value: "LIGT"
- name: osd_switch_indicator_zero_channel
description: "RC Channel to use for OSD switch indicator 0."
field: osd_switch_indicator0_channel
min: 5
max: MAX_SUPPORTED_RC_CHANNEL_COUNT
default_value: 5
- name: osd_switch_indicator_one_channel
description: "RC Channel to use for OSD switch indicator 1."
field: osd_switch_indicator1_channel
min: 5
max: MAX_SUPPORTED_RC_CHANNEL_COUNT
default_value: 5
- name: osd_switch_indicator_two_channel
description: "RC Channel to use for OSD switch indicator 2."
field: osd_switch_indicator2_channel
min: 5
max: MAX_SUPPORTED_RC_CHANNEL_COUNT
default_value: 5
- name: osd_switch_indicator_three_channel
description: "RC Channel to use for OSD switch indicator 3."
field: osd_switch_indicator3_channel
min: 5
max: MAX_SUPPORTED_RC_CHANNEL_COUNT
default_value: 5
- name: osd_switch_indicators_align_left
description: "Align text to left of switch indicators"
field: osd_switch_indicators_align_left
type: bool
default_value: ON
- name: osd_system_msg_display_time
description: System message display cycle time for multiple messages (milliseconds).
field: system_msg_display_time
default_value: 1000
min: 500
max: 5000
- name: osd_highlight_djis_missing_font_symbols
description: Show question marks where there is no symbol in the DJI font to represent the INAV OSD element's symbol. When off, blank spaces will be used. Only relevent for DJICOMPAT modes.
field: highlight_djis_missing_characters
default_value: ON
type: bool
- name: PG_OSD_COMMON_CONFIG
type: osdCommonConfig_t
headers: ["io/osd_common.h"]
@ -4170,3 +4225,92 @@ groups:
field: maxTailwind
min: 0
max: 3000
- name: PG_GIMBAL_CONFIG
type: gimbalConfig_t
headers: ["drivers/gimbal_common.h"]
condition: USE_SERIAL_GIMBAL
members:
- name: gimbal_pan_channel
description: "Gimbal pan rc channel index. 0 is no channel."
default_value: 0
field: panChannel
min: 0
max: 32
- name: gimbal_roll_channel
description: "Gimbal roll rc channel index. 0 is no channel."
default_value: 0
field: rollChannel
min: 0
max: 32
- name: gimbal_tilt_channel
description: "Gimbal tilt rc channel index. 0 is no channel."
default_value: 0
field: tiltChannel
min: 0
max: 32
- name: gimbal_sensitivity
description: "Gimbal sensitivity is similar to gain and will affect how quickly the gimbal will react."
default_value: 0
field: sensitivity
min: -16
max: 15
- name: gimbal_pan_trim
field: panTrim
description: "Trim gimbal pan center position."
default_value: 0
min: -500
max: 500
- name: gimbal_tilt_trim
field: tiltTrim
description: "Trim gimbal tilt center position."
default_value: 0
min: -500
max: 500
- name: gimbal_roll_trim
field: rollTrim
description: "Trim gimbal roll center position."
default_value: 0
min: -500
max: 500
- name: PG_GIMBAL_SERIAL_CONFIG
type: gimbalSerialConfig_t
headers: ["io/gimbal_serial.h"]
condition: USE_SERIAL_GIMBAL
members:
- name: gimbal_serial_single_uart
description: "Gimbal serial and headtracker device share same UART. FC RX goes to headtracker device, FC TX goes to gimbal."
type: bool
default_value: OFF
field: singleUart
- name: PG_HEADTRACKER_CONFIG
type: headTrackerConfig_t
headers: ["drivers/headtracker_common.h"]
condition: USE_HEADTRACKER
members:
- name: headtracker_type
description: "Type of headtrackr dervice"
default_value: "NONE"
field: devType
type: uint8_t
table: headtracker_dev_type
- name: headtracker_pan_ratio
description: "Head pan movement vs camera movement ratio"
type: float
default_value: 1
field: pan_ratio
min: 0
max: 5
- name: headtracker_tilt_ratio
description: "Head tilt movement vs camera movement ratio"
type: float
default_value: 1
field: tilt_ratio
min: 0
max: 5
- name: headtracker_roll_ratio
description: "Head roll movement vs camera movement ratio"
type: float
default_value: 1
field: roll_ratio
min: 0
max: 5

190
src/main/flight/adaptive_filter.c Normal file
View file

@ -0,0 +1,190 @@
/*
* This file is part of INAV Project.
*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
* You can obtain one at http://mozilla.org/MPL/2.0/.
*
* Alternatively, the contents of this file may be used under the terms
* of the GNU General Public License Version 3, as described below:
*
* This file is free software: you may copy, redistribute and/or modify
* it under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or (at your
* option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
* Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see http://www.gnu.org/licenses/.
*/
#include "platform.h"
#ifdef USE_ADAPTIVE_FILTER
#include <stdlib.h>
#include "flight/adaptive_filter.h"
#include "arm_math.h"
#include <math.h>
#include "common/maths.h"
#include "common/axis.h"
#include "common/filter.h"
#include "build/debug.h"
#include "fc/config.h"
#include "fc/runtime_config.h"
#include "fc/rc_controls.h"
#include "sensors/gyro.h"
STATIC_FASTRAM float32_t adaptiveFilterSamples[XYZ_AXIS_COUNT][ADAPTIVE_FILTER_BUFFER_SIZE];
STATIC_FASTRAM uint8_t adaptiveFilterSampleIndex = 0;
STATIC_FASTRAM pt1Filter_t stdFilter[XYZ_AXIS_COUNT];
STATIC_FASTRAM pt1Filter_t hpfFilter[XYZ_AXIS_COUNT];
/*
We want to run adaptive filter only when UAV is commanded to stay stationary
Any rotation request on axis will add noise that we are not interested in as it will
automatically cause LPF frequency to be lowered
*/
STATIC_FASTRAM float axisAttenuationFactor[XYZ_AXIS_COUNT];
STATIC_FASTRAM uint8_t adaptiveFilterInitialized = 0;
STATIC_FASTRAM uint8_t hpfFilterInitialized = 0;
//Defines if current, min and max values for the filter were set and filter is ready to work
STATIC_FASTRAM uint8_t targetsSet = 0;
STATIC_FASTRAM float currentLpf;
STATIC_FASTRAM float initialLpf;
STATIC_FASTRAM float minLpf;
STATIC_FASTRAM float maxLpf;
STATIC_FASTRAM float adaptiveFilterIntegrator;
STATIC_FASTRAM float adaptiveIntegratorTarget;
/**
* This function is called at pid rate, so has to be initialized at PID loop frequency
*/
void adaptiveFilterPush(const flight_dynamics_index_t index, const float value) {
if (!hpfFilterInitialized) {
//Initialize the filter
for (flight_dynamics_index_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
pt1FilterInit(&hpfFilter[axis], gyroConfig()->adaptiveFilterHpfHz, US2S(getLooptime()));
}
hpfFilterInitialized = 1;
}
//Apply high pass filter, we are not interested in slowly changing values, only noise
const float filteredGyro = value - pt1FilterApply(&hpfFilter[index], value);
//Push new sample to the buffer so later we can compute RMS and other measures
adaptiveFilterSamples[index][adaptiveFilterSampleIndex] = filteredGyro;
adaptiveFilterSampleIndex = (adaptiveFilterSampleIndex + 1) % ADAPTIVE_FILTER_BUFFER_SIZE;
}
void adaptiveFilterPushRate(const flight_dynamics_index_t index, const float rate, const uint8_t configRate) {
const float maxRate = configRate * 10.0f;
axisAttenuationFactor[index] = scaleRangef(fabsf(rate), 0.0f, maxRate, 1.0f, 0.0f);
axisAttenuationFactor[index] = constrainf(axisAttenuationFactor[index], 0.0f, 1.0f);
}
void adaptiveFilterResetIntegrator(void) {
adaptiveFilterIntegrator = 0.0f;
}
void adaptiveFilterSetDefaultFrequency(int lpf, int min, int max) {
currentLpf = lpf;
minLpf = min;
maxLpf = max;
initialLpf = currentLpf;
targetsSet = 1;
}
void adaptiveFilterTask(timeUs_t currentTimeUs) {
//If we don't have current, min and max values for the filter, we can't run it yet
if (!targetsSet) {
return;
}
static timeUs_t previousUpdateTimeUs = 0;
//Initialization procedure, filter setup etc.
if (!adaptiveFilterInitialized) {
adaptiveIntegratorTarget = 3.5f;
previousUpdateTimeUs = currentTimeUs;
//Initialize the filter
for (flight_dynamics_index_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
pt1FilterInit(&stdFilter[axis], gyroConfig()->adaptiveFilterStdLpfHz, 1.0f / ADAPTIVE_FILTER_RATE_HZ);
}
adaptiveFilterInitialized = 1;
}
//If not armed, leave this routine but reset integrator and set default LPF
if (!ARMING_FLAG(ARMED)) {
currentLpf = initialLpf;
adaptiveFilterResetIntegrator();
gyroUpdateDynamicLpf(currentLpf);
return;
}
//Do not run adaptive filter when throttle is low
if (rcCommand[THROTTLE] < 1200) {
return;
}
//Prepare time delta to normalize time factor of the integrator
const float dT = US2S(currentTimeUs - previousUpdateTimeUs);
previousUpdateTimeUs = currentTimeUs;
float combinedStd = 0.0f;
//Compute RMS for each axis
for (flight_dynamics_index_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
//Copy axis samples to a temporary buffer
float32_t tempBuffer[ADAPTIVE_FILTER_BUFFER_SIZE];
//Copute STD from buffer using arm_std_f32
float32_t std;
memcpy(tempBuffer, adaptiveFilterSamples[axis], sizeof(adaptiveFilterSamples[axis]));
arm_std_f32(tempBuffer, ADAPTIVE_FILTER_BUFFER_SIZE, &std);
const float filteredStd = pt1FilterApply(&stdFilter[axis], std);
const float error = filteredStd - adaptiveIntegratorTarget;
const float adjustedError = error * axisAttenuationFactor[axis];
const float timeAdjustedError = adjustedError * dT;
//Put into integrator
adaptiveFilterIntegrator += timeAdjustedError;
combinedStd += std;
}
if (adaptiveFilterIntegrator > gyroConfig()->adaptiveFilterIntegratorThresholdHigh) {
//In this case there is too much noise, we need to lower the LPF frequency
currentLpf = constrainf(currentLpf - 1.0f, minLpf, maxLpf);
gyroUpdateDynamicLpf(currentLpf);
adaptiveFilterResetIntegrator();
} else if (adaptiveFilterIntegrator < gyroConfig()->adaptiveFilterIntegratorThresholdLow) {
//In this case there is too little noise, we can to increase the LPF frequency
currentLpf = constrainf(currentLpf + 1.0f, minLpf, maxLpf);
gyroUpdateDynamicLpf(currentLpf);
adaptiveFilterResetIntegrator();
}
combinedStd /= XYZ_AXIS_COUNT;
DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 0, combinedStd * 1000.0f);
DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 1, adaptiveFilterIntegrator * 10.0f);
DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 2, currentLpf);
}
#endif /* USE_ADAPTIVE_FILTER */

35
src/main/flight/adaptive_filter.h Normal file
View file

@ -0,0 +1,35 @@
/*
* This file is part of INAV Project.
*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
* You can obtain one at http://mozilla.org/MPL/2.0/.
*
* Alternatively, the contents of this file may be used under the terms
* of the GNU General Public License Version 3, as described below:
*
* This file is free software: you may copy, redistribute and/or modify
* it under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or (at your
* option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
* Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see http://www.gnu.org/licenses/.
*/
#include "common/axis.h"
#include "common/time.h"
#define ADAPTIVE_FILTER_BUFFER_SIZE 64
#define ADAPTIVE_FILTER_RATE_HZ 100
void adaptiveFilterPush(const flight_dynamics_index_t index, const float value);
void adaptiveFilterPushRate(const flight_dynamics_index_t index, const float rate, const uint8_t configRate);
void adaptiveFilterResetIntegrator(void);
void adaptiveFilterSetDefaultFrequency(int lpf, int min, int max);
void adaptiveFilterTask(timeUs_t currentTimeUs);

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

@ -37,7 +37,7 @@ static float dynLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLp
void dynamicLpfGyroTask(void) {
if (!gyroConfig()->useDynamicLpf) {
if (gyroConfig()->gyroFilterMode != GYRO_FILTER_MODE_DYNAMIC) {
return;
}

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

@ -109,7 +109,7 @@ void ezTuneUpdate(void) {
#endif
//Disable dynamic LPF
gyroConfigMutable()->useDynamicLpf = 0;
gyroConfigMutable()->gyroFilterMode = GYRO_FILTER_MODE_STATIC;
//Setup PID controller

16
src/main/flight/failsafe.c
View file

@ -84,7 +84,7 @@ PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig,
.failsafe_mission_delay = SETTING_FAILSAFE_MISSION_DELAY_DEFAULT, // Time delay before Failsafe activated during WP mission (s)
#ifdef USE_GPS_FIX_ESTIMATION
.failsafe_gps_fix_estimation_delay = SETTING_FAILSAFE_GPS_FIX_ESTIMATION_DELAY_DEFAULT, // Time delay before Failsafe activated when GPS Fix estimation is allied
#endif
#endif
);
typedef enum {
@ -350,16 +350,16 @@ static failsafeProcedure_e failsafeChooseFailsafeProcedure(void)
}
}
// Inhibit Failsafe if emergency landing triggered manually
if (posControl.flags.manualEmergLandActive) {
// Inhibit Failsafe if emergency landing triggered manually or if landing is detected
if (posControl.flags.manualEmergLandActive || STATE(LANDING_DETECTED)) {
return FAILSAFE_PROCEDURE_NONE;
}
// Craft is closer than minimum failsafe procedure distance (if set to non-zero)
// GPS must also be working, and home position set
if (failsafeConfig()->failsafe_min_distance > 0 &&
((sensors(SENSOR_GPS) && STATE(GPS_FIX))
#ifdef USE_GPS_FIX_ESTIMATION
((sensors(SENSOR_GPS) && STATE(GPS_FIX))
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) && STATE(GPS_FIX_HOME)) {
@ -429,8 +429,8 @@ void failsafeUpdateState(void)
#ifdef USE_GPS_FIX_ESTIMATION
if ( checkGPSFixFailsafe() ) {
reprocessState = true;
} else
#endif
} else
#endif
if (!receivingRxDataAndNotFailsafeMode) {
if ((failsafeConfig()->failsafe_throttle_low_delay && (millis() > failsafeState.throttleLowPeriod)) || STATE(NAV_MOTOR_STOP_OR_IDLE)) {
// JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds or waiting for launch
@ -499,7 +499,7 @@ void failsafeUpdateState(void)
} else if (failsafeChooseFailsafeProcedure() != FAILSAFE_PROCEDURE_NONE) { // trigger new failsafe procedure if changed
failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED;
reprocessState = true;
}
}
#ifdef USE_GPS_FIX_ESTIMATION
else {
if ( checkGPSFixFailsafe() ) {

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

@ -40,15 +40,15 @@ typedef enum {
PLATFORM_HELICOPTER = 2,
PLATFORM_TRICOPTER = 3,
PLATFORM_ROVER = 4,
PLATFORM_BOAT = 5,
PLATFORM_OTHER = 6
PLATFORM_BOAT = 5
} flyingPlatformType_e;
typedef enum {
OUTPUT_MODE_AUTO = 0,
OUTPUT_MODE_MOTORS,
OUTPUT_MODE_SERVOS
OUTPUT_MODE_SERVOS,
OUTPUT_MODE_LED
} outputMode_e;
typedef struct motorAxisCorrectionLimits_s {
@ -131,4 +131,4 @@ void stopPwmAllMotors(void);
void loadPrimaryMotorMixer(void);
bool areMotorsRunning(void);
uint16_t getMaxThrottle(void);
uint16_t getMaxThrottle(void);

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

@ -47,6 +47,7 @@
#include "flight/rpm_filter.h"
#include "flight/kalman.h"
#include "flight/smith_predictor.h"
#include "flight/adaptive_filter.h"
#include "io/gps.h"
@ -65,6 +66,14 @@
#include "programming/logic_condition.h"
typedef struct {
float aP;
float aI;
float aD;
float aFF;
timeMs_t targetOverThresholdTimeMs;
} fwPidAttenuation_t;
typedef struct {
uint8_t axis;
float kP; // Proportional gain
@ -106,6 +115,8 @@ typedef struct {
pt3Filter_t rateTargetFilter;
smithPredictor_t smithPredictor;
fwPidAttenuation_t attenuation;
} pidState_t;
STATIC_FASTRAM bool pidFiltersConfigured = false;
@ -157,7 +168,6 @@ static EXTENDED_FASTRAM uint8_t usedPidControllerType;
typedef void (*pidControllerFnPtr)(pidState_t *pidState, float dT, float dT_inv);
static EXTENDED_FASTRAM pidControllerFnPtr pidControllerApplyFn;
static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn;
static EXTENDED_FASTRAM bool levelingEnabled = false;
static EXTENDED_FASTRAM bool restartAngleHoldMode = true;
static EXTENDED_FASTRAM bool angleHoldIsLevel = false;
@ -169,7 +179,7 @@ static EXTENDED_FASTRAM bool angleHoldIsLevel = false;
static EXTENDED_FASTRAM float fixedWingLevelTrim;
static EXTENDED_FASTRAM pidController_t fixedWingLevelTrimController;
PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 8);
PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 9);
PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.bank_mc = {
@ -268,7 +278,6 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.fixedWingReferenceAirspeed = SETTING_FW_REFERENCE_AIRSPEED_DEFAULT,
.fixedWingCoordinatedYawGain = SETTING_FW_TURN_ASSIST_YAW_GAIN_DEFAULT,
.fixedWingCoordinatedPitchGain = SETTING_FW_TURN_ASSIST_PITCH_GAIN_DEFAULT,
.fixedWingItermLimitOnStickPosition = SETTING_FW_ITERM_LIMIT_STICK_POSITION_DEFAULT,
.fixedWingYawItermBankFreeze = SETTING_FW_YAW_ITERM_FREEZE_BANK_ANGLE_DEFAULT,
.navVelXyDTermLpfHz = SETTING_NAV_MC_VEL_XY_DTERM_LPF_HZ_DEFAULT,
@ -304,6 +313,9 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.smithPredictorDelay = SETTING_SMITH_PREDICTOR_DELAY_DEFAULT,
.smithPredictorFilterHz = SETTING_SMITH_PREDICTOR_LPF_HZ_DEFAULT,
#endif
.fwItermLockTimeMaxMs = SETTING_FW_ITERM_LOCK_TIME_MAX_MS_DEFAULT,
.fwItermLockRateLimit = SETTING_FW_ITERM_LOCK_RATE_THRESHOLD_DEFAULT,
.fwItermLockEngageThreshold = SETTING_FW_ITERM_LOCK_ENGAGE_THRESHOLD_DEFAULT,
);
bool pidInitFilters(void)
@ -670,19 +682,6 @@ static void pidApplySetpointRateLimiting(pidState_t *pidState, flight_dynamics_i
}
}
bool isFixedWingItermLimitActive(float stickPosition)
{
/*
* Iterm anti windup whould be active only when pilot controls the rotation
* velocity directly, not when ANGLE or HORIZON are used
*/
if (levelingEnabled) {
return false;
}
return fabsf(stickPosition) > pidProfile()->fixedWingItermLimitOnStickPosition;
}
static float pTermProcess(pidState_t *pidState, float rateError, float dT) {
float newPTerm = rateError * pidState->kP;
@ -751,20 +750,61 @@ static void nullRateController(pidState_t *pidState, float dT, float dT_inv) {
UNUSED(dT_inv);
}
static void fwRateAttenuation(pidState_t *pidState, const float rateTarget, const float rateError) {
const float maxRate = currentControlRateProfile->stabilized.rates[pidState->axis] * 10.0f;
const float dampingFactor = attenuation(rateTarget, maxRate * pidProfile()->fwItermLockRateLimit / 100.0f);
/*
* Iterm damping is applied (down to 0) when:
* abs(error) > 10% rate and sticks were moved in the last 500ms (hard stop at this mark)
* itermAttenuation = MIN(curve(setpoint), (abs(error) > 10%) && (sticks were deflected in 500ms) ? 0 : 1)
*/
//If error is greater than 10% or max rate
const bool errorThresholdReached = fabsf(rateError) > maxRate * pidProfile()->fwItermLockEngageThreshold / 100.0f;
//If stick (setpoint) was moved above threshold in the last 500ms
if (fabsf(rateTarget) > maxRate * 0.2f) {
pidState->attenuation.targetOverThresholdTimeMs = millis();
}
//If error is below threshold, we no longer track time for lock mechanism
if (!errorThresholdReached) {
pidState->attenuation.targetOverThresholdTimeMs = 0;
}
pidState->attenuation.aI = MIN(dampingFactor, (errorThresholdReached && (millis() - pidState->attenuation.targetOverThresholdTimeMs) < pidProfile()->fwItermLockTimeMaxMs) ? 0.0f : 1.0f);
//P & D damping factors are always the same and based on current damping factor
pidState->attenuation.aP = dampingFactor;
pidState->attenuation.aD = dampingFactor;
if (pidState->axis == FD_ROLL) {
DEBUG_SET(DEBUG_ALWAYS, 0, pidState->attenuation.aP * 1000);
DEBUG_SET(DEBUG_ALWAYS, 1, pidState->attenuation.aI * 1000);
DEBUG_SET(DEBUG_ALWAYS, 2, pidState->attenuation.aD * 1000);
}
}
static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, float dT, float dT_inv)
{
const float rateTarget = getFlightAxisRateOverride(pidState->axis, pidState->rateTarget);
const float rateError = rateTarget - pidState->gyroRate;
const float newPTerm = pTermProcess(pidState, rateError, dT);
const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv);
fwRateAttenuation(pidState, rateTarget, rateError);
const float newPTerm = pTermProcess(pidState, rateError, dT) * pidState->attenuation.aP;
const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv) * pidState->attenuation.aD;
const float newFFTerm = rateTarget * pidState->kFF;
/*
* Integral should be updated only if axis Iterm is not frozen
*/
if (!pidState->itermFreezeActive) {
pidState->errorGyroIf += rateError * pidState->kI * dT;
pidState->errorGyroIf += rateError * pidState->kI * dT * pidState->attenuation.aI;
}
applyItermLimiting(pidState);
@ -1046,11 +1086,9 @@ static void pidApplyFpvCameraAngleMix(pidState_t *pidState, uint8_t fpvCameraAng
void checkItermLimitingActive(pidState_t *pidState)
{
bool shouldActivate;
if (usedPidControllerType == PID_TYPE_PIFF) {
shouldActivate = isFixedWingItermLimitActive(pidState->stickPosition);
} else
{
bool shouldActivate = false;
if (usedPidControllerType == PID_TYPE_PID) {
shouldActivate = mixerIsOutputSaturated(); //just in case, since it is already managed by itermWindupPointPercent
}
@ -1062,7 +1100,7 @@ void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis
if (usedPidControllerType == PID_TYPE_PIFF && pidProfile()->fixedWingYawItermBankFreeze != 0 && axis == FD_YAW) {
// Do not allow yaw I-term to grow when bank angle is too large
float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll);
if (fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankFreeze && !(FLIGHT_MODE(AUTO_TUNE) || FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance())){
if (fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankFreeze && !(FLIGHT_MODE(AUTO_TUNE) || FLIGHT_MODE(TURN_ASSISTANT))) {
pidState->itermFreezeActive = true;
} else
{
@ -1168,6 +1206,10 @@ void FAST_CODE pidController(float dT)
// Limit desired rate to something gyro can measure reliably
pidState[axis].rateTarget = constrainf(rateTarget, -GYRO_SATURATION_LIMIT, +GYRO_SATURATION_LIMIT);
#ifdef USE_ADAPTIVE_FILTER
adaptiveFilterPushRate(axis, pidState[axis].rateTarget, currentControlRateProfile->stabilized.rates[axis]);
#endif
#ifdef USE_GYRO_KALMAN
gyroKalmanUpdateSetpoint(axis, pidState[axis].rateTarget);
@ -1180,7 +1222,6 @@ void FAST_CODE pidController(float dT)
// Step 3: Run control for ANGLE_MODE, HORIZON_MODE and ANGLEHOLD_MODE
const float horizonRateMagnitude = FLIGHT_MODE(HORIZON_MODE) ? calcHorizonRateMagnitude() : 0.0f;
levelingEnabled = false;
angleHoldIsLevel = false;
for (uint8_t axis = FD_ROLL; axis <= FD_PITCH; axis++) {
@ -1200,14 +1241,13 @@ void FAST_CODE pidController(float dT)
// Apply the Level PID controller
pidLevel(angleTarget, &pidState[axis], axis, horizonRateMagnitude, dT);
canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with ANGLE/HORIZON
levelingEnabled = true;
} else {
restartAngleHoldMode = true;
}
}
// Apply Turn Assistance
if ((FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
if (FLIGHT_MODE(TURN_ASSISTANT) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
float bankAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_ROLL], pidProfile()->max_angle_inclination[FD_ROLL]));
float pitchAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_PITCH], pidProfile()->max_angle_inclination[FD_PITCH]));
pidTurnAssistant(pidState, bankAngleTarget, pitchAngleTarget);
@ -1345,7 +1385,7 @@ pidBank_t * pidBankMutable(void) {
bool isFixedWingLevelTrimActive(void)
{
return IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) && !areSticksDeflected() &&
return isFwAutoModeActive(BOXAUTOLEVEL) && !areSticksDeflected() &&
(FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) &&
!FLIGHT_MODE(SOARING_MODE) && !FLIGHT_MODE(MANUAL_MODE) &&
!navigationIsControllingAltitude() && !(navCheckActiveAngleHoldAxis() == FD_PITCH && !angleHoldIsLevel);
@ -1369,7 +1409,7 @@ void updateFixedWingLevelTrim(timeUs_t currentTimeUs)
previousArmingState = ARMING_FLAG(ARMED);
// return if not active or disarmed
if (!IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) || !ARMING_FLAG(ARMED)) {
if (!isFwAutoModeActive(BOXAUTOLEVEL) || !ARMING_FLAG(ARMED)) {
return;
}

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

@ -121,7 +121,6 @@ typedef struct pidProfile_s {
float fixedWingReferenceAirspeed; // Reference tuning airspeed for the airplane - the speed for which PID gains are tuned
float fixedWingCoordinatedYawGain; // This is the gain of the yaw rate required to keep the yaw rate consistent with the turn rate for a coordinated turn.
float fixedWingCoordinatedPitchGain; // This is the gain of the pitch rate to keep the pitch angle constant during coordinated turns.
float fixedWingItermLimitOnStickPosition; //Do not allow Iterm to grow when stick position is above this point
uint16_t fixedWingYawItermBankFreeze; // Freeze yaw Iterm when bank angle is more than this many degrees
float navVelXyDTermLpfHz;
@ -156,6 +155,12 @@ typedef struct pidProfile_s {
float smithPredictorDelay;
uint16_t smithPredictorFilterHz;
#endif
uint16_t fwItermLockTimeMaxMs;
uint8_t fwItermLockRateLimit;
uint8_t fwItermLockEngageThreshold;
} pidProfile_t;
typedef struct pidAutotuneConfig_s {

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

@ -40,6 +40,7 @@
#include "fc/config.h"
#include "fc/controlrate_profile.h"
#include "fc/rc_controls.h"
#include "fc/rc_modes.h"
#include "fc/rc_adjustments.h"
#include "fc/runtime_config.h"
#include "fc/settings.h"
@ -130,7 +131,7 @@ void autotuneStart(void)
void autotuneUpdateState(void)
{
if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE) && STATE(AIRPLANE) && ARMING_FLAG(ARMED)) {
if (isFwAutoModeActive(BOXAUTOTUNE) && STATE(AIRPLANE) && ARMING_FLAG(ARMED)) {
if (!FLIGHT_MODE(AUTO_TUNE)) {
autotuneStart();
ENABLE_FLIGHT_MODE(AUTO_TUNE);
@ -202,7 +203,7 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa
if ((tuneCurrent[axis].updateCount & 25) == 0 && tuneCurrent[axis].updateCount >= AUTOTUNE_FIXED_WING_MIN_SAMPLES) {
if (pidAutotuneConfig()->fw_rate_adjustment != FIXED && !FLIGHT_MODE(ANGLE_MODE)) { // Rate discovery is not possible in ANGLE mode
// Target 80% control surface deflection to leave some room for P and I to work
float pidSumTarget = (pidAutotuneConfig()->fw_max_rate_deflection / 100.0f) * pidSumLimit;

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

@ -222,7 +222,7 @@ float calculateRemainingDistanceBeforeRTH(bool takeWindIntoAccount) {
// check requirements
const bool areBatterySettingsOK = feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER) && batteryWasFullWhenPluggedIn();
const bool areRTHEstimatorSettingsOK = batteryMetersConfig()->cruise_power > 0 && currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH &&currentBatteryProfile->capacity.value > 0 && navConfig()->fw.cruise_speed > 0;
const bool areRTHEstimatorSettingsOK = batteryMetersConfig()->cruise_power > 0 && batteryMetersConfig()->capacity_unit == BAT_CAPACITY_UNIT_MWH && currentBatteryProfile->capacity.value > 0 && navConfig()->fw.cruise_speed > 0;
const bool isNavigationOK = navigationPositionEstimateIsHealthy() && isImuHeadingValid();
if (!(areBatterySettingsOK && areRTHEstimatorSettingsOK && isNavigationOK)) {

63
src/main/flight/servos.c
View file

@ -38,6 +38,8 @@
#include "drivers/pwm_output.h"
#include "drivers/pwm_mapping.h"
#include "drivers/time.h"
#include "drivers/gimbal_common.h"
#include "drivers/headtracker_common.h"
#include "fc/config.h"
#include "fc/fc_core.h"
@ -82,7 +84,7 @@ void Reset_servoMixers(servoMixer_t *instance)
#ifdef USE_PROGRAMMING_FRAMEWORK
,.conditionId = -1
#endif
);
);
}
}
@ -96,7 +98,7 @@ void pgResetFn_servoParams(servoParam_t *instance)
.max = DEFAULT_SERVO_MAX,
.middle = DEFAULT_SERVO_MIDDLE,
.rate = 100
);
);
}
}
@ -194,7 +196,7 @@ void servosInit(void)
}
int getServoCount(void)
{
{
if (mixerUsesServos) {
return 1 + maxServoIndex - minServoIndex;
}
@ -246,7 +248,7 @@ static void filterServos(void)
void writeServos(void)
{
filterServos();
#if !defined(SITL_BUILD)
int servoIndex = 0;
bool zeroServoValue = false;
@ -345,8 +347,45 @@ void servoMixer(float dT)
input[INPUT_RC_CH14] = GET_RX_CHANNEL_INPUT(AUX10);
input[INPUT_RC_CH15] = GET_RX_CHANNEL_INPUT(AUX11);
input[INPUT_RC_CH16] = GET_RX_CHANNEL_INPUT(AUX12);
input[INPUT_RC_CH17] = GET_RX_CHANNEL_INPUT(AUX13);
input[INPUT_RC_CH18] = GET_RX_CHANNEL_INPUT(AUX14);
#ifdef USE_34CHANNELS
input[INPUT_RC_CH19] = GET_RX_CHANNEL_INPUT(AUX15);
input[INPUT_RC_CH20] = GET_RX_CHANNEL_INPUT(AUX16);
input[INPUT_RC_CH21] = GET_RX_CHANNEL_INPUT(AUX17);
input[INPUT_RC_CH22] = GET_RX_CHANNEL_INPUT(AUX18);
input[INPUT_RC_CH23] = GET_RX_CHANNEL_INPUT(AUX19);
input[INPUT_RC_CH24] = GET_RX_CHANNEL_INPUT(AUX20);
input[INPUT_RC_CH25] = GET_RX_CHANNEL_INPUT(AUX21);
input[INPUT_RC_CH26] = GET_RX_CHANNEL_INPUT(AUX22);
input[INPUT_RC_CH27] = GET_RX_CHANNEL_INPUT(AUX23);
input[INPUT_RC_CH28] = GET_RX_CHANNEL_INPUT(AUX24);
input[INPUT_RC_CH29] = GET_RX_CHANNEL_INPUT(AUX25);
input[INPUT_RC_CH30] = GET_RX_CHANNEL_INPUT(AUX26);
input[INPUT_RC_CH31] = GET_RX_CHANNEL_INPUT(AUX27);
input[INPUT_RC_CH32] = GET_RX_CHANNEL_INPUT(AUX28);
input[INPUT_RC_CH33] = GET_RX_CHANNEL_INPUT(AUX29);
input[INPUT_RC_CH34] = GET_RX_CHANNEL_INPUT(AUX30);
#endif
#undef GET_RX_CHANNEL_INPUT
#ifdef USE_HEADTRACKER
headTrackerDevice_t *dev = headTrackerCommonDevice();
if(dev && headTrackerCommonIsValid(dev) && !IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)) {
input[INPUT_HEADTRACKER_PAN] = headTrackerCommonGetPanPWM(dev) - PWM_RANGE_MIDDLE;
input[INPUT_HEADTRACKER_TILT] = headTrackerCommonGetTiltPWM(dev) - PWM_RANGE_MIDDLE;
input[INPUT_HEADTRACKER_ROLL] = headTrackerCommonGetRollPWM(dev) - PWM_RANGE_MIDDLE;
} else {
input[INPUT_HEADTRACKER_PAN] = 0;
input[INPUT_HEADTRACKER_TILT] = 0;
input[INPUT_HEADTRACKER_ROLL] = 0;
}
#else
input[INPUT_HEADTRACKER_PAN] = 0;
input[INPUT_HEADTRACKER_TILT] = 0;
input[INPUT_HEADTRACKER_ROLL] = 0;
#endif
#ifdef USE_SIMULATOR
simulatorData.input[INPUT_STABILIZED_ROLL] = input[INPUT_STABILIZED_ROLL];
simulatorData.input[INPUT_STABILIZED_PITCH] = input[INPUT_STABILIZED_PITCH];
@ -449,7 +488,7 @@ void processServoAutotrimMode(void)
static int32_t servoMiddleAccum[MAX_SUPPORTED_SERVOS];
static int32_t servoMiddleAccumCount[MAX_SUPPORTED_SERVOS];
if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM)) {
if (isFwAutoModeActive(BOXAUTOTRIM)) {
switch (trimState) {
case AUTOTRIM_IDLE:
if (ARMING_FLAG(ARMED)) {
@ -544,7 +583,7 @@ void processServoAutotrimMode(void)
void processContinuousServoAutotrim(const float dT)
{
static timeMs_t lastUpdateTimeMs;
static servoAutotrimState_e trimState = AUTOTRIM_IDLE;
static servoAutotrimState_e trimState = AUTOTRIM_IDLE;
static uint32_t servoMiddleUpdateCount;
const float rotRateMagnitudeFiltered = pt1FilterApply4(&rotRateFilter, fast_fsqrtf(vectorNormSquared(&imuMeasuredRotationBF)), SERVO_AUTOTRIM_FILTER_CUTOFF, dT);
@ -556,16 +595,16 @@ void processContinuousServoAutotrim(const float dT)
const bool planeIsFlyingStraight = rotRateMagnitudeFiltered <= DEGREES_TO_RADIANS(servoConfig()->servo_autotrim_rotation_limit);
const bool noRotationCommanded = targetRateMagnitudeFiltered <= servoConfig()->servo_autotrim_rotation_limit;
const bool sticksAreCentered = !areSticksDeflected();
const bool planeIsFlyingLevel = ABS(attitude.values.pitch + DEGREES_TO_DECIDEGREES(getFixedWingLevelTrim())) <= SERVO_AUTOTRIM_ATTITUDE_LIMIT
const bool planeIsFlyingLevel = ABS(attitude.values.pitch + DEGREES_TO_DECIDEGREES(getFixedWingLevelTrim())) <= SERVO_AUTOTRIM_ATTITUDE_LIMIT
&& ABS(attitude.values.roll) <= SERVO_AUTOTRIM_ATTITUDE_LIMIT;
if (
planeIsFlyingStraight &&
noRotationCommanded &&
planeIsFlyingStraight &&
noRotationCommanded &&
planeIsFlyingLevel &&
sticksAreCentered &&
!FLIGHT_MODE(MANUAL_MODE) &&
!FLIGHT_MODE(MANUAL_MODE) &&
isGPSHeadingValid() // TODO: proper flying detection
) {
) {
// Plane is flying straight and level: trim servos
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
// For each stabilized axis, add 5 units of I-term to all associated servo midpoints
@ -610,7 +649,7 @@ void processContinuousServoAutotrim(const float dT)
DEBUG_SET(DEBUG_AUTOTRIM, 1, servoMiddleUpdateCount);
DEBUG_SET(DEBUG_AUTOTRIM, 3, MAX(RADIANS_TO_DEGREES(rotRateMagnitudeFiltered), targetRateMagnitudeFiltered));
DEBUG_SET(DEBUG_AUTOTRIM, 5, axisPID_I[FD_ROLL]);
DEBUG_SET(DEBUG_AUTOTRIM, 7, axisPID_I[FD_PITCH]);
DEBUG_SET(DEBUG_AUTOTRIM, 7, axisPID_I[FD_PITCH]);
}
void processServoAutotrim(const float dT) {

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

@ -20,7 +20,7 @@
#include "config/parameter_group.h"
#include "programming/logic_condition.h"
#define MAX_SUPPORTED_SERVOS 16
#define MAX_SUPPORTED_SERVOS 18
// These must be consecutive
typedef enum {
@ -63,6 +63,27 @@ typedef enum {
INPUT_GVAR_6 = 36,
INPUT_GVAR_7 = 37,
INPUT_MIXER_TRANSITION = 38,
INPUT_HEADTRACKER_PAN = 39,
INPUT_HEADTRACKER_TILT = 40,
INPUT_HEADTRACKER_ROLL = 41,
INPUT_RC_CH17 = 42,
INPUT_RC_CH18 = 43,
INPUT_RC_CH19 = 44,
INPUT_RC_CH20 = 45,
INPUT_RC_CH21 = 46,
INPUT_RC_CH22 = 47,
INPUT_RC_CH23 = 48,
INPUT_RC_CH24 = 49,
INPUT_RC_CH25 = 50,
INPUT_RC_CH26 = 51,
INPUT_RC_CH27 = 52,
INPUT_RC_CH28 = 53,
INPUT_RC_CH29 = 54,
INPUT_RC_CH30 = 55,
INPUT_RC_CH31 = 56,
INPUT_RC_CH32 = 57,
INPUT_RC_CH33 = 58,
INPUT_RC_CH34 = 59,
INPUT_SOURCE_COUNT
} inputSource_e;

163
src/main/io/bf_osd_symbols.h
View file

@ -1,163 +0,0 @@
/* @file max7456_symbols.h
* @brief max7456 symbols for the mwosd font set
*
* @author Nathan Tsoi nathan@vertile.com
*
* Copyright (C) 2016 Nathan Tsoi
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>
*/
#pragma once
//Misc
#define BF_SYM_NONE 0x00
#define BF_SYM_END_OF_FONT 0xFF
#define BF_SYM_BLANK 0x20
#define BF_SYM_HYPHEN 0x2D
#define BF_SYM_BBLOG 0x10
#define BF_SYM_HOMEFLAG 0x11
#define BF_SYM_RPM 0x12
#define BF_SYM_ROLL 0x14
#define BF_SYM_PITCH 0x15
#define BF_SYM_TEMPERATURE 0x7A
// GPS and navigation
#define BF_SYM_LAT 0x89
#define BF_SYM_LON 0x98
#define BF_SYM_ALTITUDE 0x7F
#define BF_SYM_TOTAL_DISTANCE 0x71
#define BF_SYM_OVER_HOME 0x05
// RSSI
#define BF_SYM_RSSI 0x01
#define BF_SYM_LINK_QUALITY 0x7B
// Throttle Position (%)
#define BF_SYM_THR 0x04
// Unit Icons (Metric)
#define BF_SYM_M 0x0C
#define BF_SYM_KM 0x7D
#define BF_SYM_C 0x0E
// Unit Icons (Imperial)
#define BF_SYM_FT 0x0F
#define BF_SYM_MILES 0x7E
#define BF_SYM_F 0x0D
// Heading Graphics
#define BF_SYM_HEADING_N 0x18
#define BF_SYM_HEADING_S 0x19
#define BF_SYM_HEADING_E 0x1A
#define BF_SYM_HEADING_W 0x1B
#define BF_SYM_HEADING_DIVIDED_LINE 0x1C
#define BF_SYM_HEADING_LINE 0x1D
// AH Center screen Graphics
#define BF_SYM_AH_CENTER_LINE 0x72
#define BF_SYM_AH_CENTER 0x73
#define BF_SYM_AH_CENTER_LINE_RIGHT 0x74
#define BF_SYM_AH_RIGHT 0x02
#define BF_SYM_AH_LEFT 0x03
#define BF_SYM_AH_DECORATION 0x13
// Satellite Graphics
#define BF_SYM_SAT_L 0x1E
#define BF_SYM_SAT_R 0x1F
// Direction arrows
#define BF_SYM_ARROW_SOUTH 0x60
#define BF_SYM_ARROW_2 0x61
#define BF_SYM_ARROW_3 0x62
#define BF_SYM_ARROW_4 0x63
#define BF_SYM_ARROW_EAST 0x64
#define BF_SYM_ARROW_6 0x65
#define BF_SYM_ARROW_7 0x66
#define BF_SYM_ARROW_8 0x67
#define BF_SYM_ARROW_NORTH 0x68
#define BF_SYM_ARROW_10 0x69
#define BF_SYM_ARROW_11 0x6A
#define BF_SYM_ARROW_12 0x6B
#define BF_SYM_ARROW_WEST 0x6C
#define BF_SYM_ARROW_14 0x6D
#define BF_SYM_ARROW_15 0x6E
#define BF_SYM_ARROW_16 0x6F
#define BF_SYM_ARROW_SMALL_UP 0x75
#define BF_SYM_ARROW_SMALL_DOWN 0x76
// AH Bars
#define BF_SYM_AH_BAR9_0 0x80
#define BF_SYM_AH_BAR9_1 0x81
#define BF_SYM_AH_BAR9_2 0x82
#define BF_SYM_AH_BAR9_3 0x83
#define BF_SYM_AH_BAR9_4 0x84
#define BF_SYM_AH_BAR9_5 0x85
#define BF_SYM_AH_BAR9_6 0x86
#define BF_SYM_AH_BAR9_7 0x87
#define BF_SYM_AH_BAR9_8 0x88
// Progress bar
#define BF_SYM_PB_START 0x8A
#define BF_SYM_PB_FULL 0x8B
#define BF_SYM_PB_HALF 0x8C
#define BF_SYM_PB_EMPTY 0x8D
#define BF_SYM_PB_END 0x8E
#define BF_SYM_PB_CLOSE 0x8F
// Batt evolution
#define BF_SYM_BATT_FULL 0x90
#define BF_SYM_BATT_5 0x91
#define BF_SYM_BATT_4 0x92
#define BF_SYM_BATT_3 0x93
#define BF_SYM_BATT_2 0x94
#define BF_SYM_BATT_1 0x95
#define BF_SYM_BATT_EMPTY 0x96
// Batt Icons
#define BF_SYM_MAIN_BATT 0x97
// Voltage and amperage
#define BF_SYM_VOLT 0x06
#define BF_SYM_AMP 0x9A
#define BF_SYM_MAH 0x07
#define BF_SYM_WATT 0x57 // 0x57 is 'W'
// Time
#define BF_SYM_ON_M 0x9B
#define BF_SYM_FLY_M 0x9C
// Speed
#define BF_SYM_SPEED 0x70
#define BF_SYM_KPH 0x9E
#define BF_SYM_MPH 0x9D
#define BF_SYM_MPS 0x9F
#define BF_SYM_FTPS 0x99
// Menu cursor
#define BF_SYM_CURSOR BF_SYM_AH_LEFT
// Stick overlays
#define BF_SYM_STICK_OVERLAY_SPRITE_HIGH 0x08
#define BF_SYM_STICK_OVERLAY_SPRITE_MID 0x09
#define BF_SYM_STICK_OVERLAY_SPRITE_LOW 0x0A
#define BF_SYM_STICK_OVERLAY_CENTER 0x0B
#define BF_SYM_STICK_OVERLAY_VERTICAL 0x16
#define BF_SYM_STICK_OVERLAY_HORIZONTAL 0x17
// GPS degree/minute/second symbols
#define BF_SYM_GPS_DEGREE BF_SYM_STICK_OVERLAY_SPRITE_HIGH // kind of looks like the degree symbol
#define BF_SYM_GPS_MINUTE 0x27 // '
#define BF_SYM_GPS_SECOND 0x22 // "

748
src/main/io/displayport_msp_bf_compat.c
View file

@ -1,748 +0,0 @@
/*
* This file is part of INAV Project.
*
* INAV is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* 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 "platform.h"
#if defined(USE_OSD) && defined(USE_MSP_DISPLAYPORT)
#ifndef DISABLE_MSP_BF_COMPAT
#include "io/displayport_msp_bf_compat.h"
#include "io/bf_osd_symbols.h"
#include "drivers/osd_symbols.h"
uint8_t getBfCharacter(uint8_t ch, uint8_t page)
{
uint16_t ech = ch | ((page & 0x3)<< 8) ;
if (ech >= 0x20 && ech <= 0x5F) { // ASCII range
return ch;
}
if (ech >= SYM_AH_DECORATION_MIN && ech <= SYM_AH_DECORATION_MAX) {
return BF_SYM_AH_DECORATION;
}
switch (ech) {
case SYM_RSSI:
return BF_SYM_RSSI;
case SYM_LQ:
return BF_SYM_LINK_QUALITY;
case SYM_LAT:
return BF_SYM_LAT;
case SYM_LON:
return BF_SYM_LON;
case SYM_SAT_L:
return BF_SYM_SAT_L;
case SYM_SAT_R:
return BF_SYM_SAT_R;
case SYM_HOME_NEAR:
return BF_SYM_HOMEFLAG;
case SYM_DEGREES:
return BF_SYM_GPS_DEGREE;
/*
case SYM_HEADING:
return BF_SYM_HEADING;
case SYM_SCALE:
return BF_SYM_SCALE;
case SYM_HDP_L:
return BF_SYM_HDP_L;
case SYM_HDP_R:
return BF_SYM_HDP_R;
*/
case SYM_HOME:
return BF_SYM_HOMEFLAG;
case SYM_2RSS:
return BF_SYM_RSSI;
/*
case SYM_DB:
return BF_SYM_DB
case SYM_DBM:
return BF_SYM_DBM;
case SYM_SNR:
return BF_SYM_SNR;
case SYM_AH_DECORATION_UP:
return BF_SYM_AH_DECORATION_UP;
case SYM_AH_DECORATION_DOWN:
return BF_SYM_AH_DECORATION_DOWN;
*/
case SYM_DIRECTION:
return BF_SYM_ARROW_NORTH;
case SYM_DIRECTION + 1: // NE pointing arrow
return BF_SYM_ARROW_7;
case SYM_DIRECTION + 2: // E pointing arrow
return BF_SYM_ARROW_EAST;
case SYM_DIRECTION + 3: // SE pointing arrow
return BF_SYM_ARROW_3;
case SYM_DIRECTION + 4: // S pointing arrow
return BF_SYM_ARROW_SOUTH;
case SYM_DIRECTION + 5: // SW pointing arrow
return BF_SYM_ARROW_15;
case SYM_DIRECTION + 6: // W pointing arrow
return BF_SYM_ARROW_WEST;
case SYM_DIRECTION + 7: // NW pointing arrow
return BF_SYM_ARROW_11;
case SYM_VOLT:
return BF_SYM_VOLT;
case SYM_MAH:
return BF_SYM_MAH;
case SYM_AH_KM:
return BF_SYM_KM;
case SYM_AH_MI:
return BF_SYM_MILES;
/*
case SYM_VTX_POWER:
return BF_SYM_VTX_POWER;
case SYM_AH_NM:
return BF_SYM_AH_NM;
case SYM_MAH_NM_0:
return BF_SYM_MAH_NM_0;
case SYM_MAH_NM_1:
return BF_SYM_MAH_NM_1;
case SYM_MAH_KM_0:
return BF_SYM_MAH_KM_0;
case SYM_MAH_KM_1:
return BF_SYM_MAH_KM_1;
case SYM_MILLIOHM:
return BF_SYM_MILLIOHM;
*/
case SYM_BATT_FULL:
return BF_SYM_BATT_FULL;
case SYM_BATT_5:
return BF_SYM_BATT_5;
case SYM_BATT_4:
return BF_SYM_BATT_4;
case SYM_BATT_3:
return BF_SYM_BATT_3;
case SYM_BATT_2:
return BF_SYM_BATT_2;
case SYM_BATT_1:
return BF_SYM_BATT_1;
case SYM_BATT_EMPTY:
return BF_SYM_BATT_EMPTY;
case SYM_AMP:
return BF_SYM_AMP;
/*
case SYM_WH:
return BF_SYM_WH;
case SYM_WH_KM:
return BF_SYM_WH_KM;
case SYM_WH_MI:
return BF_SYM_WH_MI;
case SYM_WH_NM:
return BF_SYM_WH_NM;
*/
case SYM_WATT:
return BF_SYM_WATT;
/*
case SYM_MW:
return BF_SYM_MW;
case SYM_KILOWATT:
return BF_SYM_KILOWATT;
*/
case SYM_FT:
return BF_SYM_FT;
case SYM_ALT_FT:
return BF_SYM_FT;
case SYM_ALT_M:
return BF_SYM_M;
case SYM_TOTAL:
return BF_SYM_TOTAL_DISTANCE;
/*
case SYM_ALT_KM:
return BF_SYM_ALT_KM;
case SYM_ALT_KFT:
return BF_SYM_ALT_KFT;
case SYM_DIST_M:
return BF_SYM_DIST_M;
case SYM_DIST_KM:
return BF_SYM_DIST_KM;
case SYM_DIST_FT:
return BF_SYM_DIST_FT;
case SYM_DIST_MI:
return BF_SYM_DIST_MI;
case SYM_DIST_NM:
return BF_SYM_DIST_NM;
*/
case SYM_M:
return BF_SYM_M;
case SYM_KM:
return BF_SYM_KM;
case SYM_MI:
return BF_SYM_MILES;
/*
case SYM_NM:
return BF_SYM_NM;
*/
case SYM_WIND_HORIZONTAL:
return 'W'; // W for wind
/*
case SYM_WIND_VERTICAL:
return BF_SYM_WIND_VERTICAL;
case SYM_3D_KT:
return BF_SYM_3D_KT;
*/
case SYM_AIR:
return 'A'; // A for airspeed
case SYM_3D_KMH:
return BF_SYM_KPH;
case SYM_3D_MPH:
return BF_SYM_MPH;
case SYM_RPM:
return BF_SYM_RPM;
case SYM_FTS:
return BF_SYM_FTPS;
/*
case SYM_100FTM:
return BF_SYM_100FTM;
*/
case SYM_MS:
return BF_SYM_MPS;
case SYM_KMH:
return BF_SYM_KPH;
case SYM_MPH:
return BF_SYM_MPH;
/*
case SYM_KT:
return BF_SYM_KT
case SYM_MAH_MI_0:
return BF_SYM_MAH_MI_0;
case SYM_MAH_MI_1:
return BF_SYM_MAH_MI_1;
*/
case SYM_THR:
return BF_SYM_THR;
case SYM_TEMP_F:
return BF_SYM_F;
case SYM_TEMP_C:
return BF_SYM_C;
case SYM_BLANK:
return BF_SYM_BLANK;
/*
case SYM_ON_H:
return BF_SYM_ON_H;
case SYM_FLY_H:
return BF_SYM_FLY_H;
*/
case SYM_ON_M:
return BF_SYM_ON_M;
case SYM_FLY_M:
return BF_SYM_FLY_M;
/*
case SYM_GLIDESLOPE:
return BF_SYM_GLIDESLOPE;
case SYM_WAYPOINT:
return BF_SYM_WAYPOINT;
case SYM_CLOCK:
return BF_SYM_CLOCK;
case SYM_ZERO_HALF_TRAILING_DOT:
return BF_SYM_ZERO_HALF_TRAILING_DOT;
case SYM_ZERO_HALF_LEADING_DOT:
return BF_SYM_ZERO_HALF_LEADING_DOT;
*/
case SYM_AUTO_THR0:
return 'A';
case SYM_AUTO_THR1:
return BF_SYM_THR;
case SYM_ROLL_LEFT:
return BF_SYM_ROLL;
case SYM_ROLL_LEVEL:
return BF_SYM_ROLL;
case SYM_ROLL_RIGHT:
return BF_SYM_ROLL;
case SYM_PITCH_UP:
return BF_SYM_PITCH;
case SYM_PITCH_DOWN:
return BF_SYM_PITCH;
case SYM_GFORCE:
return 'G';
/*
case SYM_GFORCE_X:
return BF_SYM_GFORCE_X;
case SYM_GFORCE_Y:
return BF_SYM_GFORCE_Y;
case SYM_GFORCE_Z:
return BF_SYM_GFORCE_Z;
case SYM_BARO_TEMP:
return BF_SYM_BARO_TEMP;
case SYM_IMU_TEMP:
return BF_SYM_IMU_TEMP;
case SYM_TEMP:
return BF_SYM_TEMP;
case SYM_TEMP_SENSOR_FIRST:
return BF_SYM_TEMP_SENSOR_FIRST;
case SYM_ESC_TEMP:
return BF_SYM_ESC_TEMP;
case SYM_TEMP_SENSOR_LAST:
return BF_SYM_TEMP_SENSOR_LAST;
case TEMP_SENSOR_SYM_COUNT:
return BF_TEMP_SENSOR_SYM_COUNT;
*/
case SYM_HEADING_N:
return BF_SYM_HEADING_N;
case SYM_HEADING_S:
return BF_SYM_HEADING_S;
case SYM_HEADING_E:
return BF_SYM_HEADING_E;
case SYM_HEADING_W:
return BF_SYM_HEADING_W;
case SYM_HEADING_DIVIDED_LINE:
return BF_SYM_HEADING_DIVIDED_LINE;
case SYM_HEADING_LINE:
return BF_SYM_HEADING_LINE;
/*
case SYM_MAX:
return BF_SYM_MAX;
case SYM_PROFILE:
return BF_SYM_PROFILE;
case SYM_SWITCH_INDICATOR_LOW:
return BF_SYM_SWITCH_INDICATOR_LOW;
case SYM_SWITCH_INDICATOR_MID:
return BF_SYM_SWITCH_INDICATOR_MID;
case SYM_SWITCH_INDICATOR_HIGH:
return BF_SYM_SWITCH_INDICATOR_HIGH;
case SYM_AH:
return BF_SYM_AH;
case SYM_GLIDE_DIST:
return BF_SYM_GLIDE_DIST;
case SYM_GLIDE_MINS:
return BF_SYM_GLIDE_MINS;
case SYM_AH_V_FT_0:
return BF_SYM_AH_V_FT_0;
case SYM_AH_V_FT_1:
return BF_SYM_AH_V_FT_1;
case SYM_AH_V_M_0:
return BF_SYM_AH_V_M_0;
case SYM_AH_V_M_1:
return BF_SYM_AH_V_M_1;
case SYM_FLIGHT_MINS_REMAINING:
return BF_SYM_FLIGHT_MINS_REMAINING;
case SYM_FLIGHT_HOURS_REMAINING:
return BF_SYM_FLIGHT_HOURS_REMAINING;
case SYM_GROUND_COURSE:
return BF_SYM_GROUND_COURSE;
case SYM_CROSS_TRACK_ERROR:
return BF_SYM_CROSS_TRACK_ERROR;
case SYM_LOGO_START:
return BF_SYM_LOGO_START;
case SYM_LOGO_WIDTH:
return BF_SYM_LOGO_WIDTH;
case SYM_LOGO_HEIGHT:
return BF_SYM_LOGO_HEIGHT;
*/
case SYM_AH_LEFT:
return BF_SYM_AH_LEFT;
case SYM_AH_RIGHT:
return BF_SYM_AH_RIGHT;
/*
case SYM_AH_DECORATION_COUNT:
return BF_SYM_AH_DECORATION_COUNT;
*/
case SYM_AH_CH_LEFT:
case SYM_AH_CH_TYPE3:
case SYM_AH_CH_TYPE4:
case SYM_AH_CH_TYPE5:
case SYM_AH_CH_TYPE6:
case SYM_AH_CH_TYPE7:
case SYM_AH_CH_TYPE8:
case SYM_AH_CH_AIRCRAFT1:
return BF_SYM_AH_CENTER_LINE;
case SYM_AH_CH_RIGHT:
case (SYM_AH_CH_TYPE3+2):
case (SYM_AH_CH_TYPE4+2):
case (SYM_AH_CH_TYPE5+2):
case (SYM_AH_CH_TYPE6+2):
case (SYM_AH_CH_TYPE7+2):
case (SYM_AH_CH_TYPE8+2):
case SYM_AH_CH_AIRCRAFT3:
return BF_SYM_AH_CENTER_LINE_RIGHT;
case SYM_AH_CH_AIRCRAFT0:
case SYM_AH_CH_AIRCRAFT4:
return ' ';
case SYM_ARROW_UP:
return BF_SYM_ARROW_NORTH;
case SYM_ARROW_2:
return BF_SYM_ARROW_8;
case SYM_ARROW_3:
return BF_SYM_ARROW_7;
case SYM_ARROW_4:
return BF_SYM_ARROW_6;
case SYM_ARROW_RIGHT:
return BF_SYM_ARROW_EAST;
case SYM_ARROW_6:
return BF_SYM_ARROW_4;
case SYM_ARROW_7:
return BF_SYM_ARROW_3;
case SYM_ARROW_8:
return BF_SYM_ARROW_2;
case SYM_ARROW_DOWN:
return BF_SYM_ARROW_SOUTH;
case SYM_ARROW_10:
return BF_SYM_ARROW_16;
case SYM_ARROW_11:
return BF_SYM_ARROW_15;
case SYM_ARROW_12:
return BF_SYM_ARROW_14;
case SYM_ARROW_LEFT:
return BF_SYM_ARROW_WEST;
case SYM_ARROW_14:
return BF_SYM_ARROW_12;
case SYM_ARROW_15:
return BF_SYM_ARROW_11;
case SYM_ARROW_16:
return BF_SYM_ARROW_10;
case SYM_AH_H_START:
return BF_SYM_AH_BAR9_0;
case (SYM_AH_H_START+1):
return BF_SYM_AH_BAR9_1;
case (SYM_AH_H_START+2):
return BF_SYM_AH_BAR9_2;
case (SYM_AH_H_START+3):
return BF_SYM_AH_BAR9_3;
case (SYM_AH_H_START+4):
return BF_SYM_AH_BAR9_4;
case (SYM_AH_H_START+5):
return BF_SYM_AH_BAR9_5;
case (SYM_AH_H_START+6):
return BF_SYM_AH_BAR9_6;
case (SYM_AH_H_START+7):
return BF_SYM_AH_BAR9_7;
case (SYM_AH_H_START+8):
return BF_SYM_AH_BAR9_8;
// BF does not have vertical artificial horizon. replace with middle horizontal one
case SYM_AH_V_START:
case (SYM_AH_V_START+1):
case (SYM_AH_V_START+2):
case (SYM_AH_V_START+3):
case (SYM_AH_V_START+4):
case (SYM_AH_V_START+5):
return BF_SYM_AH_BAR9_4;
// BF for ESP_RADAR Symbols
case SYM_HUD_CARDINAL:
return BF_SYM_ARROW_SOUTH;
case SYM_HUD_CARDINAL + 1:
return BF_SYM_ARROW_16;
case SYM_HUD_CARDINAL + 2:
return BF_SYM_ARROW_15;
case SYM_HUD_CARDINAL + 3:
return BF_SYM_ARROW_WEST;
case SYM_HUD_CARDINAL + 4:
return BF_SYM_ARROW_12;
case SYM_HUD_CARDINAL + 5:
return BF_SYM_ARROW_11;
case SYM_HUD_CARDINAL + 6:
return BF_SYM_ARROW_NORTH;
case SYM_HUD_CARDINAL + 7:
return BF_SYM_ARROW_7;
case SYM_HUD_CARDINAL + 8:
return BF_SYM_ARROW_6;
case SYM_HUD_CARDINAL + 9:
return BF_SYM_ARROW_EAST;
case SYM_HUD_CARDINAL + 10:
return BF_SYM_ARROW_3;
case SYM_HUD_CARDINAL + 11:
return BF_SYM_ARROW_2;
case SYM_HUD_ARROWS_R3:
return BF_SYM_AH_RIGHT;
case SYM_HUD_ARROWS_L3:
return BF_SYM_AH_LEFT;
case SYM_HUD_SIGNAL_0:
return BF_SYM_AH_BAR9_1;
case SYM_HUD_SIGNAL_1:
return BF_SYM_AH_BAR9_3;
case SYM_HUD_SIGNAL_2:
return BF_SYM_AH_BAR9_4;
case SYM_HUD_SIGNAL_3:
return BF_SYM_AH_BAR9_5;
case SYM_HUD_SIGNAL_4:
return BF_SYM_AH_BAR9_7;
/*
case SYM_VARIO_UP_2A:
return BF_SYM_VARIO_UP_2A;
case SYM_VARIO_UP_1A:
return BF_SYM_VARIO_UP_1A;
case SYM_VARIO_DOWN_1A:
return BF_SYM_VARIO_DOWN_1A;
case SYM_VARIO_DOWN_2A:
return BF_SYM_VARIO_DOWN_2A;
*/
case SYM_ALT:
return BF_SYM_ALTITUDE;
/*
case SYM_HUD_SIGNAL_0:
return BF_SYM_HUD_SIGNAL_0;
case SYM_HUD_SIGNAL_1:
return BF_SYM_HUD_SIGNAL_1;
case SYM_HUD_SIGNAL_2:
return BF_SYM_HUD_SIGNAL_2;
case SYM_HUD_SIGNAL_3:
return BF_SYM_HUD_SIGNAL_3;
case SYM_HUD_SIGNAL_4:
return BF_SYM_HUD_SIGNAL_4;
case SYM_HOME_DIST:
return BF_SYM_HOME_DIST;
*/
case SYM_AH_CH_CENTER:
case (SYM_AH_CH_TYPE3+1):
case (SYM_AH_CH_TYPE4+1):
case (SYM_AH_CH_TYPE5+1):
case (SYM_AH_CH_TYPE6+1):
case (SYM_AH_CH_TYPE7+1):
case (SYM_AH_CH_TYPE8+1):
case SYM_AH_CH_AIRCRAFT2:
return BF_SYM_AH_CENTER;
/*
case SYM_FLIGHT_DIST_REMAINING:
return BF_SYM_FLIGHT_DIST_REMAINING;
case SYM_AH_CH_TYPE3:
return BF_SYM_AH_CH_TYPE3;
case SYM_AH_CH_TYPE4:
return BF_SYM_AH_CH_TYPE4;
case SYM_AH_CH_TYPE5:
return BF_SYM_AH_CH_TYPE5;
case SYM_AH_CH_TYPE6:
return BF_SYM_AH_CH_TYPE6;
case SYM_AH_CH_TYPE7:
return BF_SYM_AH_CH_TYPE7;
case SYM_AH_CH_TYPE8:
return BF_SYM_AH_CH_TYPE8;
case SYM_AH_CH_AIRCRAFT0:
return BF_SYM_AH_CH_AIRCRAFT0;
case SYM_AH_CH_AIRCRAFT1:
return BF_SYM_AH_CH_AIRCRAFT1;
case SYM_AH_CH_AIRCRAFT2:
return BF_SYM_AH_CH_AIRCRAFT2;
case SYM_AH_CH_AIRCRAFT3:
return BF_SYM_AH_CH_AIRCRAFT3;
case SYM_AH_CH_AIRCRAFT4:
return BF_SYM_AH_CH_AIRCRAFT4;
case SYM_HUD_ARROWS_L1:
return BF_SYM_HUD_ARROWS_L1;
case SYM_HUD_ARROWS_L2:
return BF_SYM_HUD_ARROWS_L2;
case SYM_HUD_ARROWS_L3:
return BF_SYM_HUD_ARROWS_L3;
case SYM_HUD_ARROWS_R1:
return BF_SYM_HUD_ARROWS_R1;
case SYM_HUD_ARROWS_R2:
return BF_SYM_HUD_ARROWS_R2;
case SYM_HUD_ARROWS_R3:
return BF_SYM_HUD_ARROWS_R3;
case SYM_HUD_ARROWS_U1:
return BF_SYM_HUD_ARROWS_U1;
case SYM_HUD_ARROWS_U2:
return BF_SYM_HUD_ARROWS_U2;
case SYM_HUD_ARROWS_U3:
return BF_SYM_HUD_ARROWS_U3;
case SYM_HUD_ARROWS_D1:
return BF_SYM_HUD_ARROWS_D1;
case SYM_HUD_ARROWS_D2:
return BF_SYM_HUD_ARROWS_D2;
case SYM_HUD_ARROWS_D3:
return BF_SYM_HUD_ARROWS_D3;
*/
default:
break;
}
return '?'; // Missing/not mapped character
}
#endif
#endif

732
src/main/io/displayport_msp_dji_compat.c Normal file
View file

@ -0,0 +1,732 @@
/*
* This file is part of INAV Project.
*
* INAV is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* 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 "platform.h"
#if defined(USE_OSD) && defined(USE_MSP_DISPLAYPORT)
#ifndef DISABLE_MSP_DJI_COMPAT
#include "io/displayport_msp_dji_compat.h"
#include "io/dji_osd_symbols.h"
#include "drivers/osd_symbols.h"
uint8_t getDJICharacter(uint8_t ch, uint8_t page)
{
uint16_t ech = ch | ((page & 0x3)<< 8) ;
if (ech >= 0x20 && ech <= 0x5F) { // ASCII range
return ch;
}
if (ech >= SYM_AH_DECORATION_MIN && ech <= SYM_AH_DECORATION_MAX) {
return DJI_SYM_AH_DECORATION;
}
switch (ech) {
case SYM_RSSI:
return DJI_SYM_RSSI;
case SYM_LQ:
return 'Q';
case SYM_LAT:
return DJI_SYM_LAT;
case SYM_LON:
return DJI_SYM_LON;
case SYM_SAT_L:
return DJI_SYM_SAT_L;
case SYM_SAT_R:
return DJI_SYM_SAT_R;
case SYM_HOME_NEAR:
return DJI_SYM_HOMEFLAG;
case SYM_DEGREES:
return DJI_SYM_GPS_DEGREE;
/*
case SYM_HEADING:
return DJI_SYM_HEADING;
case SYM_SCALE:
return DJI_SYM_SCALE;
case SYM_HDP_L:
return DJI_SYM_HDP_L;
case SYM_HDP_R:
return DJI_SYM_HDP_R;
*/
case SYM_HOME:
return DJI_SYM_HOMEFLAG;
case SYM_2RSS:
return DJI_SYM_RSSI;
/*
case SYM_DB:
return DJI_SYM_DB
case SYM_DBM:
return DJI_SYM_DBM;
case SYM_SNR:
return DJI_SYM_SNR;
*/
case SYM_AH_DECORATION_UP:
return DJI_SYM_ARROW_SMALL_UP;
case SYM_AH_DECORATION_DOWN:
return DJI_SYM_ARROW_SMALL_DOWN;
case SYM_DECORATION:
return DJI_SYM_ARROW_SMALL_UP;
case SYM_DECORATION + 1: // NE pointing arrow
return DJI_SYM_ARROW_7;
case SYM_DECORATION + 2: // E pointing arrow
return DJI_SYM_ARROW_EAST;
case SYM_DECORATION + 3: // SE pointing arrow
return DJI_SYM_ARROW_3;
case SYM_DECORATION + 4: // S pointing arrow
return DJI_SYM_ARROW_SOUTH;
case SYM_DECORATION + 5: // SW pointing arrow
return DJI_SYM_ARROW_15;
case SYM_DECORATION + 6: // W pointing arrow
return DJI_SYM_ARROW_WEST;
case SYM_DECORATION + 7: // NW pointing arrow
return DJI_SYM_ARROW_11;
case SYM_VOLT:
return DJI_SYM_VOLT;
case SYM_MAH:
return DJI_SYM_MAH;
case SYM_AH_KM:
return 'K';
case SYM_AH_MI:
return 'M';
/*
case SYM_VTX_POWER:
return DJI_SYM_VTX_POWER;
case SYM_AH_NM:
return DJI_SYM_AH_NM;
case SYM_MAH_NM_0:
return DJI_SYM_MAH_NM_0;
case SYM_MAH_NM_1:
return DJI_SYM_MAH_NM_1;
case SYM_MAH_KM_0:
return DJI_SYM_MAH_KM_0;
case SYM_MAH_KM_1:
return DJI_SYM_MAH_KM_1;
case SYM_MILLIOHM:
return DJI_SYM_MILLIOHM;
*/
case SYM_BATT_FULL:
return DJI_SYM_BATT_FULL;
case SYM_BATT_5:
return DJI_SYM_BATT_5;
case SYM_BATT_4:
return DJI_SYM_BATT_4;
case SYM_BATT_3:
return DJI_SYM_BATT_3;
case SYM_BATT_2:
return DJI_SYM_BATT_2;
case SYM_BATT_1:
return DJI_SYM_BATT_1;
case SYM_BATT_EMPTY:
return DJI_SYM_BATT_EMPTY;
case SYM_AMP:
return DJI_SYM_AMP;
/*
case SYM_WH:
return DJI_SYM_WH;
case SYM_WH_KM:
return DJI_SYM_WH_KM;
case SYM_WH_MI:
return DJI_SYM_WH_MI;
case SYM_WH_NM:
return DJI_SYM_WH_NM;
*/
case SYM_WATT:
return DJI_SYM_WATT;
/*
case SYM_MW:
return DJI_SYM_MW;
case SYM_KILOWATT:
return DJI_SYM_KILOWATT;
*/
case SYM_FT:
return DJI_SYM_FT;
case SYM_ALT_FT:
return DJI_SYM_FT;
case SYM_ALT_M:
return DJI_SYM_M;
case SYM_TOTAL:
return DJI_SYM_FLY_H;
/*
case SYM_ALT_KM:
return DJI_SYM_ALT_KM;
case SYM_ALT_KFT:
return DJI_SYM_ALT_KFT;
case SYM_DIST_M:
return DJI_SYM_DIST_M;
case SYM_DIST_KM:
return DJI_SYM_DIST_KM;
case SYM_DIST_FT:
return DJI_SYM_DIST_FT;
case SYM_DIST_MI:
return DJI_SYM_DIST_MI;
case SYM_DIST_NM:
return DJI_SYM_DIST_NM;
*/
case SYM_M:
return DJI_SYM_M;
case SYM_KM:
return 'K';
case SYM_MI:
return 'M';
/*
case SYM_NM:
return DJI_SYM_NM;
*/
case SYM_WIND_HORIZONTAL:
return 'W'; // W for wind
/*
case SYM_WIND_VERTICAL:
return DJI_SYM_WIND_VERTICAL;
case SYM_3D_KT:
return DJI_SYM_3D_KT;
*/
case SYM_AIR:
return 'A'; // A for airspeed
case SYM_3D_KMH:
return DJI_SYM_KPH;
case SYM_3D_MPH:
return DJI_SYM_MPH;
case SYM_RPM:
return DJI_SYM_RPM;
case SYM_FTS:
return DJI_SYM_FTPS;
/*
case SYM_100FTM:
return DJI_SYM_100FTM;
*/
case SYM_MS:
return DJI_SYM_MPS;
case SYM_KMH:
return DJI_SYM_KPH;
case SYM_MPH:
return DJI_SYM_MPH;
/*
case SYM_KT:
return DJI_SYM_KT
case SYM_MAH_MI_0:
return DJI_SYM_MAH_MI_0;
case SYM_MAH_MI_1:
return DJI_SYM_MAH_MI_1;
*/
case SYM_THR:
return DJI_SYM_THR;
case SYM_TEMP_F:
return DJI_SYM_F;
case SYM_TEMP_C:
return DJI_SYM_C;
case SYM_BLANK:
return DJI_SYM_BLANK;
case SYM_ON_H:
return DJI_SYM_ON_H;
case SYM_FLY_H:
return DJI_SYM_FLY_H;
case SYM_ON_M:
return DJI_SYM_ON_M;
case SYM_FLY_M:
return DJI_SYM_FLY_M;
/*
case SYM_GLIDESLOPE:
return DJI_SYM_GLIDESLOPE;
case SYM_WAYPOINT:
return DJI_SYM_WAYPOINT;
case SYM_CLOCK:
return DJI_SYM_CLOCK;
case SYM_ZERO_HALF_TRAILING_DOT:
return DJI_SYM_ZERO_HALF_TRAILING_DOT;
case SYM_ZERO_HALF_LEADING_DOT:
return DJI_SYM_ZERO_HALF_LEADING_DOT;
*/
case SYM_AUTO_THR0:
return 'A';
case SYM_AUTO_THR1:
return DJI_SYM_THR;
case SYM_ROLL_LEFT:
return DJI_SYM_ROLL;
case SYM_ROLL_LEVEL:
return DJI_SYM_ROLL;
case SYM_ROLL_RIGHT:
return DJI_SYM_ROLL;
case SYM_PITCH_UP:
return DJI_SYM_PITCH;
case SYM_PITCH_DOWN:
return DJI_SYM_PITCH;
case SYM_GFORCE:
return 'G';
/*
case SYM_GFORCE_X:
return DJI_SYM_GFORCE_X;
case SYM_GFORCE_Y:
return DJI_SYM_GFORCE_Y;
case SYM_GFORCE_Z:
return DJI_SYM_GFORCE_Z;
*/
case SYM_BARO_TEMP:
return DJI_SYM_TEMPERATURE;
case SYM_IMU_TEMP:
return DJI_SYM_TEMPERATURE;
case SYM_TEMP:
return DJI_SYM_TEMPERATURE;
case SYM_ESC_TEMP:
return DJI_SYM_TEMPERATURE;
/*
case SYM_TEMP_SENSOR_FIRST:
return DJI_SYM_TEMP_SENSOR_FIRST;
case SYM_TEMP_SENSOR_LAST:
return DJI_SYM_TEMP_SENSOR_LAST;
case TEMP_SENSOR_SYM_COUNT:
return DJI_TEMP_SENSOR_SYM_COUNT;
*/
case SYM_HEADING_N:
return DJI_SYM_HEADING_N;
case SYM_HEADING_S:
return DJI_SYM_HEADING_S;
case SYM_HEADING_E:
return DJI_SYM_HEADING_E;
case SYM_HEADING_W:
return DJI_SYM_HEADING_W;
case SYM_HEADING_DIVIDED_LINE:
return DJI_SYM_HEADING_DIVIDED_LINE;
case SYM_HEADING_LINE:
return DJI_SYM_HEADING_LINE;
case SYM_MAX:
return DJI_SYM_MAX;
/*
case SYM_PROFILE:
return DJI_SYM_PROFILE;
*/
case SYM_SWITCH_INDICATOR_LOW:
return DJI_SYM_STICK_OVERLAY_SPRITE_LOW;
case SYM_SWITCH_INDICATOR_MID:
return DJI_SYM_STICK_OVERLAY_SPRITE_MID;
case SYM_SWITCH_INDICATOR_HIGH:
return DJI_SYM_STICK_OVERLAY_SPRITE_HIGH;
/*
case SYM_AH:
return DJI_SYM_AH;
case SYM_GLIDE_DIST:
return DJI_SYM_GLIDE_DIST;
case SYM_GLIDE_MINS:
return DJI_SYM_GLIDE_MINS;
case SYM_AH_V_FT_0:
return DJI_SYM_AH_V_FT_0;
case SYM_AH_V_FT_1:
return DJI_SYM_AH_V_FT_1;
case SYM_AH_V_M_0:
return DJI_SYM_AH_V_M_0;
case SYM_AH_V_M_1:
return DJI_SYM_AH_V_M_1;
case SYM_FLIGHT_MINS_REMAINING:
return DJI_SYM_FLIGHT_MINS_REMAINING;
case SYM_FLIGHT_HOURS_REMAINING:
return DJI_SYM_FLIGHT_HOURS_REMAINING;
case SYM_GROUND_COURSE:
return DJI_SYM_GROUND_COURSE;
case SYM_CROSS_TRACK_ERROR:
return DJI_SYM_CROSS_TRACK_ERROR;
case SYM_LOGO_START:
return DJI_SYM_LOGO_START;
case SYM_LOGO_WIDTH:
return DJI_SYM_LOGO_WIDTH;
case SYM_LOGO_HEIGHT:
return DJI_SYM_LOGO_HEIGHT;
*/
case SYM_AH_LEFT:
return DJI_SYM_AH_LEFT;
case SYM_AH_RIGHT:
return DJI_SYM_AH_RIGHT;
/*
case SYM_AH_DECORATION_COUNT:
return DJI_SYM_AH_DECORATION_COUNT;
*/
case SYM_AH_CH_LEFT:
case SYM_AH_CH_AIRCRAFT1:
return DJI_SYM_CROSSHAIR_LEFT;
case SYM_AH_CH_CENTER:
case SYM_AH_CH_AIRCRAFT2:
return DJI_SYM_CROSSHAIR_CENTRE;
case SYM_AH_CH_RIGHT:
case SYM_AH_CH_AIRCRAFT3:
return DJI_SYM_CROSSHAIR_RIGHT;
case SYM_AH_CH_AIRCRAFT0:
case SYM_AH_CH_AIRCRAFT4:
return DJI_SYM_BLANK;
case SYM_AH_CH_TYPE3:
return DJI_SYM_NONE;
case (SYM_AH_CH_TYPE3+1):
return DJI_SYM_SMALL_CROSSHAIR;
case (SYM_AH_CH_TYPE3+2):
return DJI_SYM_NONE;
case SYM_AH_CH_TYPE4:
return DJI_SYM_HYPHEN;
case (SYM_AH_CH_TYPE4+1):
return DJI_SYM_SMALL_CROSSHAIR;
case (SYM_AH_CH_TYPE4+2):
return DJI_SYM_HYPHEN;
case SYM_AH_CH_TYPE5:
return DJI_SYM_STICK_OVERLAY_HORIZONTAL;
case (SYM_AH_CH_TYPE5+1):
return DJI_SYM_SMALL_CROSSHAIR;
case (SYM_AH_CH_TYPE5+2):
return DJI_SYM_STICK_OVERLAY_HORIZONTAL;
case SYM_AH_CH_TYPE6:
return DJI_SYM_NONE;
case (SYM_AH_CH_TYPE6+1):
return DJI_SYM_STICK_OVERLAY_SPRITE_MID;
case (SYM_AH_CH_TYPE6+2):
return DJI_SYM_NONE;
case SYM_AH_CH_TYPE7:
return DJI_SYM_ARROW_SMALL_LEFT;
case (SYM_AH_CH_TYPE7+1):
return DJI_SYM_SMALL_CROSSHAIR;
case (SYM_AH_CH_TYPE7+2):
return DJI_SYM_ARROW_SMALL_RIGHT;
case SYM_AH_CH_TYPE8:
return DJI_SYM_AH_LEFT;
case (SYM_AH_CH_TYPE8+1):
return DJI_SYM_SMALL_CROSSHAIR;
case (SYM_AH_CH_TYPE8+2):
return DJI_SYM_AH_RIGHT;
case SYM_ARROW_UP:
return DJI_SYM_ARROW_NORTH;
case SYM_ARROW_2:
return DJI_SYM_ARROW_8;
case SYM_ARROW_3:
return DJI_SYM_ARROW_7;
case SYM_ARROW_4:
return DJI_SYM_ARROW_6;
case SYM_ARROW_RIGHT:
return DJI_SYM_ARROW_EAST;
case SYM_ARROW_6:
return DJI_SYM_ARROW_4;
case SYM_ARROW_7:
return DJI_SYM_ARROW_3;
case SYM_ARROW_8:
return DJI_SYM_ARROW_2;
case SYM_ARROW_DOWN:
return DJI_SYM_ARROW_SOUTH;
case SYM_ARROW_10:
return DJI_SYM_ARROW_16;
case SYM_ARROW_11:
return DJI_SYM_ARROW_15;
case SYM_ARROW_12:
return DJI_SYM_ARROW_14;
case SYM_ARROW_LEFT:
return DJI_SYM_ARROW_WEST;
case SYM_ARROW_14:
return DJI_SYM_ARROW_12;
case SYM_ARROW_15:
return DJI_SYM_ARROW_11;
case SYM_ARROW_16:
return DJI_SYM_ARROW_10;
case SYM_AH_H_START:
return DJI_SYM_AH_BAR9_0;
case (SYM_AH_H_START+1):
return DJI_SYM_AH_BAR9_1;
case (SYM_AH_H_START+2):
return DJI_SYM_AH_BAR9_2;
case (SYM_AH_H_START+3):
return DJI_SYM_AH_BAR9_3;
case (SYM_AH_H_START+4):
return DJI_SYM_AH_BAR9_4;
case (SYM_AH_H_START+5):
return DJI_SYM_AH_BAR9_5;
case (SYM_AH_H_START+6):
return DJI_SYM_AH_BAR9_6;
case (SYM_AH_H_START+7):
return DJI_SYM_AH_BAR9_7;
case (SYM_AH_H_START+8):
return DJI_SYM_AH_BAR9_8;
// DJI does not have vertical artificial horizon. replace with middle horizontal one
case SYM_AH_V_START:
case (SYM_AH_V_START+1):
case (SYM_AH_V_START+2):
case (SYM_AH_V_START+3):
case (SYM_AH_V_START+4):
case (SYM_AH_V_START+5):
return DJI_SYM_AH_BAR9_4;
// DJI for ESP_RADAR Symbols
case SYM_HUD_CARDINAL:
return DJI_SYM_ARROW_SOUTH;
case SYM_HUD_CARDINAL + 1:
return DJI_SYM_ARROW_16;
case SYM_HUD_CARDINAL + 2:
return DJI_SYM_ARROW_15;
case SYM_HUD_CARDINAL + 3:
return DJI_SYM_ARROW_WEST;
case SYM_HUD_CARDINAL + 4:
return DJI_SYM_ARROW_12;
case SYM_HUD_CARDINAL + 5:
return DJI_SYM_ARROW_11;
case SYM_HUD_CARDINAL + 6:
return DJI_SYM_ARROW_NORTH;
case SYM_HUD_CARDINAL + 7:
return DJI_SYM_ARROW_7;
case SYM_HUD_CARDINAL + 8:
return DJI_SYM_ARROW_6;
case SYM_HUD_CARDINAL + 9:
return DJI_SYM_ARROW_EAST;
case SYM_HUD_CARDINAL + 10:
return DJI_SYM_ARROW_3;
case SYM_HUD_CARDINAL + 11:
return DJI_SYM_ARROW_2;
case SYM_HUD_SIGNAL_0:
return DJI_SYM_AH_BAR9_1;
case SYM_HUD_SIGNAL_1:
return DJI_SYM_AH_BAR9_3;
case SYM_HUD_SIGNAL_2:
return DJI_SYM_AH_BAR9_4;
case SYM_HUD_SIGNAL_3:
return DJI_SYM_AH_BAR9_5;
case SYM_HUD_SIGNAL_4:
return DJI_SYM_AH_BAR9_7;
case SYM_VARIO_UP_2A:
return DJI_SYM_ARROW_SMALL_UP;
case SYM_VARIO_UP_1A:
return DJI_SYM_ARROW_SMALL_UP;
case SYM_VARIO_DOWN_1A:
return DJI_SYM_ARROW_SMALL_DOWN;
case SYM_VARIO_DOWN_2A:
return DJI_SYM_ARROW_SMALL_DOWN;
case SYM_ALT:
return DJI_SYM_ALTITUDE;
/*
case SYM_HUD_SIGNAL_0:
return DJI_SYM_HUD_SIGNAL_0;
case SYM_HUD_SIGNAL_1:
return DJI_SYM_HUD_SIGNAL_1;
case SYM_HUD_SIGNAL_2:
return DJI_SYM_HUD_SIGNAL_2;
case SYM_HUD_SIGNAL_3:
return DJI_SYM_HUD_SIGNAL_3;
case SYM_HUD_SIGNAL_4:
return DJI_SYM_HUD_SIGNAL_4;
case SYM_HOME_DIST:
return DJI_SYM_HOME_DIST;
case SYM_FLIGHT_DIST_REMAINING:
return DJI_SYM_FLIGHT_DIST_REMAINING;
*/
case SYM_HUD_ARROWS_L1:
return DJI_SYM_ARROW_SMALL_LEFT;
case SYM_HUD_ARROWS_L2:
return DJI_SYM_ARROW_SMALL_LEFT;
case SYM_HUD_ARROWS_L3:
return DJI_SYM_ARROW_SMALL_LEFT;
case SYM_HUD_ARROWS_R1:
return DJI_SYM_ARROW_SMALL_RIGHT;
case SYM_HUD_ARROWS_R2:
return DJI_SYM_ARROW_SMALL_RIGHT;
case SYM_HUD_ARROWS_R3:
return DJI_SYM_ARROW_SMALL_RIGHT;
case SYM_HUD_ARROWS_U1:
return DJI_SYM_ARROW_SMALL_UP;
case SYM_HUD_ARROWS_U2:
return DJI_SYM_ARROW_SMALL_UP;
case SYM_HUD_ARROWS_U3:
return DJI_SYM_ARROW_SMALL_UP;
case SYM_HUD_ARROWS_D1:
return DJI_SYM_ARROW_SMALL_DOWN;
case SYM_HUD_ARROWS_D2:
return DJI_SYM_ARROW_SMALL_DOWN;
case SYM_HUD_ARROWS_D3:
return DJI_SYM_ARROW_SMALL_DOWN;
default:
break;
}
return (osdConfig()->highlight_djis_missing_characters) ? '?' : SYM_BLANK; // Missing/not mapped character
}
#endif
#endif

12
src/main/io/displayport_msp_bf_compat.h → src/main/io/displayport_msp_dji_compat.h
View file

@ -22,15 +22,15 @@
#include "platform.h"
#if defined(USE_OSD) && defined(USE_MSP_DISPLAYPORT) && !defined(DISABLE_MSP_BF_COMPAT)
#if defined(USE_OSD) && defined(USE_MSP_DISPLAYPORT) && !defined(DISABLE_MSP_DJI_COMPAT)
#include "osd.h"
uint8_t getBfCharacter(uint8_t ch, uint8_t page);
#define isBfCompatibleVideoSystem(osdConfigPtr) (osdConfigPtr->video_system == VIDEO_SYSTEM_BFCOMPAT || osdConfigPtr->video_system == VIDEO_SYSTEM_BFCOMPAT_HD)
uint8_t getDJICharacter(uint8_t ch, uint8_t page);
#define isDJICompatibleVideoSystem(osdConfigPtr) (osdConfigPtr->video_system == VIDEO_SYSTEM_DJICOMPAT || osdConfigPtr->video_system == VIDEO_SYSTEM_DJICOMPAT_HD)
#else
#define getBfCharacter(x, page) (x)
#define getDJICharacter(x, page) (x)
#ifdef OSD_UNIT_TEST
#define isBfCompatibleVideoSystem(osdConfigPtr) (true)
#define isDJICompatibleVideoSystem(osdConfigPtr) (true)
#else
#define isBfCompatibleVideoSystem(osdConfigPtr) (false)
#define isDJICompatibleVideoSystem(osdConfigPtr) (false)
#endif
#endif

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

@ -51,7 +51,7 @@
#include "msp/msp_serial.h"
#include "displayport_msp_osd.h"
#include "displayport_msp_bf_compat.h"
#include "displayport_msp_dji_compat.h"
#define FONT_VERSION 3
@ -307,7 +307,7 @@ static int drawScreen(displayPort_t *displayPort) // 250Hz
uint8_t len = 4;
do {
bitArrayClr(dirty, pos);
subcmd[len] = isBfCompatibleVideoSystem(osdConfig()) ? getBfCharacter(screen[pos++], page): screen[pos++];
subcmd[len] = isDJICompatibleVideoSystem(osdConfig()) ? getDJICharacter(screen[pos++], page): screen[pos++];
len++;
if (bitArrayGet(dirty, pos)) {
@ -315,7 +315,7 @@ static int drawScreen(displayPort_t *displayPort) // 250Hz
}
} while (next == pos && next < endOfLine && getAttrPage(attrs[next]) == page && getAttrBlink(attrs[next]) == blink);
if (!isBfCompatibleVideoSystem(osdConfig())) {
if (!isDJICompatibleVideoSystem(osdConfig())) {
attributes |= (page << DISPLAYPORT_MSP_ATTR_FONTPAGE);
}
@ -368,7 +368,7 @@ static uint32_t txBytesFree(const displayPort_t *displayPort)
static bool getFontMetadata(displayFontMetadata_t *metadata, const displayPort_t *displayPort)
{
UNUSED(displayPort);
metadata->charCount = 512;
metadata->charCount = 1024;
metadata->version = FONT_VERSION;
return true;
}
@ -465,7 +465,7 @@ displayPort_t* mspOsdDisplayPortInit(const videoSystem_e videoSystem)
if (mspOsdSerialInit()) {
switch(videoSystem) {
case VIDEO_SYSTEM_AUTO:
case VIDEO_SYSTEM_BFCOMPAT:
case VIDEO_SYSTEM_DJICOMPAT:
case VIDEO_SYSTEM_PAL:
currentOsdMode = SD_3016;
screenRows = PAL_ROWS;
@ -486,7 +486,7 @@ displayPort_t* mspOsdDisplayPortInit(const videoSystem_e videoSystem)
screenRows = DJI_ROWS;
screenCols = DJI_COLS;
break;
case VIDEO_SYSTEM_BFCOMPAT_HD:
case VIDEO_SYSTEM_DJICOMPAT_HD:
case VIDEO_SYSTEM_AVATAR:
currentOsdMode = HD_5320;
screenRows = AVATAR_ROWS;
@ -500,10 +500,10 @@ displayPort_t* mspOsdDisplayPortInit(const videoSystem_e videoSystem)
init();
displayInit(&mspOsdDisplayPort, &mspOsdVTable);
if (osdVideoSystem == VIDEO_SYSTEM_BFCOMPAT) {
mspOsdDisplayPort.displayPortType = "MSP DisplayPort: BetaFlight Compatability mode";
} else if (osdVideoSystem == VIDEO_SYSTEM_BFCOMPAT_HD) {
mspOsdDisplayPort.displayPortType = "MSP DisplayPort: BetaFlight Compatability mode (HD)";
if (osdVideoSystem == VIDEO_SYSTEM_DJICOMPAT) {
mspOsdDisplayPort.displayPortType = "MSP DisplayPort: DJI Compatability mode";
} else if (osdVideoSystem == VIDEO_SYSTEM_DJICOMPAT_HD) {
mspOsdDisplayPort.displayPortType = "MSP DisplayPort: DJI Compatability mode (HD)";
} else {
mspOsdDisplayPort.displayPortType = "MSP DisplayPort";
}

161
src/main/io/dji_osd_symbols.h Normal file
View file

@ -0,0 +1,161 @@
/* @file max7456_symbols.h
* @brief max7456 symbols for the mwosd font set
*
* @author Nathan Tsoi nathan@vertile.com
*
* Copyright (C) 2016 Nathan Tsoi
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>
*/
#pragma once
//Misc
#define DJI_SYM_NONE 0x00
#define DJI_SYM_END_OF_FONT 0xFF
#define DJI_SYM_BLANK 0x20
#define DJI_SYM_HYPHEN 0x2D
#define DJI_SYM_BBLOG 0x10
#define DJI_SYM_HOMEFLAG 0x11
#define DJI_SYM_RPM 0x12
#define DJI_SYM_ROLL 0x14
#define DJI_SYM_PITCH 0x15
#define DJI_SYM_TEMPERATURE 0x7A
#define DJI_SYM_MAX 0x24
// GPS and navigation
#define DJI_SYM_LAT 0x89
#define DJI_SYM_LON 0x98
#define DJI_SYM_ALTITUDE 0x7F
#define DJI_SYM_OVER_HOME 0x05
// RSSI
#define DJI_SYM_RSSI 0x01
// Throttle Position (%)
#define DJI_SYM_THR 0x04
// Unit Icons (Metric)
#define DJI_SYM_M 0x0C
#define DJI_SYM_C 0x0E
// Unit Icons (Imperial)
#define DJI_SYM_F 0x0D
#define DJI_SYM_FT 0x0F
// Heading Graphics
#define DJI_SYM_HEADING_N 0x18
#define DJI_SYM_HEADING_S 0x19
#define DJI_SYM_HEADING_E 0x1A
#define DJI_SYM_HEADING_W 0x1B
#define DJI_SYM_HEADING_DIVIDED_LINE 0x1C
#define DJI_SYM_HEADING_LINE 0x1D
// AH Center screen Graphics
#define DJI_SYM_CROSSHAIR_LEFT 0x72
#define DJI_SYM_CROSSHAIR_CENTRE 0x73
#define DJI_SYM_CROSSHAIR_RIGHT 0x74
#define DJI_SYM_AH_RIGHT 0x02
#define DJI_SYM_AH_LEFT 0x03
#define DJI_SYM_AH_DECORATION 0x13
#define DJI_SYM_SMALL_CROSSHAIR 0x7E
// Satellite Graphics
#define DJI_SYM_SAT_L 0x1E
#define DJI_SYM_SAT_R 0x1F
// Direction arrows
#define DJI_SYM_ARROW_SOUTH 0x60
#define DJI_SYM_ARROW_2 0x61
#define DJI_SYM_ARROW_3 0x62
#define DJI_SYM_ARROW_4 0x63
#define DJI_SYM_ARROW_EAST 0x64
#define DJI_SYM_ARROW_6 0x65
#define DJI_SYM_ARROW_7 0x66
#define DJI_SYM_ARROW_8 0x67
#define DJI_SYM_ARROW_NORTH 0x68
#define DJI_SYM_ARROW_10 0x69
#define DJI_SYM_ARROW_11 0x6A
#define DJI_SYM_ARROW_12 0x6B
#define DJI_SYM_ARROW_WEST 0x6C
#define DJI_SYM_ARROW_14 0x6D
#define DJI_SYM_ARROW_15 0x6E
#define DJI_SYM_ARROW_16 0x6F
#define DJI_SYM_ARROW_SMALL_UP 0x75
#define DJI_SYM_ARROW_SMALL_DOWN 0x76
#define DJI_SYM_ARROW_SMALL_RIGHT 0x77
#define DJI_SYM_ARROW_SMALL_LEFT 0x78
// AH Bars
#define DJI_SYM_AH_BAR9_0 0x80
#define DJI_SYM_AH_BAR9_1 0x81
#define DJI_SYM_AH_BAR9_2 0x82
#define DJI_SYM_AH_BAR9_3 0x83
#define DJI_SYM_AH_BAR9_4 0x84
#define DJI_SYM_AH_BAR9_5 0x85
#define DJI_SYM_AH_BAR9_6 0x86
#define DJI_SYM_AH_BAR9_7 0x87
#define DJI_SYM_AH_BAR9_8 0x88
// Progress bar
#define DJI_SYM_PB_START 0x8A
#define DJI_SYM_PB_FULL 0x8B
#define DJI_SYM_PB_HALF 0x8C
#define DJI_SYM_PB_EMPTY 0x8D
#define DJI_SYM_PB_END 0x8E
#define DJI_SYM_PB_CLOSE 0x8F
// Batt evolution
#define DJI_SYM_BATT_FULL 0x90
#define DJI_SYM_BATT_5 0x91
#define DJI_SYM_BATT_4 0x92
#define DJI_SYM_BATT_3 0x93
#define DJI_SYM_BATT_2 0x94
#define DJI_SYM_BATT_1 0x95
#define DJI_SYM_BATT_EMPTY 0x96
// Batt Icons
#define DJI_SYM_MAIN_BATT 0x97
// Voltage and amperage
#define DJI_SYM_VOLT 0x06
#define DJI_SYM_AMP 0x9A
#define DJI_SYM_MAH 0x07
#define DJI_SYM_WATT 0x57 // 0x57 is 'W'
// Time
#define DJI_SYM_ON_H 0x70
#define DJI_SYM_FLY_H 0x71
#define DJI_SYM_ON_M 0x9B
#define DJI_SYM_FLY_M 0x9C
// Speed
#define DJI_SYM_KPH 0x9E
#define DJI_SYM_MPH 0x9D
#define DJI_SYM_MPS 0x9F
#define DJI_SYM_FTPS 0x99
// Stick overlays
#define DJI_SYM_STICK_OVERLAY_SPRITE_HIGH 0x08
#define DJI_SYM_STICK_OVERLAY_SPRITE_MID 0x09
#define DJI_SYM_STICK_OVERLAY_SPRITE_LOW 0x0A
#define DJI_SYM_STICK_OVERLAY_CENTER 0x0B
#define DJI_SYM_STICK_OVERLAY_VERTICAL 0x16
#define DJI_SYM_STICK_OVERLAY_HORIZONTAL 0x17
// GPS degree/minute/second symbols
#define DJI_SYM_GPS_DEGREE DJI_SYM_STICK_OVERLAY_SPRITE_HIGH // kind of looks like the degree symbol
#define DJI_SYM_GPS_MINUTE 0x27 // '
#define DJI_SYM_GPS_SECOND 0x22 // "

443
src/main/io/gimbal_serial.c Normal file
View file

@ -0,0 +1,443 @@
/*
* 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 INAV. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdio.h>
#include <stdint.h>
#include <unistd.h>
#include <string.h>
#include "platform.h"
#ifdef USE_SERIAL_GIMBAL
#include <common/crc.h>
#include <common/utils.h>
#include <common/maths.h>
#include <build/debug.h>
#include <drivers/gimbal_common.h>
#include <drivers/headtracker_common.h>
#include <drivers/serial.h>
#include <drivers/time.h>
#include <io/gimbal_serial.h>
#include <io/serial.h>
#include <rx/rx.h>
#include <fc/rc_modes.h>
#include <config/parameter_group_ids.h>
#include "settings_generated.h"
PG_REGISTER_WITH_RESET_TEMPLATE(gimbalSerialConfig_t, gimbalSerialConfig, PG_GIMBAL_SERIAL_CONFIG, 0);
PG_RESET_TEMPLATE(gimbalSerialConfig_t, gimbalSerialConfig,
.singleUart = SETTING_GIMBAL_SERIAL_SINGLE_UART_DEFAULT
);
STATIC_ASSERT(sizeof(gimbalHtkAttitudePkt_t) == 10, gimbalHtkAttitudePkt_t_size_not_10);
#define GIMBAL_SERIAL_BUFFER_SIZE 512
#ifndef GIMBAL_UNIT_TEST
static volatile uint8_t txBuffer[GIMBAL_SERIAL_BUFFER_SIZE];
#if defined(USE_HEADTRACKER) && defined(USE_HEADTRACKER_SERIAL)
static gimbalSerialHtrkState_t headTrackerState = {
.payloadSize = 0,
.attitude = {},
.state = WAITING_HDR1,
};
#endif
#endif
static serialPort_t *headTrackerPort = NULL;
static serialPort_t *gimbalPort = NULL;
gimbalVTable_t gimbalSerialVTable = {
.process = gimbalSerialProcess,
.getDeviceType = gimbalSerialGetDeviceType,
.isReady = gimbalSerialIsReady,
.hasHeadTracker = gimbalSerialHasHeadTracker,
};
static gimbalDevice_t serialGimbalDevice = {
.vTable = &gimbalSerialVTable,
.currentPanPWM = PWM_RANGE_MIDDLE
};
#if (defined(USE_HEADTRACKER) && defined(USE_HEADTRACKER_SERIAL))
static headTrackerVTable_t headTrackerVTable = {
.process = headtrackerSerialProcess,
.getDeviceType = headtrackerSerialGetDeviceType,
};
headTrackerDevice_t headTrackerDevice = {
.vTable = &headTrackerVTable,
.pan = 0,
.tilt = 0,
.roll = 0,
.expires = 0
};
#endif
gimbalDevType_e gimbalSerialGetDeviceType(const gimbalDevice_t *gimbalDevice)
{
UNUSED(gimbalDevice);
return GIMBAL_DEV_SERIAL;
}
bool gimbalSerialIsReady(const gimbalDevice_t *gimbalDevice)
{
return gimbalPort != NULL && gimbalDevice->vTable != NULL;
}
bool gimbalSerialHasHeadTracker(const gimbalDevice_t *gimbalDevice)
{
UNUSED(gimbalDevice);
return headTrackerPort || (gimbalSerialConfig()->singleUart && gimbalPort);
}
bool gimbalSerialInit(void)
{
if (gimbalSerialDetect()) {
SD(fprintf(stderr, "Setting gimbal device\n"));
gimbalCommonSetDevice(&serialGimbalDevice);
return true;
}
return false;
}
#ifdef GIMBAL_UNIT_TEST
bool gimbalSerialDetect(void)
{
return false;
}
#else
bool gimbalSerialDetect(void)
{
SD(fprintf(stderr, "[GIMBAL]: serial Detect...\n"));
serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_GIMBAL);
bool singleUart = gimbalSerialConfig()->singleUart;
if (portConfig) {
SD(fprintf(stderr, "[GIMBAL]: found port...\n"));
#if defined(USE_HEADTRACKER) && defined(USE_HEADTRACKER_SERIAL)
gimbalPort = openSerialPort(portConfig->identifier, FUNCTION_GIMBAL, singleUart ? gimbalSerialHeadTrackerReceive : NULL, singleUart ? &headTrackerState : NULL,
baudRates[portConfig->peripheral_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED);
#else
UNUSED(singleUart);
gimbalPort = openSerialPort(portConfig->identifier, FUNCTION_GIMBAL, NULL, NULL,
baudRates[portConfig->peripheral_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED);
#endif
if (gimbalPort) {
SD(fprintf(stderr, "[GIMBAL]: port open!\n"));
gimbalPort->txBuffer = txBuffer;
gimbalPort->txBufferSize = GIMBAL_SERIAL_BUFFER_SIZE;
gimbalPort->txBufferTail = 0;
gimbalPort->txBufferHead = 0;
} else {
SD(fprintf(stderr, "[GIMBAL]: port NOT open!\n"));
return false;
}
}
SD(fprintf(stderr, "[GIMBAL]: gimbalPort: %p\n", gimbalPort));
return gimbalPort;
}
#endif
#ifdef GIMBAL_UNIT_TEST
void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
{
UNUSED(gimbalDevice);
UNUSED(currentTime);
}
#else
void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
{
UNUSED(currentTime);
if (!gimbalSerialIsReady(gimbalDevice)) {
SD(fprintf(stderr, "[GIMBAL] gimbal not ready...\n"));
return;
}
gimbalHtkAttitudePkt_t attitude = {
.sync = {HTKATTITUDE_SYNC0, HTKATTITUDE_SYNC1},
.mode = GIMBAL_MODE_DEFAULT,
.pan = 0,
.tilt = 0,
.roll = 0
};
const gimbalConfig_t *cfg = gimbalConfig();
int panPWM = PWM_RANGE_MIDDLE + cfg->panTrim;
int tiltPWM = PWM_RANGE_MIDDLE + cfg->tiltTrim;
int rollPWM = PWM_RANGE_MIDDLE + cfg->rollTrim;
if (IS_RC_MODE_ACTIVE(BOXGIMBALTLOCK)) {
attitude.mode |= GIMBAL_MODE_TILT_LOCK;
}
if (IS_RC_MODE_ACTIVE(BOXGIMBALRLOCK)) {
attitude.mode |= GIMBAL_MODE_ROLL_LOCK;
}
// Follow center overrides all
if (IS_RC_MODE_ACTIVE(BOXGIMBALCENTER) || IS_RC_MODE_ACTIVE(BOXGIMBALHTRK)) {
attitude.mode = GIMBAL_MODE_FOLLOW;
}
if (rxAreFlightChannelsValid() && !IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)) {
if (cfg->panChannel > 0) {
panPWM = rxGetChannelValue(cfg->panChannel - 1) + cfg->panTrim;
panPWM = constrain(panPWM, PWM_RANGE_MIN, PWM_RANGE_MAX);
}
if (cfg->tiltChannel > 0) {
tiltPWM = rxGetChannelValue(cfg->tiltChannel - 1) + cfg->tiltTrim;
tiltPWM = constrain(tiltPWM, PWM_RANGE_MIN, PWM_RANGE_MAX);
}
if (cfg->rollChannel > 0) {
rollPWM = rxGetChannelValue(cfg->rollChannel - 1) + cfg->rollTrim;
rollPWM = constrain(rollPWM, PWM_RANGE_MIN, PWM_RANGE_MAX);
}
}
#ifdef USE_HEADTRACKER
if(IS_RC_MODE_ACTIVE(BOXGIMBALHTRK)) {
headTrackerDevice_t *dev = headTrackerCommonDevice();
if (gimbalCommonHtrkIsEnabled() && dev && headTrackerCommonIsValid(dev)) {
attitude.pan = headTrackerCommonGetPan(dev);
attitude.tilt = headTrackerCommonGetTilt(dev);
attitude.roll = headTrackerCommonGetRoll(dev);
DEBUG_SET(DEBUG_HEADTRACKING, 4, 1);
} else {
attitude.pan = constrain(gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, PWM_RANGE_MIDDLE + cfg->panTrim), HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX);
attitude.tilt = constrain(gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, PWM_RANGE_MIDDLE + cfg->tiltTrim), HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX);
attitude.roll = constrain(gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, PWM_RANGE_MIDDLE + cfg->rollTrim), HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX);
DEBUG_SET(DEBUG_HEADTRACKING, 4, -1);
}
} else {
#else
{
#endif
DEBUG_SET(DEBUG_HEADTRACKING, 4, 0);
// Radio endpoints may need to be adjusted, as it seems ot go a bit
// bananas at the extremes
attitude.pan = gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, panPWM);
attitude.tilt = gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, tiltPWM);
attitude.roll = gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, rollPWM);
}
DEBUG_SET(DEBUG_HEADTRACKING, 5, attitude.pan);
DEBUG_SET(DEBUG_HEADTRACKING, 6, attitude.tilt);
DEBUG_SET(DEBUG_HEADTRACKING, 7, attitude.roll);
attitude.sensibility = cfg->sensitivity;
uint16_t crc16 = 0;
uint8_t *b = (uint8_t *)&attitude;
for (uint8_t i = 0; i < sizeof(gimbalHtkAttitudePkt_t) - 2; i++) {
crc16 = crc16_ccitt(crc16, *(b + i));
}
attitude.crch = (crc16 >> 8) & 0xFF;
attitude.crcl = crc16 & 0xFF;
serialGimbalDevice.currentPanPWM = gimbal2pwm(attitude.pan);
serialBeginWrite(gimbalPort);
serialWriteBuf(gimbalPort, (uint8_t *)&attitude, sizeof(gimbalHtkAttitudePkt_t));
serialEndWrite(gimbalPort);
}
#endif
int16_t gimbal2pwm(int16_t value)
{
int16_t ret = 0;
ret = scaleRange(value, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX, PWM_RANGE_MIN, PWM_RANGE_MAX);
return ret;
}
int16_t gimbal_scale12(int16_t inputMin, int16_t inputMax, int16_t value)
{
int16_t ret = 0;
ret = scaleRange(value, inputMin, inputMax, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX);
return ret;
}
#ifndef GIMBAL_UNIT_TEST
#if (defined(USE_HEADTRACKER) && defined(USE_HEADTRACKER_SERIAL))
static void resetState(gimbalSerialHtrkState_t *state)
{
state->state = WAITING_HDR1;
state->payloadSize = 0;
}
static bool checkCrc(gimbalHtkAttitudePkt_t *attitude)
{
uint8_t *attitudePkt = (uint8_t *)attitude;
uint16_t crc = 0;
for(uint8_t i = 0; i < sizeof(gimbalHtkAttitudePkt_t) - 2; ++i) {
crc = crc16_ccitt(crc, attitudePkt[i]);
}
return (attitude->crch == ((crc >> 8) & 0xFF)) &&
(attitude->crcl == (crc & 0xFF));
}
void gimbalSerialHeadTrackerReceive(uint16_t c, void *data)
{
static int charCount = 0;
static int pktCount = 0;
static int errorCount = 0;
gimbalSerialHtrkState_t *state = (gimbalSerialHtrkState_t *)data;
uint8_t *payload = (uint8_t *)&(state->attitude);
payload += 2;
DEBUG_SET(DEBUG_HEADTRACKING, 0, charCount++);
DEBUG_SET(DEBUG_HEADTRACKING, 1, state->state);
switch(state->state) {
case WAITING_HDR1:
if(c == HTKATTITUDE_SYNC0) {
state->attitude.sync[0] = c;
state->state = WAITING_HDR2;
}
break;
case WAITING_HDR2:
if(c == HTKATTITUDE_SYNC1) {
state->attitude.sync[1] = c;
state->state = WAITING_PAYLOAD;
} else {
resetState(state);
}
break;
case WAITING_PAYLOAD:
payload[state->payloadSize++] = c;
if(state->payloadSize == HEADTRACKER_PAYLOAD_SIZE)
{
state->state = WAITING_CRCH;
}
break;
case WAITING_CRCH:
state->attitude.crch = c;
state->state = WAITING_CRCL;
break;
case WAITING_CRCL:
state->attitude.crcl = c;
if(checkCrc(&(state->attitude))) {
headTrackerDevice.expires = micros() + MAX_HEADTRACKER_DATA_AGE_US;
headTrackerDevice.pan = constrain(state->attitude.pan, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX);
headTrackerDevice.tilt = constrain(state->attitude.tilt, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX);
headTrackerDevice.roll = constrain(state->attitude.roll, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX);
DEBUG_SET(DEBUG_HEADTRACKING, 2, pktCount++);
} else {
DEBUG_SET(DEBUG_HEADTRACKING, 3, errorCount++);
}
resetState(state);
break;
}
}
bool gimbalSerialHeadTrackerDetect(void)
{
bool singleUart = gimbalSerialConfig()->singleUart;
SD(fprintf(stderr, "[GIMBAL_HTRK]: headtracker Detect...\n"));
serialPortConfig_t *portConfig = singleUart ? NULL : findSerialPortConfig(FUNCTION_GIMBAL_HEADTRACKER);
if (portConfig) {
SD(fprintf(stderr, "[GIMBAL_HTRK]: found port...\n"));
headTrackerPort = openSerialPort(portConfig->identifier, FUNCTION_GIMBAL_HEADTRACKER, gimbalSerialHeadTrackerReceive, &headTrackerState,
baudRates[portConfig->peripheral_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED);
if (headTrackerPort) {
SD(fprintf(stderr, "[GIMBAL_HTRK]: port open!\n"));
headTrackerPort->txBuffer = txBuffer;
headTrackerPort->txBufferSize = GIMBAL_SERIAL_BUFFER_SIZE;
headTrackerPort->txBufferTail = 0;
headTrackerPort->txBufferHead = 0;
} else {
SD(fprintf(stderr, "[GIMBAL_HTRK]: port NOT open!\n"));
return false;
}
}
SD(fprintf(stderr, "[GIMBAL]: gimbalPort: %p headTrackerPort: %p\n", gimbalPort, headTrackerPort));
return (singleUart && gimbalPort) || headTrackerPort;
}
bool gimbalSerialHeadTrackerInit(void)
{
if(headTrackerConfig()->devType == HEADTRACKER_SERIAL) {
if (gimbalSerialHeadTrackerDetect()) {
SD(fprintf(stderr, "Setting gimbal device\n"));
headTrackerCommonSetDevice(&headTrackerDevice);
return true;
}
}
return false;
}
void headtrackerSerialProcess(headTrackerDevice_t *headTrackerDevice, timeUs_t currentTimeUs)
{
UNUSED(headTrackerDevice);
UNUSED(currentTimeUs);
return;
}
headTrackerDevType_e headtrackerSerialGetDeviceType(const headTrackerDevice_t *headTrackerDevice)
{
UNUSED(headTrackerDevice);
return HEADTRACKER_SERIAL;
}
#else
void gimbalSerialHeadTrackerReceive(uint16_t c, void *data)
{
UNUSED(c);
UNUSED(data);
}
#endif
#endif
#endif

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