diff --git a/.github/PULL_REQUEST_TEMPLATE/new_target_pull_request.md b/.github/PULL_REQUEST_TEMPLATE/new_target_pull_request.md new file mode 100644 index 0000000000..a3ed117dde --- /dev/null +++ b/.github/PULL_REQUEST_TEMPLATE/new_target_pull_request.md @@ -0,0 +1,38 @@ +This pull requests adds support for ``$TARGET_NAME`` + + * [ ] Target complies with ``docs/policies/NEW_HARDWARE_POLICY.md`` + +The following items have been checked and confirmed working by an INAV developer on actual hardware: (feel free to remove non-relevant items) + + * [ ] Samples received + * [ ] Flash firmware + * [ ] Calibrate + * [ ] Orientation matches + * [ ] Gyro working + * [ ] Accel working + * [ ] Voltage correct + * [ ] Current correct + * [ ] Baro working + * [ ] Mag I2C Bus + * [ ] Additional I2C2 Buses (Airspeed/other accessories) + * [ ] UART1 + * [ ] UART2 + * [ ] UART3 + * [ ] UART4 + * [ ] UART5 + * [ ] UART6 + * [ ] UART7 + * [ ] UART8 + * [ ] Analog Camera working + * [ ] Video Out working + * [ ] OSD working + * [ ] LEDs working + * [ ] Buzzer working + * [ ] Motor outputs + * [ ] DShot support on m1-4 + * [ ] Servo outputs + * [ ] Blackbox + * [ ] PINIO1 + * [ ] PINIO2 + * [ ] PINIO3 + * [ ] PINIO4 diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 68eefc7b9c..d730344f3b 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -13,12 +13,21 @@ on: - 'CMakeLists.txt' - '*.sh' + workflow_call: + #inputs: + # release_build: + # description: 'Specifies if it is a build that should include commit hash in hex file names or not' + # default: false + # required: false + # type: boolean + + 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] + id: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14] steps: - uses: actions/checkout@v4 @@ -37,7 +46,7 @@ jobs: # 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 }') + 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 "NUM_CORES=$(grep processor /proc/cpuinfo | wc -l)" >> $GITHUB_ENV @@ -72,7 +81,7 @@ jobs: # 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 }') + 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 "NUM_CORES=$(grep processor /proc/cpuinfo | wc -l)" >> $GITHUB_ENV @@ -96,6 +105,42 @@ jobs: name: targets path: targets.txt + build-SITL-Linux-arm64: + runs-on: ubuntu-22.04-arm + 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//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)]/, "", $2); print $2 }') + echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV + echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV + echo "NUM_CORES=$(grep processor /proc/cpuinfo | wc -l)" >> $GITHUB_ENV + - name: Build SITL + run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja -j${{ env.NUM_CORES }} + - name: Strip version number + run: | + for f in build_SITL/*_SITL; do + mv $f $(echo $f | sed -e 's/_[0-9]\+\.[0-9]\+\.[0-9]\+//') + done + - name: Upload artifacts + uses: actions/upload-artifact@v4 + with: + name: ${{ env.BUILD_NAME }}_SITL-Linux-aarch64 + path: ./build_SITL/*_SITL + build-SITL-Linux: runs-on: ubuntu-latest steps: @@ -115,12 +160,17 @@ jobs: # 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 }') + 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 "NUM_CORES=$(grep processor /proc/cpuinfo | wc -l)" >> $GITHUB_ENV - name: Build SITL run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja -j${{ env.NUM_CORES }} + - name: Strip version number + run: | + for f in build_SITL/*_SITL; do + mv $f $(echo $f | sed -e 's/_[0-9]\+\.[0-9]\+\.[0-9]\+//') + done - name: Upload artifacts uses: actions/upload-artifact@v4 with: @@ -148,7 +198,7 @@ jobs: # 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 }') + 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 "NUM_CORES=$(grep processor /proc/cpuinfo | wc -l)" >> $GITHUB_ENV @@ -157,7 +207,11 @@ jobs: mkdir -p build_SITL && cd build_SITL cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -DCMAKE_OSX_ARCHITECTURES="arm64;x86_64" -G Ninja .. ninja -j4 - + - name: Strip version number + run: | + for f in build_SITL/*_SITL; do + mv -v $f $(echo $f | sed -Ee 's/_[0-9]+\.[0-9]+\.[0-9]+//') + done - name: Upload artifacts uses: actions/upload-artifact@v4 with: @@ -188,19 +242,23 @@ jobs: # 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 }') + 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 .. && ninja -j4 + - name: Strip version number + run: | + for f in ./build_SITL/*_SITL.exe; do + mv $f $(echo $f | sed -e 's/_[0-9]\+\.[0-9]\+\.[0-9]\+//') + done - name: Upload artifacts uses: actions/upload-artifact@v4 with: name: ${{ env.BUILD_NAME }}_SITL-WIN path: ./build_SITL/*.exe - test: #needs: [build] runs-on: ubuntu-latest diff --git a/.github/workflows/dev-builds.yml b/.github/workflows/dev-builds.yml deleted file mode 100644 index abc8a9eff7..0000000000 --- a/.github/workflows/dev-builds.yml +++ /dev/null @@ -1,245 +0,0 @@ -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] - - 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//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 - echo "NUM_CORES=$(grep processor /proc/cpuinfo | wc -l)" >> $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 -j${{ env.NUM_CORES }} 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//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 "NUM_CORES=$(grep processor /proc/cpuinfo | wc -l)" >> $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 -j${{ env.NUM_CORES }} - - 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//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 -j4 - - - 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//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 -j4 - - 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 }} - diff --git a/.github/workflows/nightly-build.yml b/.github/workflows/nightly-build.yml new file mode 100644 index 0000000000..89b5f7dd40 --- /dev/null +++ b/.github/workflows/nightly-build.yml @@ -0,0 +1,103 @@ +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: + name: build + uses: ./.github/workflows/ci.yml + + release: + name: Release + needs: [build] + 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: matrix-inav-* + merge-multiple: true + - name: download sitl linux + uses: actions/download-artifact@v4 + with: + path: resources/sitl/linux + pattern: inav-*SITL-Linux + merge-multiple: true + - name: download sitl linux aarch64 + uses: actions/download-artifact@v4 + with: + path: resources/sitl/linux/arm64 + pattern: inav-*SITL-Linux-aarch64 + merge-multiple: true + - name: download sitl windows + uses: actions/download-artifact@v4 + with: + path: resources/sitl/windows + pattern: inav-*SITL-WIN + merge-multiple: true + - name: download sitl mac + uses: actions/download-artifact@v4 + with: + path: resources/sitl/macos + pattern: inav-*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 }} + diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 3a8d8d1c8c..eade8435a3 100755 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -34,11 +34,29 @@ "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_BARO_DPS310", "USE_ADAPTIVE_FILTER", - "MCU_FLASH_SIZE 1024", + "MCU_FLASH_SIZE 1024" ], "configurationProvider": "ms-vscode.cmake-tools" } ], "version": 4 -} \ No newline at end of file +} diff --git a/.vscode/settings.json b/.vscode/settings.json index d1eb2357c0..5de759a0db 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -4,7 +4,7 @@ "cmath": "c", "ranges": "c", "navigation.h": "c", - "rth_trackback.h": "c" + "rth_trackback.h": "c", "platform.h": "c", "timer.h": "c", "bus.h": "c" @@ -15,4 +15,4 @@ "editor.expandTabs": true, "C_Cpp.clang_format_fallbackStyle": "{ BasedOnStyle: Google, IndentWidth: 4, BreakBeforeBraces: Mozilla }" -} +} \ No newline at end of file diff --git a/cmake/ci.cmake b/cmake/ci.cmake index 4546a8b630..daa17337e6 100644 --- a/cmake/ci.cmake +++ b/cmake/ci.cmake @@ -3,13 +3,20 @@ if(DEFINED CI_JOB_INDEX AND DEFINED CI_JOB_COUNT) message("-- configuring CI job ${job_name}/${CI_JOB_COUNT}") get_property(targets GLOBAL PROPERTY RELEASE_TARGETS) list(LENGTH targets count) + message("-- ${count} total targets") math(EXPR per_job "(${count}+${CI_JOB_COUNT}-1)/${CI_JOB_COUNT}") - message("-- ${per_job} targets per job") math(EXPR start "${CI_JOB_INDEX}*${per_job}") - list(SUBLIST targets ${start} ${per_job} ci_targets) - message("-- will build targets: ${ci_targets}") - add_custom_target(ci - ${CMAKE_COMMAND} -E true - DEPENDS ${ci_targets} -) + message("-- ${per_job} targets per job, starting at ${start}") + if(${start} LESS ${count}) + list(SUBLIST targets ${start} ${per_job} ci_targets) + message("-- will build targets: ${ci_targets}") + add_custom_target(ci + ${CMAKE_COMMAND} -E true + DEPENDS ${ci_targets} + ) + else() + add_custom_target(ci + ${CMAKE_COMMAND} -E true + ) + endif() endif() diff --git a/cmake/stm32f7.cmake b/cmake/stm32f7.cmake index 6aa2a9fd80..62819928d5 100644 --- a/cmake/stm32f7.cmake +++ b/cmake/stm32f7.cmake @@ -38,6 +38,8 @@ set(STM32F7_HAL_SRC stm32f7xx_ll_tim.c stm32f7xx_ll_usb.c stm32f7xx_ll_utils.c + stm32f7xx_hal_sd.c + stm32f7xx_ll_sdmmc.c ) list(TRANSFORM STM32F7_HAL_SRC PREPEND "${STM32F7_HAL_DIR}/Src/") @@ -74,7 +76,7 @@ main_sources(STM32F7_SRC drivers/system_stm32f7xx.c drivers/serial_uart_stm32f7xx.c drivers/serial_uart_hal.c - drivers/sdcard/sdmmc_sdio_f7xx.c + drivers/sdcard/sdmmc_sdio_hal.c ) main_sources(STM32F7_MSC_SRC diff --git a/cmake/stm32h7.cmake b/cmake/stm32h7.cmake index 81a8b2d889..34e26f409b 100644 --- a/cmake/stm32h7.cmake +++ b/cmake/stm32h7.cmake @@ -161,7 +161,7 @@ main_sources(STM32H7_SRC drivers/serial_uart_stm32h7xx.c drivers/serial_uart_hal.c drivers/sdio.h - drivers/sdcard/sdmmc_sdio_h7xx.c + drivers/sdcard/sdmmc_sdio_hal.c ) main_sources(STM32H7_MSC_SRC diff --git a/docs/Geozones.md b/docs/Geozones.md new file mode 100644 index 0000000000..483faaef05 --- /dev/null +++ b/docs/Geozones.md @@ -0,0 +1,135 @@ +# Geozones + +## Introduction +The Geozone feature allows pilots to define one or multiple areas on the map in Mission Control, to prevent accidental flying outside of allowed zones (Flight-Zones, FZ) or to avoid certain areas they are not allowed or not willing to fly in (No-Flight-Zone, NFZ). +This type of feature might be known to many pilots as a "Geofence" and despite providing the same core functionality, INAV Geozones are significantly more versatile and provide more safety and features. + +Geozones can not only inform the Pilot on the OSD, if the aircraft is approaching a geozone border, it also gives the distance and direction to the closest border and the remaining flight distance to the breach point. Additionally, it provides autonomous avoidance features, if the Aircraft is in any kind of self-leveling flight mode. +The most important feature for safety is the automatic path planning for RTH (Start Return To Home), that automatically avoids NFZ areas if possible. + +![image](https://github.com/user-attachments/assets/48e3e5cc-8517-4096-8497-399cf00ee541) + + +## Compatibility +- [INAV Version: 8.0 or later](https://github.com/iNavFlight/inav/releases) +- [INAV Configurator: 8.0 or Later](https://github.com/iNavFlight/inav-configurator/releases) +- [MWPTools: Snapshot 2024-11-15 or later](https://github.com/stronnag/mwptools) +- Only flight controller with more than 512k of Flash (STM32F405, STM32F765, STM32H743, etc.) +- Plane, Multirotor (Rover and Boat are untested at time of writing) + +## Setup Procedure +- In the INAV Configurator, switch to the Configuration Panel and enable "Geozone" in the features. +- Switch to the Mission Control Panel and you will see a new Icon to call up the Geozone Editor. If Zones are already set up, they will be loaded automatically, as soon as you enter Mission Control. + ![image](https://github.com/user-attachments/assets/23cd2149-b6a2-4945-b349-ee99863e74f0) +- Click on the "+" Symbol to add a new zone and define its parameters to your desire. +- The following Options are available: + - Shape: Configures a Zone as a Circle or a Polygon + - Type: Inclusive (FZ, green) or Exclusive (NFZ, red) + - Min. Alt (cm): lower ceiling of the Zone (0 represents the ground relative from the launch location or AMSL. No action will be taken at a minimum altitude of 0, so the aircraft can "dive out" of an Inclusive FZ on a hill. To have a Minimum Altitude action, set a negative altitude of -1 or lower) + - Max. Alt (cm): upper ceiling of the Zone (A value if 0 means no upper altitude limit) + - Action: Action to execute if an aircraft approaches the border of that Zone + - Radius: Circular Zone only, Radius of the Circle +- Move the Zone-Markers to the desired locations, to create a bordered area with the shape and size needed (Or change the radius in case of a Circular Zone) +- To add additional vertices, click on the borderline of the zone you are editing. This will add a new vertex to that line to move around. + ![image](https://github.com/user-attachments/assets/eacb6d3c-62d3-4bab-8874-1543c0a6b06d) +- Add additional Zones as you like, Zones can be separated but also overlapping (See [Limitations]( ) for details) +- After finishing the zone setup, click the "Store in EEPROM" Button to save the zones on the Flight Controller. It is important that the FC reboots after storing, as the Zones can only be used after a fresh boot process. + ![image](https://github.com/user-attachments/assets/4b278dd0-aa65-45f6-b914-22bdd753feaf) + +## Global Settings +- In the Advanced Tuning Panel, you will find additional global settings for Geozones + ![image](https://github.com/user-attachments/assets/db567521-e256-4fb6-8ca6-6e6b8b57d7a9) + - Detection Distance `geozone_detection_distance`: Defines at what distance a Geozone will be shown as a System Message if a breach is imminent. + - Avoid Altitude Range `geozone_avoid_altitude_range`: When the Aircraft approaches an NFZ that has a upper limit (can be overflown at higher altitude), INAV will climb above the Zone automatically if the altitude difference between Zone ceiling and current Aircraft altitude is less, than this value. For fixed wing, you need to consider how steep the possible climb angle is. + - Safe Altitude Distance `geozone_safe_altitude_distance`: Vertical safety margin to avoid a ceiling or floor altitude breach at high vertical speed. If your FZ Ceiling is at 100m and this value set to 10m, the aircraft will not allow you to fly above 90m and descents if the Aircraft overshoots. + - Safehome as Inclusive `geozone_safehome_as_inclusive`: Defines every Safehome location as a circular Inclusive zone with the radius of `safehome_max_distance` to allow a FZ at ground altitude (For Landings) if the general FZ around it might have a minimum altitude. + - Safehome Zone Action `geozone_safehome_zone_action`: Defines the action on zone breach if Safehome is enabled as inclusive. This is helpful for flying fields with distance or altitude restrictions for LOS Pilots. + - Multirotor Stop Distance `geozone_mr_stop_distance:`: Distance from the Border a Multirotor will stop, if the Fence Action is Avoid or Loiter (For fixed wings, this will be calculated from the Loiter-Radius of the Plane). + - No Way Home Action `geozone_no_way_home_action`: If RTH cannot find a possible route in FS or RTH modes, the Aircraft will either emergency land or fly straight back home and ignores NFZ. + +## Functions and Behaviors +- Zone Type: Inclusive + - If craft is armed inside the Inclusive FZ, everything outside that zone is considered an NFZ. + - Inclusive FZ can be combined if they overlap and will be handled as one zone. + - Overlapping multiple FZ allows different upper and lower altitude limits for each zone, as long as they still overlap in 3D Space (Both overlapping zones have to have a overlapping altitude range as well). + - Arming the aircraft outside of an Inclusive Zone is prohibited within a 2km distance to the next vertex (Distance to a border between two vertex is not checked). Arming override can be used. Arming at a distance bigger than 2km is possible. + - Arming a craft outside of an Inclusive FZ will disable all Inclusive zones. +- Zone Type: Exclusive + - Arming inside an Exclusive NFZ is prohibited. Arming override can be used but not recommended. If armed inside an NFZ the Pilot keeps control until the NFZ is left, then any configured Action will be executed on re-enter. + - Exclusive Zones can be combined and overlapped as needed. + - Exclusive NFZ with an upper or lower limit other than 0 can be overflown and underflown. The Automatic avoidance will only fly above NFZ if possible and never below. +- Actions: + - Avoid: Also called “Bounce” (only airplanes): The aircraft flies away from the boundary at the same angle it approached it, like a pool ball bouncing off the table border. Multirotor will switch into "Position Hold". + - Hold: Position in front of the boundary is held. Airplances will adjust their loiter center according to the loider radius, to stay away from the border while circling. + - RTH: Triggers Return To Home. The Failsafe RTH Procedure is executed, so RTH Trackback is also used if enabled for Failsafe situations. + - None: No action (only info in OSD). +- RTH: + - If RTH is enabled by Failsafe, User Command or Zone Action, INAV will calculate a path to the Home Location that automatically avoids NFZ and tries to stay inside the current FZ. + - If no Path can be calculated (Not able to climb over a blocking NFZ, No Intersection between FZ, too tight gaps between blocking NFZ) a configurable alternative action will be executed. + - Direct RTH: Ignores Flight zones and comes back in a direct path. + - Emergency Land: Executes a GPS enabled Emergency Landing (Circle down with horizontal position hold active on Planes). + - When direct "Line of sight" with the Home location is reached (No zones blocking path), regular RTH settings are executed. + - To abort the Smart-RTH feature and come back on a direct way, disable and Re-Enable RTH within 1 Second. This temporarily ignores all FZ and NFZ borders. +- Return to Zone: + - If the Aircraft breaches into an NFZ or out of a FZ (by avoiding tight angled Zones or flown in Acro mode and then the mode switches to any Self-Level mode), RTZ is initiated and the aircraft flies back into the last permitted zone on the shortest possible course. + +## OSD Elements +- Three dedicated OSD Elements have been added: + - Fence-Distance Horizontal shows the distance to the nearest Fence Border and the heading to that border. (ID 145) + - Fence-Distance Vertical shows the distance to the nearest ceiling or floor of a zone. (ID 146) + - Fence-Direction Vertical is an optional element to show if the nearest vertical border is above or below the aircraft. (ID 144) + ![image](https://github.com/user-attachments/assets/87dd3c5a-1046-4bd4-93af-5f8c9078b868) +- The Flight-Mode will show AUTO if the Aircraft executes any kind of Fence-Action. +- The System-Message shows the distance to a potential fence breach point, based on the current aircraft Attitude and Heading. +- Additionally, the System Message shows the current Fence Action that is Executed. + + +## Limitations +- The maximum number of dedicated zones of any type is 63. +- The maximum number of vertices of all zones combined is 127. Every circular zone uses 2 vertices while every polygon has to consist of at least 3 vertices. +- INAV can only execute one border-breach action at a time. This is especially important to consider for Airplanes that cannot hover. + - Complicated Zone setups with overlaps and tight areas can cause a loiter or "bounce" into another NFZ that was not considered before. + - This can lead to a "Return to FZ" action that tries to find the shortest path into an allowed area. +- All Geozone Actions are disabled when in Waypoint Mode. The Pilot is responsible for planning his mission accordingly, to not create a path that crosses NFZ areas. If a mission leads to such an area and the pilot disables WP mode, a "Return to FZ" action will be executed. +- All Geozone Actions are disabled in ACRO and MANUAL Mode. INAV will not take over control in these modes and only OSD Warnings are shown. +- Planning the Geozone as a polygon, needs to have the vertex numbering to be counter clockwise in ascending order. One vertex must not be dragged over another border to create crossing borders within one zone. INAV Configurator and MWP will check for that before uploading. + - Examples of Zones that are not allowed: + ![image](https://github.com/user-attachments/assets/50f1a441-39da-4f1c-9128-7375bc593fa5) +- To properly combine multiple Inclusion FZ into one area, the Zones need to overlap at 2 borders and the points where the borders touch, must be at least 2.5x Loiter-Radius apart from Airplanes at least 2.5x Multirotor Stop Distance apart for Multirotor. + - Example: + ![image](https://github.com/user-attachments/assets/cc50e24b-dc83-4408-bcba-90d6da33eb63) +- If multiple zones with different minimum and maximum altitudes are combined, they need to vertically overlap at least 50m. +- There is a chance that Smart RTH cannot find a path around NFZ areas, if there are multiple very big zones blocking the path. Due to hardware limitations, the amount of waypoints that Smart RTH can create are limited. Many Zones with very long border lines (>500m) cause additional waypoints. +- It is not recommended to edit geozones in CLI by hand as this bypasses a lot of sanity checks. Potential errors in zones will disable them or can lead to unexpected behaviors. Transferring Geozones with a DIFF between aircraft is fine. + +## CLI +The Geozone Information are stored in two separate data arrays. The first array holds the main Geozone Information and settings. The second array holds the Geozone vertices. +The following commands are available for users: + +- `geozone` without argument lists the current settings +- `geozone reset ` resets a specific geozone and all related vertices. If no ID proveded, all geozones and vertices will be deleted. +- `geozone vertex` - lists all vertices. +- `geozone vertex reset` - deletes all vertices. +- `geozone vertex reset ` - Deletes all vertices of the zone. +- `geozone vertex reset ` - Deletes the vertex with the corresponding id from a zone. + +The following information are for app-developers. _DO NOT EDIT GEOZONES MANUALLY CLI_! + +`geozone ` + +- id: 0 - 63 +- shape: 0 = Circular, 1 = Polygonal +- type: 0 = Exclusive, 1 = Inclusive +- minimum altitude: In centimetres, 0 = ground +- maximum altitude: In centimetres, 0 = infinity +- is_amsl: 0 = relative, 1 = AMSL +- fence action: 0 = None, 1 = Avoid, 2 = Position hold, 3 = Return To Home +- vertices count: 0-126 - Sanity check if number of vertices matches with configured zones + +`geozone vertex ` + +- zone id: (0-63) The zone id to which this vertex belongs +- vertex idx: Index of the vertex (0-126) +- latitude/ logitude: Longitude and latitude of the vertex. Values in decimal degrees * 1e7. Example:the value 47.562004o becomes 475620040 + + diff --git a/docs/Navigation.md b/docs/Navigation.md index 6caf4dc628..4d7b9740d9 100755 --- a/docs/Navigation.md +++ b/docs/Navigation.md @@ -5,13 +5,13 @@ The navigation system in INAV is responsible for assisting the pilot allowing al ## NAV ALTHOLD mode - altitude hold Altitude hold requires a valid source of altitude - barometer, GPS or rangefinder. The best source is chosen automatically. -In this mode THROTTLE stick controls climb rate (vertical velocity). When pilot moves stick up - quad goes up, pilot moves stick down - quad descends, you keep stick at neutral position - quad hovers. +In this mode THROTTLE stick controls climb rate (vertical velocity). When pilot moves stick up - aircraft goes up, pilot moves stick down - +aircraft descends, you keep stick at neutral position - aircraft maintains current altitude. -By default, GPS is available as an altitude source for airplanes only. Multirotor requires barometer, unless *inav_use_gps_no_baro* is enabled. ### CLI parameters affecting ALTHOLD mode: * *nav_use_midthr_for_althold* - when set to "0", firmware will remember where your throttle stick was when ALTHOLD was activated - this will be considered neutral position. When set to "1" - 50% throttle will be considered neutral position. -* *inav_use_gps_no_baro* - Multirotor only: enable althold based on GPS only, without baromemer installed. Default is OFF. + ### Related PIDs PIDs affecting altitude hold: ALT & VEL @@ -25,12 +25,12 @@ Throttle tilt compensation attempts to maintain constant vertical thrust when co ## NAV POSHOLD mode - position hold -Position hold requires GPS, accelerometer and compass sensors. Multirotor requires barometer, unless *inav_use_gps_no_baro* is enabled. Flight modes that require a compass (POSHOLD, RTH) are locked until compass is properly calibrated. +Position hold requires GPS, accelerometer and compass sensors. Multirotor requires barometer, unless is enabled. Flight modes that require a compass (POSHOLD, RTH) are locked until compass is properly calibrated. When activated, this mode will attempt to keep copter where it is (based on GPS coordinates). From INAV 2.0, POSHOLD is a full 3D position hold. Heading hold in this mode is assumed and activated automatically. ### CLI parameters affecting POSHOLD mode: * *nav_user_control_mode* - can be set to "0" (GPS_ATTI) or "1" (GPS_CRUISE), controls how firmware will respond to roll/pitch stick movement. When in GPS_ATTI mode, right stick controls attitude, when it is released, new position is recorded and held. When in GPS_CRUISE mode right stick controls velocity and firmware calculates required attitude on its own. -* *inav_use_gps_no_baro* - Multirotor only: enable althold based on GPS only, without baromemer installed. Default is OFF. + ### Related PIDs PIDs affecting position hold: POS, POSR @@ -42,7 +42,7 @@ PID meaning: Home for RTH is the position where vehicle was first armed. This position may be offset by the CLI settings `nav_rth_home_offset_distance` and `nav_rth_home_offset_direction`. This position may also be overridden with Safehomes. RTH requires accelerometer, compass and GPS sensors. -RTH requires barometer for multirotor, unless unless *inav_use_gps_no_baro* is enabled. +RTH requires barometer for multirotor. RTH will maintain altitude during the return. When home is reached, a copter will attempt automated landing. An airplane will either loiter around the home position, or attempt an automated landing, depending on your settings. When deciding what altitude to maintain, RTH has 6 different modes of operation (controlled by *nav_rth_alt_mode* and *nav_rth_altitude* cli variables): diff --git a/docs/OSD.md b/docs/OSD.md index b492e537ce..dc34fc75c8 100644 --- a/docs/OSD.md +++ b/docs/OSD.md @@ -4,7 +4,7 @@ The On Screen Display, or OSD, is a feature that overlays flight data over the v 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) +* [Custom OSD Elements](https://github.com/iNavFlight/inav/wiki/Custom-OSD-Elements) * [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) @@ -189,6 +189,13 @@ Here are the OSD Elements provided by INAV. | 156 | OSD_CUSTOM_ELEMENT_6 | 8.0.0 | | | 157 | OSD_CUSTOM_ELEMENT_7 | 8.0.0 | | | 158 | OSD_CUSTOM_ELEMENT_8 | 8.0.0 | | +| 159 | OSD_LQ_DOWNLINK | 8.0.0 | | +| 160 | OSD_RX_POWER_DOWNLINK | 8.0.0 | | +| 161 | OSD_RX_BAND | 8.0.0 | | +| 162 | OSD_RX_MODE | 8.0.0 | | +| 163 | OSD_COURSE_TO_FENCE | 8.0.0 | | +| 164 | OSD_H_DIST_TO_FENCE | 8.0.0 | | +| 165 | OSD_V_DIST_TO_FENCE | 8.0.0 | | # Pilot Logos diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md index c2314dc627..db8aa787cb 100644 --- a/docs/Programming Framework.md +++ b/docs/Programming Framework.md @@ -96,7 +96,7 @@ IPF can be edited using INAV Configurator user interface, or via CLI. To use COn | 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. | +| 50 | Delta | This returns `true` when the value of `Operand A` has changed by the value of `Operand B` or greater within 100ms. ( \|ΔA\| >= B ) | | 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. | diff --git a/docs/SITL/SITL.md b/docs/SITL/SITL.md index df3279f6d3..96ed4c19f9 100644 --- a/docs/SITL/SITL.md +++ b/docs/SITL/SITL.md @@ -62,7 +62,7 @@ All other interfaces (I2C, SPI, etc.) are not emulated. ## Remote control Multiple methods for connecting RC Controllers are available: - MSP_RX (TCP/IP) -- joystick (via simulator) +- joystick / radio attached via USB (via simulator) - serial receiver via USB to serial converter - any receiver with proxy flight controller @@ -75,6 +75,7 @@ MSP_RX is the default, 18 channels are supported over TCP/IP connection. Only 8 channels are supported. Select "SIM (SITL)" as the receiver and set up a joystick in the simulator. +Many RC transmittters (radios) can function as a joystick by plugging them in to the computer via USB, making this the simplest option in many cases. *Not available with INAV-X-Plane-HITL plugin.* diff --git a/docs/Settings.md b/docs/Settings.md index 362f65b92d..688a6b6ff7 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -674,7 +674,7 @@ Defines debug values exposed in debug variables (developer / debugging setting) ### disarm_always -Disarms the motors independently of throttle value. Setting to OFF reverts to the old behaviour of disarming only when the throttle is low. +When you switch to Disarm, do so regardless of throttle position. If this Setting is `OFF`. It will only disarm only when the throttle is low. This is similar to the previous `disarm_kill_switch` option. Default setting is the same as the old default behaviour. | Default | Min | Max | | --- | --- | --- | @@ -842,6 +842,16 @@ Q factor for dynamic notches --- +### enable_broken_o4_workaround + +DJI O4 release firmware has a broken MSP DisplayPort implementation. This enables a workaround to restore ARM detection. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + ### esc_sensor_listen_only Enable when BLHeli32 Auto Telemetry function is used. Disable in every other case @@ -908,7 +918,7 @@ EzTune filter cutoff frequency | Default | Min | Max | | --- | --- | --- | -| 110 | 10 | 300 | +| 110 | 20 | 300 | --- @@ -1178,7 +1188,7 @@ The target percentage of maximum mixer output used for determining the rates in | Default | Min | Max | | --- | --- | --- | -| 80 | 50 | 100 | +| 90 | 50 | 100 | --- @@ -1472,6 +1482,76 @@ Yaw Iterm is frozen when bank angle is above this threshold [degrees]. This solv --- +### geozone_avoid_altitude_range + +Altitude range in which an attempt is made to avoid a geozone upwards + +| Default | Min | Max | +| --- | --- | --- | +| 5000 | 0 | 1000000 | + +--- + +### geozone_detection_distance + +Distance from which a geozone is detected + +| Default | Min | Max | +| --- | --- | --- | +| 50000 | 0 | 1000000 | + +--- + +### geozone_mr_stop_distance + +Distance in which multirotors stops before the border + +| Default | Min | Max | +| --- | --- | --- | +| 15000 | 0 | 100000 | + +--- + +### geozone_no_way_home_action + +Action if RTH with active geozones is unable to calculate a course to home + +| Default | Min | Max | +| --- | --- | --- | +| RTH | | | + +--- + +### geozone_safe_altitude_distance + +Vertical distance that must be maintained to the upper and lower limits of the zone. + +| Default | Min | Max | +| --- | --- | --- | +| 1000 | 0 | 10000 | + +--- + +### geozone_safehome_as_inclusive + +Treat nearest safehome as inclusive geozone + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + +### geozone_safehome_zone_action + +Fence action for safehome zone + +| Default | Min | Max | +| --- | --- | --- | +| NONE | | | + +--- + ### gimbal_pan_channel Gimbal pan rc channel index. 0 is no channel. @@ -2002,6 +2082,16 @@ Uncertainty value for barometric sensor [cm] --- +### inav_default_alt_sensor + +Sets the default altitude sensor to use. Settings GPS and BARO always use both sensors unless there is an altitude error between the sensors that exceeds a set limit. In this case only the selected sensor will be used while the altitude error limit is exceeded. GPS error limit = 2 * inav_max_eph_epv. BARO error limit = 4 * inav_baro_epv. Settings GPS_ONLY and BARO_ONLY will use only the selected sensor even if the other sensor is working. The other sensor will only be used as a backup if the selected sensor is no longer available to use. + +| Default | Min | Max | +| --- | --- | --- | +| GPS | | | + +--- + ### inav_gravity_cal_tolerance Unarmed gravity calibration tolerance level. Won't finish the calibration until estimated gravity error falls below this value. @@ -2122,6 +2212,16 @@ Weight of barometer measurements in estimated altitude and climb rate. Setting i --- +### inav_w_z_baro_v + +Weight of barometer climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors. + +| Default | Min | Max | +| --- | --- | --- | +| 0.1 | 0 | 10 | + +--- + ### inav_w_z_gps_p Weight of GPS altitude measurements in estimated altitude. Setting is used on both airplanes and multirotors. @@ -2552,6 +2652,16 @@ Rate of the extra3 message for MAVLink telemetry --- +### mavlink_min_txbuffer + +Minimum percent of TX buffer space free, before attempting to transmit telemetry. Requuires RADIO_STATUS messages to be processed. 0 = always transmits. + +| Default | Min | Max | +| --- | --- | --- | +| 33 | 0 | 100 | + +--- + ### mavlink_pos_rate Rate of the position message for MAVLink telemetry @@ -2562,6 +2672,16 @@ Rate of the position message for MAVLink telemetry --- +### mavlink_radio_type + +Mavlink radio type. Affects how RSSI and LQ are reported on OSD. + +| Default | Min | Max | +| --- | --- | --- | +| GENERIC | | | + +--- + ### mavlink_rc_chan_rate Rate of the RC channels message for MAVLink telemetry @@ -2908,7 +3028,7 @@ If enabled, motor will stop when throttle is low on this mixer_profile | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| ON | OFF | ON | --- @@ -3008,7 +3128,7 @@ Adjusts the deceleration response of fixed wing altitude control as the target a | Default | Min | Max | | --- | --- | --- | -| 20 | 5 | 100 | +| 40 | 5 | 100 | --- @@ -3492,6 +3612,16 @@ D gain of altitude PID controller (Fixedwing) --- +### nav_fw_pos_z_ff + +FF gain of altitude PID controller (Fixedwing) + +| Default | Min | Max | +| --- | --- | --- | +| 10 | 0 | 255 | + +--- + ### nav_fw_pos_z_i I gain of altitude PID controller (Fixedwing) @@ -3508,7 +3638,7 @@ P gain of altitude PID controller (Fixedwing) | Default | Min | Max | | --- | --- | --- | -| 40 | 0 | 255 | +| 30 | 0 | 255 | --- @@ -4542,6 +4672,26 @@ Value above which the OSD current consumption element will start blinking. Measu --- +### osd_decimals_altitude + +Number of decimals for the altitude displayed in the OSD [3-5]. + +| Default | Min | Max | +| --- | --- | --- | +| 3 | 3 | 5 | + +--- + +### osd_decimals_distance + +Number of decimals for distance displayed in the OSD [3-5]. This includes distance from home, total distance, and distance remaining. + +| Default | Min | Max | +| --- | --- | --- | +| 3 | 3 | 5 | + +--- + ### osd_dist_alarm Value above which to make the OSD distance from home indicator blink (meters) @@ -4908,7 +5058,7 @@ Number of digits used for mAh precision. Currently used by mAh Used and Battery | Default | Min | Max | | --- | --- | --- | -| 4 | 4 | 6 | +| 4 | 3 | 6 | --- @@ -5118,7 +5268,7 @@ Value below which Crossfire SNR Alarm pops-up. (dB) | Default | Min | Max | | --- | --- | --- | -| 4 | -20 | 10 | +| 4 | -20 | 99 | --- diff --git a/docs/VTOL.md b/docs/VTOL.md index 3873eeb4e3..78f2a4f330 100644 --- a/docs/VTOL.md +++ b/docs/VTOL.md @@ -113,7 +113,7 @@ set mc_iterm_relax = RPY save ``` -# STEP1&2: Configuring as a Normal fixed-wing/Multi-Copter in two profiles separately +# STEP1: Configuring as a normal fixed-wing in Profile 1 1. **Select the fisrt Mixer Profile and PID Profile:** - In the CLI, switch to the mixer_profile and pid_profile you wish to set first. You can also switch mixer_profile/pid_profile through gui if with aforementioned presets loaded. @@ -132,7 +132,10 @@ save ![Alt text](Screenshots/mixerprofile_fw_mixer.png) -3. **Switch to Another Mixer Profile with PID Profile:** + +# STEP2: Configuring as a Multi-Copter in Profile 2 + +1. **Switch to Another Mixer Profile with PID Profile:** - In the CLI, switch to another mixer_profile along with the appropriate pid_profile. You can also switch mixer_profile/pid_profile through gui if with aforementioned presets loaded. ``` mixer_profile 2 @@ -141,7 +144,7 @@ save save ``` -4. **Configure the Multi-Copter/fixed-wing:** +2. **Configure the Multicopter/tricopter:** - Set up your multi-copter/fixed-wing as usual, this time for mixer_profile 2 and pid_profile 2. - Utilize the 'MAX' input in the servo mixer to tilt the motors without altering the servo midpoint. - At this stage, focus on configuring profile-specific settings. You can streamline this process by copying and pasting the default PID settings. @@ -174,7 +177,59 @@ save Conduct a bench test on the model (without props attached). The model can now switch between fixed-wing and multi-copter modes while armed. Furthermore, it is capable of mid-air switching, resulting in an immediate stall upon entering fixed-wing profile -# STEP4: Transition Mixing (Multi-Rotor Profile)(Recommended) +# STEP4: Tilting Servo Setup (Recommended) +### Setting up the tilting servos to operate correclty is crucial for correct yaw control of the craft. Using the default setup works, but will most likely result in your craft crawling forward with evey yaw input. +The steps below describe how you can fine-tune the tilting servos such as to obtian the desired result. + +1. **Set the tilt servos at 45 degrees:** + - Connect and power the tilting servos with your flight controller. + - Enter transition mode (your switch should be in the mid-position). + - Check from the Outputs tab output that the tilt servo channels are exactly at 1500μs. + - In this mode, your tilt servos should be at the 45-degree position and you can now mount the motor and prop to your tilt servo such that the angle of the motor mounting plate is at 45 degrees upwards. + - NOTE1: If you have dedicated tilt servos, you may have engraved indeces on the servos and tilting motor sassembly to help you with this step. If the servos don't end up exactly at 45 degrees due to the teeth on the servo and the control arm/plate, don't worry, this will be automatically adjusted after completing the other steps below. + - NOTE2: If you are using control rods to adjust the tilt of the servos, adjust the lenth of your control rod and the position of the control arm position the control arm as close as possible to the mid position. It will depend on the oriatation of the servo, but generally speaking, the control arm of the servo should be pointed perpendicular to the fuselage when the motor mounts are at the 45 degree setting. + +2. **Switch to Multicopter/Tricopter:** + - Assuming that you have set up your mixer similar to STEP1 and STEP2, you can now switch to the tricopter/multicopter mode and your servos should be tilting the motors upwards. If this is not the case, reverse the servo(s) in the Outputs tab such that the servo(s) is/are pointed upwards. + - It is OK for the servos not to point exactly 90 degrees upwards, but they should be as close as possible to that position. + - Also, ensure that your MAX values in the Mixer tab are at 100 and -100, so that your servo will move to the maximum position, as shown in the screenshots in STEP1 and STEP2. + +3. **Adjust the maximum throws for the Multicopter/Tricopter mode:** + - While in tricopter mode, go to the Outputs tab and adjust the endpoints MIN and MAX values such that when your motors are pointed slightly backwards. + - Rotate the prop such that it is pointed backwards towards the wing/motor mount and ensure that the gap is the same on both sides by adjusting the MIN and MAX values for the tilt servo channels. + - NOTE: You can check the distance with calipers or gauge blocks. Alternatively, you can adjust the MIN and MAX for your tilting servos such that the props are just touching the top of the wing or motor mount, and then you can increase/degrease the MIN and MAX values for each channel by the same ammount for both servos. This should ensure that you have the same gap between the tip of the prop and the wing or motor mount for both sides. + +4. **Adjust the minimum position for the Fixed-wing mode:** + - Repeat the same step as point 3 with the model in fixed-wing mode, where the servos are tilted forwards. + - For this step, you just have to make sure that the motors are pointed exactly forwards. + - You can do this by adjusting the respective MIN and MAX values in the Output tab for the tilt servo channels while in fixed-wing mode. + - NOTE: Ensuring that your servos are tilted exactly forward is a crucial step as it can cause the plane to roll slightly if that it is not the case. However, ensuring the exact aligment will depend on your specific setup. If you are using dedicated tilting motor servos rather than standard servos with control arms and pushrods, you can make sure that you are exact by measuring the distance between the front edge of the tilting servo and the of the motor mounting plate. If the disances are the uniform across each mount and same on both motors, your servos are pointed forwads correctly. + +5. **Adjsut the vertival position of the tilt servos:** + - Switch back to multicopter/tricopter mode and open the Mixer tab. + - Start adjusting the `MAX` mixer lines from STEP2 such that the servos are pointed exactly upwards. In other words, start reducing the values of 100 and -100 to something like 80 and -80 until the motors are are pointed exaxctly upwards. + - You will have to `Save & reboot` for adjustement for the changes to take effect, so be patient, take your time and don't forget to `Save & reboot`. + - Move the YAW stick to either extreme position and ensure that the servos are tilting the motors both forwards and backwards. + - NOTE: When yawing fully left, the left motor should tilt backwards and the right motor should tilt forwards. + +6. **Adjsut the throws of the tilt servos:** + - The final step is to adjust the throws of the servos such that they are the same in both directions. + - To do this, move back to the Mixer tab while in multicopter/tricopter mode and start adjusting the previously set up 50 and -50 values from the Stabilised Yaw lines. + - You can try with lower values of about 30 and -30 and then increase the values until you reach the maximum travel point. + - NOTE: The maximum is reached when both servos are moving the same ammount in oposite directions and one servo does not continue to move after the other has stopped. + +7. **Check correct operation and direction:** + - Cycle back and forth between the plane, transition and tricopter modes to make sure that your servos are maintaining the same setting. + - For the fixed wing setting, your tilt servos should point the motors exactly forwards. + - For the multicoper/tricopter mode, the tilt servos should point the motors exactly upwards and when moving the yaw stick, both servos should tilt the motors the same ammount in opposite directions. + +Optional Setup Step for Tilt Servos: + +8. **Reversing tilt servos and mixer signs:** +If you have set up the mixer as suggested in STEP1 and STEP2, you may have to deal with negative values for the mixer. You may wish to reverese a servo so that you don't have to deal with the negative signs. In that case, you may have to adjust the MIN and MAX values from point 4 again, so that your tilt servos are operating correctly. Check the operation of the servos once again for the YAW control in multicopter/tricipter mode as well as the horixontal position of the tilt servos in fixed-wing mode. + + +# STEP5: Transition Mixing (Multi-Rotor Profile)(Recommended) ### Transition Mixing is typically useful in multi-copter profile to gain airspeed in prior to entering the fixed-wing profile. When the `MIXER TRANSITION` mode is activated, the associated motor or servo will move according to your configured Transition Mixing. Please note that transition input is disabled when a navigation mode is activated. The use of Transition Mixing is necessary to enable additional features such as VTOL RTH with out stalling. diff --git a/docs/boards/Omnibus F4.md b/docs/boards/Omnibus F4.md index 6a4a4de910..e847773041 100644 --- a/docs/boards/Omnibus F4.md +++ b/docs/boards/Omnibus F4.md @@ -59,6 +59,12 @@ More target options: * Note that in multirotor configuration, servos are not enabled on S5 and S6 * Uses target **OMNIBUSF4V6** +For the Omnibus V6 and the Fireworks V2 boards, run the following command in the CLI tab to select the correct accelerometer / gyro: + +set gyro_to_use = 1 +set acc_hardware = auto +save + ### [Omnibus F4 Pro](https://inavflight.com/shop/p/OMNIBUSF4PRO) diff --git a/docs/boards/RADIOLINKF722.md b/docs/boards/RADIOLINKF722.md new file mode 100644 index 0000000000..3f9598830e --- /dev/null +++ b/docs/boards/RADIOLINKF722.md @@ -0,0 +1,3 @@ +# RADIOLINKF722 + +> Notice: DSHOT does not work on M6 output. If using M6, please use a different motor protocol. diff --git a/docs/development/Converting Betaflight Targets.md b/docs/development/Converting Betaflight Targets.md new file mode 100644 index 0000000000..47f460f634 --- /dev/null +++ b/docs/development/Converting Betaflight Targets.md @@ -0,0 +1,74 @@ +If your flight controller does not have an official INAV target, it is possible to use src/utils/bf2inav.py script to generate a good starting point for an unofficial INAV target. + +This script can read the config.h from [Betaflight's target configuration repository](https://github.com/betaflight/config) and it currently supports STM32F405, STM32F722, STM32F745, STM32H743 and AT32F435 targets. +Locate your Flight Controller target config.h in BetaFlight's repo, eg.: [config.h](https://github.com/betaflight/config/blob/master/configs/BETAFPVF405/config.h), for BETAFPVF435 target. + +It is also advisable to record the output of the timer command from BetaFlight, as it will provide useful information on `timer` usage that can be used to adjust the generated target later. + +``` +# timer +timer B08 AF3 +# pin B08: TIM10 CH1 (AF3) +timer C08 AF3 +# pin C08: TIM8 CH3 (AF3) +timer B00 AF2 +# pin B00: TIM3 CH3 (AF2) +timer B01 AF2 +# pin B01: TIM3 CH4 (AF2) +timer A03 AF1 +# pin A03: TIM2 CH4 (AF1) +timer A02 AF1 +# pin A02: TIM2 CH3 (AF1) +timer B06 AF2 +# pin B06: TIM4 CH1 (AF2) +timer A08 AF1 +# pin A08: TIM1 CH1 (AF1) +timer A09 AF1 +# pin A09: TIM1 CH2 (AF1) +timer A10 AF1 +# pin A10: TIM1 CH3 (AF1) +``` +In the above example, `pin B08: TIM10 CH1 (AF3)` means that pind to CH1. This information can be used to fix the generated timer assigned to match BetaFlight's allocation by editing the `target.c` file generated by the `bf2inav.py` script. + + +Using the BETAFPVF405 target mentioned above, to create the target now we need to: + +1. Download INAV source code and be able to build +2. Download the config.h from BetaFlight repository +3. Create a target folder that will be used as the output folder for the `bf2inav.py` script, eg: `inav/src/main/targets/BETAFPVF405` +4. Navigate to the script folder in `inav/src/utils/` +5. `python3 ./bf2inav.py -i config.h -o ../main/target/BETAFPVF405/` +6. Edit generated `target.c` and chose the correct timer definitions to match Betaflight's timer definitions. +``` +timerHardware_t timerHardware[] = { + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), + //DEF_TIM(TIM8, CH2N, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), + //DEF_TIM(TIM1, CH2N, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), + + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), + //DEF_TIM(TIM8, CH3N, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), + //DEF_TIM(TIM1, CH3N, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), + + //DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 0), + //DEF_TIM(TIM9, CH2, PA3, TIM_USE_OUTPUT_AUTO, 0, 0), + DEF_TIM(TIM2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 0), + + //DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0), + //DEF_TIM(TIM9, CH1, PA2, TIM_USE_OUTPUT_AUTO, 0, 0), + DEF_TIM(TIM2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0), + + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), + //DEF_TIM(TIM3, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), + + //DEF_TIM(TIM3, CH1, PB4, TIM_USE_BEEPER, 0, 0), + + DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), + +}; +``` +In this particular example, PA3, PA2 were changed to match Betaflight's mapping, and the timer PB4 was disabled, due to a timer conflict. Normal channels are prefered over N channels (CH1, over CH1N) or C channels in AT32 architectures. +7. Now update yout build scripts by running `cmake` and build the target you just created. The target name can be checked in the generated `CMakeLists.txt`, but should match the Betaflight target name. + +For information on how to build INAV, check the documents in the [docs/development](https://github.com/iNavFlight/inav/tree/master/docs/development) folder. \ No newline at end of file diff --git a/readme.md b/readme.md index 1c313de356..5107d4661e 100644 --- a/readme.md +++ b/readme.md @@ -1,3 +1,11 @@ +# INAV 8.0 feature freeze + +It is that time of the year again, and the time for a new INAV release is near! + +The current plan is to have a feature freeze on **15th of November 2024**. + +For a preview of what is comming, have a look at ![milestone 8.0](https://github.com/iNavFlight/inav/milestone/43). + # INAV - navigation capable flight controller # F411 PSA diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index b6ff0e1098..7f188974cd 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -576,6 +576,7 @@ main_sources(COMMON_SRC navigation/navigation_pos_estimator_flow.c navigation/navigation_private.h navigation/navigation_rover_boat.c + navigation/navigation_geozone.c navigation/sqrt_controller.c navigation/sqrt_controller.h navigation/rth_trackback.c diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 5fd9df7d97..1f8eb9bb55 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -227,6 +227,7 @@ static const OSD_Entry cmsx_menuPidAltMagEntries[] = OTHER_PIDFF_ENTRY("ALT P", &cmsx_pidPosZ.P), OTHER_PIDFF_ENTRY("ALT I", &cmsx_pidPosZ.I), OTHER_PIDFF_ENTRY("ALT D", &cmsx_pidPosZ.D), + OTHER_PIDFF_ENTRY("ALT FF", &cmsx_pidPosZ.FF), OTHER_PIDFF_ENTRY("VEL P", &cmsx_pidVelZ.P), OTHER_PIDFF_ENTRY("VEL I", &cmsx_pidVelZ.I), diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c index e99e0cc52f..0e85d0e656 100644 --- a/src/main/cms/cms_menu_osd.c +++ b/src/main/cms/cms_menu_osd.c @@ -147,10 +147,10 @@ static const OSD_Entry menuCrsfRxEntries[]= OSD_SETTING_ENTRY("LQ ALARM LEVEL", SETTING_OSD_LINK_QUALITY_ALARM), OSD_SETTING_ENTRY("SNR ALARM LEVEL", SETTING_OSD_SNR_ALARM), OSD_SETTING_ENTRY("RX SENSITIVITY", SETTING_OSD_RSSI_DBM_MIN), - OSD_ELEMENT_ENTRY("RX RSSI DBM", OSD_CRSF_RSSI_DBM), - OSD_ELEMENT_ENTRY("RX LQ", OSD_CRSF_LQ), - OSD_ELEMENT_ENTRY("RX SNR ALARM", OSD_CRSF_SNR_DB), - OSD_ELEMENT_ENTRY("TX POWER", OSD_CRSF_TX_POWER), + OSD_ELEMENT_ENTRY("RX RSSI DBM", OSD_RSSI_DBM), + OSD_ELEMENT_ENTRY("RX LQ", OSD_LQ_UPLINK), + OSD_ELEMENT_ENTRY("RX SNR ALARM", OSD_SNR_DB), + OSD_ELEMENT_ENTRY("TX POWER", OSD_TX_POWER_UPLINK), OSD_BACK_AND_END_ENTRY, }; diff --git a/src/main/common/streambuf.c b/src/main/common/streambuf.c index 7a4e8c8cc9..5a76642342 100644 --- a/src/main/common/streambuf.c +++ b/src/main/common/streambuf.c @@ -98,6 +98,11 @@ uint8_t sbufReadU8(sbuf_t *src) return *src->ptr++; } +int8_t sbufReadI8(sbuf_t *src) +{ + return *src->ptr++; +} + uint16_t sbufReadU16(sbuf_t *src) { uint16_t ret; diff --git a/src/main/common/streambuf.h b/src/main/common/streambuf.h index 74331147da..a2ac1f681a 100644 --- a/src/main/common/streambuf.h +++ b/src/main/common/streambuf.h @@ -42,6 +42,7 @@ void sbufWriteU16BigEndian(sbuf_t *dst, uint16_t val); void sbufWriteU32BigEndian(sbuf_t *dst, uint32_t val); uint8_t sbufReadU8(sbuf_t *src); +int8_t sbufReadI8(sbuf_t *src); uint16_t sbufReadU16(sbuf_t *src); uint32_t sbufReadU32(sbuf_t *src); void sbufReadData(const sbuf_t *dst, void *data, int len); diff --git a/src/main/common/vector.h b/src/main/common/vector.h index 449a0973b3..737c29d56a 100644 --- a/src/main/common/vector.h +++ b/src/main/common/vector.h @@ -29,6 +29,13 @@ typedef union { }; } fpVector3_t; +typedef union { + float v[2]; + struct { + float x,y; + }; +} fpVector2_t; + typedef struct { float m[3][3]; } fpMat3_t; @@ -116,3 +123,79 @@ static inline fpVector3_t * vectorScale(fpVector3_t * result, const fpVector3_t *result = ab; return result; } + +static inline fpVector3_t* vectorSub(fpVector3_t* result, const fpVector3_t* a, const fpVector3_t* b) +{ + fpVector3_t ab; + + ab.x = a->x - b->x; + ab.y = a->y - b->y; + ab.z = a->z - b->z; + + *result = ab; + return result; +} + +static inline float vectorDotProduct(const fpVector3_t* a, const fpVector3_t* b) +{ + return a->x * b->x + a->y * b->y + a->z * b->z; +} + +static inline float vector2NormSquared(const fpVector2_t * v) +{ + return sq(v->x) + sq(v->y); +} + +static inline fpVector2_t* vector2Normalize(fpVector2_t* result, const fpVector2_t* v) +{ + float sqa = vector2NormSquared(v); + float length = sqrtf(sqa); + if (length != 0) { + result->x = v->x / length; + result->y = v->y / length; + } else { + result->x = 0; + result->y = 0; + } + return result; +} + + +static inline fpVector2_t* vector2Sub(fpVector2_t* result, const fpVector2_t* a, const fpVector2_t* b) +{ + fpVector2_t ab; + + ab.x = a->x - b->x; + ab.y = a->y - b->y; + + *result = ab; + return result; +} + +static inline fpVector2_t * vector2Add(fpVector2_t * result, const fpVector2_t * a, const fpVector2_t * b) +{ + fpVector2_t ab; + + ab.x = a->x + b->x; + ab.y = a->y + b->y; + + *result = ab; + return result; +} + + +static inline float vector2DotProduct(const fpVector2_t* a, const fpVector2_t* b) +{ + return a->x * b->x + a->y * b->y; +} + +static inline fpVector2_t * vector2Scale(fpVector2_t * result, const fpVector2_t * a, const float b) +{ + fpVector2_t ab; + + ab.x = a->x * b; + ab.y = a->y * b; + + *result = ab; + return result; +} diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index b4062201c7..26715ab3ce 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -129,7 +129,10 @@ #define PG_GIMBAL_CONFIG 1039 #define PG_GIMBAL_SERIAL_CONFIG 1040 #define PG_HEADTRACKER_CONFIG 1041 -#define PG_INAV_END PG_HEADTRACKER_CONFIG +#define PG_GEOZONE_CONFIG 1042 +#define PG_GEOZONES 1043 +#define PG_GEOZONE_VERTICES 1044 +#define PG_INAV_END PG_GEOZONE_VERTICES // OSD configuration (subject to change) //#define PG_OSD_FONT_CONFIG 2047 diff --git a/src/main/drivers/accgyro/accgyro_icm42605.c b/src/main/drivers/accgyro/accgyro_icm42605.c index 13bc22fcee..d0c4e3490a 100644 --- a/src/main/drivers/accgyro/accgyro_icm42605.c +++ b/src/main/drivers/accgyro/accgyro_icm42605.c @@ -62,6 +62,7 @@ #define ICM42605_RA_GYRO_DATA_X1 0x25 #define ICM42605_RA_ACCEL_DATA_X1 0x1F +#define ICM42605_RA_TEMP_DATA1 0x1D #define ICM42605_RA_INT_CONFIG 0x14 #define ICM42605_INT1_MODE_PULSED (0 << 2) @@ -321,6 +322,20 @@ static bool icm42605GyroRead(gyroDev_t *gyro) return true; } +static bool icm42605ReadTemperature(gyroDev_t *gyro, int16_t * temp) +{ + uint8_t data[2]; + + const bool ack = busReadBuf(gyro->busDev, ICM42605_RA_TEMP_DATA1, data, 2); + if (!ack) { + return false; + } + // From datasheet: Temperature in Degrees Centigrade = (TEMP_DATA / 132.48) + 25 + *temp = ( int16_val_big_endian(data, 0) / 13.248 ) + 250; // Temperature stored as degC*10 + + return true; +} + bool icm42605GyroDetect(gyroDev_t *gyro) { gyro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_ICM42605, gyro->imuSensorToUse, OWNER_MPU); @@ -340,7 +355,7 @@ bool icm42605GyroDetect(gyroDev_t *gyro) gyro->initFn = icm42605AccAndGyroInit; gyro->readFn = icm42605GyroRead; gyro->intStatusFn = gyroCheckDataReady; - gyro->temperatureFn = NULL; + gyro->temperatureFn = icm42605ReadTemperature; gyro->scale = 1.0f / 16.4f; // 16.4 dps/lsb scalefactor gyro->gyroAlign = gyro->busDev->param; diff --git a/src/main/drivers/barometer/barometer_bmp085.c b/src/main/drivers/barometer/barometer_bmp085.c index 70065b85c1..a60a64e0d7 100644 --- a/src/main/drivers/barometer/barometer_bmp085.c +++ b/src/main/drivers/barometer/barometer_bmp085.c @@ -34,7 +34,9 @@ #if defined(USE_BARO_BMP085) +#if !defined(BMP085_I2C_ADDR) #define BMP085_I2C_ADDR 0x77 +#endif #define BMP085_CHIP_ID 0x55 typedef struct { diff --git a/src/main/drivers/barometer/barometer_bmp388.c b/src/main/drivers/barometer/barometer_bmp388.c index 0abdc6ab7b..1d5a8ad6e4 100644 --- a/src/main/drivers/barometer/barometer_bmp388.c +++ b/src/main/drivers/barometer/barometer_bmp388.c @@ -37,10 +37,13 @@ #include "drivers/barometer/barometer.h" #include "drivers/barometer/barometer_bmp388.h" -#if defined(USE_BARO) && (defined(USE_BARO_BMP388) || defined(USE_BARO_SPI_BMP388)) +#if defined(USE_BARO) && (defined(USE_BARO_BMP388) || defined(USE_BARO_SPI_BMP388) || defined(USE_BARO_BMP390) || defined(USE_BARO_SPI_BMP390)) +#if !defined(BMP388_I2C_ADDR) #define BMP388_I2C_ADDR (0x76) // same as BMP280/BMP180 +#endif #define BMP388_DEFAULT_CHIP_ID (0x50) // from https://github.com/BoschSensortec/BMP3-Sensor-API/blob/master/bmp3_defs.h#L130 +#define BMP390_DEFAULT_CHIP_ID (0x60) // from https://github.com/BoschSensortec/BMP3-Sensor-API/blob/master/bmp3_defs.h#L133 #define BMP388_CMD_REG (0x7E) #define BMP388_RESERVED_UPPER_REG (0x7D) @@ -314,7 +317,7 @@ static bool deviceDetect(busDevice_t * busDev) bool ack = busReadBuf(busDev, BMP388_CHIP_ID_REG, chipId, nRead); - if (ack && *pId == BMP388_DEFAULT_CHIP_ID) { + if (ack && (*pId == BMP388_DEFAULT_CHIP_ID || *pId == BMP390_DEFAULT_CHIP_ID)) { return true; } }; diff --git a/src/main/drivers/barometer/barometer_dps310.c b/src/main/drivers/barometer/barometer_dps310.c index 51e18eb12b..663dcb2711 100644 --- a/src/main/drivers/barometer/barometer_dps310.c +++ b/src/main/drivers/barometer/barometer_dps310.c @@ -61,6 +61,7 @@ #define DPS310_ID_REV_AND_PROD_ID (0x10) +#define SPL07_003_CHIP_ID (0x11) #define DPS310_RESET_BIT_SOFT_RST (0x09) // 0b1001 @@ -96,6 +97,8 @@ typedef struct { int16_t c20; // 16bit int16_t c21; // 16bit int16_t c30; // 16bit + int16_t c31; // 12bit + int16_t c40; // 12bit } calibrationCoefficients_t; typedef struct { @@ -105,6 +108,7 @@ typedef struct { } baroState_t; static baroState_t baroState; +static uint8_t chipId[1]; // Helper functions @@ -167,7 +171,10 @@ static bool deviceConfigure(busDevice_t * busDev) // 1. Read the pressure calibration coefficients (c00, c10, c20, c30, c01, c11, and c21) from the Calibration Coefficient register. // Note: The coefficients read from the coefficient register are 2's complement numbers. - uint8_t coef[18]; + + unsigned coefficientLength = chipId[0] == SPL07_003_CHIP_ID ? 21 : 18; + uint8_t coef[coefficientLength]; + if (!busReadBuf(busDev, DPS310_REG_COEF, coef, sizeof(coef))) { return false; } @@ -199,6 +206,17 @@ static bool deviceConfigure(busDevice_t * busDev) // 0x20 c30 [15:8] + 0x21 c30 [7:0] baroState.calib.c30 = getTwosComplement(((uint32_t)coef[16] << 8) | (uint32_t)coef[17], 16); + if (chipId[0] == SPL07_003_CHIP_ID) { + // 0x23 c31 [3:0] + 0x22 c31 [11:4] + baroState.calib.c31 = getTwosComplement(((uint32_t)coef[18] << 4) | (((uint32_t)coef[19] >> 4) & 0x0F), 12); + + // 0x23 c40 [11:8] + 0x24 c40 [7:0] + baroState.calib.c40 = getTwosComplement((((uint32_t)coef[19] & 0x0F) << 8) | (uint32_t)coef[20], 12); + } else { + baroState.calib.c31 = 0; + baroState.calib.c40 = 0; + } + // MEAS_CFG: Make sure the device is in IDLE mode registerWriteBits(busDev, DPS310_REG_MEAS_CFG, DPS310_MEAS_CFG_MEAS_CTRL_MASK, DPS310_MEAS_CFG_MEAS_IDLE); @@ -218,8 +236,12 @@ static bool deviceConfigure(busDevice_t * busDev) registerSetBits(busDev, DPS310_REG_PRS_CFG, DPS310_PRS_CFG_BIT_PM_RATE_32HZ | DPS310_PRS_CFG_BIT_PM_PRC_16); // TMP_CFG: temperature measurement rate (32 Hz) and oversampling (16 times) - const uint8_t TMP_COEF_SRCE = registerRead(busDev, DPS310_REG_COEF_SRCE) & DPS310_COEF_SRCE_BIT_TMP_COEF_SRCE; - registerSetBits(busDev, DPS310_REG_TMP_CFG, DPS310_TMP_CFG_BIT_TMP_RATE_32HZ | DPS310_TMP_CFG_BIT_TMP_PRC_16 | TMP_COEF_SRCE); + if (chipId[0] == SPL07_003_CHIP_ID) { + registerSetBits(busDev, DPS310_REG_TMP_CFG, DPS310_TMP_CFG_BIT_TMP_RATE_32HZ | DPS310_TMP_CFG_BIT_TMP_PRC_16); + } else { + const uint8_t TMP_COEF_SRCE = registerRead(busDev, DPS310_REG_COEF_SRCE) & DPS310_COEF_SRCE_BIT_TMP_COEF_SRCE; + registerSetBits(busDev, DPS310_REG_TMP_CFG, DPS310_TMP_CFG_BIT_TMP_RATE_32HZ | DPS310_TMP_CFG_BIT_TMP_PRC_16 | TMP_COEF_SRCE); + } // CFG_REG: set pressure and temperature result bit-shift (required when the oversampling rate is >8 times) registerSetBits(busDev, DPS310_REG_CFG_REG, DPS310_CFG_REG_BIT_T_SHIFT | DPS310_CFG_REG_BIT_P_SHIFT); @@ -265,9 +287,17 @@ static bool deviceReadMeasurement(baroDev_t *baro) const float c20 = baroState.calib.c20; const float c21 = baroState.calib.c21; const float c30 = baroState.calib.c30; + const float c31 = baroState.calib.c31; + const float c40 = baroState.calib.c40; // See section 4.9.1, How to Calculate Compensated Pressure Values, of datasheet - baroState.pressure = c00 + Praw_sc * (c10 + Praw_sc * (c20 + Praw_sc * c30)) + Traw_sc * c01 + Traw_sc * Praw_sc * (c11 + Praw_sc * c21); + // baroState.pressure = c00 + Praw_sc * (c10 + Praw_sc * (c20 + Praw_sc * c30)) + Traw_sc * c01 + Traw_sc * Praw_sc * (c11 + Praw_sc * c21); + if (chipId[0] == SPL07_003_CHIP_ID) { + baroState.pressure = c00 + Praw_sc * (c10 + Praw_sc * (c20 + Praw_sc * (c30 + Praw_sc * c40))) + Traw_sc * c01 + Traw_sc * Praw_sc * (c11 + Praw_sc * (c21 + Praw_sc * c31)); + } else { + baroState.pressure = c00 + Praw_sc * (c10 + Praw_sc * (c20 + Praw_sc * c30)) + Traw_sc * c01 + Traw_sc * Praw_sc * (c11 + Praw_sc * c21); + } + const float c0 = baroState.calib.c0; const float c1 = baroState.calib.c1; @@ -299,13 +329,11 @@ static bool deviceCalculate(baroDev_t *baro, int32_t *pressure, int32_t *tempera static bool deviceDetect(busDevice_t * busDev) { for (int retry = 0; retry < DETECTION_MAX_RETRY_COUNT; retry++) { - uint8_t chipId[1]; - delay(100); bool ack = busReadBuf(busDev, DPS310_REG_ID, chipId, 1); - if (ack && chipId[0] == DPS310_ID_REV_AND_PROD_ID) { + if (ack && (chipId[0] == DPS310_ID_REV_AND_PROD_ID || chipId[0] == SPL07_003_CHIP_ID)) { return true; } }; diff --git a/src/main/drivers/barometer/barometer_spl06.h b/src/main/drivers/barometer/barometer_spl06.h index 096cdb7a9e..a92f1c57ec 100644 --- a/src/main/drivers/barometer/barometer_spl06.h +++ b/src/main/drivers/barometer/barometer_spl06.h @@ -17,7 +17,9 @@ #pragma once +#if !defined(SPL06_I2C_ADDR) #define SPL06_I2C_ADDR 0x76 +#endif #define SPL06_DEFAULT_CHIP_ID 0x10 #define SPL06_PRESSURE_START_REG 0x00 diff --git a/src/main/drivers/gimbal_common.h b/src/main/drivers/gimbal_common.h index 879e81116a..111da760b8 100644 --- a/src/main/drivers/gimbal_common.h +++ b/src/main/drivers/gimbal_common.h @@ -61,10 +61,10 @@ 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; + int8_t sensitivity; + int16_t panTrim; + int16_t tiltTrim; + int16_t rollTrim; } gimbalConfig_t; PG_DECLARE(gimbalConfig_t, gimbalConfig); diff --git a/src/main/drivers/osd_symbols.h b/src/main/drivers/osd_symbols.h index 90c0bc9713..35b9c61a42 100644 --- a/src/main/drivers/osd_symbols.h +++ b/src/main/drivers/osd_symbols.h @@ -162,9 +162,9 @@ #define SYM_MAX 0xCE // 206 MAX symbol #define SYM_PROFILE 0xCF // 207 Profile symbol -#define SYM_SWITCH_INDICATOR_LOW 0xD0 // 208 Switch High -#define SYM_SWITCH_INDICATOR_MID 0xD1 // 209 Switch Mid -#define SYM_SWITCH_INDICATOR_HIGH 0xD2 // 210 Switch Low +#define SYM_SWITCH_INDICATOR_LOW 0xD0 // 208 Switch Indicator Down/Low - Note: Some systems have HIGH us values for a switch in the down position +#define SYM_SWITCH_INDICATOR_MID 0xD1 // 209 Switch Indicator Mid +#define SYM_SWITCH_INDICATOR_HIGH 0xD2 // 210 Switch Indicator Up/High - Note: Some systems have LOW us values for a switch in the up position #define SYM_AH 0xD3 // 211 Amphours symbol #define SYM_GLIDE_DIST 0xD4 // 212 Glide Distance #define SYM_GLIDE_MINS 0xD5 // 213 Glide Minutes @@ -234,6 +234,8 @@ #define SYM_AH_CH_CENTER 0x166 // 358 Crossair center #define SYM_FLIGHT_DIST_REMAINING 0x167 // 359 Flight distance reminaing #define SYM_ODOMETER 0x168 // 360 Odometer +#define SYM_RX_BAND 0x169 // 361 RX Band +#define SYM_RX_MODE 0x16A // 362 RX Mode #define SYM_AH_CH_TYPE3 0x190 // 400 to 402, crosshair 3 #define SYM_AH_CH_TYPE4 0x193 // 403 to 405, crosshair 4 diff --git a/src/main/drivers/sdcard/sdcard_sdio.c b/src/main/drivers/sdcard/sdcard_sdio.c index 58fc79acc1..9914719fb7 100644 --- a/src/main/drivers/sdcard/sdcard_sdio.c +++ b/src/main/drivers/sdcard/sdcard_sdio.c @@ -95,14 +95,21 @@ static bool sdcardSdio_isFunctional(void) */ static void sdcardSdio_reset(void) { - if (SD_Init() != 0) { - sdcard.failureCount++; - if (sdcard.failureCount >= SDCARD_MAX_CONSECUTIVE_FAILURES || !sdcard_isInserted()) { - sdcard.state = SDCARD_STATE_NOT_PRESENT; - } else { - sdcard.operationStartTime = millis(); - sdcard.state = SDCARD_STATE_RESET; - } + if (!sdcard_isInserted()) { + sdcard.state = SDCARD_STATE_NOT_PRESENT; + return; + } + if (SD_Init() != SD_OK) { + sdcard.state = SDCARD_STATE_NOT_PRESENT; + return; + } + + sdcard.failureCount++; + if (sdcard.failureCount >= SDCARD_MAX_CONSECUTIVE_FAILURES) { + sdcard.state = SDCARD_STATE_NOT_PRESENT; + } else { + sdcard.operationStartTime = millis(); + sdcard.state = SDCARD_STATE_RESET; } } @@ -347,8 +354,20 @@ static bool sdcardSdio_poll(void) break; // Timeout not reached yet so keep waiting } // Timeout has expired, so fall through to convert to a fatal error + FALLTHROUGH; case SDCARD_RECEIVE_ERROR: + sdcardSdio_reset(); + + if (sdcard.pendingOperation.callback) { + sdcard.pendingOperation.callback( + SDCARD_BLOCK_OPERATION_READ, + sdcard.pendingOperation.blockIndex, + NULL, + sdcard.pendingOperation.callbackData + ); + } + goto doMore; break; } @@ -561,17 +580,13 @@ void sdcardSdio_init(void) return; } - if (!SD_Initialize_LL(sdcard.dma->ref)) { + if (!SD_Initialize_LL(sdcard.dma)) { sdcard.dma = NULL; sdcard.state = SDCARD_STATE_NOT_PRESENT; return; } -#else - if (!SD_Initialize_LL(0)) { - sdcard.state = SDCARD_STATE_NOT_PRESENT; - return; - } #endif + // We don't support hot insertion if (!sdcard_isInserted()) { sdcard.state = SDCARD_STATE_NOT_PRESENT; diff --git a/src/main/drivers/sdcard/sdmmc_sdio.h b/src/main/drivers/sdcard/sdmmc_sdio.h index f488d334f9..4c4e60ca35 100644 --- a/src/main/drivers/sdcard/sdmmc_sdio.h +++ b/src/main/drivers/sdcard/sdmmc_sdio.h @@ -31,22 +31,7 @@ #if defined(USE_SDCARD_SDIO) #include "platform.h" - -#ifdef STM32F4 -#include "stm32f4xx.h" -#endif - -#ifdef STM32F7 -#include "stm32f7xx.h" -#endif - -#ifdef STM32H7 -#include "stm32h7xx.h" -#endif - -#ifdef AT32F43x -#include "at32f435_437.h" -#endif +#include "drivers/dma.h" /* SDCARD pinouts * @@ -221,9 +206,9 @@ extern SD_CardType_t SD_CardType; #ifdef AT32F43x // TODO:AT32 TARGES NOT USE SD CARD ANT TF CARD FOR NOW -void SD_Initialize_LL (dma_channel_type *dma); +void SD_Initialize_LL (DMA_t dma); #else -bool SD_Initialize_LL (DMA_Stream_TypeDef *dma); +bool SD_Initialize_LL (DMA_t dma); #endif bool SD_Init(void); bool SD_IsDetected(void); diff --git a/src/main/drivers/sdcard/sdmmc_sdio_f4xx.c b/src/main/drivers/sdcard/sdmmc_sdio_f4xx.c index 592ce4cf64..8999cdd719 100644 --- a/src/main/drivers/sdcard/sdmmc_sdio_f4xx.c +++ b/src/main/drivers/sdcard/sdmmc_sdio_f4xx.c @@ -1356,8 +1356,10 @@ static SD_Error_t SD_FindSCR(uint32_t *pSCR) /** * @brief Initialize the SDIO module, DMA, and IO */ -bool SD_Initialize_LL(DMA_Stream_TypeDef *dmaRef) +bool SD_Initialize_LL(DMA_t dma) { + DMA_Stream_TypeDef *dmaRef = dma->ref; + // Sanity check DMA stread - we only support two possible if (((uint32_t)dmaRef != (uint32_t)DMA2_Stream3) && ((uint32_t)dmaRef != (uint32_t)DMA2_Stream6)) { return false; diff --git a/src/main/drivers/sdcard/sdmmc_sdio_f7xx.c b/src/main/drivers/sdcard/sdmmc_sdio_f7xx.c deleted file mode 100644 index 20fe8ca8dd..0000000000 --- a/src/main/drivers/sdcard/sdmmc_sdio_f7xx.c +++ /dev/null @@ -1,1603 +0,0 @@ -/* - * This file is part of INAV, Cleanflight and Betaflight. - * - * INAV, Cleanflight and Betaflight are free software. You can redistribute - * this software and/or modify this software under the terms of the - * GNU General Public License as published by the Free Software - * Foundation, either version 3 of the License, or (at your option) - * any later version. - * - * Cleanflight and Betaflight are distributed in the hope that they - * will be useful, but WITHOUT ANY WARRANTY; without even the implied - * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. - * See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this software. - * - * If not, see . - */ - -/* - * Original author: Alain (https://github.com/aroyer-qc) - * Modified for BF source: Chris Hockuba (https://github.com/conkerkh) - * Modified for INAV: Konstantin (https://github.com/digitalentity) - */ - -/* Include(s) -------------------------------------------------------------------------------------------------------*/ - -#include "stdbool.h" -#include - -#include "platform.h" - -#ifdef USE_SDCARD_SDIO - -#include "sdmmc_sdio.h" -#include "stm32f7xx.h" - -#include "drivers/io.h" -#include "drivers/io_impl.h" -#include "drivers/nvic.h" -#include "drivers/time.h" -#include "drivers/rcc.h" -#include "drivers/dma.h" - -#include "build/debug.h" - - -/* Define(s) --------------------------------------------------------------------------------------------------------*/ - -//#define DMA_CHANNEL_4 ((uint32_t)0x08000000) -#define DMA_MEMORY_TO_PERIPH ((uint32_t)DMA_SxCR_DIR_0) - -//#define DMA_PERIPH_TO_MEMORY ((uint32_t)0x00) -#define DMA_MINC_ENABLE ((uint32_t)DMA_SxCR_MINC) -#define DMA_MDATAALIGN_WORD ((uint32_t)DMA_SxCR_MSIZE_1) -#define DMA_PDATAALIGN_WORD ((uint32_t)DMA_SxCR_PSIZE_1) -#define DMA_PRIORITY_LOW ((uint32_t)0x00000000U) -#define DMA_PRIORITY_MEDIUM ((uint32_t)DMA_SxCR_PL_0) -#define DMA_PRIORITY_HIGH ((uint32_t)DMA_SxCR_PL_1) -#define DMA_PRIORITY_VERY_HIGH ((uint32_t)DMA_SxCR_PL) -#define DMA_MBURST_INC4 ((uint32_t)DMA_SxCR_MBURST_0) -#define DMA_PBURST_INC4 ((uint32_t)DMA_SxCR_PBURST_0) - -#define BLOCK_SIZE ((uint32_t)(512)) - -#define IFCR_CLEAR_MASK_STREAM3 (DMA_LIFCR_CTCIF3 | DMA_LIFCR_CHTIF3 | DMA_LIFCR_CTEIF3 | DMA_LIFCR_CDMEIF3 | DMA_LIFCR_CFEIF3) -#define IFCR_CLEAR_MASK_STREAM6 (DMA_HIFCR_CTCIF6 | DMA_HIFCR_CHTIF6 | DMA_HIFCR_CTEIF6 | DMA_HIFCR_CDMEIF6 | DMA_HIFCR_CFEIF6) - -#define SDMMC_ICR_STATIC_FLAGS ((uint32_t)(SDMMC_ICR_CCRCFAILC | SDMMC_ICR_DCRCFAILC | SDMMC_ICR_CTIMEOUTC |\ - SDMMC_ICR_DTIMEOUTC | SDMMC_ICR_TXUNDERRC | SDMMC_ICR_RXOVERRC |\ - SDMMC_ICR_CMDRENDC | SDMMC_ICR_CMDSENTC | SDMMC_ICR_DATAENDC |\ - SDMMC_ICR_DBCKENDC)) - -#define SD_SOFTWARE_COMMAND_TIMEOUT ((uint32_t)0x00020000) - -#define SD_OCR_ADDR_OUT_OF_RANGE ((uint32_t)0x80000000) -#define SD_OCR_ADDR_MISALIGNED ((uint32_t)0x40000000) -#define SD_OCR_BLOCK_LEN_ERR ((uint32_t)0x20000000) -#define SD_OCR_ERASE_SEQ_ERR ((uint32_t)0x10000000) -#define SD_OCR_BAD_ERASE_PARAM ((uint32_t)0x08000000) -#define SD_OCR_WRITE_PROT_VIOLATION ((uint32_t)0x04000000) -#define SD_OCR_LOCK_UNLOCK_FAILED ((uint32_t)0x01000000) -#define SD_OCR_COM_CRC_FAILED ((uint32_t)0x00800000) -#define SD_OCR_ILLEGAL_CMD ((uint32_t)0x00400000) -#define SD_OCR_CARD_ECC_FAILED ((uint32_t)0x00200000) -#define SD_OCR_CC_ERROR ((uint32_t)0x00100000) -#define SD_OCR_GENERAL_UNKNOWN_ERROR ((uint32_t)0x00080000) -#define SD_OCR_STREAM_READ_UNDERRUN ((uint32_t)0x00040000) -#define SD_OCR_STREAM_WRITE_OVERRUN ((uint32_t)0x00020000) -#define SD_OCR_CID_CSD_OVERWRITE ((uint32_t)0x00010000) -#define SD_OCR_WP_ERASE_SKIP ((uint32_t)0x00008000) -#define SD_OCR_CARD_ECC_DISABLED ((uint32_t)0x00004000) -#define SD_OCR_ERASE_RESET ((uint32_t)0x00002000) -#define SD_OCR_AKE_SEQ_ERROR ((uint32_t)0x00000008) -#define SD_OCR_ERRORBITS ((uint32_t)0xFDFFE008) - -#define SD_R6_GENERAL_UNKNOWN_ERROR ((uint32_t)0x00002000) -#define SD_R6_ILLEGAL_CMD ((uint32_t)0x00004000) -#define SD_R6_COM_CRC_FAILED ((uint32_t)0x00008000) - -#define SD_VOLTAGE_WINDOW_SD ((uint32_t)0x80100000) -#define SD_RESP_HIGH_CAPACITY ((uint32_t)0x40000000) -#define SD_RESP_STD_CAPACITY ((uint32_t)0x00000000) -#define SD_CHECK_PATTERN ((uint32_t)0x000001AA) - -#define SD_MAX_VOLT_TRIAL ((uint32_t)0x0000FFFF) -#define SD_ALLZERO ((uint32_t)0x00000000) - -#define SD_WIDE_BUS_SUPPORT ((uint32_t)0x00040000) -#define SD_SINGLE_BUS_SUPPORT ((uint32_t)0x00010000) -#define SD_CARD_LOCKED ((uint32_t)0x02000000) - -#define SD_0TO7BITS ((uint32_t)0x000000FF) -#define SD_8TO15BITS ((uint32_t)0x0000FF00) -#define SD_16TO23BITS ((uint32_t)0x00FF0000) -#define SD_24TO31BITS ((uint32_t)0xFF000000) -#define SD_MAX_DATA_LENGTH ((uint32_t)0x01FFFFFF) - -#define SD_CCCC_ERASE ((uint32_t)0x00000020) - -#define SD_SDMMC_SEND_IF_COND ((uint32_t)SD_CMD_HS_SEND_EXT_CSD) - - - -#define SD_BUS_WIDE_1B ((uint32_t)0x00000000) -#define SD_BUS_WIDE_4B SDMMC_CLKCR_WIDBUS_0 -#define SD_BUS_WIDE_8B SDMMC_CLKCR_WIDBUS_1 - -#define SD_CMD_RESPONSE_SHORT SDMMC_CMD_WAITRESP_0 -#define SD_CMD_RESPONSE_LONG SDMMC_CMD_WAITRESP - -#define SD_DATABLOCK_SIZE_8B (SDMMC_DCTRL_DBLOCKSIZE_0|SDMMC_DCTRL_DBLOCKSIZE_1) -#define SD_DATABLOCK_SIZE_64B (SDMMC_DCTRL_DBLOCKSIZE_1|SDMMC_DCTRL_DBLOCKSIZE_2) -#define SD_DATABLOCK_SIZE_512B (SDMMC_DCTRL_DBLOCKSIZE_0|SDMMC_DCTRL_DBLOCKSIZE_3) - -#define CLKCR_CLEAR_MASK ((uint32_t)(SDMMC_CLKCR_CLKDIV | SDMMC_CLKCR_PWRSAV |\ - SDMMC_CLKCR_BYPASS | SDMMC_CLKCR_WIDBUS |\ - SDMMC_CLKCR_NEGEDGE | SDMMC_CLKCR_HWFC_EN)) - -#define DCTRL_CLEAR_MASK ((uint32_t)(SDMMC_DCTRL_DTEN | SDMMC_DCTRL_DTDIR |\ - SDMMC_DCTRL_DTMODE | SDMMC_DCTRL_DBLOCKSIZE)) - -#define CMD_CLEAR_MASK ((uint32_t)(SDMMC_CMD_CMDINDEX | SDMMC_CMD_WAITRESP |\ - SDMMC_CMD_WAITINT | SDMMC_CMD_WAITPEND |\ - SDMMC_CMD_CPSMEN | SDMMC_CMD_SDMMC1SUSPEND)) - -#define SDMMC_INIT_CLK_DIV ((uint8_t)0x76) -#define SDMMC_CLK_DIV ((uint8_t)0x00) - - -#define SD_CMD_GO_IDLE_STATE ((uint8_t)0) // Resets the SD memory card. -#define SD_CMD_SEND_OP_COND ((uint8_t)1) // Sends host capacity support information and activates the card's initialization process. -#define SD_CMD_ALL_SEND_CID ((uint8_t)2) // Asks any card connected to the host to send the CID numbers on the CMD line. -#define SD_CMD_SET_REL_ADDR ((uint8_t)3) // Asks the card to publish a new relative address (RCA). -#define SD_CMD_HS_SWITCH ((uint8_t)6) // Checks switchable function (mode 0) and switch card function (mode 1). -#define SD_CMD_SEL_DESEL_CARD ((uint8_t)7) // Selects the card by its own relative address and gets deselected by any other address -#define SD_CMD_HS_SEND_EXT_CSD ((uint8_t)8) // Sends SD Memory Card interface condition, which includes host supply voltage information - // and asks the card whether card supports voltage. -#define SD_CMD_SEND_CSD ((uint8_t)9) // Addressed card sends its card specific data (CSD) on the CMD line. -#define SD_CMD_SEND_CID ((uint8_t)10) // Addressed card sends its card identification (CID) on the CMD line. -#define SD_CMD_STOP_TRANSMISSION ((uint8_t)12) // Forces the card to stop transmission. -#define SD_CMD_SEND_STATUS ((uint8_t)13) // Addressed card sends its status register. -#define SD_CMD_SET_BLOCKLEN ((uint8_t)16) // Sets the block length (in bytes for SDSC) for all following block commands - // (read, write, lock). Default block length is fixed to 512 Bytes. Not effective - // for SDHS and SDXC. -#define SD_CMD_READ_SINGLE_BLOCK ((uint8_t)17) // Reads single block of size selected by SET_BLOCKLEN in case of SDSC, and a block of - // fixed 512 bytes in case of SDHC and SDXC. -#define SD_CMD_READ_MULT_BLOCK ((uint8_t)18) // Continuously transfers data blocks from card to host until interrupted by - // STOP_TRANSMISSION command. -#define SD_CMD_WRITE_SINGLE_BLOCK ((uint8_t)24) // Writes single block of size selected by SET_BLOCKLEN in case of SDSC, and a block of - // fixed 512 bytes in case of SDHC and SDXC. -#define SD_CMD_WRITE_MULT_BLOCK ((uint8_t)25) // Continuously writes blocks of data until a STOP_TRANSMISSION follows. -#define SD_CMD_SD_ERASE_GRP_START ((uint8_t)32) // Sets the address of the first write block to be erased. (For SD card only). -#define SD_CMD_SD_ERASE_GRP_END ((uint8_t)33) // Sets the address of the last write block of the continuous range to be erased. - // system set by switch function command (CMD6). -#define SD_CMD_ERASE ((uint8_t)38) // Reserved for SD security applications. -#define SD_CMD_FAST_IO ((uint8_t)39) // SD card doesn't support it (Reserved). -#define SD_CMD_APP_CMD ((uint8_t)55) // Indicates to the card that the next command is an application specific command rather - // than a standard command. - -/* Following commands are SD Card Specific commands. - SDMMC_APP_CMD should be sent before sending these commands. */ -#define SD_CMD_APP_SD_SET_BUSWIDTH ((uint8_t)6) // (ACMD6) Defines the data bus width to be used for data transfer. The allowed data bus - // widths are given in SCR register. -#define SD_CMD_SD_APP_STATUS ((uint8_t)13) // (ACMD13) Sends the SD status. -#define SD_CMD_SD_APP_OP_COND ((uint8_t)41) // (ACMD41) Sends host capacity support information (HCS) and asks the accessed card to - // send its operating condition register (OCR) content in the response on the CMD line. -#define SD_CMD_SD_APP_SEND_SCR ((uint8_t)51) // Reads the SD Configuration Register (SCR). - -#define SDMMC_DIR_TX 1 -#define SDMMC_DIR_RX 0 - -#define SDMMC_DMA_ST3 1 - - -/* Typedef(s) -------------------------------------------------------------------------------------------------------*/ - -typedef enum -{ - SD_SINGLE_BLOCK = 0, // Single block operation - SD_MULTIPLE_BLOCK = 1, // Multiple blocks operation -} SD_Operation_t; - -typedef struct -{ - uint32_t CSD[4]; // SD card specific data table - uint32_t CID[4]; // SD card identification number table - volatile uint32_t TransferComplete; // SD transfer complete flag in non blocking mode - volatile uint32_t TransferError; // SD transfer error flag in non blocking mode - volatile uint32_t RXCplt; // SD RX Complete is equal 0 when no transfer - volatile uint32_t TXCplt; // SD TX Complete is equal 0 when no transfer - volatile uint32_t Operation; // SD transfer operation (read/write) -} SD_Handle_t; - -typedef enum -{ - SD_CARD_READY = ((uint32_t)0x00000001), // Card state is ready - SD_CARD_IDENTIFICATION = ((uint32_t)0x00000002), // Card is in identification state - SD_CARD_STANDBY = ((uint32_t)0x00000003), // Card is in standby state - SD_CARD_TRANSFER = ((uint32_t)0x00000004), // Card is in transfer state - SD_CARD_SENDING = ((uint32_t)0x00000005), // Card is sending an operation - SD_CARD_RECEIVING = ((uint32_t)0x00000006), // Card is receiving operation information - SD_CARD_PROGRAMMING = ((uint32_t)0x00000007), // Card is in programming state - SD_CARD_DISCONNECTED = ((uint32_t)0x00000008), // Card is disconnected - SD_CARD_ERROR = ((uint32_t)0x000000FF) // Card is in error state -} SD_CardState_t; - -/* Variable(s) ------------------------------------------------------------------------------------------------------*/ - -static SD_Handle_t SD_Handle; -SD_CardInfo_t SD_CardInfo; -static uint32_t SD_Status; -static uint32_t SD_CardRCA; -SD_CardType_t SD_CardType; -static volatile uint32_t TimeOut; -DMA_Stream_TypeDef *dma_stream; - -/* Private function(s) ----------------------------------------------------------------------------------------------*/ - -static void SD_DataTransferInit (uint32_t Size, uint32_t DataBlockSize, bool IsItReadFromCard); -static SD_Error_t SD_TransmitCommand (uint32_t Command, uint32_t Argument, int8_t ResponseType); -static SD_Error_t SD_CmdResponse (uint8_t SD_CMD, int8_t ResponseType); -static void SD_GetResponse (uint32_t* pResponse); -static SD_Error_t CheckOCR_Response (uint32_t Response_R1); -static void SD_DMA_Complete (DMA_Stream_TypeDef* pDMA_Stream); -static SD_Error_t SD_InitializeCard (void); - -static SD_Error_t SD_PowerON (void); -static SD_Error_t SD_WideBusOperationConfig (uint32_t WideMode); -static SD_Error_t SD_FindSCR (uint32_t *pSCR); - -void SDMMC_DMA_ST3_IRQHandler(dmaChannelDescriptor_t *dma); -void SDMMC_DMA_ST6_IRQHandler(dmaChannelDescriptor_t *dma); - -/** -----------------------------------------------------------------------------------------------------------------*/ -/** DataTransferInit - * - * @brief Prepare the state machine for transfer - * @param SD_TransferType_e TransfertDir - * @param SD_CARD_BlockSize_e Size - */ -static void SD_DataTransferInit(uint32_t Size, uint32_t DataBlockSize, bool IsItReadFromCard) -{ - uint32_t Direction; - - SDMMC1->DTIMER = SD_DATATIMEOUT; // Set the SDMMC1 Data TimeOut value - SDMMC1->DLEN = Size; // Set the SDMMC1 DataLength value - Direction = (IsItReadFromCard == true) ? SDMMC_DCTRL_DTDIR : 0; - SDMMC1->DCTRL |= (uint32_t)(DataBlockSize | Direction | SDMMC_DCTRL_DTEN | 0x01); - return; -} - - -/** -----------------------------------------------------------------------------------------------------------------*/ -/** SD_TransmitCommand - * - * @brief Send the commande to SDMMC1 - * @param uint32_t Command - * @param uint32_t Argument Must provide the response size - * @param uint8_t ResponseType - * @retval SD Card error state - */ -static SD_Error_t SD_TransmitCommand(uint32_t Command, uint32_t Argument, int8_t ResponseType) -{ - SD_Error_t ErrorState; - - WRITE_REG(SDMMC1->ICR, SDMMC_ICR_STATIC_FLAGS); // Clear the Command Flags - WRITE_REG(SDMMC1->ARG, (uint32_t)Argument); // Set the SDMMC1 Argument value - WRITE_REG(SDMMC1->CMD, (uint32_t)(Command | SDMMC_CMD_CPSMEN)); // Set SDMMC1 command parameters - - if ((Argument == 0) && (ResponseType == 0)) { - ResponseType = -1; // Go idle command - } - - ErrorState = SD_CmdResponse(Command & SDMMC_CMD_CMDINDEX, ResponseType); - WRITE_REG(SDMMC1->ICR, SDMMC_ICR_STATIC_FLAGS); // Clear the Command Flags - - return ErrorState; -} - - -/** -----------------------------------------------------------------------------------------------------------------*/ -/** - * @brief Checks for error conditions for any response. - * - R2 (CID or CSD) response. - * - R3 (OCR) response. - * - * @param SD_CMD: The sent command Index - * @retval SD Card error state - */ -static SD_Error_t SD_CmdResponse(uint8_t SD_CMD, int8_t ResponseType) -{ - uint32_t Response_R1; - uint32_t TimeOut; - uint32_t Flag; - - if (ResponseType == -1) { - Flag = SDMMC_STA_CMDSENT; - } - else { - Flag = SDMMC_STA_CCRCFAIL | SDMMC_STA_CMDREND | SDMMC_STA_CTIMEOUT; - } - - TimeOut = SD_SOFTWARE_COMMAND_TIMEOUT; - do { - SD_Status = SDMMC1->STA; - TimeOut--; - } while (((SD_Status & Flag) == 0) && (TimeOut > 0)); - - if (ResponseType <= 0) { - if (TimeOut == 0){ - return SD_CMD_RSP_TIMEOUT; - } - else { - return SD_OK; - } - } - - if ((SDMMC1->STA & SDMMC_STA_CTIMEOUT) != 0) { - return SD_CMD_RSP_TIMEOUT; - } - if (ResponseType == 3) { - if (TimeOut == 0) { - return SD_CMD_RSP_TIMEOUT; // Card is not V2.0 compliant or card does not support the set voltage range - } - else { - return SD_OK; // Card is SD V2.0 compliant - } - } - - if ((SDMMC1->STA & SDMMC_STA_CCRCFAIL) != 0) { - return SD_CMD_CRC_FAIL; - } - if (ResponseType == 2) { - return SD_OK; - } - if ((uint8_t)SDMMC1->RESPCMD != SD_CMD) { - return SD_ILLEGAL_CMD; // Check if response is of desired command - } - - Response_R1 = SDMMC1->RESP1; // We have received response, retrieve it for analysis - - if (ResponseType == 1) { - return CheckOCR_Response(Response_R1); - } - else if (ResponseType == 6) { - if ((Response_R1 & (SD_R6_GENERAL_UNKNOWN_ERROR | SD_R6_ILLEGAL_CMD | SD_R6_COM_CRC_FAILED)) == SD_ALLZERO) { - SD_CardRCA = Response_R1; - } - if ((Response_R1 & SD_R6_GENERAL_UNKNOWN_ERROR) == SD_R6_GENERAL_UNKNOWN_ERROR) { - return SD_GENERAL_UNKNOWN_ERROR; - } - if ((Response_R1 & SD_R6_ILLEGAL_CMD) == SD_R6_ILLEGAL_CMD) { - return SD_ILLEGAL_CMD; - } - if ((Response_R1 & SD_R6_COM_CRC_FAILED) == SD_R6_COM_CRC_FAILED) { - return SD_COM_CRC_FAILED; - } - } - - return SD_OK; -} - - -/** -----------------------------------------------------------------------------------------------------------------*/ -/** - * @brief Analyze the OCR response and return the appropriate error code - * @param Response_R1: OCR Response code - * @retval SD Card error state - */ -static SD_Error_t CheckOCR_Response(uint32_t Response_R1) -{ - if ((Response_R1 & SD_OCR_ERRORBITS) == SD_ALLZERO) return SD_OK; - if ((Response_R1 & SD_OCR_ADDR_OUT_OF_RANGE) == SD_OCR_ADDR_OUT_OF_RANGE) return SD_ADDR_OUT_OF_RANGE; - if ((Response_R1 & SD_OCR_ADDR_MISALIGNED) == SD_OCR_ADDR_MISALIGNED) return SD_ADDR_MISALIGNED; - if ((Response_R1 & SD_OCR_BLOCK_LEN_ERR) == SD_OCR_BLOCK_LEN_ERR) return SD_BLOCK_LEN_ERR; - if ((Response_R1 & SD_OCR_ERASE_SEQ_ERR) == SD_OCR_ERASE_SEQ_ERR) return SD_ERASE_SEQ_ERR; - if ((Response_R1 & SD_OCR_BAD_ERASE_PARAM) == SD_OCR_BAD_ERASE_PARAM) return SD_BAD_ERASE_PARAM; - if ((Response_R1 & SD_OCR_WRITE_PROT_VIOLATION) == SD_OCR_WRITE_PROT_VIOLATION) return SD_WRITE_PROT_VIOLATION; - if ((Response_R1 & SD_OCR_LOCK_UNLOCK_FAILED) == SD_OCR_LOCK_UNLOCK_FAILED) return SD_LOCK_UNLOCK_FAILED; - if ((Response_R1 & SD_OCR_COM_CRC_FAILED) == SD_OCR_COM_CRC_FAILED) return SD_COM_CRC_FAILED; - if ((Response_R1 & SD_OCR_ILLEGAL_CMD) == SD_OCR_ILLEGAL_CMD) return SD_ILLEGAL_CMD; - if ((Response_R1 & SD_OCR_CARD_ECC_FAILED) == SD_OCR_CARD_ECC_FAILED) return SD_CARD_ECC_FAILED; - if ((Response_R1 & SD_OCR_CC_ERROR) == SD_OCR_CC_ERROR) return SD_CC_ERROR; - if ((Response_R1 & SD_OCR_GENERAL_UNKNOWN_ERROR) == SD_OCR_GENERAL_UNKNOWN_ERROR)return SD_GENERAL_UNKNOWN_ERROR; - if ((Response_R1 & SD_OCR_STREAM_READ_UNDERRUN) == SD_OCR_STREAM_READ_UNDERRUN) return SD_STREAM_READ_UNDERRUN; - if ((Response_R1 & SD_OCR_STREAM_WRITE_OVERRUN) == SD_OCR_STREAM_WRITE_OVERRUN) return SD_STREAM_WRITE_OVERRUN; - if ((Response_R1 & SD_OCR_CID_CSD_OVERWRITE) == SD_OCR_CID_CSD_OVERWRITE) return SD_CID_CSD_OVERWRITE; - if ((Response_R1 & SD_OCR_WP_ERASE_SKIP) == SD_OCR_WP_ERASE_SKIP) return SD_WP_ERASE_SKIP; - if ((Response_R1 & SD_OCR_CARD_ECC_DISABLED) == SD_OCR_CARD_ECC_DISABLED) return SD_CARD_ECC_DISABLED; - if ((Response_R1 & SD_OCR_ERASE_RESET) == SD_OCR_ERASE_RESET) return SD_ERASE_RESET; - if ((Response_R1 & SD_OCR_AKE_SEQ_ERROR) == SD_OCR_AKE_SEQ_ERROR) return SD_AKE_SEQ_ERROR; - - return SD_OK; -} - - -/** -----------------------------------------------------------------------------------------------------------------*/ -/** GetResponse - * - * @brief Get response from SD device - * @param uint32_t* pResponse - */ -static void SD_GetResponse(uint32_t* pResponse) -{ - pResponse[0] = SDMMC1->RESP1; - pResponse[1] = SDMMC1->RESP2; - pResponse[2] = SDMMC1->RESP3; - pResponse[3] = SDMMC1->RESP4; -} - - -/** -----------------------------------------------------------------------------------------------------------------*/ -/** - * @brief SD DMA transfer complete RX and TX callback. - * @param DMA_Stream_TypeDef* pDMA_Stream - */ -static void SD_DMA_Complete(DMA_Stream_TypeDef* pDMA_Stream) -{ - if (SD_Handle.RXCplt) { - if (SD_Handle.Operation == ((SDMMC_DIR_RX << 1) | SD_MULTIPLE_BLOCK)) { - /* Send stop command in multiblock write */ - SD_TransmitCommand((SD_CMD_STOP_TRANSMISSION | SD_CMD_RESPONSE_SHORT), 0, 1); - } - - /* Disable the DMA transfer for transmit request by setting the DMAEN bit - in the SD DCTRL register */ - SDMMC1->DCTRL &= (uint32_t)~((uint32_t)SDMMC_DCTRL_DMAEN); - - /* Clear all the static flags */ - SDMMC1->ICR = SDMMC_ICR_STATIC_FLAGS; - - /* Clear flag */ - SD_Handle.RXCplt = 0; - - /* Disable the stream */ - pDMA_Stream->CR &= ~DMA_SxCR_EN; - } - else { - /* Enable Dataend IE */ - SDMMC1->MASK |= SDMMC_MASK_DATAENDIE; - } -} - - -/** -----------------------------------------------------------------------------------------------------------------*/ -/** - * @brief Initializes all cards or single card as the case may be Card(s) come - * into standby state. - * @retval SD Card error state - */ -static SD_Error_t SD_InitializeCard(void) -{ - SD_Error_t ErrorState = SD_OK; - - if ((SDMMC1->POWER & SDMMC_POWER_PWRCTRL) != 0) { // Power off - if (SD_CardType != SD_SECURE_DIGITAL_IO) { - // Send CMD2 ALL_SEND_CID - if ((ErrorState = SD_TransmitCommand((SD_CMD_ALL_SEND_CID | SD_CMD_RESPONSE_LONG), 0, 2)) != SD_OK) { - return ErrorState; - } - - // Get Card identification number data - SD_GetResponse(SD_Handle.CID); - } - - if ((SD_CardType == SD_STD_CAPACITY_V1_1) || (SD_CardType == SD_STD_CAPACITY_V2_0) || - (SD_CardType == SD_SECURE_DIGITAL_IO_COMBO) || (SD_CardType == SD_HIGH_CAPACITY)) { - // Send CMD3 SET_REL_ADDR with argument 0 - // SD Card publishes its RCA. - if ((ErrorState = SD_TransmitCommand((SD_CMD_SET_REL_ADDR | SD_CMD_RESPONSE_SHORT), 0, 6)) != SD_OK) { - return ErrorState; - } - } - - if (SD_CardType != SD_SECURE_DIGITAL_IO) { - // Send CMD9 SEND_CSD with argument as card's RCA - if ((ErrorState = SD_TransmitCommand((SD_CMD_SEND_CSD | SD_CMD_RESPONSE_LONG), SD_CardRCA, 2)) == SD_OK) { - // Get Card Specific Data - SD_GetResponse(SD_Handle.CSD); - } - } - } - else { - ErrorState = SD_REQUEST_NOT_APPLICABLE; - } - - return ErrorState; -} - - -/** -----------------------------------------------------------------------------------------------------------------*/ -/** - * @brief Prepre the DMA transfer - * @param pDMA: DMA Stream to use for the DMA operation - * @param pBuffer: Pointer to the buffer that will contain the data to transmit - * @param BlockSize: The SD card Data block size - * @note BlockSize must be 512 bytes. - * @param NumberOfBlocks: Number of blocks to write - * @retval SD Card error state - */ -static void SD_StartBlockTransfert(uint32_t* pBuffer, uint32_t BlockSize, uint32_t NumberOfBlocks, uint8_t dir) -{ - DMA_Stream_TypeDef *pDMA = dma_stream; - - SDMMC1->DCTRL = 0; // Initialize data control register - SD_Handle.TransferComplete = 0; // Initialize handle flags - SD_Handle.TransferError = SD_OK; - SD_Handle.Operation = (NumberOfBlocks > 1) ? SD_MULTIPLE_BLOCK : SD_SINGLE_BLOCK; // Initialize SD Read operation - SD_Handle.Operation |= dir << 1; - SDMMC1->MASK = 0; - - if (dir == SDMMC_DIR_RX) { - SDMMC1->MASK |= (SDMMC_MASK_DCRCFAILIE | SDMMC_MASK_DTIMEOUTIE | // Enable transfer interrupts - SDMMC_MASK_DATAENDIE | SDMMC_MASK_RXOVERRIE); - } - else { - SDMMC1->MASK |= (SDMMC_MASK_DCRCFAILIE | SDMMC_MASK_DTIMEOUTIE | // Enable transfer interrupts - SDMMC_MASK_TXUNDERRIE); - } - - if (dir == SDMMC_DIR_TX) { - SDMMC1->DCTRL |= SDMMC_DCTRL_DMAEN; // Enable SDMMC1 DMA transfer - } - - pDMA->CR &= ~DMA_SxCR_EN; // Disable the Peripheral - while (pDMA->CR & DMA_SxCR_EN); - - pDMA->NDTR = (uint32_t) (BlockSize * NumberOfBlocks) / 4; // Configure DMA Stream data length - pDMA->M0AR = (uint32_t) pBuffer; // Configure DMA Stream memory address - - if (dir == SDMMC_DIR_RX) { - pDMA->CR &= ~(0x01U << 6U); // Sets peripheral to memory - } - else { - pDMA->CR |= DMA_MEMORY_TO_PERIPH; // Sets memory to peripheral - } - - // Clear the transfer error flag - if (dma_stream == DMA2_Stream3) { - DMA2->LIFCR = DMA_LIFCR_CTEIF3 | DMA_LIFCR_CDMEIF3 | DMA_LIFCR_CFEIF3 | DMA_LIFCR_CHTIF3 | DMA_LIFCR_CTCIF3; - } - else { - DMA2->HIFCR = DMA_HIFCR_CTEIF6 | DMA_HIFCR_CDMEIF6 | DMA_HIFCR_CFEIF6 | DMA_HIFCR_CHTIF6 | DMA_HIFCR_CTCIF6; - } - - pDMA->CR |= DMA_SxCR_TCIE | DMA_SxCR_HTIE | DMA_SxCR_TEIE | DMA_SxCR_DMEIE; // Enable all interrupts - pDMA->FCR |= DMA_SxFCR_FEIE; - pDMA->CR |= DMA_SxCR_EN; // Enable the Peripheral - - if (dir == SDMMC_DIR_RX) { - SDMMC1->DCTRL |= SDMMC_DCTRL_DMAEN; // Enable SDMMC1 DMA transfer - } -} - - -/** -----------------------------------------------------------------------------------------------------------------*/ -/** - * @brief Reads block(s) from a specified address in a card. The Data transfer - * is managed by DMA mode. - * @note This API should be followed by the function SD_CheckOperation() - * to check the completion of the read process - * @param pReadBuffer: Pointer to the buffer that will contain the received data - * @param ReadAddr: Address from where data is to be read - * @param BlockSize: SD card Data block size - * @note BlockSize must be 512 bytes. - * @param NumberOfBlocks: Number of blocks to read. - * @retval SD Card error state - */ -SD_Error_t SD_ReadBlocks_DMA(uint64_t ReadAddress, uint32_t *buffer, uint32_t BlockSize, uint32_t NumberOfBlocks) -{ - SD_Error_t ErrorState; - uint32_t CmdIndex; - SD_Handle.RXCplt = 1; - - //printf("Reading at %ld into %p %ld blocks\n", (uint32_t)ReadAddress, (void*)buffer, NumberOfBlocks); - - if (SD_CardType != SD_HIGH_CAPACITY) { - ReadAddress *= 512; - } - - SD_StartBlockTransfert(buffer, BlockSize, NumberOfBlocks, SDMMC_DIR_RX); - - // Configure the SD DPSM (Data Path State Machine) - SD_DataTransferInit(BlockSize * NumberOfBlocks, SD_DATABLOCK_SIZE_512B, true); - - // Set Block Size for Card - ErrorState = SD_TransmitCommand((SD_CMD_SET_BLOCKLEN | SD_CMD_RESPONSE_SHORT), BlockSize, 1); - - // Send CMD18 READ_MULT_BLOCK with argument data address - // or send CMD17 READ_SINGLE_BLOCK depending on number of block - uint8_t retries = 10; - CmdIndex = (NumberOfBlocks > 1) ? SD_CMD_READ_MULT_BLOCK : SD_CMD_READ_SINGLE_BLOCK; - - do { - ErrorState = SD_TransmitCommand((CmdIndex | SD_CMD_RESPONSE_SHORT), (uint32_t)ReadAddress, 1); - if (ErrorState != SD_OK && retries--) { - ErrorState = SD_TransmitCommand((SD_CMD_APP_CMD | SD_CMD_RESPONSE_SHORT), 0, 1); - } - } while (ErrorState != SD_OK && retries); - - if (ErrorState != SD_OK) { - SD_Handle.RXCplt = 0; - } - - // Update the SD transfer error in SD handle - SD_Handle.TransferError = ErrorState; - - return ErrorState; -} - - -/** -----------------------------------------------------------------------------------------------------------------*/ -/** - * @brief Writes block(s) to a specified address in a card. The Data transfer - * is managed by DMA mode. - * @note This API should be followed by the function SD_CheckOperation() - * to check the completion of the write process (by SD current status polling). - * @param pWriteBuffer: pointer to the buffer that will contain the data to transmit - * @param WriteAddress: Address from where data is to be read - * @param BlockSize: the SD card Data block size - * @note BlockSize must be 512 bytes. - * @param NumberOfBlocks: Number of blocks to write - * @retval SD Card error state - */ -SD_Error_t SD_WriteBlocks_DMA(uint64_t WriteAddress, uint32_t *buffer, uint32_t BlockSize, uint32_t NumberOfBlocks) -{ - SD_Error_t ErrorState; - uint32_t CmdIndex; - SD_Handle.TXCplt = 1; - - if (SD_CardType != SD_HIGH_CAPACITY) { - WriteAddress *= 512; - } - - // Check number of blocks command - // Send CMD24 WRITE_SINGLE_BLOCK - // Send CMD25 WRITE_MULT_BLOCK with argument data address - CmdIndex = (NumberOfBlocks > 1) ? SD_CMD_WRITE_MULT_BLOCK : SD_CMD_WRITE_SINGLE_BLOCK; - - // Set Block Size for Card - uint8_t retries = 10; - do { - ErrorState = SD_TransmitCommand((CmdIndex | SD_CMD_RESPONSE_SHORT), (uint32_t)WriteAddress, 1); - if (ErrorState != SD_OK && retries--) { - ErrorState = SD_TransmitCommand((SD_CMD_APP_CMD | SD_CMD_RESPONSE_SHORT), 0, 1); - } - } while (ErrorState != SD_OK && retries); - - if (ErrorState != SD_OK) { - SD_Handle.TXCplt = 0; - return ErrorState; - } - - SD_StartBlockTransfert(buffer, BlockSize, NumberOfBlocks, SDMMC_DIR_TX); - - // Configure the SD DPSM (Data Path State Machine) - SD_DataTransferInit(BlockSize * NumberOfBlocks, SD_DATABLOCK_SIZE_512B, false); - - SD_Handle.TransferError = ErrorState; - - return ErrorState; -} - -SD_Error_t SD_CheckWrite(void) -{ - if (SD_Handle.TXCplt != 0) return SD_BUSY; - return SD_OK; -} - -SD_Error_t SD_CheckRead(void) -{ - if (SD_Handle.RXCplt != 0) return SD_BUSY; - return SD_OK; -} - -/** -----------------------------------------------------------------------------------------------------------------*/ -/** - * @brief Returns information about specific card. - * contains all SD cardinformation - * @retval SD Card error state - */ -SD_Error_t SD_GetCardInfo(void) -{ - SD_Error_t ErrorState = SD_OK; - uint32_t Temp = 0; - - // Byte 0 - Temp = (SD_Handle.CSD[0] & 0xFF000000) >> 24; - SD_CardInfo.SD_csd.CSDStruct = (uint8_t)((Temp & 0xC0) >> 6); - SD_CardInfo.SD_csd.SysSpecVersion = (uint8_t)((Temp & 0x3C) >> 2); - SD_CardInfo.SD_csd.Reserved1 = Temp & 0x03; - - // Byte 1 - Temp = (SD_Handle.CSD[0] & 0x00FF0000) >> 16; - SD_CardInfo.SD_csd.TAAC = (uint8_t)Temp; - - // Byte 2 - Temp = (SD_Handle.CSD[0] & 0x0000FF00) >> 8; - SD_CardInfo.SD_csd.NSAC = (uint8_t)Temp; - - // Byte 3 - Temp = SD_Handle.CSD[0] & 0x000000FF; - SD_CardInfo.SD_csd.MaxBusClkFrec = (uint8_t)Temp; - - // Byte 4 - Temp = (SD_Handle.CSD[1] & 0xFF000000) >> 24; - SD_CardInfo.SD_csd.CardComdClasses = (uint16_t)(Temp << 4); - - // Byte 5 - Temp = (SD_Handle.CSD[1] & 0x00FF0000) >> 16; - SD_CardInfo.SD_csd.CardComdClasses |= (uint16_t)((Temp & 0xF0) >> 4); - SD_CardInfo.SD_csd.RdBlockLen = (uint8_t)(Temp & 0x0F); - - // Byte 6 - Temp = (SD_Handle.CSD[1] & 0x0000FF00) >> 8; - SD_CardInfo.SD_csd.PartBlockRead = (uint8_t)((Temp & 0x80) >> 7); - SD_CardInfo.SD_csd.WrBlockMisalign = (uint8_t)((Temp & 0x40) >> 6); - SD_CardInfo.SD_csd.RdBlockMisalign = (uint8_t)((Temp & 0x20) >> 5); - SD_CardInfo.SD_csd.DSRImpl = (uint8_t)((Temp & 0x10) >> 4); - SD_CardInfo.SD_csd.Reserved2 = 0; /*!< Reserved */ - - if ((SD_CardType == SD_STD_CAPACITY_V1_1) || (SD_CardType == SD_STD_CAPACITY_V2_0)) { - SD_CardInfo.SD_csd.DeviceSize = (Temp & 0x03) << 10; - - // Byte 7 - Temp = (uint8_t)(SD_Handle.CSD[1] & 0x000000FF); - SD_CardInfo.SD_csd.DeviceSize |= (Temp) << 2; - - // Byte 8 - Temp = (uint8_t)((SD_Handle.CSD[2] & 0xFF000000) >> 24); - SD_CardInfo.SD_csd.DeviceSize |= (Temp & 0xC0) >> 6; - - SD_CardInfo.SD_csd.MaxRdCurrentVDDMin = (Temp & 0x38) >> 3; - SD_CardInfo.SD_csd.MaxRdCurrentVDDMax = (Temp & 0x07); - - // Byte 9 - Temp = (uint8_t)((SD_Handle.CSD[2] & 0x00FF0000) >> 16); - SD_CardInfo.SD_csd.MaxWrCurrentVDDMin = (Temp & 0xE0) >> 5; - SD_CardInfo.SD_csd.MaxWrCurrentVDDMax = (Temp & 0x1C) >> 2; - SD_CardInfo.SD_csd.DeviceSizeMul = (Temp & 0x03) << 1; - - // Byte 10 - Temp = (uint8_t)((SD_Handle.CSD[2] & 0x0000FF00) >> 8); - SD_CardInfo.SD_csd.DeviceSizeMul |= (Temp & 0x80) >> 7; - - SD_CardInfo.CardCapacity = (SD_CardInfo.SD_csd.DeviceSize + 1) ; - SD_CardInfo.CardCapacity *= (1 << (SD_CardInfo.SD_csd.DeviceSizeMul + 2)); - SD_CardInfo.CardBlockSize = 1 << (SD_CardInfo.SD_csd.RdBlockLen); - SD_CardInfo.CardCapacity *= SD_CardInfo.CardBlockSize; - } - else if (SD_CardType == SD_HIGH_CAPACITY) { - // Byte 7 - Temp = (uint8_t)(SD_Handle.CSD[1] & 0x000000FF); - SD_CardInfo.SD_csd.DeviceSize = (Temp & 0x3F) << 16; - - // Byte 8 - Temp = (uint8_t)((SD_Handle.CSD[2] & 0xFF000000) >> 24); - - SD_CardInfo.SD_csd.DeviceSize |= (Temp << 8); - - // Byte 9 - Temp = (uint8_t)((SD_Handle.CSD[2] & 0x00FF0000) >> 16); - - SD_CardInfo.SD_csd.DeviceSize |= (Temp); - - // Byte 10 - Temp = (uint8_t)((SD_Handle.CSD[2] & 0x0000FF00) >> 8); - - SD_CardInfo.CardCapacity = ((uint64_t)SD_CardInfo.SD_csd.DeviceSize + 1) * 1024; - SD_CardInfo.CardBlockSize = 512; - } - else { - // Not supported card type - ErrorState = SD_ERROR; - } - - SD_CardInfo.SD_csd.EraseGrSize = (Temp & 0x40) >> 6; - SD_CardInfo.SD_csd.EraseGrMul = (Temp & 0x3F) << 1; - - // Byte 11 - Temp = (uint8_t)(SD_Handle.CSD[2] & 0x000000FF); - SD_CardInfo.SD_csd.EraseGrMul |= (Temp & 0x80) >> 7; - SD_CardInfo.SD_csd.WrProtectGrSize = (Temp & 0x7F); - - // Byte 12 - Temp = (uint8_t)((SD_Handle.CSD[3] & 0xFF000000) >> 24); - SD_CardInfo.SD_csd.WrProtectGrEnable = (Temp & 0x80) >> 7; - SD_CardInfo.SD_csd.ManDeflECC = (Temp & 0x60) >> 5; - SD_CardInfo.SD_csd.WrSpeedFact = (Temp & 0x1C) >> 2; - SD_CardInfo.SD_csd.MaxWrBlockLen = (Temp & 0x03) << 2; - - // Byte 13 - Temp = (uint8_t)((SD_Handle.CSD[3] & 0x00FF0000) >> 16); - SD_CardInfo.SD_csd.MaxWrBlockLen |= (Temp & 0xC0) >> 6; - SD_CardInfo.SD_csd.WriteBlockPaPartial = (Temp & 0x20) >> 5; - SD_CardInfo.SD_csd.Reserved3 = 0; - SD_CardInfo.SD_csd.ContentProtectAppli = (Temp & 0x01); - - // Byte 14 - Temp = (uint8_t)((SD_Handle.CSD[3] & 0x0000FF00) >> 8); - SD_CardInfo.SD_csd.FileFormatGrouop = (Temp & 0x80) >> 7; - SD_CardInfo.SD_csd.CopyFlag = (Temp & 0x40) >> 6; - SD_CardInfo.SD_csd.PermWrProtect = (Temp & 0x20) >> 5; - SD_CardInfo.SD_csd.TempWrProtect = (Temp & 0x10) >> 4; - SD_CardInfo.SD_csd.FileFormat = (Temp & 0x0C) >> 2; - SD_CardInfo.SD_csd.ECC = (Temp & 0x03); - - // Byte 15 - Temp = (uint8_t)(SD_Handle.CSD[3] & 0x000000FF); - SD_CardInfo.SD_csd.CSD_CRC = (Temp & 0xFE) >> 1; - SD_CardInfo.SD_csd.Reserved4 = 1; - - // Byte 0 - Temp = (uint8_t)((SD_Handle.CID[0] & 0xFF000000) >> 24); - SD_CardInfo.SD_cid.ManufacturerID = Temp; - - // Byte 1 - Temp = (uint8_t)((SD_Handle.CID[0] & 0x00FF0000) >> 16); - SD_CardInfo.SD_cid.OEM_AppliID = Temp << 8; - - // Byte 2 - Temp = (uint8_t)((SD_Handle.CID[0] & 0x000000FF00) >> 8); - SD_CardInfo.SD_cid.OEM_AppliID |= Temp; - - // Byte 3 - Temp = (uint8_t)(SD_Handle.CID[0] & 0x000000FF); - SD_CardInfo.SD_cid.ProdName1 = Temp << 24; - - // Byte 4 - Temp = (uint8_t)((SD_Handle.CID[1] & 0xFF000000) >> 24); - SD_CardInfo.SD_cid.ProdName1 |= Temp << 16; - - // Byte 5 - Temp = (uint8_t)((SD_Handle.CID[1] & 0x00FF0000) >> 16); - SD_CardInfo.SD_cid.ProdName1 |= Temp << 8; - - // Byte 6 - Temp = (uint8_t)((SD_Handle.CID[1] & 0x0000FF00) >> 8); - SD_CardInfo.SD_cid.ProdName1 |= Temp; - - // Byte 7 - Temp = (uint8_t)(SD_Handle.CID[1] & 0x000000FF); - SD_CardInfo.SD_cid.ProdName2 = Temp; - - // Byte 8 - Temp = (uint8_t)((SD_Handle.CID[2] & 0xFF000000) >> 24); - SD_CardInfo.SD_cid.ProdRev = Temp; - - // Byte 9 - Temp = (uint8_t)((SD_Handle.CID[2] & 0x00FF0000) >> 16); - SD_CardInfo.SD_cid.ProdSN = Temp << 24; - - // Byte 10 - Temp = (uint8_t)((SD_Handle.CID[2] & 0x0000FF00) >> 8); - SD_CardInfo.SD_cid.ProdSN |= Temp << 16; - - // Byte 11 - Temp = (uint8_t)(SD_Handle.CID[2] & 0x000000FF); - SD_CardInfo.SD_cid.ProdSN |= Temp << 8; - - // Byte 12 - Temp = (uint8_t)((SD_Handle.CID[3] & 0xFF000000) >> 24); - SD_CardInfo.SD_cid.ProdSN |= Temp; - - // Byte 13 - Temp = (uint8_t)((SD_Handle.CID[3] & 0x00FF0000) >> 16); - SD_CardInfo.SD_cid.Reserved1 |= (Temp & 0xF0) >> 4; - SD_CardInfo.SD_cid.ManufactDate = (Temp & 0x0F) << 8; - - // Byte 14 - Temp = (uint8_t)((SD_Handle.CID[3] & 0x0000FF00) >> 8); - SD_CardInfo.SD_cid.ManufactDate |= Temp; - - // Byte 15 - Temp = (uint8_t)(SD_Handle.CID[3] & 0x000000FF); - SD_CardInfo.SD_cid.CID_CRC = (Temp & 0xFE) >> 1; - SD_CardInfo.SD_cid.Reserved2 = 1; - - return ErrorState; -} - - -/** -----------------------------------------------------------------------------------------------------------------*/ -/** - * @brief Enables wide bus operation for the requested card if supported by - * card. - * @param WideMode: Specifies the SD card wide bus mode - * This parameter can be one of the following values: - * @arg SD_BUS_WIDE_8B: 8-bit data transfer (Only for MMC) - * @arg SD_BUS_WIDE_4B: 4-bit data transfer - * @arg SD_BUS_WIDE_1B: 1-bit data transfer - * @retval SD Card error state - */ -static SD_Error_t SD_WideBusOperationConfig(uint32_t WideMode) -{ - SD_Error_t ErrorState = SD_OK; - uint32_t Temp; - uint32_t SCR[2] = {0, 0}; - - if ((SD_CardType == SD_STD_CAPACITY_V1_1) || (SD_CardType == SD_STD_CAPACITY_V2_0) || (SD_CardType == SD_HIGH_CAPACITY)) { - if (WideMode == SD_BUS_WIDE_8B) { - ErrorState = SD_UNSUPPORTED_FEATURE; - } - else if ((WideMode == SD_BUS_WIDE_4B) || (WideMode == SD_BUS_WIDE_1B)) { - if ((SDMMC1->RESP1 & SD_CARD_LOCKED) != SD_CARD_LOCKED) { - // Get SCR Register - ErrorState = SD_FindSCR(SCR); - - if (ErrorState == SD_OK) { - Temp = (WideMode == SD_BUS_WIDE_4B) ? SD_WIDE_BUS_SUPPORT : SD_SINGLE_BUS_SUPPORT; - - // If requested card supports wide bus operation - if ((SCR[1] & Temp) != SD_ALLZERO) { - // Send CMD55 APP_CMD with argument as card's RCA. - ErrorState = SD_TransmitCommand((SD_CMD_APP_CMD | SD_CMD_RESPONSE_SHORT), SD_CardRCA, 1); - if (ErrorState == SD_OK) { - Temp = (WideMode == SD_BUS_WIDE_4B) ? 2 : 0; - - // Send ACMD6 APP_CMD with argument as 2 for wide bus mode - ErrorState = SD_TransmitCommand((SD_CMD_APP_SD_SET_BUSWIDTH | SD_CMD_RESPONSE_SHORT), Temp, 1); - } - } - else { - ErrorState = SD_REQUEST_NOT_APPLICABLE; - } - } - } - else { - ErrorState = SD_LOCK_UNLOCK_FAILED; - } - } - else { - ErrorState = SD_INVALID_PARAMETER; // WideMode is not a valid argument - } - - if (ErrorState == SD_OK) { - // Configure the SDMMC1 peripheral, we need this delay for some reason... - while ((READ_REG(SDMMC1->CLKCR) & 0x800) != WideMode) { - MODIFY_REG(SDMMC1->CLKCR, CLKCR_CLEAR_MASK, (uint32_t) WideMode); - } - } - } - else { - ErrorState = SD_UNSUPPORTED_FEATURE; - } - - - return ErrorState; -} - - -/** -----------------------------------------------------------------------------------------------------------------*/ -/** - * @brief Gets the current card's data status. - * @retval Data Transfer state - */ -SD_Error_t SD_GetStatus(void) -{ - SD_Error_t ErrorState; - uint32_t Response1; - SD_CardState_t CardState; - - - // Send Status command - if ((ErrorState = SD_TransmitCommand((SD_CMD_SEND_STATUS | SD_CMD_RESPONSE_SHORT), SD_CardRCA, 1)) == SD_OK) { - Response1 = SDMMC1->RESP1; - CardState = (SD_CardState_t)((Response1 >> 9) & 0x0F); - - // Find SD status according to card state - if (CardState == SD_CARD_TRANSFER) { - ErrorState = SD_OK; - } - else if (CardState == SD_CARD_ERROR) { - ErrorState = SD_ERROR; - } - else { - ErrorState = SD_BUSY; - } - } - else { - ErrorState = SD_ERROR; - } - - return ErrorState; -} - - -/** -----------------------------------------------------------------------------------------------------------------*/ -/** - * @brief Gets the SD card status. - * @retval SD Card error state - */ -SD_Error_t SD_GetCardStatus(SD_CardStatus_t* pCardStatus) -{ - SD_Error_t ErrorState; - uint32_t Temp = 0; - uint32_t Status[16]; - uint32_t Count; - - // Check SD response - if ((SDMMC1->RESP1 & SD_CARD_LOCKED) == SD_CARD_LOCKED) { - return SD_LOCK_UNLOCK_FAILED; - } - - // Set block size for card if it is not equal to current block size for card - if ((ErrorState = SD_TransmitCommand((SD_CMD_SET_BLOCKLEN | SD_CMD_RESPONSE_SHORT), 64, 1)) != SD_OK) { - return ErrorState; - } - - // Send CMD55 - if ((ErrorState = SD_TransmitCommand((SD_CMD_APP_CMD | SD_CMD_RESPONSE_SHORT), SD_CardRCA, 1)) != SD_OK) { - return ErrorState; - } - - // Configure the SD DPSM (Data Path State Machine) - SD_DataTransferInit(64, SD_DATABLOCK_SIZE_64B, true); - - // Send ACMD13 (SD_APP_STAUS) with argument as card's RCA - if ((ErrorState = SD_TransmitCommand((SD_CMD_SD_APP_STATUS | SD_CMD_RESPONSE_SHORT), 0, 1)) != SD_OK) { - return ErrorState; - } - - // Get status data - while ((SDMMC1->STA & (SDMMC_STA_RXOVERR | SDMMC_STA_DCRCFAIL | SDMMC_STA_DTIMEOUT | SDMMC_STA_DBCKEND)) == 0) { - if ((SDMMC1->STA & SDMMC_STA_RXFIFOHF) != 0) { - for(Count = 0; Count < 8; Count++) { - Status[Count] = SDMMC1->FIFO; - } - } - } - - if ((SDMMC1->STA & SDMMC_STA_DTIMEOUT) != 0) { - return SD_DATA_TIMEOUT; - } - else if ((SDMMC1->STA & SDMMC_STA_DCRCFAIL) != 0) { - return SD_DATA_CRC_FAIL; - } - else if ((SDMMC1->STA & SDMMC_STA_RXOVERR) != 0) { - return SD_RX_OVERRUN; - } - else { - Count = SD_DATATIMEOUT; - while (((SDMMC1->STA & SDMMC_STA_RXDAVL) != 0) && (Count > 0)) { - volatile uint8_t tmp = SDMMC1->FIFO; - (void)tmp; - Count--; - } - } - - // Byte 0 - Temp = (Status[0] & 0xC0) >> 6; - pCardStatus->DAT_BUS_WIDTH = (uint8_t)Temp; - - // Byte 0 - Temp = (Status[0] & 0x20) >> 5; - pCardStatus->SECURED_MODE = (uint8_t)Temp; - - // Byte 2 - Temp = (Status[2] & 0xFF); - pCardStatus->SD_CARD_TYPE = (uint8_t)(Temp << 8); - - // Byte 3 - Temp = (Status[3] & 0xFF); - pCardStatus->SD_CARD_TYPE |= (uint8_t)Temp; - - // Byte 4 - Temp = (Status[4] & 0xFF); - pCardStatus->SIZE_OF_PROTECTED_AREA = (uint8_t)(Temp << 24); - - // Byte 5 - Temp = (Status[5] & 0xFF); - pCardStatus->SIZE_OF_PROTECTED_AREA |= (uint8_t)(Temp << 16); - - // Byte 6 - Temp = (Status[6] & 0xFF); - pCardStatus->SIZE_OF_PROTECTED_AREA |= (uint8_t)(Temp << 8); - - // Byte 7 - Temp = (Status[7] & 0xFF); - pCardStatus->SIZE_OF_PROTECTED_AREA |= (uint8_t)Temp; - - // Byte 8 - Temp = (Status[8] & 0xFF); - pCardStatus->SPEED_CLASS = (uint8_t)Temp; - - // Byte 9 - Temp = (Status[9] & 0xFF); - pCardStatus->PERFORMANCE_MOVE = (uint8_t)Temp; - - // Byte 10 - Temp = (Status[10] & 0xF0) >> 4; - pCardStatus->AU_SIZE = (uint8_t)Temp; - - // Byte 11 - Temp = (Status[11] & 0xFF); - pCardStatus->ERASE_SIZE = (uint8_t)(Temp << 8); - - // Byte 12 - Temp = (Status[12] & 0xFF); - pCardStatus->ERASE_SIZE |= (uint8_t)Temp; - - // Byte 13 - Temp = (Status[13] & 0xFC) >> 2; - pCardStatus->ERASE_TIMEOUT = (uint8_t)Temp; - - // Byte 13 - Temp = (Status[13] & 0x3); - pCardStatus->ERASE_OFFSET = (uint8_t)Temp; - - return SD_OK; -} - - -/** -----------------------------------------------------------------------------------------------------------------*/ -/** - * @brief Enquires cards about their operating voltage and configures clock - * controls and stores SD information that will be needed in future - * in the SD handle. - * @retval SD Card error state - */ -static SD_Error_t SD_PowerON(void) -{ - SD_Error_t ErrorState; - uint32_t Response; - uint32_t Count; - uint32_t ValidVoltage; - uint32_t SD_Type; - //uint32_t TickStart; - - Count = 0; - ValidVoltage = 0; - SD_Type = SD_RESP_STD_CAPACITY; - - // Power ON Sequence ------------------------------------------------------- - SDMMC1->CLKCR &= ~SDMMC_CLKCR_CLKEN; // Disable SDMMC1 Clock - SDMMC1->POWER = SDMMC_POWER_PWRCTRL; // Set Power State to ON - - // 1ms: required power up waiting time before starting the SD initialization sequence (make it 2 to be safe) - delay(2); - - SDMMC1->CLKCR |= SDMMC_CLKCR_CLKEN; // Enable SDMMC1 Clock - - // CMD0: GO_IDLE_STATE ----------------------------------------------------- - // No CMD response required - if ((ErrorState = SD_TransmitCommand(SD_CMD_GO_IDLE_STATE, 0, 0)) != SD_OK) { - // CMD Response Timeout (wait for CMDSENT flag) - return ErrorState; - } - - // CMD8: SEND_IF_COND ------------------------------------------------------ - // Send CMD8 to verify SD card interface operating condition - // Argument: - [31:12]: Reserved (shall be set to '0') - //- [11:8]: Supply Voltage (VHS) 0x1 (Range: 2.7-3.6 V) - //- [7:0]: Check Pattern (recommended 0xAA) - // CMD Response: R7 */ - if ((ErrorState = SD_TransmitCommand((SD_SDMMC_SEND_IF_COND | SD_CMD_RESPONSE_SHORT), SD_CHECK_PATTERN, 7)) == SD_OK) { - // SD Card 2.0 - SD_CardType = SD_STD_CAPACITY_V2_0; - SD_Type = SD_RESP_HIGH_CAPACITY; - } - - // Send CMD55 - // If ErrorState is Command Timeout, it is a MMC card - // If ErrorState is SD_OK it is a SD card: SD card 2.0 (voltage range mismatch) or SD card 1.x - if ((ErrorState = SD_TransmitCommand((SD_CMD_APP_CMD | SD_CMD_RESPONSE_SHORT), 0, 1)) == SD_OK) { - // SD CARD - // Send ACMD41 SD_APP_OP_COND with Argument 0x80100000 - while ((ValidVoltage == 0) && (Count < SD_MAX_VOLT_TRIAL)) { - // SEND CMD55 APP_CMD with RCA as 0 - if ((ErrorState = SD_TransmitCommand((SD_CMD_APP_CMD | SD_CMD_RESPONSE_SHORT), 0, 1)) != SD_OK) { - return ErrorState; - } - - // Send CMD41 - if ((ErrorState = SD_TransmitCommand((SD_CMD_SD_APP_OP_COND | SD_CMD_RESPONSE_SHORT), SD_VOLTAGE_WINDOW_SD | SD_Type, 3)) != SD_OK) { - return ErrorState; - } - - Response = SDMMC1->RESP1; // Get command response - ValidVoltage = (((Response >> 31) == 1) ? 1 : 0); // Get operating voltage - Count++; - } - - if (Count >= SD_MAX_VOLT_TRIAL) { - return SD_INVALID_VOLTRANGE; - } - - if ((Response & SD_RESP_HIGH_CAPACITY) == SD_RESP_HIGH_CAPACITY) { - SD_CardType = SD_HIGH_CAPACITY; - } - } // else MMC Card - - return ErrorState; -} - - -/** -----------------------------------------------------------------------------------------------------------------*/ -/** - * @brief Finds the SD card SCR register value. - * @param pSCR: pointer to the buffer that will contain the SCR value - * @retval SD Card error state - */ -static SD_Error_t SD_FindSCR(uint32_t *pSCR) -{ - SD_Error_t ErrorState; - uint32_t Index = 0; - uint32_t tempscr[2] = {0, 0}; - - // Set Block Size To 8 Bytes - // Send CMD55 APP_CMD with argument as card's RCA - if ((ErrorState = SD_TransmitCommand((SD_CMD_SET_BLOCKLEN | SD_CMD_RESPONSE_SHORT), 8, 1)) == SD_OK) { - // Send CMD55 APP_CMD with argument as card's RCA - if ((ErrorState = SD_TransmitCommand((SD_CMD_APP_CMD | SD_CMD_RESPONSE_SHORT), SD_CardRCA, 1)) == SD_OK) { - SD_DataTransferInit(8, SD_DATABLOCK_SIZE_8B, true); - - // Send ACMD51 SD_APP_SEND_SCR with argument as 0 - if ((ErrorState = SD_TransmitCommand((SD_CMD_SD_APP_SEND_SCR | SD_CMD_RESPONSE_SHORT), 0, 1)) == SD_OK) { - while ((SDMMC1->STA & (SDMMC_STA_RXOVERR | SDMMC_STA_DCRCFAIL | SDMMC_STA_DTIMEOUT | SDMMC_STA_DBCKEND)) == 0) { - if ((SDMMC1->STA & SDMMC_STA_RXDAVL) != 0) { - *(tempscr + Index) = SDMMC1->FIFO; - Index++; - } - } - - if ((SDMMC1->STA & SDMMC_STA_DTIMEOUT) != 0) { - ErrorState = SD_DATA_TIMEOUT; - } - else if ((SDMMC1->STA & SDMMC_STA_DCRCFAIL) != 0) { - ErrorState = SD_DATA_CRC_FAIL; - } - else if ((SDMMC1->STA & SDMMC_STA_RXOVERR) != 0) { - ErrorState = SD_RX_OVERRUN; - } - else if ((SDMMC1->STA & SDMMC_STA_RXDAVL) != 0) { - ErrorState = SD_OUT_OF_BOUND; - } - else { - *(pSCR + 1) = ((tempscr[0] & SD_0TO7BITS) << 24) | ((tempscr[0] & SD_8TO15BITS) << 8) | - ((tempscr[0] & SD_16TO23BITS) >> 8) | ((tempscr[0] & SD_24TO31BITS) >> 24); - - *(pSCR) = ((tempscr[1] & SD_0TO7BITS) << 24) | ((tempscr[1] & SD_8TO15BITS) << 8) | - ((tempscr[1] & SD_16TO23BITS) >> 8) | ((tempscr[1] & SD_24TO31BITS) >> 24); - } - } - } - } - - return ErrorState; -} - - -/** -----------------------------------------------------------------------------------------------------------------*/ -/** - * @brief Initialize the SDMMC1 module, DMA, and IO - */ -bool SD_Initialize_LL(DMA_Stream_TypeDef * dmaRef) -{ - // Sanity check DMA stread - we only support two possible - if (((uint32_t)dmaRef != (uint32_t)DMA2_Stream3) && ((uint32_t)dmaRef != (uint32_t)DMA2_Stream6)) { - return false; - } - - // Reset SDMMC1 Module - RCC->APB2RSTR |= RCC_APB2RSTR_SDMMC1RST; - delay(1); - RCC->APB2RSTR &= ~RCC_APB2RSTR_SDMMC1RST; - delay(1); - - // Enable SDMMC1 clock - RCC->APB2ENR |= RCC_APB2ENR_SDMMC1EN; - - // Enable DMA2 clocks - RCC->AHB1ENR |= RCC_AHB1ENR_DMA2EN; - - //Configure Pins - RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN | RCC_AHB1ENR_GPIODEN; - - const IO_t d0 = IOGetByTag(IO_TAG(PC8)); - const IO_t clk = IOGetByTag(IO_TAG(PC12)); - const IO_t cmd = IOGetByTag(IO_TAG(PD2)); - -#define SDMMC_DATA IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_NOPULL) -#define SDMMC_CMD IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_NOPULL) -#define SDMMC_CLK IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_NOPULL) - - IOInit(d0, OWNER_SDCARD, RESOURCE_NONE, 0); - IOConfigGPIOAF(d0, SDMMC_DATA, GPIO_AF12_SDMMC1); - -#ifdef SDCARD_SDIO_4BIT - const IO_t d1 = IOGetByTag(IO_TAG(PC9)); - const IO_t d2 = IOGetByTag(IO_TAG(PC10)); - const IO_t d3 = IOGetByTag(IO_TAG(PC11)); - - IOInit(d1, OWNER_SDCARD, RESOURCE_NONE, 0); - IOConfigGPIOAF(d1, SDMMC_DATA, GPIO_AF12_SDMMC1); - - IOInit(d2, OWNER_SDCARD, RESOURCE_NONE, 0); - IOConfigGPIOAF(d2, SDMMC_DATA, GPIO_AF12_SDMMC1); - - IOInit(d3, OWNER_SDCARD, RESOURCE_NONE, 0); - IOConfigGPIOAF(d3, SDMMC_DATA, GPIO_AF12_SDMMC1); -#endif - - IOInit(clk, OWNER_SDCARD, RESOURCE_NONE, 0); - IOConfigGPIOAF(clk, SDMMC_CLK, GPIO_AF12_SDMMC1); - - IOInit(cmd, OWNER_SDCARD, RESOURCE_NONE, 0); - IOConfigGPIOAF(cmd, SDMMC_CMD, GPIO_AF12_SDMMC1); - - // NVIC configuration for SDIO interrupts - NVIC_SetPriority(SDMMC1_IRQn, NVIC_PRIO_SDIO); - NVIC_EnableIRQ(SDMMC1_IRQn); - - dma_stream = dmaRef; - RCC->AHB1ENR |= RCC_AHB1ENR_DMA2EN; - if ((uint32_t)dma_stream == (uint32_t)DMA2_Stream3) { - // Initialize DMA2 channel 3 - DMA2_Stream3->CR = 0; // Reset DMA Stream control register - DMA2_Stream3->PAR = (uint32_t)&SDMMC1->FIFO; - DMA2->LIFCR = IFCR_CLEAR_MASK_STREAM3; // Clear all interrupt flags - DMA2_Stream3->CR = (DMA_CHANNEL_4 | DMA_SxCR_PFCTRL | // Prepare the DMA Stream configuration - DMA_MINC_ENABLE | DMA_PDATAALIGN_WORD | // And write to DMA Stream CR register - DMA_MDATAALIGN_WORD | DMA_PRIORITY_VERY_HIGH | - DMA_MBURST_INC4 | DMA_PBURST_INC4 | - DMA_MEMORY_TO_PERIPH); - DMA2_Stream3->FCR = (DMA_SxFCR_DMDIS | DMA_SxFCR_FTH); // Configuration FIFO control register - dmaInit(dmaGetByRef(DMA2_Stream3), OWNER_SDCARD, 0); - dmaSetHandler(dmaGetByRef(DMA2_Stream3), SDMMC_DMA_ST3_IRQHandler, 1, 0); - } - else { - // Initialize DMA2 channel 6 - DMA2_Stream6->CR = 0; // Reset DMA Stream control register - DMA2_Stream6->PAR = (uint32_t)&SDMMC1->FIFO; - DMA2->HIFCR = IFCR_CLEAR_MASK_STREAM6; // Clear all interrupt flags - DMA2_Stream6->CR = (DMA_CHANNEL_4 | DMA_SxCR_PFCTRL | // Prepare the DMA Stream configuration - DMA_MINC_ENABLE | DMA_PDATAALIGN_WORD | // And write to DMA Stream CR register - DMA_MDATAALIGN_WORD | DMA_PRIORITY_VERY_HIGH | - DMA_MBURST_INC4 | DMA_PBURST_INC4 | - DMA_MEMORY_TO_PERIPH); - DMA2_Stream6->FCR = (DMA_SxFCR_DMDIS | DMA_SxFCR_FTH); // Configuration FIFO control register - dmaInit(dmaGetByRef(DMA2_Stream6), OWNER_SDCARD, 0); - dmaSetHandler(dmaGetByRef(DMA2_Stream6), SDMMC_DMA_ST6_IRQHandler, 1, 0); - } - - return true; -} - - -/** -----------------------------------------------------------------------------------------------------------------*/ -bool SD_GetState(void) -{ - // Check SDCARD status - if (SD_GetStatus() == SD_OK) return true; - return false; -} - - -/** -----------------------------------------------------------------------------------------------------------------*/ -bool SD_Init(void) -{ - SD_Error_t ErrorState; - - // Initialize SDMMC1 peripheral interface with default configuration for SD card initialization - MODIFY_REG(SDMMC1->CLKCR, CLKCR_CLEAR_MASK, (uint32_t) SDMMC_INIT_CLK_DIV); - - delay(100); - - // Identify card operating voltage - if ((ErrorState = SD_PowerON()) == SD_OK) { - delay(100); - // Initialize the present card and put them in idle state - if ((ErrorState = SD_InitializeCard()) == SD_OK) { - delay(100); - // Read CSD/CID MSD registers - if ((ErrorState = SD_GetCardInfo()) == SD_OK) { - // Select the Card - Send CMD7 SDMMC_SEL_DESEL_CARD - ErrorState = SD_TransmitCommand((SD_CMD_SEL_DESEL_CARD | SD_CMD_RESPONSE_SHORT), SD_CardRCA, 1); - delay(100); - MODIFY_REG(SDMMC1->CLKCR, CLKCR_CLEAR_MASK, (uint32_t) SDMMC_CLK_DIV); // Configure SDMMC1 peripheral interface - } - } - } - - // Configure SD Bus width - if (ErrorState == SD_OK) { - delay(100); - // Enable wide operation -#ifdef SDCARD_SDIO_4BIT - ErrorState = SD_WideBusOperationConfig(SD_BUS_WIDE_4B); -#else - ErrorState = SD_WideBusOperationConfig(SD_BUS_WIDE_1B); -#endif - delay(100); - } - - // Configure the SDCARD device - return ErrorState; -} - -/** -----------------------------------------------------------------------------------------------------------------*/ -/** - * @brief This function handles SD card interrupt request. - */ -void SDMMC1_IRQHandler(void) -{ - // Check for SDMMC1 interrupt flags - if ((SDMMC1->STA & SDMMC_STA_DATAEND) != 0) { - SDMMC1->ICR = SDMMC_ICR_DATAENDC; - SDMMC1->ICR = SDMMC_ICR_STATIC_FLAGS; - SDMMC1->MASK &= ~(SDMMC_MASK_DATAENDIE | SDMMC_MASK_DCRCFAILIE | SDMMC_MASK_DTIMEOUTIE | - SDMMC_MASK_TXUNDERRIE | SDMMC_MASK_RXOVERRIE | SDMMC_MASK_TXFIFOHEIE | SDMMC_MASK_RXFIFOHFIE); - - /* Currently doesn't implement multiple block write handling */ - if ((SD_Handle.Operation & 0x02) == (SDMMC_DIR_TX << 1)) { - /* Disable the stream */ - dma_stream->CR &= ~DMA_SxCR_EN; - SDMMC1->DCTRL &= ~(SDMMC_DCTRL_DMAEN); - /* Transfer is complete */ - SD_Handle.TXCplt = 0; - if ((SD_Handle.Operation & 0x01) == SD_MULTIPLE_BLOCK) { - /* Send stop command in multiblock write */ - SD_TransmitCommand((SD_CMD_STOP_TRANSMISSION | SD_CMD_RESPONSE_SHORT), 0, 1); - } - } - SD_Handle.TransferComplete = 1; - SD_Handle.TransferError = SD_OK; // No transfer error - } - else if ((SDMMC1->STA & SDMMC_STA_DCRCFAIL) != 0) { - SD_Handle.TransferError = SD_DATA_CRC_FAIL; - } - else if ((SDMMC1->STA & SDMMC_STA_DTIMEOUT) != 0) { - SD_Handle.TransferError = SD_DATA_TIMEOUT; - } - else if ((SDMMC1->STA & SDMMC_STA_RXOVERR) != 0) { - SD_Handle.TransferError = SD_RX_OVERRUN; - } - else if ((SDMMC1->STA & SDMMC_STA_TXUNDERR) != 0) { - SD_Handle.TransferError = SD_TX_UNDERRUN; - } - - SDMMC1->ICR = SDMMC_ICR_STATIC_FLAGS; - - // Disable all SDMMC1 peripheral interrupt sources - SDMMC1->MASK &= ~(SDMMC_MASK_DCRCFAILIE | SDMMC_MASK_DTIMEOUTIE | SDMMC_MASK_DATAENDIE | - SDMMC_MASK_TXFIFOHEIE | SDMMC_MASK_RXFIFOHFIE | SDMMC_MASK_TXUNDERRIE | - SDMMC_MASK_RXOVERRIE); -} - -/** -----------------------------------------------------------------------------------------------------------------*/ -/** - * @brief This function handles DMA2 Stream 3 interrupt request. - */ -void SDMMC_DMA_ST3_IRQHandler(dmaChannelDescriptor_t *dma) -{ - UNUSED(dma); - // Transfer Error Interrupt management - if ((DMA2->LISR & DMA_LISR_TEIF3) != 0) { - if ((DMA2_Stream3->CR & DMA_SxCR_TEIE) != 0) { - DMA2_Stream3->CR &= ~DMA_SxCR_TEIE; // Disable the transfer error interrupt - DMA2->LIFCR = DMA_LIFCR_CTEIF3; // Clear the transfer error flag - } - } - - // FIFO Error Interrupt management - if ((DMA2->LISR & DMA_LISR_FEIF3) != 0) { - if ((DMA2_Stream3->FCR & DMA_SxFCR_FEIE) != 0) { - DMA2_Stream3->FCR &= ~DMA_SxFCR_FEIE; // Disable the FIFO Error interrupt - DMA2->LIFCR = DMA_LIFCR_CFEIF3; // Clear the FIFO error flag - } - } - - // Direct Mode Error Interrupt management - if ((DMA2->LISR & DMA_LISR_DMEIF3) != 0) { - if ((DMA2_Stream3->CR & DMA_SxCR_DMEIE) != 0) { - DMA2_Stream3->CR &= ~DMA_SxCR_DMEIE; // Disable the direct mode Error interrupt - DMA2->LIFCR = DMA_LIFCR_CDMEIF3; // Clear the FIFO error flag - } - } - - // Half Transfer Complete Interrupt management - if ((DMA2->LISR & DMA_LISR_HTIF3) != 0) { - if ((DMA2_Stream3->CR & DMA_SxCR_HTIE) != 0) { - if (((DMA2_Stream3->CR) & (uint32_t)(DMA_SxCR_DBM)) != 0) { // Multi_Buffering mode enabled - DMA2->LIFCR = DMA_LIFCR_CHTIF3; // Clear the half transfer complete flag - } - else { - if ((DMA2_Stream3->CR & DMA_SxCR_CIRC) == 0) { // Disable the half transfer interrupt if the DMA mode is not CIRCULAR - DMA2_Stream3->CR &= ~DMA_SxCR_HTIE; // Disable the half transfer interrupt - } - - DMA2->LIFCR = DMA_LIFCR_CHTIF3; // Clear the half transfer complete flag - } - } - } - - // Transfer Complete Interrupt management - if ((DMA2->LISR & DMA_LISR_TCIF3) != 0) { - if ((DMA2_Stream3->CR & DMA_SxCR_TCIE) != 0) { - if ((DMA2_Stream3->CR & (uint32_t)(DMA_SxCR_DBM)) != 0) { - DMA2->LIFCR = DMA_LIFCR_CTCIF3; // Clear the transfer complete flag - } - else { - if ((DMA2_Stream3->CR & DMA_SxCR_CIRC) == 0) { - DMA2_Stream3->CR &= ~DMA_SxCR_TCIE; // Disable the transfer complete interrupt - } - - DMA2->LIFCR = DMA_LIFCR_CTCIF3; // Clear the transfer complete flag - SD_DMA_Complete(DMA2_Stream3); - } - } - } -} - - -/** -----------------------------------------------------------------------------------------------------------------*/ -/** - * @brief This function handles DMA2 Stream 6 interrupt request. - */ -void SDMMC_DMA_ST6_IRQHandler(dmaChannelDescriptor_t *dma) -{ - UNUSED(dma); - // Transfer Error Interrupt management - if ((DMA2->HISR & DMA_HISR_TEIF6) != 0) { - if ((DMA2_Stream6->CR & DMA_SxCR_TEIE) != 0) { - DMA2_Stream6->CR &= ~DMA_SxCR_TEIE; // Disable the transfer error interrupt - DMA2->HIFCR = DMA_HIFCR_CTEIF6; // Clear the transfer error flag - } - } - - // FIFO Error Interrupt management - if ((DMA2->HISR & DMA_HISR_FEIF6) != 0) { - if ((DMA2_Stream6->FCR & DMA_SxFCR_FEIE) != 0) { - DMA2_Stream6->FCR &= ~DMA_SxFCR_FEIE; // Disable the FIFO Error interrupt - DMA2->HIFCR = DMA_HIFCR_CFEIF6; // Clear the FIFO error flag - } - } - - // Direct Mode Error Interrupt management - if ((DMA2->HISR & DMA_HISR_DMEIF6) != 0) { - if ((DMA2_Stream6->CR & DMA_SxCR_DMEIE) != 0) { - DMA2_Stream6->CR &= ~DMA_SxCR_DMEIE; // Disable the direct mode Error interrupt - DMA2->HIFCR = DMA_HIFCR_CDMEIF6; // Clear the FIFO error flag - } - } - - // Half Transfer Complete Interrupt management - if ((DMA2->HISR & DMA_HISR_HTIF6) != 0) { - if ((DMA2_Stream6->CR & DMA_SxCR_HTIE) != 0) { - if (((DMA2_Stream6->CR) & (uint32_t)(DMA_SxCR_DBM)) != 0) { // Multi_Buffering mode enabled - DMA2->HIFCR = DMA_HIFCR_CHTIF6; // Clear the half transfer complete flag - } - else { - if ((DMA2_Stream6->CR & DMA_SxCR_CIRC) == 0) { // Disable the half transfer interrupt if the DMA mode is not CIRCULAR - DMA2_Stream6->CR &= ~DMA_SxCR_HTIE; // Disable the half transfer interrupt - } - - DMA2->HIFCR = DMA_HIFCR_CHTIF6; // Clear the half transfer complete flag - } - } - } - - // Transfer Complete Interrupt management - if ((DMA2->HISR & DMA_HISR_TCIF6) != 0) { - if ((DMA2_Stream6->CR & DMA_SxCR_TCIE) != 0) { - if ((DMA2_Stream6->CR & (uint32_t)(DMA_SxCR_DBM)) != 0) { - DMA2->HIFCR = DMA_HIFCR_CTCIF6; // Clear the transfer complete flag - } - else { - if ((DMA2_Stream6->CR & DMA_SxCR_CIRC) == 0) { - DMA2_Stream6->CR &= ~DMA_SxCR_TCIE; // Disable the transfer complete interrupt - } - - DMA2->HIFCR = DMA_HIFCR_CTCIF6; // Clear the transfer complete flag - SD_DMA_Complete(DMA2_Stream6); - } - } - } -} - -/* ------------------------------------------------------------------------------------------------------------------*/ -#endif diff --git a/src/main/drivers/sdcard/sdmmc_sdio_h7xx.c b/src/main/drivers/sdcard/sdmmc_sdio_hal.c similarity index 78% rename from src/main/drivers/sdcard/sdmmc_sdio_h7xx.c rename to src/main/drivers/sdcard/sdmmc_sdio_hal.c index d8cf16ba83..a0813bd35d 100644 --- a/src/main/drivers/sdcard/sdmmc_sdio_h7xx.c +++ b/src/main/drivers/sdcard/sdmmc_sdio_hal.c @@ -33,7 +33,6 @@ #ifdef USE_SDCARD_SDIO #include "sdmmc_sdio.h" -#include "stm32h7xx.h" #include "drivers/sdio.h" #include "drivers/io.h" @@ -53,12 +52,12 @@ typedef struct SD_Handle_s volatile uint32_t TXCplt; // SD TX Complete is equal 0 when no transfer } SD_Handle_t; -SD_HandleTypeDef hsd1; - SD_CardInfo_t SD_CardInfo; SD_CardType_t SD_CardType; -static SD_Handle_t SD_Handle; +static SD_Handle_t SD_Handle; +static DMA_HandleTypeDef sd_dma; +static SD_HandleTypeDef hsd; typedef struct sdioPin_s { ioTag_t pin; @@ -75,6 +74,18 @@ typedef struct sdioPin_s { #define SDIO_MAX_PINDEFS 2 +#define IOCFG_SDMMC IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_NOPULL) + +#ifdef STM32F7 +#define SDMMC_CLK_DIV SDMMC_TRANSFER_CLK_DIV +#else +#if defined(SDCARD_SDIO_NORMAL_SPEED) +#define SDMMC_CLK_DIV SDMMC_NSpeed_CLK_DIV +#else +#define SDMMC_CLK_DIV SDMMC_HSpeed_CLK_DIV +#endif +#endif + typedef struct sdioHardware_s { SDMMC_TypeDef *instance; IRQn_Type irqn; @@ -90,6 +101,7 @@ typedef struct sdioHardware_s { #define PINDEF(device, pin, afnum) { DEFIO_TAG_E(pin), GPIO_AF ## afnum ## _SDMMC ## device } +#ifdef STM32H7 static const sdioHardware_t sdioPinHardware[SDIODEV_COUNT] = { { .instance = SDMMC1, @@ -112,6 +124,36 @@ static const sdioHardware_t sdioPinHardware[SDIODEV_COUNT] = { .sdioPinD3 = { PINDEF(2, PB4, 9) }, } }; +#endif + +#ifdef STM32F7 +static const sdioHardware_t sdioPinHardware[SDIODEV_COUNT] = { + { + .instance = SDMMC1, + .irqn = SDMMC1_IRQn, + .sdioPinCK = { PINDEF(1, PC12, 12) }, + .sdioPinCMD = { PINDEF(1, PD2, 12) }, + .sdioPinD0 = { PINDEF(1, PC8, 12) }, + .sdioPinD1 = { PINDEF(1, PC9, 12) }, + .sdioPinD2 = { PINDEF(1, PC10, 12) }, + .sdioPinD3 = { PINDEF(1, PC11, 12) }, + //.sdioPinD4 = { PINDEF(1, PB8, 12) }, + //.sdioPinD5 = { PINDEF(1, PB9, 12) }, + //.sdioPinD6 = { PINDEF(1, PC7, 12) }, + //.sdioPinD7 = { PINDEF(1, PC11, 12) }, + }, + { + .instance = SDMMC2, + .irqn = SDMMC2_IRQn, + .sdioPinCK = { PINDEF(2, PD6, 11) }, + .sdioPinCMD = { PINDEF(2, PD7, 11) }, + .sdioPinD0 = { PINDEF(2, PB14, 10), PINDEF(2, PG0, 11) }, + .sdioPinD1 = { PINDEF(2, PB15, 10), PINDEF(2, PG10, 11) }, + .sdioPinD2 = { PINDEF(2, PB3, 10), PINDEF(2, PG11, 10) }, + .sdioPinD3 = { PINDEF(2, PB4, 10), PINDEF(2, PG12, 11) }, + } +}; +#endif #undef PINDEF @@ -137,83 +179,33 @@ void sdioPinConfigure(void) #else sdioPin[SDIO_PIN_CMD] = sdioHardware->sdioPinCMD[0]; #endif - sdioPin[SDIO_PIN_D0] = sdioHardware->sdioPinD0[0]; - -#ifdef SDCARD_SDIO_4BIT - sdioPin[SDIO_PIN_D1] = sdioHardware->sdioPinD1[0]; - sdioPin[SDIO_PIN_D2] = sdioHardware->sdioPinD2[0]; - sdioPin[SDIO_PIN_D3] = sdioHardware->sdioPinD3[0]; -#endif -} - -#define IOCFG_SDMMC IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_NOPULL) - -void HAL_SD_MspInit(SD_HandleTypeDef* hsd) -{ - UNUSED(hsd); - - if (!sdioHardware) { - return; - } - - if (sdioHardware->instance == SDMMC1) { - __HAL_RCC_SDMMC1_CLK_DISABLE(); - __HAL_RCC_SDMMC1_FORCE_RESET(); - __HAL_RCC_SDMMC1_RELEASE_RESET(); - __HAL_RCC_SDMMC1_CLK_ENABLE(); - } else if (sdioHardware->instance == SDMMC2) { - __HAL_RCC_SDMMC2_CLK_DISABLE(); - __HAL_RCC_SDMMC2_FORCE_RESET(); - __HAL_RCC_SDMMC2_RELEASE_RESET(); - __HAL_RCC_SDMMC2_CLK_ENABLE(); - } + sdioPin[SDIO_PIN_D0] = sdioHardware->sdioPinD0[0]; const IO_t clk = IOGetByTag(sdioPin[SDIO_PIN_CK].pin); const IO_t cmd = IOGetByTag(sdioPin[SDIO_PIN_CMD].pin); const IO_t d0 = IOGetByTag(sdioPin[SDIO_PIN_D0].pin); - const IO_t d1 = IOGetByTag(sdioPin[SDIO_PIN_D1].pin); - const IO_t d2 = IOGetByTag(sdioPin[SDIO_PIN_D2].pin); - const IO_t d3 = IOGetByTag(sdioPin[SDIO_PIN_D3].pin); - - IOConfigGPIOAF(clk, IOCFG_SDMMC, sdioPin[SDIO_PIN_CK].af); - IOConfigGPIOAF(cmd, IOCFG_SDMMC, sdioPin[SDIO_PIN_CMD].af); - IOConfigGPIOAF(d0, IOCFG_SDMMC, sdioPin[SDIO_PIN_D0].af); - -#ifdef SDCARD_SDIO_4BIT - IOConfigGPIOAF(d1, IOCFG_SDMMC, sdioPin[SDIO_PIN_D1].af); - IOConfigGPIOAF(d2, IOCFG_SDMMC, sdioPin[SDIO_PIN_D2].af); - IOConfigGPIOAF(d3, IOCFG_SDMMC, sdioPin[SDIO_PIN_D3].af); -#endif - - HAL_NVIC_SetPriority(sdioHardware->irqn, 0, 0); - HAL_NVIC_EnableIRQ(sdioHardware->irqn); -} - -void SDIO_GPIO_Init(void) -{ - if (!sdioHardware) { - return; - } - - const IO_t clk = IOGetByTag(sdioPin[SDIO_PIN_CK].pin); - const IO_t cmd = IOGetByTag(sdioPin[SDIO_PIN_CMD].pin); - const IO_t d0 = IOGetByTag(sdioPin[SDIO_PIN_D0].pin); - const IO_t d1 = IOGetByTag(sdioPin[SDIO_PIN_D1].pin); - const IO_t d2 = IOGetByTag(sdioPin[SDIO_PIN_D2].pin); - const IO_t d3 = IOGetByTag(sdioPin[SDIO_PIN_D3].pin); IOInit(clk, OWNER_SDCARD, RESOURCE_NONE, 0); IOInit(cmd, OWNER_SDCARD, RESOURCE_NONE, 0); IOInit(d0, OWNER_SDCARD, RESOURCE_NONE, 0); #ifdef SDCARD_SDIO_4BIT + sdioPin[SDIO_PIN_D1] = sdioHardware->sdioPinD1[0]; + sdioPin[SDIO_PIN_D2] = sdioHardware->sdioPinD2[0]; + sdioPin[SDIO_PIN_D3] = sdioHardware->sdioPinD3[0]; + + const IO_t d1 = IOGetByTag(sdioPin[SDIO_PIN_D1].pin); + const IO_t d2 = IOGetByTag(sdioPin[SDIO_PIN_D2].pin); + const IO_t d3 = IOGetByTag(sdioPin[SDIO_PIN_D3].pin); + IOInit(d1, OWNER_SDCARD, RESOURCE_NONE, 0); IOInit(d2, OWNER_SDCARD, RESOURCE_NONE, 0); IOInit(d3, OWNER_SDCARD, RESOURCE_NONE, 0); #endif // - // Setting all the SDIO pins to high for a short time results in more robust initialisation. + // Setting all the SDIO pins to high for a short time results in more robust + // initialisation. // IOHi(d0); IOConfigGPIO(d0, IOCFG_OUT_PP); @@ -233,50 +225,133 @@ void SDIO_GPIO_Init(void) IOConfigGPIO(cmd, IOCFG_OUT_PP); } -bool SD_Initialize_LL(DMA_Stream_TypeDef *dma) +void SDMMC_DMA_IRQHandler(DMA_t channel) { + UNUSED(channel); + + HAL_DMA_IRQHandler(&sd_dma); +} + +void HAL_SD_MspInit(SD_HandleTypeDef* hsd) +{ + if (!sdioHardware) { + return; + } + + if (sdioHardware->instance == SDMMC1) { + __HAL_RCC_SDMMC1_CLK_DISABLE(); + __HAL_RCC_SDMMC1_CLK_ENABLE(); + + __HAL_RCC_SDMMC1_FORCE_RESET(); + __HAL_RCC_SDMMC1_RELEASE_RESET(); + } else if (sdioHardware->instance == SDMMC2) { + __HAL_RCC_SDMMC2_CLK_DISABLE(); + __HAL_RCC_SDMMC2_CLK_ENABLE(); + + __HAL_RCC_SDMMC2_FORCE_RESET(); + __HAL_RCC_SDMMC2_RELEASE_RESET(); + } + + const IO_t clk = IOGetByTag(sdioPin[SDIO_PIN_CK].pin); + const IO_t cmd = IOGetByTag(sdioPin[SDIO_PIN_CMD].pin); + const IO_t d0 = IOGetByTag(sdioPin[SDIO_PIN_D0].pin); + const IO_t d1 = IOGetByTag(sdioPin[SDIO_PIN_D1].pin); + const IO_t d2 = IOGetByTag(sdioPin[SDIO_PIN_D2].pin); + const IO_t d3 = IOGetByTag(sdioPin[SDIO_PIN_D3].pin); + + IOConfigGPIOAF(clk, IOCFG_SDMMC, sdioPin[SDIO_PIN_CK].af); + IOConfigGPIOAF(cmd, IOCFG_SDMMC, sdioPin[SDIO_PIN_CMD].af); + IOConfigGPIOAF(d0, IOCFG_SDMMC, sdioPin[SDIO_PIN_D0].af); + +#ifdef SDCARD_SDIO_4BIT + IOConfigGPIOAF(d1, IOCFG_SDMMC, sdioPin[SDIO_PIN_D1].af); + IOConfigGPIOAF(d2, IOCFG_SDMMC, sdioPin[SDIO_PIN_D2].af); + IOConfigGPIOAF(d3, IOCFG_SDMMC, sdioPin[SDIO_PIN_D3].af); +#endif + +#ifdef STM32F7 + sd_dma.Init.Direction = DMA_PERIPH_TO_MEMORY; + sd_dma.Init.PeriphInc = DMA_PINC_DISABLE; + sd_dma.Init.MemInc = DMA_MINC_ENABLE; + sd_dma.Init.PeriphDataAlignment = DMA_PDATAALIGN_WORD; + sd_dma.Init.MemDataAlignment = DMA_MDATAALIGN_WORD; + sd_dma.Init.Mode = DMA_PFCTRL; + sd_dma.Init.Priority = DMA_PRIORITY_LOW; + sd_dma.Init.FIFOMode = DMA_FIFOMODE_ENABLE; + sd_dma.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_FULL; + sd_dma.Init.MemBurst = DMA_MBURST_INC4; + sd_dma.Init.PeriphBurst = DMA_PBURST_INC4; + + dmaInit(dmaGetByRef(sd_dma.Instance), OWNER_SDCARD, 0); + if (HAL_DMA_Init(&sd_dma) != HAL_OK) { + return; + } + dmaSetHandler(dmaGetByRef(sd_dma.Instance), SDMMC_DMA_IRQHandler, 1, 0); + + __HAL_LINKDMA(hsd, hdmarx, sd_dma); + __HAL_LINKDMA(hsd, hdmatx, sd_dma); +#else + UNUSED(hsd); +#endif + + HAL_NVIC_SetPriority(sdioHardware->irqn, 2, 0); + HAL_NVIC_EnableIRQ(sdioHardware->irqn); +} + +bool SD_Initialize_LL(DMA_t dma) +{ +#ifdef STM32F7 + sd_dma.Instance = dma->ref; + sd_dma.Init.Channel = dmaGetChannelByTag(SDCARD_SDIO_DMA); +#else UNUSED(dma); +#endif return true; } bool SD_GetState(void) { - HAL_SD_CardStateTypedef cardState = HAL_SD_GetCardState(&hsd1); + HAL_SD_CardStateTypedef cardState = HAL_SD_GetCardState(&hsd); return (cardState == HAL_SD_CARD_TRANSFER); } bool SD_Init(void) { - HAL_StatusTypeDef status; + memset(&hsd, 0, sizeof(hsd)); - memset(&hsd1, 0, sizeof(hsd1)); + hsd.Instance = sdioHardware->instance; - hsd1.Instance = sdioHardware->instance; + // falling seems to work better ?? no idea whats "right" here + hsd.Init.ClockEdge = SDMMC_CLOCK_EDGE_FALLING; - hsd1.Init.ClockEdge = SDMMC_CLOCK_EDGE_RISING; - hsd1.Init.ClockPowerSave = SDMMC_CLOCK_POWER_SAVE_ENABLE; -#ifdef SDCARD_SDIO_4BIT - hsd1.Init.BusWide = SDMMC_BUS_WIDE_4B; -#else - hsd1.Init.BusWide = SDMMC_BUS_WIDE_1B; // FIXME untested -#endif - hsd1.Init.HardwareFlowControl = SDMMC_HARDWARE_FLOW_CONTROL_ENABLE; -#ifdef SDCARD_SDIO_NORMAL_SPEED - hsd1.Init.ClockDiv = SDMMC_NSpeed_CLK_DIV; -#else - hsd1.Init.ClockDiv = 1; // 200Mhz / (2 * 1 ) = 100Mhz, used for "UltraHigh speed SD card" only, see HAL_SD_ConfigWideBusOperation, SDMMC_HSpeed_CLK_DIV, SDMMC_NSpeed_CLK_DIV + // drastically increases the time to respond from the sdcard + // lets leave it off + hsd.Init.ClockPowerSave = SDMMC_CLOCK_POWER_SAVE_DISABLE; +#ifdef STM32F7 + hsd.Init.ClockBypass = SDMMC_CLOCK_BYPASS_DISABLE; #endif + hsd.Init.BusWide = SDMMC_BUS_WIDE_1B; + hsd.Init.HardwareFlowControl = SDMMC_HARDWARE_FLOW_CONTROL_ENABLE; + hsd.Init.ClockDiv = SDMMC_CLK_DIV; - status = HAL_SD_Init(&hsd1); // Will call HAL_SD_MspInit - - if (status != HAL_OK) { + // Will call HAL_SD_MspInit + if (HAL_SD_Init(&hsd) != HAL_OK) { + return SD_ERROR; + } + if (hsd.SdCard.BlockNbr == 0) { return SD_ERROR; } - switch(hsd1.SdCard.CardType) { +#ifdef SDCARD_SDIO_4BIT + if (HAL_SD_ConfigWideBusOperation(&hsd, SDMMC_BUS_WIDE_4B) != HAL_OK) { + return SD_ERROR; + } +#endif + + switch(hsd.SdCard.CardType) { case CARD_SDSC: - switch (hsd1.SdCard.CardVersion) { + switch (hsd.SdCard.CardVersion) { case CARD_V1_X: SD_CardType = SD_STD_CAPACITY_V1_1; break; @@ -296,11 +371,11 @@ bool SD_Init(void) return SD_ERROR; } - // STATIC_ASSERT(sizeof(SD_Handle.CSD) == sizeof(hsd1.CSD), hal-csd-size-error); - memcpy(&SD_Handle.CSD, &hsd1.CSD, sizeof(SD_Handle.CSD)); + // STATIC_ASSERT(sizeof(SD_Handle.CSD) == sizeof(hsd.CSD), hal-csd-size-error); + memcpy(&SD_Handle.CSD, &hsd.CSD, sizeof(SD_Handle.CSD)); - // STATIC_ASSERT(sizeof(SD_Handle.CID) == sizeof(hsd1.CID), hal-cid-size-error); - memcpy(&SD_Handle.CID, &hsd1.CID, sizeof(SD_Handle.CID)); + // STATIC_ASSERT(sizeof(SD_Handle.CID) == sizeof(hsd.CID), hal-cid-size-error); + memcpy(&SD_Handle.CID, &hsd.CID, sizeof(SD_Handle.CID)); return SD_OK; } @@ -525,15 +600,24 @@ SD_Error_t SD_WriteBlocks_DMA(uint64_t WriteAddress, uint32_t *buffer, uint32_t return SD_ERROR; // unsupported. } +#ifndef STM32F7 if ((uint32_t)buffer & 0x1f) { return SD_ADDR_MISALIGNED; } +#endif // Ensure the data is flushed to main memory SCB_CleanDCache_by_Addr(buffer, NumberOfBlocks * BlockSize); - HAL_StatusTypeDef status; - if ((status = HAL_SD_WriteBlocks_DMA(&hsd1, (uint8_t *)buffer, WriteAddress, NumberOfBlocks)) != HAL_OK) { +#ifdef STM32F7 + if (sd_dma.Init.Direction != DMA_MEMORY_TO_PERIPH) { + sd_dma.Init.Direction = DMA_MEMORY_TO_PERIPH; + if (HAL_DMA_Init(&sd_dma) != HAL_OK) { + return SD_ERROR; + } + } +#endif + if (HAL_SD_WriteBlocks_DMA(&hsd, (uint8_t *)buffer, WriteAddress, NumberOfBlocks) != HAL_OK) { return SD_ERROR; } @@ -556,9 +640,11 @@ SD_Error_t SD_ReadBlocks_DMA(uint64_t ReadAddress, uint32_t *buffer, uint32_t Bl return SD_ERROR; // unsupported. } +#ifndef STM32F7 if ((uint32_t)buffer & 0x1f) { return SD_ADDR_MISALIGNED; } +#endif SD_Handle.RXCplt = 1; @@ -566,8 +652,15 @@ SD_Error_t SD_ReadBlocks_DMA(uint64_t ReadAddress, uint32_t *buffer, uint32_t Bl sdReadParameters.BlockSize = BlockSize; sdReadParameters.NumberOfBlocks = NumberOfBlocks; - HAL_StatusTypeDef status; - if ((status = HAL_SD_ReadBlocks_DMA(&hsd1, (uint8_t *)buffer, ReadAddress, NumberOfBlocks)) != HAL_OK) { +#ifdef STM32F7 + if (sd_dma.Init.Direction != DMA_PERIPH_TO_MEMORY) { + sd_dma.Init.Direction = DMA_PERIPH_TO_MEMORY; + if (HAL_DMA_Init(&sd_dma) != HAL_OK) { + return SD_ERROR; + } + } +#endif + if (HAL_SD_ReadBlocks_DMA(&hsd, (uint8_t *)buffer, ReadAddress, NumberOfBlocks) != HAL_OK) { return SD_ERROR; } @@ -615,12 +708,12 @@ void HAL_SD_AbortCallback(SD_HandleTypeDef *hsd) void SDMMC1_IRQHandler(void) { - HAL_SD_IRQHandler(&hsd1); + HAL_SD_IRQHandler(&hsd); } void SDMMC2_IRQHandler(void) { - HAL_SD_IRQHandler(&hsd1); + HAL_SD_IRQHandler(&hsd); } #endif diff --git a/src/main/drivers/sdio.h b/src/main/drivers/sdio.h index 2464e1594d..d411a17ad1 100644 --- a/src/main/drivers/sdio.h +++ b/src/main/drivers/sdio.h @@ -29,7 +29,6 @@ typedef enum { #define SDIODEV_COUNT 2 -#if defined(STM32H7) +#if defined(STM32H7) || defined(STM32F7) void sdioPinConfigure(void); -void SDIO_GPIO_Init(void); #endif diff --git a/src/main/drivers/sdmmc_sdio.h b/src/main/drivers/sdmmc_sdio.h deleted file mode 100644 index 075768189d..0000000000 --- a/src/main/drivers/sdmmc_sdio.h +++ /dev/null @@ -1,241 +0,0 @@ -/* - * This file is part of Cleanflight and Betaflight. - * - * Cleanflight and Betaflight are free software. You can redistribute - * this software and/or modify this software under the terms of the - * GNU General Public License as published by the Free Software - * Foundation, either version 3 of the License, or (at your option) - * any later version. - * - * Cleanflight and Betaflight are distributed in the hope that they - * will be useful, but WITHOUT ANY WARRANTY; without even the implied - * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. - * See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this software. - * - * If not, see . - */ - -/* - * Original author: Alain (https://github.com/aroyer-qc) - * Modified for F4 and BF source: Chris Hockuba (https://github.com/conkerkh) - * - */ - -#ifndef __fatfs_sd_sdio_H__ -#define __fatfs_sd_sdio_H__ - -#include - -#include "platform.h" -#ifdef STM32F4 -#include "stm32f4xx.h" -#endif - -#ifdef STM32F7 -#include "stm32f7xx.h" -#endif - - /* SDCARD pinouts - * - * SD CARD PINS - _________________ - / 1 2 3 4 5 6 7 8 | NR |SDIO INTERFACE - / | |NAME STM32F746 DESCRIPTION -/ 9 | | 4-BIT 1-BIT -| | | -| | 1 |CD/DAT3 PC11 - Connector data line 3 -| | 2 |CMD PD2 PD2 Command/Response line -| | 3 |VSS1 GND GND GND -| SD CARD Pinout | 4 |VDD 3.3V 3.3V 3.3V Power supply -| | 5 |CLK PC12 PC12 Clock -| | 6 |VSS2 GND GND GND -| | 7 |DAT0 PC8 PC8 Connector data line 0 -| | 8 |DAT1 PC9 - Connector data line 1 -|___________________| 9 |DAT2 PC10 - Connector data line 2 - - */ - -/* Define(s) --------------------------------------------------------------------------------------------------------*/ - -//#define MSD_OK ((uint8_t)0x00) -#define MSD_ERROR ((uint8_t)0x01) -#define MSD_ERROR_SD_NOT_PRESENT ((uint8_t)0x02) - -#define SD_PRESENT ((uint8_t)0x01) -#define SD_NOT_PRESENT ((uint8_t)0x00) - -#define SD_DATATIMEOUT ((uint32_t)100000000) - -#define SD_DETECT_GPIO_PORT GPIOC -#define SD_DETECT_PIN GPIO_PIN_13 - -/* Structure(s) -----------------------------------------------------------------------------------------------------*/ - -typedef enum -{ - // SD specific error defines - SD_CMD_CRC_FAIL = (1), // Command response received (but CRC check failed) - SD_DATA_CRC_FAIL = (2), // Data block sent/received (CRC check failed) - SD_CMD_RSP_TIMEOUT = (3), // Command response TimeOut - SD_DATA_TIMEOUT = (4), // Data TimeOut - SD_TX_UNDERRUN = (5), // Transmit FIFO underrun - SD_RX_OVERRUN = (6), // Receive FIFO overrun - SD_START_BIT_ERR = (7), // Start bit not detected on all data signals in wide bus mode - SD_CMD_OUT_OF_RANGE = (8), // Command's argument was out of range. - SD_ADDR_MISALIGNED = (9), // Misaligned address - SD_BLOCK_LEN_ERR = (10), // Transferred block length is not allowed for the card or the number of transferred bytes does not match the block length - SD_ERASE_SEQ_ERR = (11), // An error in the sequence of erase command occurs. - SD_BAD_ERASE_PARAM = (12), // An invalid selection for erase groups - SD_WRITE_PROT_VIOLATION = (13), // Attempt to program a write protect block - SD_LOCK_UNLOCK_FAILED = (14), // Sequence or password error has been detected in unlock command or if there was an attempt to access a locked card - SD_COM_CRC_FAILED = (15), // CRC check of the previous command failed - SD_ILLEGAL_CMD = (16), // Command is not legal for the card state - SD_CARD_ECC_FAILED = (17), // Card internal ECC was applied but failed to correct the data - SD_CC_ERROR = (18), // Internal card controller error - SD_GENERAL_UNKNOWN_ERROR = (19), // General or unknown error - SD_STREAM_READ_UNDERRUN = (20), // The card could not sustain data transfer in stream read operation. - SD_STREAM_WRITE_OVERRUN = (21), // The card could not sustain data programming in stream mode - SD_CID_CSD_OVERWRITE = (22), // CID/CSD overwrite error - SD_WP_ERASE_SKIP = (23), // Only partial address space was erased - SD_CARD_ECC_DISABLED = (24), // Command has been executed without using internal ECC - SD_ERASE_RESET = (25), // Erase sequence was cleared before executing because an out of erase sequence command was received - SD_AKE_SEQ_ERROR = (26), // Error in sequence of authentication. - SD_INVALID_VOLTRANGE = (27), - SD_ADDR_OUT_OF_RANGE = (28), - SD_SWITCH_ERROR = (29), - SD_SDMMC_DISABLED = (30), - SD_SDMMC_FUNCTION_BUSY = (31), - SD_SDMMC_FUNCTION_FAILED = (32), - SD_SDMMC_UNKNOWN_FUNCTION = (33), - SD_OUT_OF_BOUND = (34), - - - // Standard error defines - SD_INTERNAL_ERROR = (35), - SD_NOT_CONFIGURED = (36), - SD_REQUEST_PENDING = (37), - SD_REQUEST_NOT_APPLICABLE = (38), - SD_INVALID_PARAMETER = (39), - SD_UNSUPPORTED_FEATURE = (40), - SD_UNSUPPORTED_HW = (41), - SD_ERROR = (42), - SD_BUSY = (43), - SD_OK = (0) -} SD_Error_t; - - -typedef struct -{ - uint8_t DAT_BUS_WIDTH; // Shows the currently defined data bus width - uint8_t SECURED_MODE; // Card is in secured mode of operation - uint16_t SD_CARD_TYPE; // Carries information about card type - uint32_t SIZE_OF_PROTECTED_AREA; // Carries information about the capacity of protected area - uint8_t SPEED_CLASS; // Carries information about the speed class of the card - uint8_t PERFORMANCE_MOVE; // Carries information about the card's performance move - uint8_t AU_SIZE; // Carries information about the card's allocation unit size - uint16_t ERASE_SIZE; // Determines the number of AUs to be erased in one operation - uint8_t ERASE_TIMEOUT; // Determines the TimeOut for any number of AU erase - uint8_t ERASE_OFFSET; // Carries information about the erase offset -} SD_CardStatus_t; - -typedef struct -{ - uint8_t CSDStruct; // CSD structure - uint8_t SysSpecVersion; // System specification version - uint8_t Reserved1; // Reserved - uint8_t TAAC; // Data read access time 1 - uint8_t NSAC; // Data read access time 2 in CLK cycles - uint8_t MaxBusClkFrec; // Max. bus clock frequency - uint16_t CardComdClasses; // Card command classes - uint8_t RdBlockLen; // Max. read data block length - uint8_t PartBlockRead; // Partial blocks for read allowed - uint8_t WrBlockMisalign; // Write block misalignment - uint8_t RdBlockMisalign; // Read block misalignment - uint8_t DSRImpl; // DSR implemented - uint8_t Reserved2; // Reserved - uint32_t DeviceSize; // Device Size - uint8_t MaxRdCurrentVDDMin; // Max. read current @ VDD min - uint8_t MaxRdCurrentVDDMax; // Max. read current @ VDD max - uint8_t MaxWrCurrentVDDMin; // Max. write current @ VDD min - uint8_t MaxWrCurrentVDDMax; // Max. write current @ VDD max - uint8_t DeviceSizeMul; // Device size multiplier - uint8_t EraseGrSize; // Erase group size - uint8_t EraseGrMul; // Erase group size multiplier - uint8_t WrProtectGrSize; // Write protect group size - uint8_t WrProtectGrEnable; // Write protect group enable - uint8_t ManDeflECC; // Manufacturer default ECC - uint8_t WrSpeedFact; // Write speed factor - uint8_t MaxWrBlockLen; // Max. write data block length - uint8_t WriteBlockPaPartial; // Partial blocks for write allowed - uint8_t Reserved3; // Reserved - uint8_t ContentProtectAppli; // Content protection application - uint8_t FileFormatGrouop; // File format group - uint8_t CopyFlag; // Copy flag (OTP) - uint8_t PermWrProtect; // Permanent write protection - uint8_t TempWrProtect; // Temporary write protection - uint8_t FileFormat; // File format - uint8_t ECC; // ECC code - uint8_t CSD_CRC; // CSD CRC - uint8_t Reserved4; // Always 1 -} SD_CSD_t; - -typedef struct -{ - uint8_t ManufacturerID; // Manufacturer ID - uint16_t OEM_AppliID; // OEM/Application ID - uint32_t ProdName1; // Product Name part1 - uint8_t ProdName2; // Product Name part2 - uint8_t ProdRev; // Product Revision - uint32_t ProdSN; // Product Serial Number - uint8_t Reserved1; // Reserved1 - uint16_t ManufactDate; // Manufacturing Date - uint8_t CID_CRC; // CID CRC - uint8_t Reserved2; // Always 1 - -} SD_CID_t; - -typedef enum -{ - SD_STD_CAPACITY_V1_1 = 0, - SD_STD_CAPACITY_V2_0 = 1, - SD_HIGH_CAPACITY = 2, - SD_MULTIMEDIA = 3, - SD_SECURE_DIGITAL_IO = 4, - SD_HIGH_SPEED_MULTIMEDIA = 5, - SD_SECURE_DIGITAL_IO_COMBO = 6, - SD_HIGH_CAPACITY_MMC = 7, -} SD_CardType_t; - -typedef struct -{ - volatile SD_CSD_t SD_csd; // SD card specific data register - volatile SD_CID_t SD_cid; // SD card identification number register - uint64_t CardCapacity; // Card capacity - uint32_t CardBlockSize; // Card block size -} SD_CardInfo_t; - -/* Prototype(s) -----------------------------------------------------------------------------------------------------*/ - -extern SD_CardInfo_t SD_CardInfo; -extern SD_CardType_t SD_CardType; - -void SD_Initialize_LL (DMA_Stream_TypeDef *dma); -bool SD_Init (void); -bool SD_IsDetected (void); -bool SD_GetState (void); -SD_Error_t SD_GetCardInfo (void); - -SD_Error_t SD_ReadBlocks_DMA (uint64_t ReadAddress, uint32_t *buffer, uint32_t BlockSize, uint32_t NumberOfBlocks); -SD_Error_t SD_CheckRead (void); -SD_Error_t SD_WriteBlocks_DMA (uint64_t WriteAddress, uint32_t *buffer, uint32_t BlockSize, uint32_t NumberOfBlocks); -SD_Error_t SD_CheckWrite (void); - -SD_Error_t SD_Erase (uint64_t StartAddress, uint64_t EndAddress); -SD_Error_t SD_GetCardStatus (SD_CardStatus_t* pCardStatus); - -/* ------------------------------------------------------------------------------------------------------------------*/ - -#endif // __fatfs_sd_sdio_H__ diff --git a/src/main/drivers/serial_uart_at32f43x.c b/src/main/drivers/serial_uart_at32f43x.c index 90ebe05ef8..f58e0a956a 100644 --- a/src/main/drivers/serial_uart_at32f43x.c +++ b/src/main/drivers/serial_uart_at32f43x.c @@ -36,25 +36,38 @@ typedef struct uartDevice_s { usart_type* dev; uartPort_t port; ioTag_t rx; + uint8_t rx_af; ioTag_t tx; + uint8_t tx_af; volatile uint8_t rxBuffer[UART_RX_BUFFER_SIZE]; volatile uint8_t txBuffer[UART_TX_BUFFER_SIZE]; uint32_t rcc_ahb1; rccPeriphTag_t rcc_apb2; rccPeriphTag_t rcc_apb1; - uint8_t af; uint8_t irq; uint32_t irqPriority; bool pinSwap; } uartDevice_t; #ifdef USE_UART1 -static uartDevice_t uart1 = -{ +static uartDevice_t uart1 = { .dev = USART1, .rx = IO_TAG(UART1_RX_PIN), .tx = IO_TAG(UART1_TX_PIN), - .af = GPIO_MUX_7, +#if defined(UART1_RX_AF) + .rx_af = CONCAT(GPIO_MUX_, UART1_RX_AF), +#elif defined(UART1_AF) + .rx_af = CONCAT(GPIO_MUX_, UART1_AF), +#else + .rx_af = GPIO_MUX_7, +#endif +#if defined(UART1_TX_AF) + .tx_af = CONCAT(GPIO_MUX_, UART1_TX_AF), +#elif defined(UART1_AF) + .tx_af = CONCAT(GPIO_MUX_, UART1_AF), +#else + .tx_af = GPIO_MUX_7, +#endif #ifdef UART1_AHB1_PERIPHERALS .rcc_ahb1 = UART1_AHB1_PERIPHERALS, #endif @@ -75,7 +88,20 @@ static uartDevice_t uart2 = .dev = USART2, .rx = IO_TAG(UART2_RX_PIN), .tx = IO_TAG(UART2_TX_PIN), - .af = GPIO_MUX_7, +#if defined(UART2_RX_AF) + .rx_af = CONCAT(GPIO_MUX_, UART2_RX_AF), +#elif defined(UART2_AF) + .rx_af = CONCAT(GPIO_MUX_, UART2_AF), +#else + .rx_af = GPIO_MUX_7, +#endif +#if defined(UART2_TX_AF) + .tx_af = CONCAT(GPIO_MUX_, UART2_TX_AF), +#elif defined(UART2_AF) + .tx_af = CONCAT(GPIO_MUX_, UART2_AF), +#else + .tx_af = GPIO_MUX_7, +#endif #ifdef UART2_AHB1_PERIPHERALS .rcc_ahb1 = UART2_AHB1_PERIPHERALS, #endif @@ -96,7 +122,20 @@ static uartDevice_t uart3 = .dev = USART3, .rx = IO_TAG(UART3_RX_PIN), .tx = IO_TAG(UART3_TX_PIN), - .af = GPIO_MUX_7, +#if defined(UART3_RX_AF) + .rx_af = CONCAT(GPIO_MUX_, UART3_RX_AF), +#elif defined(UART3_AF) + .rx_af = CONCAT(GPIO_MUX_, UART3_AF), +#else + .rx_af = GPIO_MUX_7, +#endif +#if defined(UART3_TX_AF) + .tx_af = CONCAT(GPIO_MUX_, UART3_TX_AF), +#elif defined(UART3_AF) + .tx_af = CONCAT(GPIO_MUX_, UART3_AF), +#else + .tx_af = GPIO_MUX_7, +#endif #ifdef UART3_AHB1_PERIPHERALS .rcc_ahb1 = UART3_AHB1_PERIPHERALS, #endif @@ -117,7 +156,20 @@ static uartDevice_t uart4 = .dev = UART4, .rx = IO_TAG(UART4_RX_PIN), .tx = IO_TAG(UART4_TX_PIN), - .af = GPIO_MUX_8, +#if defined(UART4_RX_AF) + .rx_af = CONCAT(GPIO_MUX_, UART4_RX_AF), +#elif defined(UART4_AF) + .rx_af = CONCAT(GPIO_MUX_, UART4_AF), +#else + .rx_af = GPIO_MUX_8, +#endif +#if defined(UART4_TX_AF) + .tx_af = CONCAT(GPIO_MUX_, UART4_TX_AF), +#elif defined(UART4_AF) + .tx_af = CONCAT(GPIO_MUX_, UART4_AF), +#else + .tx_af = GPIO_MUX_8, +#endif #ifdef UART4_AHB1_PERIPHERALS .rcc_ahb1 = UART4_AHB1_PERIPHERALS, #endif @@ -138,7 +190,20 @@ static uartDevice_t uart5 = .dev = UART5, .rx = IO_TAG(UART5_RX_PIN), .tx = IO_TAG(UART5_TX_PIN), - .af = GPIO_MUX_8, +#if defined(UART5_RX_AF) + .rx_af = CONCAT(GPIO_MUX_, UART5_RX_AF), +#elif defined(UART5_AF) + .rx_af = CONCAT(GPIO_MUX_, UART5_AF), +#else + .rx_af = GPIO_MUX_8, +#endif +#if defined(UART5_TX_AF) + .tx_af = CONCAT(GPIO_MUX_, UART5_TX_AF), +#elif defined(UART5_AF) + .tx_af = CONCAT(GPIO_MUX_, UART5_AF), +#else + .tx_af = GPIO_MUX_8, +#endif #ifdef UART5_AHB1_PERIPHERALS .rcc_ahb1 = UART5_AHB1_PERIPHERALS, #endif @@ -159,7 +224,20 @@ static uartDevice_t uart6 = .dev = USART6, .rx = IO_TAG(UART6_RX_PIN), .tx = IO_TAG(UART6_TX_PIN), - .af = GPIO_MUX_8, +#if defined(UART6_RX_AF) + .rx_af = CONCAT(GPIO_MUX_, UART6_RX_AF), +#elif defined(UART6_AF) + .rx_af = CONCAT(GPIO_MUX_, UART6_AF), +#else + .rx_af = GPIO_MUX_8, +#endif +#if defined(UART6_TX_AF) + .tx_af = CONCAT(GPIO_MUX_, UART6_TX_AF), +#elif defined(UART6_AF) + .tx_af = CONCAT(GPIO_MUX_, UART6_AF), +#else + .tx_af = GPIO_MUX_8, +#endif #ifdef UART6_AHB1_PERIPHERALS .rcc_ahb1 = UART6_AHB1_PERIPHERALS, #endif @@ -180,7 +258,20 @@ static uartDevice_t uart7 = .dev = UART7, .rx = IO_TAG(UART7_RX_PIN), .tx = IO_TAG(UART7_TX_PIN), - .af = GPIO_MUX_8, +#if defined(UART7_RX_AF) + .rx_af = CONCAT(GPIO_MUX_, UART7_RX_AF), +#elif defined(UART7_AF) + .rx_af = CONCAT(GPIO_MUX_, UART7_AF), +#else + .rx_af = GPIO_MUX_8, +#endif +#if defined(UART7_TX_AF) + .tx_af = CONCAT(GPIO_MUX_, UART7_TX_AF), +#elif defined(UART7_AF) + .tx_af = CONCAT(GPIO_MUX_, UART7_AF), +#else + .tx_af = GPIO_MUX_8, +#endif .rcc_apb1 = RCC_APB1(UART7), .irq = UART7_IRQn, .irqPriority = NVIC_PRIO_SERIALUART, @@ -198,7 +289,20 @@ static uartDevice_t uart8 = .dev = UART8, .rx = IO_TAG(UART8_RX_PIN), .tx = IO_TAG(UART8_TX_PIN), - .af = GPIO_MUX_8, +#if defined(UART8_RX_AF) + .rx_af = CONCAT(GPIO_MUX_, UART8_RX_AF), +#elif defined(UART8_AF) + .rx_af = CONCAT(GPIO_MUX_, UART8_AF), +#else + .rx_af = GPIO_MUX_8, +#endif +#if defined(UART8_TX_AF) + .tx_af = CONCAT(GPIO_MUX_, UART8_TX_AF), +#elif defined(UART8_AF) + .tx_af = CONCAT(GPIO_MUX_, UART8_AF), +#else + .tx_af = GPIO_MUX_8, +#endif .rcc_apb1 = RCC_APB1(UART8), .irq = UART8_IRQn, .irqPriority = NVIC_PRIO_SERIALUART, @@ -357,22 +461,22 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_t mode, if (options & SERIAL_BIDIR) { IOInit(tx, OWNER_SERIAL, RESOURCE_UART_TXRX, RESOURCE_INDEX(device)); if (options & SERIAL_BIDIR_PP) { - IOConfigGPIOAF(tx, IOCFG_AF_PP, uart->af); + IOConfigGPIOAF(tx, IOCFG_AF_PP, uart->tx_af); } else { IOConfigGPIOAF(tx, (options & SERIAL_BIDIR_NOPULL) ? IOCFG_AF_OD : IOCFG_AF_OD_UP, - uart->af); + uart->tx_af); } } else { if (mode & MODE_TX) { IOInit(tx, OWNER_SERIAL, RESOURCE_UART_TX, RESOURCE_INDEX(device)); - IOConfigGPIOAF(tx, IOCFG_AF_PP, uart->af); + IOConfigGPIOAF(tx, IOCFG_AF_PP, uart->rx_af); } if (mode & MODE_RX) { IOInit(rx, OWNER_SERIAL, RESOURCE_UART_RX, RESOURCE_INDEX(device)); - IOConfigGPIOAF(rx, IOCFG_AF_PP, uart->af); + IOConfigGPIOAF(rx, IOCFG_AF_PP, uart->rx_af); } } diff --git a/src/main/drivers/timer_def_stm32h7xx.h b/src/main/drivers/timer_def_stm32h7xx.h index ef4d60166f..649a037eb8 100644 --- a/src/main/drivers/timer_def_stm32h7xx.h +++ b/src/main/drivers/timer_def_stm32h7xx.h @@ -87,8 +87,10 @@ #define DEF_TIM_DMA__BTCH_TIM15_CH2 NONE #define DEF_TIM_DMA__BTCH_TIM16_CH1 DEF_TIM_DMA_FULL(DMA_REQUEST_TIM16_CH1) +#define DEF_TIM_DMA__BTCH_TIM16_CH1N NONE #define DEF_TIM_DMA__BTCH_TIM17_CH1 DEF_TIM_DMA_FULL(DMA_REQUEST_TIM17_CH1) +#define DEF_TIM_DMA__BTCH_TIM17_CH1N NONE // TIM_UP table #define DEF_TIM_DMA__BTCH_TIM1_UP DEF_TIM_DMA_FULL(DMA_REQUEST_TIM1_UP) diff --git a/src/main/drivers/uart_inverter.c b/src/main/drivers/uart_inverter.c index b815883df7..5168831b47 100644 --- a/src/main/drivers/uart_inverter.c +++ b/src/main/drivers/uart_inverter.c @@ -145,7 +145,12 @@ void uartInverterSet(USART_TypeDef *USARTx, uartInverterLine_e line, bool enable // UART4 #if defined(INVERTER_PIN_UART4_RX) || defined(INVERTER_PIN_UART4_TX) - if (USARTx == USART4) { +#if defined(STM32F4) + if (USARTx == UART4) +#else + if (USARTx == USART4) +#endif + { #if defined(INVERTER_PIN_UART4_RX) rx_pin = IOGetByTag(IO_TAG(INVERTER_PIN_UART4_RX)); #endif diff --git a/src/main/drivers/usb_msc_f4xx.c b/src/main/drivers/usb_msc_f4xx.c index 8bc9a9ade3..43fcec9b73 100644 --- a/src/main/drivers/usb_msc_f4xx.c +++ b/src/main/drivers/usb_msc_f4xx.c @@ -42,7 +42,7 @@ #include "drivers/light_led.h" #include "drivers/nvic.h" #include "drivers/persistent.h" -#include "drivers/sdmmc_sdio.h" +#include "drivers/sdcard/sdmmc_sdio.h" #include "drivers/time.h" #include "drivers/usb_msc.h" diff --git a/src/main/drivers/vtx_common.h b/src/main/drivers/vtx_common.h index 80b957c5d1..83e608dc49 100644 --- a/src/main/drivers/vtx_common.h +++ b/src/main/drivers/vtx_common.h @@ -36,11 +36,11 @@ #define VTX_SETTINGS_POWER_COUNT 8 #define VTX_SETTINGS_DEFAULT_POWER 1 -#define VTX_SETTINGS_MIN_POWER 1 +#define VTX_SETTINGS_MIN_POWER 0 #define VTX_SETTINGS_MIN_USER_FREQ 5000 #define VTX_SETTINGS_MAX_USER_FREQ 5999 #define VTX_SETTINGS_FREQCMD -#define VTX_SETTINGS_MAX_POWER (VTX_SETTINGS_POWER_COUNT - VTX_SETTINGS_MIN_POWER + 1) +#define VTX_SETTINGS_MAX_POWER (VTX_SETTINGS_POWER_COUNT - VTX_SETTINGS_MIN_POWER) #else diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index ee5cb289a9..62697014c3 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -151,7 +151,7 @@ static uint8_t commandBatchErrorCount = 0; // sync this with features_e static const char * const featureNames[] = { - "THR_VBAT_COMP", "VBAT", "TX_PROF_SEL", "BAT_PROF_AUTOSWITCH", "MOTOR_STOP", + "THR_VBAT_COMP", "VBAT", "TX_PROF_SEL", "BAT_PROF_AUTOSWITCH", "GEOZONE", "", "SOFTSERIAL", "GPS", "RPM_FILTERS", "", "TELEMETRY", "CURRENT_METER", "REVERSIBLE_MOTORS", "", "", "RSSI_ADC", "LED_STRIP", "DASHBOARD", "", @@ -1561,6 +1561,285 @@ static void cliSafeHomes(char *cmdline) } #endif + +#if defined(USE_GEOZONE) +static void printGeozones(uint8_t dumpMask, const geoZoneConfig_t *geoZone, const geoZoneConfig_t *defaultGeoZone) +{ + const char *format = "geozone %u %u %u %d %d %u %u %u"; + for (uint8_t i = 0; i < MAX_GEOZONES_IN_CONFIG; i++) { + bool equalsDefault = false; + if (defaultGeoZone) { + equalsDefault = geoZone[i].fenceAction == defaultGeoZone->fenceAction + && geoZone[i].shape == defaultGeoZone->shape + && geoZone[i].type == defaultGeoZone->type + && geoZone[i].maxAltitude == defaultGeoZone->maxAltitude + && geoZone[i].minAltitude == defaultGeoZone->minAltitude + && geoZone[i].isSealevelRef == defaultGeoZone->isSealevelRef + && geoZone[i].fenceAction == defaultGeoZone->fenceAction + && geoZone[i].vertexCount == defaultGeoZone->vertexCount; + + cliDefaultPrintLinef(dumpMask, equalsDefault, format, defaultGeoZone[i].shape, defaultGeoZone[i].type, defaultGeoZone[i].minAltitude, defaultGeoZone[i].maxAltitude, defaultGeoZone[i].isSealevelRef, defaultGeoZone[i].fenceAction, defaultGeoZone[i].vertexCount); + } + cliDumpPrintLinef(dumpMask, equalsDefault, format, i, geoZone[i].shape, geoZone[i].type, geoZone[i].minAltitude, geoZone[i].maxAltitude, geoZone[i].isSealevelRef, geoZone[i].fenceAction, geoZone[i].vertexCount); + } +} + +static void printGeozoneVertices(uint8_t dumpMask, const vertexConfig_t *vertices, const vertexConfig_t *defaultVertices) +{ + const char *format = "geozone vertex %d %u %d %d"; + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + bool equalsDefault = false; + if (defaultVertices) { + equalsDefault = vertices[i].idx == defaultVertices->idx + && vertices[i].lat == defaultVertices->lat + && vertices[i].lon == defaultVertices->lon + && vertices[i].zoneId == defaultVertices->zoneId; + + cliDefaultPrintLinef(dumpMask, equalsDefault, format, defaultVertices[i].zoneId, defaultVertices[i].idx, defaultVertices[i].lat, defaultVertices[i].lon); + } + + cliDumpPrintLinef(dumpMask, equalsDefault, format, vertices[i].zoneId, vertices[i].idx, vertices[i].lat, vertices[i].lon); + } + + if (!defaultVertices) { + uint8_t totalVertices = geozoneGetUsedVerticesCount(); + cliPrintLinef("# %u vertices free (Used %u of %u)", MAX_VERTICES_IN_CONFIG - totalVertices, totalVertices, MAX_VERTICES_IN_CONFIG); + } +} + +static void cliGeozone(char* cmdLine) +{ + if (isEmpty(cmdLine)) { + printGeozones(DUMP_MASTER, geoZonesConfig(0), NULL); + } else if (sl_strcasecmp(cmdLine, "vertex") == 0) { + printGeozoneVertices(DUMP_MASTER, geoZoneVertices(0), NULL); + } else if (sl_strncasecmp(cmdLine, "vertex reset", 12) == 0) { + const char* ptr = &cmdLine[12]; + uint8_t zoneId = 0, idx = 0; + uint8_t argumentCount = 1; + + if ((ptr = nextArg(ptr))) { + zoneId = fastA2I(ptr); + } else { + geozoneResetVertices(-1, -1); + return; + } + + if ((ptr = nextArg(ptr))) { + argumentCount++; + idx = fastA2I(ptr); + } else { + geozoneResetVertices(zoneId, -1); + return; + } + + if (argumentCount != 2) { + cliShowParseError(); + return; + } + + geozoneResetVertices(zoneId, idx); + + } else if (sl_strncasecmp(cmdLine, "vertex", 6) == 0) { + int32_t lat = 0, lon = 0; + int8_t zoneId = 0; + int16_t vertexIdx = -1; + uint8_t vertexZoneIdx = 0; + const char* ptr = cmdLine; + uint8_t argumentCount = 1; + + if ((ptr = nextArg(ptr))) { + zoneId = fastA2I(ptr); + if (zoneId < 0) { + return; + } + + if (zoneId >= MAX_GEOZONES_IN_CONFIG) { + cliShowArgumentRangeError("geozone index", 0, MAX_GEOZONES_IN_CONFIG - 1); + return; + } + } else { + cliShowParseError(); + return; + } + + if ((ptr = nextArg(ptr))) { + argumentCount++; + vertexZoneIdx = fastA2I(ptr); + } else { + cliShowParseError(); + return; + } + + if ((ptr = nextArg(ptr))) { + argumentCount++; + lat = fastA2I(ptr); + } else { + cliShowParseError(); + return; + } + + if ((ptr = nextArg(ptr))) { + argumentCount++; + lon = fastA2I(ptr); + } else { + cliShowParseError(); + return; + } + + if ((ptr = nextArg(ptr))) { + argumentCount++; + } + + if (argumentCount != 4) { + cliShowParseError(); + return; + } + + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + if (geoZoneVertices(i)->zoneId == zoneId && geoZoneVertices(i)->idx == vertexZoneIdx) { + geoZoneVerticesMutable(i)->lat = lat; + geoZoneVerticesMutable(i)->lon = lon; + return; + } + } + + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + if (geoZoneVertices(i)->zoneId == -1) { + vertexIdx = i; + break; + } + } + + if (vertexIdx < 0 || vertexIdx >= MAX_VERTICES_IN_CONFIG || vertexZoneIdx > MAX_VERTICES_IN_CONFIG) { + cliPrintError("Maximum number of vertices reached."); + return; + } + + geoZoneVerticesMutable(vertexIdx)->lat = lat; + geoZoneVerticesMutable(vertexIdx)->lon = lon; + geoZoneVerticesMutable(vertexIdx)->zoneId = zoneId; + geoZoneVerticesMutable(vertexIdx)->idx = vertexZoneIdx; + + uint8_t totalVertices = geozoneGetUsedVerticesCount(); + cliPrintLinef("# %u vertices free (Used %u of %u)", MAX_VERTICES_IN_CONFIG - totalVertices, totalVertices, MAX_VERTICES_IN_CONFIG); + + } else if (sl_strncasecmp(cmdLine, "reset", 5) == 0) { + const char* ptr = &cmdLine[5]; + if ((ptr = nextArg(ptr))) { + int idx = fastA2I(ptr); + geozoneReset(idx); + geozoneResetVertices(idx, -1); + } else { + geozoneReset(-1); + geozoneResetVertices(-1, -1); + } + } else { + int8_t idx = 0, isPolygon = 0, isInclusive = 0, fenceAction = 0, seaLevelRef = 0, vertexCount = 0; + int32_t minAltitude = 0, maxAltitude = 0; + const char* ptr = cmdLine; + uint8_t argumentCount = 1; + + idx = fastA2I(ptr); + if (idx < 0 || idx > MAX_GEOZONES_IN_CONFIG) { + cliShowArgumentRangeError("geozone index", 0, MAX_GEOZONES_IN_CONFIG - 1); + return; + } + + if ((ptr = nextArg(ptr))) { + argumentCount++; + isPolygon = fastA2I(ptr); + } else { + cliShowParseError(); + return; + } + + if ((ptr = nextArg(ptr))){ + argumentCount++; + isInclusive = fastA2I(ptr); + } else { + cliShowParseError(); + return; + } + + if ((ptr = nextArg(ptr))){ + argumentCount++; + minAltitude = fastA2I(ptr); + } else { + cliShowParseError(); + return; + } + + if ((ptr = nextArg(ptr))){ + argumentCount++; + maxAltitude = fastA2I(ptr); + } else { + cliShowParseError(); + return; + } + + if ((ptr = nextArg(ptr))){ + argumentCount++; + seaLevelRef = fastA2I(ptr); + } else { + cliShowParseError(); + return; + } + + if ((ptr = nextArg(ptr))){ + argumentCount++; + fenceAction = fastA2I(ptr); + if (fenceAction < 0 || fenceAction > GEOFENCE_ACTION_RTH) { + cliShowArgumentRangeError("fence action", 0, GEOFENCE_ACTION_RTH); + return; + } + } else { + cliShowParseError(); + return; + } + + if ((ptr = nextArg(ptr))){ + argumentCount++; + vertexCount = fastA2I(ptr); + if (vertexCount < 1 || vertexCount > MAX_VERTICES_IN_CONFIG) { + cliShowArgumentRangeError("vertex count", 1, MAX_VERTICES_IN_CONFIG); + return; + } + } else { + cliShowParseError(); + return; + } + + if ((ptr = nextArg(ptr))){ + argumentCount++; + } + + if (argumentCount != 8) { + cliShowParseError(); + return; + } + + if (isPolygon) { + geoZonesConfigMutable(idx)->shape = GEOZONE_SHAPE_POLYGON; + } else { + geoZonesConfigMutable(idx)->shape = GEOZONE_SHAPE_CIRCULAR; + } + + if (isInclusive) { + geoZonesConfigMutable(idx)->type = GEOZONE_TYPE_INCLUSIVE; + } else { + geoZonesConfigMutable(idx)->type = GEOZONE_TYPE_EXCLUSIVE; + } + + geoZonesConfigMutable(idx)->maxAltitude = maxAltitude; + geoZonesConfigMutable(idx)->minAltitude = minAltitude; + geoZonesConfigMutable(idx)->isSealevelRef = (bool)seaLevelRef; + geoZonesConfigMutable(idx)->fenceAction = fenceAction; + geoZonesConfigMutable(idx)->vertexCount = vertexCount; + } +} +#endif + #if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE) && defined(NAV_NON_VOLATILE_WAYPOINT_CLI) static void printWaypoints(uint8_t dumpMask, const navWaypoint_t *navWaypoint, const navWaypoint_t *defaultNavWaypoint) { @@ -2524,11 +2803,11 @@ static void osdCustom(char *cmdline){ int32_t i = args[INDEX]; if ( i >= 0 && i < MAX_CUSTOM_ELEMENTS && - args[PART0_TYPE] >= 0 && args[PART0_TYPE] <= 7 && + args[PART0_TYPE] >= 0 && args[PART0_TYPE] < CUSTOM_ELEMENT_TYPE_END && args[PART0_VALUE] >= 0 && args[PART0_VALUE] <= UINT8_MAX && - args[PART1_TYPE] >= 0 && args[PART1_TYPE] <= 7 && + args[PART1_TYPE] >= 0 && args[PART1_TYPE] < CUSTOM_ELEMENT_TYPE_END && args[PART1_VALUE] >= 0 && args[PART1_VALUE] <= UINT8_MAX && - args[PART2_TYPE] >= 0 && args[PART2_TYPE] <= 7 && + args[PART2_TYPE] >= 0 && args[PART2_TYPE] < CUSTOM_ELEMENT_TYPE_END && args[PART2_VALUE] >= 0 && args[PART2_VALUE] <= UINT8_MAX && args[VISIBILITY_TYPE] >= 0 && args[VISIBILITY_TYPE] <= 2 && args[VISIBILITY_VALUE] >= 0 && args[VISIBILITY_VALUE] <= UINT8_MAX @@ -3714,6 +3993,17 @@ static void cliSet(char *cmdline) } if (changeValue) { + // If changing the battery capacity unit, update the osd stats energy unit to match + if (strcmp(name, "battery_capacity_unit") == 0) { + if (batteryMetersConfig()->capacity_unit != (uint8_t)tmp.int_value) { + if (tmp.int_value == BAT_CAPACITY_UNIT_MAH) { + osdConfigMutable()->stats_energy_unit = OSD_STATS_ENERGY_UNIT_MAH; + } else { + osdConfigMutable()->stats_energy_unit = OSD_STATS_ENERGY_UNIT_WH; + } + } + } + cliSetIntFloatVar(val, tmp); cliPrintf("%s set to ", name); @@ -4167,6 +4457,14 @@ static void printConfig(const char *cmdline, bool doDiff) printFwAutolandApproach(dumpMask, fwAutolandApproachConfig_CopyArray, fwAutolandApproachConfig(0)); #endif +#if defined(USE_GEOZONE) + cliPrintHashLine("geozone"); + printGeozones(dumpMask, geoZonesConfig_CopyArray, geoZonesConfig(0)); + + cliPrintHashLine("geozone vertices"); + printGeozoneVertices(dumpMask, geoZoneVertices_CopyArray, geoZoneVertices(0)); +#endif + cliPrintHashLine("features"); printFeature(dumpMask, &featureConfig_Copy, featureConfig()); @@ -4550,6 +4848,9 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("fwapproach", "Fixed Wing Approach Settings", NULL, cliFwAutolandApproach), #endif CLI_COMMAND_DEF("get", "get variable value", "[name]", cliGet), +#ifdef USE_GEOZONE + CLI_COMMAND_DEF("geozone", "get or set geo zones", NULL, cliGeozone), +#endif #ifdef USE_GPS CLI_COMMAND_DEF("gpspassthrough", "passthrough gps to serial", NULL, cliGpsPassthrough), CLI_COMMAND_DEF("gpssats", "show GPS satellites", NULL, cliUbloxPrintSatelites), diff --git a/src/main/fc/config.h b/src/main/fc/config.h index ddb6a826b8..17c8ded8c1 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -36,7 +36,7 @@ typedef enum { FEATURE_VBAT = 1 << 1, FEATURE_TX_PROF_SEL = 1 << 2, // Profile selection by TX stick command FEATURE_BAT_PROFILE_AUTOSWITCH = 1 << 3, - FEATURE_UNUSED_12 = 1 << 4, //was FEATURE_MOTOR_STOP + FEATURE_GEOZONE = 1 << 4, //was FEATURE_MOTOR_STOP FEATURE_UNUSED_1 = 1 << 5, // was FEATURE_SERVO_TILT was FEATURE_DYNAMIC_FILTERS FEATURE_SOFTSERIAL = 1 << 6, FEATURE_GPS = 1 << 7, diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index f7603eee69..b2a6c1c802 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -278,6 +278,14 @@ static void updateArmingStatus(void) } #endif +#ifdef USE_GEOZONE + if (feature(FEATURE_GEOZONE) && geozoneIsBlockingArming()) { + ENABLE_ARMING_FLAG(ARMING_DISABLED_GEOZONE); + } else { + DISABLE_ARMING_FLAG(ARMING_DISABLED_GEOZONE); + } +#endif + /* CHECK: */ if ( sensors(SENSOR_ACC) && diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index e9ed6efe4c..413b6914e1 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -372,9 +372,8 @@ void init(void) updateHardwareRevision(); #endif -#if defined(USE_SDCARD_SDIO) && defined(STM32H7) +#if defined(USE_SDCARD_SDIO) && (defined(STM32H7) || defined(STM32F7)) sdioPinConfigure(); - SDIO_GPIO_Init(); #endif #ifdef USE_USB_MSC @@ -524,6 +523,10 @@ void init(void) ezTuneUpdate(); #endif +#ifndef USE_GEOZONE + featureClear(FEATURE_GEOZONE); +#endif + if (!sensorsAutodetect()) { // if gyro was not detected due to whatever reason, we give up now. failureMode(FAILURE_MISSING_ACC); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index f82f3cbaea..b4b6fce282 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -34,6 +34,7 @@ #include "common/color.h" #include "common/maths.h" #include "common/streambuf.h" +#include "common/string_light.h" #include "common/bitarray.h" #include "common/time.h" #include "common/utils.h" @@ -215,7 +216,7 @@ static void mspSerialPassthroughFn(serialPort_t *serialPort) static void mspFcSetPassthroughCommand(sbuf_t *dst, sbuf_t *src, mspPostProcessFnPtr *mspPostProcessFn) { - const unsigned int dataSize = sbufBytesRemaining(src); + const unsigned int dataSize = sbufBytesRemaining(src); /* Payload size in Bytes */ if (dataSize == 0) { // Legacy format @@ -1487,11 +1488,18 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, vtxSettingsConfig()->channel); sbufWriteU8(dst, vtxSettingsConfig()->power); sbufWriteU8(dst, pitmode); + // technically there is bug here, we are missing the 16bit + // freqency bf is transmitting + // sbufWriteU16(dst, vtxSettingsConfig()->freq); // Betaflight < 4 doesn't send these fields sbufWriteU8(dst, vtxCommonDeviceIsReady(vtxDevice) ? 1 : 0); sbufWriteU8(dst, vtxSettingsConfig()->lowPowerDisarm); - // future extensions here... + + sbufWriteU8(dst, 1); // vtxtable is available + sbufWriteU8(dst, vtxDevice->capability.bandCount); + sbufWriteU8(dst, vtxDevice->capability.channelCount); + sbufWriteU8(dst, vtxDevice->capability.powerCount); } else { sbufWriteU8(dst, VTXDEV_UNKNOWN); // no VTX configured @@ -1674,6 +1682,18 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF } } break; + + case MSP2_INAV_ESC_TELEM: + { + uint8_t motorCount = getMotorCount(); + sbufWriteU8(dst, motorCount); + + for (uint8_t i = 0; i < motorCount; i++){ + const escSensorData_t *escState = getEscTelemetry(i); //Get ESC telemetry + sbufWriteDataSafe(dst, escState, sizeof(escSensorData_t)); + } + } + break; #endif #ifdef USE_EZ_TUNE @@ -1759,6 +1779,54 @@ static mspResult_e mspFwApproachOutCommand(sbuf_t *dst, sbuf_t *src) } #endif +#ifdef USE_GEOZONE +static mspResult_e mspFcGeozoneOutCommand(sbuf_t *dst, sbuf_t *src) +{ + const uint8_t idx = sbufReadU8(src); + if (idx < MAX_GEOZONES_IN_CONFIG) { + sbufWriteU8(dst, idx); + sbufWriteU8(dst, geoZonesConfig(idx)->type); + sbufWriteU8(dst, geoZonesConfig(idx)->shape); + sbufWriteU32(dst, geoZonesConfig(idx)->minAltitude); + sbufWriteU32(dst, geoZonesConfig(idx)->maxAltitude); + sbufWriteU8(dst, geoZonesConfig(idx)->isSealevelRef); + sbufWriteU8(dst, geoZonesConfig(idx)->fenceAction); + sbufWriteU8(dst, geoZonesConfig(idx)->vertexCount); + return MSP_RESULT_ACK; + } else { + return MSP_RESULT_ERROR; + } +} + +static mspResult_e mspFcGeozoneVerteciesOutCommand(sbuf_t *dst, sbuf_t *src) +{ + const uint8_t zoneId = sbufReadU8(src); + const uint8_t vertexId = sbufReadU8(src); + if (zoneId < MAX_GEOZONES_IN_CONFIG) { + int8_t vertexIdx = geozoneGetVertexIdx(zoneId, vertexId); + if (vertexIdx >= 0) { + sbufWriteU8(dst, geoZoneVertices(vertexIdx)->zoneId); + sbufWriteU8(dst, geoZoneVertices(vertexIdx)->idx); + sbufWriteU32(dst, geoZoneVertices(vertexIdx)->lat); + sbufWriteU32(dst, geoZoneVertices(vertexIdx)->lon); + if (geoZonesConfig(zoneId)->shape == GEOZONE_SHAPE_CIRCULAR) { + int8_t vertexRadiusIdx = geozoneGetVertexIdx(zoneId, vertexId + 1); + if (vertexRadiusIdx >= 0) { + sbufWriteU32(dst, geoZoneVertices(vertexRadiusIdx)->lat); + } else { + return MSP_RESULT_ERROR; + } + } + return MSP_RESULT_ACK; + } else { + return MSP_RESULT_ERROR; + } + } else { + return MSP_RESULT_ERROR; + } +} +#endif + static mspResult_e mspFcLogicConditionCommand(sbuf_t *dst, sbuf_t *src) { const uint8_t idx = sbufReadU8(src); if (idx < MAX_LOGIC_CONDITIONS) { @@ -1795,7 +1863,7 @@ static void mspFcWaypointOutCommand(sbuf_t *dst, sbuf_t *src) #ifdef USE_FLASHFS static void mspFcDataFlashReadCommand(sbuf_t *dst, sbuf_t *src) { - const unsigned int dataSize = sbufBytesRemaining(src); + const unsigned int dataSize = sbufBytesRemaining(src); /* Payload size in Bytes */ uint16_t readLength; const uint32_t readAddress = sbufReadU32(src); @@ -1819,7 +1887,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) uint8_t tmp_u8; uint16_t tmp_u16; - const unsigned int dataSize = sbufBytesRemaining(src); + const unsigned int dataSize = sbufBytesRemaining(src); /* Payload size in Bytes */ switch (cmdMSP) { case MSP_SELECT_SETTING: @@ -2082,6 +2150,7 @@ 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); + uint8_t currentCapacityUnit = batteryMetersConfigMutable()->capacity_unit; batteryMetersConfigMutable()->capacity_unit = sbufReadU8(src); if ((batteryMetersConfig()->voltageSource != BAT_VOLTAGE_RAW) && (batteryMetersConfig()->voltageSource != BAT_VOLTAGE_SAG_COMP)) { batteryMetersConfigMutable()->voltageSource = BAT_VOLTAGE_RAW; @@ -2090,6 +2159,12 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) 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 if (currentCapacityUnit != batteryMetersConfig()->capacity_unit) { + if (batteryMetersConfig()->capacity_unit == BAT_CAPACITY_UNIT_MAH) { + osdConfigMutable()->stats_energy_unit = OSD_STATS_ENERGY_UNIT_MAH; + } else { + osdConfigMutable()->stats_energy_unit = OSD_STATS_ENERGY_UNIT_WH; + } } } else return MSP_RESULT_ERROR; @@ -2121,6 +2196,7 @@ 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); + uint8_t currentCapacityUnit = batteryMetersConfigMutable()->capacity_unit; batteryMetersConfigMutable()->capacity_unit = sbufReadU8(src); if ((batteryMetersConfig()->voltageSource != BAT_VOLTAGE_RAW) && (batteryMetersConfig()->voltageSource != BAT_VOLTAGE_SAG_COMP)) { batteryMetersConfigMutable()->voltageSource = BAT_VOLTAGE_RAW; @@ -2129,6 +2205,12 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) 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 if (currentCapacityUnit != batteryMetersConfig()->capacity_unit) { + if (batteryMetersConfig()->capacity_unit == BAT_CAPACITY_UNIT_MAH) { + osdConfigMutable()->stats_energy_unit = OSD_STATS_ENERGY_UNIT_MAH; + } else { + osdConfigMutable()->stats_energy_unit = OSD_STATS_ENERGY_UNIT_WH; + } } } else return MSP_RESULT_ERROR; @@ -2666,21 +2748,15 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) if (newFrequency <= VTXCOMMON_MSP_BANDCHAN_CHKVAL) { //value is band and channel const uint8_t newBand = (newFrequency / 8) + 1; const uint8_t newChannel = (newFrequency % 8) + 1; - - if(vtxSettingsConfig()->band != newBand || vtxSettingsConfig()->channel != newChannel) { - vtxCommonSetBandAndChannel(vtxDevice, newBand, newChannel); + if (vtxSettingsConfig()->band != newBand || vtxSettingsConfig()->channel != newChannel) { + vtxSettingsConfigMutable()->band = newBand; + vtxSettingsConfigMutable()->channel = newChannel; } - - vtxSettingsConfigMutable()->band = newBand; - vtxSettingsConfigMutable()->channel = newChannel; } if (sbufBytesRemaining(src) > 1) { uint8_t newPower = sbufReadU8(src); - uint8_t currentPower = 0; - vtxCommonGetPowerIndex(vtxDevice, ¤tPower); - if (newPower != currentPower) { - vtxCommonSetPowerByIndex(vtxDevice, newPower); + if (vtxSettingsConfig()->power != newPower) { vtxSettingsConfigMutable()->power = newPower; } @@ -2696,9 +2772,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) vtxSettingsConfigMutable()->lowPowerDisarm = sbufReadU8(src); } - // ////////////////////////////////////////////////////////// - // this code is taken from BF, it's hack for HDZERO VTX MSP frame - // API version 1.42 - this parameter kept separate since clients may already be supplying + // API version 1.42 - extension for pitmode frequency if (sbufBytesRemaining(src) >= 2) { sbufReadU16(src); //skip pitModeFreq } @@ -2706,18 +2780,29 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) // API version 1.42 - extensions for non-encoded versions of the band, channel or frequency if (sbufBytesRemaining(src) >= 4) { uint8_t newBand = sbufReadU8(src); + if (vtxSettingsConfig()->band != newBand) { + vtxSettingsConfigMutable()->band = newBand; + } + const uint8_t newChannel = sbufReadU8(src); - vtxSettingsConfigMutable()->band = newBand; - vtxSettingsConfigMutable()->channel = newChannel; + if (vtxSettingsConfig()->channel != newChannel) { + vtxSettingsConfigMutable()->channel = newChannel; + } } - /* if (sbufBytesRemaining(src) >= 4) { - sbufRead8(src); // freq_l - sbufRead8(src); // freq_h - sbufRead8(src); // band count - sbufRead8(src); // channel count - }*/ - // ////////////////////////////////////////////////////////// + if (sbufBytesRemaining(src) >= 2) { + sbufReadU16(src); // freq + } + + if (sbufBytesRemaining(src) >= 3) { + sbufReadU8(src); // band count + sbufReadU8(src); // channel count + + uint8_t newPowerCount = sbufReadU8(src); + if (newPowerCount > 0 && newPowerCount < (vtxDevice->capability.powerCount)) { + vtxDevice->capability.powerCount = newPowerCount; + } + } } } } @@ -2888,6 +2973,53 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) return MSP_RESULT_ERROR; break; +#ifdef USE_RX_MSP + case MSP2_COMMON_SET_MSP_RC_LINK_STATS: { + if (dataSize >= 7) { + uint8_t sublinkID = sbufReadU8(src); // Sublink ID + sbufReadU8(src); // Valid link (Failsafe backup) + if (sublinkID == 0) { + setRSSIFromMSP_RC(sbufReadU8(src)); // RSSI % + rxLinkStatistics.uplinkRSSI = -sbufReadU8(src); + rxLinkStatistics.downlinkLQ = sbufReadU8(src); + rxLinkStatistics.uplinkLQ = sbufReadU8(src); + rxLinkStatistics.uplinkSNR = sbufReadI8(src); + } + + return MSP_RESULT_NO_REPLY; + } else + return MSP_RESULT_ERROR; + } + break; + + case MSP2_COMMON_SET_MSP_RC_INFO: { + if (dataSize >= 15) { + uint8_t sublinkID = sbufReadU8(src); + + if (sublinkID == 0) { + rxLinkStatistics.uplinkTXPower = sbufReadU16(src); + rxLinkStatistics.downlinkTXPower = sbufReadU16(src); + + for (int i = 0; i < 4; i++) { + rxLinkStatistics.band[i] = sbufReadU8(src); + } + + sl_toupperptr(rxLinkStatistics.band); + + for (int i = 0; i < 6; i++) { + rxLinkStatistics.mode[i] = sbufReadU8(src); + } + + sl_toupperptr(rxLinkStatistics.mode); + } + + return MSP_RESULT_NO_REPLY; + } else + return MSP_RESULT_ERROR; + } + break; +#endif + case MSP_SET_FAILSAFE_CONFIG: if (dataSize == 20) { failsafeConfigMutable()->failsafe_delay = sbufReadU8(src); @@ -3292,6 +3424,50 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) gpsUbloxSendCommand(src->ptr, dataSize, 0); break; +#ifdef USE_GEOZONE + case MSP2_INAV_SET_GEOZONE: + if (dataSize == 14) { + uint8_t geozoneId; + if (!sbufReadU8Safe(&geozoneId, src) || geozoneId >= MAX_GEOZONES_IN_CONFIG) { + return MSP_RESULT_ERROR; + } + + geozoneResetVertices(geozoneId, -1); + geoZonesConfigMutable(geozoneId)->type = sbufReadU8(src); + geoZonesConfigMutable(geozoneId)->shape = sbufReadU8(src); + geoZonesConfigMutable(geozoneId)->minAltitude = sbufReadU32(src); + geoZonesConfigMutable(geozoneId)->maxAltitude = sbufReadU32(src); + geoZonesConfigMutable(geozoneId)->isSealevelRef = sbufReadU8(src); + geoZonesConfigMutable(geozoneId)->fenceAction = sbufReadU8(src); + geoZonesConfigMutable(geozoneId)->vertexCount = sbufReadU8(src); + } else { + return MSP_RESULT_ERROR; + } + break; + case MSP2_INAV_SET_GEOZONE_VERTEX: + if (dataSize == 10 || dataSize == 14) { + uint8_t geozoneId = 0; + if (!sbufReadU8Safe(&geozoneId, src) || geozoneId >= MAX_GEOZONES_IN_CONFIG) { + return MSP_RESULT_ERROR; + } + uint8_t vertexId = sbufReadU8(src); + int32_t lat = sbufReadU32(src); + int32_t lon = sbufReadU32(src); + if (!geozoneSetVertex(geozoneId, vertexId, lat, lon)) { + return MSP_RESULT_ERROR; + } + + if (geoZonesConfig(geozoneId)->shape == GEOZONE_SHAPE_CIRCULAR) { + if (!geozoneSetVertex(geozoneId, vertexId + 1, sbufReadU32(src), 0)) { + return MSP_RESULT_ERROR; + } + } + } else { + return MSP_RESULT_ERROR; + } + break; +#endif + #ifdef USE_EZ_TUNE case MSP2_INAV_EZ_TUNE_SET: @@ -3344,7 +3520,11 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) sbufReadU8Safe(&tmp_u8, src); if ((dataSize == (OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1) + (CUSTOM_ELEMENTS_PARTS * 3) + 4) && (tmp_u8 < MAX_CUSTOM_ELEMENTS)) { for (int i = 0; i < CUSTOM_ELEMENTS_PARTS; i++) { - osdCustomElementsMutable(tmp_u8)->part[i].type = sbufReadU8(src); + uint8_t type = sbufReadU8(src); + if (type >= CUSTOM_ELEMENT_TYPE_END) + return MSP_RESULT_ERROR; + + osdCustomElementsMutable(tmp_u8)->part[i].type = type; osdCustomElementsMutable(tmp_u8)->part[i].value = sbufReadU16(src); } osdCustomElementsMutable(tmp_u8)->visibility.type = sbufReadU8(src); @@ -3674,7 +3854,7 @@ void mspWriteSimulatorOSD(sbuf_t *dst) while (bytesCount < 80) //whole response should be less 155 bytes at worst. { bool blink1; - uint16_t lastChar; + uint16_t lastChar = 0; count = 0; while ( true ) @@ -3870,6 +4050,14 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu *ret = mspFwApproachOutCommand(dst, src); break; #endif +#ifdef USE_GEOZONE + case MSP2_INAV_GEOZONE: + *ret = mspFcGeozoneOutCommand(dst, src); + break; + case MSP2_INAV_GEOZONE_VERTEX: + *ret = mspFcGeozoneVerteciesOutCommand(dst, src); + break; +#endif #ifdef USE_SIMULATOR case MSP_SIMULATOR: tmp_u8 = sbufReadU8(src); // Get the Simulator MSP version @@ -4084,6 +4272,28 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu break; #endif + case MSP_VTXTABLE_POWERLEVEL: { + vtxDevice_t *vtxDevice = vtxCommonDevice(); + if (!vtxDevice) { + return MSP_RESULT_ERROR; + } + + const uint8_t powerLevel = sbufBytesRemaining(src) ? sbufReadU8(src) : 0; + if (powerLevel == 0 || powerLevel > vtxDevice->capability.powerCount) { + return MSP_RESULT_ERROR; + } + + sbufWriteU8(dst, powerLevel); + sbufWriteU16(dst, 0); + + const char *str = vtxDevice->capability.powerNames[powerLevel - 1]; + const uint32_t str_len = strnlen(str, 5); // these _should_ all be null-terminated + sbufWriteU8(dst, str_len); + for (uint32_t i = 0; i < str_len; i++) + sbufWriteU8(dst, str[i]); + + } break; + default: // Not handled return false; @@ -4172,7 +4382,7 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro if (cmd->flags & MSP_FLAG_DONT_REPLY) { ret = MSP_RESULT_NO_REPLY; } - + reply->flags = cmd->flags; reply->result = ret; return ret; } diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index afb880db52..4ca125d5c6 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -120,9 +120,6 @@ void taskHandleSerial(timeUs_t currentTimeUs) #ifdef USE_MSP_OSD // Capture MSP Displayport messages to determine if VTX is connected mspOsdSerialProcess(mspFcProcessCommand); -#ifdef USE_VTX_MSP - mspVtxSerialProcess(mspFcProcessCommand); -#endif #endif } @@ -339,6 +336,15 @@ void taskUpdateAux(timeUs_t currentTimeUs) #endif } +#ifdef USE_GEOZONE +void geozoneUpdateTask(timeUs_t currentTimeUs) +{ + if (feature(FEATURE_GEOZONE)) { + geozoneUpdate(currentTimeUs); + } +} +#endif + void fcTasksInit(void) { schedulerInit(); @@ -409,7 +415,7 @@ void fcTasksInit(void) setTaskEnabled(TASK_OPFLOW, sensors(SENSOR_OPFLOW)); #endif #ifdef USE_VTX_CONTROL -#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) +#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) || defined(USE_VTX_MSP) setTaskEnabled(TASK_VTXCTRL, true); #endif #endif @@ -453,6 +459,11 @@ void fcTasksInit(void) #if defined(SITL_BUILD) serialProxyStart(); #endif + +#ifdef USE_GEOZONE + setTaskEnabled(TASK_GEOZONE, feature(FEATURE_GEOZONE)); +#endif + } cfTask_t cfTasks[TASK_COUNT] = { @@ -740,4 +751,13 @@ cfTask_t cfTasks[TASK_COUNT] = { }, #endif +#ifdef USE_GEOZONE + [TASK_GEOZONE] = { + .taskName = "GEOZONE", + .taskFunc = geozoneUpdateTask, + .desiredPeriod = TASK_PERIOD_HZ(5), + .staticPriority = TASK_PRIORITY_MEDIUM, + }, +#endif + }; diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index 37c338fedd..47c359b00c 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -302,6 +302,10 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU .adjustmentFunction = ADJUSTMENT_NAV_WP_MULTI_MISSION_INDEX, .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_NAV_FW_ALT_CONTROL_RESPONSE, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} } }; @@ -545,6 +549,9 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t applyAdjustmentPID(ADJUSTMENT_POS_Z_D, &pidBankMutable()->pid[PID_POS_Z].D, delta); navigationUsePIDs(); break; + case ADJUSTMENT_NAV_FW_ALT_CONTROL_RESPONSE: + applyAdjustmentU8(ADJUSTMENT_NAV_FW_ALT_CONTROL_RESPONSE, &pidProfileMutable()->fwAltControlResponseFactor, delta, SETTING_NAV_FW_ALT_CONTROL_RESPONSE_MIN, SETTING_NAV_FW_ALT_CONTROL_RESPONSE_MAX); + break; case ADJUSTMENT_HEADING_P: applyAdjustmentPID(ADJUSTMENT_HEADING_P, &pidBankMutable()->pid[PID_HEADING].P, delta); // TODO: navigationUsePIDs()? @@ -576,7 +583,7 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t case ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE: applyAdjustmentU16(ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE, &navConfigMutable()->fw.minThrottleDownPitchAngle, delta, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MIN, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MAX); break; -#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) +#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) || defined(USE_VTX_MSP) case ADJUSTMENT_VTX_POWER_LEVEL: { vtxDeviceCapability_t vtxDeviceCapability; diff --git a/src/main/fc/rc_adjustments.h b/src/main/fc/rc_adjustments.h index d5119ae927..d900dc8247 100644 --- a/src/main/fc/rc_adjustments.h +++ b/src/main/fc/rc_adjustments.h @@ -85,6 +85,7 @@ typedef enum { ADJUSTMENT_FW_TPA_TIME_CONSTANT = 57, ADJUSTMENT_FW_LEVEL_TRIM = 58, ADJUSTMENT_NAV_WP_MULTI_MISSION_INDEX = 59, + ADJUSTMENT_NAV_FW_ALT_CONTROL_RESPONSE = 60, ADJUSTMENT_FUNCTION_COUNT // must be last } adjustmentFunction_e; diff --git a/src/main/fc/runtime_config.c b/src/main/fc/runtime_config.c index 44118ef54b..c98c90e7ec 100644 --- a/src/main/fc/runtime_config.c +++ b/src/main/fc/runtime_config.c @@ -34,7 +34,7 @@ static EXTENDED_FASTRAM uint32_t enabledSensors = 0; #if !defined(CLI_MINIMAL_VERBOSITY) const char *armingDisableFlagNames[]= { "FS", "ANGLE", "CAL", "OVRLD", "NAV", "COMPASS", - "ACC", "ARMSW", "HWFAIL", "BOXFS", "RX", + "ACC", "ARMSW", "HWFAIL", "BOXFS", "PLACEHOLDER", "RX", "THR", "CLI", "CMS", "OSD", "ROLL/PITCH", "AUTOTRIM", "OOM", "SETTINGFAIL", "PWMOUT", "NOPREARM", "DSHOTBEEPER", "LANDED" }; diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 974f90f8c4..452256a6b6 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -23,7 +23,7 @@ typedef enum { WAS_EVER_ARMED = (1 << 3), SIMULATOR_MODE_HITL = (1 << 4), SIMULATOR_MODE_SITL = (1 << 5), - + ARMING_DISABLED_GEOZONE = (1 << 6), ARMING_DISABLED_FAILSAFE_SYSTEM = (1 << 7), ARMING_DISABLED_NOT_LEVEL = (1 << 8), ARMING_DISABLED_SENSORS_CALIBRATING = (1 << 9), @@ -49,8 +49,8 @@ typedef enum { ARMING_DISABLED_DSHOT_BEEPER = (1 << 29), ARMING_DISABLED_LANDING_DETECTED = (1 << 30), - ARMING_DISABLED_ALL_FLAGS = (ARMING_DISABLED_FAILSAFE_SYSTEM | ARMING_DISABLED_NOT_LEVEL | ARMING_DISABLED_SENSORS_CALIBRATING | - ARMING_DISABLED_SYSTEM_OVERLOADED | ARMING_DISABLED_NAVIGATION_UNSAFE | + ARMING_DISABLED_ALL_FLAGS = (ARMING_DISABLED_GEOZONE | ARMING_DISABLED_FAILSAFE_SYSTEM | ARMING_DISABLED_NOT_LEVEL | + ARMING_DISABLED_SENSORS_CALIBRATING | ARMING_DISABLED_SYSTEM_OVERLOADED | ARMING_DISABLED_NAVIGATION_UNSAFE | ARMING_DISABLED_COMPASS_NOT_CALIBRATED | ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED | ARMING_DISABLED_ARM_SWITCH | ARMING_DISABLED_HARDWARE_FAILURE | ARMING_DISABLED_BOXFAILSAFE | ARMING_DISABLED_RC_LINK | ARMING_DISABLED_THROTTLE | ARMING_DISABLED_CLI | @@ -65,7 +65,8 @@ typedef enum { // situations where we might just need the motors to spin so the // aircraft can move (even unpredictably) and get unstuck (e.g. // crashed into a high tree). -#define ARMING_DISABLED_EMERGENCY_OVERRIDE (ARMING_DISABLED_NOT_LEVEL \ +#define ARMING_DISABLED_EMERGENCY_OVERRIDE (ARMING_DISABLED_GEOZONE \ + | ARMING_DISABLED_NOT_LEVEL \ | ARMING_DISABLED_NAVIGATION_UNSAFE \ | ARMING_DISABLED_COMPASS_NOT_CALIBRATED \ | ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED \ @@ -106,6 +107,7 @@ typedef enum { SOARING_MODE = (1 << 16), ANGLEHOLD_MODE = (1 << 17), NAV_FW_AUTOLAND = (1 << 18), + NAV_SEND_TO = (1 << 19), } flightModeFlags_e; extern uint32_t flightModeFlags; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 0524a1e935..90338f2dc6 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -198,6 +198,17 @@ tables: - name: headtracker_dev_type values: ["NONE", "SERIAL", "MSP"] enum: headTrackerDevType_e + - name: mavlink_radio_type + values: ["GENERIC", "ELRS", "SIK"] + enum: mavlinkRadio_e + - name: default_altitude_source + values: ["GPS", "BARO", "GPS_ONLY", "BARO_ONLY"] + enum: navDefaultAltitudeSensor_e + - name: fence_action + values: ["NONE", "AVOID", "POS_HOLD", "RTH"] + enum: fenceAction_e + - name: geozone_rth_no_way_home + values: [RTH, EMRG_LAND] constants: RPYL_PID_MIN: 0 @@ -1220,7 +1231,7 @@ groups: max: INT16_MAX - name: motorstop_on_low description: "If enabled, motor will stop when throttle is low on this mixer_profile" - default_value: OFF + default_value: ON field: mixer_config.motorstopOnLow type: bool - name: mixer_pid_profile_linking @@ -1536,7 +1547,7 @@ groups: default_value: OFF type: bool - name: disarm_always - description: "Disarms the motors independently of throttle value. Setting to OFF reverts to the old behaviour of disarming only when the throttle is low." + description: "When you switch to Disarm, do so regardless of throttle position. If this Setting is `OFF`. It will only disarm only when the throttle is low. This is similar to the previous `disarm_kill_switch` option. Default setting is the same as the old default behaviour." default_value: ON type: bool - name: switch_disarm_delay @@ -1578,7 +1589,7 @@ groups: description: "EzTune filter cutoff frequency" default_value: 110 field: filterHz - min: 10 + min: 20 max: 300 - name: ez_axis_ratio description: "EzTune axis ratio" @@ -2120,7 +2131,7 @@ groups: max: 100 - name: nav_fw_pos_z_p description: "P gain of altitude PID controller (Fixedwing)" - default_value: 40 + default_value: 30 field: bank_fw.pid[PID_POS_Z].P min: 0 max: 255 @@ -2136,9 +2147,15 @@ groups: field: bank_fw.pid[PID_POS_Z].D min: 0 max: 255 + - name: nav_fw_pos_z_ff + description: "FF gain of altitude PID controller (Fixedwing)" + default_value: 10 + field: bank_fw.pid[PID_POS_Z].FF + min: 0 + max: 255 - name: nav_fw_alt_control_response description: "Adjusts the deceleration response of fixed wing altitude control as the target altitude is approached. Decrease value to help avoid overshooting the target altitude." - default_value: 20 + default_value: 40 field: fwAltControlResponseFactor min: 5 max: 100 @@ -2331,7 +2348,7 @@ groups: type: uint8_t - name: fw_autotune_max_rate_deflection description: "The target percentage of maximum mixer output used for determining the rates in `AUTO` and `LIMIT`." - default_value: 80 + default_value: 90 field: fw_max_rate_deflection min: 50 max: 100 @@ -2407,6 +2424,12 @@ groups: min: 0 max: 10 default_value: 0.35 + - name: inav_w_z_baro_v + description: "Weight of barometer climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors." + field: w_z_baro_v + min: 0 + max: 10 + default_value: 0.1 - name: inav_w_z_gps_p description: "Weight of GPS altitude measurements in estimated altitude. Setting is used on both airplanes and multirotors." field: w_z_gps_p @@ -2461,6 +2484,11 @@ groups: field: baro_epv min: 0 max: 9999 + - name: inav_default_alt_sensor + description: "Sets the default altitude sensor to use. Settings GPS and BARO always use both sensors unless there is an altitude error between the sensors that exceeds a set limit. In this case only the selected sensor will be used while the altitude error limit is exceeded. GPS error limit = 2 * inav_max_eph_epv. BARO error limit = 4 * inav_baro_epv. Settings GPS_ONLY and BARO_ONLY will use only the selected sensor even if the other sensor is working. The other sensor will only be used as a backup if the selected sensor is no longer available to use." + default_value: "GPS" + field: default_alt_sensor + table: default_altitude_source - name: PG_NAV_CONFIG type: navConfig_t @@ -3195,6 +3223,18 @@ groups: min: 1 max: 2 default_value: 2 + - name: mavlink_min_txbuffer + field: mavlink.min_txbuff + description: "Minimum percent of TX buffer space free, before attempting to transmit telemetry. Requuires RADIO_STATUS messages to be processed. 0 = always transmits." + default_value: 33 + min: 0 + max: 100 + - name: mavlink_radio_type + description: "Mavlink radio type. Affects how RSSI and LQ are reported on OSD." + field: mavlink.radio_type + table: mavlink_radio_type + default_value: "GENERIC" + type: uint8_t - name: PG_OSD_CONFIG type: osdConfig_t @@ -3342,35 +3382,35 @@ groups: min: -550 max: 1250 - name: osd_snr_alarm - condition: USE_SERIALRX_CRSF + condition: USE_SERIALRX_CRSF || USE_RX_MSP description: "Value below which Crossfire SNR Alarm pops-up. (dB)" default_value: 4 field: snr_alarm min: -20 - max: 10 + max: 99 - name: osd_link_quality_alarm - condition: USE_SERIALRX_CRSF + condition: USE_SERIALRX_CRSF || USE_RX_MSP description: "LQ % indicator blinks below this value. For Crossfire use 70%, for Tracer use 50%" default_value: 70 field: link_quality_alarm min: 0 max: 100 - name: osd_rssi_dbm_alarm - condition: USE_SERIALRX_CRSF + condition: USE_SERIALRX_CRSF || USE_RX_MSP description: "RSSI dBm indicator blinks below this value [dBm]. 0 disables this alarm" default_value: 0 field: rssi_dbm_alarm min: -130 max: 0 - name: osd_rssi_dbm_max - condition: USE_SERIALRX_CRSF + condition: USE_SERIALRX_CRSF || USE_RX_MSP description: "RSSI dBm upper end of curve. Perfect rssi (max) = 100%" default_value: -30 field: rssi_dbm_max min: -50 max: 0 - name: osd_rssi_dbm_min - condition: USE_SERIALRX_CRSF + condition: USE_SERIALRX_CRSF || USE_RX_MSP description: "RSSI dBm lower end of curve or RX sensitivity level. Worst rssi (min) = 0%" default_value: -120 field: rssi_dbm_min @@ -3537,6 +3577,20 @@ groups: field: main_voltage_decimals min: 1 max: 2 + - name: osd_decimals_altitude + description: "Number of decimals for the altitude displayed in the OSD [3-5]." + default_value: 3 + field: decimals_altitude + type: uint8_t + min: 3 + max: 5 + - name: osd_decimals_distance + description: "Number of decimals for distance displayed in the OSD [3-5]. This includes distance from home, total distance, and distance remaining." + default_value: 3 + field: decimals_distance + type: uint8_t + min: 3 + max: 5 - name: osd_coordinate_digits description: "Number of digits for the coordinates displayed in the OSD [8-11]." field: coordinate_digits @@ -3690,7 +3744,8 @@ groups: - 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 + default: 4 + min: 3 max: 6 default_value: 4 - name: osd_use_pilot_logo @@ -3721,6 +3776,12 @@ groups: field: highlight_djis_missing_characters default_value: ON type: bool + - name: enable_broken_o4_workaround + field: enable_broken_o4_workaround + description: DJI O4 release firmware has a broken MSP DisplayPort implementation. This enables a workaround to restore ARM detection. + condition: USE_DJI_HD_OSD + default_value: OFF + type: bool - name: osd_switch_indicator_zero_name description: "Character to use for OSD switch incicator 0." field: osd_switch_indicator0_name @@ -4318,3 +4379,49 @@ groups: field: roll_ratio min: 0 max: 5 + - name: PG_GEOZONE_CONFIG + type: geozone_config_t + headers: ["navigation/navigation.h"] + condition: USE_GEOZONE && USE_GPS + members: + - name: geozone_detection_distance + description: "Distance from which a geozone is detected" + default_value: 50000 + field: fenceDetectionDistance + min: 0 + max: 1000000 + - name: geozone_avoid_altitude_range + description: "Altitude range in which an attempt is made to avoid a geozone upwards" + default_value: 5000 + field: avoidAltitudeRange + min: 0 + max: 1000000 + - name: geozone_safe_altitude_distance + description: "Vertical distance that must be maintained to the upper and lower limits of the zone." + default_value: 1000 + field: safeAltitudeDistance + min: 0 + max: 10000 + - name: geozone_safehome_as_inclusive + description: "Treat nearest safehome as inclusive geozone" + type: bool + field: nearestSafeHomeAsInclusivZone + default_value: OFF + - name: geozone_safehome_zone_action + description: "Fence action for safehome zone" + default_value: "NONE" + field: safeHomeFenceAction + table: fence_action + type: uint8_t + - name: geozone_mr_stop_distance + description: "Distance in which multirotors stops before the border" + default_value: 15000 + min: 0 + max: 100000 + field: copterFenceStopDistance + - name: geozone_no_way_home_action + description: "Action if RTH with active geozones is unable to calculate a course to home" + default_value: RTH + field: noWayHomeAction + table: geozone_rth_no_way_home + type: uint8_t diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 24896663dc..109882b5f6 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -535,6 +535,7 @@ void failsafeUpdateState(void) abortForcedRTH(); failsafeSetActiveProcedure(FAILSAFE_PROCEDURE_AUTO_LANDING); failsafeActivate(FAILSAFE_LANDING); + activateForcedEmergLanding(); reprocessState = true; break; } diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index f1b2f56781..ae8eb57fb3 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -179,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, 9); +PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 10); PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .bank_mc = { @@ -242,8 +242,8 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, [PID_POS_Z] = { .P = SETTING_NAV_FW_POS_Z_P_DEFAULT, // FW_POS_Z_P * 100 .I = SETTING_NAV_FW_POS_Z_I_DEFAULT, // FW_POS_Z_I * 100 - .D = SETTING_NAV_FW_POS_Z_D_DEFAULT, // FW_POS_Z_D * 100 - .FF = 0, + .D = SETTING_NAV_FW_POS_Z_D_DEFAULT, // FW_POS_Z_D * 200 + .FF = SETTING_NAV_FW_POS_Z_FF_DEFAULT, // FW_POS_Z_FF * 100 }, [PID_POS_XY] = { .P = SETTING_NAV_FW_POS_XY_P_DEFAULT, // FW_POS_XY_P * 100 @@ -1206,7 +1206,7 @@ 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 diff --git a/src/main/io/displayport_msp_dji_compat.c b/src/main/io/displayport_msp_dji_compat.c index fbd7445f65..8daffbb4d1 100644 --- a/src/main/io/displayport_msp_dji_compat.c +++ b/src/main/io/displayport_msp_dji_compat.c @@ -24,6 +24,13 @@ #include "io/displayport_msp_dji_compat.h" #include "io/dji_osd_symbols.h" #include "drivers/osd_symbols.h" +#include + +// 0123456789 +static char *dji_logo = " DJI, " + " PLEASE " + " FIX YOUR " + " OSD FONT "; uint8_t getDJICharacter(uint8_t ch, uint8_t page) { @@ -37,12 +44,20 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page) return DJI_SYM_AH_DECORATION; } + if (ech >= SYM_LOGO_START && ech <= 297) { + return dji_logo[(ech - SYM_LOGO_START) % (strlen(dji_logo) + 1)]; + } + + if (ech >= SYM_PILOT_LOGO_LRG_START && ech <= 511) { + return dji_logo[(ech - SYM_LOGO_START) % (strlen(dji_logo) + 1)]; + } + switch (ech) { case SYM_RSSI: return DJI_SYM_RSSI; case SYM_LQ: - return 'Q'; + return DJI_SYM_LINK_QUALITY; case SYM_LAT: return DJI_SYM_LAT; @@ -127,13 +142,13 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page) case SYM_MAH: return DJI_SYM_MAH; - - case SYM_AH_KM: - return 'K'; - - case SYM_AH_MI: - return 'M'; /* + case SYM_AH_KM: // AH / KM + return '?'; + + case SYM_AH_MI: // AH / MI + return '?'; + case SYM_VTX_POWER: return DJI_SYM_VTX_POWER; @@ -210,8 +225,9 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page) return DJI_SYM_M; case SYM_TOTAL: - return DJI_SYM_FLY_H; -/* + return DJI_SYM_TOTAL_DISTANCE; + + /* case SYM_ALT_KM: return DJI_SYM_ALT_KM; @@ -234,20 +250,22 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page) case SYM_DIST_NM: return DJI_SYM_DIST_NM; */ + case SYM_M: return DJI_SYM_M; case SYM_KM: - return 'K'; + return DJI_SYM_KM; case SYM_MI: - return 'M'; + return DJI_SYM_MILES; + /* case SYM_NM: return DJI_SYM_NM; -*/ case SYM_WIND_HORIZONTAL: return 'W'; // W for wind + */ /* case SYM_WIND_VERTICAL: @@ -255,7 +273,8 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page) case SYM_3D_KT: return DJI_SYM_3D_KT; -*/ + */ +/* case SYM_AIR: return 'A'; // A for airspeed @@ -264,7 +283,7 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page) case SYM_3D_MPH: return DJI_SYM_MPH; - + */ case SYM_RPM: return DJI_SYM_RPM; @@ -273,7 +292,7 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page) /* case SYM_100FTM: return DJI_SYM_100FTM; -*/ + */ case SYM_MS: return DJI_SYM_MPS; @@ -291,7 +310,7 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page) case SYM_MAH_MI_1: return DJI_SYM_MAH_MI_1; -*/ + */ case SYM_THR: return DJI_SYM_THR; @@ -304,11 +323,13 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page) 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; @@ -330,7 +351,7 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page) case SYM_ZERO_HALF_LEADING_DOT: return DJI_SYM_ZERO_HALF_LEADING_DOT; -*/ + */ case SYM_AUTO_THR0: return 'A'; @@ -365,7 +386,7 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page) case SYM_GFORCE_Z: return DJI_SYM_GFORCE_Z; -*/ + */ case SYM_BARO_TEMP: return DJI_SYM_TEMPERATURE; @@ -386,7 +407,7 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page) case TEMP_SENSOR_SYM_COUNT: return DJI_TEMP_SENSOR_SYM_COUNT; -*/ + */ case SYM_HEADING_N: return DJI_SYM_HEADING_N; @@ -410,7 +431,7 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page) /* case SYM_PROFILE: return DJI_SYM_PROFILE; -*/ + */ case SYM_SWITCH_INDICATOR_LOW: return DJI_SYM_STICK_OVERLAY_SPRITE_LOW; @@ -452,16 +473,8 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page) 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; @@ -470,16 +483,17 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page) /* 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; + return DJI_SYM_AH_CENTER_LINE; case SYM_AH_CH_CENTER: case SYM_AH_CH_AIRCRAFT2: - return DJI_SYM_CROSSHAIR_CENTRE; + return DJI_SYM_AH_CENTER; case SYM_AH_CH_RIGHT: case SYM_AH_CH_AIRCRAFT3: - return DJI_SYM_CROSSHAIR_RIGHT; + return DJI_SYM_AH_CENTER_LINE_RIGHT; case SYM_AH_CH_AIRCRAFT0: case SYM_AH_CH_AIRCRAFT4: @@ -488,21 +502,21 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page) case SYM_AH_CH_TYPE3: return DJI_SYM_NONE; case (SYM_AH_CH_TYPE3+1): - return DJI_SYM_SMALL_CROSSHAIR; + return DJI_SYM_AH_CENTER; 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; + return DJI_SYM_AH_CENTER; 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; + return DJI_SYM_AH_CENTER; case (SYM_AH_CH_TYPE5+2): return DJI_SYM_STICK_OVERLAY_HORIZONTAL; @@ -516,14 +530,14 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page) case SYM_AH_CH_TYPE7: return DJI_SYM_ARROW_SMALL_LEFT; case (SYM_AH_CH_TYPE7+1): - return DJI_SYM_SMALL_CROSSHAIR; + return DJI_SYM_AH_CENTER; 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; + return DJI_SYM_AH_CENTER; case (SYM_AH_CH_TYPE8+2): return DJI_SYM_AH_RIGHT; @@ -683,7 +697,7 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page) case SYM_FLIGHT_DIST_REMAINING: return DJI_SYM_FLIGHT_DIST_REMAINING; -*/ + */ case SYM_HUD_ARROWS_L1: return DJI_SYM_ARROW_SMALL_LEFT; diff --git a/src/main/io/displayport_msp_osd.c b/src/main/io/displayport_msp_osd.c index 0b9be8c6d8..ca9f96848f 100644 --- a/src/main/io/displayport_msp_osd.c +++ b/src/main/io/displayport_msp_osd.c @@ -53,6 +53,12 @@ #include "displayport_msp_osd.h" #include "displayport_msp_dji_compat.h" +#include "osd_dji_hd.h" +#include "fc/fc_msp_box.h" +#include "scheduler/scheduler.h" +#include "fc/config.h" +#include "common/maths.h" + #define FONT_VERSION 3 typedef enum { // defines are from hdzero code @@ -531,11 +537,72 @@ static mspResult_e processMspCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPo return mspProcessCommand(cmd, reply, mspPostProcessFn); } +#if defined(USE_OSD) && defined(USE_DJI_HD_OSD) +extern timeDelta_t cycleTime; +static mspResult_e fixDjiBrokenO4ProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn) { + UNUSED(mspPostProcessFn); + + sbuf_t *dst = &reply->buf; + + // If users is using a buggy O4 air unit, re-use the OLD DJI FPV system workaround for status messages + if (osdConfig()->enable_broken_o4_workaround && ((cmd->cmd == DJI_MSP_STATUS) || (cmd->cmd == DJI_MSP_STATUS_EX))) { + // Start initializing the reply message + reply->cmd = cmd->cmd; + reply->result = MSP_RESULT_ACK; + + // DJI OSD relies on a statically defined bit order and doesn't use + // MSP_BOXIDS to get actual BOX order. We need a special + // packBoxModeFlags() + // This is a regression from O3 + boxBitmask_t flightModeBitmask; + djiPackBoxModeBitmask(&flightModeBitmask); + + sbufWriteU16(dst, (uint16_t)cycleTime); + sbufWriteU16(dst, 0); + sbufWriteU16(dst, packSensorStatus()); + sbufWriteData(dst, &flightModeBitmask, + 4); // unconditional part of flags, first 32 bits + sbufWriteU8(dst, getConfigProfile()); + + sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100)); + if (cmd->cmd == MSP_STATUS_EX) { + sbufWriteU8(dst, 3); // PID_PROFILE_COUNT + sbufWriteU8(dst, 1); // getCurrentControlRateProfileIndex() + } else { + sbufWriteU16(dst, cycleTime); // gyro cycle time + } + + // Cap BoxModeFlags to 32 bits + // write flightModeFlags header. Lowest 4 bits contain number of bytes + // that follow + sbufWriteU8(dst, 0); + // sbufWriteData(dst, ((uint8_t*)&flightModeBitmask) + 4, byteCount); + + // Write arming disable flags + sbufWriteU8(dst, DJI_ARMING_DISABLE_FLAGS_COUNT); + sbufWriteU32(dst, djiPackArmingDisabledFlags()); + + // Extra flags + sbufWriteU8(dst, 0); + // Process DONT_REPLY flag + if (cmd->flags & MSP_FLAG_DONT_REPLY) { + reply->result = MSP_RESULT_NO_REPLY; + } + + return reply->result; + } + + return processMspCommand(cmd, reply, mspPostProcessFn); +} +#else +#define fixDjiBrokenO4ProcessMspCommand processMspCommand +#endif + void mspOsdSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn) { if (mspPort.port) { mspProcessCommand = mspProcessCommandFn; - mspSerialProcessOnePort(&mspPort, MSP_SKIP_NON_MSP_DATA, processMspCommand); + mspSerialProcessOnePort(&mspPort, MSP_SKIP_NON_MSP_DATA, fixDjiBrokenO4ProcessMspCommand); } } diff --git a/src/main/io/dji_osd_symbols.h b/src/main/io/dji_osd_symbols.h index 83ccad7c82..7c63eedb64 100644 --- a/src/main/io/dji_osd_symbols.h +++ b/src/main/io/dji_osd_symbols.h @@ -38,21 +38,25 @@ #define DJI_SYM_LAT 0x89 #define DJI_SYM_LON 0x98 #define DJI_SYM_ALTITUDE 0x7F +#define DJI_SYM_TOTAL_DISTANCE 0x71 #define DJI_SYM_OVER_HOME 0x05 // RSSI #define DJI_SYM_RSSI 0x01 +#define DJI_SYM_LINK_QUALITY 0x7B // Throttle Position (%) #define DJI_SYM_THR 0x04 // Unit Icons (Metric) #define DJI_SYM_M 0x0C +#define DJI_SYM_KM 0x7D #define DJI_SYM_C 0x0E // Unit Icons (Imperial) -#define DJI_SYM_F 0x0D #define DJI_SYM_FT 0x0F +#define DJI_SYM_MILES 0x7E +#define DJI_SYM_F 0x0D // Heading Graphics #define DJI_SYM_HEADING_N 0x18 @@ -63,13 +67,12 @@ #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_CENTER_LINE 0x72 +#define DJI_SYM_AH_CENTER 0x73 +#define DJI_SYM_AH_CENTER_LINE_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 @@ -136,17 +139,19 @@ #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_SPEED 0x70 #define DJI_SYM_KPH 0x9E #define DJI_SYM_MPH 0x9D #define DJI_SYM_MPS 0x9F #define DJI_SYM_FTPS 0x99 +// Menu cursor +#define DJI_SYM_CURSOR DJI_SYM_AH_LEFT + // Stick overlays #define DJI_SYM_STICK_OVERLAY_SPRITE_HIGH 0x08 #define DJI_SYM_STICK_OVERLAY_SPRITE_MID 0x09 diff --git a/src/main/io/gps.c b/src/main/io/gps.c index e7aecd3e0d..c8dc6788df 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -392,11 +392,13 @@ void gpsProcessNewSolutionData(bool timeout) // Update time gpsUpdateTime(); - // Update timeout - gpsSetProtocolTimeout(gpsState.baseTimeoutMs); + if (!timeout) { + // Update timeout + gpsSetProtocolTimeout(gpsState.baseTimeoutMs); - // Update statistics - gpsStats.lastMessageDt = gpsState.lastMessageMs - gpsState.lastLastMessageMs; + // Update statistics + gpsStats.lastMessageDt = gpsState.lastMessageMs - gpsState.lastLastMessageMs; + } gpsSol.flags.hasNewData = true; // Toggle heartbeat diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 1b88226b9c..d44ea0a898 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -181,7 +181,7 @@ typedef struct statistic_s { uint16_t min_voltage; // /100 int16_t max_current; int32_t max_power; - int16_t min_rssi; + uint8_t min_rssi; int16_t min_lq; // for CRSF int16_t min_rssi_dbm; // for CRSF int32_t max_altitude; @@ -224,8 +224,8 @@ static bool osdDisplayHasCanvas; #define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees) -PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 12); -PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 2); +PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 15); +PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 3); void osdStartedSaveProcess(void) { savingSettings = true; @@ -259,10 +259,11 @@ bool osdIsNotMetric(void) { * prefixed by a a symbol to indicate the unit used. * @param dist Distance in centimeters */ -static void osdFormatDistanceSymbol(char *buff, int32_t dist, uint8_t decimals) +static void osdFormatDistanceSymbol(char *buff, int32_t dist, uint8_t decimals, uint8_t digits) { - uint8_t digits = 3U; // Total number of digits (including decimal point) - uint8_t sym_index = 3U; // Position (index) at buffer of units symbol + if (digits == 0) // Total number of digits (including decimal point) + digits = 3U; + uint8_t sym_index = digits; // Position (index) at buffer of units symbol uint8_t symbol_m = SYM_DIST_M; uint8_t symbol_km = SYM_DIST_KM; uint8_t symbol_ft = SYM_DIST_FT; @@ -485,13 +486,13 @@ static void osdFormatWindSpeedStr(char *buff, int32_t ws, bool isValid) */ void osdFormatAltitudeSymbol(char *buff, int32_t alt) { - uint8_t totalDigits = 4U; - uint8_t digits = 4U; - uint8_t symbolIndex = 4U; + uint8_t digits = osdConfig()->decimals_altitude + 1; + uint8_t totalDigits = digits; + uint8_t symbolIndex = digits; uint8_t symbolKFt = SYM_ALT_KFT; if (alt >= 0) { - digits = 3U; + digits--; buff[0] = ' '; } @@ -616,11 +617,11 @@ char *osdFormatTrimWhiteSpace(char *buff) /** * Converts RSSI into a % value used by the OSD. + * Range is [0, 100] */ -static uint16_t osdConvertRSSI(void) +static uint8_t osdConvertRSSI(void) { - // change range to [0, 99] - return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 99); + return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 100); } static uint16_t osdGetCrsfLQ(void) @@ -806,7 +807,7 @@ static const char * osdArmingDisabledReasonMessage(void) case NAV_ARMING_BLOCKER_NAV_IS_ALREADY_ACTIVE: return OSD_MESSAGE_STR(OSD_MSG_DISABLE_NAV_FIRST); case NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR: - osdFormatDistanceSymbol(buf, distanceToFirstWP(), 0); + osdFormatDistanceSymbol(buf, distanceToFirstWP(), 0, 3); tfp_sprintf(messageBuf, "FIRST WP TOO FAR (%s)", buf); return message = messageBuf; case NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR: @@ -866,6 +867,14 @@ static const char * osdArmingDisabledReasonMessage(void) return OSD_MESSAGE_STR(OSD_MSG_NO_PREARM); case ARMING_DISABLED_DSHOT_BEEPER: return OSD_MESSAGE_STR(OSD_MSG_DSHOT_BEEPER); + + case ARMING_DISABLED_GEOZONE: +#ifdef USE_GEOZONE + return OSD_MESSAGE_STR(OSD_MSG_NFZ); +#else + FALLTHROUGH; +#endif + // Cases without message case ARMING_DISABLED_LANDING_DETECTED: FALLTHROUGH; @@ -1644,6 +1653,20 @@ void osdDisplaySwitchIndicator(const char *swName, int rcValue, char *buff) { buff[ptr] = '\0'; } +static bool osdElementEnabled(uint8_t elementID, bool onlyCurrentLayout) { + bool elementEnabled = false; + + if (onlyCurrentLayout) { + elementEnabled = OSD_VISIBLE(osdLayoutsConfig()->item_pos[currentLayout][elementID]); + } else { + for (uint8_t layout = 0; layout < 4 && !elementEnabled; layout++) { + elementEnabled = OSD_VISIBLE(osdLayoutsConfig()->item_pos[layout][elementID]); + } + } + + return elementEnabled; +} + static bool osdDrawSingleElement(uint8_t item) { uint16_t pos = osdLayoutsConfig()->item_pos[currentLayout][item]; @@ -1698,9 +1721,13 @@ static bool osdDrawSingleElement(uint8_t item) } case OSD_RSSI_VALUE: { - uint16_t osdRssi = osdConvertRSSI(); + uint8_t osdRssi = osdConvertRSSI(); buff[0] = SYM_RSSI; - tfp_sprintf(buff + 1, "%2d", osdRssi); + if (osdRssi < 100) + tfp_sprintf(buff + 1, "%2d", osdRssi); + else + tfp_sprintf(buff + 1, "%c ", SYM_MAX); + if (osdRssi < osdConfig()->rssi_alarm) { TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } @@ -1753,7 +1780,7 @@ static bool osdDrawSingleElement(uint8_t item) } else #endif { - if (osdFormatCentiNumber(buff, getMAhDrawn() * 100, 1000, 0, (mah_digits - 2), mah_digits, false)) { + if (osdFormatCentiNumber(buff, getMAhDrawn() * 100, 1000, 0, 2, mah_digits, false)) { // Shown in Ah buff[mah_digits] = SYM_AH; } else { @@ -1795,7 +1822,7 @@ static bool osdDrawSingleElement(uint8_t item) } else #endif { - if (osdFormatCentiNumber(buff, getBatteryRemainingCapacity() * 100, 1000, 0, (mah_digits - 2), mah_digits, false)) { + if (osdFormatCentiNumber(buff, getBatteryRemainingCapacity() * 100, 1000, 0, 2, mah_digits, false)) { // Shown in Ah buff[mah_digits] = SYM_AH; } else { @@ -1952,7 +1979,7 @@ static bool osdDrawSingleElement(uint8_t item) { buff[0] = SYM_HOME; uint32_t distance_to_home_cm = GPS_distanceToHome * 100; - osdFormatDistanceSymbol(&buff[1], distance_to_home_cm, 0); + osdFormatDistanceSymbol(&buff[1], distance_to_home_cm, 0, osdConfig()->decimals_distance); uint16_t dist_alarm = osdConfig()->dist_alarm; if (dist_alarm > 0 && GPS_distanceToHome > dist_alarm) { @@ -1963,7 +1990,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_TRIP_DIST: buff[0] = SYM_TOTAL; - osdFormatDistanceSymbol(buff + 1, getTotalTravelDistance(), 0); + osdFormatDistanceSymbol(buff + 1, getTotalTravelDistance(), 0, osdConfig()->decimals_distance); break; case OSD_BLACKBOX: @@ -2082,7 +2109,7 @@ static bool osdDrawSingleElement(uint8_t item) { if (isWaypointNavTrackingActive()) { buff[0] = SYM_CROSS_TRACK_ERROR; - osdFormatDistanceSymbol(buff + 1, navigationGetCrossTrackError(), 0); + osdFormatDistanceSymbol(buff + 1, navigationGetCrossTrackError(), 0, 3); } else { displayWrite(osdDisplayPort, elemPosX, elemPosY, " "); return true; @@ -2241,7 +2268,7 @@ static bool osdDrawSingleElement(uint8_t item) buff[1] = '-'; buff[2] = '-'; } else { - osdFormatDistanceSymbol(buff, range, 1); + osdFormatDistanceSymbol(buff, range, 1, 3); } } break; @@ -2323,31 +2350,33 @@ static bool osdDrawSingleElement(uint8_t item) displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_FLIGHT_DIST_REMAINING); if ((!ARMING_FLAG(ARMED)) || (distanceMeters == -1)) { - buff[3] = SYM_BLANK; - buff[4] = '\0'; + buff[osdConfig()->decimals_distance] = SYM_BLANK; + buff[osdConfig()->decimals_distance + 1] = '\0'; strcpy(buff, "---"); } else if (distanceMeters == -2) { // Wind is too strong to come back with cruise throttle - buff[0] = buff[1] = buff[2] = SYM_WIND_HORIZONTAL; + for (uint8_t i = 0; i < osdConfig()->decimals_distance; i++) { + buff[i] = SYM_WIND_HORIZONTAL; + } switch ((osd_unit_e)osdConfig()->units){ case OSD_UNIT_UK: FALLTHROUGH; case OSD_UNIT_IMPERIAL: - buff[3] = SYM_DIST_MI; + buff[osdConfig()->decimals_distance] = SYM_DIST_MI; break; case OSD_UNIT_METRIC_MPH: FALLTHROUGH; case OSD_UNIT_METRIC: - buff[3] = SYM_DIST_KM; + buff[osdConfig()->decimals_distance] = SYM_DIST_KM; break; case OSD_UNIT_GA: - buff[3] = SYM_DIST_NM; + buff[osdConfig()->decimals_distance] = SYM_DIST_NM; break; } - buff[4] = '\0'; + buff[osdConfig()->decimals_distance+1] = '\0'; TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } else { - osdFormatDistanceSymbol(buff, distanceMeters * 100, 0); + osdFormatDistanceSymbol(buff, distanceMeters * 100, 0, osdConfig()->decimals_distance); if (distanceMeters == 0) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } @@ -2361,6 +2390,11 @@ static bool osdDrawSingleElement(uint8_t item) if (FLIGHT_MODE(NAV_FW_AUTOLAND)) p = "LAND"; else +#endif +#ifdef USE_GEOZONE + if (FLIGHT_MODE(NAV_SEND_TO)) + p = "AUTO"; + else #endif if (FLIGHT_MODE(FAILSAFE_MODE)) p = "!FS!"; @@ -2447,15 +2481,15 @@ static bool osdDrawSingleElement(uint8_t item) return true; } -#if defined(USE_SERIALRX_CRSF) - case OSD_CRSF_RSSI_DBM: +#if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP) + case OSD_RSSI_DBM: { int16_t rssi = rxLinkStatistics.uplinkRSSI; buff[0] = (rxLinkStatistics.activeAntenna == 0) ? SYM_RSSI : SYM_2RSS; // Separate symbols for each antenna if (rssi <= -100) { tfp_sprintf(buff + 1, "%4d%c", rssi, SYM_DBM); } else { - tfp_sprintf(buff + 1, "%3d%c%c", rssi, SYM_DBM, ' '); + tfp_sprintf(buff + 1, " %3d%c", rssi, SYM_DBM); } if (!failsafeIsReceivingRxData()){ TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); @@ -2464,19 +2498,15 @@ static bool osdDrawSingleElement(uint8_t item) } break; } - case OSD_CRSF_LQ: + case OSD_LQ_UPLINK: { buff[0] = SYM_LQ; - int16_t statsLQ = rxLinkStatistics.uplinkLQ; - int16_t scaledLQ = scaleRange(constrain(statsLQ, 0, 100), 0, 100, 170, 300); - switch (osdConfig()->crsf_lq_format) { - case OSD_CRSF_LQ_TYPE1: - if (!failsafeIsReceivingRxData()) { - tfp_sprintf(buff+1, "%3d", 0); - } else { - tfp_sprintf(buff+1, "%3d", rxLinkStatistics.uplinkLQ); - } - break; + uint8_t lqFormat = osdConfig()->crsf_lq_format; + + if (rxConfig()->receiverType == RX_TYPE_MSP) + lqFormat = OSD_CRSF_LQ_TYPE1; + + switch (lqFormat) { case OSD_CRSF_LQ_TYPE2: if (!failsafeIsReceivingRxData()) { tfp_sprintf(buff+1, "%s:%3d", " ", 0); @@ -2488,9 +2518,18 @@ static bool osdDrawSingleElement(uint8_t item) if (!failsafeIsReceivingRxData()) { tfp_sprintf(buff+1, "%3d", 0); } else { + int16_t scaledLQ = scaleRange(constrain(rxLinkStatistics.uplinkLQ, 0, 100), 0, 100, 170, 300); tfp_sprintf(buff+1, "%3d", rxLinkStatistics.rfMode >= 2 ? scaledLQ : rxLinkStatistics.uplinkLQ); } break; + case OSD_CRSF_LQ_TYPE1: + default: + if (!failsafeIsReceivingRxData()) { + tfp_sprintf(buff+1, "%3d", 0); + } else { + tfp_sprintf(buff+1, "%3d", rxLinkStatistics.uplinkLQ); + } + break; } if (!failsafeIsReceivingRxData()) { TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); @@ -2500,7 +2539,24 @@ static bool osdDrawSingleElement(uint8_t item) break; } - case OSD_CRSF_SNR_DB: + case OSD_LQ_DOWNLINK: + { + buff[0] = SYM_LQ; + if (!failsafeIsReceivingRxData()) { + tfp_sprintf(buff+1, "%3d%c", 0, SYM_AH_DECORATION_DOWN); + } else { + tfp_sprintf(buff+1, "%3d%c", rxLinkStatistics.downlinkLQ, SYM_AH_DECORATION_DOWN); + } + + if (!failsafeIsReceivingRxData()) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } else if (rxLinkStatistics.downlinkLQ < osdConfig()->link_quality_alarm) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + break; + } + + case OSD_SNR_DB: { static pt1Filter_t snrFilterState; static timeMs_t snrUpdated = 0; @@ -2519,23 +2575,49 @@ static bool osdDrawSingleElement(uint8_t item) } } else if (snrFiltered <= osdConfig()->snr_alarm) { buff[0] = SYM_SNR; - if (snrFiltered <= -10) { + if (snrFiltered <= -10 || snrFiltered >= 10) { tfp_sprintf(buff + 1, "%3d%c", snrFiltered, SYM_DB); } else { - tfp_sprintf(buff + 1, "%2d%c%c", snrFiltered, SYM_DB, ' '); + tfp_sprintf(buff + 1, " %2d%c", snrFiltered, SYM_DB); } } break; } - case OSD_CRSF_TX_POWER: + case OSD_TX_POWER_UPLINK: { if (!failsafeIsReceivingRxData()) - tfp_sprintf(buff, "%s%c", " ", SYM_BLANK); + tfp_sprintf(buff, "%s%c", " ", SYM_MW); else tfp_sprintf(buff, "%4d%c", rxLinkStatistics.uplinkTXPower, SYM_MW); break; } + + case OSD_RX_POWER_DOWNLINK: + { + if (!failsafeIsReceivingRxData()) + tfp_sprintf(buff, "%s%c%c", " ", SYM_MW, SYM_AH_DECORATION_DOWN); + else + tfp_sprintf(buff, "%4d%c%c", rxLinkStatistics.downlinkTXPower, SYM_MW, SYM_AH_DECORATION_DOWN); + break; + } + case OSD_RX_BAND: + displayWriteChar(osdDisplayPort, elemPosX++, elemPosY, SYM_RX_BAND); + strcat(buff, rxLinkStatistics.band); + if (strlen(rxLinkStatistics.band) < 4) + for (uint8_t i = strlen(rxLinkStatistics.band); i < 4; i++) + buff[i] = ' '; + buff[4] = '\0'; + break; + + case OSD_RX_MODE: + displayWriteChar(osdDisplayPort, elemPosX++, elemPosY, SYM_RX_MODE); + strcat(buff, rxLinkStatistics.mode); + if (strlen(rxLinkStatistics.mode) < 6) + for (uint8_t i = strlen(rxLinkStatistics.mode); i < 6; i++) + buff[i] = ' '; + buff[6] = '\0'; + break; #endif case OSD_FORMATION_FLIGHT: @@ -2889,7 +2971,7 @@ static bool osdDrawSingleElement(uint8_t item) buff[0] = SYM_GLIDE_DIST; if (glideSeconds > 0) { uint32_t glideRangeCM = glideSeconds * gpsSol.groundSpeed; - osdFormatDistanceSymbol(buff + 1, glideRangeCM, 0); + osdFormatDistanceSymbol(buff + 1, glideRangeCM, 0, 3); } else { tfp_sprintf(buff + 1, "%s%c", "---", SYM_BLANK); buff[5] = '\0'; @@ -3002,6 +3084,10 @@ static bool osdDrawSingleElement(uint8_t item) osdDisplayNavPIDValues(elemPosX, elemPosY, "VZ", PID_VEL_Z, ADJUSTMENT_VEL_Z_P, ADJUSTMENT_VEL_Z_I, ADJUSTMENT_VEL_Z_D); return true; + case OSD_NAV_FW_ALT_CONTROL_RESPONSE: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "ACR", 0, pidProfile()->fwAltControlResponseFactor, 3, 0, ADJUSTMENT_NAV_FW_ALT_CONTROL_RESPONSE); + return true; + case OSD_HEADING_P: osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "HP", 0, pidBank()->pid[PID_HEADING].P, 3, 0, ADJUSTMENT_HEADING_P); return true; @@ -3821,6 +3907,52 @@ static bool osdDrawSingleElement(uint8_t item) clearMultiFunction = true; break; } +#if defined(USE_GEOZONE) + case OSD_COURSE_TO_FENCE: + { + if (navigationPositionEstimateIsHealthy() && isGeozoneActive()) { + int16_t panHomeDirOffset = 0; + if (!(osdConfig()->pan_servo_pwm2centideg == 0)){ + panHomeDirOffset = osdGetPanServoOffset(); + } + int16_t flightDirection = STATE(AIRPLANE) ? CENTIDEGREES_TO_DEGREES(posControl.actualState.cog) : DECIDEGREES_TO_DEGREES(osdGetHeading()); + int direction = CENTIDEGREES_TO_DEGREES(geozone.directionToNearestZone) - flightDirection + panHomeDirOffset; + osdDrawDirArrow(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), direction); + } else { + if (isGeozoneActive()) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + displayWriteCharWithAttr(osdDisplayPort, elemPosX, elemPosY, '-', elemAttr); + } + break; + } + + case OSD_H_DIST_TO_FENCE: + { + if (navigationPositionEstimateIsHealthy() && isGeozoneActive()) { + char buff2[12]; + osdFormatDistanceSymbol(buff2, geozone.distanceHorToNearestZone, 0, 3); + tfp_sprintf(buff, "FD %s", buff2 ); + } else { + strcpy(buff, "FD ---"); + } + } + break; + + case OSD_V_DIST_TO_FENCE: + { + if (navigationPositionEstimateIsHealthy() && isGeozoneActive()) { + char buff2[12]; + osdFormatAltitudeSymbol(buff2, abs(geozone.distanceVertToNearestZone)); + tfp_sprintf(buff, "FD%s", buff2); + displayWriteCharWithAttr(osdDisplayPort, elemPosX + 8, elemPosY, geozone.distanceVertToNearestZone < 0 ? SYM_DECORATION + 4 : SYM_DECORATION, elemAttr); + } else { + strcpy(buff, "FD ---"); + } + + break; + } +#endif default: return false; @@ -3976,7 +4108,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .adsb_distance_alert = SETTING_OSD_ADSB_DISTANCE_ALERT_DEFAULT, .adsb_ignore_plane_above_me_limit = SETTING_OSD_ADSB_IGNORE_PLANE_ABOVE_ME_LIMIT_DEFAULT, #endif -#ifdef USE_SERIALRX_CRSF +#if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP) .snr_alarm = SETTING_OSD_SNR_ALARM_DEFAULT, .crsf_lq_format = SETTING_OSD_CRSF_LQ_FORMAT_DEFAULT, .link_quality_alarm = SETTING_OSD_LINK_QUALITY_ALARM_DEFAULT, @@ -4044,6 +4176,8 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .system_msg_display_time = SETTING_OSD_SYSTEM_MSG_DISPLAY_TIME_DEFAULT, .units = SETTING_OSD_UNITS_DEFAULT, .main_voltage_decimals = SETTING_OSD_MAIN_VOLTAGE_DECIMALS_DEFAULT, + .decimals_altitude = SETTING_OSD_DECIMALS_ALTITUDE_DEFAULT, + .decimals_distance = SETTING_OSD_DECIMALS_DISTANCE_DEFAULT, .use_pilot_logo = SETTING_OSD_USE_PILOT_LOGO_DEFAULT, .inav_to_pilot_logo_spacing = SETTING_OSD_INAV_TO_PILOT_LOGO_SPACING_DEFAULT, .arm_screen_display_time = SETTING_OSD_ARM_SCREEN_DISPLAY_TIME_DEFAULT, @@ -4125,11 +4259,15 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig) osdLayoutsConfig->item_pos[0][OSD_PILOT_LOGO] = OSD_POS(20, 3); osdLayoutsConfig->item_pos[0][OSD_VTX_CHANNEL] = OSD_POS(8, 6); -#ifdef USE_SERIALRX_CRSF - osdLayoutsConfig->item_pos[0][OSD_CRSF_RSSI_DBM] = OSD_POS(23, 12); - osdLayoutsConfig->item_pos[0][OSD_CRSF_LQ] = OSD_POS(23, 11); - osdLayoutsConfig->item_pos[0][OSD_CRSF_SNR_DB] = OSD_POS(24, 9); - osdLayoutsConfig->item_pos[0][OSD_CRSF_TX_POWER] = OSD_POS(24, 10); +#if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP) + osdLayoutsConfig->item_pos[0][OSD_RSSI_DBM] = OSD_POS(23, 12); + osdLayoutsConfig->item_pos[0][OSD_LQ_UPLINK] = OSD_POS(23, 10); + osdLayoutsConfig->item_pos[0][OSD_LQ_DOWNLINK] = OSD_POS(23, 11); + osdLayoutsConfig->item_pos[0][OSD_SNR_DB] = OSD_POS(24, 9); + osdLayoutsConfig->item_pos[0][OSD_TX_POWER_UPLINK] = OSD_POS(24, 10); + osdLayoutsConfig->item_pos[0][OSD_RX_POWER_DOWNLINK] = OSD_POS(24, 11); + osdLayoutsConfig->item_pos[0][OSD_RX_BAND] = OSD_POS(24, 12); + osdLayoutsConfig->item_pos[0][OSD_RX_MODE] = OSD_POS(24, 13); #endif osdLayoutsConfig->item_pos[0][OSD_ONTIME] = OSD_POS(23, 8); @@ -4264,18 +4402,11 @@ uint8_t drawLogos(bool singular, uint8_t row) { bool usePilotLogo = (osdConfig()->use_pilot_logo && osdDisplayIsHD()); bool useINAVLogo = (singular && !usePilotLogo) || !singular; -#ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is in use, the pilot logo cannot be used, due to font issues. - if (isDJICompatibleVideoSystem(osdConfig())) { - usePilotLogo = false; - useINAVLogo = false; - } -#endif - uint8_t logoSpacing = osdConfig()->inav_to_pilot_logo_spacing; if (logoSpacing > 0 && ((osdDisplayPort->cols % 2) != (logoSpacing % 2))) { logoSpacing++; // Add extra 1 character space between logos, if the odd/even of the OSD cols doesn't match the odd/even of the logo spacing -} + } // Draw Logo(s) if (usePilotLogo && !singular) { @@ -4564,7 +4695,7 @@ static void osdResetStats(void) stats.max_3D_speed = 0; stats.max_air_speed = 0; stats.min_voltage = 12000; - stats.min_rssi = 99; + stats.min_rssi = 100; stats.min_lq = 300; stats.min_rssi_dbm = 0; stats.max_altitude = 0; @@ -4789,7 +4920,7 @@ uint8_t drawStat_UsedEnergy(uint8_t col, uint8_t row, uint8_t statValX) strcat(buff, "/"); osdFormatCentiNumber(preBuff, getMWhDrawn() / 10, 0, 2, 0, 3, false); strcat(buff, osdFormatTrimWhiteSpace(preBuff)); - tfp_sprintf(buff + strlen(buff), "%s%c", buff, SYM_WH); + tfp_sprintf(buff + strlen(buff), "%c", SYM_WH); } displayWrite(osdDisplayPort, statValX, row++, buff); @@ -4851,7 +4982,7 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b osdFormatCentiNumber(buff, (int32_t)((getMWhDrawn() - stats.flightStartMWh) * 10.0f * METERS_PER_MILE / totalDistance), 0, 2, 0, digits, false); strcat(outBuff, osdFormatTrimWhiteSpace(buff)); strcat(outBuff, "/"); - osdFormatCentiNumber(buff + strlen(buff), (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_MILE / totalDistance), 0, 2, 0, digits, false); + osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_MILE / totalDistance), 0, 2, 0, digits, false); strcat(outBuff, osdFormatTrimWhiteSpace(buff)); } else { strcat(outBuff, "---/---"); @@ -5203,65 +5334,92 @@ static void osdShowHDArmScreen(void) dateTime_t dt; char buf[MAX(osdDisplayPort->cols, FORMATTED_DATE_TIME_BUFSIZE)]; char buf2[MAX(osdDisplayPort->cols, FORMATTED_DATE_TIME_BUFSIZE)]; - char craftNameBuf[MAX_NAME_LENGTH]; + char craftNameBuf[MAX_NAME_LENGTH]; char versionBuf[osdDisplayPort->cols]; uint8_t safehomeRow = 0; - uint8_t armScreenRow = 2; // Start at row 2 + uint8_t armScreenRow = 1; + + bool showPilotOrCraftName = false; armScreenRow = drawLogos(false, armScreenRow); armScreenRow++; + memset(buf2, '\0', sizeof(buf2)); + if (!osdConfig()->use_pilot_logo && strlen(systemConfig()->pilotName) > 0) { + osdFormatPilotName(buf2); + showPilotOrCraftName = true; + } + + memset(craftNameBuf, '\0', sizeof(craftNameBuf)); if (strlen(systemConfig()->craftName) > 0) { osdFormatCraftName(craftNameBuf); - strcpy(buf2, "ARMED!"); - tfp_sprintf(buf, "%s - %s", craftNameBuf, buf2); - } else { - strcpy(buf, "ARMED!"); + if (strlen(buf2) > 0) { + strcat(buf2, " : "); + } + showPilotOrCraftName = true; } + + if (showPilotOrCraftName) { + tfp_sprintf(buf, "%s%s: ! ARMED !", buf2, craftNameBuf); + } else { + strcpy(buf, " ! ARMED !"); + } + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + memset(buf, '\0', sizeof(buf)); + memset(buf2, '\0', sizeof(buf2)); + #if defined(USE_GPS) #if defined (USE_SAFE_HOME) if (posControl.safehomeState.distance) { safehomeRow = armScreenRow; - armScreenRow++; + armScreenRow +=2; } #endif // USE_SAFE_HOME #endif // USE_GPS - armScreenRow++; if (posControl.waypointListValid && posControl.waypointCount > 0) { #ifdef USE_MULTI_MISSION tfp_sprintf(buf, "MISSION %u/%u (%u WP)", posControl.loadedMultiMissionIndex, posControl.multiMissionCount, posControl.waypointCount); displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + memset(buf, '\0', sizeof(buf)); #else strcpy(buf, "*MISSION LOADED*"); displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + memset(buf, '\0', sizeof(buf)); #endif } - armScreenRow++; #if defined(USE_GPS) if (feature(FEATURE_GPS)) { if (STATE(GPS_FIX_HOME)) { - if (osdConfig()->osd_home_position_arm_screen){ - osdFormatCoordinate(buf, SYM_LAT, GPS_home.lat); - osdFormatCoordinate(buf2, SYM_LON, GPS_home.lon); - uint8_t gap = 1; - uint8_t col = strlen(buf) + strlen(buf2) + gap; + if (osdConfig()->osd_home_position_arm_screen) { + // Show pluscode if enabled on any OSD layout. Otherwise show GNSS cordinates. + if (osdElementEnabled(OSD_PLUS_CODE, false)) { + int digits = osdConfig()->plus_code_digits; + olc_encode(GPS_home.lat, GPS_home.lon, digits, buf, sizeof(buf)); + tfp_sprintf(buf2, "+CODE: %s%c", buf, '\0'); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf2)) / 2, armScreenRow++, buf2); + memset(buf, '\0', sizeof(buf)); + memset(buf2, '\0', sizeof(buf2)); + } else { + osdFormatCoordinate(buf, SYM_LAT, GPS_home.lat); + osdFormatCoordinate(buf2, SYM_LON, GPS_home.lon); + uint8_t gap = 1; + uint8_t col = strlen(buf) + strlen(buf2) + gap; - if ((osdDisplayPort->cols %2) != (col %2)) { - gap++; - col++; + if ((osdDisplayPort->cols %2) != (col %2)) { + gap++; + col++; + } + + col = (osdDisplayPort->cols - col) / 2; + + displayWrite(osdDisplayPort, col, armScreenRow, buf); + displayWrite(osdDisplayPort, col + strlen(buf) + gap, armScreenRow++, buf2); + memset(buf, '\0', sizeof(buf)); + memset(buf2, '\0', sizeof(buf2)); } - - col = (osdDisplayPort->cols - col) / 2; - - displayWrite(osdDisplayPort, col, armScreenRow, buf); - displayWrite(osdDisplayPort, col + strlen(buf) + gap, armScreenRow++, buf2); - - int digits = osdConfig()->plus_code_digits; - olc_encode(GPS_home.lat, GPS_home.lon, digits, buf, sizeof(buf)); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); } #if defined (USE_SAFE_HOME) @@ -5275,19 +5433,23 @@ static void osdShowHDArmScreen(void) textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT; // write this message below the ARMED message to make it obvious displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, safehomeRow, buf, elemAttr); + memset(buf, '\0', sizeof(buf)); + memset(buf2, '\0', sizeof(buf2)); } #endif } else { strcpy(buf, "!NO HOME POSITION!"); displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + memset(buf, '\0', sizeof(buf)); + armScreenRow++; } - armScreenRow++; } #endif if (rtcGetDateTimeLocal(&dt)) { tfp_sprintf(buf, "%04u-%02u-%02u %02u:%02u:%02u", dt.year, dt.month, dt.day, dt.hours, dt.minutes, dt.seconds); displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + memset(buf, '\0', sizeof(buf)); armScreenRow++; } @@ -5308,45 +5470,79 @@ static void osdShowSDArmScreen(void) char buf2[MAX(osdDisplayPort->cols, FORMATTED_DATE_TIME_BUFSIZE)]; char craftNameBuf[MAX_NAME_LENGTH]; char versionBuf[osdDisplayPort->cols]; - // We need 12 visible rows, start row never < first fully visible row 1 - uint8_t armScreenRow = osdDisplayPort->rows > 13 ? (osdDisplayPort->rows - 12) / 2 : 1; + uint8_t armScreenRow = 1; uint8_t safehomeRow = 0; + bool showPilotOrCraftName = false; - strcpy(buf, "ARMED!"); + strcpy(buf, "! ARMED !"); displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); - safehomeRow = armScreenRow; - armScreenRow++; + memset(buf, '\0', sizeof(buf)); +#if defined(USE_GPS) +#if defined (USE_SAFE_HOME) + if (posControl.safehomeState.distance) { + safehomeRow = armScreenRow; + armScreenRow += 2; + } +#endif +#endif + memset(buf2, '\0', sizeof(buf2)); + if (strlen(systemConfig()->pilotName) > 0) { + osdFormatPilotName(buf2); + showPilotOrCraftName = true; + } + + memset(craftNameBuf, '\0', sizeof(craftNameBuf)); if (strlen(systemConfig()->craftName) > 0) { osdFormatCraftName(craftNameBuf); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(systemConfig()->craftName)) / 2, armScreenRow++, craftNameBuf ); + if (strlen(buf2) > 0) { + strcat(buf2, " : "); + } + showPilotOrCraftName = true; + } + + if (showPilotOrCraftName) { + tfp_sprintf(buf, "%s%s", buf2, craftNameBuf); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf ); + memset(buf, '\0', sizeof(buf)); + memset(buf2, '\0', sizeof(buf2)); + armScreenRow++; } if (posControl.waypointListValid && posControl.waypointCount > 0) { #ifdef USE_MULTI_MISSION tfp_sprintf(buf, "MISSION %u/%u (%u WP)", posControl.loadedMultiMissionIndex, posControl.multiMissionCount, posControl.waypointCount); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow, buf); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + memset(buf, '\0', sizeof(buf)); #else strcpy(buf, "*MISSION LOADED*"); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow, buf); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + memset(buf, '\0', sizeof(buf)); #endif } - armScreenRow++; #if defined(USE_GPS) if (feature(FEATURE_GPS)) { if (STATE(GPS_FIX_HOME)) { if (osdConfig()->osd_home_position_arm_screen) { - osdFormatCoordinate(buf, SYM_LAT, GPS_home.lat); - osdFormatCoordinate(buf2, SYM_LON, GPS_home.lon); + // Show pluscode if enabled on any OSD layout. Otherwise show GNSS cordinates. + if (osdElementEnabled(OSD_PLUS_CODE, false)) { + int digits = osdConfig()->plus_code_digits; + olc_encode(GPS_home.lat, GPS_home.lon, digits, buf, sizeof(buf)); + tfp_sprintf(buf2, "+CODE: %s%c", buf, '\0'); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf2)) / 2, armScreenRow++, buf2); + memset(buf, '\0', sizeof(buf)); + memset(buf2, '\0', sizeof(buf2)); + } else { + osdFormatCoordinate(buf, SYM_LAT, GPS_home.lat); + osdFormatCoordinate(buf2, SYM_LON, GPS_home.lon); - uint8_t gpsStartCol = (osdDisplayPort->cols - (strlen(buf) + strlen(buf2) + 2)) / 2; - displayWrite(osdDisplayPort, gpsStartCol, armScreenRow, buf); - displayWrite(osdDisplayPort, gpsStartCol + strlen(buf) + 2, armScreenRow++, buf2); - - int digits = osdConfig()->plus_code_digits; - olc_encode(GPS_home.lat, GPS_home.lon, digits, buf, sizeof(buf)); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + uint8_t gpsStartCol = (osdDisplayPort->cols - (strlen(buf) + strlen(buf2) + 2)) / 2; + displayWrite(osdDisplayPort, gpsStartCol, armScreenRow, buf); + displayWrite(osdDisplayPort, gpsStartCol + strlen(buf) + 2, armScreenRow++, buf2); + memset(buf, '\0', sizeof(buf)); + memset(buf2, '\0', sizeof(buf2)); + } } #if defined (USE_SAFE_HOME) @@ -5357,22 +5553,25 @@ static void osdShowSDArmScreen(void) osdFormatDistanceStr(buf2, posControl.safehomeState.distance); tfp_sprintf(buf, "%c SAFEHOME %u @ %s", SYM_HOME, posControl.safehomeState.index, buf2); } - textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT; // write this message below the ARMED message to make it obvious - displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, safehomeRow, buf, elemAttr); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, safehomeRow, buf); + memset(buf, '\0', sizeof(buf)); + memset(buf2, '\0', sizeof(buf2)); } #endif } else { strcpy(buf, "!NO HOME POSITION!"); displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + memset(buf, '\0', sizeof(buf)); + armScreenRow++; } - armScreenRow++; } #endif if (rtcGetDateTimeLocal(&dt)) { tfp_sprintf(buf, "%04u-%02u-%02u %02u:%02u:%02u", dt.year, dt.month, dt.day, dt.hours, dt.minutes, dt.seconds); displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + memset(buf, '\0', sizeof(buf)); armScreenRow++; } @@ -5779,7 +5978,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter } else if (NAV_Status.state == MW_NAV_STATE_WP_ENROUTE) { // Countdown display for remaining Waypoints char buf[6]; - osdFormatDistanceSymbol(buf, posControl.wpDistance, 0); + osdFormatDistanceSymbol(buf, posControl.wpDistance, 0, 3); tfp_sprintf(messageBuf, "TO WP %u/%u (%s)", getGeoWaypointNumber(posControl.activeWaypointIndex), posControl.geoWaypointCount, buf); messages[messageCount++] = messageBuf; } else if (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED) { @@ -5808,6 +6007,12 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter messages[messageCount++] = safehomeMessage; } #endif + +#ifdef USE_GEOZONE + if (geozone.avoidInRTHInProgress) { + messages[messageCount++] = OSD_MSG_AVOID_ZONES_RTH; + } +#endif if (FLIGHT_MODE(FAILSAFE_MODE)) { // In FS mode while armed if (NAV_Status.state == MW_NAV_STATE_LAND_SETTLE && posControl.landingDelay > 0) { uint16_t remainingHoldSec = MS2S(posControl.landingDelay - millis()); @@ -5832,6 +6037,64 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter } else if (STATE(LANDING_DETECTED)) { messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_LANDED); } else { +#ifdef USE_GEOZONE + char buf[12], buf1[12]; + switch (geozone.messageState) { + case GEOZONE_MESSAGE_STATE_NFZ: + messages[messageCount++] = OSD_MSG_NFZ; + break; + case GEOZONE_MESSAGE_STATE_LEAVING_FZ: + osdFormatDistanceSymbol(buf, geozone.distanceToZoneBorder3d, 0, 3); + tfp_sprintf(messageBuf, OSD_MSG_LEAVING_FZ, buf); + messages[messageCount++] = messageBuf; + break; + case GEOZONE_MESSAGE_STATE_OUTSIDE_FZ: + messages[messageCount++] = OSD_MSG_OUTSIDE_FZ; + break; + case GEOZONE_MESSAGE_STATE_ENTERING_NFZ: + osdFormatDistanceSymbol(buf, geozone.distanceToZoneBorder3d, 0, 3); + if (geozone.zoneInfo == INT32_MAX) { + tfp_sprintf(buf1, "%s%c", "INF", SYM_ALT_M); + } else { + osdFormatAltitudeSymbol(buf1, geozone.zoneInfo); + } + tfp_sprintf(messageBuf, OSD_MSG_ENTERING_NFZ, buf, buf1); + messages[messageCount++] = messageBuf; + break; + case GEOZONE_MESSAGE_STATE_AVOIDING_FB: + messages[messageCount++] = OSD_MSG_AVOIDING_FB; + if (!posControl.sendTo.lockSticks) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_RETURN_TO_ZONE: + messages[messageCount++] = OSD_MSG_RETURN_TO_ZONE; + if (!posControl.sendTo.lockSticks) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_AVOIDING_ALTITUDE_BREACH: + messages[messageCount++] = OSD_MSG_AVOIDING_ALT_BREACH; + if (!posControl.sendTo.lockSticks) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_FLYOUT_NFZ: + messages[messageCount++] = OSD_MSG_FLYOUT_NFZ; + if (!posControl.sendTo.lockSticks) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_POS_HOLD: + messages[messageCount++] = OSD_MSG_AVOIDING_FB; + if (!geozone.sticksLocked) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_NONE: + break; + } +#endif /* Messages shown only when Failsafe, WP, RTH or Emergency Landing not active and landed state inactive */ /* ADDS MAXIMUM OF 3 MESSAGES TO TOTAL */ if (STATE(AIRPLANE)) { /* ADDS MAXIMUM OF 3 MESSAGES TO TOTAL */ @@ -5870,6 +6133,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ANGLEHOLD_PITCH); } } + } else if (STATE(MULTIROTOR)) { /* ADDS MAXIMUM OF 2 MESSAGES TO TOTAL */ if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { if (posControl.cruise.multicopterSpeed >= 50.0f) { diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 0cf93b6916..b2e94b5272 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -132,6 +132,19 @@ #define OSD_MSG_LOITERING_SAFEHOME "LOITERING AROUND SAFEHOME" #endif +#if defined(USE_GEOZONE) +#define OSD_MSG_NFZ "NO FLY ZONE" +#define OSD_MSG_LEAVING_FZ "LEAVING FZ IN %s" +#define OSD_MSG_OUTSIDE_FZ "OUTSIDE FZ" +#define OSD_MSG_ENTERING_NFZ "ENTERING NFZ IN %s %s" +#define OSD_MSG_AVOIDING_FB "AVOIDING FENCE BREACH" +#define OSD_MSG_RETURN_TO_ZONE "RETURN TO FZ" +#define OSD_MSG_FLYOUT_NFZ "FLY OUT NFZ" +#define OSD_MSG_AVOIDING_ALT_BREACH "REACHED ZONE ALTITUDE LIMIT" +#define OSD_MSG_AVOID_ZONES_RTH "AVOIDING NO FLY ZONES" +#define OSD_MSG_GEOZONE_ACTION "PERFORM ACTION IN %s %s" +#endif + typedef enum { OSD_RSSI_VALUE, OSD_MAIN_BATT_VOLTAGE, @@ -242,10 +255,10 @@ typedef enum { OSD_ESC_RPM, OSD_ESC_TEMPERATURE, OSD_AZIMUTH, - OSD_CRSF_RSSI_DBM, - OSD_CRSF_LQ, - OSD_CRSF_SNR_DB, - OSD_CRSF_TX_POWER, + OSD_RSSI_DBM, + OSD_LQ_UPLINK, + OSD_SNR_DB, + OSD_TX_POWER_UPLINK, OSD_GVAR_0, OSD_GVAR_1, OSD_GVAR_2, @@ -291,7 +304,15 @@ typedef enum { OSD_CUSTOM_ELEMENT_5, OSD_CUSTOM_ELEMENT_6, OSD_CUSTOM_ELEMENT_7, - OSD_CUSTOM_ELEMENT_8, // 158 + OSD_CUSTOM_ELEMENT_8, + OSD_LQ_DOWNLINK, + OSD_RX_POWER_DOWNLINK, // 160 + OSD_RX_BAND, + OSD_RX_MODE, + OSD_COURSE_TO_FENCE, + OSD_H_DIST_TO_FENCE, + OSD_V_DIST_TO_FENCE, + OSD_NAV_FW_ALT_CONTROL_RESPONSE, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; @@ -369,7 +390,7 @@ typedef struct osdConfig_s { float gforce_alarm; float gforce_axis_alarm_min; float gforce_axis_alarm_max; -#ifdef USE_SERIALRX_CRSF +#if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP) int8_t snr_alarm; //CRSF SNR alarm in dB int8_t link_quality_alarm; int16_t rssi_dbm_alarm; // in dBm @@ -394,6 +415,8 @@ typedef struct osdConfig_s { // Preferences uint8_t main_voltage_decimals; + uint8_t decimals_altitude; + uint8_t decimals_distance; uint8_t ahi_reverse_roll; uint8_t ahi_max_pitch; uint8_t crosshairs_style; // from osd_crosshairs_style_e @@ -466,12 +489,17 @@ typedef struct osdConfig_s { #ifndef DISABLE_MSP_DJI_COMPAT bool highlight_djis_missing_characters; // If enabled, show question marks where there is no character in DJI's font to represent an OSD element symbol #endif - #ifdef USE_ADSB + bool enable_broken_o4_workaround; // If enabled, override STATUS/STATUS_EX messages to work around DJI's broken O4 air unit MSP DisplayPort implementation +#ifdef USE_ADSB uint16_t adsb_distance_warning; // in metres uint16_t adsb_distance_alert; // in metres uint16_t adsb_ignore_plane_above_me_limit; // in metres - #endif +#endif uint8_t radar_peers_display_time; // in seconds +#ifdef USE_GEOZONE + uint8_t geozoneDistanceWarning; // Distance to fence or action + bool geozoneDistanceType; // Shows a countdown timer or distance to fence/action +#endif } osdConfig_t; PG_DECLARE(osdConfig_t, osdConfig); diff --git a/src/main/io/osd/custom_elements.c b/src/main/io/osd/custom_elements.c index df84ddd76b..15dc8714c9 100644 --- a/src/main/io/osd/custom_elements.c +++ b/src/main/io/osd/custom_elements.c @@ -98,6 +98,11 @@ uint8_t customElementDrawPart(char *buff, uint8_t customElementIndex, uint8_t cu osdFormatCentiNumber(buff, (int32_t) (constrain(gvGet(customPartValue), -99, 99) * (int32_t) 10), 1, 1, 0, 3, false); return 3; } + case CUSTOM_ELEMENT_TYPE_GV_FLOAT_1_2: + { + osdFormatCentiNumber(buff, (int32_t) (constrain(gvGet(customPartValue), -999, 999)), 1, 2, 0, 4, false); + return 4; + } case CUSTOM_ELEMENT_TYPE_GV_FLOAT_2_1: { osdFormatCentiNumber(buff, (int32_t) (constrain(gvGet(customPartValue), -999, 999) * (int32_t) 10), 1, 1, 0, 4, false); @@ -154,6 +159,11 @@ uint8_t customElementDrawPart(char *buff, uint8_t customElementIndex, uint8_t cu osdFormatCentiNumber(buff, (int32_t) (constrain(logicConditionGetValue(customPartValue), -99, 99) * (int32_t) 10), 1, 1, 0, 3, false); return 3; } + case CUSTOM_ELEMENT_TYPE_LC_FLOAT_1_2: + { + osdFormatCentiNumber(buff, (int32_t) (constrain(logicConditionGetValue(customPartValue), -999, 999)), 1, 2, 0, 4, false); + return 4; + } case CUSTOM_ELEMENT_TYPE_LC_FLOAT_2_1: { osdFormatCentiNumber(buff, (int32_t) (constrain(logicConditionGetValue(customPartValue), -999, 999) * (int32_t) 10), 1, 1, 0, 4, false); diff --git a/src/main/io/osd/custom_elements.h b/src/main/io/osd/custom_elements.h index 9c12ede673..8f5ee15881 100644 --- a/src/main/io/osd/custom_elements.h +++ b/src/main/io/osd/custom_elements.h @@ -35,22 +35,25 @@ typedef enum { CUSTOM_ELEMENT_TYPE_GV_4 = 8, CUSTOM_ELEMENT_TYPE_GV_5 = 9, CUSTOM_ELEMENT_TYPE_GV_FLOAT_1_1 = 10, - CUSTOM_ELEMENT_TYPE_GV_FLOAT_2_1 = 11, - CUSTOM_ELEMENT_TYPE_GV_FLOAT_2_2 = 12, - CUSTOM_ELEMENT_TYPE_GV_FLOAT_3_1 = 13, - CUSTOM_ELEMENT_TYPE_GV_FLOAT_3_2 = 14, - CUSTOM_ELEMENT_TYPE_GV_FLOAT_4_1 = 15, - CUSTOM_ELEMENT_TYPE_LC_1 = 16, - CUSTOM_ELEMENT_TYPE_LC_2 = 17, - CUSTOM_ELEMENT_TYPE_LC_3 = 18, - CUSTOM_ELEMENT_TYPE_LC_4 = 19, - CUSTOM_ELEMENT_TYPE_LC_5 = 20, - CUSTOM_ELEMENT_TYPE_LC_FLOAT_1_1 = 21, - CUSTOM_ELEMENT_TYPE_LC_FLOAT_2_1 = 22, - CUSTOM_ELEMENT_TYPE_LC_FLOAT_2_2 = 23, - CUSTOM_ELEMENT_TYPE_LC_FLOAT_3_1 = 24, - CUSTOM_ELEMENT_TYPE_LC_FLOAT_3_2 = 25, - CUSTOM_ELEMENT_TYPE_LC_FLOAT_4_1 = 26, + CUSTOM_ELEMENT_TYPE_GV_FLOAT_1_2 = 11, + CUSTOM_ELEMENT_TYPE_GV_FLOAT_2_1 = 12, + CUSTOM_ELEMENT_TYPE_GV_FLOAT_2_2 = 13, + CUSTOM_ELEMENT_TYPE_GV_FLOAT_3_1 = 14, + CUSTOM_ELEMENT_TYPE_GV_FLOAT_3_2 = 15, + CUSTOM_ELEMENT_TYPE_GV_FLOAT_4_1 = 16, + CUSTOM_ELEMENT_TYPE_LC_1 = 17, + CUSTOM_ELEMENT_TYPE_LC_2 = 18, + CUSTOM_ELEMENT_TYPE_LC_3 = 19, + CUSTOM_ELEMENT_TYPE_LC_4 = 20, + CUSTOM_ELEMENT_TYPE_LC_5 = 21, + CUSTOM_ELEMENT_TYPE_LC_FLOAT_1_1 = 22, + CUSTOM_ELEMENT_TYPE_LC_FLOAT_1_2 = 23, + CUSTOM_ELEMENT_TYPE_LC_FLOAT_2_1 = 24, + CUSTOM_ELEMENT_TYPE_LC_FLOAT_2_2 = 25, + CUSTOM_ELEMENT_TYPE_LC_FLOAT_3_1 = 26, + CUSTOM_ELEMENT_TYPE_LC_FLOAT_3_2 = 27, + CUSTOM_ELEMENT_TYPE_LC_FLOAT_4_1 = 28, + CUSTOM_ELEMENT_TYPE_END // Must be last } osdCustomElementType_e; typedef enum { diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index b018d8f906..cf4f5f8872 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -89,13 +89,6 @@ #if defined(USE_DJI_HD_OSD) -#define DJI_MSP_BAUDRATE 115200 - -#define DJI_ARMING_DISABLE_FLAGS_COUNT 25 -#define DJI_OSD_WARNING_COUNT 16 -#define DJI_OSD_TIMER_COUNT 2 -#define DJI_OSD_FLAGS_OSD_FEATURE (1 << 0) -#define EFFICIENCY_UPDATE_INTERVAL (5 * 1000) #define RC_RX_LINK_LOST_MSG "!RC RX LINK LOST!" @@ -269,7 +262,7 @@ void djiOsdSerialInit(void) } } -static void djiPackBoxModeBitmask(boxBitmask_t * flightModeBitmask) +void djiPackBoxModeBitmask(boxBitmask_t * flightModeBitmask) { memset(flightModeBitmask, 0, sizeof(boxBitmask_t)); @@ -311,7 +304,7 @@ static void djiPackBoxModeBitmask(boxBitmask_t * flightModeBitmask) } } -static uint32_t djiPackArmingDisabledFlags(void) +uint32_t djiPackArmingDisabledFlags(void) { // TODO: Map INAV arming disabled flags to DJI/BF ones // https://github.com/betaflight/betaflight/blob/c6e5882dd91fa20d246b8f8af10cf6c92876bc3d/src/main/fc/runtime_config.h#L42 @@ -525,6 +518,8 @@ static char * osdArmingDisabledReasonMessage(void) case ARMING_DISABLED_DSHOT_BEEPER: return OSD_MESSAGE_STR("MOTOR BEEPER ACTIVE"); // Cases without message + case ARMING_DISABLED_GEOZONE: + return OSD_MESSAGE_STR("NO FLY ZONE"); case ARMING_DISABLED_LANDING_DETECTED: FALLTHROUGH; case ARMING_DISABLED_CMS_MENU: @@ -956,6 +951,9 @@ static void osdDJIAdjustmentMessage(char *buff, uint8_t adjustmentFunction) case ADJUSTMENT_VEL_Z_D: tfp_sprintf(buff, "VZD %3d", pidBankMutable()->pid[PID_VEL_Z].D); break; + case ADJUSTMENT_NAV_FW_ALT_CONTROL_RESPONSE: + tfp_sprintf(buff, "ACR %3d", pidProfileMutable()->fwAltControlResponseFactor); + break; case ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE: tfp_sprintf(buff, "MTDPA %4d", navConfigMutable()->fw.minThrottleDownPitchAngle); break; diff --git a/src/main/io/osd_dji_hd.h b/src/main/io/osd_dji_hd.h index f109f84fb1..35b573e1d6 100644 --- a/src/main/io/osd_dji_hd.h +++ b/src/main/io/osd_dji_hd.h @@ -30,6 +30,7 @@ #include "msp/msp_serial.h" #include "config/parameter_group.h" +#include "fc/rc_modes.h" #if defined(USE_DJI_HD_OSD) @@ -66,6 +67,14 @@ #define DJI_ALTERNATING_DURATION_LONG (djiOsdConfig()->craftNameAlternatingDuration * 100) #define DJI_ALTERNATING_DURATION_SHORT 1000 +#define DJI_MSP_BAUDRATE 115200 + +#define DJI_ARMING_DISABLE_FLAGS_COUNT 25 +#define DJI_OSD_WARNING_COUNT 16 +#define DJI_OSD_TIMER_COUNT 2 +#define DJI_OSD_FLAGS_OSD_FEATURE (1 << 0) +#define EFFICIENCY_UPDATE_INTERVAL (5 * 1000) + enum djiOsdTempSource_e { DJI_OSD_TEMP_ESC = 0, DJI_OSD_TEMP_CORE = 1, @@ -95,4 +104,7 @@ PG_DECLARE(djiOsdConfig_t, djiOsdConfig); void djiOsdSerialInit(void); void djiOsdSerialProcess(void); +uint32_t djiPackArmingDisabledFlags(void); +void djiPackBoxModeBitmask(boxBitmask_t * flightModeBitmask); + #endif diff --git a/src/main/io/vtx_msp.c b/src/main/io/vtx_msp.c index 696918e570..84746e0c9f 100644 --- a/src/main/io/vtx_msp.c +++ b/src/main/io/vtx_msp.c @@ -17,157 +17,102 @@ * * If not, see . */ +/* Created by geoffsim */ -/* Created by phobos- */ - -#include #include -#include -#include -#include #include +#include #include "platform.h" #if defined(USE_VTX_MSP) && defined(USE_VTX_CONTROL) && defined(USE_VTX_COMMON) -#include "build/debug.h" - -//#include "cms/cms_menu_vtx_msp.h" -#include "common/crc.h" #include "common/log.h" -#include "config/feature.h" - +#include "common/crc.h" #include "drivers/vtx_common.h" -//#include "drivers/vtx_table.h" - -#include "fc/runtime_config.h" -#include "flight/failsafe.h" - -#include "io/serial.h" -#include "io/vtx_msp.h" -#include "io/vtx_control.h" -#include "io/vtx_string.h" -#include "io/vtx_smartaudio.h" -#include "io/vtx.h" -#include "io/displayport_msp_osd.h" - #include "msp/msp_protocol.h" -#include "msp/msp_serial.h" -#include "msp/msp.h" - -//#include "pg/vtx_table.h" -#include "fc/settings.h" - -#include "rx/crsf.h" -//#include "rx/crsf_protocol.h" #include "rx/rx.h" +#include "rx/crsf.h" +#include "telemetry/crsf.h" +#include "vtx.h" +#include "displayport_msp_osd.h" +#include "vtx_string.h" +#include "vtx_msp.h" -#include "telemetry/msp_shared.h" +#define VTX_MSP_MIN_BAND (1) +#define VTX_MSP_MAX_BAND (VTX_MSP_MIN_BAND + VTX_MSP_BAND_COUNT - 1) +#define VTX_MSP_MIN_CHANNEL (1) +#define VTX_MSP_MAX_CHANNEL (VTX_MSP_MIN_CHANNEL + VTX_MSP_CHANNEL_COUNT -1) -//static uint16_t mspConfFreq = 0; -static uint8_t mspConfBand = SETTING_VTX_BAND_DEFAULT; -static uint8_t mspConfChannel = SETTING_VTX_CHANNEL_DEFAULT; -//static uint16_t mspConfPower = 0; -static uint16_t mspConfPowerIndex = SETTING_VTX_POWER_DEFAULT; -static uint8_t mspConfPitMode = 0; -static bool mspVtxConfigChanged = false; -static timeUs_t mspVtxLastTimeUs = 0; -static bool prevLowPowerDisarmedState = false; +#define VTX_UPDATE_REQ_NONE 0x00 +#define VTX_UPDATE_REQ_FREQUENCY 0x01 +#define VTX_UPDATE_REQ_POWER 0x02 +#define VTX_UPDATE_REQ_PIT_MODE 0x04 -static const vtxVTable_t mspVTable; // forward -static vtxDevice_t vtxMsp = { - .vTable = &mspVTable, - .capability.bandCount = VTX_MSP_TABLE_MAX_BANDS, - .capability.channelCount = VTX_MSP_TABLE_MAX_CHANNELS, - .capability.powerCount = VTX_MSP_TABLE_MAX_POWER_LEVELS, - .capability.bandNames = (char **)vtx58BandNames, - .capability.channelNames = (char **)vtx58ChannelNames, - .capability.powerNames = (char**)saPowerNames +typedef struct { + bool ready; + uint8_t timeouts; + uint8_t updateReqMask; + bool crsfTelemetryEnabled; + struct { + uint8_t band; + uint8_t channel; + uint16_t freq; + uint8_t power; + uint8_t powerIndex; + uint8_t pitMode; + } request; +; +} vtxProtoState_t; + +const char * const vtxMspBandNames[VTX_MSP_BAND_COUNT + 1] = { + "-----", "A 2.4", "B 2.4", "E 2.4", "F 2.4", "R 2.4" }; -STATIC_UNIT_TESTED mspVtxStatus_e mspVtxStatus = MSP_VTX_STATUS_OFFLINE; -static uint8_t mspVtxPortIdentifier = 255; +const char * vtxMspBandLetters = "-ABEFR"; -#define MSP_VTX_REQUEST_PERIOD_US (200 * 1000) // 200ms +const char * const vtxMspChannelNames[VTX_MSP_CHANNEL_COUNT + 1] = { + "-", "1", "2", "3", "4", "5", "6", "7", "8" +}; -static bool isCrsfPortConfig(const serialPortConfig_t *portConfig) +const char * const vtxMspPowerNames[VTX_MSP_POWER_COUNT + 1] = { + "0", "25", "200", "500", "MAX" +}; + +const unsigned vtxMspPowerTable[VTX_MSP_POWER_COUNT + 1] = { + 0, 25, 200, 500, 1000 +}; + +static serialPortIdentifier_e mspVtxPortIdentifier; +static vtxProtoState_t vtxState; + +static vtxDevType_e vtxMspGetDeviceType(const vtxDevice_t *); +static bool vtxMspIsReady(const vtxDevice_t *); +static vtxDevice_t vtxMsp; + +static void prepareMspFrame(vtxDevice_t *vtxDevice, uint8_t *mspFrame) { - return portConfig->functionMask & FUNCTION_RX_SERIAL && portConfig->functionMask & FUNCTION_VTX_MSP && rxConfig()->serialrx_provider == SERIALRX_CRSF; -} - -static bool isLowPowerDisarmed(void) -{ - return (!ARMING_FLAG(ARMED) && !failsafeIsActive() && - (vtxSettingsConfig()->lowPowerDisarm == VTX_LOW_POWER_DISARM_ALWAYS || - (vtxSettingsConfig()->lowPowerDisarm == VTX_LOW_POWER_DISARM_UNTIL_FIRST_ARM && !ARMING_FLAG(WAS_EVER_ARMED)))); -} - -bool isVtxConfigValid(const vtxConfig_t *cfg) -{ - LOG_DEBUG(VTX, "msp isVtxConfigValid\r\n"); - for (int i = 0; i < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; ++i) { - - if (cfg->vtxChannelActivationConditions[i].band || - (cfg->vtxChannelActivationConditions[i].range.startStep && cfg->vtxChannelActivationConditions[i].range.endStep) || - cfg->vtxChannelActivationConditions[i].auxChannelIndex || - cfg->vtxChannelActivationConditions[i].channel) { - return true; - } - } - - LOG_DEBUG(VTX, "msp Invalid Config!\r\n"); - return false; -} - - -void setMspVtxDeviceStatusReady(const int descriptor) -{ - LOG_DEBUG(VTX, "msp setMspVtxDeviceStatusReady\r\n"); - UNUSED(descriptor); - - mspVtxStatus = MSP_VTX_STATUS_READY; - mspVtxConfigChanged = true; -} - - -void prepareMspFrame(uint8_t *mspFrame) -{ - LOG_DEBUG(VTX, "msp PrepareMspFrame\r\n"); -/* -HDZERO parsing - fc_band_rx = msp_rx_buf[1]; - fc_channel_rx = msp_rx_buf[2]; - fc_pwr_rx = msp_rx_buf[3]; - fc_pit_rx = msp_rx_buf[4]; - fc_lp_rx = msp_rx_buf[8]; -*/ - - uint8_t pitmode = 0; - vtxCommonGetPitMode(&vtxMsp, &pitmode); - - mspFrame[0] = VTXDEV_MSP, - mspFrame[1] = vtxSettingsConfig()->band; - mspFrame[2] = vtxSettingsConfig()->channel; - mspFrame[3] = isLowPowerDisarmed() ? 1 : vtxSettingsConfig()->power; // index based - mspFrame[4] = pitmode; - mspFrame[5] = 0; // Freq_L - mspFrame[6] = 0; // Freq_H - mspFrame[7] = (mspVtxStatus == MSP_VTX_STATUS_READY) ? 1 : 0; - mspFrame[8] = isLowPowerDisarmed(); - mspFrame[9] = 0; // Pitmode freq Low - mspFrame[10] = 0; // pitmod freq High + // Send an MSP_VTX_V2 frame to the VTX + mspFrame[0] = vtxMspGetDeviceType(vtxDevice); + mspFrame[1] = vtxState.request.band; + mspFrame[2] = vtxState.request.channel; + mspFrame[3] = vtxState.request.powerIndex; + mspFrame[4] = vtxState.request.pitMode; + mspFrame[5] = 0; // Freq_L + mspFrame[6] = 0; // Freq_H + mspFrame[7] = vtxMspIsReady(vtxDevice); + mspFrame[8] = vtxSettingsConfig()->lowPowerDisarm; + mspFrame[9] = 0; // pitmode freq Low + mspFrame[10] = 0; // pitmode freq High mspFrame[11] = 0; // 1 if using vtx table - mspFrame[12] = 0; // vtx table bands or 0 - mspFrame[13] = 0; // vtx table channels or 0 - mspFrame[14] = 0; // vtx table power levels or 0 -} + mspFrame[12] = vtxMsp.capability.bandCount; // bands or 0 + mspFrame[13] = vtxMsp.capability.channelCount; // channels or 0 + mspFrame[14] = vtxMsp.capability.powerCount; // power levels or 0 +} static void mspCrsfPush(const uint8_t mspCommand, const uint8_t *mspFrame, const uint8_t mspFrameSize) { - - LOG_DEBUG(VTX, "msp CrsfPush\r\n"); #ifndef USE_TELEMETRY_CRSF UNUSED(mspCommand); UNUSED(mspFrame); @@ -201,285 +146,144 @@ static void mspCrsfPush(const uint8_t mspCommand, const uint8_t *mspFrame, const crc8_dvb_s2_sbuf_append(dst, &crsfFrame[2]); // start at byte 2, since CRC does not include device address and frame length sbufSwitchToReader(dst, crsfFrame); - crsfRxSendTelemetryData(); //give the FC a chance to send outstanding telemetry + crsfRxSendTelemetryData(); // give the FC a chance to send outstanding telemetry crsfRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst)); crsfRxSendTelemetryData(); #endif } -static uint16_t packetCounter = 0; - -static bool isVtxConfigChanged(void) -{ - if(mspVtxStatus == MSP_VTX_STATUS_READY) { - if (mspVtxConfigChanged == true) - return true; - - if (isLowPowerDisarmed() != prevLowPowerDisarmedState) { - LOG_DEBUG(VTX, "msp vtx config changed (lower power disarm 2)\r\n"); - mspVtxConfigChanged = true; - prevLowPowerDisarmedState = isLowPowerDisarmed(); - } - - if (mspConfPowerIndex != vtxSettingsConfig()->power) { - LOG_DEBUG(VTX, "msp vtx config changed (power 2)\r\n"); - mspVtxConfigChanged = true; - } - - if (mspConfBand != vtxSettingsConfig()->band || mspConfChannel != vtxSettingsConfig()->channel) { - LOG_DEBUG(VTX, "msp vtx config changed (band and channel 2)\r\n"); - mspVtxConfigChanged = true; - } - - return mspVtxConfigChanged; - } - - return false; -} - static void vtxMspProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs) { UNUSED(vtxDevice); - const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_MSP_OSD); - uint8_t frame[15]; + uint8_t mspFrame[15]; - switch (mspVtxStatus) { - case MSP_VTX_STATUS_OFFLINE: - LOG_DEBUG(VTX, "msp MspProcess: OFFLINE\r\n"); - // wait for MSP communication from the VTX -#ifdef USE_CMS - //mspCmsUpdateStatusString(); -#endif - break; - case MSP_VTX_STATUS_READY: - LOG_DEBUG(VTX, "msp MspProcess: READY\r\n"); - // send an update if stuff has changed with 200ms period - if ((isVtxConfigChanged()) && cmp32(currentTimeUs, mspVtxLastTimeUs) >= MSP_VTX_REQUEST_PERIOD_US) { - - LOG_DEBUG(VTX, "msp-vtx: vtxInfo Changed\r\n"); - prepareMspFrame(frame); - - if (isCrsfPortConfig(portConfig)) { - mspCrsfPush(MSP_VTX_CONFIG, frame, sizeof(frame)); - } else { - mspPort_t *port = getMspOsdPort(); - if(port != NULL && port->port) { - LOG_DEBUG(VTX, "msp-vtx: mspSerialPushPort\r\n"); - int sent = mspSerialPushPort(MSP_VTX_CONFIG, frame, sizeof(frame), port, MSP_V2_NATIVE); - if (sent <= 0) { - break; - } - } + mspPort_t *mspPort = getMspOsdPort(); + unsigned lastActivity = (currentTimeUs/1000) - mspPort->lastActivityMs; + if (lastActivity > VTX_MSP_TIMEOUT) { + if (vtxState.timeouts++ > 3) { + if (vtxState.ready) { + vtxState.ready = false; } - packetCounter++; - mspVtxLastTimeUs = currentTimeUs; - mspVtxConfigChanged = false; - -#ifdef USE_CMS - //mspCmsUpdateStatusString(); -#endif } - break; - default: - mspVtxStatus = MSP_VTX_STATUS_OFFLINE; - break; + } else { // active + if (!vtxState.ready) { + vtxState.ready = true; + } } -#if 0 - DEBUG_SET(DEBUG_VTX_MSP, 0, packetCounter); - DEBUG_SET(DEBUG_VTX_MSP, 1, isCrsfPortConfig(portConfig)); - DEBUG_SET(DEBUG_VTX_MSP, 2, isLowPowerDisarmed()); -#if defined(USE_MSP_OVER_TELEMETRY) - DEBUG_SET(DEBUG_VTX_MSP, 3, isCrsfPortConfig(portConfig) ? getMspTelemetryDescriptor() : getMspSerialPortDescriptor(mspVtxPortIdentifier)); -#else - DEBUG_SET(DEBUG_VTX_MSP, 3, getMspSerialPortDescriptor(mspVtxPortIdentifier)); -#endif -#endif + if (vtxState.ready) { + if (vtxState.updateReqMask != VTX_UPDATE_REQ_NONE) { + prepareMspFrame(vtxDevice, mspFrame); + if (vtxState.crsfTelemetryEnabled) { // keep ELRS LUA up to date ? + mspCrsfPush(MSP_VTX_CONFIG, mspFrame, sizeof(mspFrame)); + } + + int sent = mspSerialPushPort(MSP_VTX_CONFIG, mspFrame, sizeof(mspFrame), mspPort, MSP_V2_NATIVE); + if (sent > 0) { + vtxState.updateReqMask = VTX_UPDATE_REQ_NONE; + } + } + } } static vtxDevType_e vtxMspGetDeviceType(const vtxDevice_t *vtxDevice) { - //LOG_DEBUG(VTX, "msp GetDeviceType\r\n"); UNUSED(vtxDevice); return VTXDEV_MSP; } static bool vtxMspIsReady(const vtxDevice_t *vtxDevice) { - //LOG_DEBUG(VTX, "msp vtxIsReady: %s\r\n", (vtxDevice != NULL && mspVtxStatus == MSP_VTX_STATUS_READY) ? "Y": "N"); - return vtxDevice != NULL && mspVtxStatus == MSP_VTX_STATUS_READY; + return vtxDevice != NULL && mspVtxPortIdentifier >=0 && vtxState.ready; } static void vtxMspSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel) { - LOG_DEBUG(VTX, "msp SetBandAndChannel\r\n"); UNUSED(vtxDevice); - if (band != mspConfBand || channel != mspConfChannel) { - LOG_DEBUG(VTX, "msp vtx config changed (band and channel)\r\n"); - mspVtxConfigChanged = true; + + if (band < VTX_MSP_MIN_BAND || band > VTX_MSP_MAX_BAND || channel < VTX_MSP_MIN_CHANNEL || channel > VTX_MSP_MAX_CHANNEL) { + return; } - mspConfBand = band; - mspConfChannel = channel; + + vtxState.request.band = band; + vtxState.request.channel = channel; + vtxState.request.freq = vtx58_Bandchan2Freq(band, channel); + vtxState.updateReqMask |= VTX_UPDATE_REQ_FREQUENCY; } static void vtxMspSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index) { - LOG_DEBUG(VTX, "msp SetPowerByIndex\r\n"); UNUSED(vtxDevice); - if (index > 0 && (index < VTX_MSP_TABLE_MAX_POWER_LEVELS)) - { - if (index != mspConfPowerIndex) - { - LOG_DEBUG(VTX, "msp vtx config changed (power by index)\r\n"); - mspVtxConfigChanged = true; - } - mspConfPowerIndex = index; - } + vtxState.request.power = vtxMspPowerTable[index]; + vtxState.request.powerIndex = index; + vtxState.updateReqMask |= VTX_UPDATE_REQ_POWER; } -static void vtxMspSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff) -{ - LOG_DEBUG(VTX, "msp SetPitMode\r\n"); - UNUSED(vtxDevice); - if (onoff != mspConfPitMode) { - LOG_DEBUG(VTX, "msp vtx config changed (pitmode)\r\n"); - mspVtxConfigChanged = true; - } - mspConfPitMode = onoff; -} - -#if 0 -static void vtxMspSetFreq(vtxDevice_t *vtxDevice, uint16_t freq) +static void vtxMspSetPitMode(vtxDevice_t *vtxDevice, uint8_t onOff) { UNUSED(vtxDevice); - if (freq != mspConfFreq) { - mspVtxConfigChanged = true; - } - mspConfFreq = freq; + + vtxState.request.pitMode = onOff; + vtxState.updateReqMask |= VTX_UPDATE_REQ_PIT_MODE; } -#endif - - - static bool vtxMspGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel) { - if (!vtxMspIsReady(vtxDevice)) { - return false; - } - - *pBand = vtxSettingsConfig()->band; - *pChannel = vtxSettingsConfig()->channel; - - //LOG_DEBUG(VTX, "msp GetBandAndChannel: %02x:%02x\r\n", vtxSettingsConfig()->band, vtxSettingsConfig()->channel); + UNUSED(vtxDevice); + *pBand = vtxState.request.band; + *pChannel = vtxState.request.channel; return true; } static bool vtxMspGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex) { - if (!vtxMspIsReady(vtxDevice)) { - return false; - } + UNUSED(vtxDevice); - uint8_t power = isLowPowerDisarmed() ? 1 : vtxSettingsConfig()->power; - // Special case, power not set - if (power > VTX_MSP_TABLE_MAX_POWER_LEVELS) { - *pIndex = 0; - //LOG_DEBUG(VTX, "msp GetPowerIndex: %u\r\n", *pIndex); - return true; - } + *pIndex = vtxState.request.powerIndex; + return true; +} - *pIndex = power; +static bool vtxMspGetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff) +{ + UNUSED(vtxDevice); - //LOG_DEBUG(VTX, "msp GetPowerIndex: %u\r\n", *pIndex); + *pOnOff = vtxState.request.pitMode; return true; } static bool vtxMspGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq) { - LOG_DEBUG(VTX, "msp GetFreq\r\n"); - if (!vtxMspIsReady(vtxDevice)) { - return false; - } + UNUSED(vtxDevice); - *pFreq = 5800; + *pFreq = vtxState.request.freq; return true; } static bool vtxMspGetPower(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16_t *pPowerMw) { - LOG_DEBUG(VTX, "msp GetPower\r\n"); - uint8_t powerIndex; + UNUSED(vtxDevice); - if (!vtxMspGetPowerIndex(vtxDevice, &powerIndex)) { - return false; - } - - - *pIndex = powerIndex; - *pPowerMw = *pIndex; + *pIndex = vtxState.request.powerIndex; + *pPowerMw = vtxState.request.power; return true; } -static bool vtxMspGetOsdInfo(const vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t * pOsdInfo) +static bool vtxMspGetOsdInfo(const vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t *pOsdInfo) { - LOG_DEBUG(VTX, "msp GetOsdInfo\r\n"); - uint8_t powerIndex; - uint16_t powerMw; - uint16_t freq; - uint8_t band, channel; + UNUSED(vtxDevice); - if (!vtxMspGetBandAndChannel(vtxDevice, &band, &channel)) { - return false; - } - - if (!vtxMspGetFreq(vtxDevice, &freq)) { - return false; - } - - if (!vtxMspGetPower(vtxDevice, &powerIndex, &powerMw)) { - return false; - } - - pOsdInfo->band = band; - pOsdInfo->channel = channel; - pOsdInfo->frequency = freq; - pOsdInfo->powerIndex = powerIndex; - pOsdInfo->powerMilliwatt = powerMw; - pOsdInfo->bandLetter = vtx58BandNames[band][0]; - pOsdInfo->bandName = vtx58BandNames[band]; - pOsdInfo->channelName = vtx58ChannelNames[channel]; - pOsdInfo->powerIndexLetter = '0' + powerIndex; - return true; -} - - -bool vtxMspInit(void) -{ - LOG_DEBUG(VTX, "msp %s\r\n", __FUNCTION__); - // don't bother setting up this device if we don't have MSP vtx enabled - // Port is shared with MSP_OSD - const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_MSP_OSD); - if (!portConfig) { - return false; - } - - mspVtxPortIdentifier = portConfig->identifier; - - // XXX Effect of USE_VTX_COMMON should be reviewed, as following call to vtxInit will do nothing if vtxCommonSetDevice is not called. -#if defined(USE_VTX_COMMON) - vtxCommonSetDevice(&vtxMsp); -#endif - - mspConfBand = vtxSettingsConfig()->band; - mspConfChannel = vtxSettingsConfig()->channel; - mspConfPowerIndex = isLowPowerDisarmed() ? 1 : vtxSettingsConfig()->power; // index based - vtxCommonGetPitMode(&vtxMsp, &mspConfPitMode); - - vtxInit(); + pOsdInfo->band = vtxState.request.band; + pOsdInfo->channel = vtxState.request.channel; + pOsdInfo->frequency = vtxState.request.freq; + pOsdInfo->powerIndex = vtxState.request.powerIndex; + pOsdInfo->powerMilliwatt = vtxState.request.power; + pOsdInfo->bandName = vtxMspBandNames[vtxState.request.band]; + pOsdInfo->bandLetter = vtxMspBandLetters[vtxState.request.band]; + pOsdInfo->channelName = vtxMspChannelNames[vtxState.request.channel]; + pOsdInfo->powerIndexLetter = '0' + vtxState.request.powerIndex; return true; } @@ -491,211 +295,48 @@ static const vtxVTable_t mspVTable = { .setBandAndChannel = vtxMspSetBandAndChannel, .setPowerByIndex = vtxMspSetPowerByIndex, .setPitMode = vtxMspSetPitMode, - //.setFrequency = vtxMspSetFreq, .getBandAndChannel = vtxMspGetBandAndChannel, .getPowerIndex = vtxMspGetPowerIndex, + .getPitMode = vtxMspGetPitMode, .getFrequency = vtxMspGetFreq, - //.getStatus = vtxMspGetStatus, .getPower = vtxMspGetPower, - //.serializeCustomDeviceStatus = NULL, - .getOsdInfo = vtxMspGetOsdInfo, + .getOsdInfo = vtxMspGetOsdInfo }; -static mspResult_e mspVtxProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn) +static vtxDevice_t vtxMsp = { + .vTable = &mspVTable, + .capability.bandCount = VTX_MSP_MAX_BAND, + .capability.channelCount = VTX_MSP_MAX_CHANNEL, + .capability.powerCount = VTX_MSP_POWER_COUNT, + .capability.bandNames = (char **)vtxMspBandNames, + .capability.channelNames = (char **)vtxMspChannelNames, + .capability.powerNames = (char **)vtxMspPowerNames +}; + +bool vtxMspInit(void) { - //LOG_DEBUG(VTX, "msp VTX_MSP_PROCESS\r\n"); - UNUSED(mspPostProcessFn); - - sbuf_t *dst = &reply->buf; - sbuf_t *src = &cmd->buf; - - const unsigned int dataSize = sbufBytesRemaining(src); - UNUSED(dst); - UNUSED(src); - - // Start initializing the reply message - reply->cmd = cmd->cmd; - reply->result = MSP_RESULT_ACK; - - vtxDevice_t *vtxDevice = vtxCommonDevice(); - if (!vtxDevice || vtxCommonGetDeviceType(vtxDevice) != VTXDEV_MSP) { - LOG_DEBUG(VTX, "msp wrong vtx\r\n"); - return MSP_RESULT_ERROR; + const serialPortConfig_t *portConfig; + + // Shares MSP_OSD port + portConfig = findSerialPortConfig(FUNCTION_VTX_MSP); + if (!portConfig) { + return false; } - switch (cmd->cmd) - { - case MSP_VTXTABLE_BAND: - { - LOG_DEBUG(VTX, "msp MSP_VTXTABLE_BAND\r\n"); - uint8_t deviceType = vtxCommonGetDeviceType(vtxDevice); - if (deviceType == VTXDEV_MSP) - { - /* - char bandName[MSP_VTX_TABLE_BAND_NAME_LENGTH + 1]; - memset(bandName, 0, MSP_VTX_TABLE_BAND_NAME_LENGTH + 1); - uint16_t frequencies[MSP_VTX_TABLE_MAX_CHANNELS]; - const uint8_t band = sbufReadU8(src); - const uint8_t bandNameLength = sbufReadU8(src); - for (int i = 0; i < bandNameLength; i++) { - const char nameChar = sbufReadU8(src); - if (i < MSP_VTX_TABLE_BAND_NAME_LENGTH) { - bandName[i] = toupper(nameChar); - } - } - const char bandLetter = toupper(sbufReadU8(src)); - const bool isFactoryBand = (bool)sbufReadU8(src); - const uint8_t channelCount = sbufReadU8(src); - for (int i = 0; i < channelCount; i++) - { - const uint16_t frequency = sbufReadU16(src); - if (i < vtxTableConfig()->channels) - { - frequencies[i] = frequency; - } - } - */ + portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); - setMspVtxDeviceStatusReady(1); - } - break; - } - case MSP_VTXTABLE_POWERLEVEL: - { - LOG_DEBUG(VTX, "msp MSP_VTXTABLE_POWERLEVEL\r\n"); - - /* - char powerLevelLabel[VTX_TABLE_POWER_LABEL_LENGTH + 1]; - memset(powerLevelLabel, 0, VTX_TABLE_POWER_LABEL_LENGTH + 1); - const uint8_t powerLevel = sbufReadU8(src); - const uint16_t powerValue = sbufReadU16(src); - const uint8_t powerLevelLabelLength = sbufReadU8(src); - for (int i = 0; i < powerLevelLabelLength; i++) - { - const char labelChar = sbufReadU8(src); - if (i < VTX_TABLE_POWER_LABEL_LENGTH) - { - powerLevelLabel[i] = toupper(labelChar); - } - } - */ - setMspVtxDeviceStatusReady(1); - } - break; - case MSP_VTX_CONFIG: - { - LOG_DEBUG(VTX, "msp MSP_VTX_CONFIG received\r\n"); - LOG_DEBUG(VTX, "msp MSP_VTX_CONFIG VTXDEV_MSP\r\n"); - uint8_t pitmode = 0; - vtxCommonGetPitMode(vtxDevice, &pitmode); + vtxState.ready = false; + vtxState.timeouts = 0; + vtxState.updateReqMask = VTX_UPDATE_REQ_NONE; + vtxState.crsfTelemetryEnabled = crsfRxIsActive(); + vtxState.request.band = vtxSettingsConfig()->band; + vtxState.request.channel = vtxSettingsConfig()->channel; + vtxState.request.freq = vtx58_Bandchan2Freq(vtxState.request.band, vtxState.request.channel); + vtxState.request.power = vtxSettingsConfig()->power; + vtxState.request.pitMode = 0; + vtxCommonSetDevice(&vtxMsp); - // VTXDEV_MSP, - sbufWriteU8(dst, VTXDEV_MSP); - // band; - sbufWriteU8(dst, vtxSettingsConfig()->band); - // channel; - sbufWriteU8(dst, vtxSettingsConfig()->channel); - // power; // index based - sbufWriteU8(dst, vtxSettingsConfig()->power); - // pit mode; - // Freq_L - sbufWriteU8(dst, 0); - // Freq_H - sbufWriteU8(dst, 0); - // vtx status - sbufWriteU8(dst, 1); - // lowPowerDisarm - - sbufWriteU8(dst, vtxSettingsConfig()->lowPowerDisarm); - // Pitmode freq Low - sbufWriteU8(dst, 0); - // pitmod freq High - sbufWriteU8(dst, 0); - // 1 if using vtx table - sbufWriteU8(dst, 0); - // vtx table bands or 0 - sbufWriteU8(dst, 0); - // vtx table channels or 0 - sbufWriteU8(dst, 0); - - setMspVtxDeviceStatusReady(1); - break; - } - case MSP_SET_VTX_CONFIG: - LOG_DEBUG(VTX, "msp MSP_SET_VTX_CONFIG\r\n"); - if (dataSize == 15) - { - if (vtxCommonGetDeviceType(vtxDevice) != VTXDEV_UNKNOWN) - { - for (int i = 0; i < 15; ++i) - { - uint8_t data = sbufReadU8(src); - switch (i) - { - case 1: - vtxSettingsConfigMutable()->band = data; - break; - case 2: - vtxSettingsConfigMutable()->channel = data; - break; - case 3: - vtxSettingsConfigMutable()->power = data; - break; - case 4: - vtxCommonSetPitMode(vtxDevice, data); - break; - case 7: - // vtx ready - break; - case 8: - vtxSettingsConfigMutable()->lowPowerDisarm = data; - break; - } - } - } - - setMspVtxDeviceStatusReady(1); - break; - } - LOG_DEBUG(VTX, "msp MSP_RESULT_ERROR\r\n"); - return MSP_RESULT_ERROR; - default: - // debug[1]++; - // debug[2] = cmd->cmd; - if(cmd->cmd != MSP_STATUS && cmd->cmd != MSP_STATUS_EX && cmd->cmd != MSP_RC) { - LOG_DEBUG(VTX, "msp default: %02x\r\n", cmd->cmd); - } - reply->result = MSP_RESULT_ERROR; - break; - } - - // Process DONT_REPLY flag - if (cmd->flags & MSP_FLAG_DONT_REPLY) - { - reply->result = MSP_RESULT_NO_REPLY; - } - - return reply->result; + return true; } -void mspVtxSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn) -{ - UNUSED(mspProcessCommandFn); - // Check if VTX is ready - /* - if (mspVtxStatus != MSP_VTX_STATUS_READY) { - LOG_DEBUG(VTX, "msp VTX NOT READY, skipping\r\n"); - return; - } - */ - - mspPort_t *port = getMspOsdPort(); - - if(port) { - mspSerialProcessOnePort(port, MSP_SKIP_NON_MSP_DATA, mspVtxProcessMspCommand); - } - -} - - #endif \ No newline at end of file diff --git a/src/main/io/vtx_msp.h b/src/main/io/vtx_msp.h index 30ca245fed..ceb2d5bea1 100644 --- a/src/main/io/vtx_msp.h +++ b/src/main/io/vtx_msp.h @@ -17,37 +17,17 @@ * * If not, see . */ +/* Created by geoffsim */ -#pragma once +#ifndef _VTX_MSP_H +#define _VTX_MSP_H -#include - -#include "build/build_config.h" - -#include "msp/msp_protocol.h" -#include "msp/msp_serial.h" - -typedef enum { - // Offline - device hasn't responded yet - MSP_VTX_STATUS_OFFLINE = 0, - MSP_VTX_STATUS_READY, -} mspVtxStatus_e; - -typedef struct mspPowerTable_s { - int mW; // rfpower - int16_t dbi; // valueV1 -} mspPowerTable_t; - -#define VTX_MSP_TABLE_MAX_BANDS 5 // default freq table has 5 bands -#define VTX_MSP_TABLE_MAX_CHANNELS 8 // and eight channels -#define VTX_MSP_TABLE_MAX_POWER_LEVELS 5 //max of VTX_TRAMP_POWER_COUNT, VTX_SMARTAUDIO_POWER_COUNT and VTX_RTC6705_POWER_COUNT -#define VTX_MSP_TABLE_CHANNEL_NAME_LENGTH 1 -#define VTX_MSP_TABLE_BAND_NAME_LENGTH 8 -#define VTX_MSP_TABLE_POWER_LABEL_LENGTH 3 +#define VTX_MSP_TIMEOUT 250 // ms +#define VTX_MSP_BAND_COUNT 5 +#define VTX_MSP_CHANNEL_COUNT 8 +#define VTX_MSP_POWER_COUNT 4 bool vtxMspInit(void); -void setMspVtxDeviceStatusReady(const int descriptor); -void prepareMspFrame(uint8_t *mspFrame); -void mspVtxSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn); +#endif // _VTX_MSP_H diff --git a/src/main/msc/usbd_storage_sd_spi.c b/src/main/msc/usbd_storage_sd_spi.c index 03f53eed72..4573d156bd 100644 --- a/src/main/msc/usbd_storage_sd_spi.c +++ b/src/main/msc/usbd_storage_sd_spi.c @@ -213,8 +213,9 @@ static int8_t STORAGE_Read (uint8_t lun, UNUSED(lun); LED1_ON; for (int i = 0; i < blk_len; i++) { - while (sdcard_readBlock(blk_addr + i, buf + (512 * i), NULL, 0) == 0); - while (sdcard_poll() == 0); + while (sdcard_readBlock(blk_addr + i, buf + (512 * i), NULL, 0) == 0) + sdcard_poll(); + while (sdcard_poll() == 0); } LED1_OFF; return 0; diff --git a/src/main/msc/usbd_storage_sdio.c b/src/main/msc/usbd_storage_sdio.c deleted file mode 100644 index 4a262419ab..0000000000 --- a/src/main/msc/usbd_storage_sdio.c +++ /dev/null @@ -1,289 +0,0 @@ -/** - ****************************************************************************** - * @file usbd_storage_template.c - * @author MCD Application Team - * @version V1.2.0 - * @date 09-November-2015 - * @brief Memory management layer - ****************************************************************************** - * @attention - * - *

© COPYRIGHT 2015 STMicroelectronics

- * - * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); - * You may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.st.com/software_license_agreement_liberty_v2 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ****************************************************************************** - */ - - -/* Includes ------------------------------------------------------------------*/ -#include -#include - -#include "platform.h" - -#include "drivers/sdmmc_sdio.h" -#include "drivers/light_led.h" -#include "drivers/io.h" -#include "common/utils.h" - -#ifdef USE_HAL_DRIVER -#include "usbd_msc.h" -#else -#include "usbd_msc_mem.h" -#include "usbd_msc_core.h" -#endif - -#include "usbd_storage.h" - -/* Private typedef -----------------------------------------------------------*/ -/* Private define ------------------------------------------------------------*/ -/* Private macro -------------------------------------------------------------*/ -/* Private variables ---------------------------------------------------------*/ -/* Private function prototypes -----------------------------------------------*/ -/* Extern function prototypes ------------------------------------------------*/ -/* Private functions ---------------------------------------------------------*/ - -/* USB NVIC Priority has to be lower than both DMA and SDIO priority, - * otherwise SDIO won't be able to preempt USB. - */ - -#define STORAGE_LUN_NBR 1 -#define STORAGE_BLK_NBR 0x10000 -#define STORAGE_BLK_SIZ 0x200 - -static int8_t STORAGE_Init (uint8_t lun); - -#ifdef USE_HAL_DRIVER -static int8_t STORAGE_GetCapacity (uint8_t lun, - uint32_t *block_num, - uint16_t *block_size); -#else -static int8_t STORAGE_GetCapacity (uint8_t lun, - uint32_t *block_num, - uint32_t *block_size); -#endif - -static int8_t STORAGE_IsReady (uint8_t lun); - -static int8_t STORAGE_IsWriteProtected (uint8_t lun); - -static int8_t STORAGE_Read (uint8_t lun, - uint8_t *buf, - uint32_t blk_addr, - uint16_t blk_len); - -static int8_t STORAGE_Write (uint8_t lun, - uint8_t *buf, - uint32_t blk_addr, - uint16_t blk_len); - -static int8_t STORAGE_GetMaxLun (void); - -/* USB Mass storage Standard Inquiry Data */ -static uint8_t STORAGE_Inquirydata[] = {//36 - - /* LUN 0 */ - 0x00, - 0x80, - 0x02, - 0x02, -#ifdef USE_HAL_DRIVER - (STANDARD_INQUIRY_DATA_LEN - 5), -#else - (USBD_STD_INQUIRY_LENGTH - 5), -#endif - 0x00, - 0x00, - 0x00, - 'S', 'T', 'M', ' ', ' ', ' ', ' ', ' ', /* Manufacturer : 8 bytes */ - 'P', 'r', 'o', 'd', 'u', 't', ' ', ' ', /* Product : 16 Bytes */ - ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', - '0', '.', '0' ,'1', /* Version : 4 Bytes */ -}; - -#ifdef USE_HAL_DRIVER -USBD_StorageTypeDef USBD_MSC_MICRO_SDIO_fops = -{ - STORAGE_Init, - STORAGE_GetCapacity, - STORAGE_IsReady, - STORAGE_IsWriteProtected, - STORAGE_Read, - STORAGE_Write, - STORAGE_GetMaxLun, - (int8_t*)STORAGE_Inquirydata, -}; -#else -USBD_STORAGE_cb_TypeDef USBD_MSC_MICRO_SDIO_fops = -{ - STORAGE_Init, - STORAGE_GetCapacity, - STORAGE_IsReady, - STORAGE_IsWriteProtected, - STORAGE_Read, - STORAGE_Write, - STORAGE_GetMaxLun, - (int8_t*)STORAGE_Inquirydata, -}; -#endif - -/******************************************************************************* -* Function Name : Read_Memory -* Description : Handle the Read operation from the microSD card. -* Input : None. -* Output : None. -* Return : None. -*******************************************************************************/ -static int8_t STORAGE_Init (uint8_t lun) -{ - //Initialize SD_DET -#ifdef SDCARD_DETECT_PIN - const IO_t sd_det = IOGetByTag(IO_TAG(SDCARD_DETECT_PIN)); - IOInit(sd_det, OWNER_SDCARD_DETECT, 0); - IOConfigGPIO(sd_det, IOCFG_IPU); -#endif - - UNUSED(lun); - LED0_OFF; -#if defined(STM32H7) // H7 uses IDMA - SD_Initialize_LL(0); -#else - SD_Initialize_LL(SDIO_DMA); -#endif - if (SD_Init() != 0) return 1; - LED0_ON; - return 0; -} - -/******************************************************************************* -* Function Name : Read_Memory -* Description : Handle the Read operation from the STORAGE card. -* Input : None. -* Output : None. -* Return : None. -*******************************************************************************/ -#ifdef USE_HAL_DRIVER -static int8_t STORAGE_GetCapacity (uint8_t lun, uint32_t *block_num, uint16_t *block_size) -#else -static int8_t STORAGE_GetCapacity (uint8_t lun, uint32_t *block_num, uint32_t *block_size) -#endif -{ - UNUSED(lun); - if (SD_IsDetected() == 0) { - return -1; - } - SD_GetCardInfo(); - - *block_num = SD_CardInfo.CardCapacity; - *block_size = 512; - return (0); -} - -/******************************************************************************* -* Function Name : Read_Memory -* Description : Handle the Read operation from the STORAGE card. -* Input : None. -* Output : None. -* Return : None. -*******************************************************************************/ -static int8_t STORAGE_IsReady (uint8_t lun) -{ - UNUSED(lun); - int8_t ret = -1; - if (SD_GetState() == true && SD_IsDetected() == SD_PRESENT) { - ret = 0; - } - return ret; -} - -/******************************************************************************* -* Function Name : Read_Memory -* Description : Handle the Read operation from the STORAGE card. -* Input : None. -* Output : None. -* Return : None. -*******************************************************************************/ -static int8_t STORAGE_IsWriteProtected (uint8_t lun) -{ - UNUSED(lun); - return 0; -} - -/******************************************************************************* -* Function Name : Read_Memory -* Description : Handle the Read operation from the STORAGE card. -* Input : None. -* Output : None. -* Return : None. -*******************************************************************************/ -static int8_t STORAGE_Read (uint8_t lun, - uint8_t *buf, - uint32_t blk_addr, - uint16_t blk_len) -{ - UNUSED(lun); - if (SD_IsDetected() == 0) { - return -1; - } - LED1_ON; - //buf should be 32bit aligned, but usually is so we don't do byte alignment - if (SD_ReadBlocks_DMA(blk_addr, (uint32_t*) buf, 512, blk_len) == 0) { - while (SD_CheckRead()); - while(SD_GetState() == false); - LED1_OFF; - return 0; - } - LED1_OFF; - return -1; -} -/******************************************************************************* -* Function Name : Write_Memory -* Description : Handle the Write operation to the STORAGE card. -* Input : None. -* Output : None. -* Return : None. -*******************************************************************************/ -static int8_t STORAGE_Write (uint8_t lun, - uint8_t *buf, - uint32_t blk_addr, - uint16_t blk_len) -{ - UNUSED(lun); - if (SD_IsDetected() == 0) { - return -1; - } - LED1_ON; - //buf should be 32bit aligned, but usually is so we don't do byte alignment - if (SD_WriteBlocks_DMA(blk_addr, (uint32_t*) buf, 512, blk_len) == 0) { - while (SD_CheckWrite()); - while(SD_GetState() == false); - LED1_OFF; - return 0; - } - LED1_OFF; - return -1; -} -/******************************************************************************* -* Function Name : Write_Memory -* Description : Handle the Write operation to the STORAGE card. -* Input : None. -* Output : None. -* Return : None. -*******************************************************************************/ -static int8_t STORAGE_GetMaxLun (void) -{ - return (STORAGE_LUN_NBR - 1); -} - -/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/src/main/msp/msp.h b/src/main/msp/msp.h index 0c00b81ece..e988ea2edd 100644 --- a/src/main/msp/msp.h +++ b/src/main/msp/msp.h @@ -46,6 +46,7 @@ typedef struct mspPacket_s { typedef enum { MSP_FLAG_DONT_REPLY = (1 << 0), + MSP_FLAG_ILMI = (1 << 1), // "In-Line Message identifier" } mspFlags_e; struct serialPort_s; diff --git a/src/main/msp/msp_protocol_v2_common.h b/src/main/msp/msp_protocol_v2_common.h index e778a1808c..8784b25311 100644 --- a/src/main/msp/msp_protocol_v2_common.h +++ b/src/main/msp/msp_protocol_v2_common.h @@ -15,22 +15,25 @@ * along with INAV. If not, see . */ -#define MSP2_COMMON_TZ 0x1001 //out message Gets the TZ offset for the local time (returns: minutes(i16)) -#define MSP2_COMMON_SET_TZ 0x1002 //in message Sets the TZ offset for the local time (args: minutes(i16)) -#define MSP2_COMMON_SETTING 0x1003 //in/out message Returns the value for a setting -#define MSP2_COMMON_SET_SETTING 0x1004 //in message Sets the value for a setting +#define MSP2_COMMON_TZ 0x1001 //out message Gets the TZ offset for the local time (returns: minutes(i16)) +#define MSP2_COMMON_SET_TZ 0x1002 //in message Sets the TZ offset for the local time (args: minutes(i16)) +#define MSP2_COMMON_SETTING 0x1003 //in/out message Returns the value for a setting +#define MSP2_COMMON_SET_SETTING 0x1004 //in message Sets the value for a setting + +#define MSP2_COMMON_MOTOR_MIXER 0x1005 +#define MSP2_COMMON_SET_MOTOR_MIXER 0x1006 + +#define MSP2_COMMON_SETTING_INFO 0x1007 //in/out message Returns info about a setting (PG, type, flags, min/max, etc..). +#define MSP2_COMMON_PG_LIST 0x1008 //in/out message Returns a list of the PG ids used by the settings + +#define MSP2_COMMON_SERIAL_CONFIG 0x1009 +#define MSP2_COMMON_SET_SERIAL_CONFIG 0x100A + +// radar commands +#define MSP2_COMMON_SET_RADAR_POS 0x100B //SET radar position information +#define MSP2_COMMON_SET_RADAR_ITD 0x100C //SET radar information to display -#define MSP2_COMMON_MOTOR_MIXER 0x1005 -#define MSP2_COMMON_SET_MOTOR_MIXER 0x1006 +#define MSP2_COMMON_SET_MSP_RC_LINK_STATS 0x100D //in message Sets the MSP RC stats +#define MSP2_COMMON_SET_MSP_RC_INFO 0x100E //in message Sets the MSP RC info -#define MSP2_COMMON_SETTING_INFO 0x1007 //in/out message Returns info about a setting (PG, type, flags, min/max, etc..). -#define MSP2_COMMON_PG_LIST 0x1008 //in/out message Returns a list of the PG ids used by the settings - -#define MSP2_COMMON_SERIAL_CONFIG 0x1009 -#define MSP2_COMMON_SET_SERIAL_CONFIG 0x100A - -// radar commands -#define MSP2_COMMON_SET_RADAR_POS 0x100B //SET radar position information -#define MSP2_COMMON_SET_RADAR_ITD 0x100C //SET radar information to display - -#define MSP2_BETAFLIGHT_BIND 0x3000 +#define MSP2_BETAFLIGHT_BIND 0x3000 diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 02c8c979aa..dcbdeb5e71 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -89,6 +89,7 @@ #define MSP2_INAV_LOGIC_CONDITIONS_SINGLE 0x203B #define MSP2_INAV_ESC_RPM 0x2040 +#define MSP2_INAV_ESC_TELEM 0x2041 #define MSP2_INAV_LED_STRIP_CONFIG_EX 0x2048 #define MSP2_INAV_SET_LED_STRIP_CONFIG_EX 0x2049 @@ -113,4 +114,9 @@ #define MSP2_INAV_SET_CUSTOM_OSD_ELEMENTS 0x2102 #define MSP2_INAV_SERVO_CONFIG 0x2200 -#define MSP2_INAV_SET_SERVO_CONFIG 0x2201 \ No newline at end of file +#define MSP2_INAV_SET_SERVO_CONFIG 0x2201 + +#define MSP2_INAV_GEOZONE 0x2210 +#define MSP2_INAV_SET_GEOZONE 0x2211 +#define MSP2_INAV_GEOZONE_VERTEX 0x2212 +#define MSP2_INAV_SET_GEOZONE_VERTEX 0x2213 \ No newline at end of file diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 4ae4cf228d..4ad0868abb 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -356,6 +356,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FLARE(naviga static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FINISHED(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_ABORT(navigationFSMState_t previousState); #endif +#ifdef USE_GEOZONE +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_INITALIZE(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_IN_PROGRESS(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_FINISHED(navigationFSMState_t previousState); +#endif static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { /** Idle state ******************************************************/ @@ -377,6 +382,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_SEND_TO] = NAV_STATE_SEND_TO_INITALIZE, } }, @@ -449,6 +455,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_SEND_TO] = NAV_STATE_SEND_TO_INITALIZE, } }, /** CRUISE_HOLD mode ************************************************/ @@ -485,6 +492,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_SEND_TO] = NAV_STATE_SEND_TO_INITALIZE, } }, @@ -507,6 +515,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_SEND_TO] = NAV_STATE_SEND_TO_INITALIZE, } }, @@ -544,6 +553,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_SEND_TO] = NAV_STATE_SEND_TO_INITALIZE, } }, @@ -566,6 +576,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_SEND_TO] = NAV_STATE_SEND_TO_INITALIZE, } }, @@ -637,15 +648,16 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_RTH_ENROUTE, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HEAD_HOME, // re-process the state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HEAD_HOME, // re-process the state + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE] = NAV_STATE_RTH_INITIALIZE, } }, @@ -995,6 +1007,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_IDLE, [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, } }, @@ -1178,6 +1192,63 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { } }, #endif + +#ifdef USE_GEOZONE + [NAV_STATE_SEND_TO_INITALIZE] = { + .persistentId = NAV_PERSISTENT_ID_SEND_TO_INITALIZE, + .onEntry = navOnEnteringState_NAV_STATE_SEND_TO_INITALIZE, + .timeoutMs = 0, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP, + .mapToFlightModes = NAV_SEND_TO, + .mwState = MW_NAV_STATE_NONE, + .mwError = MW_NAV_ERROR_NONE, + .onEvent = { + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_SEND_TO_IN_PROGESS, + [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + } + }, + + [NAV_STATE_SEND_TO_IN_PROGESS] = { + .persistentId = NAV_PERSISTENT_ID_SEND_TO_IN_PROGRES, + .onEntry = navOnEnteringState_NAV_STATE_SEND_TO_IN_PROGRESS, + .timeoutMs = 10, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP, + .mapToFlightModes = NAV_SEND_TO, + .mwState = MW_NAV_STATE_NONE, + .mwError = MW_NAV_ERROR_NONE, + .onEvent = { + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_SEND_TO_IN_PROGESS, // re-process the state + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_SEND_TO_FINISHED, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + } + }, + [NAV_STATE_SEND_TO_FINISHED] = { + .persistentId = NAV_PERSISTENT_ID_SEND_TO_FINISHED, + .onEntry = navOnEnteringState_NAV_STATE_SEND_TO_FINISHED, + .timeoutMs = 0, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP, + .mapToFlightModes = NAV_SEND_TO, + .mwState = MW_NAV_STATE_NONE, + .mwError = MW_NAV_ERROR_NONE, + .onEvent = { + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_SEND_TO_FINISHED, // re-process the state + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + } + }, +#endif }; static navigationFSMStateFlags_t navGetStateFlags(navigationFSMState_t state) @@ -1444,6 +1515,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati // If we have valid position sensor or configured to ignore it's loss at initial stage - continue if ((posControl.flags.estPosStatus >= EST_USABLE) || navConfig()->general.flags.rth_climb_ignore_emerg) { // Prepare controllers +#ifdef USE_GEOZONE + geozoneResetRTH(); + geozoneSetupRTH(); +#endif resetPositionController(); resetAltitudeController(false); // Make sure surface tracking is not enabled - RTH uses global altitude, not AGL setupAltitudeController(); @@ -1610,22 +1685,55 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio // If we have position sensor - continue home if ((posControl.flags.estPosStatus >= EST_USABLE)) { - fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL); - - if (isWaypointReached(tmpHomePos, &posControl.activeWaypoint.bearing)) { - // Successfully reached position target - update XYZ-position - setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); - - posControl.landingDelay = 0; - - if (navConfig()->general.flags.rth_use_linear_descent && posControl.rthState.rthLinearDescentActive) - posControl.rthState.rthLinearDescentActive = false; - - return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING - } else { - setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_XY); +#ifdef USE_GEOZONE + // Check for NFZ in our way + int8_t wpCount = geozoneCheckForNFZAtCourse(true); + if (wpCount > 0) { + calculateAndSetActiveWaypointToLocalPosition(geozoneGetCurrentRthAvoidWaypoint()); return NAV_FSM_EVENT_NONE; + } else if (geozone.avoidInRTHInProgress) { + if (isWaypointReached(geozoneGetCurrentRthAvoidWaypoint(), &posControl.activeWaypoint.bearing)) { + if (geoZoneIsLastRthWaypoint()) { + // Already at Home? + fpVector3_t *tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL); + if (isWaypointReached(tmpHomePos, &posControl.activeWaypoint.bearing)) { + setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + return NAV_FSM_EVENT_SUCCESS; + } + + posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z; + return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE; + } else { + geozoneAdvanceRthAvoidWaypoint(); + calculateAndSetActiveWaypointToLocalPosition(geozoneGetCurrentRthAvoidWaypoint()); + return NAV_FSM_EVENT_NONE; + } + } + setDesiredPosition(geozoneGetCurrentRthAvoidWaypoint(), 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); + return NAV_FSM_EVENT_NONE; + } else if (wpCount < 0 && geoZoneConfig()->noWayHomeAction == NO_WAY_HOME_ACTION_EMRG_LAND) { + return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; + } else { +#endif + fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL); + + if (isWaypointReached(tmpHomePos, &posControl.activeWaypoint.bearing)) { + // Successfully reached position target - update XYZ-position + setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + + posControl.landingDelay = 0; + + if (navConfig()->general.flags.rth_use_linear_descent && posControl.rthState.rthLinearDescentActive) + posControl.rthState.rthLinearDescentActive = false; + + return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING + } else { + setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_XY); + return NAV_FSM_EVENT_NONE; + } +#ifdef USE_GEOZONE } +#endif } /* Position sensor failure timeout - land */ else if (checkForPositionSensorTimeout()) { @@ -1797,6 +1905,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigation updateClimbRateToAltitudeController(-1.1f * navConfig()->general.land_minalt_vspd, 0, ROC_TO_ALT_CONSTANT); // FIXME } +#ifdef USE_GEOZONE + geozoneResetRTH(); +#endif + // Prevent I-terms growing when already landed pidResetErrorAccumulators(); return NAV_FSM_EVENT_NONE; @@ -2452,6 +2564,64 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_ABORT(naviga } #endif +#ifdef USE_GEOZONE +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_INITALIZE(navigationFSMState_t previousState) +{ + UNUSED(previousState); + + if (checkForPositionSensorTimeout()) { + return NAV_FSM_EVENT_SWITCH_TO_IDLE; + } + + resetPositionController(); + resetAltitudeController(false); + + if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING_LEGACY)) { + setDesiredPosition(&posControl.sendTo.targetPos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST); + } else { + setDesiredPosition(&posControl.sendTo.targetPos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); + } + + posControl.sendTo.startTime = millis(); + return NAV_FSM_EVENT_SUCCESS; +} + +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_IN_PROGRESS(navigationFSMState_t previousState) +{ + UNUSED(previousState); + + // "Send to" is designed to kick in even user is making inputs, lock sticks for a short time to avoid the mode ends immediately + if (posControl.sendTo.lockSticks && millis() - posControl.sendTo.startTime > posControl.sendTo.lockStickTime) { + posControl.sendTo.lockSticks = false; + } + + if (!posControl.sendTo.lockSticks && areSticksDeflected()) { + abortSendTo(); + return NAV_FSM_EVENT_SWITCH_TO_IDLE; + } + + if (calculateDistanceToDestination(&posControl.sendTo.targetPos) <= posControl.sendTo.targetRange ) { + if (posControl.sendTo.altitudeTargetRange > 0) { + if ((getEstimatedActualPosition(Z) > posControl.sendTo.targetPos.z - posControl.sendTo.altitudeTargetRange) && (getEstimatedActualPosition(Z) < posControl.sendTo.targetPos.z + posControl.sendTo.altitudeTargetRange)) { + return NAV_FSM_EVENT_SUCCESS; + } else { + return NAV_FSM_EVENT_NONE; + } + } + return NAV_FSM_EVENT_SUCCESS; + } + return NAV_FSM_EVENT_NONE; +} + +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_FINISHED(navigationFSMState_t previousState) +{ + UNUSED(previousState); + posControl.sendTo.lockSticks = false; + posControl.flags.sendToActive = false; + return NAV_FSM_EVENT_NONE; +} +#endif + static navigationFSMState_t navSetNewFSMState(navigationFSMState_t newState) { navigationFSMState_t previousState; @@ -2539,6 +2709,11 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode) switch (mode) { case RTH_HOME_ENROUTE_INITIAL: posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthInitialAltitude; +#ifdef USE_GEOZONE + if (geozone.currentzoneMaxAltitude > 0) { + posControl.rthState.homeTmpWaypoint.z = MIN(geozone.currentzoneMaxAltitude, posControl.rthState.homeTmpWaypoint.z); + } +#endif break; case RTH_HOME_ENROUTE_PROPORTIONAL: @@ -2552,6 +2727,11 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode) posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthFinalAltitude; } } +#ifdef USE_GEOZONE + if (geozone.distanceVertToNearestZone < 0 && ABS(geozone.distanceVertToNearestZone) < geoZoneConfig()->safeAltitudeDistance) { + posControl.rthState.homeTmpWaypoint.z += geoZoneConfig()->safeAltitudeDistance; + } +#endif break; case RTH_HOME_ENROUTE_FINAL: @@ -2757,6 +2937,17 @@ const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void) return posControl.flags.isTerrainFollowEnabled ? &posControl.actualState.agl : &posControl.actualState.abs; } +/*----------------------------------------------------------- + * Calculates 2D distance between two points + *-----------------------------------------------------------*/ +float calculateDistance2(const fpVector2_t* startPos, const fpVector2_t* destinationPos) +{ + const float deltaX = destinationPos->x - startPos->x; + const float deltaY = destinationPos->y - startPos->y; + + return calc_length_pythagorean_2D(deltaX, deltaY); +} + /*----------------------------------------------------------- * Calculates distance and bearing to destination point *-----------------------------------------------------------*/ @@ -2944,6 +3135,11 @@ static void updateDesiredRTHAltitude(void) posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z; posControl.rthState.rthFinalAltitude = posControl.actualState.abs.pos.z; } +#if defined(USE_GEOZONE) + if (geozone.homeHasMaxAltitue) { + posControl.rthState.rthFinalAltitude = MIN(posControl.rthState.rthFinalAltitude, geozone.maxHomeAltitude); + } +#endif } /*----------------------------------------------------------- @@ -3154,6 +3350,9 @@ void updateHomePosition(void) setHome = true; break; } +#ifdef USE_GEOZONE + geozoneUpdateMaxHomeAltitude(); +#endif } } else { @@ -3410,7 +3609,15 @@ bool isProbablyStillFlying(void) *-----------------------------------------------------------*/ float getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros) { + const bool emergLandingIsActive = navigationIsExecutingAnEmergencyLanding(); + +#ifdef USE_GEOZONE + if (!emergLandingIsActive && geozone.nearestHorZoneHasAction && ((geozone.currentzoneMaxAltitude != 0 && navGetCurrentActualPositionAndVelocity()->pos.z >= geozone.currentzoneMaxAltitude && posControl.desiredState.climbRateDemand > 0) || + (geozone.currentzoneMinAltitude != 0 && navGetCurrentActualPositionAndVelocity()->pos.z <= geozone.currentzoneMinAltitude && posControl.desiredState.climbRateDemand < 0 ))) { + return 0.0f; + } +#endif float maxClimbRate = STATE(MULTIROTOR) ? navConfig()->mc.max_auto_climb_rate : navConfig()->fw.max_auto_climb_rate; if (posControl.flags.rocToAltMode == ROC_TO_ALT_CONSTANT) { @@ -4050,6 +4257,16 @@ static void processNavigationRCAdjustments(void) /* Process pilot's RC input. Disable all pilot's input when in FAILSAFE_MODE */ navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState); +#ifdef USE_GEOZONE + if (geozone.sticksLocked) { + posControl.flags.isAdjustingAltitude = false; + posControl.flags.isAdjustingPosition = false; + posControl.flags.isAdjustingHeading = false; + + return; + } +#endif + if (FLIGHT_MODE(FAILSAFE_MODE)) { if (STATE(MULTIROTOR) && navStateFlags & NAV_RC_POS) { resetMulticopterBrakingMode(); @@ -4096,6 +4313,10 @@ void applyWaypointNavigationAndAltitudeHold(void) // Reset RTH trackback resetRthTrackBack(); +#ifdef USE_GEOZONE + posControl.flags.sendToActive = false; +#endif + return; } @@ -4145,7 +4366,7 @@ void applyWaypointNavigationAndAltitudeHold(void) void switchNavigationFlightModes(void) { const flightModeFlags_e enabledNavFlightModes = navGetMappedFlightModes(posControl.navState); - const flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_FW_AUTOLAND | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE | NAV_COURSE_HOLD_MODE) & (~enabledNavFlightModes); + const flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_FW_AUTOLAND | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE | NAV_COURSE_HOLD_MODE | NAV_SEND_TO) & (~enabledNavFlightModes); DISABLE_FLIGHT_MODE(disabledFlightModes); ENABLE_FLIGHT_MODE(enabledNavFlightModes); } @@ -4296,6 +4517,17 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) return NAV_FSM_EVENT_SWITCH_TO_RTH; } +#ifdef USE_GEOZONE + if (posControl.flags.sendToActive) { + return NAV_FSM_EVENT_SWITCH_TO_SEND_TO; + } + + + if (posControl.flags.forcedPosholdActive) { + return NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D; + } +#endif + /* WP mission activation control: * canActivateWaypoint & waypointWasActivated are used to prevent WP mission * auto restarting after interruption by Manual or RTH modes. @@ -4728,8 +4960,8 @@ void navigationUsePIDs(void) navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 100.0f, (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 100.0f, - (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 100.0f, - 0.0f, + (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 300.0f, + (float)pidProfile()->bank_fw.pid[PID_POS_Z].FF / 100.0f, NAV_DTERM_CUT_HZ, 0.0f ); @@ -4769,6 +5001,14 @@ void navigationInit(void) #ifdef USE_MULTI_MISSION posControl.multiMissionCount = 0; #endif + +#ifdef USE_GEOZONE + posControl.flags.sendToActive = false; + posControl.sendTo.lockSticks = false; + posControl.sendTo.lockStickTime = 0; + posControl.sendTo.startTime = 0; + posControl.sendTo.targetRange = 0; +#endif /* Set initial surface invalid */ posControl.actualState.surfaceMin = -1.0f; @@ -4851,6 +5091,40 @@ rthState_e getStateOfForcedRTH(void) } } + +#ifdef USE_GEOZONE +// "Send to" is not to intended to be activated by user, only by external event +void activateSendTo(void) +{ + if (!geozone.avoidInRTHInProgress) { + abortFixedWingLaunch(); + posControl.flags.sendToActive = true; + navProcessFSMEvents(selectNavEventFromBoxModeInput()); + } +} + +void abortSendTo(void) +{ + posControl.flags.sendToActive = false; + navProcessFSMEvents(selectNavEventFromBoxModeInput()); +} + +void activateForcedPosHold(void) +{ + if (!geozone.avoidInRTHInProgress) { + abortFixedWingLaunch(); + posControl.flags.forcedPosholdActive = true; + navProcessFSMEvents(selectNavEventFromBoxModeInput()); + } +} + +void abortForcedPosHold(void) +{ + posControl.flags.forcedPosholdActive = false; + navProcessFSMEvents(selectNavEventFromBoxModeInput()); +} +#endif + /*----------------------------------------------------------- * Ability to execute Emergency Landing on external event *-----------------------------------------------------------*/ diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 1b7734c8b1..9132974e48 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -116,6 +116,123 @@ void resetFwAutolandApproach(int8_t idx); #endif +#if defined(USE_GEOZONE) + +#ifndef USE_GPS + #error "Geozone needs GPS support" +#endif + +typedef enum { + GEOZONE_MESSAGE_STATE_NONE, + GEOZONE_MESSAGE_STATE_NFZ, + GEOZONE_MESSAGE_STATE_LEAVING_FZ, + GEOZONE_MESSAGE_STATE_OUTSIDE_FZ, + GEOZONE_MESSAGE_STATE_ENTERING_NFZ, + GEOZONE_MESSAGE_STATE_AVOIDING_FB, + GEOZONE_MESSAGE_STATE_RETURN_TO_ZONE, + GEOZONE_MESSAGE_STATE_FLYOUT_NFZ, + GEOZONE_MESSAGE_STATE_AVOIDING_ALTITUDE_BREACH, + GEOZONE_MESSAGE_STATE_POS_HOLD +} geozoneMessageState_e; + +enum fenceAction_e { + GEOFENCE_ACTION_NONE, + GEOFENCE_ACTION_AVOID, + GEOFENCE_ACTION_POS_HOLD, + GEOFENCE_ACTION_RTH, +}; + +enum noWayHomeAction { + NO_WAY_HOME_ACTION_RTH, + NO_WAY_HOME_ACTION_EMRG_LAND, +}; + +#define GEOZONE_SHAPE_CIRCULAR 0 +#define GEOZONE_SHAPE_POLYGON 1 + +#define GEOZONE_TYPE_EXCLUSIVE 0 +#define GEOZONE_TYPE_INCLUSIVE 1 + +typedef struct geoZoneConfig_s +{ + uint8_t shape; + uint8_t type; + int32_t minAltitude; + int32_t maxAltitude; + bool isSealevelRef; + uint8_t fenceAction; + uint8_t vertexCount; +} geoZoneConfig_t; + +typedef struct geozone_config_s +{ + uint32_t fenceDetectionDistance; + uint16_t avoidAltitudeRange; + uint16_t safeAltitudeDistance; + bool nearestSafeHomeAsInclusivZone; + uint8_t safeHomeFenceAction; + uint32_t copterFenceStopDistance; + uint8_t noWayHomeAction; +} geozone_config_t; + +typedef struct vertexConfig_s +{ + int8_t zoneId; + uint8_t idx; + int32_t lat; + int32_t lon; +} vertexConfig_t; + +PG_DECLARE(geozone_config_t, geoZoneConfig); +PG_DECLARE_ARRAY(geoZoneConfig_t, MAX_GEOZONES_IN_CONFIG, geoZonesConfig); +PG_DECLARE_ARRAY(vertexConfig_t, MAX_VERTICES_IN_CONFIG, geoZoneVertices); + +typedef struct geozone_s { + bool insideFz; + bool insideNfz; + uint32_t distanceToZoneBorder3d; + int32_t vertDistanceToZoneBorder; + geozoneMessageState_e messageState; + int32_t directionToNearestZone; + int32_t distanceHorToNearestZone; + int32_t distanceVertToNearestZone; + int32_t zoneInfo; + int32_t currentzoneMaxAltitude; + int32_t currentzoneMinAltitude; + bool nearestHorZoneHasAction; + bool sticksLocked; + int8_t loiterDir; + bool avoidInRTHInProgress; + int32_t maxHomeAltitude; + bool homeHasMaxAltitue; +} geozone_t; + +extern geozone_t geozone; + +bool geozoneSetVertex(uint8_t zoneId, uint8_t vertexId, int32_t lat, int32_t lon); +int8_t geozoneGetVertexIdx(uint8_t zoneId, uint8_t vertexId); +bool isGeozoneActive(void); +uint8_t geozoneGetUsedVerticesCount(void); +void geozoneReset(int8_t idx); +void geozoneResetVertices(int8_t zoneId, int16_t idx); +void geozoneUpdate(timeUs_t curentTimeUs); +bool geozoneIsBlockingArming(void); +void geozoneAdvanceRthAvoidWaypoint(void); +int8_t geozoneCheckForNFZAtCourse(bool isRTH); +bool geoZoneIsLastRthWaypoint(void); +fpVector3_t *geozoneGetCurrentRthAvoidWaypoint(void); +void geozoneSetupRTH(void); +void geozoneResetRTH(void); +void geozoneUpdateMaxHomeAltitude(void); +uint32_t geozoneGetDetectionDistance(void); + +void activateSendTo(void); +void abortSendTo(void); +void activateForcedPosHold(void); +void abortForcedPosHold(void); + +#endif + #ifndef NAV_MAX_WAYPOINTS #define NAV_MAX_WAYPOINTS 15 #endif @@ -231,35 +348,37 @@ typedef enum { typedef struct positionEstimationConfig_s { uint8_t automatic_mag_declination; - uint8_t reset_altitude_type; // from nav_reset_type_e - uint8_t reset_home_type; // nav_reset_type_e - uint8_t gravity_calibration_tolerance; // Tolerance of gravity calibration (cm/s/s) + uint8_t reset_altitude_type; // from nav_reset_type_e + uint8_t reset_home_type; // nav_reset_type_e + uint8_t gravity_calibration_tolerance; // Tolerance of gravity calibration (cm/s/s) uint8_t allow_dead_reckoning; uint16_t max_surface_altitude; - float w_z_baro_p; // Weight (cutoff frequency) for barometer altitude measurements + float w_z_baro_p; // Weight (cutoff frequency) for barometer altitude measurements + float w_z_baro_v; // Weight (cutoff frequency) for barometer climb rate measurements - float w_z_surface_p; // Weight (cutoff frequency) for surface altitude measurements - float w_z_surface_v; // Weight (cutoff frequency) for surface velocity measurements + float w_z_surface_p; // Weight (cutoff frequency) for surface altitude measurements + float w_z_surface_v; // Weight (cutoff frequency) for surface velocity measurements - float w_z_gps_p; // GPS altitude data is very noisy and should be used only on airplanes - float w_z_gps_v; // Weight (cutoff frequency) for GPS climb rate measurements + float w_z_gps_p; // GPS altitude data is very noisy and should be used only on airplanes + float w_z_gps_v; // Weight (cutoff frequency) for GPS climb rate measurements - float w_xy_gps_p; // Weight (cutoff frequency) for GPS position measurements - float w_xy_gps_v; // Weight (cutoff frequency) for GPS velocity measurements + float w_xy_gps_p; // Weight (cutoff frequency) for GPS position measurements + float w_xy_gps_v; // Weight (cutoff frequency) for GPS velocity measurements float w_xy_flow_p; float w_xy_flow_v; - float w_z_res_v; // When velocity sources lost slowly decrease estimated velocity with this weight + float w_z_res_v; // When velocity sources lost slowly decrease estimated velocity with this weight float w_xy_res_v; - float w_acc_bias; // Weight (cutoff frequency) for accelerometer bias estimation. 0 to disable. + float w_acc_bias; // Weight (cutoff frequency) for accelerometer bias estimation. 0 to disable. - float max_eph_epv; // Max estimated position error acceptable for estimation (cm) - float baro_epv; // Baro position error + float max_eph_epv; // Max estimated position error acceptable for estimation (cm) + float baro_epv; // Baro position error + uint8_t default_alt_sensor; // default altitude sensor source #ifdef USE_GPS_FIX_ESTIMATION uint8_t allow_gps_fix_estimation; #endif diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index ff38b2e17e..87165cd7d6 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -250,6 +250,12 @@ static int8_t loiterDirection(void) { dir *= -1; } +#ifdef USE_GEOZONE + if (geozone.loiterDir != 0) { + dir = geozone.loiterDir; + } +#endif + return dir; } diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index cc6256dbdd..9dd4448dfd 100644 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -260,7 +260,7 @@ static bool hasIdleWakeWiggleSucceeded(timeUs_t currentTimeUs) { static timeMs_t wiggleTime = 0; static timeMs_t wigglesTime = 0; static int8_t wiggleStageOne = 0; - static uint8_t wiggleCount = 0; + static uint8_t wiggleCount = 0; const bool isAircraftWithinLaunchAngle = (calculateCosTiltAngle() >= cos_approx(DEGREES_TO_RADIANS(navConfig()->fw.launch_max_angle))); const uint8_t wiggleStrength = (navConfig()->fw.launch_wiggle_wake_idle == 1) ? 50 : 40; int8_t wiggleDirection = 0; @@ -276,12 +276,12 @@ static bool hasIdleWakeWiggleSucceeded(timeUs_t currentTimeUs) { } else if (wiggleStageOne != wiggleDirection) { wiggleStageOne = 0; wiggleCount++; - + if (wiggleCount == navConfig()->fw.launch_wiggle_wake_idle) { return true; } } - + wiggleTime = US2MS(currentTimeUs); } @@ -360,7 +360,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE_WIGGLE_WAIT(tim } if (navConfig()->fw.launch_wiggle_wake_idle == 0 || navConfig()->fw.launch_idle_motor_timer > 0 ) { - return FW_LAUNCH_EVENT_GOTO_DETECTION; + return FW_LAUNCH_EVENT_GOTO_DETECTION; } applyThrottleIdleLogic(true); @@ -613,6 +613,7 @@ uint8_t fixedWingLaunchStatus(void) void abortFixedWingLaunch(void) { setCurrentState(FW_LAUNCH_STATE_ABORTED, 0); + DISABLE_FLIGHT_MODE(NAV_LAUNCH_MODE); } const char * fixedWingLaunchStateMessage(void) diff --git a/src/main/navigation/navigation_geozone.c b/src/main/navigation/navigation_geozone.c new file mode 100755 index 0000000000..dfc7539859 --- /dev/null +++ b/src/main/navigation/navigation_geozone.c @@ -0,0 +1,2104 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + + +#include +#include +#include +#include +#include +#include +#include + +#include "platform.h" + +#include "common/utils.h" +#include "common/vector.h" +#include "common/printf.h" + +#include "config/config_reset.h" +#include "config/parameter_group_ids.h" + +#include "drivers/time.h" + +#include "fc/rc_modes.h" +#include "fc/rc_controls.h" +#include "fc/settings.h" + +#include "flight/imu.h" + +#include "rx/rx.h" + +#include "scheduler/scheduler.h" + +#include "navigation/navigation.h" +#include "navigation/navigation_private.h" + +#ifdef USE_GEOZONE + +#define MAX_VERTICES (MAX_VERTICES_IN_CONFIG + 1) +#define MAX_GEOZONES (MAX_GEOZONES_IN_CONFIG + 1) // +1 for safe home + +#define MAX_DISTANCE_FLY_OVER_POINTS 50000 +#define MAX_PATH_PONITS (2 + 2 * MAX_VERTICES) +#define POS_DETECTION_DISTANCE 7500 +#define STICK_LOCK_MIN_TIME 2500 +#define AVOID_TIMEOUT 30000 +#define MAX_LOCAL_VERTICES 128 +#define GEOZONE_INCLUSE_IGNORE_DISTANCE 2000 * 100 // m +#define STICK_MOVE_THRESHOULD 40 +#define MAX_RTH_WAYPOINTS (MAX_VERTICES / 2) +#define GEOZONE_INACTIVE INT8_MAX +#define RTH_OVERRIDE_TIMEOUT 1000 + +#define K_EPSILON 1e-8f + +#define IS_IN_TOLERANCE_RANGE(a, b, t) (((a) > (b) - (t)) && ((a) < (b) + (t))) + +typedef enum { + GEOZONE_ACTION_STATE_NONE, + GEOZONE_ACTION_STATE_AVOIDING, + GEOZONE_ACTION_STATE_AVOIDING_UPWARD, + GEOZONE_ACTION_STATE_AVOIDING_ALTITUDE, + GEOZONE_ACTION_STATE_RETURN_TO_FZ, + GEOZONE_ACTION_STATE_FLYOUT_NFZ, + GEOZONE_ACTION_STATE_POSHOLD, + GEOZONE_ACTION_STATE_RTH +} geozoneActionState_e; + +typedef struct geoZoneRuntimeConfig_s +{ + geoZoneConfig_t config; + bool enable; + bool isInfZone; + uint32_t radius; + fpVector2_t *verticesLocal; +} geoZoneRuntimeConfig_t; + +typedef struct pathPoint_s pathPoint_t; +struct pathPoint_s { + fpVector3_t point; + pathPoint_t* prev; + float distance; + bool visited; +}; + +static bool isInitalised = false; +static geoZoneRuntimeConfig_t *currentZones[MAX_GEOZONES]; +static fpVector2_t verticesLocal[MAX_VERTICES]; +static uint8_t currentZoneCount = 0; + +static bool isAtLeastOneInclusiveZoneActive = false; +static geoZoneRuntimeConfig_t activeGeoZones[MAX_GEOZONES]; +static uint8_t activeGeoZonesCount = 0; +static geoZoneConfig_t safeHomeGeozoneConfig; +static geozoneActionState_e actionState = GEOZONE_ACTION_STATE_NONE; +static timeMs_t actionStartTime = 0; +static int32_t avoidCourse; +static geoZoneRuntimeConfig_t *nearestHorZone = NULL; +static geoZoneRuntimeConfig_t *nearestInclusiveZone = NULL; +static fpVector3_t avoidingPoint; +static bool geozoneIsEnabled = false; +static fpVector3_t rthWaypoints[MAX_RTH_WAYPOINTS]; +static uint8_t rthWaypointIndex = 0; +static int8_t rthWaypointCount = 0; +static bool aboveOrUnderZone = false; +static timeMs_t rthOverrideStartTime; +static bool noZoneRTH = false; +static bool rthHomeSwitchLastState = false; +static bool lockRTZ = false; + +geozone_t geozone; + +PG_REGISTER_WITH_RESET_TEMPLATE(geozone_config_t, geoZoneConfig, PG_GEOZONE_CONFIG, 0); + +PG_RESET_TEMPLATE(geozone_config_t, geoZoneConfig, + .fenceDetectionDistance = SETTING_GEOZONE_DETECTION_DISTANCE_DEFAULT, + .avoidAltitudeRange = SETTING_GEOZONE_AVOID_ALTITUDE_RANGE_DEFAULT, + .safeAltitudeDistance = SETTING_GEOZONE_SAFE_ALTITUDE_DISTANCE_DEFAULT, + .nearestSafeHomeAsInclusivZone = SETTING_GEOZONE_SAFEHOME_AS_INCLUSIVE_DEFAULT, + .copterFenceStopDistance = SETTING_GEOZONE_MR_STOP_DISTANCE_DEFAULT, + .noWayHomeAction = SETTING_GEOZONE_NO_WAY_HOME_ACTION_DEFAULT +); + +PG_REGISTER_ARRAY_WITH_RESET_FN(geoZoneConfig_t, MAX_GEOZONES_IN_CONFIG, geoZonesConfig, PG_GEOZONES, 1); + +void pgResetFn_geoZonesConfig(geoZoneConfig_t *instance) +{ + for (int i = 0; i < MAX_GEOZONES_IN_CONFIG; i++) { + RESET_CONFIG(geoZoneConfig_t, &instance[i], + .shape = GEOZONE_TYPE_EXCLUSIVE, + .type = GEOZONE_SHAPE_CIRCULAR, + .minAltitude = 0, + .maxAltitude = 0, + .isSealevelRef = false, + .fenceAction = GEOFENCE_ACTION_NONE, + .vertexCount = 0 + ); + } +} + +PG_REGISTER_ARRAY_WITH_RESET_FN(vertexConfig_t, MAX_VERTICES_IN_CONFIG, geoZoneVertices, PG_GEOZONE_VERTICES, 0); + +void pgResetFn_geoZoneVertices(vertexConfig_t *instance) +{ + for (int i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + RESET_CONFIG(vertexConfig_t, &instance[i], + .zoneId = -1, + .idx = 0, + .lat = 0, + .lon = 0 + ); + } +} + +uint8_t geozoneGetUsedVerticesCount(void) +{ + uint8_t count = 0; + for (uint8_t i = 0; i < MAX_GEOZONES_IN_CONFIG; i++) { + count += geoZonesConfig(i)->vertexCount; + } + return count; +} + +void geozoneReset(const int8_t idx) +{ + if (idx < 0) { + for (uint8_t i = 0; i < MAX_GEOZONES_IN_CONFIG; i++) { + geoZonesConfigMutable(i)->shape = GEOZONE_SHAPE_CIRCULAR; + geoZonesConfigMutable(i)->type = GEOZONE_TYPE_EXCLUSIVE; + geoZonesConfigMutable(i)->maxAltitude = 0; + geoZonesConfigMutable(i)->minAltitude = 0; + geoZonesConfigMutable(i)->isSealevelRef = false; + geoZonesConfigMutable(i)->fenceAction = GEOFENCE_ACTION_NONE; + geoZonesConfigMutable(i)->vertexCount = 0; + } + } else if (idx < MAX_GEOZONES_IN_CONFIG) { + geoZonesConfigMutable(idx)->shape = GEOZONE_SHAPE_CIRCULAR; + geoZonesConfigMutable(idx)->type = GEOZONE_TYPE_EXCLUSIVE; + geoZonesConfigMutable(idx)->maxAltitude = 0; + geoZonesConfigMutable(idx)->minAltitude = 0; + geoZonesConfigMutable(idx)->isSealevelRef = false; + geoZonesConfigMutable(idx)->fenceAction = GEOFENCE_ACTION_NONE; + geoZonesConfigMutable(idx)->vertexCount = 0; + } +} + +void geozoneResetVertices(const int8_t zoneId, const int16_t idx) +{ + if (zoneId < 0 && idx < 0) { + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + geoZoneVerticesMutable(i)->lat = 0; + geoZoneVerticesMutable(i)->lon = 0; + geoZoneVerticesMutable(i)->idx = 0; + geoZoneVerticesMutable(i)->zoneId = -1; + } + for (uint8_t i = 0; i < MAX_GEOZONES_IN_CONFIG; i++) { + geoZonesConfigMutable(i)->vertexCount = 0; + } + } else if (zoneId >= 0 && zoneId < MAX_GEOZONES_IN_CONFIG && idx < 0) { + bool found = false; + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + if (geoZoneVertices(i)->zoneId == zoneId) { + geoZoneVerticesMutable(i)->lat = 0; + geoZoneVerticesMutable(i)->lon = 0; + geoZoneVerticesMutable(i)->idx = 0; + geoZoneVerticesMutable(i)->zoneId = -1; + found = true; + } + } + if (found) { + geoZonesConfigMutable(zoneId)->vertexCount = 0; + } + } else if (zoneId >= 0 && zoneId < MAX_GEOZONES_IN_CONFIG && idx >= 0 && idx < MAX_VERTICES_IN_CONFIG) { + bool found = false; + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + if (geoZoneVertices(i)->zoneId == zoneId && geoZoneVertices(i)->idx == idx) { + geoZoneVerticesMutable(i)->lat = 0; + geoZoneVerticesMutable(i)->lon = 0; + geoZoneVerticesMutable(i)->idx = 0; + geoZoneVerticesMutable(i)->zoneId = -1; + found = true; + break; + } + } + if (found) { + geoZonesConfigMutable(zoneId)->vertexCount--; + } + } +} + +bool geozoneSetVertex(uint8_t zoneId, uint8_t vertexId, int32_t lat, int32_t lon) +{ + if (vertexId < MAX_VERTICES_IN_CONFIG) + { + int16_t vertexConfigIdx = -1; + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + if (geoZoneVertices(i)->zoneId == -1) { + vertexConfigIdx = i; + break; + } + } + if (vertexConfigIdx >= 0) { + geoZoneVerticesMutable(vertexConfigIdx)->zoneId = zoneId; + geoZoneVerticesMutable(vertexConfigIdx)->idx = vertexId; + geoZoneVerticesMutable(vertexConfigIdx)->lat = lat; + geoZoneVerticesMutable(vertexConfigIdx)->lon = lon; + return true; + } + } + return false; +} + +int8_t geozoneGetVertexIdx(uint8_t zoneId, uint8_t vertexId) +{ + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + if (geoZoneVertices(i)->zoneId == zoneId && geoZoneVertices(i)->idx == vertexId) { + return i; + } + } + return -1; +} + + +static bool isPointInCircle(const fpVector2_t *point, const fpVector2_t *center, const float radius) +{ + return calculateDistance2(point, center) < radius; +} + +static bool isPointInPloygon(const fpVector2_t *point, fpVector2_t* vertices, const uint8_t verticesNum) +{ + bool result = false; + fpVector2_t *p1, *p2; + fpVector2_t* prev = &vertices[verticesNum - 1]; + fpVector2_t *current; + for (uint8_t i = 0; i < verticesNum; i++) + { + current = &vertices[i]; + + if (current->x > prev->x) { + p1 = prev; + p2 = current; + } else { + p1 = current; + p2 = prev; + } + + if ((current->x < point->x) == (point->x <= prev->x) + && (point->y - p1->y) * (p2->x - p1->x) < (p2->y - p1->y) * (point->x - p1->x)) + { + result = !result; + } + prev = current; + } + return result; +} + +static float angelFromSideLength(const float a, const float b, const float c) +{ + return acos_approx((sq(b) + sq(c) - sq(a)) / (2 * b * c)); +} + +static bool isPointRightFromLine(const fpVector2_t *lineStart, const fpVector2_t *lineEnd, const fpVector2_t *point) { + return (lineEnd->x - lineStart->x) * (point->y - lineStart->y) - (lineEnd->y - lineStart->y) * (point->x - lineStart->x) > 0; +} + +static float calcAngelFrom3Points(const fpVector2_t* a, const fpVector2_t* b, const fpVector2_t* c) +{ + float ab = calculateDistance2(a, b); + float ac = calculateDistance2(a, c); + float bc = calculateDistance2(b, c); + + return RADIANS_TO_DEGREES(angelFromSideLength(ab, ac, bc)); +} + +static void calcPointOnLine(fpVector2_t *result, const fpVector2_t *start, fpVector2_t *end, float distance) +{ + fpVector2_t dir, a; + float fac; + vector2Sub(&dir, end, start); + fac = distance / fast_fsqrtf(vector2NormSquared(&dir)); + vector2Scale(&a, &dir, fac); + vector2Add(result, start, &a); +} + +// Intersection of two lines https://en.wikipedia.org/wiki/Line-line_intersection +bool calcLineLineIntersection(fpVector2_t* intersection, const fpVector2_t* line1Start, const fpVector2_t* line1End, const fpVector2_t* line2Start, const fpVector2_t* line2End, bool isSegment) +{ + intersection->x = intersection->y = 0; + + // Double precision is needed here + double s1 = (double)(line1End->x - line1Start->x); + double t1 = (double)(-(line2End->x - line2Start->x)); + double r1 = (double)(line2Start->x - line1Start->x); + + double s2 = (double)(line1End->y - line1Start->y); + double t2 = (double)(-(line2End->y - line2Start->y)); + double r2 = (double)(line2Start->y - line1Start->y); + + // Use Cramer's rule for the solution of the system of linear equations + double determ = s1 * t2 - t1 * s2; + if (determ == 0) { // No solution + return false; + } + + double s0 = (r1 * t2 - t1 * r2) / determ; + double t0 = (s1 * r2 - r1 * s2) / determ; + + if (s0 == 0 && t0 == 0) { + return false; + } + + // No intersection + if (isSegment && (s0 <= 0 || s0 >= 1 || t0 <= 0 || t0 >= 1)) { + return false; + } + + intersection->x = (line1Start->x + (float)s0 * (line1End->x - line1Start->x)); + intersection->y = (line1Start->y + (float)s0 * (line1End->y - line1Start->y)); + + return true; +} + +float calculateDistance3(const fpVector3_t* startPos, const fpVector3_t* destinationPos) +{ + return fast_fsqrtf(sq(destinationPos->x - startPos->x) + sq(destinationPos->y - startPos->y) + sq(destinationPos->z - startPos->z)); +} + +static fpVector3_t calcPlaneNormalFromPoints(const fpVector3_t* p1, const fpVector3_t* p2, const fpVector3_t* p3) +{ + fpVector3_t result, a, b; + vectorSub(&a, p2, p1); + vectorSub(&b, p3, p1); + vectorCrossProduct(&result, &a, &b); + return result; +} + +static fpVector3_t calcDirVectorFromPoints(const fpVector3_t* p1, const fpVector3_t* p2) +{ + fpVector3_t result; + vectorSub(&result, p1, p2); + return result; +} + +static void generatePolygonFromCircle(fpVector2_t* polygon, const fpVector2_t* center, const float radius, const uint8_t sides) +{ + for (uint8_t i = 0, j = sides -1; i < sides; i++, j--) { + polygon[i].x = radius * cos_approx(2 * M_PIf * j / sides) + center->x; + polygon[i].y = radius * sin_approx(2 * M_PIf * j / sides) + center->y; + } +} + +// TRUE if point is in the same direction from pos as ref +static bool isInFront(const fpVector3_t *pos, const fpVector3_t *point, const fpVector3_t *ref) +{ + fpVector3_t refDir = calcDirVectorFromPoints(pos, ref); + fpVector3_t dir = calcDirVectorFromPoints(point, pos); + return vectorDotProduct(&refDir, &dir) < 0.0f; +} + +static fpVector2_t calcNearestPointOnLine(const fpVector2_t *lineStart, const fpVector2_t *lineEnd, const fpVector2_t *point) +{ + fpVector2_t ap, ab, prod2, result; + float distance, magAb, prod; + vector2Sub(&ap, point, lineStart); + vector2Sub(&ab, lineEnd, lineStart); + magAb = vector2NormSquared(&ab); + prod = vector2DotProduct(&ap, &ab); + distance = prod / magAb; + if (distance < 0) { + return *lineStart; + } else if (distance > 1) { + return *lineEnd; + } + vector2Scale(&prod2, &ab, distance); + vector2Add(&result, lineStart, &prod2); + return result; +} + +static bool isPointOnLine2(const fpVector2_t *lineStart, const fpVector2_t *lineEnd, const fpVector2_t *linepoint) +{ + float a = roundf(calculateDistance2(linepoint, lineStart)); + float b = roundf(calculateDistance2(linepoint, lineEnd)); + float c = roundf(calculateDistance2(lineStart, lineEnd)); + return a + b == c; +} + +static bool isPointOnLine3(const fpVector3_t *lineStart, const fpVector3_t *lineEnd, const fpVector3_t *linepoint) +{ + float a = roundf(calculateDistance3(linepoint, lineStart)); + float b = roundf(calculateDistance3(linepoint, lineEnd)); + float c = roundf(calculateDistance3(lineStart, lineEnd)); + return a + b == c; +} + +static bool calcIntersectionLinePlane(fpVector3_t* result, const fpVector3_t* lineVector, const fpVector3_t* linePoint, const fpVector3_t* planeNormal, const fpVector3_t* planePoint) +{ + if (vectorDotProduct(linePoint, planeNormal) == 0) { + return false; + } + fpVector3_t diff, p1, p4; + float p2 = 0, p3 = 0; + vectorSub(&diff, linePoint, planePoint); + vectorAdd(&p1, &diff, planePoint); + p2 = vectorDotProduct(&diff, planeNormal); + p3 = vectorDotProduct(lineVector, planeNormal); + vectorScale(&p4, lineVector, -p2 / p3); + vectorAdd(result, &p1, &p4); + return true; +} + + +// Möller–Trumbore intersection algorithm +static bool calcLineTriangleIntersection(fpVector3_t *intersection, const fpVector3_t* org, const fpVector3_t* dir, const fpVector3_t* v0, const fpVector3_t* v1, const fpVector3_t* v2) +{ + fpVector3_t v0v1, v0v2, pvec, tvec, quvec, prod; + float det, invDet, t, u, v; + + vectorSub(&v0v1, v1, v0); + vectorSub(&v0v2, v2, v0); + vectorCrossProduct(&pvec, dir, &v0v2); + det = vectorDotProduct(&v0v1, &pvec); + if (fabsf(det) < K_EPSILON) { + return false; + } + invDet = 1.f / det; + vectorSub(&tvec, org, v0); + u = vectorDotProduct(&tvec, &pvec) * invDet; + if (u < 0 || u > 1) { + return false; + } + vectorCrossProduct(&quvec, &tvec, &v0v1); + v = vectorDotProduct(dir, &quvec) * invDet; + if (v < 0 || u + v > 1) { + return false; + } + t = vectorDotProduct(&v0v2, &quvec) * invDet; + vectorScale(&prod, dir, t); + vectorAdd(intersection, &prod, org); + return true; +} + + +static bool calcLinePolygonIntersection(fpVector3_t *intersect, const fpVector3_t *pos, const fpVector3_t *pos2, const float height, fpVector2_t* vertices, const uint8_t verticesNum) +{ + if (verticesNum < 3) { + return false; + } + + fpVector3_t p1 = { .x = vertices[0].x, .y = vertices[0].y, .z = height }; + fpVector3_t p2 = { .x = vertices[1].x, .y = vertices[1].y, .z = height }; + fpVector3_t p3 = { .x = vertices[2].x, .y = vertices[2].y, .z = height }; + fpVector3_t normale = calcPlaneNormalFromPoints(&p1, &p2, &p3); + fpVector3_t dir = calcDirVectorFromPoints(pos, pos2); + + fpVector3_t tmp; + if (calcIntersectionLinePlane(&tmp, &dir, pos2, &normale, &p1)) { + if (isPointInPloygon((fpVector2_t*)&tmp, vertices, verticesNum)) { + memcpy(intersect, &tmp, sizeof(fpVector3_t)); + return true; + } + } + return false; +} + +static bool calcLineCircleIntersection(fpVector2_t *intersection1, fpVector2_t *intersection2, const fpVector2_t *startPos, const fpVector2_t *endPos, const fpVector2_t *circleCenter, const float radius) +{ + // Unfortunately, we need double precision here + double slope = (double)((endPos->y - startPos->y) / (endPos->x - startPos->x)); + double yIntercept = (double)startPos->y - slope * (double)startPos->x; + + double a = (double)1.0 + sq(slope); + double b = (double)-2.0 * (double)circleCenter->x + (double)2.0 * slope * (yIntercept - (double)circleCenter->y); + double c = sq((double)circleCenter->x) + (yIntercept - (double)circleCenter->y) * (yIntercept - (double)circleCenter->y) - sq((double)radius); + + double discr = sq(b) - (double)4.0 * a * c; + if (discr > 0) + { + double x1 = (-b + (double)fast_fsqrtf(discr)) / ((double)2.0 * a); + double y1 = slope * x1 + yIntercept; + double x2 = (-b - (double)fast_fsqrtf(discr)) / ((double)2.0 * a); + double y2 = slope * x2 + yIntercept; + + intersection1->x = (float)x1; + intersection1->y = (float)y1; + intersection2->x = (float)x2; + intersection2->y = (float)y2; + return true; + } + return false; +} + +static void generateOffsetPolygon(fpVector2_t* verticesNew, fpVector2_t* verticesOld, const uint8_t numVertices, const float offset) +{ + fpVector2_t* prev = &verticesOld[numVertices - 1]; + fpVector2_t* current; + fpVector2_t* next; + for (uint8_t i = 0; i < numVertices; i++) { + current = &verticesOld[i]; + if (i + 1 < numVertices) { + next = &verticesOld[i + 1]; + } + else { + next = &verticesOld[0]; + } + + fpVector2_t v, vn, vs, pcp1, pcp2, pcn1, pcn2, intersect; + vector2Sub(&v, current, prev); + vector2Normalize(&vn, &v); + vector2Scale(&vs, &vn, offset); + pcp1.x = prev->x - vs.y; + pcp1.y = prev->y + vs.x; + pcp2.x = current->x - vs.y; + pcp2.y = current->y + vs.x; + + vector2Sub(&v, next, current); + vector2Normalize(&vn, &v); + vector2Scale(&vs, &vn, offset); + pcn1.x = current->x - vs.y; + pcn1.y = current->y + vs.x; + pcn2.x = next->x - vs.y; + pcn2.y = next->y + vs.x; + + if (calcLineLineIntersection(&intersect, &pcp1, &pcp2, &pcn1, &pcn2, false)) { + verticesNew[i].x = intersect.x; + verticesNew[i].y = intersect.y; + } + prev = current; + } +} + +// Calculates the nearest intersection point +// Inspired by raytracing algortyhms +static bool calcLineCylinderIntersection(fpVector3_t* intersection, float* distance, const fpVector3_t* startPos, const fpVector3_t* endPos, const fpVector3_t* circleCenter, const float radius, const float height, const bool inside) +{ + float distToIntersection = FLT_MAX; + fpVector3_t intersect; + + fpVector2_t i1, i2; + if (!calcLineCircleIntersection(&i1, &i2, (fpVector2_t*)startPos, (fpVector2_t*)endPos, (fpVector2_t*)circleCenter, radius)) { + return false; + } + + if (calculateDistance2((fpVector2_t*)startPos, &i1) < calculateDistance2((fpVector2_t*)startPos, &i2)) { + intersect.x = i1.x; + intersect.y = i1.y; + } else { + intersect.x = i2.x; + intersect.y = i2.y; + } + + float dist1 = calculateDistance2((fpVector2_t*)endPos, (fpVector2_t*)&intersect); + float dist2 = calculateDistance2((fpVector2_t*)startPos, (fpVector2_t*)endPos); + fpVector2_t p4, p5, p6, p7; + p4.x = 0; + p4.y = endPos->z; + p5.x = dist2; + p5.y = startPos->z; + p6.x = dist1; + p6.y = circleCenter->z; + p7.x = dist1; + p7.y = circleCenter->z + height; + + fpVector2_t heightIntersection; + if (calcLineLineIntersection(&heightIntersection, &p4, &p5, &p6, &p7, true)) { + intersect.z = heightIntersection.y; + if (isInFront(startPos, &intersect, endPos)) { + distToIntersection = calculateDistance3(startPos, &intersect); + } + } + + fpVector3_t intersectCap; + fpVector3_t dir = calcDirVectorFromPoints(startPos, endPos); + if (startPos->z < circleCenter->z || (inside && circleCenter->z != 0)) { + fpVector3_t p1 = *circleCenter; + p1.x = circleCenter->x + radius; + fpVector3_t p2 = *circleCenter; + p2.y = circleCenter->y + radius; + fpVector3_t normal = calcPlaneNormalFromPoints(circleCenter, &p1, &p2); + + if (calcIntersectionLinePlane(&intersectCap, &dir, endPos, &normal, circleCenter) + && isPointInCircle((fpVector2_t*)&intersectCap, (fpVector2_t*)circleCenter, radius) + && isInFront(startPos, &intersectCap, endPos)) { + float distanceCap = calculateDistance3(startPos, &intersectCap); + if (distanceCap < distToIntersection) { + distToIntersection = distanceCap; + intersect = intersectCap; + } + } + } + + if (startPos->z > circleCenter->z + height || inside) { + fpVector3_t p1 = { .x = circleCenter->x + radius, .y = circleCenter->y, .z = circleCenter->z + height }; + fpVector3_t p2 = { .x = circleCenter->x, .y = circleCenter->y + radius, .z = circleCenter->z + height }; + fpVector3_t p3 = *circleCenter; + p3.z = circleCenter->z + height; + fpVector3_t normal = calcPlaneNormalFromPoints(&p3, &p1, &p2); + + if (calcIntersectionLinePlane(&intersectCap, &dir, startPos, &normal, &p3) + && isPointInCircle((fpVector2_t*)&intersectCap, (fpVector2_t*)circleCenter, radius) + && isInFront(startPos, &intersectCap, endPos)) { + float distanceCap = calculateDistance3(startPos, &intersectCap); + if (distanceCap < distToIntersection) { + distToIntersection = distanceCap; + intersect = intersectCap; + } + } + } + + if (distToIntersection < FLT_MAX) { + *distance = distToIntersection; + memcpy(intersection, &intersect, sizeof(fpVector3_t)); + return true; + } + return false; +} + +static bool calcLine3dPolygonIntersection(fpVector3_t *intersection, float *distance, const fpVector3_t *startPos, const fpVector3_t *endPos, fpVector2_t *vertices, const uint8_t numVertices, const float minHeight, const float maxHeight, const bool isInclusiveZone) +{ + float distToIntersection = FLT_MAX; + fpVector3_t intersect; + + fpVector2_t* prev = &vertices[numVertices - 1]; + fpVector2_t* current; + for (uint8_t i = 0; i < numVertices; i++) { + current = &vertices[i]; + + fpVector3_t p1 = { .x = prev->x, .y = prev->y, .z = minHeight }; + fpVector3_t p2 = { .x = prev->x, .y = prev->y, .z = maxHeight }; + fpVector3_t p3 = { .x = current->x, .y = current->y, .z = minHeight }; + fpVector3_t p4 = { .x = current->x, .y = current->y, .z = maxHeight}; + + fpVector3_t dir = calcDirVectorFromPoints(startPos, endPos); + fpVector3_t intersectCurrent; + if ((calcLineTriangleIntersection(&intersectCurrent, startPos, &dir, &p1, &p2, &p3) + || calcLineTriangleIntersection(&intersectCurrent, startPos, &dir, &p3, &p4, &p2)) && isInFront(startPos, &intersectCurrent, endPos) ) { + float distWall = calculateDistance3(startPos, &intersectCurrent); + if (distWall < distToIntersection) { + distToIntersection = distWall; + intersect = intersectCurrent; + } + } + prev = current; + } + + fpVector3_t intersectCap; + if (startPos->z < minHeight || (isInclusiveZone && minHeight != 0)) { + if (calcLinePolygonIntersection(&intersectCap, startPos, endPos, minHeight, vertices, numVertices) && isInFront(startPos, &intersectCap, endPos)) + { + float distanceCap = calculateDistance3(startPos, &intersectCap); + if (distanceCap < distToIntersection) { + distToIntersection = distanceCap; + intersect = intersectCap; + } + } + } + + if (startPos->z > maxHeight || isInclusiveZone) { + if (calcLinePolygonIntersection(&intersectCap, startPos, endPos, maxHeight, vertices, numVertices) && isInFront(startPos, &intersectCap, endPos)) + { + float distanceCap = calculateDistance3(startPos, &intersectCap); + if (distanceCap < distToIntersection) { + distToIntersection = distanceCap; + intersect = intersectCap; + } + } + } + + if (distToIntersection < FLT_MAX) { + *distance = distToIntersection; + memcpy(intersection, &intersect, sizeof(fpVector3_t)); + return true; + } + return false; +} + +static bool areSticksDeflectdFromChannelValue(void) +{ + return ABS(rxGetChannelValue(ROLL) - PWM_RANGE_MIDDLE) + ABS(rxGetChannelValue(PITCH) - PWM_RANGE_MIDDLE) + ABS(rxGetChannelValue(YAW) - PWM_RANGE_MIDDLE) >= STICK_MOVE_THRESHOULD; +} + +static bool isNonGeozoneModeFromBoxInput(void) +{ + return !(IS_RC_MODE_ACTIVE(BOXNAVCRUISE) || IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) || IS_RC_MODE_ACTIVE(BOXHORIZON) || IS_RC_MODE_ACTIVE(BOXANGLE)); +} + +static bool isPointOnBorder(geoZoneRuntimeConfig_t *zone, const fpVector3_t *pos) +{ + fpVector2_t *prev = &zone->verticesLocal[zone->config.vertexCount -1]; + fpVector2_t *current; + bool isOnBorder = false; + for (uint8_t i = 0; i < zone->config.vertexCount; i++) { + current = &zone->verticesLocal[i]; + if (isPointOnLine2(prev, current, (fpVector2_t*)pos)) { + isOnBorder = true; + break; + } + prev = current; + } + + if (isOnBorder) { + return (pos->z >= zone->config.minAltitude || zone->config.minAltitude == 0) && pos->z <= zone->config.maxAltitude; + } + + return isOnBorder; +} + +static bool isInZoneAltitudeRange(geoZoneRuntimeConfig_t *zone, const float pos) +{ + return (pos >= zone->config.minAltitude || zone->config.minAltitude == 0) && pos <= zone->config.maxAltitude; +} + +static bool isInGeozone(geoZoneRuntimeConfig_t *zone, const fpVector3_t *pos, bool ignoreAltitude) +{ + if (activeGeoZonesCount == 0) { + return false; + } + + bool isIn2D = false; + if (zone->config.shape == GEOZONE_SHAPE_POLYGON) { + isIn2D = isPointInPloygon((fpVector2_t*)pos, zone->verticesLocal, zone->config.vertexCount) || isPointOnBorder(zone, pos); + } else { // cylindric + isIn2D = isPointInCircle((fpVector2_t*)pos, &zone->verticesLocal[0], zone->radius); + } + + if (isIn2D && !ignoreAltitude) { + return isInZoneAltitudeRange(zone, pos->z); + } + + return isIn2D; +} + +static bool isPointInAnyOtherZone(const geoZoneRuntimeConfig_t *zone, uint8_t type, const fpVector3_t *pos) +{ + bool isInZone = false; + for (uint8_t i = 0; i < activeGeoZonesCount; i++) { + if (zone != &activeGeoZones[i] && activeGeoZones[i].config.type == type && isInGeozone(&activeGeoZones[i], pos, false)) { + isInZone = true; + break; + } + } + return isInZone; +} + +static uint8_t getZonesForPos(geoZoneRuntimeConfig_t *zones[], const fpVector3_t *pos, const bool ignoreAltitude) +{ + uint8_t count = 0; + for (int i = 0; i < activeGeoZonesCount; i++) { + if (isInGeozone(&activeGeoZones[i], pos, ignoreAltitude)) { + zones[count++] = &activeGeoZones[i]; + } + } + return count; +} + +static uint8_t getCurrentZones(geoZoneRuntimeConfig_t *zones[], const bool ignoreAltitude) +{ + return getZonesForPos(zones, &navGetCurrentActualPositionAndVelocity()->pos, ignoreAltitude); +} + +static int geoZoneRTComp(const void *a, const void *b) +{ + geoZoneRuntimeConfig_t *zoneA = (geoZoneRuntimeConfig_t*)a; + geoZoneRuntimeConfig_t *zoneB = (geoZoneRuntimeConfig_t*)b; + + if (zoneA->enable == zoneB->enable) { + return 0; + } else if (zoneA->enable) { + return -1; + } else { + return 1; + } +} + +// in cm and cms/s +static uint32_t calcTime(const int32_t distance, const int32_t speed) +{ + if (speed <= 0) { + return 0; + } + + return distance / speed; +} + +static void calcPreviewPoint(fpVector3_t *target, const int32_t distance) +{ + calculateFarAwayTarget(target, DECIDEGREES_TO_CENTIDEGREES(gpsSol.groundCourse), distance); + target->z = getEstimatedActualPosition(Z) + calcTime(geoZoneConfig()->fenceDetectionDistance, gpsSol.groundSpeed) * getEstimatedActualVelocity(Z); +} + +static bool calcIntersectionForZone(fpVector3_t *intersection, float *distance, geoZoneRuntimeConfig_t *zone, const fpVector3_t *start, const fpVector3_t *end) +{ + bool hasIntersection = false; + if (zone->config.shape == GEOZONE_SHAPE_POLYGON) { + if (calcLine3dPolygonIntersection( + intersection, + distance, + start, + end, + zone->verticesLocal, + zone->config.vertexCount, + zone->config.minAltitude, + zone->config.maxAltitude, + zone->config.type == GEOZONE_TYPE_INCLUSIVE)) { + hasIntersection = true; + } + } else if (zone->config.shape == GEOZONE_SHAPE_CIRCULAR) { + fpVector3_t circleCenter = { .x = zone->verticesLocal[0].x, .y = zone->verticesLocal[0].y, .z = zone->config.minAltitude }; + if (calcLineCylinderIntersection( + intersection, + distance, + start, + end, + &circleCenter, + zone->radius, + zone->config.maxAltitude - zone->config.minAltitude, + zone->config.type == GEOZONE_TYPE_INCLUSIVE)) { + hasIntersection = true; + } + } + + if (hasIntersection && isPointOnLine3(start, end, intersection)){ + return true; + } + *distance = -1; + return false; +} + +static int32_t calcBouceCoursePolygon(const int32_t course, const fpVector2_t* pos, const fpVector2_t *intersect, const fpVector2_t* p1, const fpVector2_t* p2) +{ + int32_t newCourse = 0; + int32_t angelp1 = DEGREES_TO_CENTIDEGREES(calcAngelFrom3Points(p1, pos, intersect)); + int32_t angelp2 = DEGREES_TO_CENTIDEGREES(calcAngelFrom3Points(p2, pos, intersect)); + if (angelp1 < angelp2) { + if (isPointRightFromLine(pos, intersect, p1)) { + newCourse = course - 2 * angelp1; + } + else { + newCourse = course + 2 * angelp1; + } + } + else { + if (isPointRightFromLine(pos, intersect, p2)) { + newCourse = course - 2 * angelp2; + } + else { + newCourse = course + 2 * angelp2; + } + } + return wrap_36000(newCourse); +} + +static int32_t calcBouceCourseCircle(const int32_t course, const fpVector2_t* pos, const fpVector2_t* intersect, const fpVector2_t* mid) +{ + int32_t newCourse = 0; + int32_t angel = DEGREES_TO_CENTIDEGREES(calcAngelFrom3Points(mid, pos, intersect)); + if (isPointRightFromLine(pos, mid, intersect)) { + newCourse = course + 2 * (angel - 9000); + } + else { + newCourse = course - 2 * (angel - 9000); + } + return wrap_36000(newCourse); +} + +static bool findNearestIntersectionZone(geoZoneRuntimeConfig_t **intersectZone, fpVector3_t *intersection, float *distance, const float detectionDistance, const fpVector3_t *start, const fpVector3_t *end) +{ + geoZoneRuntimeConfig_t *zone = NULL; + fpVector3_t intersect; + float distanceToZone = FLT_MAX; + + for (uint8_t i = 0; i < activeGeoZonesCount; i++) { + fpVector3_t currentIntersect; + float currentDistance = FLT_MAX; + if (!calcIntersectionForZone( + ¤tIntersect, + ¤tDistance, + &activeGeoZones[i], + start, + end)) { + continue; + } + + if (currentDistance < distanceToZone) { + distanceToZone = currentDistance; + zone = &activeGeoZones[i]; + intersect = currentIntersect; + } + } + + if (zone && distanceToZone < detectionDistance) { + *intersectZone = zone; + *distance = distanceToZone; + memcpy(intersection, &intersect, sizeof(fpVector3_t)); + return true; + } + + return false; +} + +static bool isPointDirectReachable(const fpVector3_t* start, const fpVector3_t *point) +{ + float currentDistance = 0; + bool pointIsInOverlappingZone = false, pointIsInExclusiveZone = false, hasIntersection = false; + + /* + if (start->x == point->x && start->y == point->y) { + return false; + } + */ + + for (uint8_t i = 0; i < activeGeoZonesCount; i++) { + fpVector3_t currentIntersect; + + if (!calcIntersectionForZone(¤tIntersect, ¤tDistance, &activeGeoZones[i], start, point)) { + continue; + } + hasIntersection = true; + + // Intersct a exclusive Zone? + geoZoneRuntimeConfig_t *intersectZones[MAX_GEOZONES]; + uint8_t intersectZonesCount = getZonesForPos(intersectZones, ¤tIntersect, false); + for (uint8_t j = 0; j < intersectZonesCount; j++) { + if (intersectZones[j]->config.type == GEOZONE_TYPE_EXCLUSIVE ) { + pointIsInExclusiveZone = true; + break; + } + } + + if (pointIsInExclusiveZone) { + break; + } + + // We targeting a exit point (in min two zones) + if (intersectZonesCount < 2) { + break; + } + + geoZoneRuntimeConfig_t *startZones[MAX_GEOZONES]; + uint8_t startZonesCount = getZonesForPos(startZones, start, false); + geoZoneRuntimeConfig_t *endZones[MAX_GEOZONES]; + uint8_t endZonesCount = getZonesForPos(endZones, point, false); + + for (uint8_t j = 0; j < intersectZonesCount; j++) { + for (uint8_t k = 0; k < startZonesCount; k++) { + for (uint8_t l = 0; l < endZonesCount; l++) { + if (intersectZones[j] == startZones[k] && intersectZones[j] == endZones[l]) { + pointIsInOverlappingZone = true; + break; + } + } + if (pointIsInOverlappingZone) { + break; + } + } + if (pointIsInOverlappingZone) { + break; + } + } + } + + return !pointIsInExclusiveZone && (!hasIntersection || pointIsInOverlappingZone); +} + +uint32_t geozoneGetDetectionDistance(void) +{ + uint32_t detctionDistance = 0; + if (STATE(AIRPLANE)) { + detctionDistance = navConfig()->fw.loiter_radius * 1.5f; + } else { + detctionDistance = geoZoneConfig()->copterFenceStopDistance; + } + return detctionDistance; +} + +static int32_t calcBounceCourseForZone(geoZoneRuntimeConfig_t *zone, fpVector3_t *prevPoint, fpVector3_t *intersection) +{ + int32_t course = 0; + if (zone->config.shape == GEOZONE_SHAPE_POLYGON) { + fpVector2_t intersect; + bool found = false; + fpVector2_t *p1 = &zone->verticesLocal[zone->config.vertexCount - 1]; + fpVector2_t *p2; + for (uint8_t i = 0; i < zone->config.vertexCount; i++) { + p2 = &zone->verticesLocal[i]; + if (calcLineLineIntersection(&intersect, p1, p2, (fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, (fpVector2_t*)prevPoint, true)) { + found = true; + break; + } + p1 = p2; + } + + if (!found) { + return -1; + } + course = calcBouceCoursePolygon(DECIDEGREES_TO_CENTIDEGREES(gpsSol.groundCourse), (fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, &intersect, p1, p2); + } else if (zone->config.shape == GEOZONE_SHAPE_CIRCULAR) { + course = calcBouceCourseCircle(DECIDEGREES_TO_CENTIDEGREES(gpsSol.groundCourse), (fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, (fpVector2_t*)intersection, &zone->verticesLocal[0]); + } + return course; +} + +static bool initPathPoint(pathPoint_t *pathPoints, const fpVector3_t pos, uint8_t *idx) +{ + if (*idx + 1 >= MAX_PATH_PONITS) { + return false; + } + + pathPoints[*idx].distance = FLT_MAX; + pathPoints[*idx].visited = false; + pathPoints[(*idx)++].point = pos; + + return true; +} + +static bool isPosInGreenAlt(geoZoneRuntimeConfig_t *zones[], const uint8_t zoneCount, const float alt) +{ + bool isInNfz = false, isInFz = false; + for (uint8_t j = 0; j < zoneCount; j++) { + if(isInZoneAltitudeRange(zones[j], alt)){ + isInFz = zones[j]->config.type == GEOZONE_TYPE_INCLUSIVE; + isInNfz = zones[j]->config.type == GEOZONE_TYPE_EXCLUSIVE; + } + } + return !isInNfz && (!isAtLeastOneInclusiveZoneActive || isInFz); +} + +static bool checkPathPointOrSetAlt(fpVector3_t *pos) +{ + geoZoneRuntimeConfig_t *zones[MAX_GEOZONES]; + uint8_t zoneCount = getZonesForPos(zones, pos, true); + + if (zoneCount == 0) { + return !isAtLeastOneInclusiveZoneActive; + } + + if (zoneCount == 1 && zones[0]->config.type == GEOZONE_TYPE_INCLUSIVE) { + return true; + } + + bool isInExclusice = false; + for (uint8_t i = 0; i < zoneCount; i++) { + if (zones[i]->config.type == GEOZONE_TYPE_EXCLUSIVE && isInGeozone(zones[i], pos, false)) { + isInExclusice = true; + if (zones[i]->config.minAltitude != 0) { + float min = zones[i]->config.minAltitude - 2 * geoZoneConfig()->safeAltitudeDistance; + if (isPosInGreenAlt(zones, zoneCount, min)) { + pos->z = min; + return true; + } + } + + if (!zones[i]->isInfZone || zones[i]->config.maxAltitude < INT32_MAX) { + float max = zones[i]->config.maxAltitude + 2 * geoZoneConfig()->safeAltitudeDistance; + if(isPosInGreenAlt(zones, zoneCount, max)) { + pos->z = max; + return true; + } + } + } + } + + return !isInExclusice; +} + +// Return value: 0 - Target direct reachable; -1 No way; >= 1 Waypoints to target +#define CIRCLE_POLY_SIDES 6 +static int8_t calcRthCourse(fpVector3_t* waypoints, const fpVector3_t* point, fpVector3_t* target) +{ + pathPoint_t pathPoints[MAX_PATH_PONITS]; + uint8_t pathPointCount = 0, waypointCount = 0; + fpVector3_t start = *point; + + if (isPointDirectReachable(&start, target)) { + return 0; + } + + // Set starting point slightly away from our current position + float offset = geozoneGetDetectionDistance(); + if (geozone.distanceVertToNearestZone <= offset) { + int bearing = wrap_36000(geozone.directionToNearestZone + 18000); + start.x += offset * cos_approx(CENTIDEGREES_TO_RADIANS(bearing)); + start.y += offset * sin_approx(CENTIDEGREES_TO_RADIANS(bearing)); + } + + pathPoints[pathPointCount].visited = true; + pathPoints[pathPointCount].distance = 0; + pathPoints[pathPointCount++].point = start; + + // Calculate possible waypoints + // Vertices of the zones are possible waypoints, + // inclusive zones are “reduced”, exclusive zones are “enlarged” to keep distance, + // round zones are converted into hexagons and long sides get additional points to be able to fly over zones. + for (uint8_t i = 0 ; i < activeGeoZonesCount; i++) { + fpVector2_t *verticesZone; + fpVector2_t verticesCirclePoly[CIRCLE_POLY_SIDES]; + uint8_t verticesZoneCount; + if (activeGeoZones[i].config.shape == GEOZONE_SHAPE_POLYGON) { + verticesZone = activeGeoZones[i].verticesLocal; + verticesZoneCount = activeGeoZones[i].config.vertexCount; + } else { + generatePolygonFromCircle(verticesCirclePoly, &activeGeoZones[i].verticesLocal[0], activeGeoZones[i].radius, CIRCLE_POLY_SIDES); + verticesZone = verticesCirclePoly; + verticesZoneCount = CIRCLE_POLY_SIDES; + } + + fpVector2_t safeZone[MAX_VERTICES]; + offset = geozoneGetDetectionDistance() * 2 / 3; + if (activeGeoZones[i].config.type == GEOZONE_TYPE_INCLUSIVE) { + offset *= -1; + } + + float zMin = start.z, zMax = 0; + if (!isInZoneAltitudeRange(&activeGeoZones[i], start.z) && activeGeoZones[i].config.minAltitude > 0) { + zMin = activeGeoZones[i].config.minAltitude + 2 * geoZoneConfig()->safeAltitudeDistance; + } + + if (activeGeoZones[i].config.type == GEOZONE_TYPE_INCLUSIVE && (!activeGeoZones[i].isInfZone || activeGeoZones[i].config.maxAltitude < INT32_MAX)) { + zMax = activeGeoZones[i].config.maxAltitude - 2 * geoZoneConfig()->safeAltitudeDistance; + } else if (activeGeoZones[i].config.type == GEOZONE_TYPE_EXCLUSIVE && (!activeGeoZones[i].isInfZone || activeGeoZones[i].config.maxAltitude < INT32_MAX)) { + zMax = activeGeoZones[i].config.maxAltitude + 2 * geoZoneConfig()->safeAltitudeDistance; + } + + generateOffsetPolygon(safeZone, verticesZone, verticesZoneCount, offset); + fpVector2_t *prev = &safeZone[verticesZoneCount - 1]; + fpVector2_t *current; + for (uint8_t j = 0; j < verticesZoneCount; j++) { + current = &safeZone[j]; + + if (zMax > 0 ) { + fpVector3_t max = { .x = current->x, .y = current->y, .z = zMax }; + if (checkPathPointOrSetAlt(&max)) { + if (!initPathPoint(pathPoints, max, &pathPointCount)) { + return -1; + } + } + + if (activeGeoZones[i].config.type == GEOZONE_TYPE_EXCLUSIVE) { + // Set some "fly over points" + float dist = calculateDistance2(prev, current); + if (dist > MAX_DISTANCE_FLY_OVER_POINTS) { + uint8_t sectionCount = (uint8_t)(dist / MAX_DISTANCE_FLY_OVER_POINTS); + float dist = MAX_DISTANCE_FLY_OVER_POINTS; + for (uint8_t k = 0; k < sectionCount; k++) { + fpVector3_t flyOverPoint; + calcPointOnLine((fpVector2_t*)&flyOverPoint, prev, current, dist); + fpVector3_t maxFo = { .x = flyOverPoint.x, .y = flyOverPoint.y, .z = zMax }; + if (checkPathPointOrSetAlt(&maxFo)) { + if (!initPathPoint(pathPoints, maxFo, &pathPointCount)) { + return -1; + } + } + dist += MAX_DISTANCE_FLY_OVER_POINTS; + } + } + } + } + + if (zMin > 0) { + fpVector3_t min = { .x = current->x, .y = current->y, .z = zMin }; + if (checkPathPointOrSetAlt(&min)) { + if (!initPathPoint(pathPoints, min, &pathPointCount)) { + return -1; + } + } + + } + prev = current; + } + } + + if (!initPathPoint(pathPoints, *target, &pathPointCount)) { + return -1; + } + + // Dijkstra + pathPoint_t *current = pathPoints; + while (!pathPoints[pathPointCount - 1].visited) { + pathPoint_t *next = current; + float min = FLT_MAX; + for (uint8_t i = 1; i < pathPointCount; i++) { + + float currentDist = FLT_MAX; + if (isPointDirectReachable(¤t->point, &pathPoints[i].point)) { + float dist2D = calculateDistance2((fpVector2_t*)¤t->point, (fpVector2_t*)&pathPoints[i].point); + float distAlt = ABS(current->point.z - pathPoints[i].point.z); + currentDist = current->distance + dist2D + 2 * distAlt; + } + + if (currentDist < pathPoints[i].distance && !pathPoints[i].visited) { + pathPoints[i].distance = currentDist; + pathPoints[i].prev = current; + } + if (min > pathPoints[i].distance && !pathPoints[i].visited) { + min = pathPoints[i].distance; + next = &pathPoints[i]; + } + } + + if (min == FLT_MAX) { + return -1; + } + + current = next; + current->visited = true; + } + + waypointCount = 0; + current = &pathPoints[pathPointCount - 1]; + while (current != pathPoints) { + waypointCount++; + current = current->prev; + } + // Don't set home to the WP list + current = pathPoints[pathPointCount - 1].prev; + uint8_t i = waypointCount - 2; + while (current != pathPoints) { + waypoints[i] = current->point; + current = current->prev; + i--; + } + return waypointCount - 1; +} + +static void updateCurrentZones(void) +{ + currentZoneCount = getCurrentZones(currentZones, false); + geozone.insideNfz = false; + geozone.insideFz = false; + for (uint8_t i = 0; i < currentZoneCount; i++) { + if (currentZones[i]->config.type == GEOZONE_TYPE_EXCLUSIVE) { + geozone.insideNfz = true; + } + if (currentZones[i]->config.type == GEOZONE_TYPE_INCLUSIVE) { + geozone.insideFz = true; + } + } +} + +static void updateZoneInfos(void) +{ + float nearestDistanceToBorder = FLT_MAX; + fpVector3_t nearestBorderPoint; + aboveOrUnderZone = false; + + nearestHorZone = NULL; + geoZoneRuntimeConfig_t *currentZones[MAX_GEOZONES]; + uint8_t currentZoneCount = getCurrentZones(currentZones, true); + int32_t currentMaxAltitude = INT32_MIN, currentMinAltitude = INT32_MAX; + + if (currentZoneCount == 1) { + currentMaxAltitude = currentZones[0]->config.maxAltitude; + currentMinAltitude = currentZones[0]->config.minAltitude; + nearestHorZone = currentZones[0]; + + } else if (currentZoneCount >= 2) { + + geoZoneRuntimeConfig_t *aboveZone = NULL, *belowZone = NULL; + float distAbove = FLT_MAX, distBelow = FLT_MAX; + geoZoneRuntimeConfig_t *current = NULL; + for (uint8_t i = 0; i < currentZoneCount; i++) { + current = currentZones[i]; + + if (isInZoneAltitudeRange(current, getEstimatedActualPosition(Z))) { + currentMaxAltitude = MAX(current->config.maxAltitude, currentMaxAltitude); + currentMinAltitude = MIN(current->config.minAltitude, currentMinAltitude); + nearestHorZone = current; + } + + if (current->config.minAltitude > getEstimatedActualPosition(Z)) { + float dist = current->config.maxAltitude - getEstimatedActualPosition(Z); + if (dist < distAbove) { + aboveZone = current; + distAbove = dist; + } + } + + if (current->config.maxAltitude < getEstimatedActualPosition(Z)) { + float dist = getEstimatedActualPosition(Z) - current->config.maxAltitude; + if (dist < distBelow) { + belowZone = current; + distBelow = dist; + } + } + } + + if (aboveZone) { + if (aboveZone->config.type == GEOZONE_TYPE_INCLUSIVE) { + currentMaxAltitude = MAX(aboveZone->config.maxAltitude, currentMaxAltitude); + nearestHorZone = aboveZone; + } else { + currentMaxAltitude = MIN(aboveZone->config.minAltitude, currentMaxAltitude); + } + } + + if (belowZone) { + if (belowZone->config.type == GEOZONE_TYPE_INCLUSIVE) { + currentMinAltitude = MIN(belowZone->config.minAltitude, currentMinAltitude); + nearestHorZone = belowZone; + } else { + currentMinAltitude = MAX(belowZone->config.maxAltitude, currentMinAltitude); + } + } + } + + if (currentMinAltitude == INT32_MAX) { + currentMinAltitude = 0; + } + + if (currentMaxAltitude == INT32_MIN) { + currentMaxAltitude = 0; + } + + if (currentMaxAltitude == INT32_MAX && currentMinAltitude != 0) { + geozone.distanceVertToNearestZone = ABS(currentMinAltitude - getEstimatedActualPosition(Z)); + } else if (currentMinAltitude == 0 && currentMaxAltitude != 0) { + geozone.distanceVertToNearestZone = currentMaxAltitude - getEstimatedActualPosition(Z); + } else if (currentMinAltitude != 0 && currentMaxAltitude > 0) { + int32_t distToMin = currentMinAltitude - getEstimatedActualPosition(Z); + int32_t distToMax = currentMaxAltitude - getEstimatedActualPosition(Z); + if (getEstimatedActualPosition(Z) > currentMinAltitude && getEstimatedActualPosition(Z) < currentMaxAltitude) { + if (ABS(distToMin) < ABS(currentMaxAltitude - currentMinAltitude) / 2 ) { + geozone.distanceVertToNearestZone = distToMin; + } else { + geozone.distanceVertToNearestZone = distToMax; + } + } else { + geozone.distanceVertToNearestZone = MIN(ABS(distToMin), distToMax); + } + } else { + geozone.distanceVertToNearestZone = 0; + } + + if (currentZoneCount == 0) { + geozone.currentzoneMaxAltitude = 0; + geozone.currentzoneMinAltitude = 0; + } else { + + if (getEstimatedActualPosition(Z) < currentMinAltitude || getEstimatedActualPosition(Z) > currentMaxAltitude) { + aboveOrUnderZone = true; + } + + if (currentMaxAltitude > 0) { + geozone.currentzoneMaxAltitude = currentMaxAltitude - geoZoneConfig()->safeAltitudeDistance; + } else { + geozone.currentzoneMaxAltitude = 0; + } + + if (currentMinAltitude > 0) { + geozone.currentzoneMinAltitude = currentMinAltitude + geoZoneConfig()->safeAltitudeDistance; + } else { + geozone.currentzoneMinAltitude = 0; + } + } + + for (uint8_t i = 0; i < activeGeoZonesCount; i++) + { + // Ignore NFZ for now, we want back fo the FZ, we will check for NFZ later at RTH + if (currentZoneCount == 0 && isAtLeastOneInclusiveZoneActive && activeGeoZones[i].config.type == GEOZONE_TYPE_EXCLUSIVE) { + continue; + } + + if (activeGeoZones[i].config.shape == GEOZONE_SHAPE_POLYGON) { + fpVector2_t* prev = &activeGeoZones[i].verticesLocal[activeGeoZones[i].config.vertexCount - 1]; + fpVector2_t* current = NULL; + for (uint8_t j = 0; j < activeGeoZones[i].config.vertexCount; j++) { + current = &activeGeoZones[i].verticesLocal[j]; + fpVector2_t pos = calcNearestPointOnLine(prev, current, (fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos); + float dist = calculateDistance2((fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, &pos); + if (dist < nearestDistanceToBorder) { + nearestDistanceToBorder = dist; + nearestBorderPoint.x = pos.x; + nearestBorderPoint.y = pos.y; + nearestBorderPoint.z = getEstimatedActualPosition(Z); + geozone.directionToNearestZone = calculateBearingToDestination(&nearestBorderPoint); + geozone.distanceHorToNearestZone = roundf(nearestDistanceToBorder); + nearestInclusiveZone = &activeGeoZones[i]; + } + prev = current; + } + } else if (activeGeoZones[i].config.shape == GEOZONE_SHAPE_CIRCULAR) { + float dist = fabsf(calculateDistance2((fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, &activeGeoZones[i].verticesLocal[0]) - activeGeoZones[i].radius); + if (dist < nearestDistanceToBorder) { + nearestDistanceToBorder = dist; + int32_t directionToBorder = calculateBearingToDestination((fpVector3_t*)&activeGeoZones[i].verticesLocal[0]); + + if (isPointInCircle((fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, &activeGeoZones[i].verticesLocal[0], activeGeoZones[i].radius)) { + directionToBorder = wrap_36000(directionToBorder + 18000); + } + geozone.directionToNearestZone = directionToBorder; + geozone.distanceHorToNearestZone = roundf(dist); + nearestInclusiveZone = &activeGeoZones[i]; + } + } + + if (aboveOrUnderZone && nearestHorZone != NULL && ABS(geozone.distanceVertToNearestZone) < geozone.distanceHorToNearestZone) { + nearestInclusiveZone = nearestHorZone; + geozone.distanceHorToNearestZone = 0; + } + } + + geozone.nearestHorZoneHasAction = nearestHorZone && nearestHorZone->config.fenceAction != GEOFENCE_ACTION_NONE; +} + +void performeFenceAction(geoZoneRuntimeConfig_t *zone, fpVector3_t *intersection) +{ + // Actions only for assisted modes now + if (isNonGeozoneModeFromBoxInput() || geozone.avoidInRTHInProgress || (calculateDistanceToDestination(intersection) > geozoneGetDetectionDistance())) { + return; + } + + int32_t alt = roundf(intersection->z); + if (alt == zone->config.maxAltitude || alt == zone->config.minAltitude) { + return; + } + + fpVector3_t prevPoint; + calcPreviewPoint(&prevPoint, geoZoneConfig()->fenceDetectionDistance); + + if (zone->config.fenceAction == GEOFENCE_ACTION_AVOID) { + bool avoidFzStep = false; + float fzStepAlt = 0; + if (zone->config.type == GEOZONE_TYPE_INCLUSIVE) { + geoZoneRuntimeConfig_t *zones[MAX_GEOZONES_IN_CONFIG]; + uint8_t zoneCount = getZonesForPos(zones, intersection, true); + + float maxAlt = FLT_MAX; + for (uint8_t i = 0; i < zoneCount; i++) { + if (zones[i]->config.type == GEOZONE_TYPE_INCLUSIVE && zones[i]->config.minAltitude > intersection->z) { + float alt = zones[i]->config.minAltitude; + if (alt < maxAlt) { + maxAlt = alt; + } + } + } + + if (maxAlt < FLT_MAX) { + fzStepAlt = maxAlt + geoZoneConfig()->safeAltitudeDistance * 2; + avoidFzStep = true; + } + } + // We can move upwards + if ((zone->config.type == GEOZONE_TYPE_EXCLUSIVE && geozone.zoneInfo > 0 && (geozone.zoneInfo < geoZoneConfig()->avoidAltitudeRange)) || avoidFzStep) { + + calculateFarAwayTarget(&posControl.sendTo.targetPos, posControl.actualState.cog, geoZoneConfig()->fenceDetectionDistance * 2); + if (avoidFzStep) { + posControl.sendTo.targetPos.z = fzStepAlt; + } else { + posControl.sendTo.targetPos.z = zone->config.maxAltitude + geoZoneConfig()->safeAltitudeDistance * 2; + } + + posControl.sendTo.lockSticks = true; + posControl.sendTo.lockStickTime = STICK_LOCK_MIN_TIME; + posControl.sendTo.targetRange = POS_DETECTION_DISTANCE; + posControl.sendTo.altitudeTargetRange = geoZoneConfig()->safeAltitudeDistance / 2; + avoidingPoint = *intersection; + actionState = GEOZONE_ACTION_STATE_AVOIDING_UPWARD; + actionStartTime = millis(); + avoidingPoint = *intersection; + activateSendTo(); + } else { + // Calc new course + avoidCourse = calcBounceCourseForZone(zone, &prevPoint, intersection); + + if (avoidCourse == -1) { + return; + } + + calculateFarAwayTarget(&posControl.sendTo.targetPos, avoidCourse, geoZoneConfig()->fenceDetectionDistance * 2); // Its too far, mode will be abort if we are on the right course + + // Check for min/max altitude + if (geozone.currentzoneMaxAltitude > 0 && getEstimatedActualPosition(Z) > geozone.currentzoneMaxAltitude) { + posControl.sendTo.targetPos.z = geozone.currentzoneMaxAltitude - geoZoneConfig()->safeAltitudeDistance * 0.25; + } else if (geozone.currentzoneMinAltitude != 0 && getEstimatedActualPosition(Z) < geozone.currentzoneMinAltitude) { + posControl.sendTo.targetPos.z = geozone.currentzoneMinAltitude + geoZoneConfig()->safeAltitudeDistance * 0.25; + } + + posControl.sendTo.lockSticks = true; + posControl.sendTo.lockStickTime = AVOID_TIMEOUT; + posControl.sendTo.targetRange = POS_DETECTION_DISTANCE; + posControl.sendTo.altitudeTargetRange = geoZoneConfig()->safeAltitudeDistance / 2; + avoidingPoint = *intersection; + actionState = GEOZONE_ACTION_STATE_AVOIDING; + actionStartTime = millis(); + avoidingPoint = *intersection; + activateSendTo(); + } + } else if (zone->config.fenceAction == GEOFENCE_ACTION_POS_HOLD) { + actionState = GEOZONE_ACTION_STATE_POSHOLD; + + if (STATE(AIRPLANE)) { + if (zone->config.shape == GEOZONE_SHAPE_POLYGON) { + fpVector3_t refPoint; + int32_t course = calcBounceCourseForZone(zone, &prevPoint, intersection); + calculateFarAwayTarget(&refPoint, course, geoZoneConfig()->fenceDetectionDistance * 2); + if (isPointRightFromLine((fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, (fpVector2_t*)&prevPoint, (fpVector2_t*)&refPoint)) { + geozone.loiterDir = 1; // Right + } else { + geozone.loiterDir = -1; // Left + } + } else if (zone->config.shape == GEOZONE_SHAPE_CIRCULAR) { + if (isPointRightFromLine((fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, (fpVector2_t*)&prevPoint, &zone->verticesLocal[0])) { + geozone.loiterDir = -1; // Left + } else { + geozone.loiterDir = 1; // Right + } + } + } + + geozone.sticksLocked = true; + activateForcedPosHold(); + actionStartTime = millis(); + } else if (zone->config.fenceAction == GEOFENCE_ACTION_RTH) { + actionState = GEOZONE_ACTION_STATE_RTH; + geozone.sticksLocked = true; + activateForcedRTH(); + actionStartTime = millis(); + } +} + +static void endFenceAction(void) +{ + posControl.sendTo.lockSticks = false; + geozone.sticksLocked = false; + geozone.sticksLocked = 0; + + switch (actionState) { + case GEOZONE_ACTION_STATE_AVOIDING: + case GEOZONE_ACTION_STATE_AVOIDING_ALTITUDE: + case GEOZONE_ACTION_STATE_FLYOUT_NFZ: + case GEOZONE_ACTION_STATE_RETURN_TO_FZ: + abortSendTo(); + break; + case GEOZONE_ACTION_STATE_POSHOLD: + abortForcedPosHold(); + break; + case GEOZONE_ACTION_STATE_RTH: + abortForcedRTH(); + break; + default: + break; + } + + actionState = GEOZONE_ACTION_STATE_NONE; + + if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCRUISE)){ + setDesiredPosition(&posControl.sendTo.targetPos, 0, NAV_POS_UPDATE_Z); + } + + abortSendTo(); +} + +static void geoZoneInit(void) +{ + activeGeoZonesCount = 0; + uint8_t expectedVertices = 0, configuredVertices = 0; + for (uint8_t i = 0; i < MAX_GEOZONES_IN_CONFIG; i++) + { + if (geoZonesConfig(i)->vertexCount > 0) { + memcpy(&activeGeoZones[activeGeoZonesCount].config, geoZonesConfig(i), sizeof(geoZoneRuntimeConfig_t)); + if (activeGeoZones[i].config.maxAltitude == 0) { + activeGeoZones[i].config.maxAltitude = INT32_MAX; + } + + if (activeGeoZones[i].config.isSealevelRef) { + + if (activeGeoZones[i].config.maxAltitude != 0) { + activeGeoZones[i].config.maxAltitude -= GPS_home.alt; + } + + if (activeGeoZones[i].config.minAltitude != 0) { + activeGeoZones[i].config.minAltitude -= GPS_home.alt; + } + } + + activeGeoZones[i].isInfZone = activeGeoZones[i].config.maxAltitude == INT32_MAX && activeGeoZones[i].config.minAltitude == 0; + + if (!STATE(AIRPLANE) && activeGeoZones[i].config.fenceAction == GEOFENCE_ACTION_AVOID) { + activeGeoZones[i].config.fenceAction = GEOFENCE_ACTION_POS_HOLD; + } + + activeGeoZones[activeGeoZonesCount].enable = true; + activeGeoZonesCount++; + } + expectedVertices += geoZonesConfig(i)->vertexCount; + } + + if (activeGeoZonesCount > 0) { + // Covert geozone vertices to local + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + gpsLocation_t vertexLoc; + fpVector3_t posLocal3; + + if (geoZoneVertices(i)->zoneId >= 0 && geoZoneVertices(i)->zoneId < MAX_GEOZONES_IN_CONFIG && geoZoneVertices(i)->idx <= MAX_VERTICES_IN_CONFIG) { + configuredVertices++; + if (geoZonesConfig(geoZoneVertices(i)->zoneId)->shape == GEOZONE_SHAPE_CIRCULAR && geoZoneVertices(i)->idx == 1) { + activeGeoZones[geoZoneVertices(i)->zoneId].radius = geoZoneVertices(i)->lat; + activeGeoZones[geoZoneVertices(i)->zoneId].config.vertexCount = 1; + continue; + } + + vertexLoc.lat = geoZoneVertices(i)->lat; + vertexLoc.lon = geoZoneVertices(i)->lon; + geoConvertGeodeticToLocal(&posLocal3, &posControl.gpsOrigin, &vertexLoc, GEO_ALT_ABSOLUTE); + + uint8_t vertexIdx = 0; + for (uint8_t j = 0; j < geoZoneVertices(i)->zoneId; j++) { + vertexIdx += activeGeoZones[j].config.vertexCount; + } + vertexIdx += geoZoneVertices(i)->idx; + + verticesLocal[vertexIdx].x = posLocal3.x; + verticesLocal[vertexIdx].y = posLocal3.y; + + if (geoZoneVertices(i)->idx == 0) { + activeGeoZones[geoZoneVertices(i)->zoneId].verticesLocal = &verticesLocal[vertexIdx]; + } + } + } + } + + if (geoZoneConfig()->nearestSafeHomeAsInclusivZone && posControl.safehomeState.index >= 0) + { + safeHomeGeozoneConfig.shape = GEOZONE_SHAPE_CIRCULAR; + safeHomeGeozoneConfig.type = GEOZONE_TYPE_INCLUSIVE; + safeHomeGeozoneConfig.fenceAction = geoZoneConfig()->safeHomeFenceAction; + safeHomeGeozoneConfig.maxAltitude = 0; + safeHomeGeozoneConfig.minAltitude = 0; + safeHomeGeozoneConfig.vertexCount = 1; + + activeGeoZones[activeGeoZonesCount].config = safeHomeGeozoneConfig; + activeGeoZones[activeGeoZonesCount].verticesLocal = (fpVector2_t*)&posControl.safehomeState.nearestSafeHome; + activeGeoZones[activeGeoZonesCount].radius = navConfig()->general.safehome_max_distance; + activeGeoZonesCount++; + expectedVertices++; + configuredVertices++; + } + + updateCurrentZones(); + uint8_t newActiveZoneCount = activeGeoZonesCount; + for (uint8_t i = 0; i < activeGeoZonesCount; i++) { + if (!geozone.insideFz) { + // Deactivate all inclusive geozones with distance > GEOZONE_INCLUSE_IGNORE_DISTANCE + if (activeGeoZones[i].config.type == GEOZONE_TYPE_INCLUSIVE && !isInGeozone(&activeGeoZones[i], &navGetCurrentActualPositionAndVelocity()->pos, false)) { + fpVector2_t pos2 = { .x = getEstimatedActualPosition(X), .y = getEstimatedActualPosition(Y) }; + + uint32_t minDistanceToZone = INT32_MAX; + for (uint8_t j = 0; j < activeGeoZones[i].config.vertexCount; j++) { + float dist = calculateDistance2(&pos2, &activeGeoZones[i].verticesLocal[j]); + if (activeGeoZones[i].config.shape == GEOZONE_SHAPE_CIRCULAR) { + minDistanceToZone = dist - activeGeoZones[i].radius; + break; + } + + if (dist < minDistanceToZone) { + minDistanceToZone = dist; + } + } + if (minDistanceToZone > GEOZONE_INCLUSE_IGNORE_DISTANCE) { + activeGeoZones[i].enable = false; + newActiveZoneCount--; + } + } + } + } + + activeGeoZonesCount = newActiveZoneCount; + if (activeGeoZonesCount == 0 || expectedVertices != configuredVertices) { + setTaskEnabled(TASK_GEOZONE, false); + geozoneIsEnabled = false; + return; + } + geozoneIsEnabled = true; + + qsort(activeGeoZones, MAX_GEOZONES, sizeof(geoZoneRuntimeConfig_t), geoZoneRTComp); + + for (int i = 0; i < activeGeoZonesCount; i++) { + if (activeGeoZones[i].config.type == GEOZONE_TYPE_INCLUSIVE) { + isAtLeastOneInclusiveZoneActive = true; + break; + } + } +} + +// Called by Scheduler +void geozoneUpdate(timeUs_t curentTimeUs) +{ + UNUSED(curentTimeUs); + + geozone.messageState = GEOZONE_MESSAGE_STATE_NONE; + if (!isInitalised && navigationPositionEstimateIsHealthy()) { + geoZoneInit(); + isInitalised = true; + } + + if (!ARMING_FLAG(ARMED) || !isInitalised || activeGeoZonesCount == 0) { + noZoneRTH = false; + return; + } + + // Zone RTH Override: Toggle RTH switch short + if (geozone.avoidInRTHInProgress) { + if (rthHomeSwitchLastState != IS_RC_MODE_ACTIVE(BOXNAVRTH)) { + if (millis() - rthOverrideStartTime < RTH_OVERRIDE_TIMEOUT) { + geozoneResetRTH(); + noZoneRTH = true; + } + rthOverrideStartTime = millis(); + } + rthHomeSwitchLastState = IS_RC_MODE_ACTIVE(BOXNAVRTH); + } + + updateCurrentZones(); + updateZoneInfos(); + + if (STATE(AIRPLANE) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) { + return; + } + + // User has switched to non geofence mode, end all actions and switch to mode from box input + if (isNonGeozoneModeFromBoxInput()) { + if (actionState != GEOZONE_ACTION_STATE_NONE) { + endFenceAction(); + } + lockRTZ = false; + return; + } + + switch (actionState) + { + case GEOZONE_ACTION_STATE_AVOIDING: + if (calculateDistanceToDestination(&avoidingPoint) > geozoneGetDetectionDistance()) { + posControl.sendTo.lockSticks = false; + } + geozone.messageState = GEOZONE_MESSAGE_STATE_AVOIDING_FB; + if (IS_IN_TOLERANCE_RANGE(avoidCourse, DECIDEGREES_TO_CENTIDEGREES(gpsSol.groundCourse), 500) || millis() - actionStartTime > AVOID_TIMEOUT || !posControl.flags.sendToActive) { + endFenceAction(); + } + return; + case GEOZONE_ACTION_STATE_AVOIDING_ALTITUDE: + geozone.messageState = GEOZONE_MESSAGE_STATE_AVOIDING_ALTITUDE_BREACH; + if (IS_IN_TOLERANCE_RANGE(getEstimatedActualPosition(Z), posControl.sendTo.targetPos.z, posControl.sendTo.altitudeTargetRange) || !posControl.flags.sendToActive || millis() - actionStartTime > AVOID_TIMEOUT) { + endFenceAction(); + } + return; + case GEOZONE_ACTION_STATE_RETURN_TO_FZ: + geozone.messageState = GEOZONE_MESSAGE_STATE_RETURN_TO_ZONE; + if ((geozone.insideFz && ABS(geozone.distanceVertToNearestZone ) > geoZoneConfig()->safeAltitudeDistance) || !posControl.flags.sendToActive) { + lockRTZ = true; + endFenceAction(); + } + return; + case GEOZONE_ACTION_STATE_FLYOUT_NFZ: + geozone.messageState = GEOZONE_MESSAGE_STATE_FLYOUT_NFZ; + if (!geozone.insideNfz || !posControl.flags.sendToActive) { + endFenceAction(); + } + return; + case GEOZONE_ACTION_STATE_AVOIDING_UPWARD: + geozone.messageState = GEOZONE_MESSAGE_STATE_AVOIDING_FB; + if (IS_IN_TOLERANCE_RANGE(getEstimatedActualPosition(Z), posControl.sendTo.targetPos.z, posControl.sendTo.altitudeTargetRange) || !posControl.flags.sendToActive || millis() - actionStartTime > AVOID_TIMEOUT) { + endFenceAction(); + } + return; + case GEOZONE_ACTION_STATE_RTH: + case GEOZONE_ACTION_STATE_POSHOLD: + geozone.messageState = GEOZONE_MESSAGE_STATE_POS_HOLD; + if (geozone.sticksLocked && millis() - actionStartTime > STICK_LOCK_MIN_TIME) { + geozone.sticksLocked = false; + } + if (!geozone.sticksLocked && areSticksDeflectdFromChannelValue()) { + endFenceAction(); + } + return; + default: + break; + } + + if ((IS_RC_MODE_ACTIVE(BOXHORIZON) || IS_RC_MODE_ACTIVE(BOXANGLE)) && + actionState == GEOZONE_ACTION_STATE_NONE && + geozone.nearestHorZoneHasAction && + ABS(geozone.distanceVertToNearestZone) > 0 && + ABS(geozone.distanceVertToNearestZone) < geoZoneConfig()->safeAltitudeDistance) { + + float targetAltitide = 0; + uint32_t extraSafteyAlt = geoZoneConfig()->safeAltitudeDistance * 0.25; + if (nearestHorZone->config.type == GEOZONE_TYPE_INCLUSIVE && geozone.insideFz) { + if (geozone.distanceVertToNearestZone > 0) { + targetAltitide = geozone.currentzoneMaxAltitude - extraSafteyAlt; + } else { + targetAltitide = geozone.currentzoneMinAltitude + extraSafteyAlt; + } + } else if (nearestHorZone->config.type == GEOZONE_TYPE_EXCLUSIVE && !geozone.insideNfz) { + if (geozone.distanceVertToNearestZone > 0) { + targetAltitide = geozone.currentzoneMinAltitude - extraSafteyAlt; + } else { + targetAltitide = geozone.currentzoneMaxAltitude + extraSafteyAlt; + } + } + + fpVector3_t targetPos; + calculateFarAwayTarget(&targetPos, posControl.actualState.cog, 100000); + posControl.sendTo.targetPos.x = targetPos.x; + posControl.sendTo.targetPos.y = targetPos.y; + posControl.sendTo.targetPos.z = targetAltitide; + posControl.sendTo.altitudeTargetRange = 200; + posControl.sendTo.targetRange = POS_DETECTION_DISTANCE; + posControl.sendTo.lockSticks = true; + posControl.sendTo.lockStickTime = STICK_LOCK_MIN_TIME; + actionState = GEOZONE_ACTION_STATE_AVOIDING_ALTITUDE; + actionStartTime = millis(); + activateSendTo(); + return; + } + + // RTH active: Further checks are done in RTH logic + if ((navGetCurrentStateFlags() & NAV_AUTO_RTH) || IS_RC_MODE_ACTIVE(BOXNAVRTH) || posControl.flags.forcedRTHActivated || FLIGHT_MODE(NAV_FW_AUTOLAND)) { + return; + } else if (geozone.avoidInRTHInProgress) { + geozoneResetRTH(); + } + + if (lockRTZ && (geozone.insideFz || geozone.insideNfz)) { + lockRTZ = false; + } + + // RTZ: Return to zone: + if (geozone.insideNfz || (!geozone.insideFz && isAtLeastOneInclusiveZoneActive)) { + + if (isAtLeastOneInclusiveZoneActive && !geozone.insideFz) { + geozone.messageState = GEOZONE_MESSAGE_STATE_OUTSIDE_FZ; + } + + if (geozone.insideNfz) { + geozone.messageState = GEOZONE_MESSAGE_STATE_NFZ; + } + + if (!isNonGeozoneModeFromBoxInput()) { + bool flyOutNfz = false; + geoZoneRuntimeConfig_t *targetZone = nearestInclusiveZone; + + for (uint8_t i = 0; i < currentZoneCount; i++) { + if (currentZones[i]->config.type == GEOZONE_TYPE_EXCLUSIVE && currentZones[i]->config.fenceAction != GEOFENCE_ACTION_NONE) { + flyOutNfz = true; + targetZone = currentZones[i]; + break; + } + } + + if (targetZone != NULL && !lockRTZ && (flyOutNfz || (!geozone.insideFz && targetZone->config.fenceAction != GEOFENCE_ACTION_NONE))) { + int32_t targetAltitude = 0; + if (getEstimatedActualPosition(Z) >= targetZone->config.maxAltitude - geoZoneConfig()->safeAltitudeDistance) { + targetAltitude = targetZone->config.maxAltitude - geoZoneConfig()->safeAltitudeDistance * 1.5f; + } else if (getEstimatedActualPosition(Z) <= targetZone->config.minAltitude + geoZoneConfig()->safeAltitudeDistance) { + targetAltitude = targetZone->config.minAltitude + geoZoneConfig()->safeAltitudeDistance * 1.5f; + } else { + targetAltitude = getEstimatedActualPosition(Z); + } + + fpVector3_t targetPos; + if (aboveOrUnderZone) { + if (ABS(geozone.distanceVertToNearestZone) < 2000) { + calculateFarAwayTarget(&targetPos, posControl.actualState.cog, 100000); + if(geozone.distanceVertToNearestZone > 0) { + targetAltitude = getEstimatedActualPosition(Z) + ABS(geozone.distanceVertToNearestZone) + geoZoneConfig()->safeAltitudeDistance * 1.5f; + } else { + targetAltitude = getEstimatedActualPosition(Z) - ABS(geozone.distanceVertToNearestZone) - geoZoneConfig()->safeAltitudeDistance * 1.5f; + } + + } else { + targetPos = navGetCurrentActualPositionAndVelocity()->pos; + } + } else { + calculateFarAwayTarget(&targetPos, geozone.directionToNearestZone, geozone.distanceHorToNearestZone + geoZoneConfig()->fenceDetectionDistance / 2); + } + + posControl.sendTo.targetPos.x = targetPos.x; + posControl.sendTo.targetPos.y = targetPos.y; + posControl.sendTo.targetPos.z = targetAltitude; + posControl.sendTo.altitudeTargetRange = 200; + posControl.sendTo.targetRange = POS_DETECTION_DISTANCE; + posControl.sendTo.lockSticks = true; + posControl.sendTo.lockStickTime = STICK_LOCK_MIN_TIME; + + if (flyOutNfz) { + actionState = GEOZONE_ACTION_STATE_FLYOUT_NFZ; + } else { + actionState = GEOZONE_ACTION_STATE_RETURN_TO_FZ; + } + + activateSendTo(); + } + } + } + + + geoZoneRuntimeConfig_t *intersectZone = NULL; + fpVector3_t intersection, prevPoint; + float distanceToZone = 0; + calcPreviewPoint(&prevPoint, geoZoneConfig()->fenceDetectionDistance); + if (findNearestIntersectionZone(&intersectZone, &intersection, &distanceToZone, geoZoneConfig()->fenceDetectionDistance, &navGetCurrentActualPositionAndVelocity()->pos, &prevPoint)) { + if (geozone.insideFz) { + if (!isPointInAnyOtherZone(intersectZone, GEOZONE_TYPE_INCLUSIVE, &intersection)) { + geozone.distanceToZoneBorder3d = (uint32_t)roundf(distanceToZone); + geozone.messageState = GEOZONE_MESSAGE_STATE_LEAVING_FZ; + performeFenceAction(intersectZone, &intersection); + } + } + + if (!geozone.insideNfz && intersectZone->config.type == GEOZONE_TYPE_EXCLUSIVE && (intersectZone->config.minAltitude != 0 || intersection.z > 0)) { + geozone.distanceToZoneBorder3d = (uint32_t)roundf(distanceToZone); + int32_t minAltitude = intersectZone->config.minAltitude; + int32_t maxAltitude = intersectZone->config.maxAltitude; + if (intersectZone->isInfZone || (minAltitude == 0 && maxAltitude == INT32_MAX)) { + geozone.zoneInfo = INT32_MAX; + } else if (maxAltitude == INT32_MAX) { + geozone.zoneInfo = minAltitude - getEstimatedActualPosition(Z); + } else if (minAltitude == 0) { + geozone.zoneInfo = maxAltitude - getEstimatedActualPosition(Z); + } else { + int32_t distToMax = maxAltitude - getEstimatedActualPosition(Z); + int32_t distToMin = minAltitude - getEstimatedActualPosition(Z); + if (ABS(distToMin) < ABS(distToMax)) { + geozone.zoneInfo = distToMin; + } else { + geozone.zoneInfo = distToMax; + } + } + + geozone.messageState = GEOZONE_MESSAGE_STATE_ENTERING_NFZ; + performeFenceAction(intersectZone, &intersection); + } + } +} + +fpVector3_t *geozoneGetCurrentRthAvoidWaypoint(void) +{ + return &rthWaypoints[rthWaypointIndex]; +} + +void geozoneAdvanceRthAvoidWaypoint(void) +{ + if (rthWaypointIndex < rthWaypointCount) { + rthWaypointIndex++; + } +} + +bool geoZoneIsLastRthWaypoint(void) +{ + return rthWaypointIndex == rthWaypointCount - 1; +} + +void geozoneResetRTH(void) +{ + geozone.avoidInRTHInProgress = false; + rthWaypointIndex = 0; + rthWaypointCount = 0; +} + +void geozoneSetupRTH(void) { + if (!geozone.insideFz && isAtLeastOneInclusiveZoneActive) { + noZoneRTH = true; + } else { + noZoneRTH = false; + } +} + +// Return value +// -1: Unable to calculate a course home +// 0: No NFZ in the way +// >0: Number of waypoints +int8_t geozoneCheckForNFZAtCourse(bool isRTH) +{ + UNUSED(isRTH); + + if (geozone.avoidInRTHInProgress || noZoneRTH || !geozoneIsEnabled || !isInitalised) { + return 0; + } + + updateCurrentZones(); + + // Never mind, lets fly out of the zone on current course + if (geozone.insideNfz || (isAtLeastOneInclusiveZoneActive && !geozone.insideFz)) { + return 0; + } + + int8_t waypointCount = calcRthCourse(rthWaypoints, &navGetCurrentActualPositionAndVelocity()->pos, &posControl.rthState.homePosition.pos); + if (waypointCount > 0) { + rthWaypointCount = waypointCount; + rthWaypointIndex = 0; + geozone.avoidInRTHInProgress = true; + return 1; + } + + return waypointCount; +} + +bool isGeozoneActive(void) +{ + return activeGeoZonesCount > 0; +} + +void geozoneUpdateMaxHomeAltitude(void) { + int32_t altitude = INT32_MIN; + geoZoneRuntimeConfig_t *zones[MAX_GEOZONES]; + uint8_t zoneCount = getZonesForPos(zones, &posControl.rthState.homePosition.pos, false); + for (uint8_t i = 0; i < zoneCount; i++) { + if (zones[i]->config.type == GEOZONE_TYPE_INCLUSIVE) { + altitude = MAX(zones[i]->config.maxAltitude, altitude); + } + } + + if (altitude > INT32_MIN) { + geozone.maxHomeAltitude = altitude - geoZoneConfig()->safeAltitudeDistance; + geozone.homeHasMaxAltitue = true; + } else { + geozone.homeHasMaxAltitue = false; + } +} + +// Avoid arming in NFZ +bool geozoneIsBlockingArming(void) +{ + // Do not generate arming flags unless we are sure about them + if (!isInitalised || !geozoneIsEnabled || activeGeoZonesCount == 0) { + return false; + } + + for (uint8_t i = 0; i < activeGeoZonesCount; i++) { + if (activeGeoZones[i].config.type == GEOZONE_TYPE_EXCLUSIVE && isInGeozone(&activeGeoZones[i], &navGetCurrentActualPositionAndVelocity()->pos, false)) { + return true; + } + } + + for (uint8_t i = 0; i < activeGeoZonesCount; i++) { + if (activeGeoZones[i].config.type == GEOZONE_TYPE_INCLUSIVE && isInGeozone(&activeGeoZones[i], &navGetCurrentActualPositionAndVelocity()->pos, false)) { + return false; + } + } + + // We aren't in any zone, is an inclusive one still active? + for (uint8_t i = 0; i < activeGeoZonesCount; i++) { + if (activeGeoZones[i].config.type == GEOZONE_TYPE_INCLUSIVE) { + return true; + } + } + + return false; +} + +#endif diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index d20087f9e3..e23b6e1f3c 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -56,7 +56,7 @@ navigationPosEstimator_t posEstimator; static float initialBaroAltitudeOffset = 0.0f; -PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 7); +PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 8); PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, // Inertial position estimator parameters @@ -69,6 +69,7 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, .max_surface_altitude = SETTING_INAV_MAX_SURFACE_ALTITUDE_DEFAULT, .w_z_baro_p = SETTING_INAV_W_Z_BARO_P_DEFAULT, + .w_z_baro_v = SETTING_INAV_W_Z_BARO_V_DEFAULT, .w_z_surface_p = SETTING_INAV_W_Z_SURFACE_P_DEFAULT, .w_z_surface_v = SETTING_INAV_W_Z_SURFACE_V_DEFAULT, @@ -89,6 +90,8 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, .max_eph_epv = SETTING_INAV_MAX_EPH_EPV_DEFAULT, .baro_epv = SETTING_INAV_BARO_EPV_DEFAULT, + + .default_alt_sensor = SETTING_INAV_DEFAULT_ALT_SENSOR_DEFAULT, #ifdef USE_GPS_FIX_ESTIMATION .allow_gps_fix_estimation = SETTING_INAV_ALLOW_GPS_FIX_ESTIMATION_DEFAULT #endif @@ -342,6 +345,7 @@ static bool gravityCalibrationComplete(void) return zeroCalibrationIsCompleteS(&posEstimator.imu.gravityCalibration); } + #define ACC_VIB_FACTOR_S 1.0f #define ACC_VIB_FACTOR_E 3.0f static void updateIMUEstimationWeight(const float dt) @@ -423,7 +427,6 @@ static void updateIMUTopic(timeUs_t currentTimeUs) posEstimator.imu.calibratedGravityCMSS = gyroConfig()->gravity_cmss_cal; } - /* If calibration is incomplete - report zero acceleration */ if (gravityCalibrationComplete()) { #ifdef USE_SIMULATOR if (ARMING_FLAG(SIMULATOR_MODE_HITL) || ARMING_FLAG(SIMULATOR_MODE_SITL)) { @@ -432,7 +435,7 @@ static void updateIMUTopic(timeUs_t currentTimeUs) #endif posEstimator.imu.accelNEU.z -= posEstimator.imu.calibratedGravityCMSS; } - else { + else { // If calibration is incomplete - report zero acceleration posEstimator.imu.accelNEU.x = 0.0f; posEstimator.imu.accelNEU.y = 0.0f; posEstimator.imu.accelNEU.z = 0.0f; @@ -476,14 +479,16 @@ static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs) /* Figure out if we have valid position data from our data sources */ uint32_t newFlags = 0; + const float max_eph_epv = positionEstimationConfig()->max_eph_epv; + if ((sensors(SENSOR_GPS) #ifdef USE_GPS_FIX_ESTIMATION || STATE(GPS_ESTIMATED_FIX) #endif ) && posControl.gpsOrigin.valid && ((currentTimeUs - posEstimator.gps.lastUpdateTime) <= MS2US(INAV_GPS_TIMEOUT_MS)) && - (posEstimator.gps.eph < positionEstimationConfig()->max_eph_epv)) { - if (posEstimator.gps.epv < positionEstimationConfig()->max_eph_epv) { + (posEstimator.gps.eph < max_eph_epv)) { + if (posEstimator.gps.epv < max_eph_epv) { newFlags |= EST_GPS_XY_VALID | EST_GPS_Z_VALID; } else { @@ -503,11 +508,11 @@ static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs) newFlags |= EST_FLOW_VALID; } - if (posEstimator.est.eph < positionEstimationConfig()->max_eph_epv) { + if (posEstimator.est.eph < max_eph_epv) { newFlags |= EST_XY_VALID; } - if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) { + if (posEstimator.est.epv < max_eph_epv) { newFlags |= EST_Z_VALID; } @@ -521,7 +526,9 @@ static void estimationPredict(estimationContext_t * ctx) if ((ctx->newFlags & EST_Z_VALID)) { posEstimator.est.pos.z += posEstimator.est.vel.z * ctx->dt; posEstimator.est.pos.z += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f; - posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt; + if (ARMING_FLAG(WAS_EVER_ARMED)) { // Hold at zero until first armed + posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt; + } } /* Prediction step: XY-axis */ @@ -552,20 +559,28 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount()); // Clip count bool correctOK = false; + const uint8_t defaultAltitudeSource = positionEstimationConfig()->default_alt_sensor; + float wGps = defaultAltitudeSource == ALTITUDE_SOURCE_BARO_ONLY && ctx->newFlags & EST_BARO_VALID ? 0.0f : 1.0f; + float wBaro = defaultAltitudeSource == ALTITUDE_SOURCE_GPS_ONLY && ctx->newFlags & EST_GPS_Z_VALID ? 0.0f : 1.0f; - float wBaro = 0.0f; - if (ctx->newFlags & EST_BARO_VALID) { - // Ignore baro if difference is too big, baro is probably wrong - const float gpsBaroResidual = ctx->newFlags & EST_GPS_Z_VALID ? fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt) : 0.0f; + if (wBaro && ctx->newFlags & EST_BARO_VALID && wGps && ctx->newFlags & EST_GPS_Z_VALID) { + const float gpsBaroResidual = fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt); - // Fade out the baro to prevent sudden jump - const float start_epv = positionEstimationConfig()->max_eph_epv; - const float end_epv = positionEstimationConfig()->max_eph_epv * 2.0f; + // Fade out the non default sensor to prevent sudden jump + uint16_t residualErrorEpvLimit = defaultAltitudeSource == ALTITUDE_SOURCE_BARO ? 2 * positionEstimationConfig()->baro_epv : positionEstimationConfig()->max_eph_epv; + const float start_epv = residualErrorEpvLimit; + const float end_epv = residualErrorEpvLimit * 2.0f; + + // Calculate residual gps/baro sensor weighting based on assumed default altitude source = GPS wBaro = scaleRangef(constrainf(gpsBaroResidual, start_epv, end_epv), start_epv, end_epv, 1.0f, 0.0f); + + if (defaultAltitudeSource == ALTITUDE_SOURCE_BARO) { // flip residual sensor weighting if default = BARO + wGps = wBaro; + wBaro = 1.0f; + } } - // Always use Baro if no GPS otherwise only use if accuracy OK compared to GPS - if (wBaro > 0.01f) { + if (ctx->newFlags & EST_BARO_VALID && wBaro) { timeUs_t currentTimeUs = micros(); if (!ARMING_FLAG(ARMED)) { @@ -588,21 +603,25 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) ((ctx->newFlags & EST_BARO_VALID) && posEstimator.state.isBaroGroundValid && posEstimator.baro.alt < posEstimator.state.baroGroundAlt)); // Altitude - const float baroAltResidual = (isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z; - ctx->estPosCorr.z += wBaro * baroAltResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt; - ctx->estVelCorr.z += wBaro * baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p) * ctx->dt; + const float baroAltResidual = wBaro * ((isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z); + const float baroVelZResidual = isAirCushionEffectDetected ? 0.0f : wBaro * (posEstimator.baro.baroAltRate - posEstimator.est.vel.z); + const float w_z_baro_p = positionEstimationConfig()->w_z_baro_p; - ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, posEstimator.baro.epv, positionEstimationConfig()->w_z_baro_p); + ctx->estPosCorr.z += baroAltResidual * w_z_baro_p * ctx->dt; + ctx->estVelCorr.z += baroAltResidual * sq(w_z_baro_p) * ctx->dt; + ctx->estVelCorr.z += baroVelZResidual * positionEstimationConfig()->w_z_baro_v * ctx->dt; + + ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, posEstimator.baro.epv, w_z_baro_p); // Accelerometer bias if (!isAirCushionEffectDetected) { - ctx->accBiasCorr.z -= wBaro * baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p); + ctx->accBiasCorr.z -= baroAltResidual * sq(w_z_baro_p); } - correctOK = true; + correctOK = ARMING_FLAG(WAS_EVER_ARMED); // No correction until first armed } - if (ctx->newFlags & EST_GPS_Z_VALID) { + if (ctx->newFlags & EST_GPS_Z_VALID && wGps) { // Reset current estimate to GPS altitude if estimate not valid if (!(ctx->newFlags & EST_Z_VALID)) { ctx->estPosCorr.z += posEstimator.gps.pos.z - posEstimator.est.pos.z; @@ -611,19 +630,20 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) } else { // Altitude - const float gpsAltResudual = posEstimator.gps.pos.z - posEstimator.est.pos.z; - const float gpsVelZResudual = posEstimator.gps.vel.z - posEstimator.est.vel.z; + const float gpsAltResidual = wGps * (posEstimator.gps.pos.z - posEstimator.est.pos.z); + const float gpsVelZResidual = wGps * (posEstimator.gps.vel.z - posEstimator.est.vel.z); + const float w_z_gps_p = positionEstimationConfig()->w_z_gps_p; - ctx->estPosCorr.z += gpsAltResudual * positionEstimationConfig()->w_z_gps_p * ctx->dt; - ctx->estVelCorr.z += gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p) * ctx->dt; - ctx->estVelCorr.z += gpsVelZResudual * positionEstimationConfig()->w_z_gps_v * ctx->dt; - ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, fabsf(gpsAltResudual)), positionEstimationConfig()->w_z_gps_p); + ctx->estPosCorr.z += gpsAltResidual * w_z_gps_p * ctx->dt; + ctx->estVelCorr.z += gpsAltResidual * sq(w_z_gps_p) * ctx->dt; + ctx->estVelCorr.z += gpsVelZResidual * positionEstimationConfig()->w_z_gps_v * ctx->dt; + ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, fabsf(gpsAltResidual)), w_z_gps_p); // Accelerometer bias - ctx->accBiasCorr.z -= gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p); + ctx->accBiasCorr.z -= gpsAltResidual * sq(w_z_gps_p); } - correctOK = true; + correctOK = ARMING_FLAG(WAS_EVER_ARMED); // No correction until first armed } return correctOK; @@ -700,21 +720,23 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) { estimationContext_t ctx; + const float max_eph_epv = positionEstimationConfig()->max_eph_epv; + /* Calculate dT */ ctx.dt = US2S(currentTimeUs - posEstimator.est.lastUpdateTime); posEstimator.est.lastUpdateTime = currentTimeUs; /* If IMU is not ready we can't estimate anything */ if (!isImuReady()) { - posEstimator.est.eph = positionEstimationConfig()->max_eph_epv + 0.001f; - posEstimator.est.epv = positionEstimationConfig()->max_eph_epv + 0.001f; + posEstimator.est.eph = max_eph_epv + 0.001f; + posEstimator.est.epv = max_eph_epv + 0.001f; posEstimator.flags = 0; return; } /* Calculate new EPH and EPV for the case we didn't update postion */ - ctx.newEPH = posEstimator.est.eph * ((posEstimator.est.eph <= positionEstimationConfig()->max_eph_epv) ? 1.0f + ctx.dt : 1.0f); - ctx.newEPV = posEstimator.est.epv * ((posEstimator.est.epv <= positionEstimationConfig()->max_eph_epv) ? 1.0f + ctx.dt : 1.0f); + ctx.newEPH = posEstimator.est.eph * ((posEstimator.est.eph <= max_eph_epv) ? 1.0f + ctx.dt : 1.0f); + ctx.newEPV = posEstimator.est.epv * ((posEstimator.est.epv <= max_eph_epv) ? 1.0f + ctx.dt : 1.0f); ctx.newFlags = calculateCurrentValidityFlags(currentTimeUs); vectorZero(&ctx.estPosCorr); vectorZero(&ctx.estVelCorr); @@ -737,12 +759,12 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) estimationCalculateCorrection_XY_FLOW(&ctx); // If we can't apply correction or accuracy is off the charts - decay velocity to zero - if (!estXYCorrectOk || ctx.newEPH > positionEstimationConfig()->max_eph_epv) { + if (!estXYCorrectOk || ctx.newEPH > max_eph_epv) { ctx.estVelCorr.x = (0.0f - posEstimator.est.vel.x) * positionEstimationConfig()->w_xy_res_v * ctx.dt; ctx.estVelCorr.y = (0.0f - posEstimator.est.vel.y) * positionEstimationConfig()->w_xy_res_v * ctx.dt; } - if (!estZCorrectOk || ctx.newEPV > positionEstimationConfig()->max_eph_epv) { + if (!estZCorrectOk || ctx.newEPV > max_eph_epv) { ctx.estVelCorr.z = (0.0f - posEstimator.est.vel.z) * positionEstimationConfig()->w_z_res_v * ctx.dt; } // Boost the corrections based on accWeight @@ -754,16 +776,17 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) vectorAdd(&posEstimator.est.vel, &posEstimator.est.vel, &ctx.estVelCorr); /* Correct accelerometer bias */ - if (positionEstimationConfig()->w_acc_bias > 0.0f) { + const float w_acc_bias = positionEstimationConfig()->w_acc_bias; + if (w_acc_bias > 0.0f) { const float accelBiasCorrMagnitudeSq = sq(ctx.accBiasCorr.x) + sq(ctx.accBiasCorr.y) + sq(ctx.accBiasCorr.z); if (accelBiasCorrMagnitudeSq < sq(INAV_ACC_BIAS_ACCEPTANCE_VALUE)) { /* transform error vector from NEU frame to body frame */ imuTransformVectorEarthToBody(&ctx.accBiasCorr); /* Correct accel bias */ - posEstimator.imu.accelBias.x += ctx.accBiasCorr.x * positionEstimationConfig()->w_acc_bias * ctx.dt; - posEstimator.imu.accelBias.y += ctx.accBiasCorr.y * positionEstimationConfig()->w_acc_bias * ctx.dt; - posEstimator.imu.accelBias.z += ctx.accBiasCorr.z * positionEstimationConfig()->w_acc_bias * ctx.dt; + posEstimator.imu.accelBias.x += ctx.accBiasCorr.x * w_acc_bias * ctx.dt; + posEstimator.imu.accelBias.y += ctx.accBiasCorr.y * w_acc_bias * ctx.dt; + posEstimator.imu.accelBias.z += ctx.accBiasCorr.z * w_acc_bias * ctx.dt; } } diff --git a/src/main/navigation/navigation_pos_estimator_private.h b/src/main/navigation/navigation_pos_estimator_private.h index 44f0333d14..5cbdc81c03 100644 --- a/src/main/navigation/navigation_pos_estimator_private.h +++ b/src/main/navigation/navigation_pos_estimator_private.h @@ -146,6 +146,13 @@ typedef enum { EST_Z_VALID = (1 << 6), } navPositionEstimationFlags_e; +typedef enum { + ALTITUDE_SOURCE_GPS, + ALTITUDE_SOURCE_BARO, + ALTITUDE_SOURCE_GPS_ONLY, + ALTITUDE_SOURCE_BARO_ONLY, +} navDefaultAltitudeSensor_e; + typedef struct { timeUs_t baroGroundTimeout; float baroGroundAlt; diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 6d7b386cbd..fe277a0061 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -113,6 +113,11 @@ typedef struct navigationFlags_s { bool rthTrackbackActive; // Activation status of RTH trackback bool wpTurnSmoothingActive; // Activation status WP turn smoothing bool manualEmergLandActive; // Activation status of manual emergency landing + +#ifdef USE_GEOZONE + bool sendToActive; + bool forcedPosholdActive; +#endif } navigationFlags_t; typedef struct { @@ -160,6 +165,7 @@ typedef enum { NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ, NAV_FSM_EVENT_SWITCH_TO_MIXERAT, NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING, + NAV_FSM_EVENT_SWITCH_TO_SEND_TO, NAV_FSM_EVENT_STATE_SPECIFIC_1, // State-specific event NAV_FSM_EVENT_STATE_SPECIFIC_2, // State-specific event @@ -245,6 +251,10 @@ typedef enum { NAV_PERSISTENT_ID_FW_LANDING_FLARE = 46, NAV_PERSISTENT_ID_FW_LANDING_ABORT = 47, NAV_PERSISTENT_ID_FW_LANDING_FINISHED = 48, + + NAV_PERSISTENT_ID_SEND_TO_INITALIZE = 49, + NAV_PERSISTENT_ID_SEND_TO_IN_PROGRES = 50, + NAV_PERSISTENT_ID_SEND_TO_FINISHED = 51 } navigationPersistentId_e; typedef enum { @@ -304,6 +314,10 @@ typedef enum { NAV_STATE_MIXERAT_IN_PROGRESS, NAV_STATE_MIXERAT_ABORT, + NAV_STATE_SEND_TO_INITALIZE, + NAV_STATE_SEND_TO_IN_PROGESS, + NAV_STATE_SEND_TO_FINISHED, + NAV_STATE_COUNT, } navigationFSMState_t; @@ -406,6 +420,17 @@ typedef enum { RTH_HOME_FINAL_LAND, // Home position and altitude } rthTargetMode_e; +#ifdef USE_GEOZONE +typedef struct navSendTo_s { + fpVector3_t targetPos; + uint16_t altitudeTargetRange; // 0 for only "2D" + uint32_t targetRange; + bool lockSticks; + uint32_t lockStickTime; + timeMs_t startTime; +} navSendTo_t; +#endif + typedef struct { fpVector3_t nearestSafeHome; // The nearestSafeHome found during arming uint32_t distance; // distance to the nearest safehome @@ -478,6 +503,10 @@ typedef struct { fwLandState_t fwLandState; #endif +#ifdef USE_GEOZONE + navSendTo_t sendTo; // Used for Geozones +#endif + /* Internals & statistics */ int16_t rcAdjustment[4]; float totalTripDistance; @@ -502,7 +531,9 @@ const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void); bool isThrustFacingDownwards(void); uint32_t calculateDistanceToDestination(const fpVector3_t * destinationPos); +void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t bearing, int32_t distance); int32_t calculateBearingToDestination(const fpVector3_t * destinationPos); +float calculateDistance2(const fpVector2_t* startPos, const fpVector2_t* destinationPos); bool isLandingDetected(void); void resetLandingDetector(void); diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 594db21417..4143664616 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -294,48 +294,53 @@ static int logicConditionCompute( return true; break; #endif - case LOGIC_CONDITION_SET_VTX_POWER_LEVEL: -#if defined(USE_VTX_CONTROL) -#if(defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP)) - if ( - logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] != operandA && - vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability) - ) { - logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] = constrain(operandA, VTX_SETTINGS_MIN_POWER, vtxDeviceCapability.powerCount); - vtxSettingsConfigMutable()->power = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL]; - return logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL]; - } else { - return false; - } - break; -#else - return false; -#endif +#if defined(USE_VTX_CONTROL) + case LOGIC_CONDITION_SET_VTX_POWER_LEVEL: + { + uint8_t newPower = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL]; + if ((newPower != operandA || newPower != vtxSettingsConfig()->power) && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { + newPower = constrain(operandA, VTX_SETTINGS_MIN_POWER, vtxDeviceCapability.powerCount); + logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] = newPower; + if (newPower != vtxSettingsConfig()->power) { + vtxCommonSetPowerByIndex(vtxCommonDevice(), newPower); // Force setting if modified elsewhere + } + vtxSettingsConfigMutable()->power = newPower; + return newPower; + } + return false; + break; + } case LOGIC_CONDITION_SET_VTX_BAND: - if ( - logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND] != operandA && - vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability) - ) { - logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND] = constrain(operandA, VTX_SETTINGS_MIN_BAND, VTX_SETTINGS_MAX_BAND); - vtxSettingsConfigMutable()->band = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND]; - return logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND]; - } else { - return false; + { + uint8_t newBand = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND]; + if ((newBand != operandA || newBand != vtxSettingsConfig()->band) && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { + newBand = constrain(operandA, VTX_SETTINGS_MIN_BAND, vtxDeviceCapability.bandCount); + logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND] = newBand; + if (newBand != vtxSettingsConfig()->band) { + vtxCommonSetBandAndChannel(vtxCommonDevice(), newBand, vtxSettingsConfig()->channel); + } + vtxSettingsConfigMutable()->band = newBand; + return newBand; } + return false; break; + } case LOGIC_CONDITION_SET_VTX_CHANNEL: - if ( - logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL] != operandA && - vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability) - ) { - logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL] = constrain(operandA, VTX_SETTINGS_MIN_CHANNEL, VTX_SETTINGS_MAX_CHANNEL); - vtxSettingsConfigMutable()->channel = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL]; - return logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL]; - } else { - return false; + { + uint8_t newChannel = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL]; + if ((newChannel != operandA || newChannel != vtxSettingsConfig()->channel) && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { + newChannel = constrain(operandA, VTX_SETTINGS_MIN_CHANNEL, vtxDeviceCapability.channelCount); + logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL] = newChannel; + if (newChannel != vtxSettingsConfig()->channel) { + vtxCommonSetBandAndChannel(vtxCommonDevice(), vtxSettingsConfig()->band, newChannel); + } + vtxSettingsConfigMutable()->channel = newChannel; + return newChannel; } + return false; break; + } #endif case LOGIC_CONDITION_INVERT_ROLL: LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_ROLL); @@ -796,20 +801,36 @@ static int logicConditionGetFlightOperandValue(int operand) { return constrain(calc_length_pythagorean_2D(GPS_distanceToHome, getEstimatedActualPosition(Z) / 100.0f), 0, INT32_MAX); break; - case LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_LQ: - #ifdef USE_SERIALRX_CRSF + case LOGIC_CONDITION_OPERAND_FLIGHT_LQ_UPLINK: +#if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP) return rxLinkStatistics.uplinkLQ; - #else +#else return 0; - #endif +#endif break; - case LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_SNR: - #ifdef USE_SERIALRX_CRSF - return rxLinkStatistics.uplinkSNR; - #else + case LOGIC_CONDITION_OPERAND_FLIGHT_UPLINK_RSSI_DBM: +#if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP) + return rxLinkStatistics.uplinkRSSI; +#else return 0; - #endif +#endif + break; + +case LOGIC_CONDITION_OPERAND_FLIGHT_LQ_DOWNLINK: +#if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP) + return rxLinkStatistics.downlinkLQ; +#else + return 0; +#endif + break; + + case LOGIC_CONDITION_OPERAND_FLIGHT_SNR: +#if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP) + return rxLinkStatistics.uplinkSNR; +#else + return 0; +#endif break; case LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE: // int diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 74a7765be4..f5653bb68c 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -129,8 +129,8 @@ typedef enum { LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_PITCH, // 26 LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW, // 27 LOGIC_CONDITION_OPERAND_FLIGHT_3D_HOME_DISTANCE, // 28 - LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_LQ, // 29 - LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_SNR, // 39 + LOGIC_CONDITION_OPERAND_FLIGHT_LQ_UPLINK, // 29 + LOGIC_CONDITION_OPERAND_FLIGHT_SNR, // 39 LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID, // 0/1 // 31 LOGIC_CONDITION_OPERAND_FLIGHT_LOITER_RADIUS, // 32 LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE, //int // 33 @@ -144,6 +144,8 @@ typedef enum { LOGIC_CONDITION_OPERAND_FLIGHT_FW_LAND_STATE, // 41 LOGIC_CONDITION_OPERAND_FLIGHT_BATT_PROFILE, // int // 42 LOGIC_CONDITION_OPERAND_FLIGHT_FLOWN_LOITER_RADIUS, // 43 + LOGIC_CONDITION_OPERAND_FLIGHT_LQ_DOWNLINK, // 44 + LOGIC_CONDITION_OPERAND_FLIGHT_UPLINK_RSSI_DBM, // 45 } logicFlightOperands_e; typedef enum { diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index f941eaebe2..00ed23ab25 100755 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -572,6 +572,18 @@ static void setRSSIValue(uint16_t rssiValue, rssiSource_e source, bool filtered) rssi = constrain(scaleRange(rssi, rssiMin, rssiMax, 0, RSSI_MAX_VALUE), 0, RSSI_MAX_VALUE); } +void setRSSIFromMSP_RC(uint8_t newMspRssi) +{ + if (activeRssiSource == RSSI_SOURCE_NONE && (rxConfig()->rssi_source == RSSI_SOURCE_MSP || rxConfig()->rssi_source == RSSI_SOURCE_AUTO)) { + activeRssiSource = RSSI_SOURCE_MSP; + } + + if (activeRssiSource == RSSI_SOURCE_MSP) { + rssi = constrain(scaleRange(constrain(newMspRssi, 0, 100), 0, 100, 0, RSSI_MAX_VALUE), 0, RSSI_MAX_VALUE); + lastMspRssiUpdateUs = micros(); + } +} + void setRSSIFromMSP(uint8_t newMspRssi) { if (activeRssiSource == RSSI_SOURCE_NONE && (rxConfig()->rssi_source == RSSI_SOURCE_MSP || rxConfig()->rssi_source == RSSI_SOURCE_AUTO)) { diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index c841838a5e..64b97b172e 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -181,12 +181,16 @@ typedef enum { } rssiSource_e; typedef struct rxLinkStatistics_s { - int16_t uplinkRSSI; // RSSI value in dBm - uint8_t uplinkLQ; // A protocol specific measure of the link quality in [0..100] - int8_t uplinkSNR; // The SNR of the uplink in dB - uint8_t rfMode; // A protocol specific measure of the transmission bandwidth [2 = 150Hz, 1 = 50Hz, 0 = 4Hz] - uint16_t uplinkTXPower; // power in mW - uint8_t activeAntenna; + int16_t uplinkRSSI; // RSSI value in dBm + uint8_t uplinkLQ; // A protocol specific measure of the link quality in [0..100] + uint8_t downlinkLQ; // A protocol specific measure of the link quality in [0..100] + int8_t uplinkSNR; // The SNR of the uplink in dB + uint8_t rfMode; // A protocol specific measure of the transmission bandwidth [2 = 150Hz, 1 = 50Hz, 0 = 4Hz] + uint16_t uplinkTXPower; // power in mW + uint16_t downlinkTXPower; // power in mW + uint8_t activeAntenna; + char band[4]; + char mode[6]; } rxLinkStatistics_t; typedef uint16_t (*rcReadRawDataFnPtr)(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); // used by receiver driver to return channel data @@ -212,6 +216,7 @@ bool isRxPulseValid(uint16_t pulseDuration); uint8_t calculateChannelRemapping(const uint8_t *channelMap, uint8_t channelMapEntryCount, uint8_t channelToRemap); void parseRcChannels(const char *input); +void setRSSIFromMSP_RC(uint8_t newMspRssi); void setRSSIFromMSP(uint8_t newMspRssi); void updateRSSI(timeUs_t currentTimeUs); // Returns RSSI in [0, RSSI_MAX_VALUE] range. diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 176812d8f3..5d38a6dba5 100755 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -137,6 +137,10 @@ typedef enum { TASK_TELEMETRY_SBUS2, #endif +#if defined (USE_GEOZONE) && defined(USE_GPS) + TASK_GEOZONE, +#endif + /* Count of real tasks */ TASK_COUNT, diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index a29fa2e07c..e9167e0912 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -351,16 +351,23 @@ static void checkBatteryCapacityState(void) void batteryUpdate(timeUs_t timeDelta) { + static timeUs_t batteryConnectedTime = 0; /* battery has just been connected*/ if (batteryState == BATTERY_NOT_PRESENT && vbat > VBATT_PRESENT_THRESHOLD) { + if(batteryConnectedTime == 0) { + batteryConnectedTime = micros(); + return; + } + + /* wait for VBatt to stabilise then we can calc number of cells + (using the filtered value takes a long time to ramp up) + Blocking can cause issues with some ESCs */ + if((micros() - batteryConnectedTime) < VBATT_STABLE_DELAY) { + return; + } /* Actual battery state is calculated below, this is really BATTERY_PRESENT */ batteryState = BATTERY_OK; - /* wait for VBatt to stabilise then we can calc number of cells - (using the filtered value takes a long time to ramp up) - We only do this on the ground so don't care if we do block, not - worse than original code anyway*/ - delay(VBATT_STABLE_DELAY); updateBatteryVoltage(timeDelta, true); int8_t detectedProfileIndex = -1; @@ -396,6 +403,7 @@ void batteryUpdate(timeUs_t timeDelta) /* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of VBATT_PRESENT_THRESHOLD */ if (batteryState != BATTERY_NOT_PRESENT && vbat <= VBATT_PRESENT_THRESHOLD) { batteryState = BATTERY_NOT_PRESENT; + batteryConnectedTime = 0; batteryCellCount = 0; batteryWarningVoltage = 0; batteryCriticalVoltage = 0; diff --git a/src/main/target/AETH743Basic/CMakeLists.txt b/src/main/target/AETH743Basic/CMakeLists.txt new file mode 100644 index 0000000000..f70f3feb42 --- /dev/null +++ b/src/main/target/AETH743Basic/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32h743xi(AETH743Basic) diff --git a/src/main/target/AETH743Basic/config.c b/src/main/target/AETH743Basic/config.c new file mode 100644 index 0000000000..6e8b960512 --- /dev/null +++ b/src/main/target/AETH743Basic/config.c @@ -0,0 +1,36 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include + +#include "platform.h" + +#include "fc/fc_msp_box.h" +#include "fc/config.h" + +#include "io/piniobox.h" +#include "io/serial.h" + + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART7)].functionMask = FUNCTION_MSP; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART7)].msp_baudrateIndex = BAUD_115200; + beeperConfigMutable()->pwmMode = true; +} diff --git a/src/main/target/AETH743Basic/target.c b/src/main/target/AETH743Basic/target.c new file mode 100644 index 0000000000..4a18ed1569 --- /dev/null +++ b/src/main/target/AETH743Basic/target.c @@ -0,0 +1,53 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/pinio.h" +#include "drivers/sensor.h" + +BUSDEV_REGISTER_SPI_TAG(busdev_icm42688_0, DEVHW_ICM42605, ICM42688_0_SPI_BUS, ICM42688_0_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_ICM42688_0_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_icm42688_1, DEVHW_ICM42605, ICM42688_1_SPI_BUS, ICM42688_1_CS_PIN, NONE, 2, DEVFLAGS_NONE, IMU_ICM42688_1_ALIGN); + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 1), // S2 + + DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 2), // S3 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 3), // S4 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 4), // S5 + DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 5), // S6 + + DEF_TIM(TIM4, CH1, PD12, TIM_USE_OUTPUT_AUTO, 0, 6), // S7 + DEF_TIM(TIM4, CH2, PD13, TIM_USE_OUTPUT_AUTO, 0, 7), // S8 + DEF_TIM(TIM4, CH3, PD14, TIM_USE_OUTPUT_AUTO, 0, 0), // S9 + DEF_TIM(TIM4, CH4, PD15, TIM_USE_OUTPUT_AUTO, 0, 0), // S10 DMA_NONE + + DEF_TIM(TIM15, CH1, PE5, TIM_USE_OUTPUT_AUTO, 0, 0), // S11 + DEF_TIM(TIM15, CH2, PE6, TIM_USE_OUTPUT_AUTO, 0, 0), // S12 DMA_NONE + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 9), // LED_2812 + DEF_TIM(TIM2, CH1, PA15, TIM_USE_BEEPER, 0, 0), // BEEPER PWM +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/AETH743Basic/target.h b/src/main/target/AETH743Basic/target.h new file mode 100644 index 0000000000..2271ae621c --- /dev/null +++ b/src/main/target/AETH743Basic/target.h @@ -0,0 +1,181 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "H743" + +#define USBD_PRODUCT_STRING "AETH743Basic" + +#define USE_TARGET_CONFIG + +#define LED0 PE3 +#define LED1 PE4 + +#define BEEPER PA15 +#define BEEPER_INVERTED +#define BEEPER_PWM_FREQUENCY 2000 + +// *************** IMU generic *********************** +#define USE_DUAL_GYRO +#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS +#define USE_SPI +#define USE_IMU_ICM42605 + +// *************** SPI1 IMU0 ICM42688 **************** +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PD7 + +#define IMU_ICM42688_0_ALIGN CW0_DEG_FLIP +#define ICM42688_0_SPI_BUS BUS_SPI1 +#define ICM42688_0_CS_PIN PC15 + +// *************** SPI4 IMU1 ICM42688 ************** +#define USE_SPI_DEVICE_4 +#define SPI4_SCK_PIN PE12 +#define SPI4_MISO_PIN PE13 +#define SPI4_MOSI_PIN PE14 + +#define IMU_ICM42688_1_ALIGN CW90_DEG_FLIP +#define ICM42688_1_SPI_BUS BUS_SPI4 +#define ICM42688_1_CS_PIN PC13 + +// *************** SPI2 OSD *********************** +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN PB12 + +// *************** I2C /Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB6 +#define I2C1_SDA PB7 + +#define USE_I2C_DEVICE_2 +#define I2C2_SCL PB10 +#define I2C2_SDA PB11 + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C2 +#define USE_BARO_SPL06 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_ALL + +#define TEMPERATURE_I2C_BUS BUS_I2C2 +#define PITOT_I2C_BUS BUS_I2C2 + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS BUS_I2C1 + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PD5 +#define UART2_RX_PIN PD6 + +#define USE_UART3 +#define UART3_TX_PIN PD8 +#define UART3_RX_PIN PD9 + +#define USE_UART4 +#define UART4_TX_PIN PB9 +#define UART4_RX_PIN PB8 + +#define USE_UART6 +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define USE_UART7 +#define UART7_TX_PIN PE8 +#define UART7_RX_PIN PE7 + +#define USE_UART8 +#define UART8_TX_PIN PE1 +#define UART8_RX_PIN PE0 + +#define SERIAL_PORT_COUNT 8 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define SERIALRX_UART SERIAL_PORT_USART6 + +#define GPS_UART SERIAL_PORT_USART2 + +// *************** SDIO SD BLACKBOX******************* +#define USE_SDCARD +#define USE_SDCARD_SDIO +#define SDCARD_SDIO_DEVICE SDIODEV_1 +#define SDCARD_SDIO_4BIT + +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 + +#define ADC_CHANNEL_1_PIN PC0 //ADC123 VBAT1 +#define ADC_CHANNEL_2_PIN PC1 //ADC123 CURR1 +#define ADC_CHANNEL_3_PIN PC5 //ADC12 RSSI +#define ADC_CHANNEL_4_PIN PC4 //ADC12 AirS +#define ADC_CHANNEL_5_PIN PA4 //ADC12 VB2 +#define ADC_CHANNEL_6_PIN PA7 //ADC12 CU2 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 +#define AIRSPEED_ADC_CHANNEL ADC_CHN_4 + +// *************** PINIO *************************** +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PD10 // 9Vsw +#define PINIO2_PIN PD11 // Camera switcher + +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PA8 + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) +#define CURRENT_METER_SCALE 160 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA (0xffff & ~(BIT(14) | BIT(13))) +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff + +#define MAX_PWM_OUTPUT_PORTS 15 +#define USE_DSHOT +#define USE_ESC_SENSOR + diff --git a/src/main/target/GEPRC_TAKER_H743/CMakeLists.txt b/src/main/target/GEPRC_TAKER_H743/CMakeLists.txt new file mode 100644 index 0000000000..f7c2c61480 --- /dev/null +++ b/src/main/target/GEPRC_TAKER_H743/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32h743xi(GEPRC_TAKER_H743) \ No newline at end of file diff --git a/src/main/target/GEPRC_TAKER_H743/config.c b/src/main/target/GEPRC_TAKER_H743/config.c new file mode 100644 index 0000000000..d2f972270a --- /dev/null +++ b/src/main/target/GEPRC_TAKER_H743/config.c @@ -0,0 +1,63 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include +#include + +#include + +#include "common/axis.h" + +#include "config/config_master.h" +#include "config/feature.h" + +#include "drivers/sensor.h" +#include "drivers/pwm_esc_detect.h" +#include "drivers/pwm_output.h" +#include "drivers/serial.h" + +#include "fc/rc_controls.h" + +#include "flight/failsafe.h" +#include "flight/mixer.h" +#include "flight/pid.h" + +#include "rx/rx.h" + +#include "io/serial.h" + +#include "sensors/battery.h" +#include "sensors/sensors.h" + +#include "telemetry/telemetry.h" + +#include "fc/fc_msp_box.h" + +#include "io/piniobox.h" + + +#define BLUETOOTH_MSP_BAUDRATE BAUD_115200 + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = BOXARM; + pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER1; + + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART3)].functionMask = FUNCTION_MSP; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART3)].msp_baudrateIndex = BLUETOOTH_MSP_BAUDRATE; +} + diff --git a/src/main/target/GEPRC_TAKER_H743/target.c b/src/main/target/GEPRC_TAKER_H743/target.c new file mode 100644 index 0000000000..a04e65b2d5 --- /dev/null +++ b/src/main/target/GEPRC_TAKER_H743/target.c @@ -0,0 +1,52 @@ +/* +* 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. +* +* INAV is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with INAV. If not, see . +*/ + +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/sensor.h" + + +BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, IMU1_SPI_BUS, IMU1_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU1_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000_2, DEVHW_MPU6000, IMU2_SPI_BUS, IMU2_CS_PIN, NONE, 1, DEVFLAGS_NONE, IMU2_ALIGN); + +BUSDEV_REGISTER_SPI_TAG(busdev_icm42688, DEVHW_MPU6000, IMU1_SPI_BUS, IMU1_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU1_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_icm42688_2, DEVHW_ICM42605, IMU2_SPI_BUS, IMU2_CS_PIN, NONE, 1, DEVFLAGS_NONE, IMU2_ALIGN); + + + +timerHardware_t timerHardware[] = { + + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 1), + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 2), + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 3), + + DEF_TIM(TIM4, CH1, PD12, TIM_USE_OUTPUT_AUTO, 0, 4), + DEF_TIM(TIM4, CH2, PD13, TIM_USE_OUTPUT_AUTO, 0, 5), + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 6), + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 7), + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 8), +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/GEPRC_TAKER_H743/target.h b/src/main/target/GEPRC_TAKER_H743/target.h new file mode 100644 index 0000000000..7f2756ebf9 --- /dev/null +++ b/src/main/target/GEPRC_TAKER_H743/target.h @@ -0,0 +1,196 @@ +/* + * 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. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "GEPR" +#define USBD_PRODUCT_STRING "GEPRC_TAKER_H743" + +#define USE_TARGET_CONFIG + +#define LED0 PC13 + +#define BEEPER PD2 +#define BEEPER_INVERTED + + +//**************** SPI BUS ************************* +#define USE_SPI + +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +#define USE_SPI_DEVICE_4 +#define SPI4_SCK_PIN PE2 +#define SPI4_MISO_PIN PE5 +#define SPI4_MOSI_PIN PE6 + + + +// *************** Gyro & ACC ********************** + +#define USE_DUAL_GYRO +#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS +#define USE_IMU_MPU6000 +#define USE_IMU_ICM42605 + + +// *****IMU1 MPU6000 & ICM42688 ON SPI1 ************** +#define IMU1_ALIGN CW180_DEG +#define IMU1_SPI_BUS BUS_SPI1 +#define IMU1_CS_PIN PA4 + + + +// *****IMU2 MPU6000 & ICM42688 ON SPI2 ************** +#define IMU2_ALIGN CW0_DEG +#define IMU2_SPI_BUS BUS_SPI2 +#define IMU2_CS_PIN PB12 + + +// *************** I2C/Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_DPS310 +#define USE_BARO_MS5611 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_ALL + +#define TEMPERATURE_I2C_BUS BUS_I2C1 + +#define PITOT_I2C_BUS BUS_I2C1 + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 + +// *************** FLASH ************************** +#define USE_SDCARD +#define USE_SDCARD_SPI +#define SDCARD_SPI_BUS BUS_SPI3 +#define SDCARD_CS_PIN PA15 + +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + + +// ************PINIO to disable BT***************** +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PE13 +#define PINIO1_FLAGS PINIO_FLAGS_INVERTED + + +// ************PINIO to other +#define PINIO2_PIN PC14 //Enable vsw + + +// *************** OSD ***************************** +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI4 +#define MAX7456_CS_PIN PE4 + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define USE_UART7 +#define UART7_RX_PIN PE7 +#define UART7_TX_PIN PE8 + +#define USE_UART8 +#define UART8_RX_PIN PE0 +#define UART8_TX_PIN PE1 + +#define SERIAL_PORT_COUNT 8 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART2 + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC3 +// #define ADC1_DMA_STREAM DMA2_Stream0 +#define ADC_CHANNEL_1_PIN PC3 +#define ADC_CHANNEL_2_PIN PC5 +#define ADC_CHANNEL_3_PIN PC2 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define RSSI_ADC_CHANNEL ADC_CHN_2 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_3 + +#define VBAT_SCALE_DEFAULT 1120 + +//**************************************************** + +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX) + +#define USE_LED_STRIP +#define WS2811_PIN PA8 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff + +#define MAX_PWM_OUTPUT_PORTS 8 + +#define USE_DSHOT +#define USE_ESC_SENSOR diff --git a/src/main/target/HGLRCF405V2/CMakeLists.txt b/src/main/target/HGLRCF405V2/CMakeLists.txt new file mode 100644 index 0000000000..78d10e3671 --- /dev/null +++ b/src/main/target/HGLRCF405V2/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f405xg(HGLRCF405V2) diff --git a/src/main/target/HGLRCF405V2/target.c b/src/main/target/HGLRCF405V2/target.c new file mode 100644 index 0000000000..ee7b97bbaa --- /dev/null +++ b/src/main/target/HGLRCF405V2/target.c @@ -0,0 +1,39 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include +#include + +#include "drivers/io.h" +#include "drivers/timer.h" + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM , 0, 1), // PPM IN + + DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 1), // S1 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 1), // S2 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 1), // S3 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 + DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 1), // S6 + DEF_TIM(TIM2, CH3, PB10, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 + DEF_TIM(TIM2, CH4, PB11, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 + + DEF_TIM(TIM3, CH4, PB1, TIM_USE_LED, 0, 0), // LED_STRIP +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/HGLRCF405V2/target.h b/src/main/target/HGLRCF405V2/target.h new file mode 100644 index 0000000000..9f3a02b48f --- /dev/null +++ b/src/main/target/HGLRCF405V2/target.h @@ -0,0 +1,157 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "HGF4V2" +#define USBD_PRODUCT_STRING "HGLRCF405V2" + +#define USE_TARGET_CONFIG + +/*** Indicators ***/ +#define LED0 PC13 +#define BEEPER PB8 +#define BEEPER_INVERTED + +/*** SPI/I2C bus ***/ +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_NSS_PIN PA4 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_SPI_DEVICE_2 +#define SPI2_NSS_PIN PA13 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_SPI_DEVICE_3 +#define SPI3_NSS_PIN PC0 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB6 +#define I2C1_SDA PB7 + + +/*** IMU sensors ***/ + +// MPU6000 +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW270_DEG +#define MPU6000_SPI_BUS BUS_SPI1 +#define MPU6000_CS_PIN SPI1_NSS_PIN + +// ICM42605/ICM42688P +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW270_DEG +#define ICM42605_SPI_BUS BUS_SPI1 +#define ICM42605_CS_PIN SPI1_NSS_PIN + +/*** OSD ***/ +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN SPI2_NSS_PIN + +/*** Onboard flash ***/ +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_CS_PIN SPI3_NSS_PIN +#define M25P16_SPI_BUS BUS_SPI3 + + +/*** Serial ports ***/ +#define USE_VCP +// #define USE_UART_INVERTER + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define USE_UART3 +#define UART3_TX_PIN PC10 +#define UART3_RX_PIN PC11 + +#define USE_UART4 +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#define USE_UART5 +#define UART5_TX_PIN PC12 +#define UART5_RX_PIN PD2 + + +#define SERIAL_PORT_COUNT 6 + +/*** BARO & MAG ***/ +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_SPL06 +#define USE_BARO_DPS310 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_ALL + +/*** ADC ***/ +#define USE_ADC +#define ADC_CHANNEL_1_PIN PC2 +#define ADC_CHANNEL_2_PIN PC1 +#define ADC_CHANNEL_3_PIN PC3 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 //PC2 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 //PC1 +#define RSSI_ADC_CHANNEL ADC_CHN_3 //PC3 + +#define VBAT_SCALE_DEFAULT 1100 +#define CURRENT_METER_SCALE 206 + +/*** LED STRIP ***/ +#define USE_LED_STRIP +#define WS2811_PIN PB1 + +/*** Default settings ***/ +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART2 + + +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY | FEATURE_SOFTSERIAL ) + +/*** Timer/PWM output ***/ +#define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define MAX_PWM_OUTPUT_PORTS 8 +#define USE_DSHOT +#define USE_ESC_SENSOR + + +/*** Used pins ***/ +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) diff --git a/src/main/target/KAKUTEF4/target.h b/src/main/target/KAKUTEF4/target.h index c1ce47b9e8..92cbcf1341 100644 --- a/src/main/target/KAKUTEF4/target.h +++ b/src/main/target/KAKUTEF4/target.h @@ -78,6 +78,8 @@ # define BARO_I2C_BUS BUS_I2C1 # define USE_BARO_MS5611 # define USE_BARO_BMP280 +# define USE_BARO_DPS310 +# define USE_BARO_SPL06 #else // V1 does not have I2C exposed, common_post.h will pull in USE_*_MSP # define USE_BARO # define USE_MAG diff --git a/src/main/target/KAKUTEF7/target.h b/src/main/target/KAKUTEF7/target.h index f3f4edfed4..79f9d07935 100644 --- a/src/main/target/KAKUTEF7/target.h +++ b/src/main/target/KAKUTEF7/target.h @@ -131,6 +131,8 @@ #define USE_BARO #define USE_BARO_BMP280 #define USE_BARO_MS5611 +#define USE_BARO_DPS310 +#define USE_BARO_SPL06 #define BARO_I2C_BUS BUS_I2C1 #define USE_MAG diff --git a/src/main/target/KAKUTEF7MINIV3/target.h b/src/main/target/KAKUTEF7MINIV3/target.h index bf485ebc82..89ba7c632b 100644 --- a/src/main/target/KAKUTEF7MINIV3/target.h +++ b/src/main/target/KAKUTEF7MINIV3/target.h @@ -117,6 +117,8 @@ #define BARO_I2C_BUS BUS_I2C1 #define USE_BARO_BMP280 #define USE_BARO_MS5611 +#define USE_BARO_DPS310 +#define USE_BARO_SPL06 /* * Mag diff --git a/src/main/target/KAKUTEH7WING/target.h b/src/main/target/KAKUTEH7WING/target.h index b4ea4a03bd..7626b99775 100644 --- a/src/main/target/KAKUTEH7WING/target.h +++ b/src/main/target/KAKUTEH7WING/target.h @@ -171,8 +171,9 @@ // *************** PINIO *************************** #define USE_PINIO #define USE_PINIOBOX -#define PINIO1_PIN PC13 // VTX power switcher -#define PINIO2_PIN PE3 // 2xCamera switcher +#define PINIO1_PIN PC13 // 2xCamera switcher +#define PINIO2_PIN PE3 // VTX power switcher +#define PINIO2_FLAGS PINIO_FLAGS_INVERTED #define PINIO3_PIN PD4 // User1 #define PINIO4_PIN PE4 // User2 diff --git a/src/main/target/MATEKF765/target.h b/src/main/target/MATEKF765/target.h index 8a726d56b3..2a75a89cda 100644 --- a/src/main/target/MATEKF765/target.h +++ b/src/main/target/MATEKF765/target.h @@ -169,6 +169,7 @@ #define USE_SDCARD_SDIO #define SDCARD_SDIO_DMA DMA_TAG(2,3,4) #define SDCARD_SDIO_4BIT +#define SDCARD_SDIO_DEVICE SDIODEV_1 #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT // *************** ADC ***************************** diff --git a/src/main/target/MATEKH743/target.h b/src/main/target/MATEKH743/target.h index f2e21b0ee8..67e76e55b6 100644 --- a/src/main/target/MATEKH743/target.h +++ b/src/main/target/MATEKH743/target.h @@ -203,7 +203,7 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE -#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTA (0xffff & ~(BIT(14) | BIT(13))) #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD 0xffff diff --git a/src/main/target/MICOAIR743/CMakeLists.txt b/src/main/target/MICOAIR743/CMakeLists.txt index 1d38163878..6847f3daef 100644 --- a/src/main/target/MICOAIR743/CMakeLists.txt +++ b/src/main/target/MICOAIR743/CMakeLists.txt @@ -1 +1,2 @@ -target_stm32h743xi(MICOAIR743) \ No newline at end of file +target_stm32h743xi(MICOAIR743) +target_stm32h743xi(MICOAIR743_EXTMAG) diff --git a/src/main/target/MICOAIR743/target.h b/src/main/target/MICOAIR743/target.h index 4ba27f3447..526cf438d0 100644 --- a/src/main/target/MICOAIR743/target.h +++ b/src/main/target/MICOAIR743/target.h @@ -100,7 +100,14 @@ #define BARO_I2C_BUS BUS_I2C2 #define USE_MAG + +#ifdef MICOAIR743_EXTMAG +// External compass #define MAG_I2C_BUS BUS_I2C1 +#else +// Onboard compass +#define MAG_I2C_BUS BUS_I2C2 +#endif #define USE_MAG_ALL // *************** ENABLE OPTICAL FLOW & RANGEFINDER ***************************** diff --git a/src/main/target/NEUTRONRCF435MINI/target.h b/src/main/target/NEUTRONRCF435MINI/target.h index 6ef53abf64..d4653ca6c2 100644 --- a/src/main/target/NEUTRONRCF435MINI/target.h +++ b/src/main/target/NEUTRONRCF435MINI/target.h @@ -141,7 +141,9 @@ #define USE_UART2 #define UART2_RX_PIN PB0 +#define UART2_RX_AF 6 #define UART2_TX_PIN PA2 +#define UART2_TX_AF 7 #define USE_UART3 #define USE_UART3_PIN_SWAP diff --git a/src/main/target/PRINCIPIOTF7/CMakeLists.txt b/src/main/target/PRINCIPIOTF7/CMakeLists.txt new file mode 100644 index 0000000000..5988719a7e --- /dev/null +++ b/src/main/target/PRINCIPIOTF7/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f722xe(PRINCIPIOTF7) diff --git a/src/main/target/PRINCIPIOTF7/config.c b/src/main/target/PRINCIPIOTF7/config.c new file mode 100644 index 0000000000..c13aeda29b --- /dev/null +++ b/src/main/target/PRINCIPIOTF7/config.c @@ -0,0 +1,32 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + * + * This target has been autgenerated by bf2inav.py + */ + +#include + +#include "platform.h" + +#include "fc/fc_msp_box.h" +#include "fc/config.h" + +#include "io/piniobox.h" + +void targetConfiguration(void) +{ + +} diff --git a/src/main/target/PRINCIPIOTF7/target.c b/src/main/target/PRINCIPIOTF7/target.c new file mode 100644 index 0000000000..9268af4558 --- /dev/null +++ b/src/main/target/PRINCIPIOTF7/target.c @@ -0,0 +1,74 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + * + * This target has been autogenerated by bf2inav.py + */ + +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/pinio.h" +//#include "drivers/sensor.h" + + +timerHardware_t timerHardware[] = { + // Motor 1 + DEF_TIM(TIM3, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + //DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), + + // Motor 2 + DEF_TIM(TIM3, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + //DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), + + // Motor 3 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 + + // Motor 4 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 + + // Motor 5 + DEF_TIM(TIM1, CH2N, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 + //DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), + //DEF_TIM(TIM8, CH2N, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), + + // Motor 6 + DEF_TIM(TIM1, CH3N, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 + //DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), + //DEF_TIM(TIM8, CH3N, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), + + // Motor 7 + DEF_TIM(TIM2, CH3, PB10, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 + + // Motor 8 + DEF_TIM(TIM2, CH4, PB11, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 + + // Servo 1 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S9 + + // Servo 2 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S10 + + // LED Strip + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), + +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/PRINCIPIOTF7/target.h b/src/main/target/PRINCIPIOTF7/target.h new file mode 100644 index 0000000000..aa486d6af2 --- /dev/null +++ b/src/main/target/PRINCIPIOTF7/target.h @@ -0,0 +1,156 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + * + * This target has been autgenerated by bf2inav.py + */ + +#pragma once + +//#define USE_TARGET_CONFIG + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY ) + + + +#define TARGET_BOARD_IDENTIFIER "PRF7" +#define USBD_PRODUCT_STRING "PRINCIPIOTF7" + +// Beeper +#define USE_BEEPER +#define BEEPER PC13 + +// Leds +#define USE_LED_STRIP +#define WS2811_PIN PA8 +#define LED0 PA4 +#define LED1 PB15 + +// UARTs +#define USB_IO +#define USE_VCP +#define VBUS_SENSING_DISABLED + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PC11 +#define UART3_TX_PIN PC10 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART5 +#define UART5_RX_PIN PD2 +#define UART5_TX_PIN PC12 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define SERIAL_PORT_COUNT 7 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_UART SERIAL_PORT_USART2 + +// SPI +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA7 +#define SPI1_MOSI_PIN PA6 + +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PC3 +#define SPI2_MOSI_PIN PB14 + +// I2C +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +// MAG +#define USE_MAG +#define USE_MAG_ALL +#define MAG_I2C_BUS BUS_I2C1 + +// ADC +#define USE_ADC +#define ADC_INSTANCE ADC1 + +#define ADC_CHANNEL_1_PIN PC2 +#define VBAT_ADC_CHANNEL ADC_CHN_1 + +#define ADC_CHANNEL_2_PIN PC1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 + +// Gyro & ACC + +// ICM42688-P +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW90_DEG +#define ICM42605_CS_PIN PB2 +#define ICM42605_SPI_BUS BUS_SPI1 + +// BARO +// DPS310 +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_DPS310 + +// Blackbox +#define USE_FLASHFS +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +// Not sure which to use for the onboard chip (W25Q128JVPIQ) +#define USE_FLASH_M25P16 +#define M25P16_SPI_BUS BUS_SPI2 +#define M25P16_CS_PIN PB12 +#define USE_FLASH_W25M +#define W25M_SPI_BUS BUS_SPI2 +#define W25M_CS_PIN PB12 +#define USE_FLASH_W25M02G +#define W25M02G_SPI_BUS BUS_SPI2 +#define W25M02G_CS_PIN PB12 +#define USE_FLASH_W25M512 +#define W25M512_SPI_BUS BUS_SPI2 +#define W25M512_CS_PIN PB12 +#define USE_FLASH_W25N01G +#define W25N01G_SPI_BUS BUS_SPI2 +#define W25N01G_CS_PIN PB12 + +// Others + +#define MAX_PWM_OUTPUT_PORTS 10 +#define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define USE_DSHOT +#define USE_ESC_SENSOR + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff +#define TARGET_IO_PORTF 0xffff diff --git a/src/main/target/RADIOLINKF722/CMakeLists.txt b/src/main/target/RADIOLINKF722/CMakeLists.txt new file mode 100644 index 0000000000..f00803f190 --- /dev/null +++ b/src/main/target/RADIOLINKF722/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f722xe(RADIOLINKF722) diff --git a/src/main/target/RADIOLINKF722/config.c b/src/main/target/RADIOLINKF722/config.c new file mode 100644 index 0000000000..9bc0a52a89 --- /dev/null +++ b/src/main/target/RADIOLINKF722/config.c @@ -0,0 +1,38 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + + +#include +#include + +#include "platform.h" + +#include "fc/fc_msp_box.h" +#include "io/serial.h" +#include "drivers/pwm_mapping.h" + +void targetConfiguration(void) +{ + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].functionMask = FUNCTION_ESCSERIAL; + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + timerOverridesMutable(timer2id(TIM2))->outputMode = OUTPUT_MODE_MOTORS; + timerOverridesMutable(timer2id(TIM3))->outputMode = OUTPUT_MODE_MOTORS; + timerOverridesMutable(timer2id(TIM4))->outputMode = OUTPUT_MODE_MOTORS; +} diff --git a/src/main/target/RADIOLINKF722/target.c b/src/main/target/RADIOLINKF722/target.c new file mode 100644 index 0000000000..6ebb384b04 --- /dev/null +++ b/src/main/target/RADIOLINKF722/target.c @@ -0,0 +1,49 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include + +#include "platform.h" +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/pinio.h" +#include "drivers/sensor.h" + +BUSDEV_REGISTER_SPI_TAG(busdev_bmi270, DEVHW_BMI270, BMI270_SPI_BUS, BMI270_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_BMI270_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_icm42688, DEVHW_ICM42605, ICM42688_SPI_BUS, ICM42688_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_ICM42605_ALIGN); + +timerHardware_t timerHardware[] = { + + DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 Clash with S2, DSHOT does not work + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 + + DEF_TIM(TIM8, CH3, PC8, TIM_USE_LED, 0, 0), // LED + DEF_TIM(TIM5, CH1, PA0, TIM_USE_ANY, 0, 0), // Camera Control +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/RADIOLINKF722/target.h b/src/main/target/RADIOLINKF722/target.h new file mode 100644 index 0000000000..2e38e23b74 --- /dev/null +++ b/src/main/target/RADIOLINKF722/target.h @@ -0,0 +1,169 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "RALI" +#define USBD_PRODUCT_STRING "RADIOLINKF722" + +#define LED0 PA14 + +#define USE_BEEPER +#define BEEPER PC13 +#define BEEPER_INVERTED + +// *************** UART ***************************** +#define USB_IO +#define USE_VCP +// #define VBUS_SENSING_PIN PC15 +// #define VBUS_SENSING_ENABLED + +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PC9 +#define PINIO1_FLAGS PINIO_FLAGS_INVERTED + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN NONE + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define SERIAL_PORT_COUNT 6 + +// *************** Gyro & ACC ********************** +#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS + +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +// ICM42605/ICM42688P +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW180_DEG +#define ICM42688_SPI_BUS BUS_SPI1 +#define ICM42688_CS_PIN PB2 + +//BMI270 +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN CW180_DEG +#define BMI270_SPI_BUS BUS_SPI1 +#define BMI270_CS_PIN PB2 + +// *************** I2C(Baro & I2C) ************************** +#define USE_I2C + +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +// Baro +#define USE_BARO +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_DPS310 +#define USE_BARO_SPL06 +#define BARO_I2C_BUS BUS_I2C1 + +// Mag +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_ALL + +// *************** FLASH ************************** + +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +#define W25N01G_SPI_BUS BUS_SPI3 +#define W25N01G_CS_PIN PD2 + +#define USE_BLACKBOX +#define USE_FLASHFS +#define USE_FLASH_W25N01G +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +// *************** OSD ***************************** +#define USE_OSD +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN PB12 + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_CHANNEL_1_PIN PC2 +#define ADC_CHANNEL_2_PIN PC0 +#define ADC_CHANNEL_3_PIN PC1 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_3 +#define RSSI_ADC_CHANNEL ADC_CHN_2 + +// *************** LED ***************************** +#define USE_LED_STRIP +#define WS2811_PIN PC8 + +// ********** Optiical Flow adn Lidar ************** +#define USE_RANGEFINDER +#define USE_RANGEFINDER_MSP +#define RANGEFINDER_I2C_BUS BUS_I2C1 +#define USE_OPFLOW +#define USE_OPFLOW_MSP + +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX) +#define USE_DSHOT +#define USE_ESC_SENSOR +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define SERIALRX_UART SERIAL_PORT_USART2 +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS + +#define MAX_PWM_OUTPUT_PORTS 8 + +#define CURRENT_METER_SCALE 490 + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) diff --git a/src/main/target/SEQUREH7/CMakeLists.txt b/src/main/target/SEQUREH7/CMakeLists.txt new file mode 100644 index 0000000000..edc5d0b657 --- /dev/null +++ b/src/main/target/SEQUREH7/CMakeLists.txt @@ -0,0 +1,2 @@ +target_stm32h743xi(SEQUREH7) +target_stm32h743xi(SEQUREH7V2) diff --git a/src/main/target/SEQUREH7/config.c b/src/main/target/SEQUREH7/config.c new file mode 100644 index 0000000000..cd5aa30d71 --- /dev/null +++ b/src/main/target/SEQUREH7/config.c @@ -0,0 +1,33 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + * + * This target has been autgenerated by bf2inav.py + */ + +#include + +#include "platform.h" + +#include "fc/fc_msp_box.h" +#include "fc/config.h" + +#include "io/piniobox.h" + +void targetConfiguration(void) +{ + +} + diff --git a/src/main/target/SEQUREH7/target.c b/src/main/target/SEQUREH7/target.c new file mode 100644 index 0000000000..0ec6c59652 --- /dev/null +++ b/src/main/target/SEQUREH7/target.c @@ -0,0 +1,50 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +// #include "drivers/pinio.h" +#include "drivers/sensor.h" + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 1), // S2 + // DEF_TIM(TIM1, CH2N, PB0, TIM_USE_OUTPUT_AUTO, 0, 2), // S3 - Timer used by LED + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 2), // S3 + // DEF_TIM(TIM8, CH2N, PB0, TIM_USE_OUTPUT_AUTO, 0, 2), // S3 + // DEF_TIM(TIM1, CH3N, PB1, TIM_USE_OUTPUT_AUTO, 0, 3), // S4 - Timer used by LED + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 3), // S4 + // DEF_TIM(TIM8, CH3N, PB1, TIM_USE_OUTPUT_AUTO, 0, 3), // S4 + + DEF_TIM(TIM16, CH1N, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 // No DMA + ///DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 - Timer used by beeper - BF Timer // No DMA + DEF_TIM(TIM17, CH1N, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 // No DMA + ///DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 - Timer used by beeper - BF Timer // No DMA + DEF_TIM(TIM2, CH3, PB10, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 // No DMA + DEF_TIM(TIM2, CH4, PB11, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 // No DMA + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED_2812 + DEF_TIM(TIM4, CH4, PD15, TIM_USE_BEEPER, 0, 0), // BEEPER PWM +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/SEQUREH7/target.h b/src/main/target/SEQUREH7/target.h new file mode 100644 index 0000000000..428ba01d00 --- /dev/null +++ b/src/main/target/SEQUREH7/target.h @@ -0,0 +1,157 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#pragma once + +#define USE_TARGET_CONFIG + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY ) + +#if defined(SEQUREH7V2) + #define TARGET_BOARD_IDENTIFIER "SQH7V2" + #define USBD_PRODUCT_STRING "SEQUREH7V2" +#else + #define TARGET_BOARD_IDENTIFIER "SQH7" + #define USBD_PRODUCT_STRING "SEQUREH7" +#endif + +// Beeper +#define USE_BEEPER +#define BEEPER PD15 +#define BEEPER_INVERTED + +// Leds +#define USE_LED_STRIP +#define WS2811_PIN PA8 +#define LED0 PC13 + +// UARTs +#define USB_IO +#define USE_VCP + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define USE_UART7 +#define UART7_RX_PIN PE7 +#define UART7_TX_PIN PE8 + +#define USE_UART8 // ESC telemetry +#define UART8_RX_PIN PE0 +#define UART8_TX_PIN PE0 + +#define SERIAL_PORT_COUNT 7 +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF + +// SPI +#define USE_SPI + +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +// I2C +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +// MAG +#define USE_MAG +#define USE_MAG_ALL + +// ADC +#define USE_ADC +#define ADC_INSTANCE ADC3 + +#define ADC_CHANNEL_1_PIN PC3 +#define ADC_CHANNEL_2_PIN PC2 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 + +#define CURRENT_METER_SCALE 1052 + +// Gyro & ACC +#if defined(SEQUREH7V2) +#define USE_IMU_ICM42605 // ICM42688P +#define ICM42605_CS_PIN PB12 +#define ICM42605_SPI_BUS BUS_SPI2 +#define IMU_ICM42605_ALIGN CW90_DEG +#else +#define USE_IMU_MPU6000 +#define MPU6000_CS_PIN PB12 +#define MPU6000_SPI_BUS BUS_SPI2 +#define IMU_MPU6000_ALIGN CW0_DEG +#endif + +// BARO +#define USE_BARO +#define USE_BARO_ALL +#define BARO_I2C_BUS BUS_I2C1 + +// OSD +#define USE_MAX7456 +#define MAX7456_CS_PIN PA4 +#define MAX7456_SPI_BUS BUS_SPI1 + +// Blackbox +#define USE_FLASHFS +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +#define USE_FLASH_M25P16 +#define M25P16_SPI_BUS BUS_SPI3 +#define M25P16_CS_PIN PA15 + +// Others +#define MAX_PWM_OUTPUT_PORTS 8 +#define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define USE_DSHOT +#define USE_ESC_SENSOR + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff +#define TARGET_IO_PORTF 0xffff +#define TARGET_IO_PORTG 0xffff diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index f6d80f7497..f7963dfdd9 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -80,6 +80,9 @@ #define USE_HEADTRACKER_MSP #undef USE_DASHBOARD +#define USE_GEOZONE +#define MAX_GEOZONES_IN_CONFIG 63 +#define MAX_VERTICES_IN_CONFIG 126 #undef USE_GYRO_KALMAN // Strange behaviour under x86/x64 ?!? #undef USE_VCP diff --git a/src/main/target/SKYSTARSF405HD/CMakeLists.txt b/src/main/target/SKYSTARSF405HD/CMakeLists.txt index b097558284..daa0577d44 100644 --- a/src/main/target/SKYSTARSF405HD/CMakeLists.txt +++ b/src/main/target/SKYSTARSF405HD/CMakeLists.txt @@ -1,2 +1,3 @@ target_stm32f405xg(SKYSTARSF405HD) -target_stm32f405xg(SKYSTARSF405HD2) \ No newline at end of file +target_stm32f405xg(SKYSTARSF405HD2) +target_stm32f405xg(SKYSTARSF405AIO) \ No newline at end of file diff --git a/src/main/target/SKYSTARSF405HD/config.c b/src/main/target/SKYSTARSF405HD/config.c index b34785c5b7..ff0dffc742 100644 --- a/src/main/target/SKYSTARSF405HD/config.c +++ b/src/main/target/SKYSTARSF405HD/config.c @@ -23,6 +23,7 @@ #include "io/serial.h" #include "rx/rx.h" #include "drivers/pwm_mapping.h" +#include "sensors/boardalignment.h" void targetConfiguration(void) { @@ -30,4 +31,9 @@ void targetConfiguration(void) // To improve backwards compatibility with INAV versions 6.x and older timerOverridesMutable(timer2id(TIM2))->outputMode = OUTPUT_MODE_MOTORS; + +#if defined(SKYSTARSF405AIO) + boardAlignmentMutable()->yawDeciDegrees = 450; +#endif + } diff --git a/src/main/target/SKYSTARSF405HD/target.c b/src/main/target/SKYSTARSF405HD/target.c index cc4ff1c5a8..8e5a4663b3 100644 --- a/src/main/target/SKYSTARSF405HD/target.c +++ b/src/main/target/SKYSTARSF405HD/target.c @@ -28,7 +28,7 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO, 0, 0), -#if defined(SKYSTARSF405HD2) +#if defined(SKYSTARSF405HD2) || defined(SKYSTARSF405AIO) DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), #endif diff --git a/src/main/target/SKYSTARSF405HD/target.h b/src/main/target/SKYSTARSF405HD/target.h index 9b9805084f..c3711fcb4c 100644 --- a/src/main/target/SKYSTARSF405HD/target.h +++ b/src/main/target/SKYSTARSF405HD/target.h @@ -36,17 +36,14 @@ #define SPI1_MOSI_PIN PA7 #define USE_IMU_MPU6000 -#define IMU_MPU6000_ALIGN CW180_DEG_FLIP #define MPU6000_SPI_BUS BUS_SPI1 #define MPU6000_CS_PIN PA4 #define USE_IMU_BMI270 -#define IMU_BMI270_ALIGN CW180_DEG_FLIP #define BMI270_SPI_BUS BUS_SPI1 #define BMI270_CS_PIN PA4 #define USE_IMU_ICM42605 -#define IMU_ICM42605_ALIGN CW180_DEG_FLIP #define ICM42605_SPI_BUS BUS_SPI1 #define ICM42605_CS_PIN PA4 @@ -156,8 +153,18 @@ #define USE_DSHOT #define USE_ESC_SENSOR -#if defined(SKYSTARSF405HD2) +#if defined(SKYSTARSF405HD2) || defined(SKYSTARSF405AIO) #define MAX_PWM_OUTPUT_PORTS 6 #else #define MAX_PWM_OUTPUT_PORTS 4 #endif + +#if defined(SKYSTARSF405AIO) +#define IMU_MPU6000_ALIGN CW0_DEG +#define IMU_BMI270_ALIGN CW0_DEG +#define IMU_ICM42605_ALIGN CW0_DEG +#else +#define IMU_MPU6000_ALIGN CW180_DEG_FLIP +#define IMU_BMI270_ALIGN CW180_DEG_FLIP +#define IMU_ICM42605_ALIGN CW180_DEG_FLIP +#endif diff --git a/src/main/target/SKYSTARSF405WING/CMakeLists.txt b/src/main/target/SKYSTARSF405WING/CMakeLists.txt new file mode 100644 index 0000000000..34324750a8 --- /dev/null +++ b/src/main/target/SKYSTARSF405WING/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f405xg(SKYSTARSF405WING) diff --git a/src/main/target/SKYSTARSF405WING/config.c b/src/main/target/SKYSTARSF405WING/config.c new file mode 100644 index 0000000000..f149b4b8ec --- /dev/null +++ b/src/main/target/SKYSTARSF405WING/config.c @@ -0,0 +1,29 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include +#include "platform.h" +#include "fc/fc_msp_box.h" +#include "fc/config.h" +#include "io/piniobox.h" + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; + beeperConfigMutable()->pwmMode = true; +} diff --git a/src/main/target/SKYSTARSF405WING/target.c b/src/main/target/SKYSTARSF405WING/target.c new file mode 100644 index 0000000000..19260ea4b2 --- /dev/null +++ b/src/main/target/SKYSTARSF405WING/target.c @@ -0,0 +1,48 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include +#include +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/sensor.h" +#include "drivers/timer_def_stm32f4xx.h" + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 1), // S1 D(2,1,6) UP256 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 D(2,7,7) UP217 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 D(2,2,0) UP217 + DEF_TIM(TIM1, CH3N, PB15, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 D(2,6,0) UP256 + + DEF_TIM(TIM1, CH3N, PB15, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 D(2,6,0) UP256 + DEF_TIM(TIM2, CH4, PB11, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 D(1,7,3) UP173 + DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 D(1,6,3) UP173 + DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 D(1,5,3) UP173 + + DEF_TIM(TIM12, CH1, PB14, TIM_USE_OUTPUT_AUTO, 0, 0), // S9 DMA NONE + DEF_TIM(TIM13, CH1, PA6, TIM_USE_OUTPUT_AUTO, 0, 0), // S10 DMA NONE + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S11 D(1,0,2) + + DEF_TIM(TIM3, CH4, PB1, TIM_USE_LED, 0, 0), // 2812LED D(1,2,5) + DEF_TIM(TIM11, CH1, PB9, TIM_USE_BEEPER, 0, 0), // BEEPER PWM + + DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), //TX2 softserial1_Tx +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/SKYSTARSF405WING/target.h b/src/main/target/SKYSTARSF405WING/target.h new file mode 100644 index 0000000000..2c219712ad --- /dev/null +++ b/src/main/target/SKYSTARSF405WING/target.h @@ -0,0 +1,153 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#pragma once +#define USE_TARGET_CONFIG + + +#define TARGET_BOARD_IDENTIFIER "SK4W" +#define USBD_PRODUCT_STRING "SKYSTARSF405WING" + +#define LED0 PC0 +#define LED1 PC1 + +#define BEEPER PB9 +#define BEEPER_INVERTED +#define BEEPER_PWM_FREQUENCY 2500 + +// *************** SPI1 IMU & OSD ******************* +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PB4 +#define SPI1_MOSI_PIN PA7 + +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW180_DEG +#define ICM42605_SPI_BUS BUS_SPI1 +#define ICM42605_CS_PIN PC14 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI1 +#define MAX7456_CS_PIN PB12 + +// *************** SPI2 Flash/SD Card **************** +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PC2 +#define SPI2_MOSI_PIN PC3 + +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_SPI_BUS BUS_SPI2 +#define M25P16_CS_PIN PC13 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +// *************** I2C /Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB7 + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_SPL06 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_ALL + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS BUS_I2C1 +#define PITOT_I2C_BUS BUS_I2C1 +#define TEMPERATURE_I2C_BUS BUS_I2C1 + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define USE_UART3 +#define UART3_TX_PIN PC10 +#define UART3_RX_PIN PC11 + +#define USE_UART4 +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#define USE_UART5 +#define UART5_TX_PIN PC12 +#define UART5_RX_PIN PD2 + +#define USE_UART6 +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define USE_SOFTSERIAL1 +#define SOFTSERIAL_1_TX_PIN PA2 +#define SOFTSERIAL_1_RX_PIN PA2 + +#define SERIAL_PORT_COUNT 8 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define SERIALRX_UART SERIAL_PORT_USART2 + +// *************** ADC *************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream4 +#define ADC_CHANNEL_1_PIN PC4 +#define ADC_CHANNEL_2_PIN PC5 +#define ADC_CHANNEL_3_PIN PB0 +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 + +// *************** PINIO *************************** +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PA4 +#define PINIO2_PIN PB5 + +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PB1 + +// *************** others ************************ +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) +#define VBAT_SCALE_DEFAULT 1100 +#define CURRENT_METER_SCALE 150 + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) + +#define MAX_PWM_OUTPUT_PORTS 11 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define USE_DSHOT +#define USE_DSHOT_DMAR +#define USE_ESC_SENSOR diff --git a/src/main/target/SPEEDYBEEF405AIO/CMakeLists.txt b/src/main/target/SPEEDYBEEF405AIO/CMakeLists.txt new file mode 100644 index 0000000000..5feb586489 --- /dev/null +++ b/src/main/target/SPEEDYBEEF405AIO/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f405xg(SPEEDYBEEF405AIO HSE_MHZ 8) diff --git a/src/main/target/SPEEDYBEEF405AIO/config.c b/src/main/target/SPEEDYBEEF405AIO/config.c new file mode 100644 index 0000000000..e73067ef72 --- /dev/null +++ b/src/main/target/SPEEDYBEEF405AIO/config.c @@ -0,0 +1,33 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include + +#include "platform.h" + +#include "fc/fc_msp_box.h" + +#include "io/piniobox.h" +#include "io/serial.h" + +void targetConfiguration(void) +{ + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART6)].functionMask = FUNCTION_RX_SERIAL; + + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].functionMask = FUNCTION_MSP; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].msp_baudrateIndex = BAUD_115200; +} diff --git a/src/main/target/SPEEDYBEEF405AIO/target.c b/src/main/target/SPEEDYBEEF405AIO/target.c new file mode 100644 index 0000000000..9d1ae18ca7 --- /dev/null +++ b/src/main/target/SPEEDYBEEF405AIO/target.c @@ -0,0 +1,39 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/pinio.h" +#include "drivers/sensor.h" + +timerHardware_t timerHardware[] = { + + DEF_TIM(TIM2, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S1 + DEF_TIM(TIM2, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S2 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S3 + DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 1 ), // S4 + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/SPEEDYBEEF405AIO/target.h b/src/main/target/SPEEDYBEEF405AIO/target.h new file mode 100644 index 0000000000..a6abdfcc37 --- /dev/null +++ b/src/main/target/SPEEDYBEEF405AIO/target.h @@ -0,0 +1,162 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "SF4A" +#define USBD_PRODUCT_STRING "SPEEDYBEEF405AIO" + +#define LED0 PD7 + +#define BEEPER PD11 +#define BEEPER_INVERTED + +/* + * SPI Buses + */ +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + +/* + * I2C + */ +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 +#define DEFAULT_I2C_BUS BUS_I2C1 +#define USE_I2C_PULLUP + +/* + * Serial + */ +#define USE_VCP + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN NONE +#define UART2_RX_PIN PD6 + +#define USE_UART3 +#define UART3_TX_PIN PD8 +#define UART3_RX_PIN PD9 + +#define USE_UART4 +#define UART4_TX_PIN PC10 +#define UART4_RX_PIN PC11 + +#define USE_UART5 +#define UART5_TX_PIN PC12 +#define UART5_RX_PIN PD2 + +#define USE_UART6 +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define SERIAL_PORT_COUNT 7 + +/* + * Gyro + */ +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW180_DEG +#define ICM42605_CS_PIN PA4 +#define ICM42605_SPI_BUS BUS_SPI1 + +/* + * Other + */ +#define USE_BARO +#define BARO_I2C_BUS DEFAULT_I2C_BUS +#define USE_BARO_ALL + +#define USE_MAG +#define MAG_I2C_BUS DEFAULT_I2C_BUS +#define USE_MAG_ALL + +#define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS +#define PITOT_I2C_BUS DEFAULT_I2C_BUS +#define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS + +/* + * OSD + */ +#define USE_MAX7456 +#define MAX7456_CS_PIN PD5 +#define MAX7456_SPI_BUS BUS_SPI3 + +/* + * Blackbox + */ +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_SPI_BUS BUS_SPI2 +#define M25P16_CS_PIN PB12 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define SERIALRX_UART SERIAL_PORT_USART6 + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 + +#define ADC_CHANNEL_1_PIN PC0 +#define ADC_CHANNEL_2_PIN PC2 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 + +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PA8 + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) +#define CURRENT_METER_SCALE 254 +#define CURRENT_METER_OFFSET 0 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff + +#define MAX_PWM_OUTPUT_PORTS 4 + +#define USE_DSHOT +#define USE_SERIALSHOT +#define USE_ESC_SENSOR diff --git a/src/main/target/SPEEDYBEEF405MINI/target.h b/src/main/target/SPEEDYBEEF405MINI/target.h index 1f25899ed9..c20ee9aa0f 100644 --- a/src/main/target/SPEEDYBEEF405MINI/target.h +++ b/src/main/target/SPEEDYBEEF405MINI/target.h @@ -101,8 +101,6 @@ */ #define USE_BARO #define BARO_I2C_BUS BUS_I2C1 -#define USE_BARO_BMP280 -#define USE_BARO_MS5611 #define USE_BARO_DPS310 #define USE_MAG diff --git a/src/main/target/TBS_LUCID_FC/target.h b/src/main/target/TBS_LUCID_FC/target.h index f5bdecc0fa..9baab07355 100644 --- a/src/main/target/TBS_LUCID_FC/target.h +++ b/src/main/target/TBS_LUCID_FC/target.h @@ -41,10 +41,12 @@ #define UART1_RX_PIN PA10 #define USE_UART2 -#define UART2_TX_PIN NONE +#define UART2_AF 6 +#define UART2_TX_PIN PB0 #define UART2_RX_PIN PB0 #define USE_UART3 +#define USE_UART3_PIN_SWAP #define UART3_TX_PIN PB11 #define UART3_RX_PIN PB10 diff --git a/src/main/target/TBS_LUCID_H7/CMakeLists.txt b/src/main/target/TBS_LUCID_H7/CMakeLists.txt new file mode 100644 index 0000000000..0586bde8b6 --- /dev/null +++ b/src/main/target/TBS_LUCID_H7/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32h743xi(TBS_LUCID_H7) diff --git a/src/main/target/TBS_LUCID_H7/config.c b/src/main/target/TBS_LUCID_H7/config.c new file mode 100644 index 0000000000..9912439df7 --- /dev/null +++ b/src/main/target/TBS_LUCID_H7/config.c @@ -0,0 +1,39 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#include + +#include "platform.h" + +#include "fc/fc_msp_box.h" +#include "fc/config.h" + +#include "io/piniobox.h" + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; + beeperConfigMutable()->pwmMode = true; +} diff --git a/src/main/target/TBS_LUCID_H7/target.c b/src/main/target/TBS_LUCID_H7/target.c new file mode 100644 index 0000000000..30e16b8482 --- /dev/null +++ b/src/main/target/TBS_LUCID_H7/target.c @@ -0,0 +1,61 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/sensor.h" + +BUSDEV_REGISTER_SPI_TAG(busdev_gyro1_mpu6000, DEVHW_MPU6000, GYRO1_SPI_BUS, GYRO1_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_MPU6000_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_gyro1_icm42688, DEVHW_ICM42605, GYRO1_SPI_BUS, GYRO1_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_ICM42605_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_gyro2_mpu6000, DEVHW_MPU6000, GYRO2_SPI_BUS, GYRO2_CS_PIN, NONE, 1, DEVFLAGS_NONE, IMU_MPU6000_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_gyro2_icm42688, DEVHW_ICM42605, GYRO2_SPI_BUS, GYRO2_CS_PIN, NONE, 1, DEVFLAGS_NONE, IMU_ICM42605_ALIGN); + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 1), // S2 + + DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 2), // S3 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 3), // S4 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 4), // S5 + DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 5), // S6 + + DEF_TIM(TIM4, CH1, PD12, TIM_USE_OUTPUT_AUTO, 0, 6), // S7 + DEF_TIM(TIM4, CH2, PD13, TIM_USE_OUTPUT_AUTO, 0, 7), // S8 + DEF_TIM(TIM4, CH3, PD14, TIM_USE_OUTPUT_AUTO, 0, 0), // S9 + DEF_TIM(TIM4, CH4, PD15, TIM_USE_OUTPUT_AUTO, 0, 0), // S10 DMA_NONE + + DEF_TIM(TIM15, CH1, PE5, TIM_USE_OUTPUT_AUTO, 0, 0), // S11 + DEF_TIM(TIM15, CH2, PE6, TIM_USE_OUTPUT_AUTO, 0, 0), // S12 DMA_NONE + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 9), // RGB_LED + DEF_TIM(TIM2, CH1, PA15, TIM_USE_BEEPER, 0, 0), // BEEPER +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/TBS_LUCID_H7/target.h b/src/main/target/TBS_LUCID_H7/target.h new file mode 100644 index 0000000000..c215c4bef8 --- /dev/null +++ b/src/main/target/TBS_LUCID_H7/target.h @@ -0,0 +1,183 @@ +/* + * 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/. + */ + + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "LUH7" + +#define USBD_PRODUCT_STRING "TBS_LUCID_H7" + +#define USE_TARGET_CONFIG + +#define LED0 PE3 +#define LED1 PE4 + +#define BEEPER PA15 +#define BEEPER_INVERTED +#define BEEPER_PWM_FREQUENCY 2500 + +#define USE_VCP + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PD5 +#define UART2_RX_PIN PD6 + +#define USE_UART3 +#define UART3_TX_PIN PD8 +#define UART3_RX_PIN PD9 + +#define USE_UART4 +#define UART4_TX_PIN PB9 +#define UART4_RX_PIN PB8 + +#define USE_UART6 +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define USE_UART7 +#define UART7_TX_PIN PE8 +#define UART7_RX_PIN PE7 + +#define USE_UART8 +#define UART8_TX_PIN PE1 +#define UART8_RX_PIN PE0 + +#define SERIAL_PORT_COUNT 8 + +#define USE_SPI + +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PD7 + +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + +#define USE_SPI_DEVICE_4 +#define SPI4_SCK_PIN PE12 +#define SPI4_MISO_PIN PE13 +#define SPI4_MOSI_PIN PE14 + +#define USE_DUAL_GYRO +#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS + +#define GYRO1_SPI_BUS BUS_SPI1 +#define GYRO1_CS_PIN PC15 +#define GYRO2_SPI_BUS BUS_SPI4 +#define GYRO2_CS_PIN PC13 + +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW0_DEG_FLIP + +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW90_DEG_FLIP + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN PB12 + +#define USE_I2C + +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB6 +#define I2C1_SDA PB7 + +#define USE_I2C_DEVICE_2 +#define I2C2_SCL PB10 +#define I2C2_SDA PB11 + +#define USE_BARO +#define USE_BARO_DPS310 +#define BARO_I2C_BUS BUS_I2C2 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_ALL + +#define TEMPERATURE_I2C_BUS BUS_I2C2 +#define PITOT_I2C_BUS BUS_I2C2 + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS BUS_I2C1 + +#define USE_SDCARD +#define USE_SDCARD_SDIO +#define SDCARD_SDIO_DEVICE SDIODEV_1 +#define SDCARD_SDIO_4BIT + +#define USE_ADC +#define ADC_INSTANCE ADC1 + +#define ADC_CHANNEL_1_PIN PC0 +#define ADC_CHANNEL_2_PIN PC1 +#define ADC_CHANNEL_3_PIN PC5 +#define ADC_CHANNEL_4_PIN PC4 +#define ADC_CHANNEL_5_PIN PA4 +#define ADC_CHANNEL_6_PIN PA7 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 +#define AIRSPEED_ADC_CHANNEL ADC_CHN_4 + +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PD10 +#define PINIO2_PIN PD11 + +#define USE_LED_STRIP +#define WS2811_PIN PA8 + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) +#define CURRENT_METER_SCALE 250 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + +#define TARGET_IO_PORTA (0xffff & ~(BIT(14) | BIT(13))) +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff + +#define MAX_PWM_OUTPUT_PORTS 15 +#define USE_DSHOT +#define USE_ESC_SENSOR + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define SERIALRX_UART SERIAL_PORT_USART6 \ No newline at end of file diff --git a/src/main/target/TUNERCF405/target.h b/src/main/target/TUNERCF405/target.h index f193377c6e..b359545e69 100644 --- a/src/main/target/TUNERCF405/target.h +++ b/src/main/target/TUNERCF405/target.h @@ -103,6 +103,8 @@ #define USE_UART4 #define UART4_RX_PIN PA1 #define UART4_TX_PIN PA0 +#define USE_UART_INVERTER +#define INVERTER_PIN_UART4_RX PC14 #define USE_UART5 #define UART5_RX_PIN PD2 diff --git a/src/main/target/common.h b/src/main/target/common.h index f7cc3f747c..af0de4bf30 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -209,6 +209,11 @@ #define USE_34CHANNELS #define MAX_MIXER_PROFILE_COUNT 2 #define USE_SMARTPORT_MASTER +#ifdef USE_GPS +#define USE_GEOZONE +#define MAX_GEOZONES_IN_CONFIG 63 +#define MAX_VERTICES_IN_CONFIG 126 +#endif #elif !defined(STM32F7) #define MAX_MIXER_PROFILE_COUNT 1 #endif @@ -221,3 +226,4 @@ #define USE_EZ_TUNE #define USE_ADAPTIVE_FILTER + diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index fddd8b2a90..104898c92f 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -115,6 +115,7 @@ extern uint8_t __config_end; #define USE_BARO_BMP085 #define USE_BARO_BMP280 #define USE_BARO_BMP388 +#define USE_BARO_BMP390 #define USE_BARO_DPS310 #define USE_BARO_LPS25H #define USE_BARO_MS5607 diff --git a/src/main/target/stm32f7xx_hal_conf.h b/src/main/target/stm32f7xx_hal_conf.h index 8c82cd3b95..7295e93421 100644 --- a/src/main/target/stm32f7xx_hal_conf.h +++ b/src/main/target/stm32f7xx_hal_conf.h @@ -71,7 +71,7 @@ /* #define HAL_RNG_MODULE_ENABLED */ #define HAL_RTC_MODULE_ENABLED /* #define HAL_SAI_MODULE_ENABLED */ -/* #define HAL_SD_MODULE_ENABLED */ +#define HAL_SD_MODULE_ENABLED /* #define HAL_MMC_MODULE_ENABLED */ /* #define HAL_SPDIFRX_MODULE_ENABLED */ #define HAL_SPI_MODULE_ENABLED diff --git a/src/main/telemetry/crsf.h b/src/main/telemetry/crsf.h old mode 100755 new mode 100644 diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 4acc20cc44..fd76288815 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -1142,11 +1142,39 @@ static bool handleIncoming_PARAM_REQUEST_LIST(void) { return true; } +static void mavlinkParseRxStats(const mavlink_radio_status_t *msg) { + switch(telemetryConfig()->mavlink.radio_type) { + case MAVLINK_RADIO_SIK: + // rssi scaling info from: https://ardupilot.org/rover/docs/common-3dr-radio-advanced-configuration-and-technical-information.html + rxLinkStatistics.uplinkRSSI = (msg->rssi / 1.9) - 127; + rxLinkStatistics.uplinkSNR = msg->noise / 1.9; + rxLinkStatistics.uplinkLQ = msg->rssi != 255 ? scaleRange(msg->rssi, 0, 254, 0, 100) : 0; + break; + case MAVLINK_RADIO_ELRS: + rxLinkStatistics.uplinkRSSI = -msg->remrssi; + rxLinkStatistics.uplinkSNR = msg->noise; + rxLinkStatistics.uplinkLQ = scaleRange(msg->rssi, 0, 255, 0, 100); + break; + case MAVLINK_RADIO_GENERIC: + default: + rxLinkStatistics.uplinkRSSI = msg->rssi; + rxLinkStatistics.uplinkSNR = msg->noise; + rxLinkStatistics.uplinkLQ = msg->rssi != 255 ? scaleRange(msg->rssi, 0, 254, 0, 100) : 0; + break; + } +} + static bool handleIncoming_RADIO_STATUS(void) { mavlink_radio_status_t msg; mavlink_msg_radio_status_decode(&mavRecvMsg, &msg); txbuff_valid = true; txbuff_free = msg.txbuf; + + if (rxConfig()->receiverType == RX_TYPE_SERIAL && + rxConfig()->serialrx_provider == SERIALRX_MAVLINK) { + mavlinkParseRxStats(&msg); + } + return true; } @@ -1243,7 +1271,7 @@ static bool processMAVLinkIncomingTelemetry(void) #endif case MAVLINK_MSG_ID_RADIO_STATUS: handleIncoming_RADIO_STATUS(); - // Don't set that we handled a message, otherwise radio status packets will block telemetry messages + // Don't set that we handled a message, otherwise radio status packets will block telemetry messages. return false; default: return false; @@ -1276,7 +1304,7 @@ void handleMAVLinkTelemetry(timeUs_t currentTimeUs) // Determine whether to send telemetry back based on flow control / pacing if (txbuff_valid) { // Use flow control if available - shouldSendTelemetry = txbuff_free >= 33; + shouldSendTelemetry = txbuff_free >= telemetryConfig()->mavlink.min_txbuff; } else { // If not, use blind frame pacing - and back off for collision avoidance if half-duplex bool halfDuplexBackoff = (isMAVLinkTelemetryHalfDuplex() && receivedMessage); diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 82fc2d2d5f..e0820234e6 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -56,7 +56,7 @@ #include "telemetry/ghst.h" -PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 6); +PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 8); PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, .telemetry_switch = SETTING_TELEMETRY_SWITCH_DEFAULT, @@ -93,7 +93,9 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, .extra1_rate = SETTING_MAVLINK_EXTRA1_RATE_DEFAULT, .extra2_rate = SETTING_MAVLINK_EXTRA2_RATE_DEFAULT, .extra3_rate = SETTING_MAVLINK_EXTRA3_RATE_DEFAULT, - .version = SETTING_MAVLINK_VERSION_DEFAULT + .version = SETTING_MAVLINK_VERSION_DEFAULT, + .min_txbuff = SETTING_MAVLINK_MIN_TXBUFFER_DEFAULT, + .radio_type = SETTING_MAVLINK_RADIO_TYPE_DEFAULT } ); diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index 7be5e50a9c..c880895927 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -36,6 +36,12 @@ typedef enum { LTM_RATE_SLOW } ltmUpdateRate_e; +typedef enum { + MAVLINK_RADIO_GENERIC, + MAVLINK_RADIO_ELRS, + MAVLINK_RADIO_SIK, +} mavlinkRadio_e; + typedef enum { SMARTPORT_FUEL_UNIT_PERCENT, SMARTPORT_FUEL_UNIT_MAH, @@ -73,6 +79,8 @@ typedef struct telemetryConfig_s { uint8_t extra2_rate; uint8_t extra3_rate; uint8_t version; + uint8_t min_txbuff; + uint8_t radio_type; } mavlink; } telemetryConfig_t; diff --git a/src/utils/bf2inav.py b/src/utils/bf2inav.py new file mode 100755 index 0000000000..aaf6ad2215 --- /dev/null +++ b/src/utils/bf2inav.py @@ -0,0 +1,983 @@ +# +# This script can be used to Generate a basic working target from a Betaflight Configuration. +# The idea is that this target can be used as a starting point for full INAV target. +# +# The generated target will often be enough to get INAV working, but may need some manual editing. +# +# Betaflight Configuration files are available at https://github.com/betaflight/config +# +# Common things to look for: target.c timer definitions. You may need to change timers around +# to get all features working. The script will add commented out lines for all timer possibilites +# for a given pin. + + +import sys +import os +import io +import getopt +import re +import json +import random +import string +import yaml + +version = '0.1' + +def translateFunctionName(bffunction, index): + return bffunction + '_' + index + +def translatePin(bfpin): + pin = re.sub(r'^([A-Z])0*(\d+)$', r'P\1\2', bfpin) + return pin + +def mcu2target(mcu): +#mcu STM32F405 + if mcu['type'] == 'STM32F405': + return 'target_stm32f405xg' + +#mcu STM32F411 + if mcu['type'] == 'STM32F411': + return 'target_stm32f411xe' + +#mcu STM32F7X2 + if mcu['type'] == 'STM32F7X2': + return 'target_stm32f722xe' + +#mcu STM32F745 + if mcu['type'] == 'STM32F745': + return 'target_stm32f745xg' + +#mcu STM32H743 + if mcu['type'] == 'STM32H743': + return 'target_stm32h743xi' + +#mcu 'AT32F435G' + if mcu['type'] == 'AT32F435G': + return 'target_at32f43x_xGT7' + +#mcu 'AT32F435M' + if mcu['type'] == 'AT32F435M': + return 'target_at32f43x_xMT7' + + print("Unknown MCU: %s!" % (mcu)) + sys.exit(-1) + +def getPortConfig(map): + mcu = map['mcu'] +#mcu STM32F405 + if mcu['type'] == 'STM32F405': + return """ +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff +#define TARGET_IO_PORTF 0xffff + +""" + +#mcu STM32F411 + if mcu['type'] == 'STM32F411': + return """ +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff +#define TARGET_IO_PORTF 0xffff +""" + +#mcu STM32F7X2 + if mcu['type'] == 'STM32F7X2': + return """ +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff +#define TARGET_IO_PORTF 0xffff +""" + +#mcu STM32F745 + if mcu['type'] == 'STM32F745': + return """ +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff +#define TARGET_IO_PORTF 0xffff +""" + +#mcu STM32H743 + if mcu['type'] == 'STM32H743': + return """ +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff +#define TARGET_IO_PORTF 0xffff +#define TARGET_IO_PORTG 0xffff +""" + +#mcu 'AT32F435G' + if mcu['type'] == 'AT32F435G': + return """#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTH 0xffff +""" + +#mcu 'AT32F435M' + if mcu['type'] == 'AT32F435M': + return """#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTH 0xffff +""" + + print("Unknown MCU: %s" % (mcu)) + sys.exit(-1) + +def writeCmakeLists(outputFolder, map): + file = open(outputFolder + '/CMakeLists.txt', "w+") + + t = mcu2target(map['mcu']) + + file.write("%s(%s SKIP_RELEASES)\n" % (t, map['board_name'])) + + return + + +def findPinsByFunction(function, map): + result = [] + for func in map['funcs']: + pattern = r"^%s" % (function) + if re.search(pattern, func): + #print ("%s: %s" % (function, func)) + result.append(map['funcs'][func]) + + return result + +def findPinByFunction(function, map): + if function in map['funcs']: + return map['funcs'][function] + + return None + + +def getPwmOutputCount(map): + motors = findPinsByFunction("MOTOR", map) + servos = findPinsByFunction("SERVO", map) + + return len(motors) + len(servos) + +def getGyroAlign(map): + bfalign = map['defines'].get('GYRO_1_ALIGN', 'CW0_DEG') + return bfalign + #m = re.search(r"^CW(\d+)(FLIP)?$", bfalign) + #if m: + # deg = m.group(1) + # flip = m.group(2) + # if flip: + # return "CW%s_DEG_FLIP" % (deg) + # else: + # return "CW%s_DEG" % (deg) + +def getSerialByFunction(map, function): + for serial in map.get("serial"): + if map['serial'][serial].get('FUNCTION') == function: + return serial + + return None + +def getSerialMspDisplayPort(map): + return getSerialByFunction(map, "131072") + +def getSerialRx(map): + rx = getSerialByFunction(map, "64") + if(rx != None): + return int(rx) + 1 + return None + +def writeTargetH(folder, map): + file = open(folder + '/target.h', "w+") + + file.write("""/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + * + * This target has been autgenerated by bf2inav.py + */ + +#pragma once + +//#define USE_TARGET_CONFIG + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY ) + + + \n""" + ) + board_id = ''.join(random.choice(string.ascii_uppercase) for i in range(4)) + file.write("#define TARGET_BOARD_IDENTIFIER \"%s\"\n" % (board_id)) + file.write("#define USBD_PRODUCT_STRING \"%s\"\n" % (map['board_name'])) + + # beeper + file.write("// Beeper\n") + pin = findPinByFunction('BEEPER', map) + #print ("BEEPER") + if pin: + #print ("BEEPER: %s" % (pin)) + file.write("#define USE_BEEPER\n") + file.write("#define BEEPER %s\n" % (pin)) + if 'BEEPER_INVERTED' in map['empty_defines']: + file.write("#define BEEPER_INVERTED\n") + #print ("INVERTED") + + # Leds + file.write("// Leds\n") + pin = findPinByFunction('LED_STRIP', map) + #print ("LED") + if pin: + #print ("LED: %s" % (pin)) + file.write('#define USE_LED_STRIP\n') + file.write("#define WS2811_PIN %s\n" % (pin)) + + for i in range(0, 9): + pin = findPinByFunction("LED%i" % (i), map) + if pin: + #print ("LED%i: %s" % (i, pin)) + file.write("#define LED%i %s\n" % (i, pin)) + + # Serial ports and usb + #print ("SERIAL") + file.write("// UARTs\n") + file.write("#define USB_IO\n") + file.write("#define USE_VCP\n") + serial_count = 1 + pin = findPinByFunction('USB_DETECT', map) + if pin: + file.write("#define USE_USB_DETECT\n") + file.write("#define USB_DETECT_PIN %s\n" % (pin)) + #file.write("#define VBUS_SENSING_ENABLED\n"); + + for i in range(1, 9): + txpin = findPinByFunction("UART%i_TX" % (i), map) + rxpin = findPinByFunction("UART%i_RX" % (i), map) + if txpin or rxpin: + #print ("UART%s" % (i)) + file.write("#define USE_UART%i\n" % (i)) + serial_count+=1 + else: + continue + + if rxpin: + file.write("#define UART%i_RX_PIN %s\n" % (i, rxpin)) + if txpin: + file.write("#define UART%i_TX_PIN %s\n" % (i, txpin)) + else: + file.write("#define UART%i_TX_PIN %s\n" % (i, rxpin)) + + # soft serial + for i in range(1, 9): + txpin = findPinByFunction("SOFTSERIAL%i_TX" % (i), map) + rxpin = findPinByFunction("SOFTSERIAL%i_RX" % (i), map) + idx = i + if txpin != None or rxpin != None: + #print ("SOFTUART%s" % (i)) + file.write("#define USE_SOFTSERIAL%i\n" % (idx)) + serial_count+=1 + else: + continue + + if txpin != None: + file.write("#define SOFTSERIAL_%i_TX_PIN %s\n" % (idx, txpin)) + else: + file.write("#define SOFTSERIAL_%i_TX_PIN %s\n" % (idx, rxpin)) + + if rxpin != None: + file.write("#define SOFTSERIAL_%i_RX_PIN %s\n" % (idx, rxpin)) + else: + file.write("#define SOFTSERIAL_%i_RX_PIN %s\n" % (idx, txpin)) + + file.write("#define SERIAL_PORT_COUNT %i\n" % (serial_count)) + + file.write("#define DEFAULT_RX_TYPE RX_TYPE_SERIAL\n") + file.write("#define SERIALRX_PROVIDER SERIALRX_CRSF\n") + + # TODO: map default serial uart + #serial_rx = getSerialRx(map) + serial_rx = None + + if serial_rx != None: + file.write("#define SERIALRX_UART SERIAL_PORT_USART%s\n" % (serial_rx)) + + file.write("// SPI\n") + use_spi_defined = False + for i in range(1, 9): + sckpin = findPinByFunction("SPI%i_SCK" % (i), map) + misopin = findPinByFunction("SPI%i_SDI" % (i), map) + mosipin = findPinByFunction("SPI%i_SDO" % (i), map) + if (sckpin or misopin or mosipin): + if (not use_spi_defined): + use_spi_defined = True + file.write("#define USE_SPI\n") + file.write("#define USE_SPI_DEVICE_%i\n" % (i)) + + if sckpin: + file.write("#define SPI%i_SCK_PIN %s\n" % (i, sckpin)) + if misopin: + file.write("#define SPI%i_MISO_PIN %s\n" % (i, misopin)) + if mosipin: + file.write("#define SPI%i_MOSI_PIN %s\n" % (i, mosipin)) + + use_i2c_defined = False + for i in range(1, 9): + sclpin = findPinByFunction("I2C%i_SCL" % (i), map) + sdapin = findPinByFunction("I2C%i_SDA" % (i), map) + if (sclpin or sdapin): + if (not use_i2c_defined): + file.write("// I2C\n") + #print ("I2C") + use_i2c_defined = True + file.write("#define USE_I2C\n") + file.write("#define USE_I2C_DEVICE_%i\n" % (i)) + + if sclpin: + file.write("#define I2C%i_SCL %s\n" % (i, sclpin)) + if sdapin: + file.write("#define I2C%i_SDA %s\n" % (i, sdapin)) + + if 'MAG_I2C_INSTANCE' in map['defines']: + file.write("// MAG\n") + bfinstance = map['defines']['MAG_I2C_INSTANCE'] + file.write("#define USE_MAG\n") + file.write("#define USE_MAG_ALL\n") + # (I2CDEV_1) + m = re.search(r'^\s*#define\s+MAG_I2C_INSTANCE\s+\(?I2CDEV_(\d+)\)?\s*$', bfinstance) + if m: + file.write("#define MAG_I2C_BUS BUS_I2C%i" % (m.group(1))) + + file.write("// ADC\n") + + + # ADC_BATT ch1 + use_adc = False + pin = findPinByFunction('ADC_VBAT', map) + if pin: + use_adc = True + file.write("#define ADC_CHANNEL_1_PIN %s\n" % (pin)) + file.write("#define VBAT_ADC_CHANNEL ADC_CHN_1\n"); + + # ADC_CURR ch2 + pin = findPinByFunction('ADC_CURR', map) + if pin: + use_adc = True + file.write("#define ADC_CHANNEL_2_PIN %s\n" % (pin)) + file.write("#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2\n"); + # ADC_RSSI ch3 + pin = findPinByFunction('ADC_RSSI', map) + if pin: + use_adc = True + file.write("#define ADC_CHANNEL_3_PIN %s\n" % (pin)) + file.write("#define RSSI_ADC_CHANNEL ADC_CHN_3\n"); + + # ADC_EXT ch4 (airspeed?) + pin = findPinByFunction('ADC_EXT_1', map) + if pin: + use_adc = True + file.write("#define ADC_CHANNEL_4_PIN %s\n" % (pin)) + file.write("#define AIRSPEED_ADC_CHANNEL ADC_CHN_4\n"); + + if use_adc: + file.write("#define USE_ADC\n") + file.write("#define ADC_INSTANCE ADC1\n") + # TODO: + #define ADC1_DMA_STREAM DMA2_Stream4 + + file.write("// Gyro & ACC\n") + for supportedgyro in ['BMI160', 'BMI270', 'ICM20689', 'ICM42605', 'MPU6000', 'MPU6500', 'MPU9250']: + found = False + for var in ['USE_ACCGYRO_', 'USE_ACC_', 'USE_ACC_SPI', 'USE_GYRO_', 'USE_GYRO_SPI_']: + val = var + supportedgyro + if val in map['empty_defines']: + found = True + break + + if found: + #print (supportedgyro) + file.write("#define USE_IMU_%s\n" % (supportedgyro)) + file.write("#define %s_CS_PIN %s\n" % (supportedgyro, findPinByFunction('GYRO_1_CS', map))) + file.write("#define %s_SPI_BUS BUS_%s\n" % (supportedgyro, map['defines']['GYRO_1_SPI_INSTANCE'])) + file.write("#define IMU_%s_ALIGN %s\n" % (supportedgyro, getGyroAlign(map))) + + + if 'USE_BARO' in map['empty_defines']: + #print ("BARO") + file.write("// BARO\n") + file.write("#define USE_BARO\n") + if 'BARO_I2C_INSTANCE' in map['defines']: + file.write("#define USE_BARO_ALL\n") + m = re.search(r'I2CDEV_(\d+)', map['defines']['BARO_I2C_INSTANCE']) + if m: + file.write("#define BARO_I2C_BUS BUS_I2C%s\n" % (m.group(1))) + if 'BARO_SPI_INSTANCE' in map['defines']: + file.write("#define USE_BARO_BMP280\n") + file.write("#define USE_BARO_SPI_BMP280\n") + file.write("#define BMP280_SPI_BUS BUS_%s\n" % (map['defines']['BARO_SPI_INSTANCE'])) + file.write("#define BMP280_CS_PIN %s\n" % (findPinByFunction('BARO_CS', map))) + + file.write("// OSD\n") + if 'USE_MAX7456' in map['empty_defines']: + #print ("ANALOG OSD") + file.write("#define USE_MAX7456\n") + pin = findPinByFunction('MAX7456_SPI_CS', map) + file.write("#define MAX7456_CS_PIN %s\n" % (pin)) + file.write("#define MAX7456_SPI_BUS BUS_%s\n" % (map['defines']['MAX7456_SPI_INSTANCE'])) + file.write("// Blackbox\n") + + # Flash: + if 'USE_FLASH' in map['empty_defines']: + #print ("FLASH BLACKBOX") + cs = findPinByFunction("FLASH_CS", map) + spiflash_bus = map['defines'].get('FLASH_SPI_INSTANCE') + if cs: + # TODO: add more drivers + suppored_flash_chips = [ + 'M25P16', + 'W25M', + 'W25M02G', + 'W25M512', + 'W25N01G' + ] + file.write("#define USE_FLASHFS\n") + file.write("#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT\n") + for flash in suppored_flash_chips: + file.write("#define USE_FLASH_%s\n" % (flash)) + file.write("#define %s_SPI_BUS BUS_%s\n" % (flash, spiflash_bus)) + file.write("#define %s_CS_PIN %s\n" % (flash, cs)) + + # SD Card: + use_sdcard = False + for i in range(1, 9): + sdio_cmd = findPinByFunction("SDIO_CMD_%i" % (i), map) + + if sdio_cmd: + if not use_sdcard: + file.write("#define USE_SDCARD\n") + file.write("#define USE_SDCARD_SDIO\n") + file.write("#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT\n") + use_sdcard = True + file.write("#define SDCARD_SDIO_4BIT\n") + file.write("#define SDCARD_SDIO_DEVICE SDIODEV_%i\n" % (i)) + + # PINIO + use_pinio = False + for i in range(1, 9): + pinio = findPinByFunction("PINIO%i" % (i), map) + if pinio != None: + if not use_pinio: + use_pinio = True + file.write("\n// PINIO\n\n") + file.write("#define USE_PINIO\n") + file.write("#define USE_PINIOBOX\n") + file.write("#define PINIO%i_PIN %s\n" % (i, pinio)) + + file.write("\n\n// Others\n\n") + + pwm_outputs = getPwmOutputCount(map) + file.write("#define MAX_PWM_OUTPUT_PORTS %i\n" % (pwm_outputs)) + file.write("#define USE_SERIAL_4WAY_BLHELI_INTERFACE\n") + + file.write("#define USE_DSHOT\n") + file.write("#define USE_ESC_SENSOR\n") + + if 'DEFAULT_VOLTAGE_METER_SCALE' in map['defines']: + file.write("#define VOLTAGE_METER_SCALE %s\n" % (map['defines']['DEFAULT_VOLTAGE_METER_SCALE'])) + if 'DEFAULT_CURRENT_METER_SCALE' in map['defines']: + file.write("#define CURRENT_METER_SCALE %s\n" % (map['defines']['DEFAULT_CURRENT_METER_SCALE'])) + + + port_config = getPortConfig(map) + + file.write(port_config) + + file.close() + return + + +def mcu2timerKey(mcu): + m = re.search(r'^AT32F435[GM]', mcu) + if m: + return 'AT32F435' + + m = re.search(r'^STM32F405', mcu) + if m: + return 'STM32F405' + + m = re.search(r'^STM32F7[2Xx]2', mcu) + if m: + return 'STM32F722' + + m = re.search(r'^STM32F7[Xx46]5', mcu) + if m: + return 'STM32F745' + + m = re.search(r'^STM32H7[45]3', mcu) + if m: + return 'STM32H743' + + print ("Unsupported MCU: %s" % (mcu)) + sys.exit(-1) + + +def getTimerInfo(map, pin): + with open("timer_pins.yaml", "r") as f: + pindb = yaml.safe_load(f) + f.close() + + mcu = map['mcu']['type'] + tk = mcu2timerKey(mcu) + + if not tk in pindb: + print ("PINDB not available for MCU: %s" % (mcu)) + sys.exit(-1) + + timers = pindb[tk].get(pin, None) + + if timers: + result = [] + for ti in timers: + timer = list(ti.keys())[0] + channel = ti[timer] + + result.append([timer, channel]) + + return result + + return None + +def writeTargetC(folder, map): + file = open(folder + '/target.c', "w+") + + file.write("""/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + * + * This target has been autgenerated by bf2inav.py + */ + +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/pinio.h" +//#include "drivers/sensor.h" + +""") + + #for supportedgyro in ['BMI160', 'BMI270', 'ICM20689', 'ICM42605', 'MPU6000', 'MPU6500', 'MPU9250']: + # found = False + # for var in ['USE_ACCGYRO_', 'USE_ACC_', 'USE_ACC_SPI', 'USE_GYRO_', 'USE_GYRO_SPI_']: + # val = var + supportedgyro + # if val in map['empty_defines']: + # found = True + # break + + # if found: + # file.write("//BUSDEV_REGISTER_SPI_TAG(busdev_%s, DEVHW_%s, %s_SPI_BUS, %s_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_%s_ALIGN);\n" % (supportedgyro.lower(), supportedgyro, supportedgyro, supportedgyro, supportedgyro)) + + snum=1 + file.write("\ntimerHardware_t timerHardware[] = {\n") + + motors = findPinsByFunction("MOTOR", map) + if motors: + for motor in motors: + timerInfo = getTimerInfo(map, motor) + if timerInfo: + first = True + #print (timerInfo) + for (t, ch) in timerInfo: + if first: + file.write(" DEF_TIM(%s, %s, %s, TIM_USE_OUTPUT_AUTO, 0, %s), // S%i\n" % (t, ch, motor, 0, snum)) + first = False + snum += 1 + else: + file.write(" //DEF_TIM(%s, %s, %s, TIM_USE_OUTPUT_AUTO, 0, %s),\n" % (t, ch, motor, 0)) + file.write("\n") + + servos = findPinsByFunction("SERVO", map) + if servos: + for servo in servos: + timerInfo = getTimerInfo(map, servo) + if timerInfo: + first = True + #print (timerInfo) + for (t, ch) in timerInfo: + if first: + file.write(" DEF_TIM(%s, %s, %s, TIM_USE_OUTPUT_AUTO, 0, %s), // S%i\n" % (t, ch, servo, 0, snum)) + first = False + snum += 1 + else: + file.write(" //DEF_TIM(%s, %s, %s, TIM_USE_OUTPUT_AUTO, 0, %s),\n" % (t, ch, servo, 0)) + file.write("\n") + + beeper = findPinByFunction("BEEPER", map) + if beeper: + timerInfo = getTimerInfo(map, beeper) + if timerInfo: + first = True + #print ("BEEPER: %s" % (timerInfo)) + for (t, ch) in timerInfo: + if first: + file.write(" DEF_TIM(%s, %s, %s, TIM_USE_BEEPER, 0, %s),\n" % (t, ch, beeper, 0)) + first = False + else: + file.write(" //DEF_TIM(%s, %s, %s, TIM_USE_BEEPER, 0, %s),\n" % (t, ch, beeper, 0)) + file.write("\n") + + led = findPinByFunction("LED_STRIP", map) + if led: + timerInfo = getTimerInfo(map, led) + if timerInfo: + first = True + #print (timerInfo) + for (t, ch) in timerInfo: + if first: + file.write(" DEF_TIM(%s, %s, %s, TIM_USE_LED, 0, %s),\n" % (t, ch, led, 0)) + first = False + else: + file.write(" //DEF_TIM(%s, %s, %s, TIM_USE_LED, 0, %s),\n" % (t, ch, led, 0)) + file.write("\n") + + file.write("""}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); +""") + + file.close() + return + +def writeConfigC(folder, map): + file = open(folder + '/config.c', "w+") + + file.write("""/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + * + * This target has been autgenerated by bf2inav.py + */ + +#include + +#include "platform.h" + +#include "fc/fc_msp_box.h" +#include "fc/config.h" + +#include "io/piniobox.h" + +void targetConfiguration(void) +{ +""") + #//pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + #//pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; + #//beeperConfigMutable()->pwmMode = true; + file.write(""" +} + +""") + + file.close() + return + +def writeTarget(outputFolder, map): + writeCmakeLists(outputFolder, map) + writeTargetH(outputFolder, map) + writeTargetC(outputFolder, map) + writeConfigC(outputFolder, map) + + return + +def buildMap(inputFile): + map = { 'defines': {}, 'empty_defines': [], 'features': ['FEATURE_OSD', 'FEATURE_TELEMETRY', 'FEATURE_CURRENT_METER', 'FEATURE_VBAT', 'FEATURE_TX_PROF_SEL', 'FEATURE_BLACKBOX'], 'pins': {}, 'funcs': {}, 'timer_pin_map': {}} + + f = open(inputFile, 'r') + while True: + l = f.readline() + if not l: + break + m = re.search(r'^#define\s+FC_TARGET_MCU\s+([0-9A-Za-z]+)$', l) + if m: + map['mcu'] = {'type': m.group(1)} + + m = re.search(r'^#define\s+BOARD_NAME\s+(\w+)$', l) + if m: + map['board_name'] = m.group(1) + + m = re.search(r'^#define\s+MANUFACTURER_ID\s+(\w+)$', l) + if m: + map['manufacturer_id'] = m.group(1) + + m = re.search(r'^#define\s+(\w+)\s*$', l) + if m: + map['empty_defines'].append(re.sub('ICM42688P', 'ICM42605', m.group(1))) + + m = re.search(r'^\s*#define\s+DEFAULT_FEATURES\s+\((.+?)\)\s*$', l) + if m: + features = m.group(1).split('|') + for feat in features: + feat = feat.strip() + if not feat in map['features']: + map['features'].append(feat) + + m = re.search(r'^#define\s+(\w+)\s+(\S+)\s*$', l) + if m: + map['defines'][m.group(1)] = m.group(2) + + + + # i: timer index + # p: pin + # o: timer channel? 1 = first + # d: dma opts + # i p o d + # TIMER_PIN_MAP( 0, PB8 , 2, -1) \ + m = re.search(r'^\s*TIMER_PIN_MAP\s*\(\s*(\d+)\s*,\s*([0-9A-Za-z]+)\s*,\s*(\d+)\s*,\s*(-?\d+)\s*\).+', l) + if m: + map['timer_pin_map'][m.group(1)] = { + 'i': m.group(1), + 'p': m.group(2), + 'o': m.group(3), + 'd': m.group(4) + } + + + m = re.search(r'^\s*#define\s+(\w+)_PIN\s+([A-Z0-9]+)\s*$', l) + if m: + pin = m.group(2) + func = m.group(1) + if not map['funcs'].get(func): + map['funcs'][func] = {} + + map['funcs'][func] = pin + + if not map['pins'].get(pin): + map['pins'][pin] = {} + + map['pins'][pin] = func + + + #m = re.search(r'^feature\s+(-?\w+)$', l) + #if m: + # map['features'].append(m.group(1)) + + #m = re.search(r'^resource\s+(-?\w+)\s+(\d+)\s+(\w+)$', l) + #if m: + # resource_type = m.group(1) + # resource_index = m.group(2) + # pin = translatePin(m.group(3)) + # if not map['pins'].get(pin): + # map['pins'][pin] = {} + + # map['pins'][pin]['function'] = translateFunctionName(resource_type, resource_index) + + #m = re.search(r'^timer\s+(\w+)\s+AF(\d+)$', l) + #if m: + # pin = translatePin(m.group(1)) + # if not map['pins'].get(pin): + # map['pins'][pin] = {} + # + # map['pins'][pin]['AF'] = m.group(2) + + #m = re.search(r'^#\s*pin\s+(\w+):\s*(TIM\d+)\s+(CH\d+).+$', l) + #if m: + # pin = translatePin(m.group(1)) + # if not map['pins'].get(pin): + # map['pins'][pin] = {} + + # map['pins'][pin]['TIM'] = m.group(2) + # map['pins'][pin]['CH'] = m.group(3) + + #m = re.search(r'^dma\s+([A-Za-z0-9]+)\s+([A-Za-z0-9]+)\s+(\d+).*$', l) + #if m: + # if(m.group(1) == 'ADC'): + # pin = 'ADC' + m.group(2) + # else: + # pin = translatePin(m.group(2)) + # if not map['dmas'].get(pin): + # map['dmas'][pin] = {} + # map['dmas'][pin]['DMA'] = m.group(3) + +# 1 2 3 4 +# pin B04: DMA1 Stream 4 Channel 5 + + #m = re.search(r'^#\s+pin\s+(\w+):\s+(DMA\d+)\s+Stream\s+(\d+)\s+Channel\s+(\d+)\s*$', l) + #if m: + # pin = translatePin(m.group(1)) + # if not map['pins'].get(pin): + # map['pins'][pin] = {} + + # map['pins'][pin]['DMA_STREAM'] = m.group(3) + # map['pins'][pin]['DMA_CHANNEL'] = m.group(4) + + #m = re.search(r'^#\s+ADC\s+(\d+):\s+(DMA\d+)\s+Stream\s+(\d+)\s+Channel\s+(\d+)\s*$', l) + #if m: + # pin = 'ADC' + m.group(1) + # if not map['dmas'].get(pin): + # map['dmas'][pin] = {} + + # map['dmas'][pin]['DMA_STREAM'] = m.group(3) + # map['dmas'][pin]['DMA_CHANNEL'] = m.group(4) + + #m = re.search(r'^#\s+TIMUP\s+(\d+):\s+(DMA\d+)\s+Stream\s+(\d+)\s+Channel\s+(\d+)\s*$', l) + #if m: + # pin = 'TIMUP' + m.group(1) + # if not map['dmas'].get(pin): + # map['dmas'][pin] = {} + # map['dmas'][pin]['DMA_STREAM'] = m.group(3) + # map['dmas'][pin]['DMA_CHANNEL'] = m.group(4) + + #m = re.search(r'^#\s+ADC\s+(\d+):\s+(DMA\d+)\s+Stream\s+(\d+)\s+Request\s+(\d+)\s*$', l) + #if m: + # pin = 'ADC' + m.group(1) + # if not map['dmas'].get(pin): + # map['dmas'][pin] = {} + + # map['dmas'][pin]['DMA_STREAM'] = m.group(3) + # map['dmas'][pin]['DMA_REQUEST'] = m.group(4) + + #m = re.search(r'^#\s+TIMUP\s+(\d+):\s+(DMA\d+)\s+Stream\s+(\d+)\s+Channel\s+(\d+)\s*$', l) + #if m: + # pin = 'TIMUP' + m.group(1) + # if not map['dmas'].get(pin): + # map['dmas'][pin] = {} + + # map['dmas'][pin]['DMA_STREAM'] = m.group(3) + # map['dmas'][pin]['DMA_CHANNEL'] = m.group(4) + + #m = re.search(r'^#\s+TIMUP\s+(\d+):\s+(DMA\d+)\s+Stream\s+(\d+)\s+Request\s+(\d+)\s*$', l) + #if m: + # pin = 'TIMUP' + m.group(1) + # if not map['dmas'].get(pin): + # map['dmas'][pin] = {} + # map['dmas'][pin]['DMA_STREAM'] = m.group(3) + # map['dmas'][pin]['DMA_REQUEST'] = m.group(4) + + #m = re.search(r'^serial\s+(\d+)\s+(\d+)\s+(\d+)\s+(\d+)\s+(\d+)\s+(\d+)$', l) + #if m: + # idx = m.group(1) + # if not map['serial'].get(idx): + # map['serial'][idx] = {} + + # map['serial'][idx]['FUNCTION'] = m.group(2) + # map['serial'][idx]['MSP_BAUD'] = m.group(3) + # map['serial'][idx]['GPS_BAUD'] = m.group(4) + # map['serial'][idx]['TELEMETRY_BAUD'] = m.group(5) + # map['serial'][idx]['BLACKBOX_BAUD'] = m.group(6) + + #m = re.search(r'^set\s+(\w+)\s*=\s*(\w+)$', l) + #if m: + # map['variables'][m.group(1)] = m.group(2) + + + return map + +def printHelp(): + print ("%s -i bf-target.config -o output-directory" % (sys.argv[0])) + print (" -i | --input-config= -- print this help") + print (" -o | --output-dir= -- print this help") + print (" -h | --help -- print this help") + print (" -v | --version -- print version") + return + +def main(argv): + inputfile = '' + outputdir = '.' + global version + + try: + opts, args = getopt.getopt(argv,"hvi:o:", ["input-config=", "output-dir=", 'version', 'help']) + except getopt.GeoptError: + printHelp() + sys.exit(2) + + for opt, arg in opts: + if opt in ('-h', '--help'): + printHelp() + sys.exit(1) + elif opt in ('-i', '--input-config'): + inputfile = arg + elif opt in ('-o', '--output-dir'): + outputdir = arg + elif opt in ('-v', '--version'): + print ("%s: %s" % (sys.argv[0], version)) + sys.exit(0) + + if (not os.path.isfile(inputfile) ): + print("no such file %s" % inputfile) + sys.exit(2) + if (not os.path.isdir(outputdir) ): + print("no such directory %s" % outputdir) + sys.exit(2) + else: + targetDefinition = buildMap(inputfile) + + + map = buildMap(inputfile) + + print (json.dumps(map, indent=4)) + + writeTarget(outputdir, map) + + sys.exit(0) + + +if( __name__ == "__main__"): + main(sys.argv[1:]) + + diff --git a/src/utils/generate-prlist-rn.rb b/src/utils/generate-prlist-rn.rb new file mode 100755 index 0000000000..9249679488 --- /dev/null +++ b/src/utils/generate-prlist-rn.rb @@ -0,0 +1,17 @@ +#!/usr/bin/env ruby + +# This script creates a list of **merged** PRs for a release + +# gh --repo inavflight/inav pr list --state merged -S "milestone:8.0" -L 500 --json "number,title,author,url" > /tmp/gh.json +# Then process the output `/tmp/gh.json` with this script +# ./generate-prlist-rn.rb /tmp/gh.json > /tmp/rel8prs.md +# +# Then merge the contents of `/tmp/rel8prs.md` into the release notes + +require 'json' +abort("Need the JSON file") unless ARGV[0] +text = File.read(ARGV[0]) +jsa = JSON.parse(text) +jsa.each do |js| + puts "* #%d %s by @%s\n" % [js['number'],js['title'],js['author']['login']] +end diff --git a/src/utils/timer_pins.yaml b/src/utils/timer_pins.yaml new file mode 100644 index 0000000000..f1b2eb699b --- /dev/null +++ b/src/utils/timer_pins.yaml @@ -0,0 +1,760 @@ +AT32F435: + PA0: + - TMR2: 'CH1' + - TMR5: 'CH1' + PA1: + - TMR2: 'CH2' + - TMR5: 'CH2' + PA2: + - TMR2: 'CH3' + - TMR5: 'CH3' + - TMR9: 'CH1' + PA3: + - TMR2: 'CH4' + - TMR5: 'CH4' + - TMR9: 'CH2' + PA5: + - TMR2: 'CH1' + - TMR8: 'CH1C' + PA6: + - TMR3: 'CH1' + PA7: + - TMR1: 'CH1C' + - TMR3: 'CH2' + - TMR8: 'CH1C' + PA8: + - TMR1: 'CH1' + PA9: + - TMR1: 'CH2' + PA10: + - TMR1: 'CH3' + PA11: + - TMR1: 'CH4' + PA15: + - TMR2: 'CH1' + PB0: + - TMR1: 'CH2C' + - TMR3: 'CH3' + - TMR8: 'CH2C' + PB1: + - TMR1: 'CH3C' + - TMR3: 'CH4' + - TMR8: 'CH3C' + PB2: + - TMR2: 'CH4' + - TMR20: 'CH1' + PB3: + - TMR2: 'CH2' + PB4: + - TMR3: 'CH1' + PB5: + - TMR3: 'CH2' + PB6: + - TMR4: 'CH1' + PB7: + - TMR4: 'CH2' + PB8: + - TMR2: 'CH1' + - TMR4: 'CH3' + - TMR10: 'CH1' + PB9: + - TMR2: 'CH2' + - TMR4: 'CH4' + - TMR11: 'CH1' + PB10: + - TMR2: 'CH3' + PB11: + - TMR2: 'CH4' + - TMR5: 'CH4' + PB12: + - TMR5: 'CH1' + PB13: + - TMR1: 'CH1C' + PB14: + - TMR1: 'CH2C' + - TMR8: 'CH2C' + PB15: + - TMR1: 'CH3C' + - TMR8: 'CH3C' + PC2: + - TMR20: 'CH2' + PC4: + - TMR9: 'CH1' + PC5: + - TMR9: 'CH2' + PC6: + - TMR3: 'CH1' + - TMR8: 'CH1' + PC7: + - TMR3: 'CH2' + - TMR8: 'CH2' + PC8: + - TMR3: 'CH3' + - TMR8: 'CH3' + PC9: + - TMR3: 'CH4' + - TMR8: 'CH4' + PC10: + - TMR5: 'CH2' + PC11: + - TMR5: 'CH3' + PC12: + - TMR11: 'CH1C' + PD12: + - TMR4: 'CH1' + PD13: + - TMR4: 'CH2' + PD14: + - TMR4: 'CH3' + PD15: + - TMR4: 'CH4' + PE3: + - TMR3: 'CH1' + PE4: + - TMR3: 'CH2' + PE5: + - TMR3: 'CH3' + - TMR9: 'CH1' + PE6: + - TMR3: 'CH4' + - TMR9: 'CH2' + PE8: + - TMR1: CH1C + PE9: + - TMR1: CH1 + PE10: + - TMR1: CH2C + PE11: + - TMR1: CH2 + PE12: + - TMR1: CH3C + PE13: + - TMR1: CH3 + PE14: + - TMR1: CH4 + PF2: + - TMR20: CH3 + PF3: + - TMR20: CH4 + PF4: + - TMR20: CH1C + PF5: + - TMR20: CH2C + PF6: + - TMR20: CH4 + - TMR10: CH1 + PF7: + - TMR11: CH1 + PF10: + - TMR5: CH4 + PF12: + - TMR20: CH1 + PF13: + - TMR20: CH2 + PF14: + - TMR20: CH3 + PF15: + - TMR20: CH4 + PG0: + - TMR20: CH1C + PG1: + - TMR20: CH2C + PG2: + - TMR20: CH3C + PH2: + - TMR5: CH1 + PH3: + - TMR5: CH2 +STM32F405: + PE5: + - TIM9: CH1 + PE6: + - TIM9: CH2 + PF6: + - TIM10: CH1 + PF7: + - TIM11: CH1 + PF8: + - TIM13: CH1 + PF9: + - TIM14: CH1 + PA0: + - TIM5: CH1 + PA1: + - TIM5: CH2 + - TIM2: CH2 + PA2: + - TIM5: CH3 + - TIM9: CH1 + - TIM2: CH3 + PA3: + - TIM5: CH4 + - TIM9: CH2 + - TIM2: CH4 + PA5: + - TIM8: CH1N + PA6: + - TIM13: CH1 + - TIM3: CH1 + PA7: + - TIM8: CH1N + - TIM14: CH1 + - TIM3: CH2 + - TIM1: CH1N + PB0: + - TIM3: CH3 + - TIM8: CH2N + - TIM1: CH2N + PB1: + - TIM3: CH4 + - TIM8: CH3N + - TIM1: CH3N + PE8: + - TIM1: CH1N + PE9: + - TIM1: CH1 + PE10: + - TIM1: CH2N + PE11: + - TIM1: CH2 + PE12: + - TIM1: CH3N + PE13: + - TIM1: CH3 + PE14: + - TIM1: CH4 + PB10: + - TIM2: CH3 + PB11: + - TIM2: CH3 + PH6: + - TIM13: CH1 + PH9: + - TIM12: CH2 + PH10: + - TIM5: CH1 + PH11: + - TIM5: CH2 + PH12: + - TIM5: CH3 + PB13: + - TIM1: CH1N + PB14: + - TIM1: CH2N + - TIM12: CH1 + - TIM8: CH2N + PB15: + - TIM1: CH3N + - TIM8: CH3N + - TIM12: CH2 + PD12: + - TIM14: CH1 + PD13: + - TIM14: CH2 + PD14: + - TIM4: CH3 + PD5: + - TIM4: CH3 + PC6: + - TIM8: CH1 + - TIM3: CH1 + PC7: + - TIM8: CH2 + - TIM3: CH2 + PC8: + - TIM8: CH3 + - TIM3: CH3 + PC9: + - TIM8: CH4 + - TIM3: CH4 + PA8: + - TIM1: CH1 + PA9: + - TIM1: CH2 + PA10: + - TIM1: CH3 + PA11: + - TIM1: CH4 + PH13: + - TIM8: CH1N + PH14: + - TIM8: CH2N + PH14: + - TIM8: CH3N + PI0: + - TIM5: CH4 + PI2: + - TIM8: CH4 + PB3: + - TIM2: CH2 + PB4: + - TIM3: CH1 + PB5: + - TIM3: CH2 + PB6: + - TIM4: CH1 + PB7: + - TIM4: CH2 + PB8: + - TIM4: CH3 + - TIM10: CH1 + PB9: + - TIM4: CH4 + - TIM11: CH1 + PI5: + - TIM8: CH1 + PI6: + - TIM8: CH2 + PI7: + - TIM8: CH3 +STM32H743: + PA0: + - TIM2: CH1 + - TIM5: CH1 + PA1: + - TIM2: CH2 + - TIM5: CH2 + - TIM15: CH1N + PA2: + - TIM2: CH3 + - TIM5: CH3 + - TIM15: CH1 + PA5: + - TIM2: CH1 + - TIM8: CH1N + PA6: + - TIM3: CH1 + PA7: + - TIM1: CH1N + - TIM3: CH2 + - TIM8: CH1N + PA8: + - TIM1: CH1 + PA9: + - TIM1: CH2 + PA10: + - TIM1: CH3 + PA11: + - TIM1: CH4 + PA15: + - TIM2: CH1 + PB0: + - TIM1: CH2N + - TIM3: CH3 + - TIM8: CH2N + PB1: + - TIM1: CH3N + - TIM3: CH4 + - TIM8: CH3N + PB3: + - TIM2: CH2 + PB4: + - TIM3: CH1 + PB5: + - TIM3: CH2 + PB6: + - TIM16: CH1N + - TIM4: CH1 + PB7: + - TIM17: CH1N + - TIM4: CH2 + PB8: + - TIM16: CH1 + - TIM4: CH3 + PB9: + - TIM17: CH1 + - TIM4: CH4 + PB10: + - TIM2: CH3 + PB11: + - TIM2: CH4 + PB13: + - TIM1: CH1N + PB14: + - TIM1: CH2N + - TIM12: CH1 + - TIM8: CH2N + PB15: + - TIM1: CH3N + - TIM12: CH2 + - TIM8: CH3N + PC6: + - TIM3: CH1 + - TIM8: CH1 + PC7: + - TIM3: CH2 + - TIM8: CH2 + PC8: + - TIM3: CH3 + - TIM8: CH3 + PC9: + - TIM3: CH4 + - TIM8: CH4 + PD12: + - TIM4: CH1 + PD13: + - TIM4: CH2 + PD14: + - TIM4: CH3 + PD15: + - TIM4: CH4 + PE4: + - TIM15: CH1N + PE5: + - TIM15: CH1 + PE6: + - TIM15: CH2 + PE8: + - TIM1: CH1N + PE9: + - TIM1: CH1 + PE10: + - TIM1: CH2N + PE11: + - TIM1: CH2 + PE12: + - TIM1: CH3N + PE13: + - TIM1: CH3 + PE14: + - TIM1: CH4 + PF6: + - TIM16: CH1 + PF7: + - TIM17: CH1 + PF8: + - TIM16: CH1N + PF9: + - TIM17: CH1N + PH6: + - TIM12: CH1 + PH9: + - TIM12: CH2 + PH10: + - TIM5: CH1 + PH11: + - TIM5: CH2 + PH12: + - TIM5: CH3 + PH13: + - TIM8: CH1N + PH14: + - TIM8: CH2N + PH15: + - TIM8: CH3N + PI0: + - TIM5: CH4 + PI2: + - TIM8: CH4 + PI5: + - TIM8: CH1 + PI6: + - TIM8: CH2 + PI7: + - TIM8: CH3 + PJ6: + - TIM8: CH2 + PJ7: + - TIM8: CH2N + PJ8: + - TIM1: CH3N + - TIM8: CH1 + PJ9: + - TIM1: CH3 + - TIM8: CH1N + PJ10: + - TIM1: CH2N + - TIM8: CH2 + PJ11: + - TIM1: CH2 + - TIM8: CH2N + PK0: + - TIM1: CH1N + - TIM8: CH3 + PK1: + - TIM1: CH1 + - TIM8: CH3N +STM32F722: + PA0: + - TIM2: CH1 + - TIM5: CH1 + PA1: + - TIM2: CH2 + - TIM5: CH2 + PA2: + - TIM2: CH3 + - TIM5: CH3 + - TIM9: CH1 + PA3: + - TIM2: CH4 + - TIM5: CH4 + - TIM9: CH2 + PA5: + - TIM2: CH1 + - TIM8: CH1N + PA6: + - TIM3: CH1 + - TIM13: CH1 + PA7: + - TIM1: CH1N + - TIM3: CH2 + - TIM8: CH1N + - TIM14: CH1 + PA8: + - TIM1: CH1 + PA9: + - TIM1: CH2 + PA10: + - TIM1: CH3 + PA11: + - TIM1: CH4 + PA15: + - TIM2: CH1 + PB0: + - TIM1: CH2N + - TIM3: CH3 + - TIM8: CH2N + PB1: + - TIM1: CH3N + - TIM3: CH4 + - TIM8: CH3N + PB3: + - TIM2: CH2 + PB4: + - TIM3: CH1 + PB5: + - TIM3: CH2 + PB6: + - TIM4: CH1 + PB7: + - TIM4: CH2 + PB8: + - TIM4: CH3 + - TIM10: CH1 + PB9: + - TIM4: CH4 + - TIM11: CH1 + PB10: + - TIM2: CH3 + PB11: + - TIM2: CH4 + PB13: + - TIM1: CH1N + PB14: + - TIM1: CH2N + - TIM8: CH2N + - TIM12: CH1 + PB15: + - TIM1: CH3N + - TIM8: CH3N + - TIM12: CH2 + PC6: + - TIM3: CH1 + - TIM8: CH1 + PC7: + - TIM3: CH2 + - TIM8: CH2 + PC8: + - TIM3: CH3 + - TIM8: CH3 + PC9: + - TIM3: CH4 + - TIM8: CH4 + PD12: + - TIM4: CH1 + PD13: + - TIM4: CH2 + PD14: + - TIM4: CH3 + PD15: + - TIM4: CH4 + PE5: + - TIM9: CH1 + PE6: + - TIM9: CH2 + PE8: + - TIM1: CH1N + PE9: + - TIM1: CH1 + PE10: + - TIM1: CH2N + PE11: + - TIM1: CH2 + PE12: + - TIM1: CH3N + PE13: + - TIM1: CH3 + PE14: + - TIM1: CH4 + PF6: + - TIM10: CH1 + PF7: + - TIM10: CH1 + PH6: + - TIM12: CH1 + PH9: + - TIM12: CH2 + PH10: + - TIM5: CH1 + PH11: + - TIM5: CH2 + PH12: + - TIM5: CH3 + PH13: + - TIM8: CH1N + PH14: + - TIM8: CH2N + PH15: + - TIM8: CH3N + PI0: + - TIM5: CH4 + PI2: + - TIM8: CH4 + PI5: + - TIM8: CH1 + PI6: + - TIM8: CH2 + PI7: + - TIM8: CH3 +STM32F745: + PA0: + - TIM2: CH1 + - TIM5: CH1 + PA1: + - TIM2: CH2 + - TIM5: CH2 + PA2: + - TIM2: CH3 + - TIM5: CH3 + - TIM9: CH1 + PA3: + - TIM2: CH4 + - TIM5: CH4 + - TIM9: CH2 + PA5: + - TIM2: CH1 + - TIM8: CH1N + PA6: + - TIM3: CH1 + PA7: + - TIM1: CH1N + - TIM3: CH2 + - TIM8: CH1N + PA8: + - TIM1: CH1 + PA9: + - TIM1: CH2 + PA10: + - TIM1: CH3 + PA11: + - TIM1: CH4 + PA15: + - TIM2: CH1 + PB0: + - TIM1: CH2N + - TIM3: CH3 + - TIM8: CH2N + PB1: + - TIM1: CH3N + - TIM3: CH4 + - TIM8: CH3N + PB3: + - TIM2: CH2 + PB4: + - TIM3: CH1 + PB5: + - TIM3: CH2 + PB6: + - TIM4: CH1 + PB7: + - TIM4: CH2 + PB8: + - TIM4: CH3 + - TIM10: CH1 + PB9: + - TIM4: CH4 + - TIM11: CH1 + PB10: + - TIM2: CH3 + PB11: + - TIM2: CH4 + PB13: + - TIM1: CH1N + PB14: + - TIM1: CH2N + - TIM8: CH2N + PB15: + - TIM1: CH3N + - TIM8: CH3N + PC6: + - TIM3: CH1 + - TIM8: CH1 + PC7: + - TIM3: CH2 + - TIM8: CH2 + PC8: + - TIM3: CH3 + - TIM8: CH3 + PC9: + - TIM3: CH4 + - TIM8: CH4 + PD12: + - TIM4: CH1 + PD13: + - TIM4: CH2 + PD14: + - TIM4: CH3 + PD15: + - TIM4: CH4 + PE5: + - TIM9: CH1 + PE6: + - TIM9: CH2 + PE8: + - TIM1: CH1N + PE9: + - TIM1: CH1 + PE10: + - TIM1: CH2N + PE11: + - TIM1: CH2 + PE12: + - TIM1: CH3N + PE13: + - TIM1: CH3 + PE14: + - TIM1: CH4 + PF6: + - TIM10: CH1 + PF7: + - TIM11: CH1 + PF8: + - TIM13: CH1 + PF9: + - TIM14: CH1 + PH6: + - TIM12: CH1 + PH9: + - TIM12: CH2 + PH10: + - TIM5: CH1 + PH11: + - TIM5: CH2 + PH12: + - TIM5: CH3 + PH13: + - TIM8: CH1N + PH14: + - TIM8: CH2N + PH15: + - TIM8: CH3N + PI0: + - TIM5: CH4 + PI2: + - TIM8: CH4 + PI5: + - TIM8: CH1 + PI6: + - TIM8: CH2 + PI7: + - TIM8: CH3 \ No newline at end of file