diff --git a/.ci/Jenkinsfile-compile b/.ci/Jenkinsfile-compile index c482b14d459d..fcf70a5a2fdc 100644 --- a/.ci/Jenkinsfile-compile +++ b/.ci/Jenkinsfile-compile @@ -35,6 +35,7 @@ pipeline { def nuttx_builds_archive = [ target: [ + "3dr_ctrl-zero-h7-oem-revg_default", "airmind_mindpx-v2_default", "ark_can-flow_canbootloader", "ark_can-flow_default", diff --git a/.editorconfig b/.editorconfig index 683f0fdc5159..1ac25a4f5860 100644 --- a/.editorconfig +++ b/.editorconfig @@ -9,6 +9,6 @@ tab_width = 8 # Not in the official standard, but supported by many editors max_line_length = 120 -[*.yaml] +[*.yaml, *.yml] indent_style = space indent_size = 2 diff --git a/.github/ISSUE_TEMPLATE/bug_report.yml b/.github/ISSUE_TEMPLATE/bug_report.yml index 60d93ea9b811..ffbb271a900f 100644 --- a/.github/ISSUE_TEMPLATE/bug_report.yml +++ b/.github/ISSUE_TEMPLATE/bug_report.yml @@ -20,14 +20,14 @@ body: 3. Took off '....' 4. See error validations: - required: true + required: false - type: textarea attributes: label: Expected behavior description: A clear and concise description of what you expected to happen. validations: - required: true + required: false - type: textarea attributes: @@ -45,7 +45,7 @@ body: placeholder: | # PASTE HERE THE LINK TO THE LOG validations: - required: true + required: false - type: markdown attributes: @@ -60,14 +60,14 @@ body: placeholder: | # If you don't know the version, paste the output of `ver all` in the MAVLink Shell of QGC validations: - required: true + required: false - type: input attributes: label: Flight controller description: Specify your flight controller model (what type is it, where was it bought from, ...). validations: - required: true + required: false - type: dropdown attributes: diff --git a/.github/workflows/build_all_targets.yml b/.github/workflows/build_all_targets.yml new file mode 100644 index 000000000000..0aa6ce92e592 --- /dev/null +++ b/.github/workflows/build_all_targets.yml @@ -0,0 +1,90 @@ +# NOTE: this workflow is now running on Dronecode / PX4 AWS account. +# - If you want to keep the tests running in GitHub Actions you need to uncomment the "runs-on: ubuntu-latest" lines +# and comment the "runs-on: [runs-on,runner=..." lines. +# - If you would like to duplicate this setup try setting up "RunsOn" on your own AWS account try https://runs-on.com + +name: Build all targets + +on: + push: + branches: + - 'main' + - 'stable' + - 'beta' + - 'release/*' + pull_request: + branches: + - '*' + +jobs: + group_targets: + name: Scan for Board Targets + # runs-on: ubuntu-latest + runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}"] + outputs: + matrix: ${{ steps.set-matrix.outputs.matrix }} + timestamp: ${{ steps.set-timestamp.outputs.timestamp }} + steps: + - uses: actions/checkout@v4 + + - name: Install Python Dependencies + uses: py-actions/py-dependency-install@v4 + with: + path: "./Tools/setup/requirements.txt" + + - id: set-matrix + run: echo "::set-output name=matrix::$(./Tools/generate_board_targets_json.py --group)" + + - id: set-timestamp + run: echo "::set-output name=timestamp::$(date +"%Y%m%d%H%M%S")" + + setup: + name: ${{ matrix.group }} + # runs-on: ubuntu-latest + runs-on: [runs-on,runner=8cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}"] + needs: group_targets + strategy: + matrix: ${{ fromJson(needs.group_targets.outputs.matrix) }} + container: + image: ${{ matrix.container }} + steps: + - uses: actions/checkout@v4 + with: + fetch-depth: 0 + + - name: ownership workaround + run: git config --system --add safe.directory '*' + + - name: ccache setup keys + uses: actions/cache@v4 + with: + path: ~/.ccache + key: ${{ matrix.group }}-ccache-${{ needs.group_targets.outputs.timestamp }} + restore-keys: ${{ matrix.group }}-ccache-${{ needs.group_targets.outputs.timestamp }} + + - name: setup ccache + run: | + mkdir -p ~/.ccache + echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf + echo "compression = true" >> ~/.ccache/ccache.conf + echo "compression_level = 6" >> ~/.ccache/ccache.conf + echo "max_size = 120M" >> ~/.ccache/ccache.conf + echo "hash_dir = false" >> ~/.ccache/ccache.conf + ccache -s + ccache -z + + - name: build target group + run: | + ./Tools/ci_build_all_runner.sh ${{matrix.targets}} + + - name: Upload px4 package + uses: actions/upload-artifact@v4 + with: + name: px4_${{matrix.group}}_build_artifacts + path: | + build/**/*.px4 + build/**/*.bin + compression-level: 0 + + - name: ccache post-run + run: ccache -s diff --git a/.github/workflows/compile_linux.yml b/.github/workflows/compile_linux.yml deleted file mode 100644 index 23e46deab7f3..000000000000 --- a/.github/workflows/compile_linux.yml +++ /dev/null @@ -1,58 +0,0 @@ -name: Compile Linux Targets - -on: - push: - branches: - - 'main' - - 'stable' - - 'beta' - - 'release/*' - pull_request: - branches: - - '*' - -jobs: - build: - runs-on: ubuntu-latest - container: px4io/px4-dev-armhf:2023-06-26 - strategy: - matrix: - config: [ - beaglebone_blue_default, - emlid_navio2_default, - px4_raspberrypi_default, - scumaker_pilotpi_default, - ] - steps: - - uses: actions/checkout@v1 - with: - token: ${{secrets.ACCESS_TOKEN}} - - name: ownership workaround - run: git config --system --add safe.directory '*' - - name: Prepare ccache timestamp - id: ccache_cache_timestamp - shell: cmake -P {0} - run: | - string(TIMESTAMP current_date "%Y-%m-%d-%H;%M;%S" UTC) - message("::set-output name=timestamp::${current_date}") - - name: ccache cache files - uses: actions/cache@v2 - with: - path: ~/.ccache - key: ${{matrix.config}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}} - restore-keys: ${{matrix.config}}-ccache- - - name: setup ccache - run: | - mkdir -p ~/.ccache - echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf - echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 100M" >> ~/.ccache/ccache.conf - echo "hash_dir = false" >> ~/.ccache/ccache.conf - ccache -s - ccache -z - - - name: make ${{matrix.config}} - run: make ${{matrix.config}} - - name: ccache post-run - run: ccache -s diff --git a/.github/workflows/compile_linux_arm64.yml b/.github/workflows/compile_linux_arm64.yml deleted file mode 100644 index 5c1c318b411e..000000000000 --- a/.github/workflows/compile_linux_arm64.yml +++ /dev/null @@ -1,54 +0,0 @@ -name: Compile Linux ARM64 Targets - -on: - push: - branches: - - 'main' - - 'stable' - - 'beta' - - 'release/*' - pull_request: - branches: - - '*' - -jobs: - build: - runs-on: ubuntu-latest - container: px4io/px4-dev-aarch64:2022-08-12 - strategy: - matrix: - config: [ - scumaker_pilotpi_arm64, - ] - steps: - - uses: actions/checkout@v1 - with: - token: ${{secrets.ACCESS_TOKEN}} - - - name: Prepare ccache timestamp - id: ccache_cache_timestamp - shell: cmake -P {0} - run: | - string(TIMESTAMP current_date "%Y-%m-%d-%H;%M;%S" UTC) - message("::set-output name=timestamp::${current_date}") - - name: ccache cache files - uses: actions/cache@v2 - with: - path: ~/.ccache - key: ${{matrix.config}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}} - restore-keys: ${{matrix.config}}-ccache- - - name: setup ccache - run: | - mkdir -p ~/.ccache - echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf - echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 100M" >> ~/.ccache/ccache.conf - echo "hash_dir = false" >> ~/.ccache/ccache.conf - ccache -s - ccache -z - - - name: make ${{matrix.config}} - run: make ${{matrix.config}} - - name: ccache post-run - run: ccache -s diff --git a/.github/workflows/compile_nuttx.yml b/.github/workflows/compile_nuttx.yml deleted file mode 100644 index 5468e51d2049..000000000000 --- a/.github/workflows/compile_nuttx.yml +++ /dev/null @@ -1,136 +0,0 @@ -name: Compile Nuttx Targets - -on: - push: - branches: - - 'main' - - 'stable' - - 'beta' - - 'release/*' - pull_request: - branches: - - '*' - -jobs: - build: - runs-on: ubuntu-latest - container: px4io/px4-dev-nuttx-focal:2022-08-12 - strategy: - fail-fast: false - matrix: - config: [ - airmind_mindpx-v2, - ark_can-flow, - ark_can-gps, - ark_can-rtk-gps, - ark_cannode, - ark_fmu-v6x, - ark_pi6x, - ark_septentrio-gps, - atl_mantis-edu, - av_x-v1, - bitcraze_crazyflie, - bitcraze_crazyflie21, - cuav_can-gps-v1, - cuav_nora, - cuav_x7pro, - cubepilot_cubeorange, - cubepilot_cubeorangeplus, - cubepilot_cubeyellow, - diatone_mamba-f405-mk2, - freefly_can-rtk-gps, - holybro_can-gps-v1, - holybro_durandal-v1, - holybro_kakutef7, - holybro_kakuteh7, - holybro_pix32v5, - matek_gnss-m9n-f4, - matek_h743, - matek_h743-mini, - matek_h743-slim, - modalai_fc-v1, - modalai_fc-v2, - mro_ctrl-zero-classic, - mro_ctrl-zero-f7, - mro_ctrl-zero-f7-oem, - mro_ctrl-zero-h7, - mro_ctrl-zero-h7-oem, - mro_pixracerpro, - mro_x21, - mro_x21-777, - nxp_fmuk66-e, - nxp_fmuk66-v3, - nxp_mr-canhubk3, - nxp_ucans32k146, - omnibus_f4sd, - px4_fmu-v2, - px4_fmu-v3, - px4_fmu-v4, - px4_fmu-v4pro, - px4_fmu-v5, - px4_fmu-v5x, - px4_fmu-v6c, - px4_fmu-v6u, - px4_fmu-v6x, - px4_fmu-v6xrt, - raspberrypi_pico, - sky-drones_smartap-airlink, - spracing_h7extreme, - uvify_core, - siyi_n7 - ] - steps: - - uses: actions/checkout@v1 - with: - token: ${{secrets.ACCESS_TOKEN}} - - - name: Prepare ccache timestamp - id: ccache_cache_timestamp - shell: cmake -P {0} - run: | - string(TIMESTAMP current_date "%Y-%m-%d-%H;%M;%S" UTC) - message("::set-output name=timestamp::${current_date}") - - name: ccache cache files - uses: actions/cache@v2 - with: - path: ~/.ccache - key: ${{matrix.config}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}} - restore-keys: ${{matrix.config}}-ccache- - - name: setup ccache - run: | - mkdir -p ~/.ccache - echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf - echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 120M" >> ~/.ccache/ccache.conf - echo "hash_dir = false" >> ~/.ccache/ccache.conf - ccache -s - ccache -z - - - name: make all_variants_${{matrix.config}} - run: make all_variants_${{matrix.config}} - timeout-minutes: 45 - - name: make ${{matrix.config}} bloaty_compileunits - run: make ${{matrix.config}} bloaty_compileunits || true - - name: make ${{matrix.config}} bloaty_inlines - run: make ${{matrix.config}} bloaty_inlines || true - - name: make ${{matrix.config}} bloaty_segments - run: make ${{matrix.config}} bloaty_segments || true - - name: make ${{matrix.config}} bloaty_symbols - run: make ${{matrix.config}} bloaty_symbols || true - - name: make ${{matrix.config}} bloaty_templates - run: make ${{matrix.config}} bloaty_templates || true - - name: make ${{matrix.config}} bloaty_ram - run: make ${{matrix.config}} bloaty_ram || true - - name: make ${{matrix.config}} bloaty_compare_master - run: make ${{matrix.config}} bloaty_compare_master || true - - name: ccache post-run - run: ccache -s - - - name: Upload px4 package - uses: actions/upload-artifact@v2 - with: - name: px4_package_${{matrix.config}} - path: | - build/**/*.px4 - build/**/*.bin diff --git a/.gitmodules b/.gitmodules index e443da19a290..a783b1693843 100644 --- a/.gitmodules +++ b/.gitmodules @@ -71,7 +71,7 @@ [submodule "src/modules/zenoh/zenoh-pico"] path = src/modules/zenoh/zenoh-pico url = https://github.com/px4/zenoh-pico - branch = pr-zubf-werror-fix + branch = dev/1.0.0-px4 [submodule "src/lib/heatshrink/heatshrink"] path = src/lib/heatshrink/heatshrink url = https://github.com/PX4/heatshrink.git diff --git a/.vscode/cmake-variants.yaml b/.vscode/cmake-variants.yaml index 1311079ff988..d55b170485e2 100644 --- a/.vscode/cmake-variants.yaml +++ b/.vscode/cmake-variants.yaml @@ -111,6 +111,11 @@ CONFIG: buildType: MinSizeRel settings: CONFIG: px4_fmu-v6xrt_bootloader + 3dr_ctrl-zero-h7-oem-revg_default: + short: 3dr_ctrl-zero-h7-oem-revg + buildType: MinSizeRel + settings: + CONFIG: 3dr_ctrl-zero-h7-oem-revg_default airmind_mindpx-v2_default: short: airmind_mindpx-v2 buildType: MinSizeRel diff --git a/Makefile b/Makefile index fe8580402dab..5de475d79074 100644 --- a/Makefile +++ b/Makefile @@ -324,6 +324,7 @@ px4io_update: git status bootloaders_update: \ + 3dr_ctrl-zero-h7-oem-revg_bootloader \ ark_fmu-v6x_bootloader \ ark_pi6x_bootloader \ cuav_nora_bootloader \ @@ -549,14 +550,14 @@ distclean: # All other targets are handled by PX4_MAKE. Add a rule here to avoid printing an error. %: $(if $(filter $(FIRST_ARG),$@), \ - $(error "Make target $@ not found. It either does not exist or $@ cannot be the first argument. Use '$(MAKE) help|list_config_targets' to get a list of all possible [configuration] targets."),@#) + $(error "Make target $@ not found. It either does not exist or $@ cannot be the first argument. Use '$(MAKE) list_config_targets' to get a list of all possible [configuration] targets."),@#) # Print a list of non-config targets (based on http://stackoverflow.com/a/26339924/1487069) help: @echo "Usage: $(MAKE) " @echo "Where is one of:" @$(MAKE) -pRrq -f $(lastword $(MAKEFILE_LIST)) : 2>/dev/null | \ - awk -v RS= -F: '/^# File/,/^# Finished Make data base/ {if ($$1 !~ "^[#.]") {print $$1}}' | sort | \ + awk -v RS= -F: '/(^|\n)# Files(\n|$$)/,/(^|\n)# Finished Make data base/ {if ($$1 !~ "^[#.]") {print $$1}}' | sort | \ egrep -v -e '^[^[:alnum:]]' -e '^($(subst $(space),|,$(ALL_CONFIG_TARGETS)))$$' -e '_default$$' -e '^(Makefile)' @echo @echo "Or, $(MAKE) []" diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1010_gazebo-classic_iris_opt_flow b/ROMFS/px4fmu_common/init.d-posix/airframes/1010_gazebo-classic_iris_opt_flow index 38d9019adc70..d334bdb43088 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1010_gazebo-classic_iris_opt_flow +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1010_gazebo-classic_iris_opt_flow @@ -47,5 +47,5 @@ param set-default MPC_ALT_MODE 2 param set-default SENS_FLOW_ROT 6 param set-default SENS_FLOW_MINHGT 0.7 -param set-default SENS_FLOW_MAXHGT 3 +param set-default SENS_FLOW_MAXHGT 15 param set-default SENS_FLOW_MAXR 2.5 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1045_gazebo-classic_quadtailsitter b/ROMFS/px4fmu_common/init.d-posix/airframes/1045_gazebo-classic_quadtailsitter index 87afb523b1a7..9539ba6d7402 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1045_gazebo-classic_quadtailsitter +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1045_gazebo-classic_quadtailsitter @@ -33,7 +33,7 @@ param set-default PWM_MAIN_FUNC3 103 param set-default PWM_MAIN_FUNC4 104 param set-default PWM_MAIN_FUNC5 0 -parm set-default FD_FAIL_R 70 +param set-default FD_FAIL_R 70 param set-default FW_P_TC 0.6 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover b/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover index 80664cf661c0..7697e23db769 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover @@ -6,16 +6,36 @@ . ${R}etc/init.d/rc.rover_differential_defaults PX4_SIMULATOR=${PX4_SIMULATOR:=gz} -PX4_GZ_WORLD=${PX4_GZ_WORLD:=default} +PX4_GZ_WORLD=${PX4_GZ_WORLD:=rover} PX4_SIM_MODEL=${PX4_SIM_MODEL:=r1_rover} param set-default SIM_GZ_EN 1 # Gazebo bridge +# Rover parameters +param set-default RD_WHEEL_TRACK 0.3 +param set-default RD_MAN_YAW_SCALE 0.1 +param set-default RD_YAW_RATE_I 0.1 +param set-default RD_YAW_RATE_P 5 +param set-default RD_MAX_ACCEL 6 +param set-default RD_MAX_JERK 30 +param set-default RD_MAX_SPEED 7 +param set-default RD_HEADING_P 5 +param set-default RD_HEADING_I 0.1 +param set-default RD_MAX_YAW_RATE 180 +param set-default RD_MISS_SPD_DEF 7 +param set-default RD_TRANS_DRV_TRN 0.349066 +param set-default RD_TRANS_TRN_DRV 0.174533 + +# Pure pursuit parameters +param set-default PP_LOOKAHD_MAX 30 +param set-default PP_LOOKAHD_MIN 2 +param set-default PP_LOOKAHD_GAIN 1 + # Simulated sensors param set-default SENS_EN_GPSSIM 1 param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 -param set-default SENS_EN_ARSPDSIM 1 +param set-default SENS_EN_ARSPDSIM 0 # Actuator mapping param set-default SIM_GZ_WH_FUNC1 101 # right wheel diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower index 1dcb917a09ad..c0d91e222365 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower @@ -15,14 +15,29 @@ param set-default SIM_GZ_EN 1 # Gazebo bridge param set-default SENS_EN_GPSSIM 1 param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 -param set-default SENS_EN_ARSPDSIM 1 +param set-default SENS_EN_ARSPDSIM 0 # We can arm and drive in manual mode when it slides and GPS check fails: param set-default COM_ARM_WO_GPS 1 -# Set Differential Drive Kinematics Library parameters: -param set RDD_WHEEL_BASE 0.9 -param set RDD_WHEEL_RADIUS 0.22 -param set RDD_WHEEL_SPEED 10.0 # Maximum wheel speed rad/s, approx 8 km/h +# Rover parameters +param set-default RD_WHEEL_TRACK 0.9 +param set-default RD_MAN_YAW_SCALE 0.1 +param set-default RD_YAW_RATE_I 0.1 +param set-default RD_YAW_RATE_P 5 +param set-default RD_MAX_ACCEL 1 +param set-default RD_MAX_JERK 3 +param set-default RD_MAX_SPEED 8 +param set-default RD_HEADING_P 5 +param set-default RD_HEADING_I 0.1 +param set-default RD_MAX_YAW_RATE 30 +param set-default RD_MISS_SPD_DEF 8 +param set-default RD_TRANS_DRV_TRN 0.349066 +param set-default RD_TRANS_TRN_DRV 0.174533 + +# Pure pursuit parameters +param set-default PP_LOOKAHD_MAX 30 +param set-default PP_LOOKAHD_MIN 2 +param set-default PP_LOOKAHD_GAIN 1 # Actuator mapping - set SITL motors/servos output parameters: @@ -36,14 +51,14 @@ param set-default SIM_GZ_WH_FUNC1 101 # right wheel param set-default SIM_GZ_WH_FUNC2 102 # left wheel #param set-default SIM_GZ_WH_MIN2 0 #param set-default SIM_GZ_WH_MAX2 200 -#aram set-default SIM_GZ_WH_DIS2 100 +#param set-default SIM_GZ_WH_DIS2 100 #param set-default SIM_GZ_WH_FAIL2 100 param set-default SIM_GZ_WH_REV 0 # no need to reverse any wheels -# Note: The servo configurations ( SIM_GZ_SV_FUNC*) outlined below are intended for educational purposes in this simulation. -# They do not have physical effects in the simulated environment, except for actuating the joints. Their definitions are meant to demonstrate -# how actuators could be mapped and configured in a real-world application, providing a foundation for understanding and implementing actuator +# Note: The servo configurations ( SIM_GZ_SV_FUNC*) outlined below are intended for educational purposes in this simulation. +# They do not have physical effects in the simulated environment, except for actuating the joints. Their definitions are meant to demonstrate +# how actuators could be mapped and configured in a real-world application, providing a foundation for understanding and implementing actuator # controls in practical scenarios. # Cutter deck blades clutch, PCA9685 servo channel 3, "RC FLAPS" (406) - leftmost switch, or "Servo 3" (203): diff --git a/ROMFS/px4fmu_common/init.d/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/CMakeLists.txt index 90b14f812f02..3319779096c5 100644 --- a/ROMFS/px4fmu_common/init.d/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/CMakeLists.txt @@ -44,6 +44,7 @@ px4_add_romfs_files( # TODO rc.balloon_apps rc.balloon_defaults + rc.sysinit ) if(CONFIG_MODULES_AIRSHIP_ATT_CONTROL) @@ -77,7 +78,7 @@ if(CONFIG_MODULES_ROVER_POS_CONTROL) ) endif() -if(CONFIG_MODULES_DIFFERENTIAL_DRIVE) +if(CONFIG_MODULES_ROVER_DIFFERENTIAL) px4_add_romfs_files( rc.rover_differential_apps rc.rover_differential_defaults diff --git a/ROMFS/px4fmu_common/init.d/airframes/50000_generic_rover_differential b/ROMFS/px4fmu_common/init.d/airframes/50000_generic_rover_differential new file mode 100644 index 000000000000..8c490d497be2 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/50000_generic_rover_differential @@ -0,0 +1,12 @@ +#!/bin/sh +# +# @name Generic Rover Differential +# +# @type Rover +# @class Rover +# +# @board px4_fmu-v2 exclude +# @board bitcraze_crazyflie exclude +# + +. ${R}etc/init.d/rc.rover_differential_defaults diff --git a/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover b/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover similarity index 85% rename from ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover rename to ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover index 20a16428b2ed..c06b158cf857 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover +++ b/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover @@ -15,9 +15,6 @@ param set-default BAT1_N_CELLS 4 -param set-default EKF2_GBIAS_INIT 0.01 -param set-default EKF2_ANGERR_INIT 0.01 - # Set geometry & output configration param set-default RBCLW_ADDRESS 128 param set-default RBCLW_FUNC1 101 diff --git a/ROMFS/px4fmu_common/init.d/airframes/50010_ackermann_rover_generic b/ROMFS/px4fmu_common/init.d/airframes/51000_generic_rover_ackermann similarity index 83% rename from ROMFS/px4fmu_common/init.d/airframes/50010_ackermann_rover_generic rename to ROMFS/px4fmu_common/init.d/airframes/51000_generic_rover_ackermann index 66b3dc0cabba..43620a50ddcd 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50010_ackermann_rover_generic +++ b/ROMFS/px4fmu_common/init.d/airframes/51000_generic_rover_ackermann @@ -1,6 +1,6 @@ #!/bin/sh # -# @name Generic ackermann rover +# @name Generic Rover Ackermann # # @type Rover # @class Rover diff --git a/ROMFS/px4fmu_common/init.d/airframes/51001_axial_scx10_2_trail_honcho b/ROMFS/px4fmu_common/init.d/airframes/51001_axial_scx10_2_trail_honcho new file mode 100644 index 000000000000..6f50c7f4a1e0 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/51001_axial_scx10_2_trail_honcho @@ -0,0 +1,37 @@ +#!/bin/sh +# +# @name Axial SCX10 2 Trail Honcho +# +# @url https://www.axialadventure.com/product/1-10-scx10-ii-trail-honcho-4wd-rock-crawler-brushed-rtr/AXID9059.html +# +# @type Rover +# @class Rover +# +# @board px4_fmu-v2 exclude +# @board bitcraze_crazyflie exclude +# + +. ${R}etc/init.d/rc.rover_ackermann_defaults + +param set-default BAT1_N_CELLS 3 + +# Rover parameters +param set-default NAV_ACC_RAD 0.5 +param set-default RA_ACC_RAD_GAIN 2 +param set-default RA_ACC_RAD_MAX 3 +param set-default RA_MAX_ACCEL 0.5 +param set-default RA_MAX_JERK 10 +param set-default RA_MAX_SPEED 2.7 +param set-default RA_MAX_STR_ANG 0.5236 +param set-default RA_MAX_STR_RATE 270 +param set-default RA_MISS_VEL_DEF 2.7 +param set-default RA_MISS_VEL_GAIN 3.5 +param set-default RA_MISS_VEL_MIN 1 +param set-default RA_SPEED_I 0.1 +param set-default RA_SPEED_P 0.5 +param set-default RA_WHEEL_BASE 0.321 + +# Pure pursuit parameters +param set-default PP_LOOKAHD_GAIN 1 +param set-default PP_LOOKAHD_MAX 10 +param set-default PP_LOOKAHD_MIN 1.5 diff --git a/ROMFS/px4fmu_common/init.d/airframes/50000_generic_ground_vehicle b/ROMFS/px4fmu_common/init.d/airframes/59000_generic_ground_vehicle similarity index 96% rename from ROMFS/px4fmu_common/init.d/airframes/50000_generic_ground_vehicle rename to ROMFS/px4fmu_common/init.d/airframes/59000_generic_ground_vehicle index 5ae6b09f4a8d..a773c1158d44 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50000_generic_ground_vehicle +++ b/ROMFS/px4fmu_common/init.d/airframes/59000_generic_ground_vehicle @@ -1,6 +1,6 @@ #!/bin/sh # -# @name Generic Ground Vehicle (Ackermann) +# @name Generic Ground Vehicle (Deprecated) # # @type Rover # @class Rover diff --git a/ROMFS/px4fmu_common/init.d/airframes/50004_nxpcup_car_dfrobot_gpx b/ROMFS/px4fmu_common/init.d/airframes/59001_nxpcup_car_dfrobot_gpx similarity index 97% rename from ROMFS/px4fmu_common/init.d/airframes/50004_nxpcup_car_dfrobot_gpx rename to ROMFS/px4fmu_common/init.d/airframes/59001_nxpcup_car_dfrobot_gpx index 67620ddc0375..9f08553d970f 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50004_nxpcup_car_dfrobot_gpx +++ b/ROMFS/px4fmu_common/init.d/airframes/59001_nxpcup_car_dfrobot_gpx @@ -1,6 +1,6 @@ #!/bin/sh # -# @name NXP Cup car: DF Robot GPX +# @name NXP Cup car: DF Robot GPX (Deprecated) # # @type Rover diff --git a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt index f763a4376606..634eb3e1b882 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt @@ -136,22 +136,27 @@ if(CONFIG_MODULES_VTOL_ATT_CONTROL) ) endif() -if(CONFIG_MODULES_ROVER_POS_CONTROL) +if(CONFIG_MODULES_ROVER_DIFFERENTIAL) px4_add_romfs_files( - 50000_generic_ground_vehicle - 50004_nxpcup_car_dfrobot_gpx + # [50000, 50999] Differential rovers + 50000_generic_rover_differential + 50001_aion_robotics_r1_rover ) endif() -if(CONFIG_MODULES_DIFFERENTIAL_DRIVE) +if(CONFIG_MODULES_ROVER_ACKERMANN) px4_add_romfs_files( - 50003_aion_robotics_r1_rover + # [51000, 51999] Ackermann rovers + 51000_generic_rover_ackermann + 51001_axial_scx10_2_trail_honcho ) endif() -if(CONFIG_MODULES_ROVER_ACKERMANN) +if(CONFIG_MODULES_ROVER_POS_CONTROL) px4_add_romfs_files( - 50010_ackermann_rover_generic + # [59000, 59999] Rover position control (deprecated) + 59000_generic_ground_vehicle + 59001_nxpcup_car_dfrobot_gpx ) endif() diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_ackermann_apps b/ROMFS/px4fmu_common/init.d/rc.rover_ackermann_apps index 5273d6aec068..181233babe26 100644 --- a/ROMFS/px4fmu_common/init.d/rc.rover_ackermann_apps +++ b/ROMFS/px4fmu_common/init.d/rc.rover_ackermann_apps @@ -1,7 +1,7 @@ #!/bin/sh -# Standard apps for a ackermann drive rover. +# Standard apps for an ackermann rover. -# Start rover ackermann drive controller. +# Start rover ackermann module. rover_ackermann start # Start Land Detector. diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_ackermann_defaults b/ROMFS/px4fmu_common/init.d/rc.rover_ackermann_defaults index 27cf144a329a..fe0ae7aa9bb8 100644 --- a/ROMFS/px4fmu_common/init.d/rc.rover_ackermann_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.rover_ackermann_defaults @@ -2,12 +2,10 @@ # Ackermann rover parameters. set VEHICLE_TYPE rover_ackermann -param set-default MAV_TYPE 10 # MAV_TYPE_GROUND_ROVER -param set-default CA_AIRFRAME 5 # Rover (Ackermann) -param set-default CA_R_REV 1 # Motor is assumed to be reversible -param set-default EKF2_MAG_TYPE 1 # make sure magnetometer is fused even when not flying +param set-default MAV_TYPE 10 # MAV_TYPE_GROUND_ROVER +param set-default CA_AIRFRAME 5 # Rover (Ackermann) +param set-default CA_R_REV 1 # Motor is assumed to be reversible +param set-default EKF2_MAG_TYPE 1 # Make sure magnetometer is fused even when not flying +param set-default NAV_ACC_RAD 0.5 # Waypoint acceptance radius param set-default EKF2_GBIAS_INIT 0.01 param set-default EKF2_ANGERR_INIT 0.01 -param set-default NAV_ACC_RAD 0.5 # Waypoint acceptance radius -param set-default NAV_RCL_ACT 6 # Disarm on manual control loss -param set-default COM_FAIL_ACT_T 1 # Delay before failsafe after rc loss diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps b/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps index 275664af7e26..e1a7ecd9ccd2 100644 --- a/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps +++ b/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps @@ -1,8 +1,8 @@ #!/bin/sh -# Standard apps for a differential drive rover. +# Standard apps for a differential rover. -# Start rover differential drive controller. -differential_drive start +# Start rover differential module. +rover_differential start # Start Land Detector. land_detector start rover diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_differential_defaults b/ROMFS/px4fmu_common/init.d/rc.rover_differential_defaults index e0cace3a3238..7c29ba364f3e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.rover_differential_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.rover_differential_defaults @@ -2,10 +2,10 @@ # Differential rover parameters. set VEHICLE_TYPE rover_differential - -param set-default MAV_TYPE 10 # MAV_TYPE_GROUND_ROVER - -param set-default CA_AIRFRAME 6 # Rover (Differential) -param set-default CA_R_REV 3 # Right and left motors reversible - -param set-default EKF2_MAG_TYPE 1 # make sure magnetometer is fused even when not flying +param set-default MAV_TYPE 10 # MAV_TYPE_GROUND_ROVER +param set-default CA_AIRFRAME 6 # Rover (Differential) +param set-default CA_R_REV 3 # Right and left motors reversible +param set-default EKF2_MAG_TYPE 1 # Make sure magnetometer is fused even when not flying +param set-default NAV_ACC_RAD 0.5 # Waypoint acceptance radius +param set-default EKF2_GBIAS_INIT 0.01 +param set-default EKF2_ANGERR_INIT 0.01 diff --git a/ROMFS/px4fmu_common/init.d/rc.sysinit b/ROMFS/px4fmu_common/init.d/rc.sysinit new file mode 100644 index 000000000000..9f878a1387bb --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.sysinit @@ -0,0 +1,4 @@ +#!/bin/sh +# +# Standard system init script +# diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 39eed1bcab4d..28804361d1ed 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -30,7 +30,7 @@ set LOGGER_BUF 8 set PARAM_FILE "" set PARAM_BACKUP_FILE "" set RC_INPUT_ARGS "" -set SDCARD_AVAILABLE no +set STORAGE_AVAILABLE no set SDCARD_EXT_PATH /fs/microsd/ext_autostart set SDCARD_FORMAT no set STARTUP_TUNE 1 @@ -62,11 +62,11 @@ then umount /fs/microsd else - set SDCARD_AVAILABLE yes + set STORAGE_AVAILABLE yes fi fi - if [ $SDCARD_AVAILABLE = no -o $SDCARD_FORMAT = yes ] + if [ $STORAGE_AVAILABLE = no -o $SDCARD_FORMAT = yes ] then echo "INFO [init] formatting /dev/mmcsd0" set STARTUP_TUNE 15 # tune 15 = SD_ERROR (overridden to SD_INIT if format + mount succeeds) @@ -77,7 +77,7 @@ then if mount -t vfat /dev/mmcsd0 /fs/microsd then - set SDCARD_AVAILABLE yes + set STORAGE_AVAILABLE yes set STARTUP_TUNE 14 # tune 14 = SD_INIT else echo "ERROR [init] card mount failed" @@ -86,16 +86,22 @@ then echo "ERROR [init] format failed" fi fi +else + # Is there a device mounted for storage + if mft query -q -k MTD -s MTD_PARAMETERS -v /mnt/microsd + then + set STORAGE_AVAILABLE yes + fi +fi - if [ $SDCARD_AVAILABLE = yes ] +if [ $STORAGE_AVAILABLE = yes ] +then + if hardfault_log check then - if hardfault_log check + set STARTUP_TUNE 2 # tune 2 = ERROR_TUNE + if hardfault_log commit then - set STARTUP_TUNE 2 # tune 2 = ERROR_TUNE - if hardfault_log commit - then - hardfault_log reset - fi + hardfault_log reset fi fi @@ -172,7 +178,7 @@ else fi fi - if [ $SDCARD_AVAILABLE = yes ] + if [ $STORAGE_AVAILABLE = yes ] then param select-backup $PARAM_BACKUP_FILE fi @@ -220,8 +226,8 @@ else if [ ${VEHICLE_TYPE} == none ] then - # Look for airframe on SD card - if [ $SDCARD_AVAILABLE = yes ] + # Use external startup file + if [ $STORAGE_AVAILABLE = yes ] then . ${R}etc/init.d/rc.autostart_ext else @@ -615,7 +621,7 @@ unset PARAM_FILE unset PARAM_BACKUP_FILE unset PARAM_DEFAULTS_VER unset RC_INPUT_ARGS -unset SDCARD_AVAILABLE +unset STORAGE_AVAILABLE unset SDCARD_EXT_PATH unset SDCARD_FORMAT unset STARTUP_TUNE diff --git a/Tools/ci_build_all_runner.sh b/Tools/ci_build_all_runner.sh new file mode 100755 index 000000000000..bfb77e2623a9 --- /dev/null +++ b/Tools/ci_build_all_runner.sh @@ -0,0 +1,19 @@ +#!/bin/bash +# This script is meant to be used by the build_all.yml workflow in a github runner +# Please only modify if you know what you are doing +set -e + +echo "### :clock1: Build Times" >> $GITHUB_STEP_SUMMARY +targets=$1 +for target in ${targets//,/ } +do + echo "::group::Building: [${target}]" + start=$(date +%s) + make $target + stop=$(date +%s) + diff=$(($stop-$start)) + build_time="$(($diff /60/60))h $(($diff /60))m $(($diff % 60))s elapsed" + echo -e "\033[0;32mBuild Time: [$build_time]" + echo "* **$target** - $build_time" >> $GITHUB_STEP_SUMMARY + echo "::endgroup::" +done diff --git a/Tools/generate_board_targets_json.py b/Tools/generate_board_targets_json.py index 931d5c14776f..649b3fceefa6 100755 --- a/Tools/generate_board_targets_json.py +++ b/Tools/generate_board_targets_json.py @@ -23,11 +23,14 @@ help='Verbose Output') parser.add_argument('-p', '--pretty', dest='pretty', action='store_true', help='Pretty output instead of a single line') +parser.add_argument('-g', '--groups', dest='group', action='store_true', + help='Groups targets') args = parser.parse_args() verbose = args.verbose build_configs = [] +grouped_targets = {} excluded_boards = ['modalai_voxl2', 'px4_ros2'] # TODO: fix and enable excluded_manufacturers = ['atlflight'] excluded_platforms = ['qurt'] @@ -37,10 +40,27 @@ 'uavcanv1', # TODO: fix and enable ] +github_action_config = { 'include': build_configs } +extra_args = {} +if args.pretty: + extra_args['indent'] = 2 + +def chunks(arr, size): + # splits array into parts + for i in range(0, len(arr), size): + yield arr[i:i + size] + +def comma_targets(targets): + # turns array of targets into a comma split string + return ",".join(targets) + def process_target(px4board_file, target_name): + # reads through the board file and grabs + # useful information for building ret = None platform = None toolchain = None + group = None if px4board_file.endswith("default.px4board") or \ px4board_file.endswith("recovery.px4board") or \ @@ -63,22 +83,34 @@ def process_target(px4board_file, target_name): # get the container based on the platform and toolchain if platform == 'posix': container = 'px4io/px4-dev-base-focal:2021-09-08' + group = 'base' if toolchain: if toolchain.startswith('aarch64'): container = 'px4io/px4-dev-aarch64:2022-08-12' + group = 'aarch64' elif toolchain == 'arm-linux-gnueabihf': container = 'px4io/px4-dev-armhf:2023-06-26' + group = 'armhf' else: if verbose: print(f'unmatched toolchain: {toolchain}') elif platform == 'nuttx': container = 'px4io/px4-dev-nuttx-focal:2022-08-12' + group = 'nuttx' else: if verbose: print(f'unmatched platform: {platform}') ret = {'target': target_name, 'container': container} + if(args.group): + ret['arch'] = group return ret +# Look for board targets in the ./boards directory +if(verbose): + print("=======================") + print("= scanning for boards =") + print("=======================") + for manufacturer in os.scandir(os.path.join(source_dir, 'boards')): if not manufacturer.is_dir(): continue @@ -105,12 +137,140 @@ def process_target(px4board_file, target_name): if verbose: print(f'excluding label {label} ({target_name})') continue target = process_target(files.path, target_name) + if (args.group and target is not None): + if (target['arch'] not in grouped_targets): + grouped_targets[target['arch']] = {} + grouped_targets[target['arch']]['container'] = target['container'] + grouped_targets[target['arch']]['manufacturers'] = {} + if(manufacturer.name not in grouped_targets[target['arch']]['manufacturers']): + grouped_targets[target['arch']]['manufacturers'][manufacturer.name] = {} + grouped_targets[target['arch']]['manufacturers'][manufacturer.name] = [] + grouped_targets[target['arch']]['manufacturers'][manufacturer.name].append(target_name) if target is not None: build_configs.append(target) +if(verbose): + import pprint + print("============================") + print("= Boards found in ./boards =") + print("============================") + pprint.pp(grouped_targets) -github_action_config = { 'include': build_configs } -extra_args = {} -if args.pretty: - extra_args['indent'] = 2 -print(json.dumps(github_action_config, **extra_args)) +if (args.group): + # if we are using this script for grouping builds + # we loop trough the manufacturers list and split their targets + # if a manufacturer has more than a LIMIT of boards then we split that + # into sub groups such as "arch-manufacturer name-index" + # example: + # nuttx-px4-0 + # nuttx-px4-1 + # nuttx-px4-2 + # nuttx-ark-0 + # nuttx-ark-1 + # if the manufacturer doesn't have more targets than LIMIT then we add + # them to a generic group with the following structure "arch-index" + # example: + # nuttx-0 + # nuttx-1 + final_groups = [] + temp_group = [] + group_number = {} + last_man = '' + last_arch = '' + SPLIT_LIMIT = 10 + LOWER_LIMIT = 5 + for arch in grouped_targets: + if(last_arch == ''): + last_arch = arch + if(arch not in group_number): + group_number[arch] = 0 + + if(last_arch != arch and len(temp_group) > 0): + group_name = last_arch + "-" + str(group_number[last_arch]) + group_number[last_arch] += 1 + targets = comma_targets(temp_group) + final_groups.append({ + "container": grouped_targets[last_arch]['container'], + "targets": targets, + "arch": last_arch, + "group": group_name, + "len": len(temp_group) + }) + last_arch = arch + temp_group = [] + for man in grouped_targets[arch]['manufacturers']: + for tar in grouped_targets[arch]['manufacturers'][man]: + if(last_man != man): + man_len = len(grouped_targets[arch]['manufacturers'][man]) + if(man_len > LOWER_LIMIT and man_len < (SPLIT_LIMIT + 1)): + # Manufacturers can have their own group + group_name = arch + "-" + man + targets = comma_targets(grouped_targets[arch]['manufacturers'][man]) + last_man = man + final_groups.append({ + "container": grouped_targets[arch]['container'], + "targets": targets, + "arch": arch, + "group": group_name, + "len": len(grouped_targets[arch]['manufacturers'][man]) + }) + elif(man_len >= (SPLIT_LIMIT + 1)): + # Split big man groups into subgroups + # example: Pixhawk + chunk_limit = SPLIT_LIMIT + chunk_counter = 0 + for chunk in chunks(grouped_targets[arch]['manufacturers'][man], chunk_limit): + group_name = arch + "-" + man + "-" + str(chunk_counter) + targets = comma_targets(chunk) + last_man = man + final_groups.append({ + "container": grouped_targets[arch]['container'], + "targets": targets, + "arch": arch, + "group": group_name, + "len": len(chunk), + }) + chunk_counter += 1 + else: + temp_group.append(tar) + + if(last_arch != arch and len(temp_group) > 0): + group_name = last_arch + "-" + str(group_number[last_arch]) + group_number[last_arch] += 1 + targets = comma_targets(temp_group) + final_groups.append({ + "container": grouped_targets[last_arch]['container'], + "targets": targets, + "arch": last_arch, + "group": group_name, + "len": len(temp_group) + }) + last_arch = arch + temp_group = [] + if(len(temp_group) > (LOWER_LIMIT - 1)): + group_name = arch + "-" + str(group_number[arch]) + last_arch = arch + group_number[arch] += 1 + targets = comma_targets(temp_group) + final_groups.append({ + "container": grouped_targets[arch]['container'], + "targets": targets, + "arch": arch, + "group": group_name, + "len": len(temp_group) + }) + temp_group = [] + if(verbose): + import pprint + print("================") + print("= final_groups =") + print("================") + pprint.pp(final_groups) + + print("===============") + print("= JSON output =") + print("===============") + + print(json.dumps({ "include": final_groups }, **extra_args)) +else: + print(json.dumps(github_action_config, **extra_args)) diff --git a/Tools/simulation/gazebo-classic/sitl_gazebo-classic b/Tools/simulation/gazebo-classic/sitl_gazebo-classic index 67431d233f0f..f1d11a612699 160000 --- a/Tools/simulation/gazebo-classic/sitl_gazebo-classic +++ b/Tools/simulation/gazebo-classic/sitl_gazebo-classic @@ -1 +1 @@ -Subproject commit 67431d233f0f08de647f0eb11239816f9c8bd6c6 +Subproject commit f1d11a6126990d487d4aa8ff68c23ff370516510 diff --git a/Tools/simulation/gz b/Tools/simulation/gz index 312cd002ff96..536305adee09 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit 312cd002ff9602644efef58eef93e447c10dcbe8 +Subproject commit 536305adee09b9ace391b16107e625cf7c6db7e7 diff --git a/Tools/upload.sh b/Tools/upload.sh index 25d41e389f76..2a6416ed374d 100755 --- a/Tools/upload.sh +++ b/Tools/upload.sh @@ -15,7 +15,7 @@ SERIAL_PORTS="/dev/tty.usbmodemPX*,/dev/tty.usbmodem*" fi if [ $SYSTYPE = "Linux" ]; then -SERIAL_PORTS="/dev/serial/by-id/*_PX4_*,/dev/serial/by-id/usb-3D_Robotics*,/dev/serial/by-id/usb-The_Autopilot*,/dev/serial/by-id/usb-Bitcraze*,/dev/serial/by-id/pci-Bitcraze*,/dev/serial/by-id/usb-Gumstix*,/dev/serial/by-id/usb-UVify*,/dev/serial/by-id/usb-ArduPilot*,/dev/serial/by-id/ARK*," +SERIAL_PORTS="/dev/serial/by-id/*_PX4_*,/dev/serial/by-id/usb-3D_Robotics*,/dev/serial/by-id/usb-The_Autopilot*,/dev/serial/by-id/usb-Bitcraze*,/dev/serial/by-id/pci-Bitcraze*,/dev/serial/by-id/usb-Gumstix*,/dev/serial/by-id/usb-UVify*,/dev/serial/by-id/usb-ArduPilot*,/dev/serial/by-id/usb-ARK*," fi if [[ $SYSTYPE = *"CYGWIN"* ]]; then diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/bootloader.px4board b/boards/3dr/ctrl-zero-h7-oem-revg/bootloader.px4board new file mode 100755 index 000000000000..19b6e662be69 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/bootloader.px4board @@ -0,0 +1,3 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ROMFSROOT="" diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/default.px4board b/boards/3dr/ctrl-zero-h7-oem-revg/default.px4board new file mode 100644 index 000000000000..549ba99b7331 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/default.px4board @@ -0,0 +1,96 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS2" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS4" +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_BAROMETER_DPS310=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_BOSCH_BMI088=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_ESC_BATTERY=y +CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_GYRO_FFT=y +CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y +CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_LOGGER=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y +CONFIG_MODULES_MAVLINK=y +CONFIG_MODULES_MC_ATT_CONTROL=y +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y +CONFIG_MODULES_MC_POS_CONTROL=y +CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UUV_ATT_CONTROL=y +CONFIG_MODULES_UUV_POS_CONTROL=y +CONFIG_MODULES_UXRCE_DDS_CLIENT=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BSONDUMP=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_REFLECT=y +CONFIG_SYSTEMCMDS_SD_BENCH=y +CONFIG_SYSTEMCMDS_SD_STRESS=y +CONFIG_SYSTEMCMDS_SERIAL_TEST=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_USB_CONNECTED=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/extras/3dr_ctrl-zero-h7-oem-revg_bootloader.bin b/boards/3dr/ctrl-zero-h7-oem-revg/extras/3dr_ctrl-zero-h7-oem-revg_bootloader.bin new file mode 100755 index 000000000000..86dc50de7b9c Binary files /dev/null and b/boards/3dr/ctrl-zero-h7-oem-revg/extras/3dr_ctrl-zero-h7-oem-revg_bootloader.bin differ diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/firmware.prototype b/boards/3dr/ctrl-zero-h7-oem-revg/firmware.prototype new file mode 100755 index 000000000000..d11458c1a3fe --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 1124, + "magic": "3dr-ctrl-zero-h7-oem-revg", + "description": "Firmware for the 3dr-ctrl-zero-h7-oem-revg board", + "image": "", + "build_time": 0, + "summary": "3dr-ctrl-zero-h7-oem-revg", + "version": "0.1", + "image_size": 0, + "image_maxsize": 1966080, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/init/rc.board_defaults b/boards/3dr/ctrl-zero-h7-oem-revg/init/rc.board_defaults new file mode 100755 index 000000000000..32117f4a5886 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/init/rc.board_defaults @@ -0,0 +1,9 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ + +param set-default BAT1_V_DIV 10.1 +param set-default BAT1_A_PER_V 17 + +safety_button start diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/init/rc.board_sensors b/boards/3dr/ctrl-zero-h7-oem-revg/init/rc.board_sensors new file mode 100755 index 000000000000..563c24bdf4fe --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/init/rc.board_sensors @@ -0,0 +1,19 @@ +#!/bin/sh +# +# board specific sensors init +#------------------------------------------------------------------------------ + +board_adc start + +# Internal ICM-20602 +icm20602 -s -b 1 -R 8 start + +# Internal SPI bus BMI088 accel & gyro +bmi088 -A -s -b 5 -R 8 start +bmi088 -G -s -b 5 -R 8 start + +# Internal ICM-20948 (with magnetometer) +icm20948 -s -b 1 -R 8 -M start + +# Interal DPS310 (barometer) +dps310 -s -b 2 start diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/bootloader/defconfig b/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/bootloader/defconfig new file mode 100755 index 000000000000..796a2853edef --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/bootloader/defconfig @@ -0,0 +1,91 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +# CONFIG_DEV_CONSOLE is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_SPI_EXCHANGE is not set +# CONFIG_STM32H7_SYSCFG is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32h7" +CONFIG_ARCH_CHIP_STM32H743II=y +CONFIG_ARCH_CHIP_STM32H7=y +CONFIG_ARCH_INTERRUPTSTACK=768 +CONFIG_ARMV7M_BASEPRI_WAR=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BOARDCTL=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_ASSERT_RESET_VALUE=0 +CONFIG_BOARD_INITTHREAD_PRIORITY=254 +CONFIG_BOARD_LATE_INITIALIZE=y +CONFIG_BOARD_LOOPSPERMSEC=95150 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x1124 +CONFIG_CDCACM_PRODUCTSTR="PX4 BL 3DR ControlZeroH7 OEM" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_VENDORSTR="3DR" +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_TCBINFO=y +CONFIG_DEFAULT_SMALL=y +CONFIG_EXPERIMENTAL=y +CONFIG_FDCLONE_DISABLE=y +CONFIG_FDCLONE_STDIO=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_IDLETHREAD_STACKSIZE=750 +CONFIG_INIT_ENTRYPOINT="bootloader_main" +CONFIG_INIT_STACKSIZE=3194 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAM_SIZE=245760 +CONFIG_RAM_START=0x20010000 +CONFIG_RAW_BINARY=y +CONFIG_SERIAL_TERMIOS=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_SPI=y +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STM32H7_BKPSRAM=y +CONFIG_STM32H7_DMA1=y +CONFIG_STM32H7_OTGFS=y +CONFIG_STM32H7_PROGMEM=y +CONFIG_STM32H7_PWR_EXTERNAL_SOURCE_SUPPLY=y +CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_USART3=y +CONFIG_SYSTEMTICK_HOOK=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_TTY_SIGINT=y +CONFIG_TTY_SIGINT_CHAR=0x03 +CONFIG_TTY_SIGTSTP=y +CONFIG_USART3_RXBUFSIZE=600 +CONFIG_USART3_TXBUFSIZE=300 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/include/board.h b/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/include/board.h new file mode 100755 index 000000000000..698e8835fc12 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/include/board.h @@ -0,0 +1,288 @@ +/************************************************************************************ + * nuttx-config/include/board.h + * + * Copyright (C) 2020 Gregory Nutt. All rights reserved. + * Authors: David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ************************************************************************************/ +#pragma once + +#include "board_dma_map.h" + +#include + +#ifndef __ASSEMBLY__ +# include +#endif + +#include "stm32_rcc.h" +#include "stm32_sdmmc.h" + +/* Clocking *************************************************************************/ +/* The board provides the following clock sources: + * + * X1: 24 MHz crystal for HSE + * + * So we have these clock source available within the STM32 + * + * HSI: 16 MHz RC factory-trimmed internal oscillator + * HSE: 24 MHz crystal for HSE + */ +#define STM32_BOARD_XTAL 24000000ul + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL +#define STM32_LSE_FREQUENCY 0 + +#define STM32_HSEBYP_ENABLE 1 + +/* Main PLL Configuration. + * + * PLL source is HSE = 24,000,000 + * + * PLL_VCOx = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * Subject to: + * + * 1 <= PLLM <= 63 + * 4 <= PLLN <= 512 + * 150 MHz <= PLL_VCOL <= 420MHz + * 192 MHz <= PLL_VCOH <= 836MHz + * + * SYSCLK = PLL_VCO / PLLP + * CPUCLK = SYSCLK / D1CPRE + * Subject to + * + * PLLP1 = {2, 4, 6, 8, ..., 128} + * PLLP2,3 = {2, 3, 4, ..., 128} + * CPUCLK <= 480 MHz + */ +#define STM32_PLLCFG_PLLSRC RCC_PLLCKSELR_PLLSRC_HSE + +/* PLL1, wide 4 - 8 MHz input, enable DIVP, DIVQ, DIVR + * + * PLL1_VCO = (24,000,000 / 2) * 80 = 960 MHz + * + * PLL1P = PLL1_VCO/2 = 960 MHz / 2 = 480 MHz + * PLL1Q = PLL1_VCO/4 = 960 MHz / 4 = 240 MHz + * PLL1R = PLL1_VCO/8 = 960 MHz / 8 = 120 MHz + */ +#define STM32_PLLCFG_PLL1CFG (RCC_PLLCFGR_PLL1VCOSEL_WIDE|RCC_PLLCFGR_PLL1RGE_4_8_MHZ|RCC_PLLCFGR_DIVP1EN|RCC_PLLCFGR_DIVQ1EN|RCC_PLLCFGR_DIVR1EN) +#define STM32_PLLCFG_PLL1M RCC_PLLCKSELR_DIVM1(2) +#define STM32_PLLCFG_PLL1N RCC_PLL1DIVR_N1(80) +#define STM32_PLLCFG_PLL1P RCC_PLL1DIVR_P1(2) +#define STM32_PLLCFG_PLL1Q RCC_PLL1DIVR_Q1(4) +#define STM32_PLLCFG_PLL1R RCC_PLL1DIVR_R1(8) + +#define STM32_VCO1_FREQUENCY ((STM32_HSE_FREQUENCY / 2) * 80) +#define STM32_PLL1P_FREQUENCY (STM32_VCO1_FREQUENCY / 2) +#define STM32_PLL1Q_FREQUENCY (STM32_VCO1_FREQUENCY / 4) +#define STM32_PLL1R_FREQUENCY (STM32_VCO1_FREQUENCY / 8) + +/* PLL2 */ +#define STM32_PLLCFG_PLL2CFG (RCC_PLLCFGR_PLL2VCOSEL_WIDE|RCC_PLLCFGR_PLL2RGE_4_8_MHZ|RCC_PLLCFGR_DIVP2EN|RCC_PLLCFGR_DIVQ2EN|RCC_PLLCFGR_DIVR2EN) +#define STM32_PLLCFG_PLL2M RCC_PLLCKSELR_DIVM2(4) +#define STM32_PLLCFG_PLL2N RCC_PLL2DIVR_N2(32) +#define STM32_PLLCFG_PLL2P RCC_PLL2DIVR_P2(2) +#define STM32_PLLCFG_PLL2Q RCC_PLL2DIVR_Q2(2) +#define STM32_PLLCFG_PLL2R RCC_PLL2DIVR_R2(2) + +#define STM32_VCO2_FREQUENCY ((STM32_HSE_FREQUENCY / 4) * 32) +#define STM32_PLL2P_FREQUENCY (STM32_VCO2_FREQUENCY / 2) +#define STM32_PLL2Q_FREQUENCY (STM32_VCO2_FREQUENCY / 2) +#define STM32_PLL2R_FREQUENCY (STM32_VCO2_FREQUENCY / 2) + +/* PLL3 */ +#define STM32_PLLCFG_PLL3CFG (RCC_PLLCFGR_PLL3VCOSEL_WIDE|RCC_PLLCFGR_PLL3RGE_4_8_MHZ|RCC_PLLCFGR_DIVQ3EN) +#define STM32_PLLCFG_PLL3M RCC_PLLCKSELR_DIVM3(4) +#define STM32_PLLCFG_PLL3N RCC_PLL3DIVR_N3(32) +#define STM32_PLLCFG_PLL3P RCC_PLL3DIVR_P3(2) +#define STM32_PLLCFG_PLL3Q RCC_PLL3DIVR_Q3(4) +#define STM32_PLLCFG_PLL3R RCC_PLL3DIVR_R3(2) + +#define STM32_VCO3_FREQUENCY ((STM32_HSE_FREQUENCY / 4) * 32) +#define STM32_PLL3P_FREQUENCY (STM32_VCO3_FREQUENCY / 2) +#define STM32_PLL3Q_FREQUENCY (STM32_VCO3_FREQUENCY / 4) +#define STM32_PLL3R_FREQUENCY (STM32_VCO3_FREQUENCY / 2) + +/* SYSCLK = PLL1P = 480MHz + * CPUCLK = SYSCLK / 1 = 480 MHz + */ +#define STM32_RCC_D1CFGR_D1CPRE (RCC_D1CFGR_D1CPRE_SYSCLK) +#define STM32_SYSCLK_FREQUENCY (STM32_PLL1P_FREQUENCY) +#define STM32_CPUCLK_FREQUENCY (STM32_SYSCLK_FREQUENCY / 1) + +/* Configure Clock Assignments */ + +/* AHB clock (HCLK) is SYSCLK/2 (240 MHz max) + * HCLK1 = HCLK2 = HCLK3 = HCLK4 = 240 + */ +#define STM32_RCC_D1CFGR_HPRE RCC_D1CFGR_HPRE_SYSCLKd2 /* HCLK = SYSCLK / 2 */ +#define STM32_ACLK_FREQUENCY (STM32_CPUCLK_FREQUENCY / 2) /* ACLK in D1, HCLK3 in D1 */ +#define STM32_HCLK_FREQUENCY (STM32_CPUCLK_FREQUENCY / 2) /* HCLK in D2, HCLK4 in D3 */ +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/2 (120 MHz) */ +#define STM32_RCC_D2CFGR_D2PPRE1 RCC_D2CFGR_D2PPRE1_HCLKd2 /* PCLK1 = HCLK / 2 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB2 clock (PCLK2) is HCLK/2 (120 MHz) */ +#define STM32_RCC_D2CFGR_D2PPRE2 RCC_D2CFGR_D2PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB3 clock (PCLK3) is HCLK/2 (120 MHz) */ +#define STM32_RCC_D1CFGR_D1PPRE RCC_D1CFGR_D1PPRE_HCLKd2 /* PCLK3 = HCLK / 2 */ +#define STM32_PCLK3_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB4 clock (PCLK4) is HCLK/4 (120 MHz) */ +#define STM32_RCC_D3CFGR_D3PPRE RCC_D3CFGR_D3PPRE_HCLKd2 /* PCLK4 = HCLK / 2 */ +#define STM32_PCLK4_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timer clock frequencies */ + +/* Timers driven from APB1 will be twice PCLK1 */ +#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* Timers driven from APB2 will be twice PCLK2 */ +#define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM15_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM16_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM17_CLKIN (2*STM32_PCLK2_FREQUENCY) + +/* Kernel Clock Configuration + * Note: look at Table 54 in ST Manual + */ +#define STM32_RCC_D2CCIP2R_I2C123SRC RCC_D2CCIP2R_I2C123SEL_HSI /* I2C123 clock source */ +#define STM32_RCC_D3CCIPR_I2C4SRC RCC_D3CCIPR_I2C4SEL_HSI /* I2C4 clock source */ +#define STM32_RCC_D2CCIP1R_SPI123SRC RCC_D2CCIP1R_SPI123SEL_PLL2 /* SPI123 clock source */ +#define STM32_RCC_D2CCIP1R_SPI45SRC RCC_D2CCIP1R_SPI45SEL_PLL2 /* SPI45 clock source */ +#define STM32_RCC_D3CCIPR_SPI6SRC RCC_D3CCIPR_SPI6SEL_PLL2 /* SPI6 clock source */ +#define STM32_RCC_D2CCIP2R_USBSRC RCC_D2CCIP2R_USBSEL_PLL3 /* USB 1 and 2 clock source */ +#define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */ +#define STM32_RCC_D2CCIP1R_FDCANSEL RCC_D2CCIP1R_FDCANSEL_HSE /* FDCAN 1 2 clock source */ + +#define STM32_FDCANCLK STM32_HSE_FREQUENCY + +/* UART clock selection */ +/* reset to default to overwrite any changes done by any bootloader */ + +#define STM32_RCC_D2CCIP2R_USART234578_SEL RCC_D2CCIP2R_USART234578SEL_RCC +#define STM32_RCC_D2CCIP2R_USART16_SEL RCC_D2CCIP2R_USART16SEL_RCC + +/* FLASH wait states */ +#define BOARD_FLASH_WAITSTATES 2 + +/* SDMMC definitions ********************************************************/ +/* Init 400kHz, freq = PLL1Q/(2*div) div = PLL1Q/(2*freq) */ +#define STM32_SDMMC_INIT_CLKDIV (300 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) + +/* 25 MHz Max for now, 25 mHZ = PLL1Q/(2*div), div = PLL1Q/(2*freq) + * div = 4.8 = 240 / 50, So round up to 5 for default speed 24 MB/s + */ +#if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA) +# define STM32_SDMMC_MMCXFR_CLKDIV (5 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#else +# define STM32_SDMMC_MMCXFR_CLKDIV (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#endif +#if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA) +# define STM32_SDMMC_SDXFR_CLKDIV (5 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#else +# define STM32_SDMMC_SDXFR_CLKDIV (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#endif + +#define STM32_SDMMC_CLKCR_EDGE STM32_SDMMC_CLKCR_NEGEDGE + + +/* UART/USART */ +#define GPIO_USART2_TX GPIO_USART2_TX_2 /* PD5 */ +#define GPIO_USART2_RX GPIO_USART2_RX_2 /* PD6 */ +#define GPIO_USART2_CTS GPIO_USART2_CTS_NSS_2 /* PD3 */ +#define GPIO_USART2_RTS GPIO_USART2_RTS_2 /* PD4 */ + +#define GPIO_USART3_TX GPIO_USART3_TX_3 /* PD8 */ +#define GPIO_USART3_RX GPIO_USART3_RX_3 /* PD9 */ +#define GPIO_USART3_CTS GPIO_USART3_CTS_NSS_2 /* PD11 */ +#define GPIO_USART3_RTS GPIO_USART3_RTS_2 /* PD12 */ + +#define GPIO_UART4_TX GPIO_UART4_TX_2 /* PA0 */ +#define GPIO_UART4_RX GPIO_UART4_RX_2 /* PA1 */ + +#define GPIO_USART6_TX 0 /* USART6 is RX-only */ +#define GPIO_USART6_RX GPIO_USART6_RX_1 /* PC7 */ + +#define GPIO_UART7_TX GPIO_UART7_TX_3 /* PE8 */ +#define GPIO_UART7_RX GPIO_UART7_RX_3 /* PE7 */ + +#define GPIO_UART8_TX GPIO_UART8_TX_1 /* PE1 */ +#define GPIO_UART8_RX GPIO_UART8_RX_1 /* PE0 */ + + +/* CAN */ +#define GPIO_CAN1_RX GPIO_CAN1_RX_3 /* PD0 */ +#define GPIO_CAN1_TX GPIO_CAN1_TX_3 /* PD1 */ + +#define GPIO_CAN2_RX GPIO_CAN2_RX_1 /* PB12 */ +#define GPIO_CAN2_TX GPIO_CAN2_TX_1 /* PB13 */ + + +/* SPI */ +#define ADJ_SLEW_RATE(p) (((p) & ~GPIO_SPEED_MASK) | (GPIO_SPEED_2MHz)) + +#define GPIO_SPI1_SCK ADJ_SLEW_RATE(GPIO_SPI1_SCK_1) /* PA5 */ +#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 /* PA6 */ +#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 /* PA7 */ + +#define GPIO_SPI2_SCK ADJ_SLEW_RATE(GPIO_SPI2_SCK_3) /* PB10 */ +#define GPIO_SPI2_MISO GPIO_SPI2_MISO_1 /* PB14 */ +#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_1 /* PB15 */ + +#define GPIO_SPI5_SCK ADJ_SLEW_RATE(GPIO_SPI5_SCK_1) /* PF7 */ +#define GPIO_SPI5_MISO GPIO_SPI5_MISO_1 /* PF8 */ +#define GPIO_SPI5_MOSI GPIO_SPI5_MOSI_2 /* PF9 */ + + +/* I2C */ +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 /* PB8 */ +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 /* PB9 */ + +#define GPIO_I2C3_SCL GPIO_I2C3_SCL_2 /* PH7 */ +#define GPIO_I2C3_SDA GPIO_I2C3_SDA_2 /* PH8 */ + +#define GPIO_I2C4_SCL GPIO_I2C4_SCL_4 /* PB6 */ +#define GPIO_I2C4_SDA GPIO_I2C4_SDA_4 /* PB7 */ diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/include/board_dma_map.h b/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/include/board_dma_map.h new file mode 100755 index 000000000000..99a2fe3ba9b9 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/include/board_dma_map.h @@ -0,0 +1,44 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +// DMAMUX1 +#define DMAMAP_SPI1_RX DMAMAP_DMA12_SPI1RX_0 /* DMA1:37 */ +#define DMAMAP_SPI1_TX DMAMAP_DMA12_SPI1TX_0 /* DMA1:38 */ + +#define DMAMAP_USART6_RX DMAMAP_DMA12_USART6RX_1 /* DMA1:71 */ +#define DMAMAP_USART6_TX DMAMAP_DMA12_USART6TX_1 /* DMA1:72 */ + +#define DMAMAP_SPI5_RX DMAMAP_DMA12_SPI5RX_0 /* DMA1:83 */ +#define DMAMAP_SPI5_TX DMAMAP_DMA12_SPI5TX_0 /* DMA1:84 */ diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/nsh/defconfig b/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/nsh/defconfig new file mode 100644 index 000000000000..c9ec46ed878e --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/nsh/defconfig @@ -0,0 +1,260 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +# CONFIG_DISABLE_ENVIRON is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_DISABLE_PTHREAD is not set +# CONFIG_MMCSD_HAVE_CARDDETECT is not set +# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set +# CONFIG_MMCSD_MMCSUPPORT is not set +# CONFIG_MMCSD_SPI is not set +# CONFIG_NSH_DISABLEBG is not set +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLE_CAT is not set +# CONFIG_NSH_DISABLE_CD is not set +# CONFIG_NSH_DISABLE_CP is not set +# CONFIG_NSH_DISABLE_DATE is not set +# CONFIG_NSH_DISABLE_DF is not set +# CONFIG_NSH_DISABLE_ECHO is not set +# CONFIG_NSH_DISABLE_ENV is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXPORT is not set +# CONFIG_NSH_DISABLE_FREE is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_HELP is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_KILL is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_LS is not set +# CONFIG_NSH_DISABLE_MKDIR is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_MOUNT is not set +# CONFIG_NSH_DISABLE_MV is not set +# CONFIG_NSH_DISABLE_PS is not set +# CONFIG_NSH_DISABLE_PSSTACKUSAGE is not set +# CONFIG_NSH_DISABLE_PWD is not set +# CONFIG_NSH_DISABLE_RM is not set +# CONFIG_NSH_DISABLE_RMDIR is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_SET is not set +# CONFIG_NSH_DISABLE_SLEEP is not set +# CONFIG_NSH_DISABLE_SOURCE is not set +# CONFIG_NSH_DISABLE_TEST is not set +# CONFIG_NSH_DISABLE_TIME is not set +# CONFIG_NSH_DISABLE_UMOUNT is not set +# CONFIG_NSH_DISABLE_UNSET is not set +# CONFIG_NSH_DISABLE_USLEEP is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32h7" +CONFIG_ARCH_CHIP_STM32H743II=y +CONFIG_ARCH_CHIP_STM32H7=y +CONFIG_ARCH_INTERRUPTSTACK=768 +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARMV7M_BASEPRI_WAR=y +CONFIG_ARMV7M_DCACHE=y +CONFIG_ARMV7M_DTCM=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARM_MPU_EARLY_RESET=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_ASSERT_RESET_VALUE=0 +CONFIG_BOARD_CRASHDUMP=y +CONFIG_BOARD_LOOPSPERMSEC=95150 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x1124 +CONFIG_CDCACM_PRODUCTSTR="3DRControlZeroH7OEM_revG" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_VENDORSTR="3DR" +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_MEMFAULT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_TCBINFO=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DEV_FIFO_SIZE=0 +CONFIG_DEV_PIPE_MAXSIZE=1024 +CONFIG_DEV_PIPE_SIZE=70 +CONFIG_EXPERIMENTAL=y +CONFIG_STM32H7_PWR_EXTERNAL_SOURCE_SUPPLY=y +CONFIG_FAT_DMAMEMORY=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_LFN_ALIAS_HASH=y +CONFIG_FDCLONE_STDIO=y +CONFIG_FS_BINFS=y +CONFIG_FS_CROMFS=y +CONFIG_FS_FAT=y +CONFIG_FS_FATTIME=y +CONFIG_FS_PROCFS=y +CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y +CONFIG_FS_PROCFS_MAX_TASKS=64 +CONFIG_FS_PROCFS_REGISTER=y +CONFIG_FS_ROMFS=y +CONFIG_GRAN=y +CONFIG_GRAN_INTR=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_I2C=y +CONFIG_I2C_RESET=y +CONFIG_IDLETHREAD_STACKSIZE=750 +CONFIG_INIT_ENTRYPOINT="nsh_main" +CONFIG_INIT_STACKSIZE=3194 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_MAX_EXITFUNS=1 +CONFIG_LIBC_STRERROR=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MMCSD_SDIO=y +CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y +CONFIG_MM_REGIONS=4 +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=y +CONFIG_MTD_PROGMEM=y +CONFIG_MTD_RAMTRON=y +CONFIG_NAME_MAX=40 +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_ARGCAT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CMDPARMS=y +CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=15 +CONFIG_NSH_NESTDEPTH=8 +CONFIG_NSH_QUOTE=y +CONFIG_NSH_ROMFSETC=y +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_VARS=y +CONFIG_OTG_ID_GPIO_DISABLE=y +CONFIG_PIPES=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTHREAD_MUTEX_ROBUST=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 +CONFIG_RAMTRON_SETSPEED=y +CONFIG_RAM_SIZE=245760 +CONFIG_RAM_START=0x20010000 +CONFIG_RAW_BINARY=y +CONFIG_READLINE_CMD_HISTORY=y +CONFIG_READLINE_TABCOMPLETION=y +CONFIG_RTC_DATETIME=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_INSTRUMENTATION_SWITCH=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SDMMC1_SDIO_PULLUP=y +CONFIG_SEM_PREALLOCHOLDERS=32 +CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y +CONFIG_SERIAL_TERMIOS=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_SIG_SIGWORK=4 +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=256 +CONFIG_STM32H7_ADC1=y +CONFIG_STM32H7_ADC3=y +CONFIG_STM32H7_BBSRAM=y +CONFIG_STM32H7_BBSRAM_FILES=5 +CONFIG_STM32H7_BDMA=y +CONFIG_STM32H7_BKPSRAM=y +CONFIG_STM32H7_DMA1=y +CONFIG_STM32H7_DMA2=y +CONFIG_STM32H7_DMACAPABLE=y +CONFIG_STM32H7_FLOWCONTROL_BROKEN=y +CONFIG_STM32H7_I2C1=y +CONFIG_STM32H7_I2C3=y +CONFIG_STM32H7_I2C4=y +CONFIG_STM32H7_I2C_DYNTIMEO=y +CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10 +CONFIG_STM32H7_OTGFS=y +CONFIG_STM32H7_PROGMEM=y +CONFIG_STM32H7_RTC=y +CONFIG_STM32H7_RTC_HSECLOCK=y +CONFIG_STM32H7_RTC_MAGIC_REG=1 +CONFIG_STM32H7_SAVE_CRASHDUMP=y +CONFIG_STM32H7_SDMMC1=y +CONFIG_STM32H7_SERIALBRK_BSDCOMPAT=y +CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32H7_SPI1=y +CONFIG_STM32H7_SPI1_DMA=y +CONFIG_STM32H7_SPI1_DMA_BUFFER=1024 +CONFIG_STM32H7_SPI2=y +CONFIG_STM32H7_SPI5=y +CONFIG_STM32H7_SPI5_DMA=y +CONFIG_STM32H7_SPI5_DMA_BUFFER=1024 +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_TIM2=y +CONFIG_STM32H7_TIM3=y +CONFIG_STM32H7_TIM4=y +CONFIG_STM32H7_TIM8=y +CONFIG_STM32H7_UART4=y +CONFIG_STM32H7_UART7=y +CONFIG_STM32H7_UART8=y +CONFIG_STM32H7_USART2=y +CONFIG_STM32H7_USART3=y +CONFIG_STM32H7_USART6=y +CONFIG_STM32H7_USART_BREAKS=y +CONFIG_STM32H7_USART_INVERT=y +CONFIG_STM32H7_USART_SINGLEWIRE=y +CONFIG_STM32H7_USART_SWAP=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_TTY_SIGINT=y +CONFIG_TTY_SIGTSTP=y +CONFIG_UART4_BAUD=57600 +CONFIG_UART4_RXBUFSIZE=600 +CONFIG_UART4_TXBUFSIZE=1500 +CONFIG_UART7_BAUD=57600 +CONFIG_UART7_RXBUFSIZE=600 +CONFIG_UART7_TXBUFSIZE=1500 +CONFIG_UART8_BAUD=57600 +CONFIG_UART8_RXBUFSIZE=600 +CONFIG_UART8_SERIAL_CONSOLE=y +CONFIG_UART8_TXBUFSIZE=1500 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_IFLOWCONTROL=y +CONFIG_USART2_OFLOWCONTROL=y +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_TXBUFSIZE=1500 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_IFLOWCONTROL=y +CONFIG_USART3_OFLOWCONTROL=y +CONFIG_USART3_RXBUFSIZE=600 +CONFIG_USART3_TXBUFSIZE=3000 +CONFIG_USART6_BAUD=57600 +CONFIG_USART6_RXBUFSIZE=600 +CONFIG_USART6_TXBUFSIZE=1500 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/scripts/bootloader_script.ld b/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/scripts/bootloader_script.ld new file mode 100755 index 000000000000..3fb4cc1f33ce --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/scripts/bootloader_script.ld @@ -0,0 +1,221 @@ +/**************************************************************************** + * scripts/script.ld + * + * Copyright (C) 2020 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* The board uses an STM32H743II has 2048Kb of main FLASH memory. + * The flash memory is partitioned into a User Flash memory and a System + * Flash memory. Each of these memories has two banks: + * + * 1) User Flash memory: + * + * Bank 1: Start address 0x0800:0000 to 0x080F:FFFF with 8 sectors, 128Kb each + * Bank 2: Start address 0x0810:0000 to 0x081F:FFFF with 8 sectors, 128Kb each + * + * 2) System Flash memory: + * + * Bank 1: Start address 0x1FF0:0000 to 0x1FF1:FFFF with 1 x 128Kb sector + * Bank 1: Start address 0x1FF4:0000 to 0x1FF5:FFFF with 1 x 128Kb sector + * + * 3) User option bytes for user configuration, only in Bank 1. + * + * In the STM32H743II, two different boot spaces can be selected through + * the BOOT pin and the boot base address programmed in the BOOT_ADD0 and + * BOOT_ADD1 option bytes: + * + * 1) BOOT=0: Boot address defined by user option byte BOOT_ADD0[15:0]. + * ST programmed value: Flash memory at 0x0800:0000 + * 2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0]. + * ST programmed value: System bootloader at 0x1FF0:0000 + * + * There's a switch on board, the BOOT0 pin is at ground so by default, + * the STM32 will boot to address 0x0800:0000 in FLASH unless the switch is + * drepresed, then the boot will be from 0x1FF0:0000 + * + * The STM32H743ZI also has 1024Kb of data SRAM. + * SRAM is split up into several blocks and into three power domains: + * + * 1) TCM SRAMs are dedicated to the Cortex-M7 and are accessible with + * 0 wait states by the Cortex-M7 and by MDMA through AHBS slave bus + * + * 1.1) 128Kb of DTCM-RAM beginning at address 0x2000:0000 + * + * The DTCM-RAM is organized as 2 x 64Kb DTCM-RAMs on 2 x 32 bit + * DTCM ports. The DTCM-RAM could be used for critical real-time + * data, such as interrupt service routines or stack / heap memory. + * Both DTCM-RAMs can be used in parallel (for load/store operations) + * thanks to the Cortex-M7 dual issue capability. + * + * 1.2) 64Kb of ITCM-RAM beginning at address 0x0000:0000 + * + * This RAM is connected to ITCM 64-bit interface designed for + * execution of critical real-times routines by the CPU. + * + * 2) AXI SRAM (D1 domain) accessible by all system masters except BDMA + * through D1 domain AXI bus matrix + * + * 2.1) 512Kb of SRAM beginning at address 0x2400:0000 + * + * 3) AHB SRAM (D2 domain) accessible by all system masters except BDMA + * through D2 domain AHB bus matrix + * + * 3.1) 128Kb of SRAM1 beginning at address 0x3000:0000 + * 3.2) 128Kb of SRAM2 beginning at address 0x3002:0000 + * 3.3) 32Kb of SRAM3 beginning at address 0x3004:0000 + * + * SRAM1 - SRAM3 are one contiguous block: 288Kb at address 0x3000:0000 + * + * 4) AHB SRAM (D3 domain) accessible by most of system masters + * through D3 domain AHB bus matrix + * + * 4.1) 64Kb of SRAM4 beginning at address 0x3800:0000 + * 4.1) 4Kb of backup RAM beginning at address 0x3880:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + */ + +MEMORY +{ + ITCM_RAM (rwx) : ORIGIN = 0x00000000, LENGTH = 64K + FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 128K + DTCM1_RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + DTCM2_RAM (rwx) : ORIGIN = 0x20010000, LENGTH = 64K + AXI_SRAM (rwx) : ORIGIN = 0x24000000, LENGTH = 512K /* D1 domain AXI bus */ + SRAM1 (rwx) : ORIGIN = 0x30000000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM2 (rwx) : ORIGIN = 0x30020000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM3 (rwx) : ORIGIN = 0x30040000, LENGTH = 32K /* D2 domain AHB bus */ + SRAM4 (rwx) : ORIGIN = 0x38000000, LENGTH = 64K /* D3 domain */ + BKPRAM (rwx) : ORIGIN = 0x38800000, LENGTH = 4K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + } > FLASH + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > FLASH + + .ARM.extab : { + *(.ARM.extab*) + } > FLASH + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > FLASH + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > AXI_SRAM AT > FLASH + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > AXI_SRAM + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } + + .ramfunc : { + _sramfuncs = .; + *(.ramfunc .ramfunc.*) + . = ALIGN(4); + _eramfuncs = .; + } > ITCM_RAM AT > FLASH + + _framfuncs = LOADADDR(.ramfunc); +} diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/scripts/script.ld b/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/scripts/script.ld new file mode 100755 index 000000000000..02e763a790fb --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/scripts/script.ld @@ -0,0 +1,228 @@ +/**************************************************************************** + * scripts/script.ld + * + * Copyright (C) 2021 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* The board uses an STM32H743XIH6 and has 2048Kb of main FLASH memory. + * The flash memory is partitioned into a User Flash memory and a System + * Flash memory. Each of these memories has two banks: + * + * 1) User Flash memory: + * + * Bank 1: Start address 0x0800:0000 to 0x080F:FFFF with 8 sectors, 128Kb each + * Bank 2: Start address 0x0810:0000 to 0x081F:FFFF with 8 sectors, 128Kb each + * + * 2) System Flash memory: + * + * Bank 1: Start address 0x1FF0:0000 to 0x1FF1:FFFF with 1 x 128Kb sector + * Bank 1: Start address 0x1FF4:0000 to 0x1FF5:FFFF with 1 x 128Kb sector + * + * 3) User option bytes for user configuration, only in Bank 1. + * + * In the STM32H743II, two different boot spaces can be selected through + * the BOOT pin and the boot base address programmed in the BOOT_ADD0 and + * BOOT_ADD1 option bytes: + * + * 1) BOOT=0: Boot address defined by user option byte BOOT_ADD0[15:0]. + * ST programmed value: Flash memory at 0x0800:0000 + * 2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0]. + * ST programmed value: System bootloader at 0x1FF0:0000 + * + * There's a switch on board, the BOOT0 pin is at ground so by default, + * the STM32 will boot to address 0x0800:0000 in FLASH unless the switch is + * drepresed, then the boot will be from 0x1FF0:0000 + * + * The STM32H743ZI also has 1024Kb of data SRAM. + * SRAM is split up into several blocks and into three power domains: + * + * 1) TCM SRAMs are dedicated to the Cortex-M7 and are accessible with + * 0 wait states by the Cortex-M7 and by MDMA through AHBS slave bus + * + * 1.1) 128Kb of DTCM-RAM beginning at address 0x2000:0000 + * + * The DTCM-RAM is organized as 2 x 64Kb DTCM-RAMs on 2 x 32 bit + * DTCM ports. The DTCM-RAM could be used for critical real-time + * data, such as interrupt service routines or stack / heap memory. + * Both DTCM-RAMs can be used in parallel (for load/store operations) + * thanks to the Cortex-M7 dual issue capability. + * + * 1.2) 64Kb of ITCM-RAM beginning at address 0x0000:0000 + * + * This RAM is connected to ITCM 64-bit interface designed for + * execution of critical real-times routines by the CPU. + * + * 2) AXI SRAM (D1 domain) accessible by all system masters except BDMA + * through D1 domain AXI bus matrix + * + * 2.1) 512Kb of SRAM beginning at address 0x2400:0000 + * + * 3) AHB SRAM (D2 domain) accessible by all system masters except BDMA + * through D2 domain AHB bus matrix + * + * 3.1) 128Kb of SRAM1 beginning at address 0x3000:0000 + * 3.2) 128Kb of SRAM2 beginning at address 0x3002:0000 + * 3.3) 32Kb of SRAM3 beginning at address 0x3004:0000 + * + * SRAM1 - SRAM3 are one contiguous block: 288Kb at address 0x3000:0000 + * + * 4) AHB SRAM (D3 domain) accessible by most of system masters + * through D3 domain AHB bus matrix + * + * 4.1) 64Kb of SRAM4 beginning at address 0x3800:0000 + * 4.1) 4Kb of backup RAM beginning at address 0x3880:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + */ + +MEMORY +{ + ITCM_RAM (rwx) : ORIGIN = 0x00000000, LENGTH = 64K + FLASH (rx) : ORIGIN = 0x08020000, LENGTH = 1920K + + DTCM1_RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + DTCM2_RAM (rwx) : ORIGIN = 0x20010000, LENGTH = 64K + AXI_SRAM (rwx) : ORIGIN = 0x24000000, LENGTH = 512K /* D1 domain AXI bus */ + SRAM1 (rwx) : ORIGIN = 0x30000000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM2 (rwx) : ORIGIN = 0x30020000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM3 (rwx) : ORIGIN = 0x30040000, LENGTH = 32K /* D2 domain AHB bus */ + SRAM4 (rwx) : ORIGIN = 0x38000000, LENGTH = 64K /* D3 domain */ + BKPRAM (rwx) : ORIGIN = 0x38800000, LENGTH = 4K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + } > FLASH + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > FLASH + + + .ARM.extab : { + *(.ARM.extab*) + } > FLASH + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > FLASH + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + + /* Pad out last section as the STM32H7 Flash write size is 256 bits. 32 bytes */ + . = ALIGN(16); + FILL(0xffff) + . += 16; + } > AXI_SRAM AT > FLASH = 0xffff + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > AXI_SRAM + + /* Emit the the D3 power domain section for locating BDMA data */ + + .sram4_reserve (NOLOAD) : + { + *(.sram4) + . = ALIGN(4); + _sram4_heap_start = ABSOLUTE(.); + } > SRAM4 + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/src/modules/differential_drive/DifferentialDriveKinematics/CMakeLists.txt b/boards/3dr/ctrl-zero-h7-oem-revg/src/CMakeLists.txt old mode 100644 new mode 100755 similarity index 72% rename from src/modules/differential_drive/DifferentialDriveKinematics/CMakeLists.txt rename to boards/3dr/ctrl-zero-h7-oem-revg/src/CMakeLists.txt index c556db2b8f3f..6a7d2ce306f7 --- a/src/modules/differential_drive/DifferentialDriveKinematics/CMakeLists.txt +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2023-2024 PX4 Development Team. All rights reserved. +# Copyright (c) 2021 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -30,11 +30,36 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ +if("${PX4_BOARD_LABEL}" STREQUAL "bootloader") + add_library(drivers_board + bootloader_main.c + usb.c + ) + target_link_libraries(drivers_board + PRIVATE + nuttx_arch + nuttx_drivers + bootloader + ) + target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader/common) -px4_add_library(DifferentialDriveKinematics - DifferentialDriveKinematics.cpp -) +else() + add_library(drivers_board + i2c.cpp + init.c + led.c + spi.cpp + timer_config.cpp + usb.c + ) -target_include_directories(DifferentialDriveKinematics PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) - -px4_add_functional_gtest(SRC DifferentialDriveKinematicsTest.cpp LINKLIBS DifferentialDriveKinematics) + target_link_libraries(drivers_board + PRIVATE + arch_io_pins + arch_spi + drivers__led + nuttx_arch + nuttx_drivers + px4_layer + ) +endif() diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/src/board_config.h b/boards/3dr/ctrl-zero-h7-oem-revg/src/board_config.h new file mode 100755 index 000000000000..bcc8bba1a619 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/board_config.h @@ -0,0 +1,187 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file board_config.h + * + * Board internal definitions + */ + +#pragma once + +#include +#include +#include +#include + +/* LEDs are driven with push open drain to support Anode to 5V or 3.3V */ +#define GPIO_nLED_RED /* PB11 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11) +#define GPIO_nLED_GREEN /* PB1 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN1) +#define GPIO_nLED_BLUE /* PB3 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN3) + +#define BOARD_HAS_CONTROL_STATUS_LEDS 1 +#define BOARD_OVERLOAD_LED LED_RED +#define BOARD_ARMED_STATE_LED LED_BLUE + +/* ADC channels */ +#define PX4_ADC_GPIO \ + /* PA2 */ GPIO_ADC12_INP14, \ + /* PA3 */ GPIO_ADC12_INP15, \ + /* PA4 */ GPIO_ADC12_INP18, \ + /* PC1 */ GPIO_ADC123_INP11 + +/* Define Channel numbers must match above GPIO pins */ +#define ADC_BATTERY_VOLTAGE_CHANNEL 14 /* PA2 BATT_VOLT_SENS */ +#define ADC_BATTERY_CURRENT_CHANNEL 15 /* PA3 BATT_CURRENT_SENS */ +#define ADC_SCALED_V5_CHANNEL 18 /* PA4 VDD_5V_SENS */ +#define ADC_RC_RSSI_CHANNEL 11 /* PC1 */ + +#define ADC_CHANNELS \ + ((1 << ADC_BATTERY_VOLTAGE_CHANNEL) | \ + (1 << ADC_BATTERY_CURRENT_CHANNEL) | \ + (1 << ADC_SCALED_V5_CHANNEL) | \ + (1 << ADC_RC_RSSI_CHANNEL)) + +/* HW has to large of R termination on ADC todo:change when HW value is chosen */ +#define BOARD_ADC_OPEN_CIRCUIT_V (5.6f) + +/* CAN Silence: Silent mode control */ +#define GPIO_CAN1_SILENT_S0 /* PF5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN5) + +/* PWM */ +#define DIRECT_PWM_OUTPUT_CHANNELS 8 + +/* Power supply control and monitoring GPIOs */ +#define GPIO_nPOWER_IN_A /* PB5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) + +#define GPIO_VDD_BRICK1_VALID GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */ +#define BOARD_NUMBER_BRICKS 1 + +#define GPIO_VDD_3V3_SPEKTRUM_POWER_EN /* PE4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4) +#define GPIO_VDD_1V2_CORE_POWER_EN /* PH5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTH|GPIO_PIN5) + +/* Define True logic Power Control in arch agnostic form */ +#define VDD_3V3_SPEKTRUM_POWER_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SPEKTRUM_POWER_EN, (!on_true)) +#define READ_VDD_3V3_SPEKTRUM_POWER_EN() (px4_arch_gpioread(GPIO_VDD_3V3_SPEKTRUM_POWER_EN) == 0) + +/* Tone alarm output */ +#define TONE_ALARM_TIMER 2 /* timer 2 */ +#define TONE_ALARM_CHANNEL 1 /* PA15 TIM2_CH1 */ + +#define GPIO_BUZZER_1 /* PA15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15) + +#define GPIO_TONE_ALARM_IDLE GPIO_BUZZER_1 +#define GPIO_TONE_ALARM GPIO_TIM2_CH1OUT_2 + +/* USB OTG FS */ +#define GPIO_OTGFS_VBUS /* PA9 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTA|GPIO_PIN9) + +/* High-resolution timer */ +#define HRT_TIMER 3 /* use timer3 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel 1 */ + +#define HRT_PPM_CHANNEL /* T3C3 */ 3 /* use capture/compare channel 3 */ +#define GPIO_PPM_IN /* PB0 T3C3 */ GPIO_TIM3_CH3IN_1 + +/* RC Serial port */ +#define RC_SERIAL_PORT "/dev/ttyS3" + +#define GPIO_RSSI_IN /* PC1 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1) + +/* Safety Switch: Enable the FMU to control it if there is no px4io fixme:This should be BOARD_SAFETY_LED(__ontrue) */ +#define GPIO_SAFETY_SWITCH_IN /* PC4 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4) +/* Enable the FMU to use the switch it if there is no px4io fixme:This should be BOARD_SAFTY_BUTTON() */ +#define GPIO_BTN_SAFETY GPIO_SAFETY_SWITCH_IN /* Enable the FMU to control it if there is no px4io */ + +/* Power switch controls ******************************************************/ +#define SPEKTRUM_POWER(_on_true) VDD_3V3_SPEKTRUM_POWER_EN(_on_true) + +/* + * Board has a separate RC_IN + * + * GPIO PPM_IN on PB0 T3CH3 + * SPEKTRUM_RX (it's TX or RX in Bind) on UART6 PC7 + * Inversion is possible in the UART and can drive GPIO_PPM_IN as an output + */ +#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0) +#define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_PPM_IN_AS_OUT) +#define SPEKTRUM_RX_AS_UART() /* Can be left as uart */ +#define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true)) + +/* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction) + * this board support the ADC system_power interface, and therefore + * provides the true logic GPIO BOARD_ADC_xxxx macros. + */ +#define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS)) +#define BOARD_ADC_USB_VALID BOARD_ADC_USB_CONNECTED +#define BOARD_ADC_SERVO_VALID (1) /* never powers off the Servo rail */ +#define BOARD_ADC_BRICK_VALID (px4_arch_gpioread(GPIO_VDD_BRICK1_VALID)) + +/* This board provides a DMA pool and APIs */ +#define BOARD_DMA_ALLOC_POOL_SIZE 5120 + +/* This board provides the board_on_reset interface */ +#define BOARD_HAS_ON_RESET 1 + +#define BOARD_HAS_STATIC_MANIFEST 1 + + +#define BOARD_NUM_IO_TIMERS 3 + + +#define BOARD_ENABLE_CONSOLE_BUFFER + +#define PX4_GPIO_INIT_LIST { \ + PX4_ADC_GPIO, \ + GPIO_CAN1_TX, \ + GPIO_CAN1_RX, \ + GPIO_CAN2_TX, \ + GPIO_CAN2_RX, \ + GPIO_CAN1_SILENT_S0, \ + GPIO_nPOWER_IN_A, \ + GPIO_VDD_3V3_SPEKTRUM_POWER_EN, \ + GPIO_VDD_1V2_CORE_POWER_EN, \ + GPIO_TONE_ALARM_IDLE, \ + GPIO_SAFETY_SWITCH_IN, \ + GPIO_OTGFS_VBUS, \ + } + +__BEGIN_DECLS +#ifndef __ASSEMBLY__ + +extern void stm32_spiinitialize(void); +extern void board_peripheral_reset(int ms); + +#include +#endif /* __ASSEMBLY__ */ +__END_DECLS diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/src/bootloader_main.c b/boards/3dr/ctrl-zero-h7-oem-revg/src/bootloader_main.c new file mode 100755 index 000000000000..bb6b4dc23aad --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/bootloader_main.c @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (c) 2020, 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file bootloader_main.c + * + * FMU-specific early startup code for bootloader +*/ + +#include "board_config.h" +#include "bl.h" + +#include +#include +#include +#include +#include +#include "arm_internal.h" +#include + +extern int sercon_main(int c, char **argv); + +__EXPORT void board_on_reset(int status) {} + +__EXPORT void stm32_boardinitialize(void) +{ + /* configure USB interfaces */ + stm32_configgpio(GPIO_OTGFS_VBUS); +} + +__EXPORT int board_app_initialize(uintptr_t arg) +{ + return 0; +} + +void board_late_initialize(void) +{ + sercon_main(0, NULL); +} + +extern void sys_tick_handler(void); +void board_timerhook(void) +{ + sys_tick_handler(); +} diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/src/hw_config.h b/boards/3dr/ctrl-zero-h7-oem-revg/src/hw_config.h new file mode 100755 index 000000000000..b212ccd60273 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/hw_config.h @@ -0,0 +1,135 @@ +/**************************************************************************** + * + * Copyright (C) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +/**************************************************************************** + * 10-8--2016: + * To simplify the ripple effect on the tools, we will be using + * /dev/serial/by-id/PX4 to locate PX4 devices. Therefore + * moving forward all Bootloaders must contain the prefix "PX4 BL " + * in the USBDEVICESTRING + * This Change will be made in an upcoming BL release + ****************************************************************************/ +/* + * Define usage to configure a bootloader + * + * + * Constant example Usage + * APP_LOAD_ADDRESS 0x08004000 - The address in Linker Script, where the app fw is org-ed + * BOOTLOADER_DELAY 5000 - Ms to wait while under USB pwr or bootloader request + * BOARD_FMUV2 + * INTERFACE_USB 1 - (Optional) Scan and use the USB interface for bootloading + * INTERFACE_USART 1 - (Optional) Scan and use the Serial interface for bootloading + * USBDEVICESTRING "PX4 BL FMU v2.x" - USB id string + * USBPRODUCTID 0x0011 - PID Should match defconfig + * BOOT_DELAY_ADDRESS 0x000001a0 - (Optional) From the linker script from Linker Script to get a custom + * delay provided by an APP FW + * BOARD_TYPE 9 - Must match .prototype boad_id + * _FLASH_KBYTES (*(uint16_t *)0x1fff7a22) - Run time flash size detection + * BOARD_FLASH_SECTORS ((_FLASH_KBYTES == 0x400) ? 11 : 23) - Run time determine the physical last sector + * BOARD_FLASH_SECTORS 11 - Hard coded zero based last sector + * BOARD_FLASH_SIZE (_FLASH_KBYTES*1024)- Total Flash size of device, determined at run time. + * (1024 * 1024) - Hard coded Total Flash of device - The bootloader and app reserved will be deducted + * programmatically + * + * BOARD_FIRST_FLASH_SECTOR_TO_ERASE 2 - Optional sectors index in the flash_sectors table (F4 only), to begin erasing. + * This is to allow sectors to be reserved for app fw usage. That will NOT be erased + * during a FW upgrade. + * The default is 0, and selects the first sector to be erased, as the 0th entry in the + * flash_sectors table. Which is the second physical sector of FLASH in the device. + * The first physical sector of FLASH is used by the bootloader, and is not defined + * in the table. + * + * APP_RESERVATION_SIZE (BOARD_FIRST_FLASH_SECTOR_TO_ERASE * 16 * 1024) - Number of bytes reserved by the APP FW. This number plus + * BOOTLOADER_RESERVATION_SIZE will be deducted from + * BOARD_FLASH_SIZE to determine the size of the App FW + * and hence the address space of FLASH to erase and program. + * USBMFGSTRING "PX4 AP" - Optional USB MFG string (default is '3D Robotics' if not defined.) + * SERIAL_BREAK_DETECT_DISABLED - Optional prevent break selection on Serial port from entering or staying in BL + * + * * Other defines are somewhat self explanatory. + */ + +/* Boot device selection list*/ +#define USB0_DEV 0x01 +#define SERIAL0_DEV 0x02 +#define SERIAL1_DEV 0x04 + +#define APP_LOAD_ADDRESS 0x08020000 +#define BOOTLOADER_DELAY 5000 +#define INTERFACE_USB 1 +#define INTERFACE_USB_CONFIG "/dev/ttyACM0" +#define BOARD_VBUS MK_GPIO_INPUT(GPIO_OTGFS_VBUS) + +//#define USE_VBUS_PULL_DOWN +#define INTERFACE_USART 1 +#define INTERFACE_USART_CONFIG "/dev/ttyS0,115200" +#define BOOT_DELAY_ADDRESS 0x000001a0 +#define BOARD_TYPE 1124 +#define _FLASH_KBYTES (*(uint32_t *)0x1FF1E880) +#define BOARD_FLASH_SECTORS (15) +#define BOARD_FLASH_SIZE (_FLASH_KBYTES * 1024) + +#define OSC_FREQ 24 + +#define BOARD_PIN_LED_ACTIVITY GPIO_nLED_RED +#define BOARD_PIN_LED_BOOTLOADER GPIO_nLED_RED +#define BOARD_LED_ON 0 +#define BOARD_LED_OFF 1 + +#define SERIAL_BREAK_DETECT_DISABLED 1 + +#if !defined(ARCH_SN_MAX_LENGTH) +# define ARCH_SN_MAX_LENGTH 12 +#endif + +#if !defined(APP_RESERVATION_SIZE) +# define APP_RESERVATION_SIZE 0 +#endif + +#if !defined(BOARD_FIRST_FLASH_SECTOR_TO_ERASE) +# define BOARD_FIRST_FLASH_SECTOR_TO_ERASE 1 +#endif + +#if !defined(USB_DATA_ALIGN) +# define USB_DATA_ALIGN +#endif + +#ifndef BOOT_DEVICES_SELECTION +# define BOOT_DEVICES_SELECTION USB0_DEV|SERIAL0_DEV|SERIAL1_DEV +#endif + +#ifndef BOOT_DEVICES_FILTER_ONUSB +# define BOOT_DEVICES_FILTER_ONUSB USB0_DEV|SERIAL0_DEV|SERIAL1_DEV +#endif diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/src/i2c.cpp b/boards/3dr/ctrl-zero-h7-oem-revg/src/i2c.cpp new file mode 100755 index 000000000000..1b8927c69939 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/i2c.cpp @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * Copyright (C) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(1), + initI2CBusExternal(3), + initI2CBusExternal(4), +}; diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/src/init.c b/boards/3dr/ctrl-zero-h7-oem-revg/src/init.c new file mode 100755 index 000000000000..daaace48fb2f --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/init.c @@ -0,0 +1,204 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file init.c + * + * board-specific early startup code. This file implements the + * board_app_initialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialisation. + */ + +#include "board_config.h" + +#include + +#include +#include +#include +#include +#include +#include "arm_internal.h" + +#include +#include +#include +#include +#include +#include +#include + +#include + +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +__END_DECLS + +/************************************************************************************ + * Name: board_peripheral_reset + * + * Description: + * + ************************************************************************************/ +__EXPORT void board_peripheral_reset(int ms) +{ + bool last = READ_VDD_3V3_SPEKTRUM_POWER_EN(); + /* Keep Spektum on to discharge rail*/ + VDD_3V3_SPEKTRUM_POWER_EN(false); + + /* wait for the peripheral rail to reach GND */ + usleep(ms * 1000); + syslog(LOG_DEBUG, "reset done, %d ms\n", ms); + + /* re-enable power */ + + /* switch the peripheral rail back on */ + VDD_3V3_SPEKTRUM_POWER_EN(last); +} + +/************************************************************************************ + * Name: board_on_reset + * + * Description: + * Optionally provided function called on entry to board_system_reset + * It should perform any house keeping prior to the rest. + * + * status - 1 if resetting to boot loader + * 0 if just resetting + * + ************************************************************************************/ +__EXPORT void board_on_reset(int status) +{ + for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) { + px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); + } + + if (status >= 0) { + up_mdelay(6); + } +} + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the initialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ +__EXPORT void stm32_boardinitialize(void) +{ + /* Reset PWM first thing */ + board_on_reset(-1); + + /* configure pins */ + const uint32_t gpio[] = PX4_GPIO_INIT_LIST; + px4_gpio_init(gpio, arraySize(gpio)); + board_control_spi_sensors_power_configgpio(); + + /* configure LEDs */ + board_autoled_initialize(); +} + +/**************************************************************************** + * Name: board_app_initialize + * + * Description: + * Perform application specific initialization. This function is never + * called directly from application code, but only indirectly via the + * (non-standard) boardctl() interface using the command BOARDIOC_INIT. + * + * Input Parameters: + * arg - The boardctl() argument is passed to the board_app_initialize() + * implementation without modification. The argument has no + * meaning to NuttX; + * + * Returned Value: + * Zero (OK) is returned on success; a negated errno value is returned on + * any failure to indicate the nature of the failure. + * + ****************************************************************************/ +__EXPORT int board_app_initialize(uintptr_t arg) +{ + /* Power on Interfaces */ + board_control_spi_sensors_power(true, 0xffff); + VDD_3V3_SPEKTRUM_POWER_EN(true); + + px4_platform_init(); + + stm32_spiinitialize(); + + /* configure the DMA allocator */ + if (board_dma_alloc_init() < 0) { + syslog(LOG_ERR, "[boot] DMA alloc FAILED\n"); + } + + + /* initial LED state */ + drv_led_start(); + led_off(LED_RED); + led_on(LED_GREEN); // Indicate Power. + led_off(LED_BLUE); + + if (board_hardfault_init(2, true) != 0) { + led_on(LED_RED); + } + +#ifdef CONFIG_MMCSD + /* Mount the SDIO-based MMC/SD block driver */ + /* First, get an instance of the SDIO interface */ + struct sdio_dev_s *sdio_dev = sdio_initialize(0); // SDIO_SLOTNO 0 Only one slot + + if (!sdio_dev) { + syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n", 0); + } + + if (mmcsd_slotinitialize(0, sdio_dev) != OK) { + syslog(LOG_ERR, "[boot] Failed to bind SDIO to the MMC/SD driver\n"); + } + + /* Assume that the SD card is inserted. What choice do we have? */ + sdio_mediachange(sdio_dev, true); +#endif /* CONFIG_MMCSD */ + + /* Configure the HW based on the manifest */ + + px4_platform_configure(); + + return OK; +} diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/src/led.c b/boards/3dr/ctrl-zero-h7-oem-revg/src/led.c new file mode 100755 index 000000000000..0e7beb3a9e3b --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/led.c @@ -0,0 +1,111 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file led.c + * + * LED backend. + */ + +#include + +#include + +#include "chip.h" +#include "stm32_gpio.h" +#include "board_config.h" + +#include +#include + +/* + * Ideally we'd be able to get these from arm_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +extern void led_toggle(int led); +__END_DECLS + +static uint32_t g_ledmap[] = { + GPIO_nLED_BLUE, // Indexed by LED_BLUE + GPIO_nLED_RED, // Indexed by LED_RED + GPIO_nLED_GREEN, // Indexed by LED_GREEN +}; + +__EXPORT void led_init(void) +{ + for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); l++) { + if (g_ledmap[l] != 0) { + stm32_configgpio(g_ledmap[l]); + } + } +} + +static void phy_set_led(int led, bool state) +{ + /* Drive Low to switch on */ + if (g_ledmap[led] != 0) { + stm32_gpiowrite(g_ledmap[led], !state); + } +} + +static bool phy_get_led(int led) +{ + /* If Low it is on */ + if (g_ledmap[led] != 0) { + return !stm32_gpioread(g_ledmap[led]); + } + + return false; +} + +__EXPORT void led_on(int led) +{ + phy_set_led(led, true); +} + +__EXPORT void led_off(int led) +{ + phy_set_led(led, false); +} + +__EXPORT void led_toggle(int led) +{ + phy_set_led(led, !phy_get_led(led)); +} diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/src/spi.cpp b/boards/3dr/ctrl-zero-h7-oem-revg/src/spi.cpp new file mode 100755 index 000000000000..4a4c3502bbd9 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/spi.cpp @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * Copyright (C) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM20948, SPI::CS{GPIO::PortE, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin12}), + }, {GPIO::PortE, GPIO::Pin3}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin10}), + initSPIDevice(DRV_BARO_DEVTYPE_DPS310, SPI::CS{GPIO::PortD, GPIO::Pin7}), + }), + initSPIBus(SPI::Bus::SPI5, { + initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin10}, SPI::DRDY{GPIO::PortF, GPIO::Pin3}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin6}, SPI::DRDY{GPIO::PortF, GPIO::Pin1}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/src/timer_config.cpp b/boards/3dr/ctrl-zero-h7-oem-revg/src/timer_config.cpp new file mode 100755 index 000000000000..e23880491444 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/timer_config.cpp @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * Copyright (C) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { + initIOTimer(Timer::Timer1, DMA{DMA::Index1}), + initIOTimer(Timer::Timer4, DMA{DMA::Index1}), + initIOTimer(Timer::Timer8, DMA{DMA::Index1}), +}; + +constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel4}, {GPIO::PortE, GPIO::Pin14}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel3}, {GPIO::PortE, GPIO::Pin13}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel2}, {GPIO::PortE, GPIO::Pin11}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel1}, {GPIO::PortE, GPIO::Pin9}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel2}, {GPIO::PortD, GPIO::Pin13}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortD, GPIO::Pin14}), + initIOTimerChannel(io_timers, {Timer::Timer8, Timer::Channel1}, {GPIO::PortI, GPIO::Pin5}), + initIOTimerChannel(io_timers, {Timer::Timer8, Timer::Channel2}, {GPIO::PortI, GPIO::Pin6}), +}; + +constexpr io_timers_channel_mapping_t io_timers_channel_mapping = + initIOTimerChannelMapping(io_timers, timer_io_channels); diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/src/usb.c b/boards/3dr/ctrl-zero-h7-oem-revg/src/usb.c new file mode 100755 index 000000000000..fe32ce71ac63 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/usb.c @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * Copyright (C) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file usb.c + * + * Board-specific USB functions. + */ + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include "board_config.h" + +/************************************************************************************ + * Name: stm32_usbsuspend + * + * Description: + * Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is + * used. This function is called whenever the USB enters or leaves suspend mode. + * This is an opportunity for the board logic to shutdown clocks, power, etc. + * while the USB is suspended. + * + ************************************************************************************/ +__EXPORT void stm32_usbinitialize(void) +{ + /* The OTG FS has an internal soft pull-up */ + + /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ + +#ifdef CONFIG_STM32H7_OTGFS + stm32_configgpio(GPIO_OTGFS_VBUS); +#endif +} + + + +__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + uinfo("resume: %d\n", resume); +} diff --git a/boards/cubepilot/cubeorange/nuttx-config/include/board.h b/boards/cubepilot/cubeorange/nuttx-config/include/board.h index 5827b1bc81d1..62605f98d13a 100644 --- a/boards/cubepilot/cubeorange/nuttx-config/include/board.h +++ b/boards/cubepilot/cubeorange/nuttx-config/include/board.h @@ -194,6 +194,12 @@ #define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */ +/* UART clock selection */ +/* reset to default to overwrite any changes done by any bootloader */ + +#define STM32_RCC_D2CCIP2R_USART234578_SEL RCC_D2CCIP2R_USART234578SEL_RCC +#define STM32_RCC_D2CCIP2R_USART16_SEL RCC_D2CCIP2R_USART16SEL_RCC + /* FLASH wait states */ #define BOARD_FLASH_WAITSTATES 2 diff --git a/boards/cubepilot/cubeorangeplus/nuttx-config/include/board.h b/boards/cubepilot/cubeorangeplus/nuttx-config/include/board.h index d650c6169118..babaf587e450 100644 --- a/boards/cubepilot/cubeorangeplus/nuttx-config/include/board.h +++ b/boards/cubepilot/cubeorangeplus/nuttx-config/include/board.h @@ -195,6 +195,12 @@ #define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */ +/* UART clock selection */ +/* reset to default to overwrite any changes done by any bootloader */ + +#define STM32_RCC_D2CCIP2R_USART234578_SEL RCC_D2CCIP2R_USART234578SEL_RCC +#define STM32_RCC_D2CCIP2R_USART16_SEL RCC_D2CCIP2R_USART16SEL_RCC + /* FLASH wait states */ #define BOARD_FLASH_WAITSTATES 2 diff --git a/boards/mro/ctrl-zero-classic/nuttx-config/include/board.h b/boards/mro/ctrl-zero-classic/nuttx-config/include/board.h index 52206cccd00f..ea2147442699 100644 --- a/boards/mro/ctrl-zero-classic/nuttx-config/include/board.h +++ b/boards/mro/ctrl-zero-classic/nuttx-config/include/board.h @@ -197,6 +197,12 @@ #define STM32_FDCANCLK STM32_HSE_FREQUENCY +/* UART clock selection */ +/* reset to default to overwrite any changes done by any bootloader */ + +#define STM32_RCC_D2CCIP2R_USART234578_SEL RCC_D2CCIP2R_USART234578SEL_RCC +#define STM32_RCC_D2CCIP2R_USART16_SEL RCC_D2CCIP2R_USART16SEL_RCC + /* FLASH wait states */ #define BOARD_FLASH_WAITSTATES 2 diff --git a/boards/mro/ctrl-zero-h7-oem/nuttx-config/include/board.h b/boards/mro/ctrl-zero-h7-oem/nuttx-config/include/board.h index d5ffa2c5848e..14a6a28d2b88 100644 --- a/boards/mro/ctrl-zero-h7-oem/nuttx-config/include/board.h +++ b/boards/mro/ctrl-zero-h7-oem/nuttx-config/include/board.h @@ -197,6 +197,12 @@ #define STM32_FDCANCLK STM32_HSE_FREQUENCY +/* UART clock selection */ +/* reset to default to overwrite any changes done by any bootloader */ + +#define STM32_RCC_D2CCIP2R_USART234578_SEL RCC_D2CCIP2R_USART234578SEL_RCC +#define STM32_RCC_D2CCIP2R_USART16_SEL RCC_D2CCIP2R_USART16SEL_RCC + /* FLASH wait states */ #define BOARD_FLASH_WAITSTATES 2 diff --git a/boards/mro/ctrl-zero-h7/nuttx-config/include/board.h b/boards/mro/ctrl-zero-h7/nuttx-config/include/board.h index cdbd3dd3e629..4a20575d70ae 100644 --- a/boards/mro/ctrl-zero-h7/nuttx-config/include/board.h +++ b/boards/mro/ctrl-zero-h7/nuttx-config/include/board.h @@ -196,6 +196,12 @@ #define STM32_FDCANCLK STM32_HSE_FREQUENCY +/* UART clock selection */ +/* reset to default to overwrite any changes done by any bootloader */ + +#define STM32_RCC_D2CCIP2R_USART234578_SEL RCC_D2CCIP2R_USART234578SEL_RCC +#define STM32_RCC_D2CCIP2R_USART16_SEL RCC_D2CCIP2R_USART16SEL_RCC + /* FLASH wait states */ #define BOARD_FLASH_WAITSTATES 2 diff --git a/boards/mro/pixracerpro/nuttx-config/include/board.h b/boards/mro/pixracerpro/nuttx-config/include/board.h index 5edda931dc10..96b2ed2022fb 100644 --- a/boards/mro/pixracerpro/nuttx-config/include/board.h +++ b/boards/mro/pixracerpro/nuttx-config/include/board.h @@ -196,6 +196,12 @@ #define STM32_FDCANCLK STM32_HSE_FREQUENCY +/* UART clock selection */ +/* reset to default to overwrite any changes done by any bootloader */ + +#define STM32_RCC_D2CCIP2R_USART234578_SEL RCC_D2CCIP2R_USART234578SEL_RCC +#define STM32_RCC_D2CCIP2R_USART16_SEL RCC_D2CCIP2R_USART16SEL_RCC + /* FLASH wait states */ #define BOARD_FLASH_WAITSTATES 2 diff --git a/boards/px4/fmu-v5/rover.px4board b/boards/px4/fmu-v5/rover.px4board index 92c265280ab9..07bfbe094121 100644 --- a/boards/px4/fmu-v5/rover.px4board +++ b/boards/px4/fmu-v5/rover.px4board @@ -17,5 +17,6 @@ CONFIG_MODULES_UUV_POS_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set -CONFIG_MODULES_DIFFERENTIAL_DRIVE=y +CONFIG_MODULES_ROVER_DIFFERENTIAL=y CONFIG_MODULES_ROVER_ACKERMANN=y +CONFIG_DRIVERS_ROBOCLAW=y diff --git a/boards/px4/fmu-v5x/default.px4board b/boards/px4/fmu-v5x/default.px4board index 46d04d8cc2eb..a48b0956876b 100644 --- a/boards/px4/fmu-v5x/default.px4board +++ b/boards/px4/fmu-v5x/default.px4board @@ -54,7 +54,6 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y diff --git a/boards/px4/fmu-v5x/rover.px4board b/boards/px4/fmu-v5x/rover.px4board index 738e6d9b0d0a..4a4458c2e6bc 100644 --- a/boards/px4/fmu-v5x/rover.px4board +++ b/boards/px4/fmu-v5x/rover.px4board @@ -14,4 +14,6 @@ CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set +CONFIG_MODULES_ROVER_DIFFERENTIAL=y CONFIG_MODULES_ROVER_ACKERMANN=y +CONFIG_DRIVERS_ROBOCLAW=y diff --git a/boards/px4/fmu-v6c/nuttx-config/include/board.h b/boards/px4/fmu-v6c/nuttx-config/include/board.h index 1951031cb825..4b4b128cf2ba 100644 --- a/boards/px4/fmu-v6c/nuttx-config/include/board.h +++ b/boards/px4/fmu-v6c/nuttx-config/include/board.h @@ -246,6 +246,12 @@ #define STM32_RCC_D2CCIP2R_USBSRC RCC_D2CCIP2R_USBSEL_PLL3 +/* UART clock selection */ +/* reset to default to overwrite any changes done by any bootloader */ + +#define STM32_RCC_D2CCIP2R_USART234578_SEL RCC_D2CCIP2R_USART234578SEL_RCC +#define STM32_RCC_D2CCIP2R_USART16_SEL RCC_D2CCIP2R_USART16SEL_RCC + /* ADC 1 2 3 clock source */ #define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 diff --git a/boards/px4/fmu-v6c/rover.px4board b/boards/px4/fmu-v6c/rover.px4board index 553bc76a52c7..7ded4008d8af 100644 --- a/boards/px4/fmu-v6c/rover.px4board +++ b/boards/px4/fmu-v6c/rover.px4board @@ -13,5 +13,6 @@ CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set -CONFIG_MODULES_DIFFERENTIAL_DRIVE=y +CONFIG_MODULES_ROVER_DIFFERENTIAL=y CONFIG_MODULES_ROVER_ACKERMANN=y +CONFIG_DRIVERS_ROBOCLAW=y diff --git a/boards/px4/fmu-v6u/rover.px4board b/boards/px4/fmu-v6u/rover.px4board index 553bc76a52c7..7ded4008d8af 100644 --- a/boards/px4/fmu-v6u/rover.px4board +++ b/boards/px4/fmu-v6u/rover.px4board @@ -13,5 +13,6 @@ CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set -CONFIG_MODULES_DIFFERENTIAL_DRIVE=y +CONFIG_MODULES_ROVER_DIFFERENTIAL=y CONFIG_MODULES_ROVER_ACKERMANN=y +CONFIG_DRIVERS_ROBOCLAW=y diff --git a/boards/px4/fmu-v6x/default.px4board b/boards/px4/fmu-v6x/default.px4board index 7b9488933fb4..7c4bf0b6f6cb 100644 --- a/boards/px4/fmu-v6x/default.px4board +++ b/boards/px4/fmu-v6x/default.px4board @@ -18,6 +18,7 @@ CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPIO_MCP23009=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_HEATER=y CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16470=y diff --git a/boards/px4/fmu-v6x/init/rc.board_defaults b/boards/px4/fmu-v6x/init/rc.board_defaults index cb1eb580a429..bdc99a68e7bd 100644 --- a/boards/px4/fmu-v6x/init/rc.board_defaults +++ b/boards/px4/fmu-v6x/init/rc.board_defaults @@ -27,3 +27,14 @@ else fi safety_button start + +# GPIO Expander driver on external I2C3 +if ver hwbasecmp 009 +then + # No USB + mcp23009 start -b 3 -X -D 0xf0 -O 0xf0 -P 0x0f -U 10 +fi +if ver hwbasecmp 00a 008 +then + mcp23009 start -b 3 -X -D 0xf1 -O 0xf0 -P 0x0f -U 10 +fi diff --git a/boards/px4/fmu-v6x/nuttx-config/include/board.h b/boards/px4/fmu-v6x/nuttx-config/include/board.h index d6b2925b795c..7907eafad194 100644 --- a/boards/px4/fmu-v6x/nuttx-config/include/board.h +++ b/boards/px4/fmu-v6x/nuttx-config/include/board.h @@ -250,6 +250,12 @@ #define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 +/* UART clock selection */ +/* reset to default to overwrite any changes done by any bootloader */ + +#define STM32_RCC_D2CCIP2R_USART234578_SEL RCC_D2CCIP2R_USART234578SEL_RCC +#define STM32_RCC_D2CCIP2R_USART16_SEL RCC_D2CCIP2R_USART16SEL_RCC + /* FDCAN 1 2 clock source */ #define STM32_RCC_D2CCIP1R_FDCANSEL RCC_D2CCIP1R_FDCANSEL_HSE /* FDCAN 1 2 clock source */ diff --git a/boards/px4/fmu-v6x/nuttx-config/nsh/defconfig b/boards/px4/fmu-v6x/nuttx-config/nsh/defconfig index 333609282e00..f0a8d6ae8653 100644 --- a/boards/px4/fmu-v6x/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v6x/nuttx-config/nsh/defconfig @@ -91,8 +91,10 @@ CONFIG_DEBUG_SYMBOLS=y CONFIG_DEBUG_TCBINFO=y CONFIG_DEFAULT_SMALL=y CONFIG_DEV_FIFO_SIZE=0 +CONFIG_DEV_GPIO=y CONFIG_DEV_PIPE_MAXSIZE=1024 CONFIG_DEV_PIPE_SIZE=70 +CONFIG_DEV_URANDOM=y CONFIG_ETH0_PHY_LAN8742A=y CONFIG_EXPERIMENTAL=y CONFIG_FAT_DMAMEMORY=y diff --git a/boards/px4/fmu-v6x/rover.px4board b/boards/px4/fmu-v6x/rover.px4board index 553bc76a52c7..7ded4008d8af 100644 --- a/boards/px4/fmu-v6x/rover.px4board +++ b/boards/px4/fmu-v6x/rover.px4board @@ -13,5 +13,6 @@ CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set -CONFIG_MODULES_DIFFERENTIAL_DRIVE=y +CONFIG_MODULES_ROVER_DIFFERENTIAL=y CONFIG_MODULES_ROVER_ACKERMANN=y +CONFIG_DRIVERS_ROBOCLAW=y diff --git a/boards/px4/fmu-v6x/src/CMakeLists.txt b/boards/px4/fmu-v6x/src/CMakeLists.txt index a120ebe33623..39ec808e1e9a 100644 --- a/boards/px4/fmu-v6x/src/CMakeLists.txt +++ b/boards/px4/fmu-v6x/src/CMakeLists.txt @@ -34,7 +34,7 @@ if("${PX4_BOARD_LABEL}" STREQUAL "bootloader") add_compile_definitions(BOOTLOADER) add_library(drivers_board bootloader_main.c - init.c + init.cpp usb.c timer_config.cpp ) @@ -52,7 +52,7 @@ else() add_library(drivers_board can.c i2c.cpp - init.c + init.cpp led.c mtd.cpp sdio.c @@ -71,5 +71,6 @@ else() nuttx_arch # sdio nuttx_drivers # sdio px4_layer + platform_gpio_mcp23009 ) endif() diff --git a/boards/px4/fmu-v6x/src/board_config.h b/boards/px4/fmu-v6x/src/board_config.h index 7cad5497ed2b..7c98f9490207 100644 --- a/boards/px4/fmu-v6x/src/board_config.h +++ b/boards/px4/fmu-v6x/src/board_config.h @@ -270,6 +270,11 @@ #define GPIO_VDD_3V3_SPEKTRUM_POWER_EN /* PH2 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTH|GPIO_PIN2) #define GPIO_VDD_3V3_SD_CARD_EN /* PC13 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) +/* MCP23009 GPIO expander */ +#define BOARD_GPIO_VDD_5V_COMP_VALID "/dev/gpio4" +#define BOARD_GPIO_VDD_5V_CAN1_GPS1_VALID "/dev/gpio5" + + /* Spare GPIO */ #define GPIO_PG6 /* PG6 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN6) diff --git a/boards/px4/fmu-v6x/src/init.c b/boards/px4/fmu-v6x/src/init.cpp similarity index 98% rename from boards/px4/fmu-v6x/src/init.c rename to boards/px4/fmu-v6x/src/init.cpp index cd6714af1664..8901bd1766f3 100644 --- a/boards/px4/fmu-v6x/src/init.c +++ b/boards/px4/fmu-v6x/src/init.cpp @@ -74,6 +74,7 @@ #include #include #include +#include /**************************************************************************** * Pre-Processor Definitions @@ -159,7 +160,7 @@ __EXPORT void board_on_reset(int status) * ************************************************************************************/ -__EXPORT void +extern "C" __EXPORT void stm32_boardinitialize(void) { board_on_reset(-1); /* Reset PWM first thing */ @@ -280,6 +281,13 @@ __EXPORT int board_app_initialize(uintptr_t arg) # endif /* CONFIG_MMCSD */ + ret = mcp23009_register_gpios(3, 0x25); + + if (ret != OK) { + led_on(LED_RED); + return ret; + } + #endif /* !defined(BOOTLOADER) */ return OK; diff --git a/boards/px4/fmu-v6xrt/rover.px4board b/boards/px4/fmu-v6xrt/rover.px4board index 101555842444..95a01552267c 100644 --- a/boards/px4/fmu-v6xrt/rover.px4board +++ b/boards/px4/fmu-v6xrt/rover.px4board @@ -14,5 +14,6 @@ CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set -CONFIG_MODULES_DIFFERENTIAL_DRIVE=y +CONFIG_MODULES_ROVER_DIFFERENTIAL=y CONFIG_MODULES_ROVER_ACKERMANN=y +CONFIG_DRIVERS_ROBOCLAW=y diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index d46a7cc7e70f..ac228113d5d8 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -1,7 +1,7 @@ CONFIG_PLATFORM_POSIX=y CONFIG_BOARD_TESTING=y CONFIG_BOARD_ETHERNET=y -CONFIG_BOARD_ROOT_PATH="./" +CONFIG_BOARD_ROOT_PATH="." CONFIG_DRIVERS_CAMERA_TRIGGER=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_OSD_MSP_OSD=y @@ -13,7 +13,6 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE=y CONFIG_MODULES_EKF2=y CONFIG_EKF2_VERBOSE_STATUS=y CONFIG_EKF2_AUX_GLOBAL_POSITION=y @@ -47,6 +46,7 @@ CONFIG_MODULES_PAYLOAD_DELIVERER=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_REPLAY=y CONFIG_MODULES_ROVER_ACKERMANN=y +CONFIG_MODULES_ROVER_DIFFERENTIAL=y CONFIG_MODULES_ROVER_POS_CONTROL=y CONFIG_MODULES_SENSORS=y CONFIG_COMMON_SIMULATION=y diff --git a/cmake/metadata.cmake b/cmake/metadata.cmake index 59ae6bd82990..ef946c703159 100644 --- a/cmake/metadata.cmake +++ b/cmake/metadata.cmake @@ -74,13 +74,13 @@ add_custom_target(metadata_parameters --markdown ${PX4_BINARY_DIR}/docs/parameters.md COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/src/lib/parameters/px_process_params.py - --src-path `find ${PX4_SOURCE_DIR}/src -maxdepth 4 -type d` + --src-path `find ${PX4_SOURCE_DIR}/src -maxdepth 4 -type d` ${generated_params_dir} --inject-xml ${PX4_SOURCE_DIR}/src/lib/parameters/parameters_injected.xml --json ${PX4_BINARY_DIR}/docs/parameters.json --compress COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/src/lib/parameters/px_process_params.py - --src-path `find ${PX4_SOURCE_DIR}/src -maxdepth 4 -type d` + --src-path `find ${PX4_SOURCE_DIR}/src -maxdepth 4 -type d` ${generated_params_dir} --inject-xml ${PX4_SOURCE_DIR}/src/lib/parameters/parameters_injected.xml --xml ${PX4_BINARY_DIR}/docs/parameters.xml diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 65ec54d8b01e..3b31b7de554d 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -71,7 +71,6 @@ set(msg_files DebugKeyValue.msg DebugValue.msg DebugVect.msg - DifferentialDriveSetpoint.msg DifferentialPressure.msg DistanceSensor.msg Ekf2Timestamps.msg @@ -147,6 +146,7 @@ set(msg_files MountOrientation.msg ModeCompleted.msg NavigatorMissionItem.msg + NavigatorStatus.msg NormalizedUnsignedSetpoint.msg NpfgStatus.msg ObstacleDistance.msg @@ -181,6 +181,8 @@ set(msg_files RegisterExtComponentRequest.msg RoverAckermannGuidanceStatus.msg RoverAckermannStatus.msg + RoverDifferentialGuidanceStatus.msg + RoverDifferentialStatus.msg Rpm.msg RtlStatus.msg RtlTimeEstimate.msg diff --git a/msg/DifferentialDriveSetpoint.msg b/msg/DifferentialDriveSetpoint.msg deleted file mode 100644 index f7e4c5840099..000000000000 --- a/msg/DifferentialDriveSetpoint.msg +++ /dev/null @@ -1,8 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 speed # [m/s] collective roll-off speed in body x-axis -bool closed_loop_speed_control # true if speed is controlled using estimator feedback, false if direct feed-forward -float32 yaw_rate # [rad/s] yaw rate -bool closed_loop_yaw_rate_control # true if yaw rate is controlled using gyroscope feedback, false if direct feed-forward - -# TOPICS differential_drive_setpoint differential_drive_control_output diff --git a/msg/FailsafeFlags.msg b/msg/FailsafeFlags.msg index 2cd31bf83598..de514fb2db4b 100644 --- a/msg/FailsafeFlags.msg +++ b/msg/FailsafeFlags.msg @@ -49,6 +49,7 @@ bool vtol_fixed_wing_system_failure # vehicle in fixed-wing system failure fai bool wind_limit_exceeded # Wind limit exceeded bool flight_time_limit_exceeded # Maximum flight time exceeded bool local_position_accuracy_low # Local position estimate has dropped below threshold, but is currently still declared valid +bool navigator_failure # Navigator failed to execute a mode # Failure detector bool fd_critical_failure # Critical failure (attitude/altitude limit exceeded, or external ATS) diff --git a/msg/NavigatorMissionItem.msg b/msg/NavigatorMissionItem.msg index 64af762f75d8..a10aa4656d51 100644 --- a/msg/NavigatorMissionItem.msg +++ b/msg/NavigatorMissionItem.msg @@ -1,7 +1,5 @@ uint64 timestamp # time since system start (microseconds) -uint32 instance_count # Instance count of this mission. Increments monotonically whenever the mission is modified - uint16 sequence_current # Sequence of the current mission item uint16 nav_cmd diff --git a/msg/NavigatorStatus.msg b/msg/NavigatorStatus.msg new file mode 100644 index 000000000000..da666533574f --- /dev/null +++ b/msg/NavigatorStatus.msg @@ -0,0 +1,9 @@ +# Current status of a Navigator mode +# The possible values of nav_state are defined in the VehicleStatus msg. +uint64 timestamp # time since system start (microseconds) + +uint8 nav_state # Source mode (values in VehicleStatus) +uint8 failure # Navigator failure enum + +uint8 FAILURE_NONE = 0 +uint8 FAILURE_HAGL = 1 # Target altitude exceeds maximum height above ground diff --git a/msg/OrbitStatus.msg b/msg/OrbitStatus.msg index a04265db46c8..531fa4145306 100644 --- a/msg/OrbitStatus.msg +++ b/msg/OrbitStatus.msg @@ -4,6 +4,7 @@ uint8 ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING = 1 uint8 ORBIT_YAW_BEHAVIOUR_UNCONTROLLED = 2 uint8 ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE = 3 uint8 ORBIT_YAW_BEHAVIOUR_RC_CONTROLLED = 4 +uint8 ORBIT_YAW_BEHAVIOUR_UNCHANGED = 5 uint64 timestamp # time since system start (microseconds) float32 radius # Radius of the orbit circle. Positive values orbit clockwise, negative values orbit counter-clockwise. [m] diff --git a/msg/RoverDifferentialGuidanceStatus.msg b/msg/RoverDifferentialGuidanceStatus.msg new file mode 100644 index 000000000000..836546e7ebb7 --- /dev/null +++ b/msg/RoverDifferentialGuidanceStatus.msg @@ -0,0 +1,10 @@ +uint64 timestamp # time since system start (microseconds) + +float32 desired_speed # [m/s] Desired forward speed for the rover +float32 lookahead_distance # [m] Lookahead distance of pure the pursuit controller +float32 heading_error_deg # [deg] Heading error of the pure pursuit controller +float32 pid_heading_integral # Integral of the PID for the desired yaw rate during missions +float32 pid_throttle_integral # Integral of the PID for the throttle during missions +uint8 state_machine # Driving state of the rover [0: SPOT_TURNING, 1: DRIVING, 2: GOAL_REACHED] + +# TOPICS rover_differential_guidance_status diff --git a/msg/RoverDifferentialStatus.msg b/msg/RoverDifferentialStatus.msg new file mode 100644 index 000000000000..31907ffa6477 --- /dev/null +++ b/msg/RoverDifferentialStatus.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) + +float32 actual_speed # [m/s] Actual forward speed of the rover +float32 desired_yaw_rate_deg_s # [deg/s] Desired yaw rate +float32 actual_yaw_rate_deg_s # [deg/s] Actual yaw rate of the rover +float32 pid_yaw_rate_integral # Integral of the PID for the desired yaw rate controller + +# TOPICS rover_differential_status diff --git a/msg/VehicleAttitudeSetpoint.msg b/msg/VehicleAttitudeSetpoint.msg index f52025d2bb51..74a753023df5 100644 --- a/msg/VehicleAttitudeSetpoint.msg +++ b/msg/VehicleAttitudeSetpoint.msg @@ -1,9 +1,5 @@ uint64 timestamp # time since system start (microseconds) -float32 roll_body # body angle in NED frame (can be NaN for FW) -float32 pitch_body # body angle in NED frame (can be NaN for FW) -float32 yaw_body # body angle in NED frame (can be NaN for FW) - float32 yaw_sp_move_rate # rad/s (commanded by user) # For quaternion-based attitude control diff --git a/msg/VehicleCommand.msg b/msg/VehicleCommand.msg index 0c1ce85d5fa8..8df0ca56b0ac 100644 --- a/msg/VehicleCommand.msg +++ b/msg/VehicleCommand.msg @@ -158,6 +158,14 @@ uint8 SPEED_TYPE_GROUNDSPEED = 1 uint8 SPEED_TYPE_CLIMB_SPEED = 2 uint8 SPEED_TYPE_DESCEND_SPEED = 3 +# used as param3 in CMD_DO_ORBIT +uint8 ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER = 0 +uint8 ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING = 1 +uint8 ORBIT_YAW_BEHAVIOUR_UNCONTROLLED = 2 +uint8 ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE = 3 +uint8 ORBIT_YAW_BEHAVIOUR_RC_CONTROLLED = 4 +uint8 ORBIT_YAW_BEHAVIOUR_UNCHANGED = 5 + # used as param1 in ARM_DISARM command int8 ARMING_ACTION_DISARM = 0 int8 ARMING_ACTION_ARM = 1 diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp index 761041fa0e83..68ac3f1ffea9 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp @@ -89,7 +89,7 @@ static constexpr wq_config_t ttyS9{"wq:ttyS9", 1728, -30}; static constexpr wq_config_t ttyACM0{"wq:ttyACM0", 1728, -31}; static constexpr wq_config_t ttyUnknown{"wq:ttyUnknown", 1728, -32}; -static constexpr wq_config_t lp_default{"wq:lp_default", 2000, -50}; +static constexpr wq_config_t lp_default{"wq:lp_default", 2350, -50}; static constexpr wq_config_t test1{"wq:test1", 2000, 0}; static constexpr wq_config_t test2{"wq:test2", 2000, 0}; diff --git a/platforms/nuttx/NuttX/nuttx b/platforms/nuttx/NuttX/nuttx index d3b85f3e545d..39bb38d82b12 160000 --- a/platforms/nuttx/NuttX/nuttx +++ b/platforms/nuttx/NuttX/nuttx @@ -1 +1 @@ -Subproject commit d3b85f3e545de3692c100143f4a9ae67762ce679 +Subproject commit 39bb38d82b128a57d0ca40c25ba3ad6601ec0a83 diff --git a/platforms/nuttx/src/px4/common/SerialImpl.cpp b/platforms/nuttx/src/px4/common/SerialImpl.cpp index 7fc5ea7520a2..4264976f4471 100644 --- a/platforms/nuttx/src/px4/common/SerialImpl.cpp +++ b/platforms/nuttx/src/px4/common/SerialImpl.cpp @@ -97,12 +97,36 @@ bool SerialImpl::configure() case 460800: speed = B460800; break; +#ifndef B500000 +#define B500000 500000 +#endif + + case 500000: speed = B500000; break; + +#ifndef B576000 +#define B576000 576000 +#endif + + case 576000: speed = B576000; break; + #ifndef B921600 #define B921600 921600 #endif case 921600: speed = B921600; break; +#ifndef B1000000 +#define B1000000 1000000 +#endif + + case 1000000: speed = B1000000; break; + +#ifndef B1500000 +#define B1500000 1500000 +#endif + + case 1500000: speed = B1500000; break; + default: speed = _baudrate; PX4_WARN("Using non-standard baudrate: %lu", _baudrate); diff --git a/platforms/nuttx/src/px4/common/px4_mtd.cpp b/platforms/nuttx/src/px4/common/px4_mtd.cpp index 24d88ed3d89a..6723c35ecadb 100644 --- a/platforms/nuttx/src/px4/common/px4_mtd.cpp +++ b/platforms/nuttx/src/px4/common/px4_mtd.cpp @@ -75,11 +75,11 @@ static int ramtron_attach(mtd_instance_s &instance) return ENXIO; #else - /* start the RAMTRON driver, attempt 10 times */ + /* start the RAMTRON driver at 30MHz */ - int spi_speed_mhz = 10; + unsigned long spi_speed_hz = 30'000'000; - for (int i = 0; i < 10; i++) { + for (int i = 0; spi_speed_hz > 0; i++) { /* initialize the right spi */ struct spi_dev_s *spi = px4_spibus_initialize(px4_find_spi_bus(instance.devid)); @@ -90,7 +90,7 @@ static int ramtron_attach(mtd_instance_s &instance) /* this resets the spi bus, set correct bus speed again */ SPI_LOCK(spi, true); - SPI_SETFREQUENCY(spi, spi_speed_mhz * 1000 * 1000); + SPI_SETFREQUENCY(spi, spi_speed_hz); SPI_SETBITS(spi, 8); SPI_SETMODE(spi, SPIDEV_MODE3); SPI_SELECT(spi, instance.devid, false); @@ -108,7 +108,7 @@ static int ramtron_attach(mtd_instance_s &instance) } // try reducing speed for next attempt - spi_speed_mhz--; + spi_speed_hz -= 1'000'000; px4_usleep(10000); } @@ -118,7 +118,7 @@ static int ramtron_attach(mtd_instance_s &instance) return -EIO; } - int ret = instance.mtd_dev->ioctl(instance.mtd_dev, MTDIOC_SETSPEED, (unsigned long)spi_speed_mhz * 1000 * 1000); + int ret = instance.mtd_dev->ioctl(instance.mtd_dev, MTDIOC_SETSPEED, spi_speed_hz); if (ret != OK) { // FIXME: From the previous warning call, it looked like this should have been fatal error instead. Tried diff --git a/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c b/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c index 6d484c0a826f..1a505e98217f 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c @@ -73,7 +73,7 @@ static char hw_base_info[HW_INFO_SIZE] = {0}; /**************************************************************************** * Protected Functions ****************************************************************************/ - +#if !defined(BOARD_HAS_ONLY_EEPROM_VERSIONING) static int dn_to_ordinal(uint16_t dn) { /* Table is scaled for 12, so if ADC is in 16 bit mode @@ -111,6 +111,7 @@ static int dn_to_ordinal(uint16_t dn) return -1; } +#endif /* BOARD_HAS_ONLY_EEPROM_VERSIONING */ /************************************************************************************ * Name: read_id_dn @@ -143,7 +144,7 @@ static int dn_to_ordinal(uint16_t dn) * -EIO - FAiled to init or read the ADC * ************************************************************************************/ - +#if !defined(BOARD_HAS_ONLY_EEPROM_VERSIONING) static int read_id_dn(int *id, uint32_t gpio_drive, uint32_t gpio_sense, int adc_channel) { int rv = -EIO; @@ -328,9 +329,15 @@ static int read_id_dn(int *id, uint32_t gpio_drive, uint32_t gpio_sense, int adc stm32_configgpio(gpio_drive); return rv; } +#endif /* BOARD_HAS_ONLY_EEPROM_VERSIONING */ static int determine_hw_info(int *revision, int *version) { +#if defined(BOARD_HAS_ONLY_EEPROM_VERSIONING) + *revision = HW_ID_EEPROM; + *version = HW_ID_EEPROM; + return OK; +#else int dn; int rv = read_id_dn(&dn, GPIO_HW_REV_DRIVE, GPIO_HW_REV_SENSE, ADC_HW_REV_SENSE_CHANNEL); @@ -344,6 +351,7 @@ static int determine_hw_info(int *revision, int *version) } return rv; +#endif } /**************************************************************************** diff --git a/platforms/posix/src/px4/common/SerialImpl.cpp b/platforms/posix/src/px4/common/SerialImpl.cpp index 822ed4255ec0..2298c3829263 100644 --- a/platforms/posix/src/px4/common/SerialImpl.cpp +++ b/platforms/posix/src/px4/common/SerialImpl.cpp @@ -90,12 +90,36 @@ bool SerialImpl::configure() case 460800: speed = B460800; break; +#ifndef B500000 +#define B500000 500000 +#endif + + case 500000: speed = B500000; break; + +#ifndef B576000 +#define B576000 576000 +#endif + + case 576000: speed = B576000; break; + #ifndef B921600 #define B921600 921600 #endif case 921600: speed = B921600; break; +#ifndef B1000000 +#define B1000000 1000000 +#endif + + case 1000000: speed = B1000000; break; + +#ifndef B1500000 +#define B1500000 1500000 +#endif + + case 1500000: speed = B1500000; break; + default: speed = _baudrate; PX4_WARN("Using non-standard baudrate: %u", _baudrate); diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 378c92b6736c..53eecce4be71 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -242,6 +242,8 @@ #define DRV_INS_DEVTYPE_VN300 0xE3 #define DRV_DIFF_PRESS_DEVTYPE_ASP5033 0xE4 +#define DRV_MAG_DEVTYPE_BMM350 0xE5 + #define DRV_DEVTYPE_UNUSED 0xff #endif /* _DRV_SENSOR_H */ diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index c1dd783be029..004be4cc8b90 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -1214,11 +1214,13 @@ GPS::publish() void GPS::publishSatelliteInfo() { - if (_instance == Instance::Main) { + if (_instance == Instance::Main || _is_gps_main_advertised.load()) { if (_p_report_sat_info != nullptr) { _report_sat_info_pub.publish(*_p_report_sat_info); } + _is_gps_main_advertised.store(true); + } else { //we don't publish satellite info for the secondary gps } diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp index 8dc0f4b79512..28fce0691ce9 100644 --- a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp +++ b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp @@ -203,57 +203,45 @@ void ICM40609D::RunImpl() case STATE::FIFO_READ: { hrt_abstime timestamp_sample = now; - uint8_t samples = 0; + uint8_t samples = FIFOReadCount(); - if (_data_ready_interrupt_enabled) { - // scheduled from interrupt if _drdy_timestamp_sample was set as expected - const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); - - if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) { - timestamp_sample = drdy_timestamp_sample; - samples = _fifo_gyro_samples; + if (samples == 0) { + perf_count(_fifo_empty_perf); - } else { - perf_count(_drdy_missed_perf); + } else { + // tolerate minor jitter, leave sample to next iteration if behind by only 1 + if (samples == _fifo_gyro_samples + 1) { + timestamp_sample -= static_cast(FIFO_SAMPLE_DT); + samples--; } - // push backup schedule back - ScheduleDelayed(_fifo_empty_interval_us * 2); - } - - if (samples == 0) { - // check current FIFO count - const uint16_t fifo_count = FIFOReadCount(); - - if (fifo_count >= FIFO::SIZE) { + if (samples > FIFO_MAX_SAMPLES) { + // not technically an overflow, but more samples than we expected or can publish FIFOReset(); perf_count(_fifo_overflow_perf); + samples = 0; + } + } - } else if (fifo_count == 0) { - perf_count(_fifo_empty_perf); + bool success = false; - } else { - // FIFO count (size in bytes) - samples = (fifo_count / sizeof(FIFO::DATA)); + if (samples > 0) { + if (_data_ready_interrupt_enabled) { + // scheduled from interrupt if _drdy_timestamp_sample was set as expected + const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); - // tolerate minor jitter, leave sample to next iteration if behind by only 1 - if (samples == _fifo_gyro_samples + 1) { - timestamp_sample -= static_cast(FIFO_SAMPLE_DT); - samples--; - } + if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) { + timestamp_sample = drdy_timestamp_sample; + samples = _fifo_gyro_samples; - if (samples > FIFO_MAX_SAMPLES) { - // not technically an overflow, but more samples than we expected or can publish - FIFOReset(); - perf_count(_fifo_overflow_perf); - samples = 0; + } else { + perf_count(_drdy_missed_perf); } - } - } - bool success = false; + // push backup schedule back + ScheduleDelayed(_fifo_empty_interval_us * 2); + } - if (samples >= 1) { if (FIFORead(timestamp_sample, samples)) { success = true; @@ -374,17 +362,11 @@ void ICM40609D::ConfigureSampleRate(int sample_rate) void ICM40609D::ConfigureFIFOWatermark(uint8_t samples) { - // FIFO watermark threshold in number of bytes - const uint16_t fifo_watermark_threshold = samples * sizeof(FIFO::DATA); - for (auto &r : _register_bank0_cfg) { if (r.reg == Register::BANK_0::FIFO_CONFIG2) { // FIFO_WM[7:0] FIFO_CONFIG2 - r.set_bits = fifo_watermark_threshold & 0xFF; + r.set_bits = samples & 0xFF; - } else if (r.reg == Register::BANK_0::FIFO_CONFIG3) { - // FIFO_WM[11:8] FIFO_CONFIG3 - r.set_bits = (fifo_watermark_threshold >> 8) & 0x0F; } } } @@ -537,25 +519,10 @@ bool ICM40609D::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples) return false; } - const uint16_t fifo_count_bytes = combine(buffer.FIFO_COUNTH, buffer.FIFO_COUNTL); - - if (fifo_count_bytes >= FIFO::SIZE) { - perf_count(_fifo_overflow_perf); - FIFOReset(); - return false; - } - - const uint8_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA); - - if (fifo_count_samples == 0) { - perf_count(_fifo_empty_perf); - return false; - } - // check FIFO header in every sample uint8_t valid_samples = 0; - for (int i = 0; i < math::min(samples, fifo_count_samples); i++) { + for (int i = 0; i < samples; i++) { bool valid = true; // With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx @@ -599,6 +566,9 @@ void ICM40609D::FIFOReset() // SIGNAL_PATH_RESET: FIFO flush RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH); + // Read INT_STATUS to clear + RegisterRead(Register::BANK_0::INT_STATUS); + // reset while FIFO is disabled _drdy_timestamp_sample.store(0); } diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp index ab6fcfa1f27b..8dee6c2fb793 100644 --- a/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp +++ b/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp @@ -160,10 +160,11 @@ class ICM40609D : public device::SPI, public I2CSPIDriver int32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; uint8_t _checked_register_bank0{0}; - static constexpr uint8_t size_register_bank0_cfg{10}; + static constexpr uint8_t size_register_bank0_cfg{11}; register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] { // Register | Set bits, Clear bits { Register::BANK_0::INT_CONFIG, INT_CONFIG_BIT::INT1_MODE | INT_CONFIG_BIT::INT1_DRIVE_CIRCUIT, INT_CONFIG_BIT::INT1_POLARITY }, + { Register::BANK_0::INTF_CONFIG0, INTF_CONFIG0_BIT::FIFO_COUNT_REC | INTF_CONFIG0_BIT::UI_SIFS_CFG_DISABLE_I2C, 0}, { Register::BANK_0::FIFO_CONFIG, FIFO_CONFIG_BIT::FIFO_MODE_STOP_ON_FULL, 0 }, { Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE, 0 }, { Register::BANK_0::GYRO_CONFIG0, GYRO_CONFIG0_BIT::GYRO_ODR_8kHz, Bit7 | Bit6 | Bit5 | Bit3 | Bit2 }, diff --git a/src/drivers/imu/invensense/icm40609d/InvenSense_ICM40609D_registers.hpp b/src/drivers/imu/invensense/icm40609d/InvenSense_ICM40609D_registers.hpp index 4d359526ba92..e9880ec8cdd0 100644 --- a/src/drivers/imu/invensense/icm40609d/InvenSense_ICM40609D_registers.hpp +++ b/src/drivers/imu/invensense/icm40609d/InvenSense_ICM40609D_registers.hpp @@ -82,6 +82,8 @@ enum class BANK_0 : uint8_t { SIGNAL_PATH_RESET = 0x4B, + INTF_CONFIG0 = 0x4C, + PWR_MGMT0 = 0x4E, GYRO_CONFIG0 = 0x4F, ACCEL_CONFIG0 = 0x50, @@ -132,6 +134,16 @@ enum SIGNAL_PATH_RESET_BIT : uint8_t { FIFO_FLUSH = Bit1, }; +// INTF_CONFIG0 +enum INTF_CONFIG0_BIT : uint8_t { + FIFO_HOLD_LAST_DATA_EN = Bit7, + FIFO_COUNT_REC = Bit6, + FIFO_COUNT_ENDIAN = Bit5, + SENSOR_DATA_ENDIAN = Bit4, + UI_SIFS_CFG_DISABLE_SPI = Bit1, + UI_SIFS_CFG_DISABLE_I2C = Bit1 | Bit0 +}; + // PWR_MGMT0 enum PWR_MGMT0_BIT : uint8_t { GYRO_MODE_LOW_NOISE = Bit3 | Bit2, // 11: Places gyroscope in Low Noise (LN) Mode diff --git a/src/drivers/magnetometer/bosch/CMakeLists.txt b/src/drivers/magnetometer/bosch/CMakeLists.txt index b814e7224fb1..d7e848ec86a4 100644 --- a/src/drivers/magnetometer/bosch/CMakeLists.txt +++ b/src/drivers/magnetometer/bosch/CMakeLists.txt @@ -32,3 +32,4 @@ ############################################################################ add_subdirectory(bmm150) +add_subdirectory(bmm350) diff --git a/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp b/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp new file mode 100755 index 000000000000..e4e8e19f9c5e --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp @@ -0,0 +1,773 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "BMM350.hpp" +using namespace time_literals; + +BMM350::BMM350(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config), + ModuleParams(nullptr), + _px4_mag(get_device_id(), config.rotation) + +{ +} + +BMM350::~BMM350() +{ + perf_free(_reset_perf); + perf_free(_bad_read_perf); + perf_free(_self_test_failed_perf); +} + +int BMM350::init() +{ + ModuleParams::updateParams(); + ParametersUpdate(true); + int ret = I2C::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("I2C::init failed (%i)", ret); + return ret; + } + + return Reset() ? 0 : -1; +} + +bool BMM350::Reset() +{ + RegisterWrite(Register::CMD, SOFT_RESET); + _state = STATE::RESET; + ScheduleClear(); + ScheduleDelayed(1_ms); + return true; +} + +void BMM350::print_status() +{ + I2CSPIDriverBase::print_status(); + + perf_print_counter(_reset_perf); + perf_print_counter(_bad_read_perf); + perf_print_counter(_self_test_failed_perf); +} + +void BMM350::ParametersUpdate(bool force) +{ + if (_parameter_update_sub.updated() || force) { + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + updateParams(); + UpdateMagParams(); + } +} + +void BMM350::UpdateMagParams() +{ + uint8_t odr = GetODR(_param_bmm350_odr.get()); + uint8_t avg = GetAVG(_param_bmm350_avg.get()); + + _mag_odr_mode = odr; + _mag_avg_mode = avg; + _mag_pad_drive = static_cast(_param_bmm350_drive.get()); + PX4_DEBUG("Set params odr = %d, avg = %d, drive = %d", _mag_odr_mode, _mag_avg_mode, _mag_pad_drive); +} + +uint8_t BMM350::GetODR(int value) +{ + switch (value) { + case 0: return ODR_400HZ; + + case 1: return ODR_200HZ; + + case 2: return ODR_100HZ; + + case 3: return ODR_50HZ; + + case 4: return ODR_25HZ; + + case 5: return ODR_12_5HZ; + + case 6: return ODR_6_25HZ; + + case 7: return ODR_3_125HZ; + + case 8: return ODR_1_5625HZ; + + default: return ODR_200HZ; + } +} + +hrt_abstime BMM350::OdrToUs(uint8_t odr) +{ + switch (odr) { + case ODR_400HZ: + return 2500_us; + + case ODR_200HZ: + return 5000_us; + + case ODR_100HZ: + return 10000_us; + + case ODR_50HZ: + return 20000_us; + + case ODR_25HZ: + return 40000_us; + + case ODR_12_5HZ: + return 80000_us; + + case ODR_6_25HZ: + return 160000_us; + + case ODR_3_125HZ: + return 320000_us; + + case ODR_1_5625HZ: + return 640000_us; + + default: + return 5000_us; + } +} + +uint8_t BMM350::GetAVG(int value) +{ + switch (value) { + case 0: return AVG_NO_AVG; + + case 1: return AVG_2; + + case 2: return AVG_4; + + case 3: return AVG_8; + + default: return AVG_2; + } +} + + +int BMM350::probe() +{ + for (int i = 0; i < 3; i++) { + uint8_t chip_id; + + if (PX4_OK == RegisterRead(Register::CHIP_ID, &chip_id)) { + PX4_DEBUG("CHIP_ID: 0x%02hhX", chip_id); + + if (chip_id == chip_identification_number) { + return PX4_OK; + } + } + } + + return PX4_ERROR; +} + +void BMM350::RunImpl() +{ + const hrt_abstime now = hrt_absolute_time(); + int ret = PX4_OK; + ParametersUpdate(); + + switch (_state) { + case STATE::RESET: { + if (RegisterWrite(Register::CMD, SOFT_RESET) == PX4_OK) { + _reset_timestamp = now; + _state = STATE::WAIT_FOR_RESET; + perf_count(_reset_perf); + } + + ScheduleDelayed(10_ms); + } + break; + + case STATE::WAIT_FOR_RESET: { + ret = probe(); + + if (ret == PX4_OK) { + _state = STATE::RESET; + uint8_t status_0; + ret = RegisterRead(Register::PMU_STATUS_0, &status_0); + + if (ret == PX4_OK && (status_0 & PWR_NORMAL) != 0) { + ret = PX4_ERROR; + } + + if (ret == PX4_OK) { + ret = UpdateMagOffsets(); + } + + if (ret == PX4_OK) { + PX4_DEBUG("Going to FGR"); + _state = STATE::FGR; + + } + + ScheduleDelayed(10_ms); + + } else { + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Reset failed, retrying"); + _state = STATE::RESET; + ScheduleDelayed(100_ms); + + } else { + PX4_DEBUG("Reset not complete, check again in 10 ms"); + ScheduleDelayed(10_ms); + } + } + } + break; + + case STATE::FGR: { + _state = STATE::RESET; + uint8_t odr_reg_data = (ODR_100HZ & 0xf); + odr_reg_data = ((odr_reg_data & ~(0x30)) | ((AVG_2 << 0x4) & 0x30)); + ret = RegisterWrite(Register::PMU_CMD_AGGR_SET, odr_reg_data); + + if (ret == PX4_OK) { + ret = RegisterWrite(Register::PMU_CMD_AXIS_EN, EN_XYZ); + } + + if (ret == PX4_OK) { + ret = RegisterWrite(Register::PMU_CMD, PMU_CMD_FGR); + } + + if (ret == PX4_OK) { + PX4_DEBUG("Going to BR"); + _state = STATE::BR; + } + + ScheduleDelayed(30_ms); + } + break; + + case STATE::BR: { + uint8_t status_0; + _state = STATE::RESET; + ret = RegisterRead(Register::PMU_STATUS_0, &status_0); + + if (ret == PX4_OK && PMU_CMD_STATUS_0_RES(status_0) == PMU_STATUS_FGR) { + ret = RegisterWrite(Register::PMU_CMD, PMU_CMD_BR_FAST); + + if (ret == PX4_OK) { + PX4_DEBUG("Going to after reset"); + _state = STATE::AFTER_RESET; + } + } + + ScheduleDelayed(4_ms); + } + break; + + case STATE::AFTER_RESET: { + uint8_t status_0; + _state = STATE::RESET; + ret = RegisterRead(Register::PMU_STATUS_0, &status_0); + + if (ret == PX4_OK && PMU_CMD_STATUS_0_RES(status_0) == PMU_STATUS_BR_FAST) { + _state = STATE::MEASURE_FORCED; + _self_test_state = SELF_TEST_STATE::INIT; + PX4_DEBUG("Going to fast FM"); + } + + ScheduleNow(); + } + break; + + case STATE::MEASURE_FORCED: { + ret = RegisterWrite(Register::PMU_CMD, PMU_CMD_FAST_FM); + + if (ret == PX4_OK) { + _state = STATE::SELF_TEST_CHECK; + + } else { + _state = STATE::RESET; + } + + ScheduleDelayed(OdrToUs(_mag_odr_mode)); + break; + } + + case STATE::SELF_TEST_CHECK: { + + float xyzt[4]; + _state = STATE::RESET; + + if (ReadOutRawData(xyzt) != PX4_OK) { + perf_count(_self_test_failed_perf); + ScheduleNow(); + break; + } + + switch (_self_test_state) { + case SELF_TEST_STATE::INIT: + memcpy(_initial_self_test_values, xyzt, sizeof(_initial_self_test_values)); + + if (RegisterWrite(Register::TMR_SELF_TEST_USER, SELF_TEST_POS_X) == PX4_OK) { + _self_test_state = SELF_TEST_STATE::POS_X; + _state = STATE::MEASURE_FORCED; + } + + break; + + case SELF_TEST_STATE::POS_X: + if (xyzt[0] - _initial_self_test_values[0] >= 130.0f && + RegisterWrite(Register::TMR_SELF_TEST_USER, SELF_TEST_NEG_X) == PX4_OK) { + _self_test_state = SELF_TEST_STATE::NEG_X; + _state = STATE::MEASURE_FORCED; + } + + break; + + case SELF_TEST_STATE::NEG_X: + if (xyzt[0] - _initial_self_test_values[0] <= -130.0f && + RegisterWrite(Register::TMR_SELF_TEST_USER, SELF_TEST_POS_Y) == PX4_OK) { + _self_test_state = SELF_TEST_STATE::POS_Y; + _state = STATE::MEASURE_FORCED; + } + + break; + + case SELF_TEST_STATE::POS_Y: + if (xyzt[1] - _initial_self_test_values[1] >= 130.0f && + RegisterWrite(Register::TMR_SELF_TEST_USER, SELF_TEST_NEG_Y) == PX4_OK) { + _self_test_state = SELF_TEST_STATE::NEG_Y; + _state = STATE::MEASURE_FORCED; + } + + break; + + case SELF_TEST_STATE::NEG_Y: + if (xyzt[1] - _initial_self_test_values[1] <= -130.0f && + RegisterWrite(Register::TMR_SELF_TEST_USER, SELF_TEST_DISABLE) == PX4_OK) { + PX4_DEBUG("Self test good, going to configure"); + _state = STATE::CONFIGURE; + } + + break; + + } + + if (_state == STATE::RESET) { + PX4_DEBUG("Self test failed"); + perf_count(_self_test_failed_perf); + } + + ScheduleDelayed(1_ms); + } + break; + + + case STATE::CONFIGURE: + if (Configure() == PX4_OK) { + _state = STATE::READ; + PX4_DEBUG("Configure went fine"); + ScheduleOnInterval(OdrToUs(_mag_odr_mode), 50_ms); + + } else { + // CONFIGURE not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Configure failed, resetting"); + _state = STATE::RESET; + + } else { + PX4_DEBUG("Configure failed, retrying"); + } + + ScheduleDelayed(100_ms); + } + + break; + + case STATE::READ: { + // -- start get_compensated_mag_xyz_temp_data + float out_data[4] = {0.0f}; + float dut_offset_coeff[3], dut_sensit_coeff[3], dut_tcos[3], dut_tcss[3]; + float cr_ax_comp_x, cr_ax_comp_y, cr_ax_comp_z; + + ret = ReadOutRawData(out_data); + + if (ret == PX4_OK) { + // Apply compensation to temperature reading + out_data[3] = (1 + _mag_comp_vals.dut_sensit_coef.t_sens) * out_data[3] + + _mag_comp_vals.dut_offset_coef.t_offs; + + // Store magnetic compensation structure to an array + dut_offset_coeff[0] = _mag_comp_vals.dut_offset_coef.offset_x; + dut_offset_coeff[1] = _mag_comp_vals.dut_offset_coef.offset_y; + dut_offset_coeff[2] = _mag_comp_vals.dut_offset_coef.offset_z; + + dut_sensit_coeff[0] = _mag_comp_vals.dut_sensit_coef.sens_x; + dut_sensit_coeff[1] = _mag_comp_vals.dut_sensit_coef.sens_y; + dut_sensit_coeff[2] = _mag_comp_vals.dut_sensit_coef.sens_z; + + dut_tcos[0] = _mag_comp_vals.dut_tco.tco_x; + dut_tcos[1] = _mag_comp_vals.dut_tco.tco_y; + dut_tcos[2] = _mag_comp_vals.dut_tco.tco_z; + + dut_tcss[0] = _mag_comp_vals.dut_tcs.tcs_x; + dut_tcss[1] = _mag_comp_vals.dut_tcs.tcs_y; + dut_tcss[2] = _mag_comp_vals.dut_tcs.tcs_z; + + for (int idx = 0; idx < 3; idx++) { + out_data[idx] *= 1 + dut_sensit_coeff[idx]; + out_data[idx] += dut_offset_coeff[idx]; + out_data[idx] += dut_tcos[idx] * (out_data[3] - _mag_comp_vals.dut_t0); + out_data[idx] /= 1 + dut_tcss[idx] * (out_data[3] - _mag_comp_vals.dut_t0); + } + + cr_ax_comp_x = (out_data[0] - _mag_comp_vals.cross_axis.cross_x_y * out_data[1]) / + (1 - _mag_comp_vals.cross_axis.cross_y_x * _mag_comp_vals.cross_axis.cross_x_y); + cr_ax_comp_y = (out_data[1] - _mag_comp_vals.cross_axis.cross_y_x * out_data[0]) / + (1 - _mag_comp_vals.cross_axis.cross_y_x * _mag_comp_vals.cross_axis.cross_x_y); + cr_ax_comp_z = + (out_data[2] + + (out_data[0] * + (_mag_comp_vals.cross_axis.cross_y_x * _mag_comp_vals.cross_axis.cross_z_y - + _mag_comp_vals.cross_axis.cross_z_x) - + out_data[1] * + (_mag_comp_vals.cross_axis.cross_z_y - _mag_comp_vals.cross_axis.cross_x_y * + _mag_comp_vals.cross_axis.cross_z_x)) / + (1 - _mag_comp_vals.cross_axis.cross_y_x * _mag_comp_vals.cross_axis.cross_x_y)); + + out_data[0] = cr_ax_comp_x; + out_data[1] = cr_ax_comp_y; + out_data[2] = cr_ax_comp_z; + _px4_mag.set_error_count(perf_event_count(_bad_read_perf) + perf_event_count(_self_test_failed_perf)); + _px4_mag.set_temperature(out_data[3]); + _px4_mag.update(now, out_data[0], out_data[1], out_data[2]); + + } else { + perf_count(_bad_read_perf); + } + } + + break; + } +} + +int BMM350::Configure() +{ + PX4_DEBUG("Configuring"); + uint8_t readData = 0; + int ret; + + // Set pad drive + ret = RegisterWrite(Register::PAD_CTRL, (_mag_pad_drive & 0x7)); + + if (ret != PX4_OK) { + PX4_DEBUG("Couldn't set pad drive, defaults to 7"); + return ret; + } + + // Set PMU data aggregation + uint8_t odr = _mag_odr_mode; + uint8_t avg = _mag_avg_mode; + + if (odr == ODR_400HZ && avg >= AVG_2) { + avg = AVG_NO_AVG; + + } else if (odr == ODR_200HZ && avg >= AVG_4) { + avg = AVG_2; + + } else if (odr == ODR_100HZ && avg >= AVG_8) { + avg = AVG_4; + } + + uint8_t odr_reg_data = (odr & 0xf); + odr_reg_data = ((odr_reg_data & ~(0x30)) | ((avg << 0x4) & 0x30)); + + ret = RegisterWrite(Register::PMU_CMD_AGGR_SET, odr_reg_data); + + if (ret != PX4_OK) { + PX4_DEBUG("Cannot set PMU AGG REG"); + return ret; + } + + ret = RegisterRead(Register::PMU_CMD_AGGR_SET, &readData); + + if (ret != PX4_OK || readData != odr_reg_data) { + PX4_DEBUG("Couldn't check PMU AGGR REG"); + return ret; + } + + odr_reg_data = PMU_CMD_UPDATE_OAE; + ret = RegisterWrite(Register::PMU_CMD, odr_reg_data); + + if (ret != PX4_OK) { + PX4_DEBUG("Couldn't write PMU CMD REG"); + return ret; + } + + ret = RegisterRead(Register::PMU_CMD, &readData); + + if (ret != PX4_OK || readData != odr_reg_data) { + PX4_DEBUG("Couldn't check PMU CMD REG"); + return ret; + } + + // Enable AXIS + uint8_t axis_data = EN_X | EN_Y | EN_Z; + // PMU_CMD_AXIS_EN + ret = RegisterWrite(Register::PMU_CMD_AXIS_EN, axis_data); + + if (ret != PX4_OK) { + PX4_DEBUG("Couldn't write AXIS data"); + return ret; + } + + ret = RegisterRead(Register::PMU_CMD_AXIS_EN, &readData); + + if (ret != PX4_OK || readData != axis_data) { + PX4_DEBUG("Couldn't cross check the set AXIS"); + return ret; + } + + ret = RegisterWrite(Register::PMU_CMD, PMU_CMD_NM); + + if (ret != PX4_OK) { + PX4_DEBUG("Failed to start mag in normal mode"); + return ret; + } + + // microTesla -> Gauss + _px4_mag.set_scale(0.01f); + + return ret; +} + +int32_t BMM350::FixSign(uint32_t inval, int8_t num_bits) +{ + int32_t power = 1 << (num_bits - 1); // Calculate 2^(num_bits - 1) + int32_t retval = static_cast(inval); + + if (retval >= power) { + retval -= (power << 1); // Equivalent to retval = retval - (power * 2) + } + + return retval; +} + +int BMM350::ReadOutRawData(float *out_data) +{ + if (out_data == NULL) { + return -1; + } + + float temp = 0.0; + struct BMM350::raw_mag_data raw_data = {0}; + + // --- Start read_uncomp_mag_temp_data + uint8_t mag_data[14] = {0}; + + uint32_t raw_mag_x, raw_mag_y, raw_mag_z, raw_temp; + uint8_t cmd = static_cast(Register::DATAX_XLSB); + + uint8_t res = transfer(&cmd, 1, (uint8_t *)&mag_data, sizeof(mag_data)); + + if (res != PX4_OK) { + return -1; + } + + raw_mag_x = mag_data[2] + ((uint32_t)mag_data[3] << 8) + ((uint32_t)mag_data[4] << 16); + raw_mag_y = mag_data[5] + ((uint32_t)mag_data[6] << 8) + ((uint32_t)mag_data[7] << 16); + raw_mag_z = mag_data[8] + ((uint32_t)mag_data[9] << 8) + ((uint32_t)mag_data[10] << 16); + raw_temp = mag_data[11] + ((uint32_t)mag_data[12] << 8) + ((uint32_t)mag_data[13] << 16); + + raw_data.raw_x = FixSign(raw_mag_x, 24); + raw_data.raw_y = FixSign(raw_mag_y, 24); + raw_data.raw_z = FixSign(raw_mag_z, 24); + raw_data.raw_t = FixSign(raw_temp, 24); + // --- End read_uncomp_mag_temp_data + + // --- Start read_out_raw_data + out_data[0] = (float)raw_data.raw_x * lsb_to_utc_degc[0]; + out_data[1] = (float)raw_data.raw_y * lsb_to_utc_degc[1]; + out_data[2] = (float)raw_data.raw_z * lsb_to_utc_degc[2]; + out_data[3] = (float)raw_data.raw_t * lsb_to_utc_degc[3]; + + // Adjust temperature + if (out_data[3] > 0.0f) { + temp = (float)(out_data[3] - (1 * 25.49f)); + + } else if (out_data[3] < 0.0f) { + temp = (float)(out_data[3] - (-1 * 25.49f)); + + } else { + temp = (float)(out_data[3]); + } + + out_data[3] = temp; + + return res; +} + +int BMM350::ReadOTPWord(uint8_t addr, uint16_t *lsb_msb) +{ + uint8_t otp_cmd = OTP_DIR_READ | (addr & OTP_WORD_MSK); + int ret = RegisterWrite(Register::OTP_CMD, otp_cmd); + uint8_t otp_status = 0; + + if (ret == PX4_OK) { + do { + ret = RegisterRead(Register::OTP_STATUS, &otp_status); + + if (ret != PX4_OK) { + return PX4_ERROR; + } + } while (!(otp_status & 0x01)); + + uint8_t msb = 0, lsb = 0; + ret = RegisterRead(Register::OTP_DATA_MSB, &msb); + + if (ret == PX4_OK) { + ret = RegisterRead(Register::OTP_DATA_LSB, &lsb); + + if (ret == PX4_OK) { + *lsb_msb = ((msb << 8) | lsb) & 0xffff; + } + } + } + + return ret; +} + +int BMM350::UpdateMagOffsets() +{ + PX4_DEBUG("DUMPING OTP"); + uint16_t otp_data[32] = {0}; + + for (int idx = 0; idx < 32; idx++) { + if (ReadOTPWord(idx, &otp_data[idx]) != PX4_OK) { + return PX4_ERROR; + } + + PX4_DEBUG("i: %i, val = %i", idx, otp_data[idx]); + } + + if (RegisterWrite(Register::OTP_CMD, PWR_OFF_OTP) != PX4_OK) { + return PX4_ERROR; + } + + PX4_DEBUG("var_id: %i", (otp_data[30] & 0x7f00) >> 9); + + PX4_DEBUG("UPDATING OFFSETS"); + uint16_t off_x_lsb_msb, off_y_lsb_msb, off_z_lsb_msb, t_off; + uint8_t sens_x, sens_y, sens_z, t_sens; + uint8_t tco_x, tco_y, tco_z; + uint8_t tcs_x, tcs_y, tcs_z; + uint8_t cross_x_y, cross_y_x, cross_z_x, cross_z_y; + + off_x_lsb_msb = otp_data[0x0E] & 0x0FFF; + off_y_lsb_msb = ((otp_data[0x0E] & 0xF000) >> 4) + + (otp_data[0x0F] & 0x00FF); + off_z_lsb_msb = (otp_data[0x0F] & 0x0F00) + + (otp_data[0x10] & 0x00FF); + t_off = otp_data[0x0D] & 0x00FF; + + _mag_comp_vals.dut_offset_coef.offset_x = FixSign(off_x_lsb_msb, 12); + _mag_comp_vals.dut_offset_coef.offset_y = FixSign(off_y_lsb_msb, 12); + _mag_comp_vals.dut_offset_coef.offset_z = FixSign(off_z_lsb_msb, 12); + _mag_comp_vals.dut_offset_coef.t_offs = FixSign(t_off, 8) / 5.0f; + + sens_x = (otp_data[0x10] & 0xFF00) >> 8; + sens_y = (otp_data[0x11] & 0x00FF); + sens_z = (otp_data[0x11] & 0xFF00) >> 8; + t_sens = (otp_data[0x0D] & 0xFF00) >> 8; + + _mag_comp_vals.dut_sensit_coef.sens_x = FixSign(sens_x, 8) / 256.0f; + _mag_comp_vals.dut_sensit_coef.sens_y = (FixSign(sens_y, 8) / 256.0f) + 0.01f; + _mag_comp_vals.dut_sensit_coef.sens_z = FixSign(sens_z, 8) / 256.0f; + _mag_comp_vals.dut_sensit_coef.t_sens = FixSign(t_sens, 8) / 512.0f; + + tco_x = (otp_data[0x12] & 0x00FF); + tco_y = (otp_data[0x13] & 0x00FF); + tco_z = (otp_data[0x14] & 0x00FF); + + _mag_comp_vals.dut_tco.tco_x = FixSign(tco_x, 8) / 32.0f; + _mag_comp_vals.dut_tco.tco_y = FixSign(tco_y, 8) / 32.0f; + _mag_comp_vals.dut_tco.tco_z = FixSign(tco_z, 8) / 32.0f; + + tcs_x = (otp_data[0x12] & 0xFF00) >> 8; + tcs_y = (otp_data[0x13] & 0xFF00) >> 8; + tcs_z = (otp_data[0x14] & 0xFF00) >> 8; + + _mag_comp_vals.dut_tcs.tcs_x = FixSign(tcs_x, 8) / 16384.0f; + _mag_comp_vals.dut_tcs.tcs_y = FixSign(tcs_y, 8) / 16384.0f; + _mag_comp_vals.dut_tcs.tcs_z = (FixSign(tcs_z, 8) / 16384.0f) - 0.0001f; + + _mag_comp_vals.dut_t0 = (FixSign(otp_data[0x18], 16) / 512.0f) + 23.0f; + + cross_x_y = (otp_data[0x15] & 0x00FF); + cross_y_x = (otp_data[0x15] & 0xFF00) >> 8; + cross_z_x = (otp_data[0x16] & 0x00FF); + cross_z_y = (otp_data[0x16] & 0xFF00) >> 8; + + _mag_comp_vals.cross_axis.cross_x_y = FixSign(cross_x_y, 8) / 800.0f; + _mag_comp_vals.cross_axis.cross_y_x = FixSign(cross_y_x, 8) / 800.0f; + _mag_comp_vals.cross_axis.cross_z_x = FixSign(cross_z_x, 8) / 800.0f; + _mag_comp_vals.cross_axis.cross_z_y = FixSign(cross_z_y, 8) / 800.0f; + return PX4_OK; +} + +int BMM350::RegisterRead(Register reg, uint8_t *value) +{ + const uint8_t cmd = static_cast(reg); + uint8_t buffer[3] = {0}; + int ret = transfer(&cmd, 1, buffer, 3); + + if (ret != PX4_OK) { + PX4_DEBUG("register read 0x%02hhX failed, ret = %d", cmd, ret); + + } else { + *value = buffer[2]; + } + + return ret; +} + +int BMM350::RegisterWrite(Register reg, uint8_t value) +{ + uint8_t buffer[2] {(uint8_t)reg, value}; + int ret = transfer(buffer, sizeof(buffer), nullptr, 0); + + if (ret != PX4_OK) { + PX4_DEBUG("register write 0x%02hhX failed, ret = %d", (uint8_t)reg, ret); + } + + return ret; +} diff --git a/src/drivers/magnetometer/bosch/bmm350/BMM350.hpp b/src/drivers/magnetometer/bosch/bmm350/BMM350.hpp new file mode 100644 index 000000000000..4ba7f535dbd9 --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm350/BMM350.hpp @@ -0,0 +1,210 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file BMM350.hpp + * + * Driver for the Bosch BMM350 connected via I2C. + * + */ + +#pragma once + +#include "Bosch_BMM350_registers.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace Bosch_BMM350; +using namespace time_literals; + +class BMM350 : public device::I2C, public I2CSPIDriver, public ModuleParams +{ +public: + BMM350(const I2CSPIDriverConfig &config); + ~BMM350() override; + + static void print_usage(); + + void RunImpl(); + + int init() override; + void print_status() override; + +private: + struct mag_temp_data { + float x; + float y; + float z; + float temp; + }; + + struct raw_mag_data { + int32_t raw_x; + int32_t raw_y; + int32_t raw_z; + int32_t raw_t; + }; + + struct register_config_t { + Register reg; + uint8_t set_bits{0}; + uint8_t clear_bits{0}; + }; + + struct dut_offset_coef { + float t_offs; + float offset_x; + float offset_y; + float offset_z; + }; + struct dut_sensit_coef { + float t_sens; + float sens_x; + float sens_y; + float sens_z; + }; + + struct dut_tco { + float tco_x; + float tco_y; + float tco_z; + }; + + struct dut_tcs { + float tcs_x; + float tcs_y; + float tcs_z; + }; + + struct cross_axis { + float cross_x_y; + float cross_y_x; + float cross_z_x; + float cross_z_y; + }; + + struct mag_compensate_vals { + struct dut_offset_coef dut_offset_coef; + struct dut_sensit_coef dut_sensit_coef; + struct dut_tco dut_tco; + struct dut_tcs dut_tcs; + float dut_t0; + struct cross_axis cross_axis; + }; + + int probe() override; + bool Reset(); + int Configure(); + + int RegisterRead(Register reg, uint8_t *value); + int RegisterWrite(Register reg, uint8_t value); + + int8_t CompensateAxisAndTemp(); + int ReadOutRawData(float *out_data); + int ReadOTPWord(uint8_t addr, uint16_t *lsb_msb); + int32_t FixSign(uint32_t inval, int8_t num_bits); + + int UpdateMagOffsets(); + void ParametersUpdate(bool force = false); + void UpdateMagParams(); + uint8_t GetODR(int value); + hrt_abstime OdrToUs(uint8_t value); + uint8_t GetAVG(int value); + + PX4Magnetometer _px4_mag; + + perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME ": reset")}; + perf_counter_t _bad_read_perf{perf_alloc(PC_COUNT, MODULE_NAME ": bad read")}; + perf_counter_t _self_test_failed_perf{perf_alloc(PC_COUNT, MODULE_NAME ": self test failed")}; + + hrt_abstime _reset_timestamp{0}; + hrt_abstime _last_config_check_timestamp{0}; + + mag_compensate_vals _mag_comp_vals{0}; + + float _initial_self_test_values[4]; + + uint8_t _mag_odr_mode = ODR_200HZ; + uint8_t _mag_avg_mode = AVG_2; + uint8_t _mag_pad_drive = 7; + + + static constexpr float BXY_SENS = 14.55f; + static constexpr float BZ_SENS = 9.0f; + static constexpr float TEMP_SENS = 0.00204f; + static constexpr float INA_XY_GAIN_TRT = 19.46f; + static constexpr float INA_Z_GAIN_TRT = 31.0f; + static constexpr float ADC_GAIN = 1 / 1.5f; + static constexpr float LUT_GAIN = 0.714607238769531f; + static constexpr float POWER = 1000000.0f / 1048576.0f; + float lsb_to_utc_degc[4] = { + (POWER / (BXY_SENS *INA_XY_GAIN_TRT *ADC_GAIN * LUT_GAIN)), + (POWER / (BXY_SENS *INA_XY_GAIN_TRT *ADC_GAIN * LUT_GAIN)), + (POWER / (BZ_SENS *INA_Z_GAIN_TRT *ADC_GAIN * LUT_GAIN)), + 1 / (TEMP_SENS *ADC_GAIN *LUT_GAIN * 1048576) + }; + + enum class STATE : uint8_t { + RESET, + WAIT_FOR_RESET, + FGR, + BR, + AFTER_RESET, + MEASURE_FORCED, + SELF_TEST_CHECK, + CONFIGURE, + READ, + } _state{STATE::RESET}; + + enum class SELF_TEST_STATE : uint8_t { + INIT, + POS_X, + NEG_X, + POS_Y, + NEG_Y + } _self_test_state{SELF_TEST_STATE::INIT}; + + DEFINE_PARAMETERS( + (ParamInt) _param_bmm350_odr, + (ParamInt) _param_bmm350_avg, + (ParamInt) _param_bmm350_drive + ) + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; +}; diff --git a/src/drivers/magnetometer/bosch/bmm350/Bosch_BMM350_registers.hpp b/src/drivers/magnetometer/bosch/bmm350/Bosch_BMM350_registers.hpp new file mode 100644 index 000000000000..dc391f8824aa --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm350/Bosch_BMM350_registers.hpp @@ -0,0 +1,159 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Bosch_BMM350_registers.hpp + * + * Bosch BMM350 registers. + * + */ + +#pragma once + +#include + +namespace Bosch_BMM350 +{ +#define ENABLE_X_AXIS(axis_data) (axis_data = (axis_data & ~(0x01)) | (1 & 0x01)) +#define ENABLE_Y_AXIS(axis_data) (axis_data = ((axis_data & ~(0x02)) | ((1 << 0x1) & 0x02))) +#define ENABLE_Z_AXIS(axis_data) (axis_data = ((axis_data & ~(0x04)) | ((1 << 0x2) & 0x04))) + +static constexpr uint32_t I2C_SPEED = 400 * 1000; +static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0x14; +static constexpr uint8_t chip_identification_number = 0x33; + +enum class Register : uint8_t { + CHIP_ID = 0x00, + + PAD_CTRL = 0x03, + PMU_CMD_AGGR_SET = 0x04, + PMU_CMD_AXIS_EN = 0x05, + PMU_CMD = 0x06, + PMU_STATUS_0 = 0x07, + PMU_STATUS_1 = 0x08, + + I2C_WDT_SET = 0x0a, + + DATAX_XLSB = 0x31, + + OTP_CMD = 0x50, + OTP_DATA_MSB = 0x52, + OTP_DATA_LSB = 0x53, + OTP_STATUS = 0x55, + + TMR_SELF_TEST_USER = 0x60, + CMD = 0x7e +}; + +enum CONTROL_CMD : uint8_t { + SOFT_RESET = 0xb6, + EN_XYZ = 0x7 +}; + +enum PMU_CONTROL_CMD : uint8_t { + PMU_CMD_SUSPEND = 0x00, + PMU_CMD_NM = 0x01, + PMU_CMD_UPDATE_OAE = 0x02, + PMU_CMD_FM = 0x03, + PMU_CMD_FAST_FM = 0x04, + PMU_CMD_FGR = 0x05, + PMU_CMD_FAST_FGR = 0x06, + PMU_CMD_BR = 0x07, + PMU_CMD_BR_FAST = 0x08, + PMU_CMD_NM_TC = 0x09 +}; + +static inline uint8_t PMU_CMD_STATUS_0_RES(uint8_t val) +{ + return (val >> 5) & 0x7; +}; + +enum PMU_STATUS_0_BIT : uint8_t { + PMU_BUSY = (1 << 0), + ODR_OVWR = (1 << 1), + AVG_OVWR = (1 << 2), + PWR_NORMAL = (1 << 3), + ILLEGAL_CMD = (1 << 4) +}; + +enum PMU_STATUS_VALUE { + PMU_STATUS_SUS = 0x0, + PMU_STATUS_NM = 0x1, + PMU_STATUS_UPDATE_OAE = 0x2, + PMU_STATUS_FM = 0x3, + PMU_STATUS_FM_FAST = 0x4, + PMU_STATUS_FGR = 0x5, + PMU_STATUS_FGR_FAST = 0x6, + PMU_STATUS_BR = 0x7, + PMU_STATUS_BR_FAST = 0x7, +}; + +enum ODR_CONTROL_CMD : uint8_t { + ODR_400HZ = 0x2, + ODR_200HZ = 0x3, + ODR_100HZ = 0x4, + ODR_50HZ = 0x5, + ODR_25HZ = 0x6, + ODR_12_5HZ = 0x7, + ODR_6_25HZ = 0x8, + ODR_3_125HZ = 0x9, + ODR_1_5625HZ = 0xa +}; + +enum AVG_CONTROL_CMD : uint8_t { + AVG_NO_AVG = 0x0, + AVG_2 = 0x1, + AVG_4 = 0x2, + AVG_8 = 0x3 +}; + +enum ENABLE_AXIS_BIT : uint8_t { + EN_X = (1 << 0), + EN_Y = (1 << 1), + EN_Z = (1 << 2) +}; + +enum OTP_CONTROL_CMD : uint8_t { + PWR_OFF_OTP = 0x80, + OTP_DIR_READ = 0x20, + OTP_WORD_MSK = 0x1F, +}; + +enum SELF_TEST_CMD : uint8_t { + SELF_TEST_DISABLE = 0x00, + SELF_TEST_POS_X = 0x0d, + SELF_TEST_NEG_X = 0x0b, + SELF_TEST_POS_Y = 0x15, + SELF_TEST_NEG_Y = 0x13, +}; +} // namespace Bosch_BMM350 diff --git a/src/modules/differential_drive/DifferentialDriveControl/CMakeLists.txt b/src/drivers/magnetometer/bosch/bmm350/CMakeLists.txt similarity index 83% rename from src/modules/differential_drive/DifferentialDriveControl/CMakeLists.txt rename to src/drivers/magnetometer/bosch/bmm350/CMakeLists.txt index 4610fa6c3d4d..ca253077287e 100644 --- a/src/modules/differential_drive/DifferentialDriveControl/CMakeLists.txt +++ b/src/drivers/magnetometer/bosch/bmm350/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2024 PX4 Development Team. All rights reserved. +# Copyright (c) 2020 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -31,9 +31,18 @@ # ############################################################################ -px4_add_library(DifferentialDriveControl - DifferentialDriveControl.cpp -) - -target_link_libraries(DifferentialDriveControl PUBLIC pid) -target_include_directories(DifferentialDriveControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +px4_add_module( + MODULE drivers__magnetometer__bosch__bmm350 + MAIN bmm350 + COMPILE_FLAGS + SRCS + BMM350.cpp + BMM350.hpp + bmm350_main.cpp + Bosch_BMM350_registers.hpp + DEPENDS + drivers_magnetometer + px4_work_queue + MODULE_CONFIG + module.yaml + ) diff --git a/src/drivers/magnetometer/bosch/bmm350/Kconfig b/src/drivers/magnetometer/bosch/bmm350/Kconfig new file mode 100644 index 000000000000..834a2c694780 --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm350/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_MAGNETOMETER_BOSCH_BMM350 + bool "bmm350" + default n + ---help--- + Enable support for bosch bmm350 diff --git a/src/drivers/magnetometer/bosch/bmm350/bmm350_main.cpp b/src/drivers/magnetometer/bosch/bmm350/bmm350_main.cpp new file mode 100644 index 000000000000..c707d7957b6b --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm350/bmm350_main.cpp @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "BMM350.hpp" + +#include +#include + +void BMM350::print_usage() +{ + PRINT_MODULE_USAGE_NAME("bmm350", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x14); + PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" int bmm350_main(int argc, char *argv[]) +{ + int ch; + using ThisDriver = BMM350; + BusCLIArguments cli{true, false}; + cli.default_i2c_frequency = I2C_SPEED; + cli.i2c_address = I2C_ADDRESS_DEFAULT; + + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { + switch (ch) { + case 'R': + cli.rotation = (enum Rotation)atoi(cli.optArg()); + break; + } + } + + const char *verb = cli.optArg(); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_BMM350); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + } + + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + } + + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; +} diff --git a/src/drivers/magnetometer/bosch/bmm350/module.yaml b/src/drivers/magnetometer/bosch/bmm350/module.yaml new file mode 100644 index 000000000000..140c5c321a8a --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm350/module.yaml @@ -0,0 +1,44 @@ +module_name: BMM350 +parameters: + - group: Magnetometer + definitions: + BMM350_ODR: + description: + short: BMM350 ODR rate + long: | + Defines which ODR rate to use during data polling. + type: enum + values: + 0: 400 Hz + 1: 200 Hz + 2: 100 Hz + 3: 50 Hz + 4: 25 Hz + 5: 12.5 Hz + 6: 6.25 Hz + 7: 3.125 Hz + 8: 1.5625 Hz + reboot_required: true + default: [3] + BMM350_AVG: + description: + short: BMM350 data averaging + long: | + Defines which averaging mode to use during data polling. + type: enum + values: + 0: No averaging + 1: 2 sample averaging + 2: 4 sample averaging + 3: 8 sample averaging + reboot_required: true + default: [1] + BMM350_DRIVE: + description: + short: BMM350 pad drive strength setting + long: | + This setting helps avoid signal problems like overshoot or undershoot. + type: int32 + min: 0 + max: 7 + default: [7] diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp index 4d4715a0d529..5b41dde3b846 100644 --- a/src/lib/battery/battery.cpp +++ b/src/lib/battery/battery.cpp @@ -108,10 +108,6 @@ void Battery::updateCurrent(const float current_a) void Battery::updateBatteryStatus(const hrt_abstime ×tamp) { - if (!_battery_initialized && _internal_resistance_initialized && _params.n_cells > 0) { - resetInternalResistanceEstimation(_voltage_v, _current_a); - } - // Require minimum voltage otherwise override connected status if (_voltage_v < LITHIUM_BATTERY_RECOGNITION_VOLTAGE) { _connected = false; @@ -121,9 +117,13 @@ void Battery::updateBatteryStatus(const hrt_abstime ×tamp) _last_unconnected_timestamp = timestamp; } - // wait with initializing filters to avoid relying on a voltage sample from the rising edge + // Wait with initializing filters to avoid relying on a voltage sample from the rising edge _battery_initialized = _connected && (timestamp > _last_unconnected_timestamp + 2_s); + if (_connected && !_battery_initialized && _internal_resistance_initialized && _params.n_cells > 0) { + resetInternalResistanceEstimation(_voltage_v, _current_a); + } + sumDischarged(timestamp, _current_a); _state_of_charge_volt_based = calculateStateOfChargeVoltageBased(_voltage_v, _current_a); diff --git a/src/lib/button/ButtonPublisher.cpp b/src/lib/button/ButtonPublisher.cpp index b3b35a7b3af1..826e5ed49f9a 100644 --- a/src/lib/button/ButtonPublisher.cpp +++ b/src/lib/button/ButtonPublisher.cpp @@ -70,9 +70,9 @@ void ButtonPublisher::pairingButtonTriggerEvent() led_control_s led_control{}; led_control.led_mask = 0xff; led_control.mode = led_control_s::MODE_BLINK_FAST; - led_control.color = led_control_s::COLOR_GREEN; - led_control.num_blinks = 1; - led_control.priority = 0; + led_control.color = led_control_s::COLOR_WHITE; + led_control.num_blinks = 20; + led_control.priority = 2; led_control.timestamp = hrt_absolute_time(); _led_control_pub.publish(led_control); diff --git a/src/lib/mixer_module/functions/FunctionManualRC.hpp b/src/lib/mixer_module/functions/FunctionManualRC.hpp index 38f7d096a94e..171c28a81750 100644 --- a/src/lib/mixer_module/functions/FunctionManualRC.hpp +++ b/src/lib/mixer_module/functions/FunctionManualRC.hpp @@ -45,9 +45,7 @@ class FunctionManualRC : public FunctionProviderBase public: FunctionManualRC() { - for (int i = 0; i < num_data_points; ++i) { - _data[i] = NAN; - } + resetAllToDisarmedValue(); } static FunctionProviderBase *allocate(const Context &context) { return new FunctionManualRC(); } @@ -57,17 +55,22 @@ class FunctionManualRC : public FunctionProviderBase manual_control_setpoint_s manual_control_setpoint; if (_topic.update(&manual_control_setpoint)) { - _data[0] = manual_control_setpoint.roll; - _data[1] = manual_control_setpoint.pitch; - _data[2] = manual_control_setpoint.throttle; - _data[3] = manual_control_setpoint.yaw; - _data[4] = manual_control_setpoint.flaps; - _data[5] = manual_control_setpoint.aux1; - _data[6] = manual_control_setpoint.aux2; - _data[7] = manual_control_setpoint.aux3; - _data[8] = manual_control_setpoint.aux4; - _data[9] = manual_control_setpoint.aux5; - _data[10] = manual_control_setpoint.aux6; + if (manual_control_setpoint.valid) { + _data[0] = manual_control_setpoint.roll; + _data[1] = manual_control_setpoint.pitch; + _data[2] = manual_control_setpoint.throttle; + _data[3] = manual_control_setpoint.yaw; + _data[4] = manual_control_setpoint.flaps; + _data[5] = manual_control_setpoint.aux1; + _data[6] = manual_control_setpoint.aux2; + _data[7] = manual_control_setpoint.aux3; + _data[8] = manual_control_setpoint.aux4; + _data[9] = manual_control_setpoint.aux5; + _data[10] = manual_control_setpoint.aux6; + + } else { + resetAllToDisarmedValue(); + } } } @@ -76,6 +79,13 @@ class FunctionManualRC : public FunctionProviderBase private: static constexpr int num_data_points = 11; + void resetAllToDisarmedValue() + { + for (int i = 0; i < num_data_points; ++i) { + _data[i] = NAN; + } + } + static_assert(num_data_points == (int)OutputFunction::RC_AUXMax - (int)OutputFunction::RC_Roll + 1, "number of functions mismatch"); diff --git a/src/lib/parameters/px4params/markdownout.py b/src/lib/parameters/px4params/markdownout.py index 34e6f9e38666..4c698fb5115a 100644 --- a/src/lib/parameters/px4params/markdownout.py +++ b/src/lib/parameters/px4params/markdownout.py @@ -26,12 +26,18 @@ def __init__(self, groups): for param in group.GetParams(): name = param.GetName() short_desc = param.GetFieldValue("short_desc") or '' + + # Add fullstop to short_desc if not present if short_desc: if not short_desc.strip().endswith('.'): short_desc += "." - #short_desc = html.escape(short_desc) + long_desc = param.GetFieldValue("long_desc") or '' - #long_desc = html.escape(long_desc) + + #Strip out short text from start of long text, if it ends in fullstop + if long_desc.startswith(short_desc): + long_desc = long_desc[len(short_desc):].lstrip() + min_val = param.GetFieldValue("min") or '' max_val = param.GetFieldValue("max") or '' increment = param.GetFieldValue("increment") or '' @@ -81,7 +87,7 @@ def __init__(self, groups): if bitmask_list: result += bitmask_output # Format the ranges as a table. - result += f"Reboot | minValue | maxValue | increment | default | unit\n--- | --- | --- | --- | --- | ---\n{reboot_required} | {min_val} | {max_val} | {increment} | {def_val} | {unit} \n\n" + result += f"Reboot | minValue | maxValue | increment | default | unit\n--- | --- | --- | --- | --- | ---\n{'✓' if reboot_required else ' ' } | {min_val} | {max_val} | {increment} | {def_val} | {unit} \n\n" self.output = result diff --git a/src/lib/pure_pursuit/PurePursuit.hpp b/src/lib/pure_pursuit/PurePursuit.hpp index 66b9b8ce7cfe..9de498c2e1b1 100644 --- a/src/lib/pure_pursuit/PurePursuit.hpp +++ b/src/lib/pure_pursuit/PurePursuit.hpp @@ -95,9 +95,9 @@ class PurePursuit : public ModuleParams * @param prev_wp_ned North/East coordinates of previous waypoint in NED frame [m]. * @param curr_pos_ned North/East coordinates of current position of the vehicle in NED frame [m]. * @param vehicle_speed Vehicle speed [m/s]. - * @param RA_LOOKAHD_GAIN Tuning parameter [-] - * @param RA_LOOKAHD_MAX Maximum lookahead distance [m] - * @param RA_LOOKAHD_MIN Minimum lookahead distance [m] + * @param PP_LOOKAHD_GAIN Tuning parameter [-] + * @param PP_LOOKAHD_MAX Maximum lookahead distance [m] + * @param PP_LOOKAHD_MIN Minimum lookahead distance [m] */ float calcDesiredHeading(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, const Vector2f &curr_pos_ned, float vehicle_speed); diff --git a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt index 7c78c47c8534..7265e042b4c0 100644 --- a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt +++ b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt @@ -55,6 +55,7 @@ px4_add_library(health_and_arming_checks checks/manualControlCheck.cpp checks/missionCheck.cpp checks/modeCheck.cpp + checks/navigatorCheck.cpp checks/offboardCheck.cpp checks/openDroneIDCheck.cpp checks/parachuteCheck.cpp diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp index af0fd9aec5f0..fdf38f5d5d77 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp @@ -49,6 +49,7 @@ #include "checks/escCheck.hpp" #include "checks/estimatorCheck.hpp" #include "checks/failureDetectorCheck.hpp" +#include "checks/navigatorCheck.hpp" #include "checks/gyroCheck.hpp" #include "checks/imuConsistencyCheck.hpp" #include "checks/loggerCheck.hpp" @@ -129,6 +130,7 @@ class HealthAndArmingChecks : public ModuleParams EscChecks _esc_checks; EstimatorChecks _estimator_checks; FailureDetectorChecks _failure_detector_checks; + NavigatorChecks _navigator_checks; GyroChecks _gyro_checks; ImuConsistencyChecks _imu_consistency_checks; LoggerChecks _logger_checks; @@ -167,6 +169,7 @@ class HealthAndArmingChecks : public ModuleParams &_esc_checks, &_estimator_checks, &_failure_detector_checks, + &_navigator_checks, &_gyro_checks, &_imu_consistency_checks, &_logger_checks, diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp index 2ede2baf6381..2b7c45318301 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp @@ -533,19 +533,6 @@ void EstimatorChecks::checkEstimatorStatusFlags(const Context &context, Report & estimator_status_flags_s estimator_status_flags; if (_estimator_status_flags_sub.copy(&estimator_status_flags)) { - - bool dead_reckoning = estimator_status_flags.cs_wind_dead_reckoning - || estimator_status_flags.cs_inertial_dead_reckoning; - - if (!dead_reckoning) { - // position requirements (update if not dead reckoning) - bool gps = estimator_status_flags.cs_gps; - bool optical_flow = estimator_status_flags.cs_opt_flow; - bool vision_position = estimator_status_flags.cs_ev_pos; - - _position_reliant_on_optical_flow = !gps && optical_flow && !vision_position; - } - // Check for a magnetometer fault and notify the user if (estimator_status_flags.cs_mag_fault) { /* EVENT @@ -722,15 +709,6 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f // Check if quality checking of position accuracy and consistency is to be performed const float lpos_eph_threshold = (_param_com_pos_fs_eph.get() < 0) ? INFINITY : _param_com_pos_fs_eph.get(); - float lpos_eph_threshold_relaxed = lpos_eph_threshold; - - // Set the allowable position uncertainty based on combination of flight and estimator state - // When we are in a operator demanded position control mode and are solely reliant on optical flow, - // do not check position error because it will gradually increase throughout flight and the operator will compensate for the drift - if (_position_reliant_on_optical_flow) { - lpos_eph_threshold_relaxed = INFINITY; - } - bool xy_valid = lpos.xy_valid && !_nav_test_failed; bool v_xy_valid = lpos.v_xy_valid && !_nav_test_failed; @@ -749,7 +727,7 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f _last_gpos_fail_time_us, !failsafe_flags.global_position_invalid); // Additional warning if the system is about to enter position-loss failsafe after dead-reckoning period - const float eph_critical = 2.5f * _param_com_pos_fs_eph.get(); // threshold used to trigger the navigation failsafe + const float eph_critical = 2.5f * lpos_eph_threshold; // threshold used to trigger the navigation failsafe const float gpos_critical_warning_thrld = math::max(0.9f * eph_critical, math::max(eph_critical - 10.f, 0.f)); estimator_status_flags_s estimator_status_flags; @@ -793,6 +771,10 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f !checkPosVelValidity(now, xy_valid, lpos.eph, lpos_eph_threshold, lpos.timestamp, _last_lpos_fail_time_us, !failsafe_flags.local_position_invalid); + + // In some modes we assume that the operator will compensate for the drift so we do not need to check the position error + const float lpos_eph_threshold_relaxed = INFINITY; + failsafe_flags.local_position_invalid_relaxed = !checkPosVelValidity(now, xy_valid, lpos.eph, lpos_eph_threshold_relaxed, lpos.timestamp, _last_lpos_relaxed_fail_time_us, !failsafe_flags.local_position_invalid_relaxed); diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp index 6578776ee99d..4f61df88670f 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp @@ -100,8 +100,6 @@ class EstimatorChecks : public HealthAndArmingCheckBase bool _nav_test_passed{false}; ///< true if the post takeoff navigation test has passed bool _nav_test_failed{false}; ///< true if the post takeoff navigation test has failed - bool _position_reliant_on_optical_flow{false}; - bool _gps_was_fused{false}; bool _gnss_spoofed{false}; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp index c60a9a4af05e..8ef3c5cfa966 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp @@ -64,6 +64,7 @@ int ExternalChecks::addRegistration(int8_t nav_mode_id, int8_t replaces_nav_stat _active_registrations_mask |= 1 << free_registration_index; _registrations[free_registration_index].nav_mode_id = nav_mode_id; _registrations[free_registration_index].replaces_nav_state = replaces_nav_state; + _registrations[free_registration_index].waiting_for_first_response = true; _registrations[free_registration_index].num_no_response = 0; _registrations[free_registration_index].unresponsive = false; _registrations[free_registration_index].total_num_unresponsive = 0; @@ -230,6 +231,7 @@ void ExternalChecks::update() && _current_request_id == reply.request_id) { _reply_received_mask |= 1u << reply.registration_id; _registrations[reply.registration_id].num_no_response = 0; + _registrations[reply.registration_id].waiting_for_first_response = false; // Prevent toggling between unresponsive & responsive state if (_registrations[reply.registration_id].total_num_unresponsive <= 3) { @@ -253,7 +255,10 @@ void ExternalChecks::update() for (int i = 0; i < MAX_NUM_REGISTRATIONS; ++i) { if ((1u << i) & no_reply) { - if (!_registrations[i].unresponsive && ++_registrations[i].num_no_response >= NUM_NO_REPLY_UNTIL_UNRESPONSIVE) { + const int max_num_no_reply = + _registrations[i].waiting_for_first_response ? NUM_NO_REPLY_UNTIL_UNRESPONSIVE_INIT : NUM_NO_REPLY_UNTIL_UNRESPONSIVE; + + if (!_registrations[i].unresponsive && ++_registrations[i].num_no_response > max_num_no_reply) { // Clear immediately if not a mode if (_registrations[i].nav_mode_id == -1) { removeRegistration(i, -1); diff --git a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp index b4ee24cba6d5..7129e4620361 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp @@ -72,6 +72,9 @@ class ExternalChecks : public HealthAndArmingCheckBase static constexpr hrt_abstime UPDATE_INTERVAL = 300_ms; static_assert(REQUEST_TIMEOUT < UPDATE_INTERVAL, "keep timeout < update interval"); static constexpr int NUM_NO_REPLY_UNTIL_UNRESPONSIVE = 3; ///< Mode timeout = this value * UPDATE_INTERVAL + /// Timeout directly after registering (in some cases ROS can take a while until the subscription gets the first + /// sample, around 800ms was observed) + static constexpr int NUM_NO_REPLY_UNTIL_UNRESPONSIVE_INIT = 10; void checkNonRegisteredModes(const Context &context, Report &reporter) const; @@ -83,6 +86,7 @@ class ExternalChecks : public HealthAndArmingCheckBase int8_t nav_mode_id{-1}; ///< associated mode, -1 if none int8_t replaces_nav_state{-1}; + bool waiting_for_first_response{true}; uint8_t num_no_response{0}; bool unresponsive{false}; uint8_t total_num_unresponsive{0}; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.cpp new file mode 100644 index 000000000000..2a0245bb1fbd --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.cpp @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "navigatorCheck.hpp" + +void NavigatorChecks::checkAndReport(const Context &context, Report &reporter) +{ + navigator_status_s status; + + if (!_navigator_status_sub.copy(&status)) { + status = {}; + } + + if (context.status().nav_state == status.nav_state) { + + reporter.failsafeFlags().navigator_failure = (status.failure != navigator_status_s::FAILURE_NONE); + + if (status.failure == navigator_status_s::FAILURE_HAGL) { + /* EVENT + */ + reporter.armingCheckFailure(NavModes::All, + health_component_t::system, + events::ID("check_navigator_failure_hagl"), + events::Log::Error, + "Waypoint above maximum height"); + } + } +} diff --git a/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.hpp new file mode 100644 index 000000000000..48c6965deaf8 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.hpp @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "../Common.hpp" +#include +#include + + +class NavigatorChecks : public HealthAndArmingCheckBase +{ +public: + NavigatorChecks() = default; + ~NavigatorChecks() = default; + + void checkAndReport(const Context &context, Report &reporter) override; + +private: + uORB::Subscription _navigator_status_sub{ORB_ID(navigator_status)}; +}; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.cpp index 0b23a949aef6..69afd1c12cce 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.cpp @@ -74,16 +74,10 @@ void PowerChecks::checkAndReport(const Context &context, Report &reporter) if (!system_power.usb_connected) { float avionics_power_rail_voltage = system_power.voltage5v_v; - const float low_error_threshold = 4.5f; - const float low_warning_threshold = 4.8f; - const float high_warning_threshold = 5.4f; + const float low_error_threshold = 4.7f; + const float high_error_threshold = 5.4f; - if (avionics_power_rail_voltage < low_warning_threshold) { - NavModes affected_groups = NavModes::None; - - if (avionics_power_rail_voltage < low_error_threshold) { - affected_groups = NavModes::All; - } + if (avionics_power_rail_voltage < low_error_threshold) { /* EVENT * @description @@ -93,16 +87,16 @@ void PowerChecks::checkAndReport(const Context &context, Report &reporter) * This check can be configured via CBRK_SUPPLY_CHK parameter. * */ - reporter.healthFailure(affected_groups, health_component_t::system, + reporter.healthFailure(NavModes::All, health_component_t::system, events::ID("check_avionics_power_low"), - events::Log::Error, "Avionics Power low: {1:.2} Volt", avionics_power_rail_voltage, low_warning_threshold); + events::Log::Error, "Avionics Power low: {1:.2} Volt", avionics_power_rail_voltage, low_error_threshold); if (reporter.mavlink_log_pub()) { mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Avionics Power low: %6.2f Volt", (double)avionics_power_rail_voltage); } - } else if (avionics_power_rail_voltage > high_warning_threshold) { + } else if (avionics_power_rail_voltage > high_error_threshold) { /* EVENT * @description * Check the voltage supply to the FMU, it must be below {2:.2} Volt. @@ -113,7 +107,7 @@ void PowerChecks::checkAndReport(const Context &context, Report &reporter) */ reporter.healthFailure(NavModes::All, health_component_t::system, events::ID("check_avionics_power_high"), - events::Log::Error, "Avionics Power high: {1:.2} Volt", avionics_power_rail_voltage, high_warning_threshold); + events::Log::Error, "Avionics Power high: {1:.2} Volt", avionics_power_rail_voltage, high_error_threshold); if (reporter.mavlink_log_pub()) { mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Avionics Power high: %6.2f Volt", diff --git a/src/modules/commander/ModeUtil/mode_requirements.cpp b/src/modules/commander/ModeUtil/mode_requirements.cpp index 0718804df282..32eb5b1e8116 100644 --- a/src/modules/commander/ModeUtil/mode_requirements.cpp +++ b/src/modules/commander/ModeUtil/mode_requirements.cpp @@ -82,11 +82,6 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags) setRequirement(vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW, flags.mode_req_local_position_relaxed); setRequirement(vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW, flags.mode_req_manual_control); - if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_global_position); - setRequirement(vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW, flags.mode_req_global_position); - } - // NAVIGATION_STATE_AUTO_MISSION setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_angular_velocity); setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_attitude); diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index fef6e2136bf3..11285af40352 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -472,6 +472,16 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, CHECK_FAILSAFE(status_flags, local_position_accuracy_low, ActionOptions(Action::RTL)); } + if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF || + state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { + CHECK_FAILSAFE(status_flags, navigator_failure, + ActionOptions(Action::Land).clearOn(ClearCondition::OnModeChangeOrDisarm)); + + } else { + CHECK_FAILSAFE(status_flags, navigator_failure, + ActionOptions(Action::Hold).clearOn(ClearCondition::OnModeChangeOrDisarm)); + } + CHECK_FAILSAFE(status_flags, geofence_breached, fromGfActParam(_param_gf_action.get()).cannotBeDeferred()); // Battery flight time remaining failsafe diff --git a/src/modules/commander/failsafe/framework.cpp b/src/modules/commander/failsafe/framework.cpp index 9f88f85f3e8e..729205ebe89e 100644 --- a/src/modules/commander/failsafe/framework.cpp +++ b/src/modules/commander/failsafe/framework.cpp @@ -224,6 +224,16 @@ void FailsafeBase::notifyUser(uint8_t user_intended_mode, Action action, Action {events::Log::Warning, events::LogInternal::Warning}, "Failsafe warning:", mavlink_mode); + } else if (action == Action::Descend || action == Action::FallbackAltCtrl || action == Action::FallbackStab) { + /* EVENT + * @description Failsafe actions that disengage the autopilot (remove position control) + * @type append_health_and_arming_messages + */ + events::send( + events::ID("commander_failsafe_enter_autopilot_disengaged"), + {events::Log::Critical, events::LogInternal::Warning}, + "Failsafe activated: Autopilot disengaged, switching to {2}", mavlink_mode, failsafe_action); + } else { /* EVENT * @type append_health_and_arming_messages @@ -231,7 +241,7 @@ void FailsafeBase::notifyUser(uint8_t user_intended_mode, Action action, Action events::send( events::ID("commander_failsafe_enter_generic"), {events::Log::Critical, events::LogInternal::Warning}, - "Failsafe activated: Autopilot disengaged, switching to {2}", mavlink_mode, failsafe_action); + "Failsafe activated: switching to {2}", mavlink_mode, failsafe_action); } } else { @@ -467,7 +477,7 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s // Check if we should enter delayed Hold if (_current_delay > 0 && !_user_takeover_active && allow_user_takeover <= UserTakeoverAllowed::AlwaysModeSwitchOnly - && selected_action != Action::Disarm && selected_action != Action::Terminate) { + && selected_action != Action::Disarm && selected_action != Action::Terminate && selected_action != Action::Hold) { returned_state.delayed_action = selected_action; selected_action = Action::Hold; allow_user_takeover = UserTakeoverAllowed::AlwaysModeSwitchOnly; diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 3399bd619016..48363eeb1e38 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -239,7 +239,7 @@ ControlAllocator::update_effectiveness_source() break; case EffectivenessSource::ROVER_DIFFERENTIAL: - // differential_drive_control does allocation and publishes directly to actuator_motors topic + // rover_differential_control does allocation and publishes directly to actuator_motors topic break; case EffectivenessSource::FIXED_WING: diff --git a/src/modules/differential_drive/DifferentialDrive.cpp b/src/modules/differential_drive/DifferentialDrive.cpp deleted file mode 100644 index a1d34e01e7d2..000000000000 --- a/src/modules/differential_drive/DifferentialDrive.cpp +++ /dev/null @@ -1,191 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2023-2024 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include "DifferentialDrive.hpp" - -DifferentialDrive::DifferentialDrive() : - ModuleParams(nullptr), - ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) -{ - updateParams(); -} - -bool DifferentialDrive::init() -{ - ScheduleOnInterval(10_ms); // 100 Hz - return true; -} - -void DifferentialDrive::updateParams() -{ - ModuleParams::updateParams(); - - _differential_drive_kinematics.setWheelBase(_param_rdd_wheel_base.get()); - - _max_speed = _param_rdd_wheel_speed.get() * _param_rdd_wheel_radius.get(); - _differential_drive_guidance.setMaxSpeed(_max_speed); - _differential_drive_kinematics.setMaxSpeed(_max_speed); - - _max_angular_velocity = _max_speed / (_param_rdd_wheel_base.get() / 2.f); - _differential_drive_guidance.setMaxAngularVelocity(_max_angular_velocity); - _differential_drive_kinematics.setMaxAngularVelocity(_max_angular_velocity); -} - -void DifferentialDrive::Run() -{ - if (should_exit()) { - ScheduleClear(); - exit_and_cleanup(); - return; - } - - hrt_abstime now = hrt_absolute_time(); - const float dt = math::min((now - _time_stamp_last), 5000_ms) / 1e6f; - _time_stamp_last = now; - - if (_parameter_update_sub.updated()) { - parameter_update_s parameter_update; - _parameter_update_sub.copy(¶meter_update); - updateParams(); - } - - if (_vehicle_control_mode_sub.updated()) { - vehicle_control_mode_s vehicle_control_mode{}; - - if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) { - _manual_driving = vehicle_control_mode.flag_control_manual_enabled; - _mission_driving = vehicle_control_mode.flag_control_auto_enabled; - } - } - - if (_vehicle_status_sub.updated()) { - vehicle_status_s vehicle_status{}; - - if (_vehicle_status_sub.copy(&vehicle_status)) { - const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); - const bool spooled_up = armed && (hrt_elapsed_time(&vehicle_status.armed_time) > _param_com_spoolup_time.get() * 1_s); - _differential_drive_kinematics.setArmed(spooled_up); - _acro_driving = (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO); - } - } - - if (_manual_driving) { - // Manual mode - // directly produce setpoints from the manual control setpoint (joystick) - if (_manual_control_setpoint_sub.updated()) { - manual_control_setpoint_s manual_control_setpoint{}; - - if (_manual_control_setpoint_sub.copy(&manual_control_setpoint)) { - differential_drive_setpoint_s setpoint{}; - setpoint.speed = manual_control_setpoint.throttle * math::max(0.f, _param_rdd_speed_scale.get()) * _max_speed; - setpoint.yaw_rate = manual_control_setpoint.roll * _param_rdd_ang_velocity_scale.get() * _max_angular_velocity; - - // if acro mode, we activate the yaw rate control - if (_acro_driving) { - setpoint.closed_loop_speed_control = false; - setpoint.closed_loop_yaw_rate_control = true; - - } else { - setpoint.closed_loop_speed_control = false; - setpoint.closed_loop_yaw_rate_control = false; - } - - setpoint.timestamp = now; - _differential_drive_setpoint_pub.publish(setpoint); - } - } - - } else if (_mission_driving) { - // Mission mode - // directly receive setpoints from the guidance library - _differential_drive_guidance.computeGuidance( - _differential_drive_control.getVehicleYaw(), - _differential_drive_control.getVehicleBodyYawRate(), - dt - ); - } - - _differential_drive_control.control(dt); - _differential_drive_kinematics.allocate(); -} - -int DifferentialDrive::task_spawn(int argc, char *argv[]) -{ - DifferentialDrive *instance = new DifferentialDrive(); - - if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; - - if (instance->init()) { - return PX4_OK; - } - - } else { - PX4_ERR("alloc failed"); - } - - delete instance; - _object.store(nullptr); - _task_id = -1; - - return PX4_ERROR; -} - -int DifferentialDrive::custom_command(int argc, char *argv[]) -{ - return print_usage("unknown command"); -} - -int DifferentialDrive::print_usage(const char *reason) -{ - if (reason) { - PX4_ERR("%s\n", reason); - } - - PRINT_MODULE_DESCRIPTION( - R"DESCR_STR( -### Description -Rover Differential Drive controller. -)DESCR_STR"); - - PRINT_MODULE_USAGE_NAME("differential_drive", "controller"); - PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); - return 0; -} - -extern "C" __EXPORT int differential_drive_main(int argc, char *argv[]) -{ - return DifferentialDrive::main(argc, argv); -} diff --git a/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.cpp b/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.cpp deleted file mode 100644 index 728c5e666a10..000000000000 --- a/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.cpp +++ /dev/null @@ -1,109 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2023-2024 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include "DifferentialDriveControl.hpp" - -using namespace matrix; - -DifferentialDriveControl::DifferentialDriveControl(ModuleParams *parent) : ModuleParams(parent) -{ - pid_init(&_pid_angular_velocity, PID_MODE_DERIVATIV_NONE, 0.001f); - pid_init(&_pid_speed, PID_MODE_DERIVATIV_NONE, 0.001f); -} - -void DifferentialDriveControl::updateParams() -{ - ModuleParams::updateParams(); - - pid_set_parameters(&_pid_angular_velocity, - _param_rdd_p_gain_angular_velocity.get(), // Proportional gain - _param_rdd_i_gain_angular_velocity.get(), // Integral gain - 0, // Derivative gain - 20, // Integral limit - 200); // Output limit - - pid_set_parameters(&_pid_speed, - _param_rdd_p_gain_speed.get(), // Proportional gain - _param_rdd_i_gain_speed.get(), // Integral gain - 0, // Derivative gain - 2, // Integral limit - 200); // Output limit -} - -void DifferentialDriveControl::control(float dt) -{ - if (_vehicle_angular_velocity_sub.updated()) { - vehicle_angular_velocity_s vehicle_angular_velocity{}; - - if (_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity)) { - _vehicle_body_yaw_rate = vehicle_angular_velocity.xyz[2]; - } - } - - if (_vehicle_attitude_sub.updated()) { - vehicle_attitude_s vehicle_attitude{}; - - if (_vehicle_attitude_sub.copy(&vehicle_attitude)) { - _vehicle_attitude_quaternion = Quatf(vehicle_attitude.q); - _vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi(); - } - } - - if (_vehicle_local_position_sub.updated()) { - vehicle_local_position_s vehicle_local_position{}; - - if (_vehicle_local_position_sub.copy(&vehicle_local_position)) { - Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); - Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame); - _vehicle_forward_speed = velocity_in_body_frame(0); - } - } - - _differential_drive_setpoint_sub.update(&_differential_drive_setpoint); - - // PID to reach setpoint using control_output - differential_drive_setpoint_s differential_drive_control_output = _differential_drive_setpoint; - - if (_differential_drive_setpoint.closed_loop_speed_control) { - differential_drive_control_output.speed += - pid_calculate(&_pid_speed, _differential_drive_setpoint.speed, _vehicle_forward_speed, 0, dt); - } - - if (_differential_drive_setpoint.closed_loop_yaw_rate_control) { - differential_drive_control_output.yaw_rate += - pid_calculate(&_pid_angular_velocity, _differential_drive_setpoint.yaw_rate, _vehicle_body_yaw_rate, 0, dt); - } - - differential_drive_control_output.timestamp = hrt_absolute_time(); - _differential_drive_control_output_pub.publish(differential_drive_control_output); -} diff --git a/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.hpp b/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.hpp deleted file mode 100644 index 8d3b7e1c216c..000000000000 --- a/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.hpp +++ /dev/null @@ -1,91 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2023-2024 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file DifferentialDriveControl.hpp - * - * Controller for heading rate and forward speed. - */ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -class DifferentialDriveControl : public ModuleParams -{ -public: - DifferentialDriveControl(ModuleParams *parent); - ~DifferentialDriveControl() = default; - - void control(float dt); - float getVehicleBodyYawRate() const { return _vehicle_body_yaw_rate; } - float getVehicleYaw() const { return _vehicle_yaw; } - -protected: - void updateParams() override; - -private: - uORB::Subscription _differential_drive_setpoint_sub{ORB_ID(differential_drive_setpoint)}; - uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; - uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; - uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; - - uORB::Publication _differential_drive_control_output_pub{ORB_ID(differential_drive_control_output)}; - - differential_drive_setpoint_s _differential_drive_setpoint{}; - - matrix::Quatf _vehicle_attitude_quaternion{}; - float _vehicle_yaw{0.f}; - - // States - float _vehicle_body_yaw_rate{0.f}; - float _vehicle_forward_speed{0.f}; - - PID_t _pid_angular_velocity; ///< The PID controller for yaw rate. - PID_t _pid_speed; ///< The PID controller for velocity. - - DEFINE_PARAMETERS( - (ParamFloat) _param_rdd_p_gain_speed, - (ParamFloat) _param_rdd_i_gain_speed, - (ParamFloat) _param_rdd_p_gain_angular_velocity, - (ParamFloat) _param_rdd_i_gain_angular_velocity - ) -}; diff --git a/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp b/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp deleted file mode 100644 index df453ff8b5c0..000000000000 --- a/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp +++ /dev/null @@ -1,138 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2023-2024 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include "DifferentialDriveGuidance.hpp" - -#include - -using namespace matrix; - -DifferentialDriveGuidance::DifferentialDriveGuidance(ModuleParams *parent) : ModuleParams(parent) -{ - updateParams(); - - pid_init(&_heading_p_controller, PID_MODE_DERIVATIV_NONE, 0.001f); -} - -void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocity, float dt) -{ - if (_position_setpoint_triplet_sub.updated()) { - _position_setpoint_triplet_sub.copy(&_position_setpoint_triplet); - } - - if (_vehicle_global_position_sub.updated()) { - _vehicle_global_position_sub.copy(&_vehicle_global_position); - } - - matrix::Vector2d global_position(_vehicle_global_position.lat, _vehicle_global_position.lon); - matrix::Vector2d current_waypoint(_position_setpoint_triplet.current.lat, _position_setpoint_triplet.current.lon); - matrix::Vector2d next_waypoint(_position_setpoint_triplet.next.lat, _position_setpoint_triplet.next.lon); - - const float distance_to_next_wp = get_distance_to_next_waypoint(global_position(0), global_position(1), - current_waypoint(0), - current_waypoint(1)); - - float desired_heading = get_bearing_to_next_waypoint(global_position(0), global_position(1), current_waypoint(0), - current_waypoint(1)); - float heading_error = matrix::wrap_pi(desired_heading - yaw); - - if (_current_waypoint != current_waypoint) { - _currentState = GuidanceState::TURNING; - } - - // Make rover stop when it arrives at the last waypoint instead of loitering and driving around weirdly. - if ((current_waypoint == next_waypoint) && distance_to_next_wp <= _param_nav_acc_rad.get()) { - _currentState = GuidanceState::GOAL_REACHED; - } - - float desired_speed = 0.f; - - switch (_currentState) { - case GuidanceState::TURNING: - desired_speed = 0.f; - - if (fabsf(heading_error) < 0.05f) { - _currentState = GuidanceState::DRIVING; - } - - break; - - case GuidanceState::DRIVING: { - const float max_velocity = math::trajectory::computeMaxSpeedFromDistance(_param_rdd_max_jerk.get(), - _param_rdd_max_accel.get(), distance_to_next_wp, 0.0f); - _forwards_velocity_smoothing.updateDurations(max_velocity); - _forwards_velocity_smoothing.updateTraj(dt); - desired_speed = math::interpolate(abs(heading_error), 0.1f, 0.8f, - _forwards_velocity_smoothing.getCurrentVelocity(), 0.0f); - break; - } - - case GuidanceState::GOAL_REACHED: - // temporary till I find a better way to stop the vehicle - desired_speed = 0.f; - heading_error = 0.f; - angular_velocity = 0.f; - _desired_angular_velocity = 0.f; - break; - } - - // Compute the desired angular velocity relative to the heading error, to steer the vehicle towards the next waypoint. - float angular_velocity_pid = pid_calculate(&_heading_p_controller, heading_error, angular_velocity, 0, - dt) + heading_error; - - differential_drive_setpoint_s output{}; - output.speed = math::constrain(desired_speed, -_max_speed, _max_speed); - output.yaw_rate = math::constrain(angular_velocity_pid, -_max_angular_velocity, _max_angular_velocity); - output.closed_loop_speed_control = output.closed_loop_yaw_rate_control = true; - output.timestamp = hrt_absolute_time(); - - _differential_drive_setpoint_pub.publish(output); - - _current_waypoint = current_waypoint; -} - -void DifferentialDriveGuidance::updateParams() -{ - ModuleParams::updateParams(); - - pid_set_parameters(&_heading_p_controller, - _param_rdd_p_gain_heading.get(), // Proportional gain - 0, // Integral gain - 0, // Derivative gain - 0, // Integral limit - 200); // Output limit - - _forwards_velocity_smoothing.setMaxJerk(_param_rdd_max_jerk.get()); - _forwards_velocity_smoothing.setMaxAccel(_param_rdd_max_accel.get()); - _forwards_velocity_smoothing.setMaxVel(_max_speed); -} diff --git a/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp b/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp deleted file mode 100644 index 0a5a29bbca0b..000000000000 --- a/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp +++ /dev/null @@ -1,140 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2023-2024 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#pragma once - -#include - -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include -#include - -#include - - -/** - * @brief Enum class for the different states of guidance. - */ -enum class GuidanceState { - TURNING, ///< The vehicle is currently turning. - DRIVING, ///< The vehicle is currently driving straight. - GOAL_REACHED ///< The vehicle has reached its goal. -}; - -/** - * @brief Class for differential drive guidance. - */ -class DifferentialDriveGuidance : public ModuleParams -{ -public: - /** - * @brief Constructor for DifferentialDriveGuidance. - * @param parent The parent ModuleParams object. - */ - DifferentialDriveGuidance(ModuleParams *parent); - ~DifferentialDriveGuidance() = default; - - /** - * @brief Compute guidance for the vehicle. - * @param global_pos The global position of the vehicle in degrees. - * @param current_waypoint The current waypoint the vehicle is heading towards in degrees. - * @param next_waypoint The next waypoint the vehicle will head towards after reaching the current waypoint in degrees. - * @param vehicle_yaw The yaw orientation of the vehicle in radians. - * @param body_velocity The velocity of the vehicle in m/s. - * @param angular_velocity The angular velocity of the vehicle in rad/s. - * @param dt The time step in seconds. - */ - void computeGuidance(float yaw, float angular_velocity, float dt); - - /** - * @brief Set the maximum speed for the vehicle. - * @param max_speed The maximum speed in m/s. - * @return The set maximum speed in m/s. - */ - float setMaxSpeed(float max_speed) { return _max_speed = max_speed; } - - - /** - * @brief Set the maximum angular velocity for the vehicle. - * @param max_angular_velocity The maximum angular velocity in rad/s. - * @return The set maximum angular velocity in rad/s. - */ - float setMaxAngularVelocity(float max_angular_velocity) { return _max_angular_velocity = max_angular_velocity; } - -protected: - /** - * @brief Update the parameters of the module. - */ - void updateParams() override; - -private: - uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; - uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; - - uORB::Publication _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)}; - - position_setpoint_triplet_s _position_setpoint_triplet{}; - vehicle_global_position_s _vehicle_global_position{}; - - GuidanceState _currentState; ///< The current state of guidance. - - float _desired_angular_velocity; ///< The desired angular velocity. - - float _max_speed; ///< The maximum speed. - float _max_angular_velocity; ///< The maximum angular velocity. - - matrix::Vector2d _current_waypoint; ///< The current waypoint. - - VelocitySmoothing _forwards_velocity_smoothing; ///< The velocity smoothing for forward motion. - PositionSmoothing _position_smoothing; ///< The position smoothing. - - PID_t _heading_p_controller; ///< The PID controller for yaw rate. - - DEFINE_PARAMETERS( - (ParamFloat) _param_rdd_p_gain_heading, - (ParamFloat) _param_nav_acc_rad, - (ParamFloat) _param_rdd_max_jerk, - (ParamFloat) _param_rdd_max_accel - ) -}; diff --git a/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp b/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp deleted file mode 100644 index 42fcbea56b66..000000000000 --- a/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp +++ /dev/null @@ -1,97 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2023-2024 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include "DifferentialDriveKinematics.hpp" - -#include - -using namespace matrix; -using namespace time_literals; - -DifferentialDriveKinematics::DifferentialDriveKinematics(ModuleParams *parent) : ModuleParams(parent) -{} - -void DifferentialDriveKinematics::allocate() -{ - hrt_abstime now = hrt_absolute_time(); - - if (_differential_drive_control_output_sub.updated()) { - _differential_drive_control_output_sub.copy(&_differential_drive_control_output); - } - - const bool setpoint_timeout = (_differential_drive_control_output.timestamp + 100_ms) < now; - - Vector2f wheel_speeds = - computeInverseKinematics(_differential_drive_control_output.speed, _differential_drive_control_output.yaw_rate); - - if (!_armed || setpoint_timeout) { - wheel_speeds = {}; // stop - } - - wheel_speeds = matrix::constrain(wheel_speeds, -1.f, 1.f); - - actuator_motors_s actuator_motors{}; - actuator_motors.reversible_flags = _param_r_rev.get(); // should be 3 see rc.rover_differential_defaults - wheel_speeds.copyTo(actuator_motors.control); - actuator_motors.timestamp = now; - _actuator_motors_pub.publish(actuator_motors); -} - -matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float linear_velocity_x, float yaw_rate) const -{ - if (_max_speed < FLT_EPSILON) { - return Vector2f(); - } - - linear_velocity_x = math::constrain(linear_velocity_x, -_max_speed, _max_speed); - yaw_rate = math::constrain(yaw_rate, -_max_angular_velocity, _max_angular_velocity); - - const float rotational_velocity = (_wheel_base / 2.f) * yaw_rate; - float combined_velocity = fabsf(linear_velocity_x) + fabsf(rotational_velocity); - - // Compute an initial gain - float gain = 1.0f; - - if (combined_velocity > _max_speed) { - float excess_velocity = fabsf(combined_velocity - _max_speed); - const float adjusted_linear_velocity = fabsf(linear_velocity_x) - excess_velocity; - gain = adjusted_linear_velocity / fabsf(linear_velocity_x); - } - - // Apply the gain - linear_velocity_x *= gain; - - // Calculate the left and right wheel speeds - return Vector2f(linear_velocity_x - rotational_velocity, - linear_velocity_x + rotational_velocity) / _max_speed; -} diff --git a/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp b/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp deleted file mode 100644 index 524a5520a78f..000000000000 --- a/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp +++ /dev/null @@ -1,103 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2023-2024 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#pragma once - -#include -#include -#include -#include -#include -#include - -/** - * @brief Differential Drive Kinematics class for computing the kinematics of a differential drive robot. - * - * This class provides functions to set the wheel base and radius, and to compute the inverse kinematics - * given linear velocity and yaw rate. - */ -class DifferentialDriveKinematics : public ModuleParams -{ -public: - DifferentialDriveKinematics(ModuleParams *parent); - ~DifferentialDriveKinematics() = default; - - /** - * @brief Sets the wheel base of the robot. - * - * @param wheel_base The distance between the centers of the wheels. - */ - void setWheelBase(const float wheel_base) { _wheel_base = wheel_base; }; - - /** - * @brief Sets the maximum speed of the robot. - * - * @param max_speed The maximum speed of the robot. - */ - void setMaxSpeed(const float max_speed) { _max_speed = max_speed; }; - - /** - * @brief Sets the maximum angular speed of the robot. - * - * @param max_angular_speed The maximum angular speed of the robot. - */ - void setMaxAngularVelocity(const float max_angular_velocity) { _max_angular_velocity = max_angular_velocity; }; - - void setArmed(const bool armed) { _armed = armed; }; - - void allocate(); - - /** - * @brief Computes the inverse kinematics for differential drive. - * - * @param linear_velocity_x Linear velocity along the x-axis. - * @param yaw_rate Yaw rate of the robot. - * @return matrix::Vector2f Motor velocities for the right and left motors. - */ - matrix::Vector2f computeInverseKinematics(float linear_velocity_x, float yaw_rate) const; - -private: - uORB::Subscription _differential_drive_control_output_sub{ORB_ID(differential_drive_control_output)}; - uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; - - differential_drive_setpoint_s _differential_drive_control_output{}; - bool _armed = false; - - float _wheel_base{0.f}; - float _max_speed{0.f}; - float _max_angular_velocity{0.f}; - - DEFINE_PARAMETERS( - (ParamInt) _param_r_rev - ) -}; diff --git a/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp b/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp deleted file mode 100644 index b3c4935c1d9d..000000000000 --- a/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp +++ /dev/null @@ -1,168 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2023-2024 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include -#include "DifferentialDriveKinematics.hpp" -#include - -using namespace matrix; - -class DifferentialDriveKinematicsTest : public ::testing::Test -{ -public: - DifferentialDriveKinematics kinematics{nullptr}; -}; - -TEST_F(DifferentialDriveKinematicsTest, AllZeroInputCase) -{ - kinematics.setWheelBase(1.f); - kinematics.setMaxSpeed(10.f); - kinematics.setMaxAngularVelocity(10.f); - - // Test with zero linear velocity and zero yaw rate (stationary vehicle) - EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 0.f), Vector2f()); -} - - -TEST_F(DifferentialDriveKinematicsTest, InvalidParameterCase) -{ - kinematics.setWheelBase(0.f); - kinematics.setMaxSpeed(10.f); - kinematics.setMaxAngularVelocity(10.f); - - // Test with invalid parameters (zero wheel base and wheel radius) - EXPECT_EQ(kinematics.computeInverseKinematics(0.f, .1f), Vector2f()); -} - - -TEST_F(DifferentialDriveKinematicsTest, UnitCase) -{ - kinematics.setWheelBase(1.f); - kinematics.setMaxSpeed(10.f); - kinematics.setMaxAngularVelocity(10.f); - - // Test with unit values for linear velocity and yaw rate - EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 1.f), Vector2f(0.05f, 0.15f)); -} - - -TEST_F(DifferentialDriveKinematicsTest, UnitSaturationCase) -{ - kinematics.setWheelBase(1.f); - kinematics.setMaxSpeed(1.f); - kinematics.setMaxAngularVelocity(1.f); - - // Test with unit values for linear velocity and yaw rate, but with max speed that requires saturation - EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 1.f), Vector2f(0, 1)); -} - - -TEST_F(DifferentialDriveKinematicsTest, OppositeUnitSaturationCase) -{ - kinematics.setWheelBase(1.f); - kinematics.setMaxSpeed(1.f); - kinematics.setMaxAngularVelocity(1.f); - - // Negative linear velocity for backward motion and positive yaw rate for turning right - EXPECT_EQ(kinematics.computeInverseKinematics(-1.f, 1.f), Vector2f(-1, 0)); -} - -TEST_F(DifferentialDriveKinematicsTest, RandomCase) -{ - kinematics.setWheelBase(2.f); - kinematics.setMaxSpeed(1.f); - kinematics.setMaxAngularVelocity(1.f); - - // Negative linear velocity for backward motion and positive yaw rate for turning right - EXPECT_EQ(kinematics.computeInverseKinematics(0.5f, 0.7f), Vector2f(-0.4f, 1.0f)); -} - -TEST_F(DifferentialDriveKinematicsTest, RotateInPlaceCase) -{ - kinematics.setWheelBase(1.f); - kinematics.setMaxSpeed(1.f); - kinematics.setMaxAngularVelocity(1.f); - - // Test rotating in place (zero linear velocity, non-zero yaw rate) - EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 1.f), Vector2f(-0.5f, 0.5f)); -} - -TEST_F(DifferentialDriveKinematicsTest, StraightMovementCase) -{ - kinematics.setWheelBase(1.f); - kinematics.setMaxSpeed(1.f); - kinematics.setMaxAngularVelocity(1.f); - - // Test moving straight (non-zero linear velocity, zero yaw rate) - EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 0.f), Vector2f(1.f, 1.f)); -} - -TEST_F(DifferentialDriveKinematicsTest, MinInputValuesCase) -{ - kinematics.setWheelBase(FLT_MIN); - kinematics.setMaxSpeed(FLT_MIN); - kinematics.setMaxAngularVelocity(FLT_MIN); - - // Test with minimum possible input values - EXPECT_EQ(kinematics.computeInverseKinematics(FLT_MIN, FLT_MIN), Vector2f(0.f, 0.f)); -} - -TEST_F(DifferentialDriveKinematicsTest, MaxSpeedLimitCase) -{ - kinematics.setWheelBase(1.f); - kinematics.setMaxSpeed(1.f); - kinematics.setMaxAngularVelocity(1.f); - - // Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed - EXPECT_EQ(kinematics.computeInverseKinematics(10.f, 10.f), Vector2f(0.f, 1.f)); -} - -TEST_F(DifferentialDriveKinematicsTest, MaxSpeedForwardsCase) -{ - kinematics.setWheelBase(1.f); - kinematics.setMaxSpeed(1.f); - kinematics.setMaxAngularVelocity(1.f); - - // Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed - EXPECT_EQ(kinematics.computeInverseKinematics(10.f, 0.f), Vector2f(1.f, 1.f)); -} - -TEST_F(DifferentialDriveKinematicsTest, MaxAngularCase) -{ - kinematics.setWheelBase(2.f); - kinematics.setMaxSpeed(1.f); - kinematics.setMaxAngularVelocity(1.f); - - // Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed - EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 10.f), Vector2f(-1.f, 1.f)); -} diff --git a/src/modules/differential_drive/Kconfig b/src/modules/differential_drive/Kconfig deleted file mode 100644 index a95897e91f99..000000000000 --- a/src/modules/differential_drive/Kconfig +++ /dev/null @@ -1,6 +0,0 @@ -menuconfig MODULES_DIFFERENTIAL_DRIVE - bool "differential_drive" - default n - depends on MODULES_CONTROL_ALLOCATOR - ---help--- - Enable support for control of differential drive rovers diff --git a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp index 264c826738f2..dc6d36bcb5c7 100644 --- a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp @@ -126,7 +126,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max); - if (isHeightResetRequired()) { + if (isHeightResetRequired() && (_height_sensor_ref == HeightSensor::BARO)) { // All height sources are failing ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME); @@ -142,10 +142,13 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) aid_src.time_last_fuse = imu_sample.time_us; } else if (is_fusion_failing) { - // Some other height source is still working ECL_WARN("stopping %s height fusion, fusion failing", HGT_SRC_NAME); stopBaroHgtFusion(); - _baro_hgt_faulty = true; + + if (isRecent(_time_last_hgt_fuse, _params.hgt_fusion_timeout_max)) { + // Some other height source is still working + _baro_hgt_faulty = true; + } } } else { diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp index e7b6f13f1181..440c0fe7acf6 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp @@ -142,7 +142,7 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max); - if (isHeightResetRequired() && quality_sufficient) { + if (isHeightResetRequired() && quality_sufficient && (_height_sensor_ref == HeightSensor::EV)) { // All height sources are failing ECL_WARN("%s fusion reset required, all height sources failing", AID_SRC_NAME); _information_events.flags.reset_hgt_to_ev = true; diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp index c141d415eec5..91a8685cebae 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp @@ -100,12 +100,12 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample) const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max); - if (isHeightResetRequired()) { + if (isHeightResetRequired() && (_height_sensor_ref == HeightSensor::GNSS)) { // All height sources are failing ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME); _information_events.flags.reset_hgt_to_gps = true; - resetVerticalPositionTo(-(measurement - bias_est.getBias()), measurement_var); + resetVerticalPositionTo(aid_src.observation, measurement_var); bias_est.setBias(_state.pos(2) + measurement); aid_src.time_last_fuse = _time_delayed_us; diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp index da7fc4e16060..94fa6f04f381 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp @@ -98,37 +98,7 @@ void Ekf::collect_gps(const gnssSample &gps) const bool gps_rough_2d_fix = (gps.fix_type >= 2) && (gps.hacc < 1000); if (gps_rough_2d_fix && (_gps_checks_passed || !_NED_origin_initialised)) { - - // If we have good GPS data set the origin's WGS-84 position to the last gps fix -#if defined(CONFIG_EKF2_MAGNETOMETER) - - // set the magnetic field data returned by the geo library using the current GPS position - const float mag_declination_gps = math::radians(get_mag_declination_degrees(gps.lat, gps.lon)); - const float mag_inclination_gps = math::radians(get_mag_inclination_degrees(gps.lat, gps.lon)); - const float mag_strength_gps = get_mag_strength_gauss(gps.lat, gps.lon); - - if (PX4_ISFINITE(mag_declination_gps) && PX4_ISFINITE(mag_inclination_gps) && PX4_ISFINITE(mag_strength_gps)) { - - const bool mag_declination_changed = (fabsf(mag_declination_gps - _mag_declination_gps) > math::radians(1.f)); - const bool mag_inclination_changed = (fabsf(mag_inclination_gps - _mag_inclination_gps) > math::radians(1.f)); - - if ((_wmm_gps_time_last_set == 0) - || !PX4_ISFINITE(_mag_declination_gps) - || !PX4_ISFINITE(_mag_inclination_gps) - || !PX4_ISFINITE(_mag_strength_gps) - || mag_declination_changed - || mag_inclination_changed - ) { - _mag_declination_gps = mag_declination_gps; - _mag_inclination_gps = mag_inclination_gps; - _mag_strength_gps = mag_strength_gps; - - _wmm_gps_time_last_set = _time_delayed_us; - } - } - -#endif // CONFIG_EKF2_MAGNETOMETER - + updateWmm(gps.lat, gps.lon); _earth_rate_NED = calcEarthRateNED((float)math::radians(gps.lat)); } diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp index 6c33d4db639a..0aafe8a1b00d 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp @@ -173,7 +173,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max); - if (isHeightResetRequired() && _control_status.flags.rng_hgt) { + if (isHeightResetRequired() && _control_status.flags.rng_hgt && (_height_sensor_ref == HeightSensor::RANGE)) { // All height sources are failing ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index f3f055434e62..a7c43567df52 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -259,6 +259,7 @@ class Ekf final : public EstimatorInterface // return true if the origin is valid bool getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const; bool setEkfGlobalOrigin(double latitude, double longitude, float altitude, float eph = 0.f, float epv = 0.f); + void updateWmm(double lat, double lon); // get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const; diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index d88f7c400d92..ae3e9e355e0a 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -96,20 +96,7 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons _pos_ref.initReference(latitude, longitude, _time_delayed_us); _gps_alt_ref = altitude; -#if defined(CONFIG_EKF2_MAGNETOMETER) - const float mag_declination_gps = math::radians(get_mag_declination_degrees(latitude, longitude)); - const float mag_inclination_gps = math::radians(get_mag_inclination_degrees(latitude, longitude)); - const float mag_strength_gps = get_mag_strength_gauss(latitude, longitude); - - if (PX4_ISFINITE(mag_declination_gps) && PX4_ISFINITE(mag_inclination_gps) && PX4_ISFINITE(mag_strength_gps)) { - _mag_declination_gps = mag_declination_gps; - _mag_inclination_gps = mag_inclination_gps; - _mag_strength_gps = mag_strength_gps; - - _wmm_gps_time_last_set = _time_delayed_us; - } - -#endif // CONFIG_EKF2_MAGNETOMETER + updateWmm(current_lat, current_lon); _gpos_origin_eph = eph; _gpos_origin_epv = epv; @@ -144,6 +131,39 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons return false; } +void Ekf::updateWmm(const double lat, const double lon) +{ +#if defined(CONFIG_EKF2_MAGNETOMETER) + + // set the magnetic field data returned by the geo library using the current GPS position + const float mag_declination_gps = math::radians(get_mag_declination_degrees(lat, lon)); + const float mag_inclination_gps = math::radians(get_mag_inclination_degrees(lat, lon)); + const float mag_strength_gps = get_mag_strength_gauss(lat, lon); + + if (PX4_ISFINITE(mag_declination_gps) && PX4_ISFINITE(mag_inclination_gps) && PX4_ISFINITE(mag_strength_gps)) { + + const bool mag_declination_changed = (fabsf(mag_declination_gps - _mag_declination_gps) > math::radians(1.f)); + const bool mag_inclination_changed = (fabsf(mag_inclination_gps - _mag_inclination_gps) > math::radians(1.f)); + + if ((_wmm_gps_time_last_set == 0) + || !PX4_ISFINITE(_mag_declination_gps) + || !PX4_ISFINITE(_mag_inclination_gps) + || !PX4_ISFINITE(_mag_strength_gps) + || mag_declination_changed + || mag_inclination_changed + ) { + _mag_declination_gps = mag_declination_gps; + _mag_inclination_gps = mag_inclination_gps; + _mag_strength_gps = mag_strength_gps; + + _wmm_gps_time_last_set = _time_delayed_us; + } + } + +#endif // CONFIG_EKF2_MAGNETOMETER +} + + void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const { float eph = INFINITY; diff --git a/src/modules/ekf2/EKF/python/tuning_tools/gyro_integration.py b/src/modules/ekf2/EKF/python/tuning_tools/gyro_integration.py new file mode 100644 index 000000000000..457c1eb8e9bd --- /dev/null +++ b/src/modules/ekf2/EKF/python/tuning_tools/gyro_integration.py @@ -0,0 +1,158 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +""" + Copyright (c) 2024 PX4 Development Team + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions + are met: + + 1. Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + 2. Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in + the documentation and/or other materials provided with the + distribution. + 3. Neither the name PX4 nor the names of its contributors may be + used to endorse or promote products derived from this software + without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + POSSIBILITY OF SUCH DAMAGE. +""" + +import matplotlib.pylab as plt +from pyulog import ULog +from pyulog.px4 import PX4ULog +import numpy as np +import sym +import symforce.symbolic as sf + +def getAllData(logfile): + log = ULog(logfile) + + gyro = np.matrix([getData(log, 'sensor_combined', 'gyro_rad[0]'), + getData(log, 'sensor_combined', 'gyro_rad[1]'), + getData(log, 'sensor_combined', 'gyro_rad[2]')]) + t_gyro = getTimestampsSeconds(log, 'sensor_combined') + + t_gyro -= t_gyro[0] + + q = np.matrix([getData(log, 'vehicle_attitude', 'q[0]'), + getData(log, 'vehicle_attitude', 'q[1]'), + getData(log, 'vehicle_attitude', 'q[2]'), + getData(log, 'vehicle_attitude', 'q[3]')]) + t_q = getTimestampsSeconds(log, 'vehicle_attitude') + t_q -= t_q[0] + + return (t_gyro, gyro, t_q, q) + +def getData(log, topic_name, variable_name, instance=0): + variable_data = np.array([]) + for elem in log.data_list: + if elem.name == topic_name: + if instance == elem.multi_id: + variable_data = elem.data[variable_name] + break + + return variable_data + +def us2s(time_us): + return time_us * 1e-6 + +def getTimestampsSeconds(log, topic_name, instance=0): + return us2s(getData(log, topic_name, 'timestamp', instance)) + +def integrateAngularRate(t, angular_rate, rot_init=sym.Rot3()): + R = rot_init + roll = [] + pitch = [] + yaw = [] + t_prev = 0 + + for i in range(len(t)): + dt = t[i] - t_prev + R = R * sym.Rot3.from_tangent(angular_rate[:, i] * dt) + att = R.to_yaw_pitch_roll() + yaw = np.append(yaw, att[0]) + pitch = np.append(pitch, att[1]) + roll = np.append(roll, att[2]) + + t_prev = t[i] + + return (roll, pitch, yaw) + +def quat2RollPitchYaw(t, q): + roll = [] + pitch = [] + yaw = [] + + for i in range(len(t)): + vect = sf.V3(float(q[1, i]), float(q[2, i]), float(q[3, i])) + quat = sf.Quaternion(w=q[0, i], xyz=vect) + R = sf.Rot3(quat) + att = R.to_yaw_pitch_roll() + yaw = np.append(yaw, float(att[0].evalf())) + pitch = np.append(pitch, float(att[1].evalf())) + roll = np.append(roll, float(att[2].evalf())) + + return (roll, pitch, yaw) + + +def run(logfile): + (t, gyro, t_q, q) = getAllData(logfile) + + (roll, pitch, yaw) = quat2RollPitchYaw(t_q, q) + (roll_raw, pitch_raw, yaw_raw) = integrateAngularRate(t, gyro, rot_init=sym.Rot3.from_yaw_pitch_roll(yaw[0], pitch[0], roll[0])) + + # Plot data + plt.figure(1) + plt.suptitle(logfile.split('/')[-1]) + + ax1 = plt.subplot(3, 1, 1) + ax1.plot(t_q, np.rad2deg(roll), '-') + ax1.plot(t, np.rad2deg(roll_raw), '--') + ax1.set_ylabel("roll (deg)") + ax1.legend(["estimated", "integrated"]) + ax1.grid() + + ax2 = plt.subplot(3, 1, 2, sharex=ax1) + ax2.plot(t_q, np.rad2deg(pitch)) + ax2.plot(t, np.rad2deg(pitch_raw), '--') + ax2.set_ylabel("pitch (deg)") + ax2.legend(["estimated", "integrated"]) + ax2.grid() + + ax3 = plt.subplot(3, 1, 3, sharex=ax1) + ax3.plot(t_q, np.rad2deg(yaw)) + ax3.plot(t, np.rad2deg(yaw_raw), '--') + ax3.set_xlabel("time (s)") + ax3.set_ylabel("yaw (deg)") + ax3.legend(["estimated", "integrated"]) + ax3.grid() + plt.show() + +if __name__ == '__main__': + import os + import argparse + + script_path = os.path.split(os.path.realpath(__file__))[0] + + parser = argparse.ArgumentParser( + description='Integrate angular velocity to attitude and compare it with attitude estimate') + + parser.add_argument('logfile', help='Full ulog file path, name and extension', type=str) + args = parser.parse_args() + + logfile = os.path.abspath(args.logfile) + + run(logfile) diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index bceb4d3dcc23..af2eb6615f40 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -2076,14 +2076,19 @@ void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps) if (_airspeed_validated_sub.update(&airspeed_validated)) { if (PX4_ISFINITE(airspeed_validated.true_airspeed_m_s) - && PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s) - && (airspeed_validated.calibrated_airspeed_m_s > 0.f) && (airspeed_validated.selected_airspeed_index > 0) ) { + float cas2tas = 1.f; + + if (PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s) + && (airspeed_validated.calibrated_airspeed_m_s > FLT_EPSILON)) { + cas2tas = airspeed_validated.true_airspeed_m_s / airspeed_validated.calibrated_airspeed_m_s; + } + airspeedSample airspeed_sample { .time_us = airspeed_validated.timestamp, .true_airspeed = airspeed_validated.true_airspeed_m_s, - .eas2tas = airspeed_validated.true_airspeed_m_s / airspeed_validated.calibrated_airspeed_m_s, + .eas2tas = cas2tas, }; _ekf.setAirspeedData(airspeed_sample); } diff --git a/src/modules/ekf2/test/sensor_simulator/gps.h b/src/modules/ekf2/test/sensor_simulator/gps.h index f962cc9bced7..e8ff206916f7 100644 --- a/src/modules/ekf2/test/sensor_simulator/gps.h +++ b/src/modules/ekf2/test/sensor_simulator/gps.h @@ -66,6 +66,7 @@ class Gps: public Sensor void setPdop(const float pdop); gnssSample getDefaultGpsData(); + const gnssSample &getData() const { return _gps_data; } private: void send(uint64_t time) override; diff --git a/src/modules/ekf2/test/test_EKF_height_fusion.cpp b/src/modules/ekf2/test/test_EKF_height_fusion.cpp index c80939f1ba6e..b322a2818245 100644 --- a/src/modules/ekf2/test/test_EKF_height_fusion.cpp +++ b/src/modules/ekf2/test/test_EKF_height_fusion.cpp @@ -261,6 +261,93 @@ TEST_F(EkfHeightFusionTest, gpsRefFailOver) EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::UNKNOWN); } +TEST_F(EkfHeightFusionTest, gpsRefAllHgtFailReset) +{ + // GIVEN: EKF that fuses GNSS (reference) and baro + _sensor_simulator.startBaro(); + _sensor_simulator.startGps(); + _ekf_wrapper.setGpsHeightRef(); + _ekf_wrapper.enableBaroHeightFusion(); + _ekf_wrapper.enableGpsHeightFusion(); + + _sensor_simulator.runSeconds(11); + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion()); + EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::GNSS); + + const Vector3f previous_position = _ekf->getPosition(); + + ResetLoggingChecker reset_logging_checker(_ekf); + reset_logging_checker.capturePreResetState(); + + // WHEN: + const float gnss_height_step = 10.f; + _sensor_simulator._gps.stepHeightByMeters(gnss_height_step); + + const float baro_height_step = 5.f; + _sensor_simulator._baro.setData(_sensor_simulator._baro.getData() + baro_height_step); + _sensor_simulator.runSeconds(15); + + // THEN: then the fusion of both sensors starts to fail and the height is reset to the + // reference sensor (GNSS) + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion()); + + const Vector3f new_position = _ekf->getPosition(); + EXPECT_NEAR(new_position(2), previous_position(2) - gnss_height_step, 0.2f); + + // Also check the reset counters to make sure the reset logic triggered + reset_logging_checker.capturePostResetState(); + EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(1)); + EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(1)); +} + +TEST_F(EkfHeightFusionTest, baroRefAllHgtFailReset) +{ + // GIVEN: EKF that fuses GNSS and baro (reference) + _sensor_simulator.startBaro(); + _sensor_simulator.startGps(); + _ekf_wrapper.setBaroHeightRef(); + _ekf_wrapper.enableBaroHeightFusion(); + _ekf_wrapper.enableGpsHeightFusion(); + + _sensor_simulator.runSeconds(11); + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion()); + EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::BARO); + + const Vector3f previous_position = _ekf->getPosition(); + + ResetLoggingChecker reset_logging_checker(_ekf); + reset_logging_checker.capturePreResetState(); + + // WHEN: + const float gnss_height_step = 10.f; + _sensor_simulator._gps.stepHeightByMeters(gnss_height_step); + + const float baro_height_step = 5.f; + _sensor_simulator._baro.setData(_sensor_simulator._baro.getData() + baro_height_step); + _sensor_simulator.runSeconds(20); + + // THEN: then the fusion of both sensors starts to fail and the height is reset to the + // reference sensor (baro) + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion()); + + const Vector3f new_position = _ekf->getPosition(); + EXPECT_NEAR(new_position(2), previous_position(2) - baro_height_step, 0.2f); + + // Also check the reset counters to make sure the reset logic triggered + reset_logging_checker.capturePostResetState(); + + // The velocity does not reset as baro only provides height measurement + EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(0)); + + // The height resets twice in a row as the baro innovation is not corrected after a height + // reset and triggers a new reset at the next iteration + EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(2)); +} + TEST_F(EkfHeightFusionTest, changeEkfOriginAlt) { _sensor_simulator.startBaro(); diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index dad05e3c31db..b2bb5632b7bf 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -452,7 +452,7 @@ bool FlightTaskAuto::_evaluateTriplets() _triplet_prev_wp(2) = -(_sub_triplet_setpoint.get().previous.alt - _reference_altitude); } else { - _triplet_prev_wp = _position; + _triplet_prev_wp = _triplet_target; } _prev_was_valid = _sub_triplet_setpoint.get().previous.valid; diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp index f639e492493d..144c6c7ba158 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp @@ -90,7 +90,14 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command, b // commanded heading behaviour if (PX4_ISFINITE(command.param3)) { - _yaw_behaviour = command.param3; + if (static_cast(command.param3 + .5f) == vehicle_command_s::ORBIT_YAW_BEHAVIOUR_UNCHANGED) { + if (!_currently_orbiting) { // only change the yaw behaviour if we are not actively orbiting + _yaw_behaviour = _param_mc_orbit_yaw_mod.get(); + } + + } else { + _yaw_behaviour = command.param3; + } } // save current yaw estimate for ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING @@ -133,9 +140,11 @@ bool FlightTaskOrbit::sendTelemetry() orbit_status.yaw_behaviour = _yaw_behaviour; if (_geo_projection.isInitialized()) { + // While chainging altitude by stick _position_setpoint(2) is not set (NAN) + float local_altitude = PX4_ISFINITE(_position_setpoint(2)) ? _position_setpoint(2) : _position(2); // local -> global _geo_projection.reproject(_center(0), _center(1), orbit_status.x, orbit_status.y); - orbit_status.z = _global_local_alt0 - _position_setpoint(2); + orbit_status.z = _global_local_alt0 - local_altitude; } else { return false; // don't send the message if the transformation failed @@ -165,6 +174,7 @@ void FlightTaskOrbit::_sanitizeParams(float &radius, float &velocity) const bool FlightTaskOrbit::activate(const trajectory_setpoint_s &last_setpoint) { bool ret = FlightTaskManualAltitude::activate(last_setpoint); + _currently_orbiting = false; _orbit_radius = _radius_min; _orbit_velocity = 1.f; _center = _position; @@ -199,6 +209,7 @@ bool FlightTaskOrbit::activate(const trajectory_setpoint_s &last_setpoint) bool FlightTaskOrbit::update() { bool ret = true; + _currently_orbiting = true; _updateTrajectoryBoundaries(); _adjustParametersByStick(); diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp index 187a47de4119..2c55bbc15ba7 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp +++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp @@ -124,6 +124,7 @@ class FlightTaskOrbit : public FlightTaskManualAltitudeSmoothVel /** yaw behaviour during the orbit flight according to MAVLink's ORBIT_YAW_BEHAVIOUR enum */ int _yaw_behaviour = orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER; bool _started_clockwise{true}; + bool _currently_orbiting{false}; float _initial_heading = 0.f; /**< the heading of the drone when the orbit command was issued */ SlewRateYaw _slew_rate_yaw; @@ -132,6 +133,7 @@ class FlightTaskOrbit : public FlightTaskManualAltitudeSmoothVel DEFINE_PARAMETERS( (ParamFloat) _param_mc_orbit_rad_max, + (ParamInt) _param_mc_orbit_yaw_mod, (ParamFloat) _param_mpc_xy_cruise, /**< cruise speed for circle approach */ (ParamFloat) _param_mpc_yawrauto_max, (ParamFloat) _param_mpc_xy_traj_p, diff --git a/src/modules/flight_mode_manager/tasks/Orbit/flight_task_orbit_params.c b/src/modules/flight_mode_manager/tasks/Orbit/flight_task_orbit_params.c index 3e655c80e2ed..c8f66eb9f15d 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/flight_task_orbit_params.c +++ b/src/modules/flight_mode_manager/tasks/Orbit/flight_task_orbit_params.c @@ -39,6 +39,18 @@ * @max 10000.0 * @increment 0.5 * @decimal 1 - * @group FlightTaskOrbit + * @group Flight Task Orbit */ PARAM_DEFINE_FLOAT(MC_ORBIT_RAD_MAX, 1000.0f); + +/** + * Yaw behaviour during orbit flight. + * + * @value 0 Front to Circle Center + * @value 1 Hold Initial Heading + * @value 2 Uncontrolled + * @value 3 Hold Front Tangent to Circle + * @value 4 RC Controlled + * @group Flight Task Orbit + */ +PARAM_DEFINE_INT32(MC_ORBIT_YAW_MOD, 0); diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index b08fc895ab9c..988853fdeaaf 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -96,17 +96,16 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body) // STABILIZED mode generate the attitude setpoint from manual user inputs - _att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_man_r_max.get()); + const float roll_body = _manual_control_setpoint.roll * radians(_param_fw_man_r_max.get()); - _att_sp.pitch_body = -_manual_control_setpoint.pitch * radians(_param_fw_man_p_max.get()) - + radians(_param_fw_psp_off.get()); - _att_sp.pitch_body = constrain(_att_sp.pitch_body, - -radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get())); + float pitch_body = -_manual_control_setpoint.pitch * radians(_param_fw_man_p_max.get()) + + radians(_param_fw_psp_off.get()); + pitch_body = constrain(pitch_body, + -radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get())); - _att_sp.yaw_body = yaw_body; // yaw is not controlled, so set setpoint to current yaw _att_sp.thrust_body[0] = (_manual_control_setpoint.throttle + 1.f) * .5f; - Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); + const Quatf q(Eulerf(roll_body, pitch_body, yaw_body)); q.copyTo(_att_sp.q_d); _att_sp.reset_integral = false; @@ -325,16 +324,22 @@ void FixedwingAttitudeControl::Run() /* Run attitude controllers */ if (_vcontrol_mode.flag_control_attitude_enabled && _in_fw_or_transition_wo_tailsitter_transition) { - if (PX4_ISFINITE(_att_sp.roll_body) && PX4_ISFINITE(_att_sp.pitch_body)) { - _roll_ctrl.control_roll(_att_sp.roll_body, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), + const Eulerf setpoint(Quatf(_att_sp.q_d)); + const float roll_body = setpoint.phi(); + const float pitch_body = setpoint.theta(); + + if (PX4_ISFINITE(roll_body) && PX4_ISFINITE(pitch_body)) { + + _roll_ctrl.control_roll(roll_body, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), euler_angles.theta()); - _pitch_ctrl.control_pitch(_att_sp.pitch_body, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), + _pitch_ctrl.control_pitch(pitch_body, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), euler_angles.theta()); - _yaw_ctrl.control_yaw(_att_sp.roll_body, _pitch_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), + _yaw_ctrl.control_yaw(roll_body, _pitch_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), euler_angles.theta(), get_airspeed_constrained()); if (wheel_control) { - _wheel_ctrl.control_attitude(_att_sp.yaw_body, euler_angles.psi()); + Eulerf attitude_setpoint(Quatf(_att_sp.q_d)); + _wheel_ctrl.control_attitude(attitude_setpoint.psi(), euler_angles.psi()); } else { _wheel_ctrl.reset_integrator(); diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index f636b0e013fd..7858fe2c1c5f 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -212,8 +212,7 @@ FixedwingPositionControl::airspeed_poll() _eas2tas = 1.0f; //this is the default value, taken in case of invalid airspeed if (PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s) - && PX4_ISFINITE(airspeed_validated.true_airspeed_m_s) - && (airspeed_validated.calibrated_airspeed_m_s > FLT_EPSILON)) { + && PX4_ISFINITE(airspeed_validated.true_airspeed_m_s)) { airspeed_valid = true; @@ -451,9 +450,6 @@ FixedwingPositionControl::status_publish() { position_controller_status_s pos_ctrl_status = {}; - pos_ctrl_status.nav_roll = _att_sp.roll_body; - pos_ctrl_status.nav_pitch = _att_sp.pitch_body; - npfg_status_s npfg_status = {}; npfg_status.wind_est_valid = _wind_valid; @@ -791,8 +787,11 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now) /* reset setpoints from other modes (auto) otherwise we won't * level out without new manual input */ - _att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + float roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); + float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + const Eulerf current_setpoint(Quatf(_att_sp.q_d)); + const Quatf setpoint(Eulerf(roll_body, current_setpoint.theta(), yaw_body)); + setpoint.copyTo(_att_sp.q_d); } _control_mode_current = FW_POSCTRL_MODE_MANUAL_POSITION; @@ -877,11 +876,16 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto } switch (position_sp_type) { - case position_setpoint_s::SETPOINT_TYPE_IDLE: - _att_sp.thrust_body[0] = 0.0f; - _att_sp.roll_body = 0.0f; - _att_sp.pitch_body = radians(_param_fw_psp_off.get()); - break; + case position_setpoint_s::SETPOINT_TYPE_IDLE: { + _att_sp.thrust_body[0] = 0.0f; + const float roll_body = 0.0f; + const float pitch_body = radians(_param_fw_psp_off.get()); + const float yaw_body = 0.0f; + + const Quatf setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + setpoint.copyTo(_att_sp.q_d); + break; + } case position_setpoint_s::SETPOINT_TYPE_POSITION: control_auto_position(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp); @@ -927,9 +931,6 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto _att_sp.thrust_body[0] = (_landed) ? min(_param_fw_thr_idle.get(), 1.f) : get_tecs_thrust(); } - /* Copy thrust and pitch values from tecs */ - _att_sp.pitch_body = get_tecs_pitch(); - if (!_vehicle_status.in_transition_to_fw) { publishLocalPositionSetpoint(current_sp); } @@ -950,8 +951,8 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_i _param_sinkrate_target.get(), _param_climbrate_target.get()); - _att_sp.roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle - _att_sp.yaw_body = 0.f; + const float roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle + const float yaw_body = 0.f; if (_landed) { _att_sp.thrust_body[0] = _param_fw_thr_min.get(); @@ -960,7 +961,9 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_i _att_sp.thrust_body[0] = min(get_tecs_thrust(), _param_fw_thr_max.get()); } - _att_sp.pitch_body = get_tecs_pitch(); + const float pitch_body = get_tecs_pitch(); + const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } @@ -970,6 +973,7 @@ FixedwingPositionControl::control_auto_descend(const float control_interval) // Hard-code descend rate to 0.5m/s. This is a compromise to give the system to recover, // but not letting it drift too far away. const float descend_rate = -0.5f; + const bool disable_underspeed_handling = false; tecs_update_pitch_throttle(control_interval, _current_altitude, @@ -980,14 +984,16 @@ FixedwingPositionControl::control_auto_descend(const float control_interval) _param_fw_thr_max.get(), _param_sinkrate_target.get(), _param_climbrate_target.get(), - false, + disable_underspeed_handling, descend_rate); - _att_sp.roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle - _att_sp.yaw_body = 0.f; + const float roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle + const float yaw_body = 0.f; _att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_min.get() : min(get_tecs_thrust(), _param_fw_thr_max.get()); - _att_sp.pitch_body = get_tecs_pitch(); + const float pitch_body = get_tecs_pitch(); + const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } uint8_t @@ -1109,10 +1115,10 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co navigateWaypoint(curr_wp_local, curr_pos_local, ground_speed, _wind_vel); } - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + float roll_body = getCorrectedNpfgRollSetpoint(); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw tecs_update_pitch_throttle(control_interval, position_sp_alt, @@ -1123,6 +1129,9 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co tecs_fw_thr_max, _param_sinkrate_target.get(), _param_climbrate_target.get()); + const float pitch_body = get_tecs_pitch(); + const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } void @@ -1158,10 +1167,11 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co _npfg.setAirspeedNom(target_airspeed * _eas2tas); _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); navigateBearing(curr_pos_local, _target_bearing, ground_speed, _wind_vel); - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + float roll_body = getCorrectedNpfgRollSetpoint(); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.yaw_body = _yaw; + float yaw_body = _yaw; + const bool disable_underspeed_handling = false; tecs_update_pitch_throttle(control_interval, position_sp_alt, @@ -1172,8 +1182,12 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co tecs_fw_thr_max, _param_sinkrate_target.get(), _param_climbrate_target.get(), - false, + disable_underspeed_handling, pos_sp_curr.vz); + const float pitch_body = get_tecs_pitch(); + + const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } void @@ -1253,10 +1267,10 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons navigateLoiter(curr_wp_local, curr_pos_local, loiter_radius, pos_sp_curr.loiter_direction_counter_clockwise, ground_speed, _wind_vel); - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + float roll_body = getCorrectedNpfgRollSetpoint(); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw float alt_sp = pos_sp_curr.alt; @@ -1267,7 +1281,7 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons } else { // continue straight until vehicle has sufficient altitude - _att_sp.roll_body = 0.0f; + roll_body = 0.0f; } _tecs.set_altitude_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get()); @@ -1282,6 +1296,11 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons tecs_fw_thr_max, _param_sinkrate_target.get(), _param_climbrate_target.get()); + + const float pitch_body = get_tecs_pitch(); + + const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } #ifdef CONFIG_FIGURE_OF_EIGHT @@ -1306,7 +1325,7 @@ FixedwingPositionControl::controlAutoFigureEight(const float control_interval, c // Apply control _figure_eight.updateSetpoint(curr_pos_local, ground_speed, params, target_airspeed); - _att_sp.roll_body = _figure_eight.getRollSetpoint(); + float roll_body = _figure_eight.getRollSetpoint(); target_airspeed = _figure_eight.getAirspeedSetpoint(); _target_bearing = _figure_eight.getTargetBearing(); _closest_point_on_path = _figure_eight.getClosestPoint(); @@ -1337,7 +1356,11 @@ FixedwingPositionControl::controlAutoFigureEight(const float control_interval, c _param_climbrate_target.get()); // Yaw - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + const float pitch_body = get_tecs_pitch(); + + const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } void FixedwingPositionControl::publishFigureEightStatus(const position_setpoint_s pos_sp) @@ -1392,10 +1415,10 @@ FixedwingPositionControl::control_auto_path(const float control_interval, const 0.0f; navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(), ground_speed, _wind_vel, curvature); - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + float roll_body = getCorrectedNpfgRollSetpoint(); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw tecs_update_pitch_throttle(control_interval, pos_sp_curr.alt, @@ -1408,7 +1431,10 @@ FixedwingPositionControl::control_auto_path(const float control_interval, const _param_climbrate_target.get()); _att_sp.thrust_body[0] = min(get_tecs_thrust(), tecs_fw_thr_max); - _att_sp.pitch_body = get_tecs_pitch(); + const float pitch_body = get_tecs_pitch(); + + const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } void @@ -1494,7 +1520,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); navigateLine(start_pos_local, takeoff_bearing, local_2D_position, ground_speed, _wind_vel); - _att_sp.roll_body = _runway_takeoff.getRoll(getCorrectedNpfgRollSetpoint()); + float roll_body = _runway_takeoff.getRoll(getCorrectedNpfgRollSetpoint()); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; @@ -1502,7 +1528,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo const float bearing = _npfg.getBearing(); // heading hold mode will override this bearing setpoint - _att_sp.yaw_body = _runway_takeoff.getYaw(bearing); + float yaw_body = _runway_takeoff.getYaw(bearing); // update tecs const float pitch_max = _runway_takeoff.getMaxPitch(math::radians(_param_fw_p_lim_max.get())); @@ -1517,6 +1543,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _tecs.resetIntegrals(); } + const bool disable_underspeed_handling = true; + tecs_update_pitch_throttle(control_interval, altitude_setpoint_amsl, target_airspeed, @@ -1525,13 +1553,19 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _param_fw_thr_min.get(), _param_fw_thr_max.get(), _param_sinkrate_target.get(), - _performance_model.getMaximumClimbRate(_air_density)); + _performance_model.getMaximumClimbRate(_air_density), + disable_underspeed_handling); _tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); // reset after TECS calculation - _att_sp.pitch_body = _runway_takeoff.getPitch(get_tecs_pitch()); + const float pitch_body = _runway_takeoff.getPitch(get_tecs_pitch()); _att_sp.thrust_body[0] = _runway_takeoff.getThrottle(_param_fw_thr_idle.get(), get_tecs_thrust()); + roll_body = constrainRollNearGround(roll_body, _current_altitude, _takeoff_ground_alt); + + const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); + _flaps_setpoint = _param_fw_flaps_to_scl.get(); // retract ladning gear once passed the climbout state @@ -1590,12 +1624,13 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _npfg.setAirspeedNom(target_airspeed * _eas2tas); _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); navigateLine(launch_local_position, takeoff_bearing, local_2D_position, ground_speed, _wind_vel); - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + float roll_body = getCorrectedNpfgRollSetpoint(); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; const float max_takeoff_throttle = (_launchDetector.getLaunchDetected() < launch_detection_status_s::STATE_FLYING) ? _param_fw_thr_idle.get() : _param_fw_thr_max.get(); + const bool disable_underspeed_handling = true; tecs_update_pitch_throttle(control_interval, altitude_setpoint_amsl, @@ -1605,7 +1640,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _param_fw_thr_min.get(), max_takeoff_throttle, _param_sinkrate_target.get(), - _performance_model.getMaximumClimbRate(_air_density)); + _performance_model.getMaximumClimbRate(_air_density), + disable_underspeed_handling); if (_launchDetector.getLaunchDetected() < launch_detection_status_s::STATE_FLYING) { // explicitly set idle throttle until motors are enabled @@ -1615,17 +1651,27 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _att_sp.thrust_body[0] = get_tecs_thrust(); } - _att_sp.pitch_body = get_tecs_pitch(); - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + float pitch_body = get_tecs_pitch(); + float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + + roll_body = constrainRollNearGround(roll_body, _current_altitude, _takeoff_ground_alt); + + Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } else { /* Tell the attitude controller to stop integrating while we are waiting for the launch */ _att_sp.reset_integral = true; /* Set default roll and pitch setpoints during detection phase */ - _att_sp.roll_body = 0.0f; + float roll_body = 0.0f; + float yaw_body = _yaw; _att_sp.thrust_body[0] = _param_fw_thr_idle.get(); - _att_sp.pitch_body = radians(_takeoff_pitch_min.get()); + float pitch_body = radians(_takeoff_pitch_min.get()); + roll_body = constrainRollNearGround(roll_body, _current_altitude, _takeoff_ground_alt); + Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); + } launch_detection_status_s launch_detection_status; @@ -1634,7 +1680,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _launch_detection_status_pub.publish(launch_detection_status); } - _att_sp.roll_body = constrainRollNearGround(_att_sp.roll_body, _current_altitude, _takeoff_ground_alt); + _flaps_setpoint = _param_fw_flaps_to_scl.get(); if (!_vehicle_status.in_transition_to_fw) { publishLocalPositionSetpoint(pos_sp_curr); @@ -1739,10 +1785,10 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); navigateLine(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + float roll_body = getCorrectedNpfgRollSetpoint(); // use npfg's bearing to commanded course, controlled via yaw angle while on runway - _att_sp.yaw_body = _npfg.getBearing(); + float yaw_body = _npfg.getBearing(); /* longitudinal guidance */ @@ -1772,6 +1818,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, const float throttle_max = flare_ramp_interpolator_sqrt * _param_fw_thr_idle.get() + (1.0f - flare_ramp_interpolator_sqrt) * _param_fw_thr_max.get(); + const bool disable_underspeed_handling = true; tecs_update_pitch_throttle(control_interval, altitude_setpoint, @@ -1782,13 +1829,19 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, throttle_max, _param_sinkrate_target.get(), _param_climbrate_target.get(), - true, + disable_underspeed_handling, height_rate_setpoint); /* set the attitude and throttle commands */ // TECS has authority (though constrained) over pitch during flare, throttle is hard set to idle - _att_sp.pitch_body = get_tecs_pitch(); + float pitch_body = get_tecs_pitch(); + + roll_body = constrainRollNearGround(roll_body, _current_altitude, terrain_alt); + + + const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); // enable direct yaw control using rudder/wheel _att_sp.fw_control_yaw_wheel = true; @@ -1818,7 +1871,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); navigateLine(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + float roll_body = getCorrectedNpfgRollSetpoint(); /* longitudinal guidance */ @@ -1840,10 +1893,15 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, /* set the attitude and throttle commands */ - _att_sp.pitch_body = get_tecs_pitch(); + float pitch_body = get_tecs_pitch(); + + roll_body = constrainRollNearGround(roll_body, _current_altitude, terrain_alt); // yaw is not controlled in nominal flight - _att_sp.yaw_body = _yaw; + float yaw_body = _yaw; + + const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); // enable direct yaw control using rudder/wheel _att_sp.fw_control_yaw_wheel = false; @@ -1853,8 +1911,6 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, _tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); // reset after TECS calculation - _att_sp.roll_body = constrainRollNearGround(_att_sp.roll_body, _current_altitude, terrain_alt); - _flaps_setpoint = _param_fw_flaps_lnd_scl.get(); _spoilers_setpoint = _param_fw_spoilers_lnd.get(); @@ -1912,6 +1968,10 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, } // the terrain estimate (if enabled) is always used to determine the flaring altitude + float roll_body; + float yaw_body; + float pitch_body; + if ((_current_altitude < terrain_alt + flare_rel_alt) || _flare_states.flaring) { // flare and land with minimal speed @@ -1944,9 +2004,9 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, pos_sp_curr.loiter_direction_counter_clockwise, ground_speed, _wind_vel); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + roll_body = getCorrectedNpfgRollSetpoint(); - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw /* longitudinal guidance */ @@ -1975,6 +2035,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, const float throttle_max = flare_ramp_interpolator_sqrt * _param_fw_thr_idle.get() + (1.0f - flare_ramp_interpolator_sqrt) * _param_fw_thr_max.get(); + const bool disable_underspeed_handling = true; tecs_update_pitch_throttle(control_interval, _current_altitude, // is not controlled, control descend rate @@ -1985,13 +2046,13 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, throttle_max, _param_sinkrate_target.get(), _param_climbrate_target.get(), - true, + disable_underspeed_handling, height_rate_setpoint); /* set the attitude and throttle commands */ // TECS has authority (though constrained) over pitch during flare, throttle is hard set to idle - _att_sp.pitch_body = get_tecs_pitch(); + pitch_body = get_tecs_pitch(); // enable direct yaw control using rudder/wheel _att_sp.fw_control_yaw_wheel = true; @@ -2021,7 +2082,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, pos_sp_curr.loiter_direction_counter_clockwise, ground_speed, _wind_vel); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + roll_body = getCorrectedNpfgRollSetpoint(); /* longitudinal guidance */ @@ -2031,6 +2092,8 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, const float glide_slope_sink_rate = airspeed_land * glide_slope / sqrtf(glide_slope * glide_slope + 1.0f); const float desired_max_sinkrate = math::min(math::max(glide_slope_sink_rate, _param_sinkrate_target.get()), _param_fw_t_sink_max.get()); + const bool disable_underspeed_handling = false; + tecs_update_pitch_throttle(control_interval, _current_altitude, // is not controlled, control descend rate target_airspeed, @@ -2040,15 +2103,15 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, _param_fw_thr_max.get(), desired_max_sinkrate, _param_climbrate_target.get(), - false, + disable_underspeed_handling, -glide_slope_sink_rate); // heightrate = -sinkrate /* set the attitude and throttle commands */ - _att_sp.pitch_body = get_tecs_pitch(); + pitch_body = get_tecs_pitch(); // yaw is not controlled in nominal flight - _att_sp.yaw_body = _yaw; + yaw_body = _yaw; // enable direct yaw control using rudder/wheel _att_sp.fw_control_yaw_wheel = false; @@ -2058,7 +2121,11 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, _tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); // reset after TECS calculation - _att_sp.roll_body = constrainRollNearGround(_att_sp.roll_body, _current_altitude, terrain_alt); + roll_body = constrainRollNearGround(roll_body, _current_altitude, terrain_alt); + + Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); + _flaps_setpoint = _param_fw_flaps_lnd_scl.get(); _spoilers_setpoint = _param_fw_spoilers_lnd.get(); @@ -2093,6 +2160,8 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval, throttle_max = 0.0f; } + const bool disable_underspeed_handling = false; + tecs_update_pitch_throttle(control_interval, _current_altitude, calibrated_airspeed_sp, @@ -2102,14 +2171,17 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval, throttle_max, _param_sinkrate_target.get(), _param_climbrate_target.get(), - false, + disable_underspeed_handling, height_rate_sp); - _att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + float roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); + float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw _att_sp.thrust_body[0] = min(get_tecs_thrust(), throttle_max); - _att_sp.pitch_body = get_tecs_pitch(); + const float pitch_body = get_tecs_pitch(); + + const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } void @@ -2138,6 +2210,11 @@ FixedwingPositionControl::control_manual_position(const float control_interval, _time_last_xy_reset = _local_pos.timestamp; } + Eulerf current_setpoint(Quatf(_att_sp.q_d)); + float yaw_body = current_setpoint.psi(); + float roll_body = current_setpoint.phi(); + float pitch_body = current_setpoint.theta(); + /* heading control */ // TODO: either make it course hold (easier) or a real heading hold (minus all the complexity here) if (fabsf(_manual_control_setpoint.roll) < HDG_HOLD_MAN_INPUT_THRESH && @@ -2159,35 +2236,34 @@ FixedwingPositionControl::control_manual_position(const float control_interval, if (_yaw_lock_engaged) { - /* just switched back from non heading-hold to heading hold */ + Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; + if (!_hdg_hold_enabled) { + // just switched back from non heading-hold to heading hold _hdg_hold_enabled = true; _hdg_hold_yaw = _yaw; - _hdg_hold_position.lat = _current_latitude; - _hdg_hold_position.lon = _current_longitude; + _hdg_hold_position = curr_pos_local; } // if there's a reset-by-fusion, the ekf needs some time to converge, // therefore we go into track holiding for 2 seconds if (_local_pos.timestamp - _time_last_xy_reset < 2_s) { - _hdg_hold_position.lat = _current_latitude; - _hdg_hold_position.lon = _current_longitude; + _hdg_hold_position = curr_pos_local; } - Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; - Vector2f curr_wp_local = _global_local_proj_ref.project(_hdg_hold_position.lat, _hdg_hold_position.lon); - _npfg.setAirspeedNom(calibrated_airspeed_sp * _eas2tas); _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); - navigateLine(curr_wp_local, _hdg_hold_yaw, curr_pos_local, ground_speed, _wind_vel); - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + navigateLine(_hdg_hold_position, _hdg_hold_yaw, curr_pos_local, ground_speed, _wind_vel); + roll_body = getCorrectedNpfgRollSetpoint(); calibrated_airspeed_sp = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw } } + const bool disable_underspeed_handling = false; + tecs_update_pitch_throttle(control_interval, _current_altitude, // TODO: check if this is really what we want.. or if we want to lock the altitude. calibrated_airspeed_sp, @@ -2197,7 +2273,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval, throttle_max, _param_sinkrate_target.get(), _param_climbrate_target.get(), - false, + disable_underspeed_handling, height_rate_sp); if (!_yaw_lock_engaged || fabsf(_manual_control_setpoint.roll) >= HDG_HOLD_MAN_INPUT_THRESH || @@ -2206,12 +2282,16 @@ FixedwingPositionControl::control_manual_position(const float control_interval, _hdg_hold_enabled = false; _yaw_lock_engaged = false; - _att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); + yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw } _att_sp.thrust_body[0] = min(get_tecs_thrust(), throttle_max); - _att_sp.pitch_body = get_tecs_pitch(); + + pitch_body = get_tecs_pitch(); + + Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } void FixedwingPositionControl::control_backtransition(const float control_interval, const Vector2f &ground_speed, @@ -2234,10 +2314,10 @@ void FixedwingPositionControl::control_backtransition(const float control_interv navigateLine(_lpos_where_backtrans_started, curr_wp_local, curr_pos_local, ground_speed, _wind_vel); - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + float roll_body = getCorrectedNpfgRollSetpoint(); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw tecs_update_pitch_throttle(control_interval, pos_sp_curr.alt, @@ -2250,7 +2330,10 @@ void FixedwingPositionControl::control_backtransition(const float control_interv _param_climbrate_target.get()); _att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_min.get() : min(get_tecs_thrust(), _param_fw_thr_max.get()); - _att_sp.pitch_body = get_tecs_pitch(); + const float pitch_body = get_tecs_pitch(); + + const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } float FixedwingPositionControl::get_tecs_pitch() @@ -2560,12 +2643,16 @@ FixedwingPositionControl::Run() if (_control_mode_current != FW_POSCTRL_MODE_OTHER) { + Eulerf attitude_setpoint(Quatf(_att_sp.q_d)); + float roll_body = attitude_setpoint.phi(); + float pitch_body = attitude_setpoint.theta(); + float yaw_body = attitude_setpoint.psi(); if (_control_mode.flag_control_manual_enabled) { - _att_sp.roll_body = constrain(_att_sp.roll_body, -radians(_param_fw_r_lim.get()), - radians(_param_fw_r_lim.get())); - _att_sp.pitch_body = constrain(_att_sp.pitch_body, radians(_param_fw_p_lim_min.get()), - radians(_param_fw_p_lim_max.get())); + roll_body = constrain(roll_body, -radians(_param_fw_r_lim.get()), + radians(_param_fw_r_lim.get())); + pitch_body = constrain(pitch_body, radians(_param_fw_p_lim_min.get()), + radians(_param_fw_p_lim_max.get())); } if (_control_mode.flag_control_position_enabled || @@ -2575,9 +2662,9 @@ FixedwingPositionControl::Run() _control_mode.flag_control_climb_rate_enabled) { // roll slew rate - _att_sp.roll_body = _roll_slew_rate.update(_att_sp.roll_body, control_interval); + roll_body = _roll_slew_rate.update(roll_body, control_interval); - const Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); + const Quatf q(Eulerf(roll_body, pitch_body, yaw_body)); q.copyTo(_att_sp.q_d); _att_sp.timestamp = hrt_absolute_time(); @@ -3166,8 +3253,10 @@ float FixedwingPositionControl::getLoadFactor() { float load_factor_from_bank_angle = 1.0f; - if (PX4_ISFINITE(_att_sp.roll_body)) { - load_factor_from_bank_angle = 1.0f / math::max(cosf(_att_sp.roll_body), FLT_EPSILON); + float roll_body = Eulerf(Quatf(_att_sp.q_d)).phi(); + + if (PX4_ISFINITE(roll_body)) { + load_factor_from_bank_angle = 1.0f / math::max(cosf(roll_body), FLT_EPSILON); } return load_factor_from_bank_angle; diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp index 77eb88eb6511..84300ae1b409 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp @@ -298,7 +298,7 @@ class FixedwingPositionControl final : public ModuleBase ${CMAKE_CURRENT_BINARY_DIR}/mavgen_${MAVLINK_DIALECT_UAVIONIX}.log DEPENDS @@ -63,7 +62,6 @@ add_custom_command( COMMAND ${PYTHON_EXECUTABLE} ${MAVLINK_GIT_DIR}/pymavlink/tools/mavgen.py --lang C --wire-protocol 2.0 - --exit-code --output ${MAVLINK_LIBRARY_DIR} ${MAVLINK_GIT_DIR}/message_definitions/v1.0/${CONFIG_MAVLINK_DIALECT}.xml > ${CMAKE_CURRENT_BINARY_DIR}/mavgen_${CONFIG_MAVLINK_DIALECT}.log DEPENDS diff --git a/src/modules/mavlink/mavlink b/src/modules/mavlink/mavlink index d65952bacc02..bb52e30d2b92 160000 --- a/src/modules/mavlink/mavlink +++ b/src/modules/mavlink/mavlink @@ -1 +1 @@ -Subproject commit d65952bacc02c4a5a1ed8249be73e6a66fa800ab +Subproject commit bb52e30d2b924d5a250f2400144d33012271a19d diff --git a/src/modules/mavlink/mavlink_log_handler.cpp b/src/modules/mavlink/mavlink_log_handler.cpp index a75daf90bad7..1afa32382b19 100644 --- a/src/modules/mavlink/mavlink_log_handler.cpp +++ b/src/modules/mavlink/mavlink_log_handler.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2014-2024 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,558 +31,498 @@ * ****************************************************************************/ -/// @file mavlink_log_handler.h -/// @author px4dev, Gus Grubba - #include "mavlink_log_handler.h" #include "mavlink_main.h" +#include #include -#include -#include - -#define MOUNTPOINT PX4_STORAGEDIR -static const char *kLogRoot = MOUNTPOINT "/log"; -static const char *kLogData = MOUNTPOINT "/logdata.txt"; -static const char *kTmpData = MOUNTPOINT "/$log$.txt"; +static constexpr int MAX_BYTES_BURST = 256 * 1024; +static const char *kLogListFilePath = PX4_STORAGEDIR "/logdata.txt"; +static const char *kLogListFilePathTemp = PX4_STORAGEDIR "/$log$.txt"; +static const char *kLogDir = PX4_STORAGEDIR "/log"; #ifdef __PX4_NUTTX #define PX4LOG_REGULAR_FILE DTYPE_FILE #define PX4LOG_DIRECTORY DTYPE_DIRECTORY +#define PX4_MAX_FILEPATH CONFIG_PATH_MAX #else +#ifndef PATH_MAX +#define PATH_MAX 1024 // maximum on macOS +#endif #define PX4LOG_REGULAR_FILE DT_REG #define PX4LOG_DIRECTORY DT_DIR +#define PX4_MAX_FILEPATH PATH_MAX #endif -//#define MAVLINK_LOG_HANDLER_VERBOSE - -#ifdef MAVLINK_LOG_HANDLER_VERBOSE -#define PX4LOG_WARN(fmt, ...) warnx(fmt, ##__VA_ARGS__) -#else -#define PX4LOG_WARN(fmt, ...) -#endif +MavlinkLogHandler::MavlinkLogHandler(Mavlink &mavlink) + : _mavlink(mavlink) +{} -//------------------------------------------------------------------- -static bool -stat_file(const char *file, time_t *date = nullptr, uint32_t *size = nullptr) +MavlinkLogHandler::~MavlinkLogHandler() { - struct stat st; - - if (stat(file, &st) == 0) { - if (date) { *date = st.st_mtime; } - - if (size) { *size = st.st_size; } + perf_free(_create_file_elapsed); + perf_free(_listing_elapsed); - return true; + if (_current_entry.fp) { + fclose(_current_entry.fp); } - return false; + unlink(kLogListFilePath); + unlink(kLogListFilePathTemp); } -//------------------------------------------------------------------- -MavlinkLogHandler::MavlinkLogHandler(Mavlink &mavlink) - : _mavlink(mavlink) +void MavlinkLogHandler::send() { + switch (_state) { + case LogHandlerState::Idle: { + state_idle(); + break; + } -} -MavlinkLogHandler::~MavlinkLogHandler() -{ - _close_and_unlink_files(); + case LogHandlerState::Listing: { + state_listing(); + break; + } + + case LogHandlerState::SendingData: { + state_sending_data(); + break; + } + } } -//------------------------------------------------------------------- -void -MavlinkLogHandler::handle_message(const mavlink_message_t *msg) +void MavlinkLogHandler::handle_message(const mavlink_message_t *msg) { switch (msg->msgid) { case MAVLINK_MSG_ID_LOG_REQUEST_LIST: - _log_request_list(msg); + handle_log_request_list(msg); break; case MAVLINK_MSG_ID_LOG_REQUEST_DATA: - _log_request_data(msg); + handle_log_request_data(msg); break; - case MAVLINK_MSG_ID_LOG_ERASE: - _log_request_erase(msg); + case MAVLINK_MSG_ID_LOG_REQUEST_END: + handle_log_request_end(msg); break; - case MAVLINK_MSG_ID_LOG_REQUEST_END: - _log_request_end(msg); + case MAVLINK_MSG_ID_LOG_ERASE: + handle_log_erase(msg); break; } } -//------------------------------------------------------------------- -void -MavlinkLogHandler::send() +void MavlinkLogHandler::state_idle() { - //-- An arbitrary count of max bytes in one go (one of the two below but never both) -#define MAX_BYTES_SEND 256 * 1024 - size_t count = 0; - - //-- Log Entries - while (_current_status == LogHandlerState::Listing - && _mavlink.get_free_tx_buf() > MAVLINK_MSG_ID_LOG_ENTRY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES - && count < MAX_BYTES_SEND) { - count += _log_send_listing(); - } - - //-- Log Data - while (_current_status == LogHandlerState::SendingData - && _mavlink.get_free_tx_buf() > MAVLINK_MSG_ID_LOG_DATA_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES - && count < MAX_BYTES_SEND) { - count += _log_send_data(); + if (_current_entry.fp && _file_send_finished) { + fclose(_current_entry.fp); + _current_entry.fp = nullptr; + + _current_entry.id = 0xffff; + _current_entry.time_utc = 0; + _current_entry.size_bytes = 0; + _current_entry.filepath[0] = 0; + _current_entry.offset = 0; + + _entry_request.id = 0xffff; + _entry_request.start_offset = 0; + _entry_request.byte_count = 0; } } -//------------------------------------------------------------------- -void -MavlinkLogHandler::_log_request_list(const mavlink_message_t *msg) +void MavlinkLogHandler::state_listing() { - mavlink_log_request_list_t request; - mavlink_msg_log_request_list_decode(msg, &request); + static constexpr uint32_t MAVLINK_PACKET_SIZE = MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_LOG_ENTRY_LEN; - //-- Check for re-requests (data loss) or new request - if (_current_status != LogHandlerState::Inactive) { - //-- Is this a new request? - if (request.start == 0) { - _current_status = LogHandlerState::Inactive; - _close_and_unlink_files(); - - } else { - _current_status = LogHandlerState::Idle; - - } + if (_mavlink.get_free_tx_buf() <= MAVLINK_PACKET_SIZE) { + return; } - if (_current_status == LogHandlerState::Inactive) { - //-- Prepare new request - - _reset_list_helper(); - _init_list_helper(); - _current_status = LogHandlerState::Idle; - } + DIR *dp = opendir(kLogDir); - if (_log_count) { - //-- Define (and clamp) range - _next_entry = request.start < _log_count ? request.start : - _log_count - 1; - _last_entry = request.end < _log_count ? request.end : - _log_count - 1; + if (!dp) { + PX4_DEBUG("No logs available"); + return; } - PX4LOG_WARN("\nMavlinkLogHandler::_log_request_list: start: %d last: %d count: %d", - _next_entry, - _last_entry, - _log_count); - //-- Enable streaming - _current_status = LogHandlerState::Listing; -} + FILE *fp = fopen(kLogListFilePath, "r"); -//------------------------------------------------------------------- -void -MavlinkLogHandler::_log_request_data(const mavlink_message_t *msg) -{ - //-- If we haven't listed, we can't do much - if (_current_status == LogHandlerState::Inactive) { - PX4LOG_WARN("MavlinkLogHandler::_log_request_data Log request with no list requested."); + if (!fp) { + PX4_DEBUG("Failed to open log list file"); + closedir(dp); return; } - mavlink_log_request_data_t request; - mavlink_msg_log_request_data_decode(msg, &request); + fseek(fp, _list_request.file_index, SEEK_SET); - //-- Does the requested log exist? - if (request.id >= _log_count) { - PX4LOG_WARN("MavlinkLogHandler::_log_request_data Requested log %" PRIu16 " but we only have %u.", request.id, - _log_count); - return; - } + size_t bytes_sent = 0; - //-- If we were sending log entries, stop it - _current_status = LogHandlerState::Idle; + char line[PX4_MAX_FILEPATH]; - if (_current_log_index != request.id) { - //-- Init send log dataset - _current_log_filename[0] = 0; - _current_log_index = request.id; - uint32_t time_utc = 0; + perf_begin(_listing_elapsed); - if (!_get_entry(_current_log_index, _current_log_size, time_utc, - _current_log_filename, sizeof(_current_log_filename))) { - PX4LOG_WARN("LogListHelper::get_entry failed."); - return; + while (fgets(line, sizeof(line), fp)) { + + if (_list_request.current_id < _list_request.first_id) { + _list_request.current_id++; + continue; } - _open_for_transmit(); - } + // We can send! + uint32_t size_bytes = 0; + uint32_t time_utc = 0; + char filepath[PX4_MAX_FILEPATH]; - _current_log_data_offset = request.ofs; + // If parsed lined successfully, send the entry + if (sscanf(line, "%" PRIu32 " %" PRIu32 " %s", &time_utc, &size_bytes, filepath) != 3) { + PX4_DEBUG("sscanf failed"); + continue; + } - if (_current_log_data_offset >= _current_log_size) { - _current_log_data_remaining = 0; + send_log_entry(time_utc, size_bytes); + bytes_sent += sizeof(mavlink_log_entry_t); + _list_request.current_id++; - } else { - _current_log_data_remaining = _current_log_size - request.ofs; + // Yield if we've exceed mavlink burst or buffer limit + if (_mavlink.get_free_tx_buf() <= MAVLINK_PACKET_SIZE || bytes_sent >= MAX_BYTES_BURST) { + _list_request.file_index = ftell(fp); + fclose(fp); + closedir(dp); + perf_end(_listing_elapsed); + return; + } } - if (_current_log_data_remaining > request.count) { - _current_log_data_remaining = request.count; - } + perf_end(_listing_elapsed); - //-- Enable streaming - _current_status = LogHandlerState::SendingData; -} + fclose(fp); + closedir(dp); -//------------------------------------------------------------------- -void -MavlinkLogHandler::_log_request_erase(const mavlink_message_t * /*msg*/) -{ - /* - mavlink_log_erase_t request; - mavlink_msg_log_erase_decode(msg, &request); - */ - _current_status = LogHandlerState::Inactive; - _close_and_unlink_files(); - - //-- Delete all logs - _delete_all(kLogRoot); + _list_request.current_id = 0; + _list_request.file_index = 0; + _state = LogHandlerState::Idle; } -//------------------------------------------------------------------- -void -MavlinkLogHandler::_log_request_end(const mavlink_message_t * /*msg*/) +void MavlinkLogHandler::state_sending_data() { - PX4LOG_WARN("MavlinkLogHandler::_log_request_end"); + static constexpr uint32_t MAVLINK_PACKET_SIZE = MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_LOG_DATA_LEN; + size_t bytes_sent = 0; - _current_status = LogHandlerState::Inactive; - _close_and_unlink_files(); -} + while (_mavlink.get_free_tx_buf() > MAVLINK_PACKET_SIZE && bytes_sent < MAX_BYTES_BURST) { -//------------------------------------------------------------------- -size_t -MavlinkLogHandler::_log_send_listing() -{ - mavlink_log_entry_t response; - uint32_t size, date; - _get_entry(_next_entry, size, date); - response.size = size; - response.time_utc = date; - response.id = _next_entry; - response.num_logs = _log_count; - response.last_log_num = _last_entry; - mavlink_msg_log_entry_send_struct(_mavlink.get_channel(), &response); - - //-- If we're done listing, flag it. - if (_next_entry >= _last_entry) { - _current_status = LogHandlerState::Idle; - - } else { - _next_entry++; - } + // Only seek if we need to + long int offset = _current_entry.offset - ftell(_current_entry.fp); - PX4LOG_WARN("MavlinkLogHandler::_log_send_listing id: %" PRIu16 " count: %" PRIu16 " last: %" PRIu16 " size: %" PRIu32 - " date: %" PRIu32 " status: %" PRIu32, - response.id, - response.num_logs, - response.last_log_num, - response.size, - response.time_utc, - (uint32_t)_current_status); - return sizeof(response); -} + if (offset && fseek(_current_entry.fp, offset, SEEK_CUR)) { + fclose(_current_entry.fp); + _current_entry.fp = nullptr; + PX4_DEBUG("seek error"); + _state = LogHandlerState::Idle; + } -//------------------------------------------------------------------- -size_t -MavlinkLogHandler::_log_send_data() -{ - mavlink_log_data_t response; - memset(&response, 0, sizeof(response)); - uint32_t len = _current_log_data_remaining; + // Prepare mavlink message + mavlink_log_data_t msg; - if (len > sizeof(response.data)) { - len = sizeof(response.data); - } + if (_current_entry.offset >= _current_entry.size_bytes) { + PX4_DEBUG("Entry offset equal to file size"); + _state = LogHandlerState::Idle; + return; + } - size_t read_size = _get_log_data(len, response.data); - response.ofs = _current_log_data_offset; - response.id = _current_log_index; - response.count = read_size; - mavlink_msg_log_data_send_struct(_mavlink.get_channel(), &response); - _current_log_data_offset += read_size; - _current_log_data_remaining -= read_size; + size_t bytes_to_read = _current_entry.size_bytes - _current_entry.offset; - if (read_size < sizeof(response.data) || _current_log_data_remaining == 0) { - _current_status = LogHandlerState::Idle; - } + if (bytes_to_read > sizeof(msg.data)) { + bytes_to_read = sizeof(msg.data); + } - return sizeof(response); -} + msg.count = fread(msg.data, 1, bytes_to_read, _current_entry.fp); + msg.id = _current_entry.id; + msg.ofs = _current_entry.offset; -//------------------------------------------------------------------- -void MavlinkLogHandler::_close_and_unlink_files() -{ - if (_current_log_filep) { - ::fclose(_current_log_filep); - _reset_list_helper(); - } + mavlink_msg_log_data_send_struct(_mavlink.get_channel(), &msg); + + bytes_sent += MAVLINK_PACKET_SIZE; + _current_entry.offset += msg.count; + + bool chunk_finished = _current_entry.offset >= (_entry_request.byte_count + _entry_request.start_offset); + _file_send_finished = _current_entry.offset >= _current_entry.size_bytes; - // Remove log data files (if any) - unlink(kLogData); - unlink(kTmpData); + if (chunk_finished || _file_send_finished) { + _state = LogHandlerState::Idle; + return; + } + } } -//------------------------------------------------------------------- -bool -MavlinkLogHandler::_get_entry(int idx, uint32_t &size, uint32_t &date, char *filename, int filename_len) +void MavlinkLogHandler::handle_log_request_list(const mavlink_message_t *msg) { - //-- Find log file in log list file created during init() - size = 0; - date = 0; - bool result = false; - //-- Open list of log files - FILE *f = ::fopen(kLogData, "r"); - - if (f) { - //--- Find requested entry - char line[160]; - int count = 0; - - while (fgets(line, sizeof(line), f)) { - //-- Found our "index" - if (count++ == idx) { - char file[160]; - - if (sscanf(line, "%" PRIu32 " %" PRIu32 " %s", &date, &size, file) == 3) { - if (filename && filename_len > 0) { - strncpy(filename, file, filename_len); - filename[filename_len - 1] = 0; // ensure null-termination - } - - result = true; - break; - } - } - } + mavlink_log_request_list_t request; + mavlink_msg_log_request_list_decode(msg, &request); - fclose(f); + if (!create_log_list_file()) { + return; } - return result; + _list_request.first_id = request.start; + _list_request.last_id = request.end == 0xffff ? _num_logs : request.end; + _list_request.current_id = 0; + _list_request.file_index = 0; + _logs_listed = true; + _state = LogHandlerState::Listing; } -//------------------------------------------------------------------- -bool -MavlinkLogHandler::_open_for_transmit() +void MavlinkLogHandler::handle_log_request_data(const mavlink_message_t *msg) { - if (_current_log_filep) { - ::fclose(_current_log_filep); - _current_log_filep = nullptr; + if (!_logs_listed) { + PX4_DEBUG("Logs not yet listed"); + _state = LogHandlerState::Idle; + return; } - _current_log_filep = ::fopen(_current_log_filename, "rb"); + mavlink_log_request_data_t request; + mavlink_msg_log_request_data_decode(msg, &request); - if (!_current_log_filep) { - PX4LOG_WARN("MavlinkLogHandler::open_for_transmit Could not open %s", _current_log_filename); - return false; + if (request.id >= _num_logs) { + PX4_DEBUG("Requested log %" PRIu16 " but we only have %u", request.id, _num_logs); + _state = LogHandlerState::Idle; + return; } - return true; -} + // Handle switching to new request ID + if (request.id != _current_entry.id) { + // Close the old file + if (_current_entry.fp) { + fclose(_current_entry.fp); + _current_entry.fp = nullptr; + } -//------------------------------------------------------------------- -size_t -MavlinkLogHandler::_get_log_data(uint8_t len, uint8_t *buffer) -{ - if (!_current_log_filename[0]) { - return 0; - } + LogEntry entry = {}; - if (!_current_log_filep) { - PX4LOG_WARN("MavlinkLogHandler::get_log_data file not open %s", _current_log_filename); - return 0; - } + if (!log_entry_from_id(request.id, &entry)) { + PX4_DEBUG("Log file ID %u does not exist", request.id); + _state = LogHandlerState::Idle; + return; + } - long int offset = _current_log_data_offset - ftell(_current_log_filep); + entry.fp = fopen(entry.filepath, "rb"); + entry.offset = request.ofs; - if (offset && fseek(_current_log_filep, offset, SEEK_CUR)) { - fclose(_current_log_filep); - _current_log_filep = nullptr; - PX4LOG_WARN("MavlinkLogHandler::get_log_data Seek error in %s", _current_log_filename); - return 0; + if (!entry.fp) { + PX4_DEBUG("Failed to open file %s", entry.filepath); + return; + } + + _current_entry = entry; } - size_t result = fread(buffer, 1, len, _current_log_filep); - return result; -} + // Stop if offset request is larger than file + if (request.ofs >= _current_entry.size_bytes) { + PX4_DEBUG("Request offset %" PRIu32 "greater than file size %" PRIu32, request.ofs, + _current_entry.size_bytes); + _state = LogHandlerState::Idle; + return; + } + _entry_request.id = request.id; + _entry_request.start_offset = request.ofs; + _entry_request.byte_count = request.count; + // Set the offset of the current entry to the requested offset + _current_entry.offset = _entry_request.start_offset; + _file_send_finished = false; + _state = LogHandlerState::SendingData; +} -void -MavlinkLogHandler::_reset_list_helper() +void MavlinkLogHandler::handle_log_request_end(const mavlink_message_t *msg) { - _next_entry = 0; - _last_entry = 0; - _log_count = 0; - _current_log_index = UINT16_MAX; - _current_log_size = 0; - _current_log_data_offset = 0; - _current_log_data_remaining = 0; - _current_log_filep = nullptr; + _state = LogHandlerState::Idle; } -void -MavlinkLogHandler::_init_list_helper() +void MavlinkLogHandler::handle_log_erase(const mavlink_message_t *msg) { - /* + if (_current_entry.fp) { + fclose(_current_entry.fp); + _current_entry.fp = nullptr; + } + + _state = LogHandlerState::Idle; + unlink(kLogListFilePath); + unlink(kLogListFilePathTemp); - When this helper is created, it scans the log directory - and collects all log files found into one file for easy, - subsequent access. - */ + delete_all_logs(kLogDir); +} - _current_log_filename[0] = 0; +bool MavlinkLogHandler::create_log_list_file() +{ + perf_begin(_create_file_elapsed); - // Remove old log data file (if any) - unlink(kLogData); - // Open log directory - DIR *dp = opendir(kLogRoot); + // clean up old file + unlink(kLogListFilePath); + _num_logs = 0; - if (dp == nullptr) { - // No log directory. Nothing to do. - return; + DIR *dp = opendir(kLogDir); + + if (!dp) { + PX4_DEBUG("No logs available"); + return false; } - // Create work file - FILE *f = ::fopen(kTmpData, "w"); + FILE *temp_fp = fopen(kLogListFilePathTemp, "w"); - if (!f) { - PX4LOG_WARN("MavlinkLogHandler::init Error creating %s", kTmpData); + if (!temp_fp) { + PX4_DEBUG("Failed to create temp file"); closedir(dp); - return; + return false; } - // Scan directory and collect log files struct dirent *result = nullptr; - while ((result = readdir(dp))) { - if (result->d_type == PX4LOG_DIRECTORY) { - time_t tt = 0; - char log_path[128]; - int ret = snprintf(log_path, sizeof(log_path), "%s/%s", kLogRoot, result->d_name); - bool path_is_ok = (ret > 0) && (ret < (int)sizeof(log_path)); + // Iterate over the log/ directory which contains subdirectories formatted: yyyy-mm-dd + while (1) { + result = readdir(dp); - if (path_is_ok) { - if (_get_session_date(log_path, result->d_name, tt)) { - _scan_logs(f, log_path, tt); - } - } + if (!result) { + // Reached end of directory + break; + } + + if (result->d_type != PX4LOG_DIRECTORY) { + // Skip stray files + continue; + } + + // Skip the '.' and '..' entries + if (strcmp(result->d_name, ".") == 0 || strcmp(result->d_name, "..") == 0) { + continue; + } + + // Open up the sub directory + char dirpath[PX4_MAX_FILEPATH]; + int ret = snprintf(dirpath, sizeof(dirpath), "%s/%s", kLogDir, result->d_name); + + bool path_is_ok = (ret > 0) && (ret < (int)sizeof(dirpath)); + + if (!path_is_ok) { + PX4_DEBUG("Log subdir path error: %s", dirpath); + continue; } + + // Iterate over files inside the subdir and write them to the file + write_entries_to_file(temp_fp, dirpath); } + fclose(temp_fp); closedir(dp); - fclose(f); // Rename temp file to data file - if (rename(kTmpData, kLogData)) { - PX4LOG_WARN("MavlinkLogHandler::init Error renaming %s", kTmpData); - _log_count = 0; + if (rename(kLogListFilePathTemp, kLogListFilePath)) { + PX4_DEBUG("Failed to rename temp file"); + return false; } + + perf_end(_create_file_elapsed); + + return true; } -//------------------------------------------------------------------- -bool -MavlinkLogHandler::_get_session_date(const char *path, const char *dir, time_t &date) +void MavlinkLogHandler::write_entries_to_file(FILE *fp, const char *dir) { - if (strlen(dir) > 4) { - // Always try to get file time first - if (stat_file(path, &date)) { - // Try to prevent taking date if it's around 1970 (use the logic below instead) - if (date > 60 * 60 * 24) { - return true; - } + DIR *dp = opendir(dir); + struct dirent *result = nullptr; + + while (1) { + result = readdir(dp); + + if (!result) { + // Reached end of directory + break; } - // Convert "sess000" to 00:00 Jan 1 1970 (day per session) - if (strncmp(dir, "sess", 4) == 0) { - unsigned u; + if (result->d_type != PX4LOG_REGULAR_FILE) { + // Skip non files + continue; + } - if (sscanf(&dir[4], "%u", &u) == 1) { - date = u * 60 * 60 * 24; - return true; - } + char filepath[PX4_MAX_FILEPATH]; + int ret = snprintf(filepath, sizeof(filepath), "%s/%s", dir, result->d_name); + bool path_is_ok = (ret > 0) && (ret < (int)sizeof(filepath)); + + if (!path_is_ok) { + PX4_DEBUG("Log subdir path error: %s", filepath); + continue; + } + + struct stat filestat; + + if (stat(filepath, &filestat) != 0) { + PX4_DEBUG("stat() failed: %s", filepath); + continue; } + + // Write to file using format: + // [ time ] [ size_bytes ] [ filepath ] + fprintf(fp, "%u %u %s\n", unsigned(filestat.st_mtime), unsigned(filestat.st_size), filepath); + _num_logs++; } - return false; + closedir(dp); } -//------------------------------------------------------------------- -void -MavlinkLogHandler::_scan_logs(FILE *f, const char *dir, time_t &date) +void MavlinkLogHandler::send_log_entry(uint32_t time_utc, uint32_t size_bytes) { - DIR *dp = opendir(dir); + mavlink_log_entry_t msg; + msg.time_utc = time_utc; + msg.size = size_bytes; + msg.id = _list_request.current_id; + msg.num_logs = _num_logs; + msg.last_log_num = _list_request.last_id; + mavlink_msg_log_entry_send_struct(_mavlink.get_channel(), &msg); +} - if (dp) { - struct dirent *result = nullptr; - - while ((result = readdir(dp))) { - if (result->d_type == PX4LOG_REGULAR_FILE) { - time_t ldate = date; - uint32_t size = 0; - char log_file_path[128]; - int ret = snprintf(log_file_path, sizeof(log_file_path), "%s/%s", dir, result->d_name); - bool path_is_ok = (ret > 0) && (ret < (int)sizeof(log_file_path)); - - if (path_is_ok) { - if (_get_log_time_size(log_file_path, result->d_name, ldate, size)) { - //-- Write result->out to list file - fprintf(f, "%u %u %s\n", (unsigned)ldate, (unsigned)size, log_file_path); - _log_count++; - } - } - } - } +bool MavlinkLogHandler::log_entry_from_id(uint16_t log_id, LogEntry *entry) +{ + DIR *dp = opendir(kLogDir); + + if (!dp) { + PX4_INFO("No logs available"); + return false; + } + + FILE *fp = fopen(kLogListFilePath, "r"); + if (!fp) { + PX4_DEBUG("Failed to open %s", kLogListFilePath); closedir(dp); + return false; } -} -//------------------------------------------------------------------- -bool -MavlinkLogHandler::_get_log_time_size(const char *path, const char *file, time_t &date, uint32_t &size) -{ - if (file && file[0]) { - if (strstr(file, ".px4log") || strstr(file, ".ulg")) { - // Always try to get file time first - if (stat_file(path, &date, &size)) { - // Try to prevent taking date if it's around 1970 (use the logic below instead) - if (date > 60 * 60 * 24) { - return true; - } - } + bool found_entry = false; + uint16_t current_id = 0; + char line[PX4_MAX_FILEPATH]; - // Convert "log000" to 00:00 (minute per flight in session) - if (strncmp(file, "log", 3) == 0) { - unsigned u; + while (fgets(line, sizeof(line), fp)) { - if (sscanf(&file[3], "%u", &u) == 1) { - date += (u * 60); + if (current_id != log_id) { + current_id++; + continue; + } - if (stat_file(path, nullptr, &size)) { - return true; - } - } - } + if (sscanf(line, "%" PRIu32 " %" PRIu32 " %s", &(entry->time_utc), &(entry->size_bytes), entry->filepath) != 3) { + PX4_DEBUG("sscanf failed"); + continue; } + + entry->id = log_id; + found_entry = true; + break; } - return false; + fclose(fp); + closedir(dp); + + return found_entry; } -//------------------------------------------------------------------- -void -MavlinkLogHandler::_delete_all(const char *dir) +void MavlinkLogHandler::delete_all_logs(const char *dir) { //-- Open log directory DIR *dp = opendir(dir); @@ -600,27 +540,27 @@ MavlinkLogHandler::_delete_all(const char *dir) } if (result->d_type == PX4LOG_DIRECTORY && result->d_name[0] != '.') { - char log_path[128]; - int ret = snprintf(log_path, sizeof(log_path), "%s/%s", dir, result->d_name); - bool path_is_ok = (ret > 0) && (ret < (int)sizeof(log_path)); + char filepath[PX4_MAX_FILEPATH]; + int ret = snprintf(filepath, sizeof(filepath), "%s/%s", dir, result->d_name); + bool path_is_ok = (ret > 0) && (ret < (int)sizeof(filepath)); if (path_is_ok) { - _delete_all(log_path); //Recursive call. TODO: consider add protection + delete_all_logs(filepath); - if (rmdir(log_path)) { - PX4LOG_WARN("MavlinkLogHandler::delete_all Error removing %s", log_path); + if (rmdir(filepath)) { + PX4_DEBUG("Error removing %s", filepath); } } } if (result->d_type == PX4LOG_REGULAR_FILE) { - char log_path[128]; - int ret = snprintf(log_path, sizeof(log_path), "%s/%s", dir, result->d_name); - bool path_is_ok = (ret > 0) && (ret < (int)sizeof(log_path)); + char filepath[PX4_MAX_FILEPATH]; + int ret = snprintf(filepath, sizeof(filepath), "%s/%s", dir, result->d_name); + bool path_is_ok = (ret > 0) && (ret < (int)sizeof(filepath)); if (path_is_ok) { - if (unlink(log_path)) { - PX4LOG_WARN("MavlinkLogHandler::delete_all Error deleting %s", log_path); + if (unlink(filepath)) { + PX4_DEBUG("Error unlinking %s", filepath); } } } diff --git a/src/modules/mavlink/mavlink_log_handler.h b/src/modules/mavlink/mavlink_log_handler.h index 965c26712f1c..eb521a61ee4f 100644 --- a/src/modules/mavlink/mavlink_log_handler.h +++ b/src/modules/mavlink/mavlink_log_handler.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014, 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2014-2024 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,76 +33,86 @@ #pragma once -/// @file mavlink_log_handler.h -/// @author px4dev, Gus Grubba - -#include -#include -#include -#include -#include -#include - +#include #include "mavlink_bridge_header.h" class Mavlink; -// MAVLink LOG_* Message Handler class MavlinkLogHandler { public: MavlinkLogHandler(Mavlink &mavlink); ~MavlinkLogHandler(); - // Handle possible LOG message + void send(); void handle_message(const mavlink_message_t *msg); - /** - * Handle sending of messages. Call this regularly at a fixed frequency. - * @param t current time - */ - void send(); +private: + struct LogEntry { + uint16_t id{0xffff}; + uint32_t time_utc{}; + uint32_t size_bytes{}; + FILE *fp{nullptr}; + char filepath[60]; + uint32_t offset{}; + }; - unsigned get_size(); + struct LogEntryRequest { + uint16_t id{0xffff}; + uint32_t start_offset{}; + uint32_t byte_count{}; + }; + + struct LogListRequest { + uint16_t first_id{0}; + uint16_t last_id{0}; + uint16_t current_id{0}; + int file_index{0}; + }; -private: enum class LogHandlerState { - Inactive, //There is no active action of log handler - Idle, //The log handler is not sending list/data, but list has been sent - Listing, //File list is being send - SendingData //File Data is being send + Idle, + Listing, + SendingData }; - void _log_message(const mavlink_message_t *msg); - void _log_request_list(const mavlink_message_t *msg); - void _log_request_data(const mavlink_message_t *msg); - void _log_request_erase(const mavlink_message_t *msg); - void _log_request_end(const mavlink_message_t *msg); - - void _reset_list_helper(); - void _init_list_helper(); - bool _get_session_date(const char *path, const char *dir, time_t &date); - void _scan_logs(FILE *f, const char *dir, time_t &date); - bool _get_log_time_size(const char *path, const char *file, time_t &date, uint32_t &size); - static void _delete_all(const char *dir); - bool _get_entry(int idx, uint32_t &size, uint32_t &date, char *filename = 0, int filename_len = 0); - bool _open_for_transmit(); - size_t _get_log_data(uint8_t len, uint8_t *buffer); - void _close_and_unlink_files(); - - size_t _log_send_listing(); - size_t _log_send_data(); - - LogHandlerState _current_status{LogHandlerState::Inactive}; + + // mavlink message handlers + void handle_log_request_list(const mavlink_message_t *msg); + void handle_log_request_data(const mavlink_message_t *msg); + void handle_log_request_end(const mavlink_message_t *msg); + void handle_log_erase(const mavlink_message_t *msg); + + // state functions + void state_idle(); + void state_listing(); + void state_sending_data(); + + // Log request list + bool create_log_list_file(); + void write_entries_to_file(FILE *f, const char *dir); + void send_log_entry(uint32_t size, uint32_t time_utc); + + // Log request data + bool log_entry_from_id(uint16_t log_id, LogEntry *entry); + + // Log erase + void delete_all_logs(const char *dir); + + +private: + LogHandlerState _state{LogHandlerState::Idle}; Mavlink &_mavlink; - int _next_entry{0}; - int _last_entry{0}; - int _log_count{0}; + // Log list + LogListRequest _list_request{}; + int _num_logs{0}; + bool _logs_listed{false}; + + // Log data + LogEntry _current_entry{}; + LogEntryRequest _entry_request{}; + bool _file_send_finished{}; - uint16_t _current_log_index{UINT16_MAX}; - uint32_t _current_log_size{0}; - uint32_t _current_log_data_offset{0}; - uint32_t _current_log_data_remaining{0}; - FILE *_current_log_filep{nullptr}; - char _current_log_filename[128]; //TODO: consider to allocate on runtime + perf_counter_t _create_file_elapsed{perf_alloc(PC_ELAPSED, MODULE_NAME": create file")}; + perf_counter_t _listing_elapsed{perf_alloc(PC_ELAPSED, MODULE_NAME": listing")}; }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 06fb97c8a65f..58d2c044ccd0 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -406,6 +406,34 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) _mavlink.handle_message(msg); } +void MavlinkReceiver::handle_messages_in_gimbal_mode(mavlink_message_t &msg) +{ + switch (msg.msgid) { + case MAVLINK_MSG_ID_HEARTBEAT: + handle_message_heartbeat(&msg); + break; + + case MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE: + handle_message_gimbal_manager_set_attitude(&msg); + break; + + case MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL: + handle_message_gimbal_manager_set_manual_control(&msg); + break; + + case MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION: + handle_message_gimbal_device_information(&msg); + break; + + case MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS: + handle_message_gimbal_device_attitude_status(&msg); + break; + } + + // Message forwarding + _mavlink.handle_message(&msg); +} + bool MavlinkReceiver::evaluate_target_ok(int command, int target_system, int target_component) { @@ -1598,11 +1626,6 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) const matrix::Quatf q{attitude_target.q}; q.copyTo(attitude_setpoint.q_d); - matrix::Eulerf euler{q}; - attitude_setpoint.roll_body = euler.phi(); - attitude_setpoint.pitch_body = euler.theta(); - attitude_setpoint.yaw_body = euler.psi(); - // TODO: review use case attitude_setpoint.yaw_sp_move_rate = (type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE) ? (float)NAN : attitude_target.body_yaw_rate; @@ -2110,6 +2133,19 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) manual_control_setpoint.yaw = mavlink_manual_control.r / 1000.f; // Pass along the button states manual_control_setpoint.buttons = mavlink_manual_control.buttons; + + if (mavlink_manual_control.enabled_extensions & (1u << 2)) { manual_control_setpoint.aux1 = mavlink_manual_control.aux1 / 1000.0f; } + + if (mavlink_manual_control.enabled_extensions & (1u << 3)) { manual_control_setpoint.aux2 = mavlink_manual_control.aux2 / 1000.0f; } + + if (mavlink_manual_control.enabled_extensions & (1u << 4)) { manual_control_setpoint.aux3 = mavlink_manual_control.aux3 / 1000.0f; } + + if (mavlink_manual_control.enabled_extensions & (1u << 5)) { manual_control_setpoint.aux4 = mavlink_manual_control.aux4 / 1000.0f; } + + if (mavlink_manual_control.enabled_extensions & (1u << 6)) { manual_control_setpoint.aux5 = mavlink_manual_control.aux5 / 1000.0f; } + + if (mavlink_manual_control.enabled_extensions & (1u << 7)) { manual_control_setpoint.aux6 = mavlink_manual_control.aux6 / 1000.0f; } + manual_control_setpoint.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0 + _mavlink.get_instance_id(); manual_control_setpoint.timestamp = manual_control_setpoint.timestamp_sample = hrt_absolute_time(); manual_control_setpoint.valid = true; @@ -3163,7 +3199,16 @@ MavlinkReceiver::run() _mavlink.set_proto_version(2); } - handle_message(&msg); + switch (_mavlink.get_mode()) { + case Mavlink::MAVLINK_MODE::MAVLINK_MODE_GIMBAL: + handle_messages_in_gimbal_mode(msg); + break; + + default: + handle_message(&msg); + break; + } + _mavlink.set_has_received_messages(true); // Received first message, unlock wait to transmit '-w' command-line flag update_rx_stats(msg); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 9f2882e84835..b95bdca59a72 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -155,6 +155,7 @@ class MavlinkReceiver : public ModuleParams float param4 = 0.0f, float param5 = 0.0f, float param6 = 0.0f, float param7 = 0.0f); void handle_message(mavlink_message_t *msg); + void handle_messages_in_gimbal_mode(mavlink_message_t &msg); void handle_message_adsb_vehicle(mavlink_message_t *msg); void handle_message_att_pos_mocap(mavlink_message_t *msg); diff --git a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp index 32d244b138da..6aca9f7d9837 100644 --- a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp +++ b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp @@ -264,7 +264,9 @@ class MavlinkStreamHighLatency2 : public MavlinkStream vehicle_attitude_setpoint_s attitude_sp; if (_attitude_sp_sub.update(&attitude_sp)) { - msg->target_heading = static_cast(math::degrees(matrix::wrap_2pi(attitude_sp.yaw_body)) * 0.5f); + + msg->target_heading = static_cast(math::degrees(matrix::wrap_2pi(matrix::Eulerf(matrix::Quatf( + attitude_sp.q_d)).psi())) * 0.5f); return true; } diff --git a/src/modules/mavlink/streams/MANUAL_CONTROL.hpp b/src/modules/mavlink/streams/MANUAL_CONTROL.hpp index 8be68fdc3fad..3feabf124d68 100644 --- a/src/modules/mavlink/streams/MANUAL_CONTROL.hpp +++ b/src/modules/mavlink/streams/MANUAL_CONTROL.hpp @@ -68,10 +68,10 @@ class MavlinkStreamManualControl : public MavlinkStream mavlink_manual_control_t msg{}; msg.target = mavlink_system.sysid; - msg.x = manual_control_setpoint.pitch * 1000; - msg.y = manual_control_setpoint.roll * 1000; - msg.z = manual_control_setpoint.throttle * 1000; - msg.r = manual_control_setpoint.yaw * 1000; + msg.x = manual_control_setpoint.pitch * 1000.f; + msg.y = manual_control_setpoint.roll * 1000.f; + msg.z = manual_control_setpoint.throttle * 1000.f; + msg.r = manual_control_setpoint.yaw * 1000.f; manual_control_switches_s manual_control_switches{}; @@ -84,6 +84,36 @@ class MavlinkStreamManualControl : public MavlinkStream msg.buttons |= (manual_control_switches.kill_switch << (shift * 6)); } + if (PX4_ISFINITE(manual_control_setpoint.aux1)) { + msg.enabled_extensions |= (1u << 2); + msg.aux1 = manual_control_setpoint.aux1 * 1000.f; + } + + if (PX4_ISFINITE(manual_control_setpoint.aux2)) { + msg.enabled_extensions |= (1u << 3); + msg.aux2 = manual_control_setpoint.aux2 * 1000.f; + } + + if (PX4_ISFINITE(manual_control_setpoint.aux3)) { + msg.enabled_extensions |= (1u << 4); + msg.aux3 = manual_control_setpoint.aux3 * 1000.f; + } + + if (PX4_ISFINITE(manual_control_setpoint.aux4)) { + msg.enabled_extensions |= (1u << 5); + msg.aux4 = manual_control_setpoint.aux4 * 1000.f; + } + + if (PX4_ISFINITE(manual_control_setpoint.aux5)) { + msg.enabled_extensions |= (1u << 6); + msg.aux5 = manual_control_setpoint.aux5 * 1000.f; + } + + if (PX4_ISFINITE(manual_control_setpoint.aux6)) { + msg.enabled_extensions |= (1u << 7); + msg.aux6 = manual_control_setpoint.aux6 * 1000.f; + } + mavlink_msg_manual_control_send_struct(_mavlink->get_channel(), &msg); return true; diff --git a/src/modules/mavlink/streams/SYS_STATUS.hpp b/src/modules/mavlink/streams/SYS_STATUS.hpp index 272947b1ad77..c875eaceaae9 100644 --- a/src/modules/mavlink/streams/SYS_STATUS.hpp +++ b/src/modules/mavlink/streams/SYS_STATUS.hpp @@ -150,7 +150,11 @@ class MavlinkStreamSysStatus : public MavlinkStream fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_3D_MAG, health_component_t::magnetometer, msg); fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_GPS, health_component_t::gps, msg); fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_RC_RECEIVER, health_component_t::remote_control, msg); - fillOutComponent(health_report, MAV_SYS_STATUS_AHRS, health_component_t::local_position_estimate, msg); + fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION, health_component_t::attitude_estimate, + msg); + fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL, health_component_t::local_position_estimate, + msg); + fillOutComponent(health_report, MAV_SYS_STATUS_AHRS, health_component_t::attitude_estimate, msg); fillOutComponent(health_report, MAV_SYS_STATUS_LOGGING, health_component_t::logging, msg); fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE, health_component_t::absolute_pressure, msg); fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE, health_component_t::differential_pressure, diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 39cb033e74dc..f6cc5db43d88 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -182,12 +182,6 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt, q_sp.copyTo(attitude_setpoint.q_d); - // Transform to euler angles for logging only - const Eulerf euler_sp(q_sp); - attitude_setpoint.roll_body = euler_sp(0); - attitude_setpoint.pitch_body = euler_sp(1); - attitude_setpoint.yaw_body = euler_sp(2); - attitude_setpoint.thrust_body[2] = -throttle_curve(_manual_control_setpoint.throttle); attitude_setpoint.timestamp = hrt_absolute_time(); diff --git a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp index b4eba96115cd..3d1b1268c2cc 100644 --- a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp +++ b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp @@ -111,12 +111,6 @@ void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpo // copy quaternion setpoint to attitude setpoint topic const Quatf q_sp{R_sp}; q_sp.copyTo(att_sp.q_d); - - // calculate euler angles, for logging only, must not be used for control - const Eulerf euler{R_sp}; - att_sp.roll_body = euler.phi(); - att_sp.pitch_body = euler.theta(); - att_sp.yaw_body = euler.psi(); } Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max) diff --git a/src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp b/src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp index e9b6fba12fb4..ebb23d7ed14e 100644 --- a/src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp +++ b/src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp @@ -108,18 +108,20 @@ TEST(ControlMathTest, ThrottleAttitudeMapping) float yaw = 0.f; vehicle_attitude_setpoint_s att{}; thrustToAttitude(thr, yaw, att); - EXPECT_FLOAT_EQ(att.roll_body, 0.f); - EXPECT_FLOAT_EQ(att.pitch_body, 0.f); - EXPECT_FLOAT_EQ(att.yaw_body, 0.f); + EXPECT_FLOAT_EQ(att.q_d[0], 1.f); + EXPECT_FLOAT_EQ(att.q_d[1], 0.f); + EXPECT_FLOAT_EQ(att.q_d[2], 0.f); + EXPECT_FLOAT_EQ(att.q_d[3], 0.f); EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f); /* expected: same as before but with 90 yaw * reason: only yaw changed */ yaw = M_PI_2_F; thrustToAttitude(thr, yaw, att); - EXPECT_FLOAT_EQ(att.roll_body, 0.f); - EXPECT_FLOAT_EQ(att.pitch_body, 0.f); - EXPECT_FLOAT_EQ(att.yaw_body, M_PI_2_F); + Eulerf euler_att(Quatf(att.q_d)); + EXPECT_FLOAT_EQ(euler_att.phi(), 0.f); + EXPECT_FLOAT_EQ(euler_att.theta(), 0.f); + EXPECT_FLOAT_EQ(euler_att.psi(), M_PI_2_F); EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f); /* expected: same as before but roll 180 @@ -127,9 +129,10 @@ TEST(ControlMathTest, ThrottleAttitudeMapping) * order is: 1. roll, 2. pitch, 3. yaw */ thr = Vector3f(0.f, 0.f, 1.f); thrustToAttitude(thr, yaw, att); - EXPECT_FLOAT_EQ(att.roll_body, -M_PI_F); - EXPECT_FLOAT_EQ(att.pitch_body, 0.f); - EXPECT_FLOAT_EQ(att.yaw_body, M_PI_2_F); + Eulerf euler_att2(Quatf(att.q_d)); + EXPECT_FLOAT_EQ(std::abs(euler_att2.phi()), std::abs(M_PI_F)); + EXPECT_FLOAT_EQ(euler_att2.theta(), 0.f); + EXPECT_FLOAT_EQ(euler_att2.psi(), M_PI_2_F); EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f); } diff --git a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp index 0e5a60619a44..9c1e72b1a5c8 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp @@ -56,9 +56,10 @@ TEST(PositionControlTest, EmptySetpoint) vehicle_attitude_setpoint_s attitude{}; position_control.getAttitudeSetpoint(attitude); - EXPECT_FLOAT_EQ(attitude.roll_body, 0.f); - EXPECT_FLOAT_EQ(attitude.pitch_body, 0.f); - EXPECT_FLOAT_EQ(attitude.yaw_body, 0.f); + Eulerf euler_att(Quatf(attitude.q_d)); + EXPECT_FLOAT_EQ(euler_att.phi(), 0.f); + EXPECT_FLOAT_EQ(euler_att.theta(), 0.f); + EXPECT_FLOAT_EQ(euler_att.psi(), 0.f); EXPECT_FLOAT_EQ(attitude.yaw_sp_move_rate, 0.f); EXPECT_EQ(Quatf(attitude.q_d), Quatf(1.f, 0.f, 0.f, 0.f)); EXPECT_EQ(Vector3f(attitude.thrust_body), Vector3f(0.f, 0.f, 0.f)); @@ -184,7 +185,8 @@ TEST_F(PositionControlBasicTest, PositionControlMaxThrustLimit) EXPECT_FLOAT_EQ(_attitude.thrust_body[2], -MAXIMUM_THRUST); // Then the horizontal margin results in a tilt with the ratio of: horizontal margin / maximum thrust - EXPECT_FLOAT_EQ(_attitude.roll_body, asin((HORIZONTAL_THRUST_MARGIN / sqrt(2.f)) / MAXIMUM_THRUST)); + Eulerf euler_att(Quatf(_attitude.q_d)); + EXPECT_FLOAT_EQ(euler_att.phi(), asin((HORIZONTAL_THRUST_MARGIN / sqrt(2.f)) / MAXIMUM_THRUST)); // TODO: add this line back once attitude setpoint generation strategy does not align body yaw with heading all the time anymore // EXPECT_FLOAT_EQ(_attitude.pitch_body, -asin((HORIZONTAL_THRUST_MARGIN / sqrt(2.f)) / MAXIMUM_THRUST)); } @@ -198,9 +200,10 @@ TEST_F(PositionControlBasicTest, PositionControlMinThrustLimit) EXPECT_FLOAT_EQ(thrust.length(), 0.1f); EXPECT_FLOAT_EQ(_attitude.thrust_body[2], -0.1f); + Eulerf euler_att(Quatf(_attitude.q_d)); - EXPECT_FLOAT_EQ(_attitude.roll_body, 0.f); - EXPECT_FLOAT_EQ(_attitude.pitch_body, -1.f); + EXPECT_FLOAT_EQ(euler_att.phi(), 0.f); + EXPECT_FLOAT_EQ(euler_att.theta(), -1.f); } TEST_F(PositionControlBasicTest, FailsafeInput) @@ -377,6 +380,7 @@ TEST_F(PositionControlBasicTest, IntegratorWindupWithInvalidSetpoint) EXPECT_TRUE(runController()); // THEN: the integral did not wind up and produce unexpected deviation - EXPECT_FLOAT_EQ(_attitude.roll_body, 0.f); - EXPECT_FLOAT_EQ(_attitude.pitch_body, 0.f); + Eulerf euler_att(Quatf(_attitude.q_d)); + EXPECT_FLOAT_EQ(euler_att.phi(), 0.f); + EXPECT_FLOAT_EQ(euler_att.theta(), 0.f); } diff --git a/src/modules/mc_pos_control/multicopter_altitude_mode_params.c b/src/modules/mc_pos_control/multicopter_altitude_mode_params.c index 3f654fade71a..9174b1ef0d83 100644 --- a/src/modules/mc_pos_control/multicopter_altitude_mode_params.c +++ b/src/modules/mc_pos_control/multicopter_altitude_mode_params.c @@ -95,7 +95,7 @@ PARAM_DEFINE_INT32(MPC_ALT_MODE, 2); /** * Maximum horizontal velocity for which position hold is enabled (use 0 to disable check) * - * Only used with MPC_POS_MODE 0 or MPC_ALT_MODE 2 + * Only used with MPC_POS_MODE Direct velocity or MPC_ALT_MODE 2 * * @unit m/s * @min 0 diff --git a/src/modules/mc_pos_control/multicopter_autonomous_params.c b/src/modules/mc_pos_control/multicopter_autonomous_params.c index 8c97e779be32..e2e453d975ed 100644 --- a/src/modules/mc_pos_control/multicopter_autonomous_params.c +++ b/src/modules/mc_pos_control/multicopter_autonomous_params.c @@ -76,7 +76,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_V_AUTO_DN, 1.5f); /** * Acceleration for autonomous and for manual modes * - * When piloting manually, this parameter is only used in MPC_POS_MODE 4. + * When piloting manually, this parameter is only used in MPC_POS_MODE Acceleration based. * * @unit m/s^2 * @min 2 diff --git a/src/modules/mc_pos_control/multicopter_position_control_limits_params.c b/src/modules/mc_pos_control/multicopter_position_control_limits_params.c index 91039f04ea6d..8307b82d7259 100644 --- a/src/modules/mc_pos_control/multicopter_position_control_limits_params.c +++ b/src/modules/mc_pos_control/multicopter_position_control_limits_params.c @@ -143,6 +143,8 @@ PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.f); * Acceleration to tilt coupling * * Set to decouple tilt from vertical acceleration. + * This provides smoother flight but slightly worse tracking in position and auto modes. + * Unset if accurate position tracking during dynamic maneuvers is more important than a smooth flight. * * @boolean * @group Multicopter Position Control diff --git a/src/modules/mc_pos_control/multicopter_position_mode_params.c b/src/modules/mc_pos_control/multicopter_position_mode_params.c index 56adad1144bf..a65ef4f5713d 100644 --- a/src/modules/mc_pos_control/multicopter_position_mode_params.c +++ b/src/modules/mc_pos_control/multicopter_position_mode_params.c @@ -35,12 +35,15 @@ * Position/Altitude mode variant * * The supported sub-modes are: - * 0 Sticks directly map to velocity setpoints without smoothing. + * - "Direct velocity": + * Sticks directly map to velocity setpoints without smoothing. * Also applies to vertical direction and Altitude mode. * Useful for velocity control tuning. - * 3 Sticks map to velocity but with maximum acceleration and jerk limits based on + * - "Smoothed velocity": + * Sticks map to velocity but with maximum acceleration and jerk limits based on * jerk optimized trajectory generator (different algorithm than 1). - * 4 Sticks map to acceleration and there's a virtual brake drag + * - "Acceleration based": + * Sticks map to acceleration and there's a virtual brake drag * * @value 0 Direct velocity * @value 3 Smoothed velocity @@ -122,7 +125,7 @@ PARAM_DEFINE_FLOAT(MPC_ACC_HOR_MAX, 5.f); * * Setting this to the maximum value essentially disables the limit. * - * Only used with smooth MPC_POS_MODE 3 and 4. + * Only used with smooth MPC_POS_MODE Smoothed velocity and Acceleration based. * * @unit m/s^3 * @min 0.5 diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp index c684f0119d9b..c2b3a8119807 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp @@ -49,7 +49,6 @@ void FeasibilityChecker::reset() _mission_validity_failed = false; _takeoff_failed = false; _land_pattern_validity_failed = false; - _distance_first_waypoint_failed = false; _distance_between_waypoints_failed = false; _fixed_wing_land_approach_failed = false; _takeoff_land_available_failed = false; @@ -118,12 +117,6 @@ void FeasibilityChecker::updateData() _is_landed = land_detected.landed; } - if (_vehicle_global_position_sub.updated()) { - vehicle_global_position_s vehicle_global_position = {}; - _vehicle_global_position_sub.copy(&vehicle_global_position); - _current_position_lat_lon = matrix::Vector2d(vehicle_global_position.lat, vehicle_global_position.lon); - } - if (_rtl_status_sub.updated()) { rtl_status_s rtl_status = {}; _rtl_status_sub.copy(&rtl_status); @@ -199,8 +192,8 @@ void FeasibilityChecker::doCommonChecks(mission_item_s &mission_item, const int _distance_between_waypoints_failed = !checkDistancesBetweenWaypoints(mission_item); } - if (!_distance_first_waypoint_failed) { - _distance_first_waypoint_failed = !checkHorizontalDistanceToFirstWaypoint(mission_item); + if (!_first_waypoint_found) { + checkHorizontalDistanceToFirstWaypoint(mission_item); } if (!_takeoff_failed) { @@ -631,31 +624,32 @@ bool FeasibilityChecker::hasMissionBothOrNeitherTakeoffAndLanding() bool FeasibilityChecker::checkHorizontalDistanceToFirstWaypoint(mission_item_s &mission_item) { if (_param_mis_dist_1wp > FLT_EPSILON && - (_current_position_lat_lon.isAllFinite()) && !_first_waypoint_found && + (_home_lat_lon.isAllFinite()) && MissionBlock::item_contains_position(mission_item)) { _first_waypoint_found = true; - float dist_to_1wp_from_current_pos = 1e6f; - - if (_current_position_lat_lon.isAllFinite()) { - dist_to_1wp_from_current_pos = get_distance_to_next_waypoint( - mission_item.lat, mission_item.lon, - _current_position_lat_lon(0), _current_position_lat_lon(1)); - } + const float dist_to_1wp_from_home_pos = get_distance_to_next_waypoint( + mission_item.lat, mission_item.lon, + _home_lat_lon(0), _home_lat_lon(1)); - if (dist_to_1wp_from_current_pos < _param_mis_dist_1wp) { + if (dist_to_1wp_from_home_pos < _param_mis_dist_1wp) { return true; } else { /* item is too far from current position */ mavlink_log_critical(_mavlink_log_pub, - "First waypoint too far away: %dm, %d max\t", - (int)dist_to_1wp_from_current_pos, (int)_param_mis_dist_1wp); - events::send(events::ID("navigator_mis_first_wp_too_far"), {events::Log::Error, events::LogInternal::Info}, - "First waypoint too far away: {1m} (maximum: {2m})", (uint32_t)dist_to_1wp_from_current_pos, - (uint32_t)_param_mis_dist_1wp); + "First waypoint far away from home: %dm. Correct mission loaded?\t", + (int)dist_to_1wp_from_home_pos); + /* EVENT + * @description + * + * This check can be configured via MIS_DIST_1WP parameter. + * + */ + events::send(events::ID("navigator_mis_first_wp_far"), {events::Log::Warning, events::LogInternal::Info}, + "First waypoint far away from Home: {1m} Correct mission loaded?", (uint32_t)dist_to_1wp_from_home_pos); return false; } diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp index fd18cbcfb68e..d0905d363545 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp @@ -78,7 +78,6 @@ class FeasibilityChecker : public ModuleParams bool someCheckFailed() { return _takeoff_failed || - _distance_first_waypoint_failed || _distance_between_waypoints_failed || _land_pattern_validity_failed || _fixed_wing_land_approach_failed || @@ -97,7 +96,6 @@ class FeasibilityChecker : public ModuleParams uORB::Subscription _home_pos_sub{ORB_ID(home_position)}; uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _land_detector_sub{ORB_ID(vehicle_land_detected)}; - uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; uORB::Subscription _rtl_status_sub{ORB_ID(rtl_status)}; // parameters @@ -110,14 +108,12 @@ class FeasibilityChecker : public ModuleParams float _home_alt_msl{NAN}; bool _has_vtol_approach{false}; matrix::Vector2d _home_lat_lon = matrix::Vector2d((double)NAN, (double)NAN); - matrix::Vector2d _current_position_lat_lon = matrix::Vector2d((double)NAN, (double)NAN); VehicleType _vehicle_type{VehicleType::RotaryWing}; // internal flags to keep track of which checks failed bool _mission_validity_failed{false}; bool _takeoff_failed{false}; bool _land_pattern_validity_failed{false}; - bool _distance_first_waypoint_failed{false}; bool _distance_between_waypoints_failed{false}; bool _fixed_wing_land_approach_failed{false}; bool _takeoff_land_available_failed{false}; diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp index 0de54f33766c..43662441488a 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp @@ -170,9 +170,9 @@ TEST_F(FeasibilityCheckerTest, check_dist_first_waypoint) mission_item.lat = lat_new; mission_item.lon = lon_new; - // THEN: fail + // THEN: pass checker.processNextItem(mission_item, 0, 1); - ASSERT_EQ(checker.someCheckFailed(), true); + ASSERT_EQ(checker.someCheckFailed(), false); // BUT WHEN: valid current position fist WP 499m away from current checker.reset(); diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 55917331a614..00a1e3f94011 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -104,7 +104,7 @@ void Geofence::run() _initiate_fence_updated = false; _dataman_state = DatamanState::Read; - geofence_status_s status; + geofence_status_s status{}; status.timestamp = hrt_absolute_time(); status.geofence_id = _opaque_id; status.status = geofence_status_s::GF_STATUS_LOADING; @@ -159,7 +159,7 @@ void Geofence::run() _dataman_state = DatamanState::UpdateRequestWait; _fence_updated = true; - geofence_status_s status; + geofence_status_s status{}; status.timestamp = hrt_absolute_time(); status.geofence_id = _opaque_id; status.status = geofence_status_s::GF_STATUS_READY; @@ -179,7 +179,7 @@ void Geofence::run() _updateFence(); _fence_updated = true; - geofence_status_s status; + geofence_status_s status{}; status.timestamp = hrt_absolute_time(); status.geofence_id = _opaque_id; status.status = geofence_status_s::GF_STATUS_READY; @@ -406,17 +406,25 @@ bool Geofence::checkPointAgainstPolygonCircle(const PolygonInfo &polygon, double { bool checksPass = true; - if (polygon.fence_type == NAV_CMD_FENCE_CIRCLE_INCLUSION) { + switch (polygon.fence_type) { + case NAV_CMD_FENCE_CIRCLE_INCLUSION: checksPass &= insideCircle(polygon, lat, lon, altitude); + break; - } else if (polygon.fence_type == NAV_CMD_FENCE_CIRCLE_EXCLUSION) { + case NAV_CMD_FENCE_CIRCLE_EXCLUSION: checksPass &= !insideCircle(polygon, lat, lon, altitude); + break; - } else if (polygon.fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION) { + case NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION: checksPass &= insidePolygon(polygon, lat, lon, altitude); + break; - } else if (polygon.fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION) { + case NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION: checksPass &= !insidePolygon(polygon, lat, lon, altitude); + break; + + default: // unknown fence type + break; } return checksPass; @@ -452,12 +460,18 @@ bool Geofence::insidePolygon(const PolygonInfo &polygon, double lat, double lon, break; } - if (temp_vertex_i.frame != NAV_FRAME_GLOBAL && temp_vertex_i.frame != NAV_FRAME_GLOBAL_INT - && temp_vertex_i.frame != NAV_FRAME_GLOBAL_RELATIVE_ALT - && temp_vertex_i.frame != NAV_FRAME_GLOBAL_RELATIVE_ALT_INT) { + switch (temp_vertex_i.frame) { + case NAV_FRAME_GLOBAL: + case NAV_FRAME_GLOBAL_INT: + case NAV_FRAME_GLOBAL_RELATIVE_ALT: + case NAV_FRAME_GLOBAL_RELATIVE_ALT_INT: + break; + + default: // TODO: handle different frames PX4_ERR("Frame type %i not supported", (int)temp_vertex_i.frame); - break; + return c; + } if (((double)temp_vertex_i.lon >= lon) != ((double)temp_vertex_j.lon >= lon) && @@ -482,12 +496,18 @@ bool Geofence::insideCircle(const PolygonInfo &polygon, double lat, double lon, return false; } - if (circle_point.frame != NAV_FRAME_GLOBAL && circle_point.frame != NAV_FRAME_GLOBAL_INT - && circle_point.frame != NAV_FRAME_GLOBAL_RELATIVE_ALT - && circle_point.frame != NAV_FRAME_GLOBAL_RELATIVE_ALT_INT) { + switch (circle_point.frame) { + case NAV_FRAME_GLOBAL: + case NAV_FRAME_GLOBAL_INT: + case NAV_FRAME_GLOBAL_RELATIVE_ALT: + case NAV_FRAME_GLOBAL_RELATIVE_ALT_INT: + break; + + default: // TODO: handle different frames PX4_ERR("Frame type %i not supported", (int)circle_point.frame); return false; + } if (!_projection_reference.isInitialized()) { @@ -648,7 +668,7 @@ Geofence::loadFromFile(const char *filename) } else { mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence: import error\t"); - events::send(events::ID("navigator_geofence_import_failed"), events::Log::Error, "Geofence: import error"); + events::send(events::ID("navigator_geofence_import_failed"), events::Log::Critical, "Geofence: import error"); } updateFence(); @@ -675,20 +695,26 @@ void Geofence::printStatus() for (int i = 0; i < _num_polygons; ++i) { total_num_vertices += _polygons[i].vertex_count; - if (_polygons[i].fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION) { + switch (_polygons[i].fence_type) { + case NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION: ++num_inclusion_polygons; - } + break; - if (_polygons[i].fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION) { + case NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION: ++num_exclusion_polygons; - } + break; - if (_polygons[i].fence_type == NAV_CMD_FENCE_CIRCLE_INCLUSION) { + case NAV_CMD_FENCE_CIRCLE_INCLUSION: ++num_inclusion_circles; - } + break; - if (_polygons[i].fence_type == NAV_CMD_FENCE_CIRCLE_EXCLUSION) { + case NAV_CMD_FENCE_CIRCLE_EXCLUSION: ++num_exclusion_circles; + break; + + default: // unknown fence type + break; + } } diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp index b6a951d1b766..8277378a6670 100644 --- a/src/modules/navigator/land.cpp +++ b/src/modules/navigator/land.cpp @@ -42,7 +42,7 @@ #include "navigator.h" Land::Land(Navigator *navigator) : - MissionBlock(navigator) + MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_LAND) { } @@ -57,8 +57,13 @@ Land::on_activation() /* convert mission item to current setpoint */ struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - pos_sp_triplet->previous.valid = false; + + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + _navigator->calculate_breaking_stop(_mission_item.lat, _mission_item.lon); + } + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->previous.valid = false; pos_sp_triplet->next.valid = false; _navigator->set_position_setpoint_triplet_updated(); @@ -93,7 +98,7 @@ Land::on_active() if (_navigator->get_land_detected()->landed) { _navigator->get_mission_result()->finished = true; _navigator->set_mission_result_updated(); - _navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND); + _navigator->mode_completed(getNavigatorStateId()); set_idle_item(&_mission_item); struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index 15408baaff05..db61030528c1 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -43,7 +43,7 @@ #include "navigator.h" Loiter::Loiter(Navigator *navigator) : - MissionBlock(navigator), + MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER), ModuleParams(navigator) { } diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 2949c88d62b3..ea7dae675217 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -65,7 +65,7 @@ using namespace time_literals; static constexpr int32_t DEFAULT_MISSION_CACHE_SIZE = 10; Mission::Mission(Navigator *navigator) : - MissionBase(navigator, DEFAULT_MISSION_CACHE_SIZE) + MissionBase(navigator, DEFAULT_MISSION_CACHE_SIZE, vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) { } diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index 0a9a76bd2b34..8963a0685d08 100644 --- a/src/modules/navigator/mission_base.cpp +++ b/src/modules/navigator/mission_base.cpp @@ -46,8 +46,8 @@ #include "mission_feasibility_checker.h" #include "navigator.h" -MissionBase::MissionBase(Navigator *navigator, int32_t dataman_cache_size_signed) : - MissionBlock(navigator), +MissionBase::MissionBase(Navigator *navigator, int32_t dataman_cache_size_signed, uint8_t navigator_state_id) : + MissionBlock(navigator, navigator_state_id), ModuleParams(navigator), _dataman_cache_size_signed(dataman_cache_size_signed) { @@ -235,6 +235,7 @@ MissionBase::on_activation() checkClimbRequired(_mission.current_seq); set_mission_items(); + _mission_activation_index = _mission.current_seq; _inactivation_index = -1; // reset // reset cruise speed @@ -293,17 +294,27 @@ MissionBase::on_active() _align_heading_necessary = false; } - // replay gimbal and camera commands immediately after resuming mission - if (haveCachedGimbalOrCameraItems()) { - replayCachedGimbalCameraItems(); + // Replay camera mode commands immediately upon mission resume + if (haveCachedCameraModeItems()) { + replayCachedCameraModeItems(); } - // replay trigger commands upon raching the resume waypoint if the trigger relay flag is set - if (cameraWasTriggering() && is_mission_item_reached_or_completed()) { - replayCachedTriggerItems(); - } - replayCachedSpeedChangeItems(); + // Replay cached mission commands once the last mission waypoint is re-reached after the mission interruption. + // Each replay function also clears the cached items afterwards + if (_mission.current_seq > _mission_activation_index) { + // replay gimbal commands + if (haveCachedGimbalItems()) { + replayCachedGimbalItems(); + } + + // replay trigger commands + if (cameraWasTriggering()) { + replayCachedTriggerItems(); + } + + replayCachedSpeedChangeItems(); + } /* lets check if we reached the current mission item */ if (_mission_type != MissionType::MISSION_TYPE_NONE && is_mission_item_reached_or_completed()) { @@ -465,7 +476,7 @@ MissionBase::set_mission_items() if (set_end_of_mission) { setEndOfMissionItems(); - _navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION); + _navigator->mode_completed(getNavigatorStateId()); } } @@ -870,7 +881,6 @@ void MissionBase::publish_navigator_mission_item() { navigator_mission_item_s navigator_mission_item{}; - navigator_mission_item.instance_count = _navigator->mission_instance_count(); navigator_mission_item.sequence_current = _mission.current_seq; navigator_mission_item.nav_cmd = _mission_item.nav_cmd; navigator_mission_item.latitude = _mission_item.lat; @@ -1256,7 +1266,7 @@ void MissionBase::cacheItem(const mission_item_s &mission_item) } } -void MissionBase::replayCachedGimbalCameraItems() +void MissionBase::replayCachedGimbalItems() { if (_last_gimbal_configure_item.nav_cmd > 0) { issue_command(_last_gimbal_configure_item); @@ -1267,7 +1277,10 @@ void MissionBase::replayCachedGimbalCameraItems() issue_command(_last_gimbal_control_item); _last_gimbal_control_item = {}; // delete cached item } +} +void MissionBase::replayCachedCameraModeItems() +{ if (_last_camera_mode_item.nav_cmd > 0) { issue_command(_last_camera_mode_item); _last_camera_mode_item = {}; // delete cached item @@ -1298,11 +1311,15 @@ void MissionBase::resetItemCache() _last_camera_trigger_item = {}; } -bool MissionBase::haveCachedGimbalOrCameraItems() +bool MissionBase::haveCachedGimbalItems() { return _last_gimbal_configure_item.nav_cmd > 0 || - _last_gimbal_control_item.nav_cmd > 0 || - _last_camera_mode_item.nav_cmd > 0; + _last_gimbal_control_item.nav_cmd > 0; +} + +bool MissionBase::haveCachedCameraModeItems() +{ + return _last_camera_mode_item.nav_cmd > 0; } bool MissionBase::cameraWasTriggering() diff --git a/src/modules/navigator/mission_base.h b/src/modules/navigator/mission_base.h index 2f416f29e206..f819b5c0e51a 100644 --- a/src/modules/navigator/mission_base.h +++ b/src/modules/navigator/mission_base.h @@ -65,7 +65,7 @@ class Navigator; class MissionBase : public MissionBlock, public ModuleParams { public: - MissionBase(Navigator *navigator, int32_t dataman_cache_size_signed); + MissionBase(Navigator *navigator, int32_t dataman_cache_size_signed, uint8_t navigator_state_id); ~MissionBase() override = default; virtual void on_inactive() override; @@ -328,6 +328,7 @@ class MissionBase : public MissionBlock, public ModuleParams mission_s _mission; /**< Currently active mission*/ float _mission_init_climb_altitude_amsl{NAN}; /**< altitude AMSL the vehicle will climb to when mission starts */ int _inactivation_index{-1}; // index of mission item at which the mission was paused. Used to resume survey missions at previous waypoint to not lose images. + int _mission_activation_index{-1}; /**< Index of the mission item that will bring the vehicle back to a mission waypoint */ int32_t _load_mission_index{-1}; /**< Mission inted of loaded mission items in dataman cache*/ int32_t _dataman_cache_size_signed; /**< Size of the dataman cache. A negativ value indicates that previous mission items should be loaded, a positiv value the next mission items*/ @@ -398,9 +399,14 @@ class MissionBase : public MissionBlock, public ModuleParams void updateCachedItemsUpToIndex(int end_index); /** - * @brief Replay the cached gimbal and camera mode items + * @brief Replay the cached gimbal items */ - void replayCachedGimbalCameraItems(); + void replayCachedGimbalItems(); + + /** + * @brief Replay the cached camera mode items + */ + void replayCachedCameraModeItems(); /** * @brief Replay the cached trigger items @@ -415,11 +421,18 @@ class MissionBase : public MissionBlock, public ModuleParams void replayCachedSpeedChangeItems(); /** - * @brief Check if there are cached gimbal or camera mode items to be replayed + * @brief Check if there are cached gimbal items to be replayed + * + * @return true if there are cached items + */ + bool haveCachedGimbalItems(); + + /** + * @brief Check if there are cached camera mode items to be replayed * * @return true if there are cached items */ - bool haveCachedGimbalOrCameraItems(); + bool haveCachedCameraModeItems(); /** * @brief Check if the camera was triggering diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 1f0c9f0dd021..2659ad95cc57 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -55,8 +55,8 @@ using matrix::wrap_pi; -MissionBlock::MissionBlock(Navigator *navigator) : - NavigatorMode(navigator) +MissionBlock::MissionBlock(Navigator *navigator, uint8_t navigator_state_id) : + NavigatorMode(navigator, navigator_state_id) { } @@ -1042,3 +1042,29 @@ void MissionBlock::updateAltToAvoidTerrainCollisionAndRepublishTriplet(mission_i _mission_item.altitude_is_relative = false; } } + +void MissionBlock::updateFailsafeChecks() +{ + updateMaxHaglFailsafe(); +} + +void MissionBlock::updateMaxHaglFailsafe() +{ + const float target_alt = _navigator->get_position_setpoint_triplet()->current.alt; + + if (_navigator->get_global_position()->terrain_alt_valid + && ((target_alt - _navigator->get_global_position()->terrain_alt) > _navigator->get_local_position()->hagl_max)) { + // Handle case where the altitude setpoint is above the maximum HAGL (height above ground level) + mavlink_log_info(_navigator->get_mavlink_log_pub(), "Target altitude higher than max HAGL\t"); + events::send(events::ID("navigator_fail_max_hagl"), events::Log::Error, "Target altitude higher than max HAGL"); + + _navigator->trigger_hagl_failsafe(getNavigatorStateId()); + + // While waiting for a failsafe action from commander, keep the curren position + setLoiterItemFromCurrentPosition(&_mission_item); + + mission_item_to_position_setpoint(_mission_item, &_navigator->get_position_setpoint_triplet()->current); + + _navigator->set_position_setpoint_triplet_updated(); + } +} diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 13815b860211..c19fcfe11877 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -64,7 +64,7 @@ class MissionBlock : public NavigatorMode /** * Constructor */ - MissionBlock(Navigator *navigator); + MissionBlock(Navigator *navigator, uint8_t navigator_state_id); virtual ~MissionBlock() = default; MissionBlock(const MissionBlock &) = delete; @@ -145,6 +145,8 @@ class MissionBlock : public NavigatorMode void set_align_mission_item(struct mission_item_s *const mission_item, const struct mission_item_s *const mission_item_next) const; + void updateFailsafeChecks() override; + protected: /** * @brief heading mode for setting navigation items @@ -249,4 +251,8 @@ class MissionBlock : public NavigatorMode bool _payload_deploy_ack_successful{false}; // Flag to keep track of whether we received an acknowledgement for a successful payload deployment hrt_abstime _payload_deployed_time{0}; // Last payload deployment start time to handle timeouts float _payload_deploy_timeout_s{0.0f}; // Timeout for payload deployment in Mission class, to prevent endless loop if successful deployment ack is never received + +private: + void updateMaxHaglFailsafe(); + }; diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index dc2420d55bf5..8049df1236cf 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -41,7 +41,6 @@ */ #include "mission_feasibility_checker.h" -#include "MissionFeasibility/FeasibilityChecker.hpp" #include "mission_block.h" #include "navigator.h" diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 93f630dbf525..ec72881fc527 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -69,6 +69,7 @@ #include #include #include +#include #include #include #include @@ -249,8 +250,6 @@ class Navigator : public ModuleBase, public ModuleParams orb_advert_t *get_mavlink_log_pub() { return &_mavlink_log_pub; } - int mission_instance_count() const { return _mission_result.mission_id; } - void set_mission_failure_heading_timeout(); bool get_mission_start_land_available() { return _mission.get_land_start_available(); } @@ -285,8 +284,12 @@ class Navigator : public ModuleBase, public ModuleParams void mode_completed(uint8_t nav_state, uint8_t result = mode_completed_s::RESULT_SUCCESS); + void set_failsafe_status(uint8_t nav_state, bool failsafe); + void sendWarningDescentStoppedDueToTerrain(); + void trigger_hagl_failsafe(uint8_t nav_state); + private: int _local_pos_sub{-1}; @@ -307,6 +310,7 @@ class Navigator : public ModuleBase, public ModuleParams uORB::Publication _geofence_result_pub{ORB_ID(geofence_result)}; uORB::Publication _mission_result_pub{ORB_ID(mission_result)}; + uORB::Publication _navigator_status_pub{ORB_ID(navigator_status)}; uORB::Publication _pos_sp_triplet_pub{ORB_ID(position_setpoint_triplet)}; uORB::Publication _vehicle_cmd_ack_pub{ORB_ID(vehicle_command_ack)}; uORB::Publication _vehicle_cmd_pub{ORB_ID(vehicle_command)}; @@ -326,6 +330,7 @@ class Navigator : public ModuleBase, public ModuleParams // Publications geofence_result_s _geofence_result{}; + navigator_status_s _navigator_status{}; position_setpoint_triplet_s _pos_sp_triplet{}; /**< triplet of position setpoints */ position_setpoint_triplet_s _reposition_triplet{}; /**< triplet for non-mission direct position command */ position_setpoint_triplet_s _takeoff_triplet{}; /**< triplet for non-mission direct takeoff command */ @@ -335,7 +340,10 @@ class Navigator : public ModuleBase, public ModuleParams Geofence _geofence; /**< class that handles the geofence */ GeofenceBreachAvoidance _gf_breach_avoidance; - hrt_abstime _last_geofence_check = 0; + hrt_abstime _last_geofence_check{0}; + + bool _navigator_status_updated{false}; + hrt_abstime _last_navigator_status_publication{0}; hrt_abstime _wait_for_vehicle_status_timestamp{0}; /**< If non-zero, wait for vehicle_status update before processing next cmd */ @@ -388,6 +396,8 @@ class Navigator : public ModuleBase, public ModuleParams */ void publish_mission_result(); + void publish_navigator_status(); + void publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t result); bool geofence_allows_position(const vehicle_global_position_s &pos); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index f2694b595ab7..15db1abdc44d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -41,6 +41,7 @@ * @author Julian Oes * @author Anton Babushkin * @author Thomas Gubler + * and many more... */ #include "navigator.h" @@ -897,6 +898,8 @@ void Navigator::run() publish_mission_result(); } + publish_navigator_status(); + _geofence.run(); perf_end(_loop_perf); @@ -1355,6 +1358,40 @@ void Navigator::set_mission_failure_heading_timeout() } } +void Navigator::trigger_hagl_failsafe(const uint8_t nav_state) +{ + if ((_navigator_status.failure != navigator_status_s::FAILURE_HAGL) || _navigator_status.nav_state != nav_state) { + _navigator_status.failure = navigator_status_s::FAILURE_HAGL; + _navigator_status.nav_state = nav_state; + + _navigator_status_updated = true; + } +} + +void Navigator::publish_navigator_status() +{ + uint8_t current_nav_state = _vstatus.nav_state; + + if (_navigation_mode != nullptr) { + current_nav_state = _navigation_mode->getNavigatorStateId(); + } + + if (_navigator_status.nav_state != current_nav_state) { + _navigator_status.nav_state = current_nav_state; + _navigator_status.failure = navigator_status_s::FAILURE_NONE; + _navigator_status_updated = true; + } + + if (_navigator_status_updated + || (hrt_elapsed_time(&_last_navigator_status_publication) > 500_ms)) { + _navigator_status.timestamp = hrt_absolute_time(); + _navigator_status_pub.publish(_navigator_status); + + _navigator_status_updated = false; + _last_navigator_status_publication = hrt_absolute_time(); + } +} + void Navigator::publish_vehicle_cmd(vehicle_command_s *vcmd) { vcmd->timestamp = hrt_absolute_time(); diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp index b0ced9006c56..1c0882923402 100644 --- a/src/modules/navigator/navigator_mode.cpp +++ b/src/modules/navigator/navigator_mode.cpp @@ -42,8 +42,9 @@ #include "navigator_mode.h" #include "navigator.h" -NavigatorMode::NavigatorMode(Navigator *navigator) : - _navigator(navigator) +NavigatorMode::NavigatorMode(Navigator *navigator, uint8_t navigator_state_id) : + _navigator(navigator), + _navigator_state_id(navigator_state_id) { /* set initial mission items */ on_inactivation(); @@ -60,6 +61,7 @@ NavigatorMode::run(bool active) } else { /* periodic updates when active */ on_active(); + updateFailsafeChecks(); } } else { diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h index 60abdcd165e0..9164a92b92fc 100644 --- a/src/modules/navigator/navigator_mode.h +++ b/src/modules/navigator/navigator_mode.h @@ -41,12 +41,14 @@ #pragma once +#include + class Navigator; class NavigatorMode { public: - NavigatorMode(Navigator *navigator); + NavigatorMode(Navigator *navigator, uint8_t navigator_state_id); virtual ~NavigatorMode() = default; NavigatorMode(const NavigatorMode &) = delete; NavigatorMode &operator=(const NavigatorMode &) = delete; @@ -56,6 +58,8 @@ class NavigatorMode bool isActive() {return _active;}; + uint8_t getNavigatorStateId() const { return _navigator_state_id; } + /** * This function is called while the mode is inactive */ @@ -76,9 +80,12 @@ class NavigatorMode */ virtual void on_active(); + virtual void updateFailsafeChecks() {}; + protected: Navigator *_navigator{nullptr}; private: bool _active{false}; + uint8_t _navigator_state_id{0}; }; diff --git a/src/modules/navigator/precland.cpp b/src/modules/navigator/precland.cpp index 080491d37ca7..74c81569cf3d 100644 --- a/src/modules/navigator/precland.cpp +++ b/src/modules/navigator/precland.cpp @@ -62,7 +62,7 @@ static constexpr const char *LOST_TARGET_ERROR_MESSAGE = "Lost landing target while landing"; PrecLand::PrecLand(Navigator *navigator) : - MissionBlock(navigator), + MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND), ModuleParams(navigator) { _handle_param_acceleration_hor = param_find("MPC_ACC_HOR"); diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 2b6bd55b3f91..63e2ab185ff1 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -55,7 +55,7 @@ static constexpr float MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES{10.0f}; // [m] We static constexpr float MIN_DIST_THRESHOLD = 2.f; RTL::RTL(Navigator *navigator) : - NavigatorMode(navigator), + NavigatorMode(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_RTL), ModuleParams(navigator), _rtl_direct(navigator) { @@ -283,10 +283,12 @@ void RTL::on_active() case RtlType::RTL_MISSION_FAST_REVERSE: case RtlType::RTL_DIRECT_MISSION_LAND: _rtl_mission_type_handle->on_active(); + _rtl_mission_type_handle->updateFailsafeChecks(); break; case RtlType::RTL_DIRECT: _rtl_direct.on_active(); + _rtl_direct.updateFailsafeChecks(); break; default: @@ -528,13 +530,14 @@ float RTL::calculate_return_alt_from_cone_half_angle(const PositionYawSetpoint & // avoid the vehicle touching the ground while still moving horizontally. const float return_altitude_min_outside_acceptance_rad_amsl = rtl_position.alt + 2.0f * _param_nav_acc_rad.get(); - float return_altitude_amsl = rtl_position.alt + _param_rtl_return_alt.get(); + const float max_return_altitude = rtl_position.alt + _param_rtl_return_alt.get(); + + float return_altitude_amsl = max_return_altitude; if (destination_dist <= _param_nav_acc_rad.get()) { return_altitude_amsl = rtl_position.alt + 2.0f * destination_dist; } else { - if (destination_dist <= _param_rtl_min_dist.get()) { // constrain cone half angle to meaningful values. All other cases are already handled above. @@ -549,7 +552,7 @@ float RTL::calculate_return_alt_from_cone_half_angle(const PositionYawSetpoint & return_altitude_amsl = max(return_altitude_amsl, return_altitude_min_outside_acceptance_rad_amsl); } - return max(return_altitude_amsl, _global_pos_sub.get().alt); + return constrain(return_altitude_amsl, _global_pos_sub.get().alt, max_return_altitude); } void RTL::init_rtl_mission_type() diff --git a/src/modules/navigator/rtl_base.h b/src/modules/navigator/rtl_base.h index dc698687fce6..a4c88e06b026 100644 --- a/src/modules/navigator/rtl_base.h +++ b/src/modules/navigator/rtl_base.h @@ -46,7 +46,7 @@ class RtlBase : public MissionBase { public: RtlBase(Navigator *navigator, int32_t dataman_cache_size_signed): - MissionBase(navigator, dataman_cache_size_signed) {}; + MissionBase(navigator, dataman_cache_size_signed, vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) {}; virtual ~RtlBase() = default; virtual rtl_time_estimate_s calc_rtl_time_estimate() = 0; diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index b028c7fb358f..145b99d0d1db 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -51,7 +51,7 @@ using namespace math; RtlDirect::RtlDirect(Navigator *navigator) : - MissionBlock(navigator), + MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_RTL), ModuleParams(navigator) { _destination.lat = static_cast(NAN); @@ -104,8 +104,9 @@ void RtlDirect::on_active() set_rtl_item(); } - if (_rtl_state == RTLState::LOITER_HOLD) { //TODO: rename _rtl_state to _rtl_state_next + if (_rtl_state != RTLState::IDLE) { //TODO: rename _rtl_state to _rtl_state_next (when in IDLE we're actually in LAND) //check for terrain collision and update altitude if needed + // note: it may trigger multiple times during a RTL, as every time the altitude set is reset updateAltToAvoidTerrainCollisionAndRepublishTriplet(_mission_item); } @@ -317,7 +318,7 @@ void RtlDirect::set_rtl_item() case RTLState::IDLE: { set_idle_item(&_mission_item); - _navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL); + _navigator->mode_completed(getNavigatorStateId()); break; } @@ -337,6 +338,8 @@ void RtlDirect::set_rtl_item() _navigator->set_position_setpoint_triplet_updated(); } } + + publish_rtl_direct_navigator_mission_item(); // for logging } RtlDirect::RTLState RtlDirect::getActivationLandState() @@ -515,3 +518,32 @@ loiter_point_s RtlDirect::sanitizeLandApproach(loiter_point_s land_approach) con return sanitized_land_approach; } + +void RtlDirect::publish_rtl_direct_navigator_mission_item() +{ + navigator_mission_item_s navigator_mission_item{}; + + navigator_mission_item.sequence_current = static_cast(_rtl_state); + navigator_mission_item.nav_cmd = _mission_item.nav_cmd; + navigator_mission_item.latitude = _mission_item.lat; + navigator_mission_item.longitude = _mission_item.lon; + navigator_mission_item.altitude = _mission_item.altitude; + + navigator_mission_item.time_inside = get_time_inside(_mission_item); + navigator_mission_item.acceptance_radius = _mission_item.acceptance_radius; + navigator_mission_item.loiter_radius = _mission_item.loiter_radius; + navigator_mission_item.yaw = _mission_item.yaw; + + navigator_mission_item.frame = _mission_item.frame; + navigator_mission_item.frame = _mission_item.origin; + + navigator_mission_item.loiter_exit_xtrack = _mission_item.loiter_exit_xtrack; + navigator_mission_item.force_heading = _mission_item.force_heading; + navigator_mission_item.altitude_is_relative = _mission_item.altitude_is_relative; + navigator_mission_item.autocontinue = _mission_item.autocontinue; + navigator_mission_item.vtol_back_transition = _mission_item.vtol_back_transition; + + navigator_mission_item.timestamp = hrt_absolute_time(); + + _navigator_mission_item_pub.publish(navigator_mission_item); +} diff --git a/src/modules/navigator/rtl_direct.h b/src/modules/navigator/rtl_direct.h index 211b2779eae0..b1f6a75b81ae 100644 --- a/src/modules/navigator/rtl_direct.h +++ b/src/modules/navigator/rtl_direct.h @@ -47,6 +47,7 @@ #include #include #include +#include #include #include #include @@ -137,6 +138,12 @@ class RtlDirect : public MissionBlock, public ModuleParams */ void parameters_update(); + /** + * @brief Publish navigator mission item + * + */ + void publish_rtl_direct_navigator_mission_item(); + RTLState getActivationLandState(); void setLoiterPosition(); @@ -167,4 +174,5 @@ class RtlDirect : public MissionBlock, public ModuleParams uORB::SubscriptionData _land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ uORB::SubscriptionData _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ uORB::SubscriptionData _wind_sub{ORB_ID(wind)}; + uORB::Publication _navigator_mission_item_pub{ORB_ID::navigator_mission_item}; /**< Navigator mission item publication*/ }; diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp index c9e59fec1e20..d9b22720f34a 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/src/modules/navigator/takeoff.cpp @@ -43,7 +43,7 @@ #include Takeoff::Takeoff(Navigator *navigator) : - MissionBlock(navigator) + MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF) { } @@ -68,7 +68,7 @@ Takeoff::on_active() } else if (is_mission_item_reached_or_completed() && !_navigator->get_mission_result()->finished) { _navigator->get_mission_result()->finished = true; _navigator->set_mission_result_updated(); - _navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF); + _navigator->mode_completed(getNavigatorStateId()); position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); diff --git a/src/modules/navigator/vtol_takeoff.cpp b/src/modules/navigator/vtol_takeoff.cpp index 7b36201856d3..f9a552ef58fc 100644 --- a/src/modules/navigator/vtol_takeoff.cpp +++ b/src/modules/navigator/vtol_takeoff.cpp @@ -43,7 +43,7 @@ using matrix::wrap_pi; VtolTakeoff::VtolTakeoff(Navigator *navigator) : - MissionBlock(navigator), + MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF), ModuleParams(navigator) { } @@ -151,7 +151,7 @@ VtolTakeoff::on_active() // the VTOL takeoff is done _navigator->get_mission_result()->finished = true; _navigator->set_mission_result_updated(); - _navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF); + _navigator->mode_completed(getNavigatorStateId()); break; } diff --git a/src/modules/rover_ackermann/RoverAckermann.cpp b/src/modules/rover_ackermann/RoverAckermann.cpp index b5468880587c..5409e40d841a 100644 --- a/src/modules/rover_ackermann/RoverAckermann.cpp +++ b/src/modules/rover_ackermann/RoverAckermann.cpp @@ -73,32 +73,15 @@ void RoverAckermann::Run() return; } - // uORB subscriber updates - if (_parameter_update_sub.updated()) { - updateParams(); - } - - if (_vehicle_status_sub.updated()) { - vehicle_status_s vehicle_status; - _vehicle_status_sub.copy(&vehicle_status); - _nav_state = vehicle_status.nav_state; - _armed = vehicle_status.arming_state == 2; - } - - if (_local_position_sub.updated()) { - vehicle_local_position_s local_position{}; - _local_position_sub.copy(&local_position); - const Vector3f rover_velocity = {local_position.vx, local_position.vy, local_position.vz}; - _actual_speed = rover_velocity.norm(); - } + updateSubscriptions(); // Timestamps hrt_abstime timestamp_prev = _timestamp; _timestamp = hrt_absolute_time(); const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + // Generate motor setpoints if (_armed) { - // Navigation modes switch (_nav_state) { case vehicle_status_s::NAVIGATION_STATE_MANUAL: { manual_control_setpoint_s manual_control_setpoint{}; @@ -118,62 +101,97 @@ void RoverAckermann::Run() default: // Unimplemented nav states will stop the rover _motor_setpoint.steering = 0.f; _motor_setpoint.throttle = 0.f; + _throttle_with_accel_limit.setForcedValue(0.f); + _steering_with_rate_limit.setForcedValue(0.f); break; } - // Sanitize actuator commands - if (!PX4_ISFINITE(_motor_setpoint.steering)) { - _motor_setpoint.steering = 0.f; - } + } else { // Reset on disarm + _motor_setpoint.steering = 0.f; + _motor_setpoint.throttle = 0.f; + _throttle_with_accel_limit.setForcedValue(0.f); + _steering_with_rate_limit.setForcedValue(0.f); + } - if (!PX4_ISFINITE(_motor_setpoint.throttle)) { - _motor_setpoint.throttle = 0.f; - } + publishMotorSetpoints(applySlewRates(_motor_setpoint, dt)); +} - // Acceleration slew rate - if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_speed.get() > FLT_EPSILON - && fabsf(_motor_setpoint.throttle) > fabsf(_throttle_with_accel_limit.getState())) { - _throttle_with_accel_limit.update(_motor_setpoint.throttle, dt); +void RoverAckermann::updateSubscriptions() +{ + if (_parameter_update_sub.updated()) { + updateParams(); + } - } else { - _throttle_with_accel_limit.setForcedValue(_motor_setpoint.throttle); - } + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status; + _vehicle_status_sub.copy(&vehicle_status); + _nav_state = vehicle_status.nav_state; + _armed = vehicle_status.arming_state == 2; + } - // Steering slew rate - if (_param_ra_max_steering_rate.get() > FLT_EPSILON && _param_ra_max_steer_angle.get() > FLT_EPSILON) { - _steering_with_rate_limit.update(_motor_setpoint.steering, dt); + if (_local_position_sub.updated()) { + vehicle_local_position_s local_position{}; + _local_position_sub.copy(&local_position); + const Vector3f rover_velocity = {local_position.vx, local_position.vy, local_position.vz}; + _actual_speed = rover_velocity.norm(); + } +} +motor_setpoint_struct RoverAckermann::applySlewRates(motor_setpoint_struct motor_setpoint, const float dt) +{ + // Sanitize actuator commands + if (!PX4_ISFINITE(motor_setpoint.steering)) { + motor_setpoint.steering = 0.f; + } - } else { - _steering_with_rate_limit.setForcedValue(_motor_setpoint.steering); - } + if (!PX4_ISFINITE(motor_setpoint.throttle)) { + motor_setpoint.throttle = 0.f; + } - // Publish rover ackermann status (logging) - rover_ackermann_status_s rover_ackermann_status{}; - rover_ackermann_status.timestamp = _timestamp; - rover_ackermann_status.throttle_setpoint = _motor_setpoint.throttle; - rover_ackermann_status.steering_setpoint = _motor_setpoint.steering; - rover_ackermann_status.actual_speed = _actual_speed; - _rover_ackermann_status_pub.publish(rover_ackermann_status); - - // Publish to motor - actuator_motors_s actuator_motors{}; - actuator_motors.reversible_flags = _param_r_rev.get(); - actuator_motors.control[0] = math::constrain(_throttle_with_accel_limit.getState(), -1.f, 1.f); - actuator_motors.timestamp = _timestamp; - _actuator_motors_pub.publish(actuator_motors); - - // Publish to servo - actuator_servos_s actuator_servos{}; - actuator_servos.control[0] = math::constrain(_steering_with_rate_limit.getState(), -1.f, 1.f); - actuator_servos.timestamp = _timestamp; - _actuator_servos_pub.publish(actuator_servos); + // Acceleration slew rate + if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_speed.get() > FLT_EPSILON + && fabsf(motor_setpoint.throttle) > fabsf(_throttle_with_accel_limit.getState())) { + _throttle_with_accel_limit.update(motor_setpoint.throttle, dt); - } else { // Reset on disarm - _motor_setpoint.steering = 0.f; - _motor_setpoint.throttle = 0.f; - _throttle_with_accel_limit.setForcedValue(0.f); - _steering_with_rate_limit.setForcedValue(0.f); + } else { + _throttle_with_accel_limit.setForcedValue(motor_setpoint.throttle); + } + + // Steering slew rate + if (_param_ra_max_steering_rate.get() > FLT_EPSILON && _param_ra_max_steer_angle.get() > FLT_EPSILON) { + _steering_with_rate_limit.update(motor_setpoint.steering, dt); + + } else { + _steering_with_rate_limit.setForcedValue(motor_setpoint.steering); } + + motor_setpoint_struct motor_setpoint_temp{}; + motor_setpoint_temp.steering = math::constrain(_steering_with_rate_limit.getState(), -1.f, 1.f); + motor_setpoint_temp.throttle = math::constrain(_throttle_with_accel_limit.getState(), -1.f, 1.f); + return motor_setpoint_temp; +} + +void RoverAckermann::publishMotorSetpoints(motor_setpoint_struct motor_setpoint_with_slew_rates) +{ + // Publish rover Ackermann status (logging) + rover_ackermann_status_s rover_ackermann_status{}; + rover_ackermann_status.timestamp = _timestamp; + rover_ackermann_status.throttle_setpoint = _motor_setpoint.throttle; + rover_ackermann_status.steering_setpoint = _motor_setpoint.steering; + rover_ackermann_status.actual_speed = _actual_speed; + _rover_ackermann_status_pub.publish(rover_ackermann_status); + + // Publish to motor + actuator_motors_s actuator_motors{}; + actuator_motors.reversible_flags = _param_r_rev.get(); + actuator_motors.control[0] = motor_setpoint_with_slew_rates.throttle; + actuator_motors.timestamp = _timestamp; + _actuator_motors_pub.publish(actuator_motors); + + // Publish to servo + actuator_servos_s actuator_servos{}; + actuator_servos.control[0] = motor_setpoint_with_slew_rates.steering; + actuator_servos.timestamp = _timestamp; + _actuator_servos_pub.publish(actuator_servos); } int RoverAckermann::task_spawn(int argc, char *argv[]) diff --git a/src/modules/rover_ackermann/RoverAckermann.hpp b/src/modules/rover_ackermann/RoverAckermann.hpp index cea9300a1b3d..3617b5b80ba6 100644 --- a/src/modules/rover_ackermann/RoverAckermann.hpp +++ b/src/modules/rover_ackermann/RoverAckermann.hpp @@ -59,6 +59,7 @@ // Local includes #include "RoverAckermannGuidance/RoverAckermannGuidance.hpp" +using motor_setpoint_struct = RoverAckermannGuidance::motor_setpoint; using namespace time_literals; @@ -83,12 +84,32 @@ class RoverAckermann : public ModuleBase, public ModuleParams, bool init(); + protected: void updateParams() override; private: void Run() override; + /** + * @brief Update uORB subscriptions. + */ + void updateSubscriptions(); + + /** + * @brief Apply slew rates to motor setpoints. + * @param motor_setpoint Normalized steering and throttle setpoints. + * @param dt Time since last update [s]. + * @return Motor setpoint with applied slew rates. + */ + motor_setpoint_struct applySlewRates(motor_setpoint_struct motor_setpoint, float dt); + + /** + * @brief Publish motor setpoints to ActuatorMotors/ActuatorServos and logging values to RoverAckermannStatus. + * @param motor_setpoint_with_slew_rate Normalized motor_setpoint with applied slew rates. + */ + void publishMotorSetpoints(motor_setpoint_struct motor_setpoint_with_slew_rates); + // uORB subscriptions uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; @@ -100,12 +121,12 @@ class RoverAckermann : public ModuleBase, public ModuleParams, uORB::Publication _actuator_servos_pub{ORB_ID(actuator_servos)}; uORB::Publication _rover_ackermann_status_pub{ORB_ID(rover_ackermann_status)}; - // Instances + // Class instances RoverAckermannGuidance _ackermann_guidance{this}; // Variables int _nav_state{0}; - RoverAckermannGuidance::motor_setpoint _motor_setpoint; + motor_setpoint_struct _motor_setpoint; hrt_abstime _timestamp{0}; float _actual_speed{0.f}; SlewRate _steering_with_rate_limit{0.f}; diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp index db0495b67a3e..1acaebb6386f 100644 --- a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp +++ b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp @@ -58,13 +58,54 @@ void RoverAckermannGuidance::updateParams() RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::computeGuidance(const int nav_state) { - // Initializations - float desired_speed{0.f}; - float vehicle_yaw{0.f}; - float actual_speed{0.f}; - bool mission_finished{false}; + updateSubscriptions(); - // uORB subscriber updates + // Catch return to launch + if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { + _mission_finished = _distance_to_next_wp < _acceptance_radius; + } + + // Guidance logic + if (_mission_finished) { // Mission is finished + _desired_steering = 0.f; + _desired_speed = 0.f; + + } else if (_distance_to_curr_wp < _acceptance_radius) { // Catch delay command + _desired_speed = 0.f; + + } else { // Regular guidance algorithm + _desired_speed = calcDesiredSpeed(_param_ra_miss_vel_def.get(), _param_ra_miss_vel_min.get(), + _param_ra_miss_vel_gain.get(), _distance_to_prev_wp, _distance_to_curr_wp, _acceptance_radius, + _prev_acceptance_radius, _param_ra_max_accel.get(), _param_ra_max_jerk.get(), nav_state); + + _desired_steering = calcDesiredSteering(_pure_pursuit, _curr_wp_ned, _prev_wp_ned, _curr_pos_ned, + _param_ra_wheel_base.get(), _desired_speed, _vehicle_yaw, _param_ra_max_steer_angle.get()); + + } + + // Calculate throttle setpoint + hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + const float throttle = calcThrottleSetpoint(_pid_throttle, _desired_speed, _actual_speed, _param_ra_max_speed.get(), + dt); + + // Publish ackermann controller status (logging) + _rover_ackermann_guidance_status.timestamp = _timestamp; + _rover_ackermann_guidance_status.desired_speed = _desired_speed; + _rover_ackermann_guidance_status.pid_throttle_integral = _pid_throttle.integral; + _rover_ackermann_guidance_status_pub.publish(_rover_ackermann_guidance_status); + + // Return motor setpoints + motor_setpoint motor_setpoint_temp; + motor_setpoint_temp.steering = math::interpolate(_desired_steering, -_param_ra_max_steer_angle.get(), + _param_ra_max_steer_angle.get(), -1.f, 1.f); // Normalize steering setpoint + motor_setpoint_temp.throttle = throttle; + return motor_setpoint_temp; +} + +void RoverAckermannGuidance::updateSubscriptions() +{ if (_vehicle_global_position_sub.updated()) { vehicle_global_position_s vehicle_global_position{}; _vehicle_global_position_sub.copy(&vehicle_global_position); @@ -82,7 +123,7 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::computeGuidance(c _curr_pos_ned = Vector2f(local_position.x, local_position.y); const Vector3f rover_velocity = {local_position.vx, local_position.vy, local_position.vz}; - actual_speed = rover_velocity.norm(); + _actual_speed = rover_velocity.norm(); } if (_home_position_sub.updated()) { @@ -92,158 +133,63 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::computeGuidance(c } if (_position_setpoint_triplet_sub.updated()) { - updateWaypoints(); + updateWaypointsAndAcceptanceRadius(); } if (_vehicle_attitude_sub.updated()) { vehicle_attitude_s vehicle_attitude{}; _vehicle_attitude_sub.copy(&vehicle_attitude); matrix::Quatf vehicle_attitude_quaternion = Quatf(vehicle_attitude.q); - vehicle_yaw = matrix::Eulerf(vehicle_attitude_quaternion).psi(); + _vehicle_yaw = matrix::Eulerf(vehicle_attitude_quaternion).psi(); } if (_mission_result_sub.updated()) { mission_result_s mission_result{}; _mission_result_sub.copy(&mission_result); - mission_finished = mission_result.finished; - } - - if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL - && get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), _next_wp(0), - _next_wp(1)) < _acceptance_radius) { // Return to launch - mission_finished = true; - } - - // Guidance logic - if (mission_finished) { - _desired_steering = 0.f; - - } else { - const float distance_to_prev_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), - _prev_wp(0), - _prev_wp(1)); - const float distance_to_curr_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), - _curr_wp(0), - _curr_wp(1)); - - if (distance_to_curr_wp < _acceptance_radius) { // Catch delay command - desired_speed = 0.f; - - } else { - // Calculate desired speed - if (_param_ra_miss_vel_min.get() > 0.f && _param_ra_miss_vel_min.get() < _param_ra_miss_vel_def.get() - && _param_ra_miss_vel_gain.get() > FLT_EPSILON) { // Cornering slow down effect - if (distance_to_prev_wp <= _prev_acceptance_radius && _prev_acceptance_radius > FLT_EPSILON) { - const float cornering_speed = _param_ra_miss_vel_gain.get() / _prev_acceptance_radius; - desired_speed = math::constrain(cornering_speed, _param_ra_miss_vel_min.get(), _param_ra_miss_vel_def.get()); - - } else if (distance_to_curr_wp <= _acceptance_radius && _acceptance_radius > FLT_EPSILON) { - const float cornering_speed = _param_ra_miss_vel_gain.get() / _acceptance_radius; - desired_speed = math::constrain(cornering_speed, _param_ra_miss_vel_min.get(), _param_ra_miss_vel_def.get()); - - } else { // Straight line speed - if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_jerk.get() > FLT_EPSILON - && _acceptance_radius > FLT_EPSILON) { - float max_velocity{0.f}; - - if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { - max_velocity = math::trajectory::computeMaxSpeedFromDistance(_param_ra_max_jerk.get(), - _param_ra_max_accel.get(), distance_to_curr_wp, 0.f); - - } else { - const float cornering_speed = _param_ra_miss_vel_gain.get() / _acceptance_radius; - max_velocity = math::trajectory::computeMaxSpeedFromDistance(_param_ra_max_jerk.get(), - _param_ra_max_accel.get(), distance_to_curr_wp - _acceptance_radius, cornering_speed); - } - - desired_speed = math::constrain(max_velocity, _param_ra_miss_vel_min.get(), _param_ra_miss_vel_def.get()); - - } else { - desired_speed = _param_ra_miss_vel_def.get(); - } - } - - } else { - desired_speed = _param_ra_miss_vel_def.get(); - } - - // Calculate desired steering - _desired_steering = calcDesiredSteering(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, _param_ra_wheel_base.get(), - desired_speed, vehicle_yaw); - _desired_steering = math::constrain(_desired_steering, -_param_ra_max_steer_angle.get(), - _param_ra_max_steer_angle.get()); - } - } - - // Throttle PID - hrt_abstime timestamp_prev = _timestamp; - _timestamp = hrt_absolute_time(); - const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; - float throttle = 0.f; - - if (desired_speed < FLT_EPSILON) { - pid_reset_integral(&_pid_throttle); - - } else { - throttle = pid_calculate(&_pid_throttle, desired_speed, actual_speed, 0, - dt); - } - - if (_param_ra_max_speed.get() > 0.f) { // Feed-forward term - throttle += math::interpolate(desired_speed, - 0.f, _param_ra_max_speed.get(), - 0.f, 1.f); + _mission_finished = mission_result.finished; } - - // Publish ackermann controller status (logging) - _rover_ackermann_guidance_status.timestamp = _timestamp; - _rover_ackermann_guidance_status.desired_speed = desired_speed; - _rover_ackermann_guidance_status.pid_throttle_integral = _pid_throttle.integral; - _rover_ackermann_guidance_status_pub.publish(_rover_ackermann_guidance_status); - - // Return motor setpoints - motor_setpoint motor_setpoint_temp; - motor_setpoint_temp.steering = math::interpolate(_desired_steering, -_param_ra_max_steer_angle.get(), - _param_ra_max_steer_angle.get(), - -1.f, 1.f); - motor_setpoint_temp.throttle = math::constrain(throttle, 0.f, 1.f); - return motor_setpoint_temp; } -void RoverAckermannGuidance::updateWaypoints() +void RoverAckermannGuidance::updateWaypointsAndAcceptanceRadius() { position_setpoint_triplet_s position_setpoint_triplet{}; _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); // Global waypoint coordinates + Vector2d prev_wp = _curr_pos.isAllFinite() ? _curr_pos : Vector2d(0, 0); // Fallback if previous waypoint is invalid + Vector2d curr_wp(0, 0); + Vector2d next_wp = _home_position.isAllFinite() ? _home_position : Vector2d(0, 0); // Enables corner slow down with RTL + if (position_setpoint_triplet.current.valid && PX4_ISFINITE(position_setpoint_triplet.current.lat) && PX4_ISFINITE(position_setpoint_triplet.current.lon)) { - _curr_wp = Vector2d(position_setpoint_triplet.current.lat, position_setpoint_triplet.current.lon); + curr_wp = Vector2d(position_setpoint_triplet.current.lat, position_setpoint_triplet.current.lon); - } else { - _curr_wp = Vector2d(0, 0); } if (position_setpoint_triplet.previous.valid && PX4_ISFINITE(position_setpoint_triplet.previous.lat) && PX4_ISFINITE(position_setpoint_triplet.previous.lon)) { - _prev_wp = Vector2d(position_setpoint_triplet.previous.lat, position_setpoint_triplet.previous.lon); + prev_wp = Vector2d(position_setpoint_triplet.previous.lat, position_setpoint_triplet.previous.lon); - } else { - _prev_wp = _curr_pos; } if (position_setpoint_triplet.next.valid && PX4_ISFINITE(position_setpoint_triplet.next.lat) && PX4_ISFINITE(position_setpoint_triplet.next.lon)) { - _next_wp = Vector2d(position_setpoint_triplet.next.lat, position_setpoint_triplet.next.lon); + next_wp = Vector2d(position_setpoint_triplet.next.lat, position_setpoint_triplet.next.lon); - } else { - _next_wp = _home_position; // Enables corner slow down with RTL } + // Distances to waypoints + _distance_to_prev_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), + prev_wp(0), prev_wp(1)); + _distance_to_curr_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), + curr_wp(0), curr_wp(1)); + _distance_to_next_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), + next_wp(0), next_wp(1)); + // NED waypoint coordinates - _curr_wp_ned = _global_ned_proj_ref.project(_curr_wp(0), _curr_wp(1)); - _prev_wp_ned = _global_ned_proj_ref.project(_prev_wp(0), _prev_wp(1)); - _next_wp_ned = _global_ned_proj_ref.project(_next_wp(0), _next_wp(1)); + _curr_wp_ned = _global_ned_proj_ref.project(curr_wp(0), curr_wp(1)); + _prev_wp_ned = _global_ned_proj_ref.project(prev_wp(0), prev_wp(1)); + _next_wp_ned = _global_ned_proj_ref.project(next_wp(0), next_wp(1)); // Update acceptance radius _prev_acceptance_radius = _acceptance_radius; @@ -268,8 +214,9 @@ float RoverAckermannGuidance::updateAcceptanceRadius(const Vector2f &curr_wp_ned // Calculate acceptance radius s.t. the rover cuts the corner tangential to the current and next line segment if (curr_to_next_wp_ned.norm() > FLT_EPSILON && curr_to_prev_wp_ned.norm() > FLT_EPSILON) { - const float theta = acosf((curr_to_prev_wp_ned * curr_to_next_wp_ned) / (curr_to_prev_wp_ned.norm() * - curr_to_next_wp_ned.norm())) / 2.f; + float cosin = curr_to_prev_wp_ned.unit_or_zero() * curr_to_next_wp_ned.unit_or_zero(); + cosin = math::constrain(cosin, -1.f, 1.f); // Protect against float precision problem + const float theta = acosf(cosin) / 2.f; const float min_turning_radius = wheel_base / sinf(max_steer_angle); const float acceptance_radius_temp = min_turning_radius / tanf(theta); const float acceptance_radius_temp_scaled = acceptance_radius_gain * @@ -286,24 +233,90 @@ float RoverAckermannGuidance::updateAcceptanceRadius(const Vector2f &curr_wp_ned return acceptance_radius; } -float RoverAckermannGuidance::calcDesiredSteering(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, - const Vector2f &curr_pos_ned, const float wheel_base, const float desired_speed, const float vehicle_yaw) +float RoverAckermannGuidance::calcDesiredSpeed(const float miss_vel_def, const float miss_vel_min, + const float miss_vel_gain, const float distance_to_prev_wp, const float distance_to_curr_wp, const float acc_rad, + const float prev_acc_rad, const float max_accel, const float max_jerk, const int nav_state) +{ + // Catch improper values + if (miss_vel_min < 0.f || miss_vel_min > miss_vel_def || miss_vel_gain < FLT_EPSILON) { + return miss_vel_def; + } + + // Cornering slow down effect + if (distance_to_prev_wp <= prev_acc_rad && prev_acc_rad > FLT_EPSILON) { + const float cornering_speed = miss_vel_gain / prev_acc_rad; + return math::constrain(cornering_speed, miss_vel_min, miss_vel_def); + + } else if (distance_to_curr_wp <= acc_rad && acc_rad > FLT_EPSILON) { + const float cornering_speed = miss_vel_gain / acc_rad; + return math::constrain(cornering_speed, miss_vel_min, miss_vel_def); + + } + + // Straight line speed + if (max_accel > FLT_EPSILON && max_jerk > FLT_EPSILON && acc_rad > FLT_EPSILON) { + float max_velocity{0.f}; + + if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { + max_velocity = math::trajectory::computeMaxSpeedFromDistance(max_jerk, + max_accel, distance_to_curr_wp, 0.f); + + } else { + const float cornering_speed = miss_vel_gain / acc_rad; + max_velocity = math::trajectory::computeMaxSpeedFromDistance(max_jerk, + max_accel, distance_to_curr_wp - acc_rad, cornering_speed); + } + + return math::constrain(max_velocity, miss_vel_min, miss_vel_def); + + } else { + return miss_vel_def; + } + +} + +float RoverAckermannGuidance::calcDesiredSteering(PurePursuit &pure_pursuit, const Vector2f &curr_wp_ned, + const Vector2f &prev_wp_ned, const Vector2f &curr_pos_ned, const float wheel_base, const float desired_speed, + const float vehicle_yaw, const float max_steering) { - // Calculate desired steering to reach lookahead point - const float desired_heading = _pure_pursuit.calcDesiredHeading(curr_wp_ned, prev_wp_ned, curr_pos_ned, + const float desired_heading = pure_pursuit.calcDesiredHeading(curr_wp_ned, prev_wp_ned, curr_pos_ned, desired_speed); - const float lookahead_distance = _pure_pursuit.getLookaheadDistance(); + const float lookahead_distance = pure_pursuit.getLookaheadDistance(); const float heading_error = matrix::wrap_pi(desired_heading - vehicle_yaw); // For logging _rover_ackermann_guidance_status.lookahead_distance = lookahead_distance; _rover_ackermann_guidance_status.heading_error = (heading_error * 180.f) / (M_PI_F); + float desired_steering{0.f}; + if (math::abs_t(heading_error) <= M_PI_2_F) { - return atanf(2 * wheel_base * sinf(heading_error) / lookahead_distance); + desired_steering = atanf(2 * wheel_base * sinf(heading_error) / lookahead_distance); + } else { - return atanf(2 * wheel_base * (sign(heading_error) * 1.0f + sinf(heading_error - sign(heading_error) * M_PI_2_F)) / - lookahead_distance); + desired_steering = atanf(2 * wheel_base * (sign(heading_error) * 1.0f + sinf(heading_error - + sign(heading_error) * M_PI_2_F)) / lookahead_distance); + } + + return math::constrain(desired_steering, -max_steering, max_steering); + +} + +float RoverAckermannGuidance::calcThrottleSetpoint(PID_t &pid_throttle, const float desired_speed, + const float actual_speed, const float max_speed, const float dt) +{ + float throttle = 0.f; + + if (desired_speed < FLT_EPSILON) { + pid_reset_integral(&pid_throttle); + + } else { + throttle = pid_calculate(&pid_throttle, desired_speed, actual_speed, 0, dt); + } + + if (_param_ra_max_speed.get() > 0.f) { // Feed-forward term + throttle += math::interpolate(desired_speed, 0.f, max_speed, 0.f, 1.f); } + return math::constrain(throttle, 0.f, 1.f); } diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp index 592083556f13..23f63ed9280e 100644 --- a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp +++ b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp @@ -73,58 +73,106 @@ class RoverAckermannGuidance : public ModuleParams RoverAckermannGuidance(ModuleParams *parent); ~RoverAckermannGuidance() = default; + /** + * @brief Struct for steering and throttle setpoints. + * @param steering Steering setpoint. + * @param throttle Throttle setpoint. + */ struct motor_setpoint { float steering{0.f}; float throttle{0.f}; }; /** - * @brief Compute guidance for ackermann rover and return motor_setpoint for throttle and steering. - * @param nav_state Vehicle navigation state + * @brief Calculate motor setpoints based on the mission plan. + * @param nav_state Vehicle navigation state. + * @return Motor setpoints for throttle and steering. */ motor_setpoint computeGuidance(int nav_state); +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + /** + * @brief Update uORB subscriptions + */ + void updateSubscriptions(); + /** - * @brief Update global/NED waypoint coordinates and acceptance radius + * @brief Update global/NED waypoint coordinates and acceptance radius. */ - void updateWaypoints(); + void updateWaypointsAndAcceptanceRadius(); /** - * @brief Returns and publishes the acceptance radius for current waypoint based on the angle between a line segment - * from the previous to the current waypoint/current to the next waypoint and maximum steer angle of - * the vehicle. - * @param curr_wp_ned Current waypoint in NED frame. - * @param prev_wp_ned Previous waypoint in NED frame. - * @param next_wp_ned Next waypoint in NED frame. - * @param default_acceptance_radius Default acceptance radius for waypoints. - * @param acceptance_radius_gain Scaling of the geometric optimal acceptance radius for the rover to cut corners. - * @param acceptance_radius_max Maximum value for the acceptance radius. - * @param wheel_base Rover wheelbase. - * @param max_steer_angle Rover maximum steer angle. + * @brief Publish the acceptance radius for current waypoint based on the angle between a line segment + * from the previous to the current waypoint/current to the next waypoint and maximum steer angle of the vehicle. + * @param curr_wp_ned Current waypoint in NED frame [m]. + * @param prev_wp_ned Previous waypoint in NED frame [m]. + * @param next_wp_ned Next waypoint in NED frame [m]. + * @param default_acceptance_radius Default acceptance radius for waypoints [m]. + * @param acceptance_radius_gain Tuning parameter that scales the geometric optimal acceptance radius for the corner cutting [-]. + * @param acceptance_radius_max Maximum value for the acceptance radius [m]. + * @param wheel_base Rover wheelbase [m]. + * @param max_steer_angle Rover maximum steer angle [rad]. + * @return Updated acceptance radius [m]. */ float updateAcceptanceRadius(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, const Vector2f &next_wp_ned, float default_acceptance_radius, float acceptance_radius_gain, float acceptance_radius_max, float wheel_base, float max_steer_angle); + /** - * @brief Calculate and return desired steering input - * @param curr_wp_ned Current waypoint in NED frame. - * @param prev_wp_ned Previous waypoint in NED frame. - * @param curr_pos_ned Current position of the vehicle in NED frame. - * @param wheel_base Rover wheelbase. - * @param desired_speed Desired speed for the rover. - * @param vehicle_yaw Current yaw of the rover. + * @brief Calculate the desired speed setpoint. During cornering the speed is calculated as the inverse + * of the acceptance radius multiplied with a tuning factor. On straight lines it is based on a velocity trajectory + * such that the rover will arrive at the next corner with the desired cornering speed under consideration of the + * maximum acceleration and jerk. + * @param miss_vel_def Default desired velocity for the rover during mission [m/s]. + * @param miss_vel_min Minimum desired velocity for the rover during mission [m/s]. + * @param miss_vel_gain Tuning parameter for the slow down effect during cornering [-]. + * @param distance_to_prev_wp Distance to the previous waypoint [m]. + * @param distance_to_curr_wp Distance to the current waypoint [m]. + * @param acc_rad Acceptance radius of the current waypoint [m]. + * @param prev_acc_rad Acceptance radius of the previous waypoint [m]. + * @param max_accel Maximum allowed acceleration for the rover [m/s^2]. + * @param max_jerk Maximum allowed jerk for the rover [m/s^3]. + * @param nav_state Current nav_state of the rover. + * @return Speed setpoint for the rover [m/s]. */ - float calcDesiredSteering(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, const Vector2f &curr_pos_ned, - float wheel_base, float desired_speed, float vehicle_yaw); + float calcDesiredSpeed(float miss_vel_def, float miss_vel_min, float miss_vel_gain, float distance_to_prev_wp, + float distance_to_curr_wp, float acc_rad, float prev_acc_rad, float max_accel, float max_jerk, int nav_state); -protected: /** - * @brief Update the parameters of the module. + * @brief Calculate desired steering angle. The desired steering is calulated as the steering that is required to + * reach the point calculated using the pure pursuit algorithm (see PurePursuit.hpp). + * @param pure_pursuit Pure pursuit class instance. + * @param curr_wp_ned Current waypoint in NED frame [m]. + * @param prev_wp_ned Previous waypoint in NED frame [m]. + * @param curr_pos_ned Current position of the vehicle in NED frame [m]. + * @param wheel_base Rover wheelbase [m]. + * @param desired_speed Desired speed for the rover [m/s]. + * @param vehicle_yaw Current yaw of the rover [rad]. + * @param max_steering Maximum steering angle of the rover [rad]. + * @return Steering setpoint for the rover [rad]. */ - void updateParams() override; + float calcDesiredSteering(PurePursuit &pure_pursuit, const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, + const Vector2f &curr_pos_ned, float wheel_base, float desired_speed, float vehicle_yaw, float max_steering); + + /** + * @brief Calculate the throttle setpoint. Calculated with a PID controller using the difference between + * the desired/actual speed and a feedforward term based on the full throttle speed. + * @param pid_throttle Reference to PID instance. + * @param desired_speed Reference speed for the rover [m/s]. + * @param actual_speed Actual speed of the rover [m/s]. + * @param max_speed Rover speed at full throttle [m/s]. + * @param dt Time interval since last update [s]. + * @return Normalized throttle setpoint [0, 1]. + */ + float calcThrottleSetpoint(PID_t &pid_throttle, float desired_speed, float actual_speed, float max_speed, float dt); -private: // uORB subscriptions uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; @@ -138,27 +186,31 @@ class RoverAckermannGuidance : public ModuleParams uORB::Publication _position_controller_status_pub{ORB_ID(position_controller_status)}; rover_ackermann_guidance_status_s _rover_ackermann_guidance_status{}; - - MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates. + // Class instances + MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates PurePursuit _pure_pursuit{this}; // Pure pursuit library // Rover variables + float _desired_steering{0.f}; + float _vehicle_yaw{0.f}; + float _desired_speed{0.f}; + float _actual_speed{0.f}; Vector2d _curr_pos{}; Vector2f _curr_pos_ned{}; PID_t _pid_throttle; hrt_abstime _timestamp{0}; - float _desired_steering{0.f}; // Waypoint variables - Vector2d _curr_wp{}; - Vector2d _next_wp{}; - Vector2d _prev_wp{}; Vector2d _home_position{}; Vector2f _curr_wp_ned{}; Vector2f _prev_wp_ned{}; Vector2f _next_wp_ned{}; + float _distance_to_prev_wp{0.f}; + float _distance_to_curr_wp{0.f}; + float _distance_to_next_wp{0.f}; float _acceptance_radius{0.5f}; float _prev_acceptance_radius{0.5f}; + bool _mission_finished{false}; // Parameters DEFINE_PARAMETERS( diff --git a/src/modules/differential_drive/CMakeLists.txt b/src/modules/rover_differential/CMakeLists.txt similarity index 84% rename from src/modules/differential_drive/CMakeLists.txt rename to src/modules/rover_differential/CMakeLists.txt index 2e0e3583b9af..beaec32a8776 100644 --- a/src/modules/differential_drive/CMakeLists.txt +++ b/src/modules/rover_differential/CMakeLists.txt @@ -31,22 +31,19 @@ # ############################################################################ -add_subdirectory(DifferentialDriveControl) -add_subdirectory(DifferentialDriveGuidance) -add_subdirectory(DifferentialDriveKinematics) +add_subdirectory(RoverDifferentialGuidance) px4_add_module( - MODULE modules__differential_drive - MAIN differential_drive + MODULE modules__rover_differential + MAIN rover_differential SRCS - DifferentialDrive.cpp - DifferentialDrive.hpp + RoverDifferential.cpp + RoverDifferential.hpp DEPENDS - DifferentialDriveControl - DifferentialDriveGuidance - DifferentialDriveKinematics + RoverDifferentialGuidance px4_work_queue modules__control_allocator # for parameter CA_R_REV + pure_pursuit MODULE_CONFIG module.yaml ) diff --git a/src/modules/rover_differential/Kconfig b/src/modules/rover_differential/Kconfig new file mode 100644 index 000000000000..840e2cdbf98f --- /dev/null +++ b/src/modules/rover_differential/Kconfig @@ -0,0 +1,6 @@ +menuconfig MODULES_ROVER_DIFFERENTIAL + bool "rover_differential" + default n + depends on MODULES_CONTROL_ALLOCATOR + ---help--- + Enable support for control of differential rovers diff --git a/src/modules/rover_differential/RoverDifferential.cpp b/src/modules/rover_differential/RoverDifferential.cpp new file mode 100644 index 000000000000..ab89392a0ec8 --- /dev/null +++ b/src/modules/rover_differential/RoverDifferential.cpp @@ -0,0 +1,256 @@ +/**************************************************************************** + * + * Copyright (c) 2023-2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "RoverDifferential.hpp" +using namespace matrix; +using namespace time_literals; + +RoverDifferential::RoverDifferential() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) +{ + updateParams(); + _rover_differential_status_pub.advertise(); + pid_init(&_pid_yaw_rate, PID_MODE_DERIVATIV_NONE, 0.001f); +} + +bool RoverDifferential::init() +{ + ScheduleOnInterval(10_ms); // 100 Hz + return true; +} + +void RoverDifferential::updateParams() +{ + ModuleParams::updateParams(); + + _max_yaw_rate = _param_rd_max_yaw_rate.get() * M_DEG_TO_RAD_F; + + pid_set_parameters(&_pid_yaw_rate, + _param_rd_p_gain_yaw_rate.get(), // Proportional gain + _param_rd_i_gain_yaw_rate.get(), // Integral gain + 0.f, // Derivative gain + 1.f, // Integral limit + 1.f); // Output limit + +} + +void RoverDifferential::Run() +{ + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + return; + } + + hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + // uORB subscriber updates + if (_parameter_update_sub.updated()) { + parameter_update_s parameter_update; + _parameter_update_sub.copy(¶meter_update); + updateParams(); + } + + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status{}; + _vehicle_status_sub.copy(&vehicle_status); + _nav_state = vehicle_status.nav_state; + } + + if (_vehicle_angular_velocity_sub.updated()) { + vehicle_angular_velocity_s vehicle_angular_velocity{}; + _vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity); + _vehicle_body_yaw_rate = vehicle_angular_velocity.xyz[2]; + } + + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude{}; + _vehicle_attitude_sub.copy(&vehicle_attitude); + _vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q); + _vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi(); + } + + if (_vehicle_local_position_sub.updated()) { + vehicle_local_position_s vehicle_local_position{}; + _vehicle_local_position_sub.copy(&vehicle_local_position); + Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); + Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame); + _vehicle_forward_speed = velocity_in_body_frame(0); + } + + // Navigation modes + switch (_nav_state) { + case vehicle_status_s::NAVIGATION_STATE_MANUAL: { + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + _differential_setpoint.throttle = manual_control_setpoint.throttle; + _differential_setpoint.yaw_rate = manual_control_setpoint.roll * _param_rd_man_yaw_scale.get(); + + } + + _differential_setpoint.closed_loop_yaw_rate = false; + } break; + + case vehicle_status_s::NAVIGATION_STATE_ACRO: { + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + _differential_setpoint.throttle = manual_control_setpoint.throttle; + _differential_setpoint.yaw_rate = math::interpolate(manual_control_setpoint.roll, + -1.f, 1.f, + -_max_yaw_rate, _max_yaw_rate); + } + + _differential_setpoint.closed_loop_yaw_rate = true; + } break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: + _differential_setpoint = _rover_differential_guidance.computeGuidance(_vehicle_yaw, _vehicle_forward_speed, + _nav_state); + break; + + default: // Unimplemented nav states will stop the rover + _differential_setpoint.throttle = 0.f; + _differential_setpoint.yaw_rate = 0.f; + _differential_setpoint.closed_loop_yaw_rate = false; + break; + } + + float speed_diff_normalized = _differential_setpoint.yaw_rate; + + // Closed loop yaw rate control + if (_differential_setpoint.closed_loop_yaw_rate) { + if (fabsf(_differential_setpoint.yaw_rate - _vehicle_body_yaw_rate) < YAW_RATE_ERROR_THRESHOLD) { + speed_diff_normalized = 0.f; + pid_reset_integral(&_pid_yaw_rate); + + } else { + const float speed_diff = _differential_setpoint.yaw_rate * _param_rd_wheel_track.get(); // Feedforward + speed_diff_normalized = math::interpolate(speed_diff, -_param_rd_max_speed.get(), + _param_rd_max_speed.get(), -1.f, 1.f); + speed_diff_normalized = math::constrain(speed_diff_normalized + + pid_calculate(&_pid_yaw_rate, _differential_setpoint.yaw_rate, _vehicle_body_yaw_rate, 0, dt), + -1.f, 1.f); // Feedback + } + + } else { + pid_reset_integral(&_pid_yaw_rate); + } + + // Publish rover differential status (logging) + rover_differential_status_s rover_differential_status{}; + rover_differential_status.timestamp = _timestamp; + rover_differential_status.actual_speed = _vehicle_forward_speed; + rover_differential_status.desired_yaw_rate_deg_s = M_RAD_TO_DEG_F * _differential_setpoint.yaw_rate; + rover_differential_status.actual_yaw_rate_deg_s = M_RAD_TO_DEG_F * _vehicle_body_yaw_rate; + rover_differential_status.pid_yaw_rate_integral = _pid_yaw_rate.integral; + _rover_differential_status_pub.publish(rover_differential_status); + + // Publish to motors + actuator_motors_s actuator_motors{}; + actuator_motors.reversible_flags = _param_r_rev.get(); + computeMotorCommands(_differential_setpoint.throttle, speed_diff_normalized).copyTo(actuator_motors.control); + actuator_motors.timestamp = _timestamp; + _actuator_motors_pub.publish(actuator_motors); + +} + +matrix::Vector2f RoverDifferential::computeMotorCommands(float forward_speed, const float speed_diff) +{ + float combined_velocity = fabsf(forward_speed) + fabsf(speed_diff); + + if (combined_velocity > 1.0f) { // Prioritize yaw rate + float excess_velocity = fabsf(combined_velocity - 1.0f); + forward_speed -= sign(forward_speed) * excess_velocity; + } + + // Calculate the left and right wheel speeds + return Vector2f(forward_speed - speed_diff, + forward_speed + speed_diff); +} + +int RoverDifferential::task_spawn(int argc, char *argv[]) +{ + RoverDifferential *instance = new RoverDifferential(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int RoverDifferential::custom_command(int argc, char *argv[]) +{ + return print_usage("unk_timestampn command"); +} + +int RoverDifferential::print_usage(const char *reason) +{ + if (reason) { + PX4_ERR("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Rover Differential controller. +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("rover_differential", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + return 0; +} + +extern "C" __EXPORT int rover_differential_main(int argc, char *argv[]) +{ + return RoverDifferential::main(argc, argv); +} diff --git a/src/modules/differential_drive/DifferentialDrive.hpp b/src/modules/rover_differential/RoverDifferential.hpp similarity index 53% rename from src/modules/differential_drive/DifferentialDrive.hpp rename to src/modules/rover_differential/RoverDifferential.hpp index 255bb0c52607..3dafe99b0566 100644 --- a/src/modules/differential_drive/DifferentialDrive.hpp +++ b/src/modules/rover_differential/RoverDifferential.hpp @@ -33,31 +33,41 @@ #pragma once +// PX4 includes #include #include #include #include #include + +// uORB includes #include +#include #include -#include #include #include -#include #include +#include +#include +#include +#include +#include + +// Standard libraries +#include +#include -#include "DifferentialDriveControl/DifferentialDriveControl.hpp" -#include "DifferentialDriveGuidance/DifferentialDriveGuidance.hpp" -#include "DifferentialDriveKinematics/DifferentialDriveKinematics.hpp" +// Local includes +#include "RoverDifferentialGuidance/RoverDifferentialGuidance.hpp" using namespace time_literals; -class DifferentialDrive : public ModuleBase, public ModuleParams, +class RoverDifferential : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: - DifferentialDrive(); - ~DifferentialDrive() override = default; + RoverDifferential(); + ~RoverDifferential() override = default; /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); @@ -70,36 +80,57 @@ class DifferentialDrive : public ModuleBase, public ModulePar bool init(); + /** + * @brief Computes motor commands for differential drive. + * + * @param forward_speed Linear velocity along the x-axis. + * @param speed_diff Speed difference between left and right wheels. + * @return matrix::Vector2f Motor velocities for the right and left motors. + */ + matrix::Vector2f computeMotorCommands(float forward_speed, const float speed_diff); + protected: void updateParams() override; private: void Run() override; + + // uORB Subscriptions uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; - uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; - + uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; - uORB::Publication _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)}; - bool _manual_driving = false; - bool _mission_driving = false; - bool _acro_driving = false; - hrt_abstime _time_stamp_last{0}; /**< time stamp when task was last updated */ + // uORB Publications + uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; + uORB::Publication _rover_differential_status_pub{ORB_ID(rover_differential_status)}; + + // Instances + RoverDifferentialGuidance _rover_differential_guidance{this}; - DifferentialDriveGuidance _differential_drive_guidance{this}; - DifferentialDriveControl _differential_drive_control{this}; - DifferentialDriveKinematics _differential_drive_kinematics{this}; + // Variables + float _vehicle_body_yaw_rate{0.f}; + float _vehicle_forward_speed{0.f}; + float _vehicle_yaw{0.f}; + float _max_yaw_rate{0.f}; + int _nav_state{0}; + matrix::Quatf _vehicle_attitude_quaternion{}; + hrt_abstime _timestamp{0}; + PID_t _pid_yaw_rate; // The PID controller for yaw rate + RoverDifferentialGuidance::differential_setpoint _differential_setpoint; - float _max_speed{0.f}; - float _max_angular_velocity{0.f}; + // Constants + static constexpr float YAW_RATE_ERROR_THRESHOLD = 0.1f; // [rad/s] Error threshold for the closed loop yaw rate control DEFINE_PARAMETERS( - (ParamFloat) _param_rdd_ang_velocity_scale, - (ParamFloat) _param_rdd_speed_scale, - (ParamFloat) _param_rdd_wheel_base, - (ParamFloat) _param_rdd_wheel_speed, - (ParamFloat) _param_rdd_wheel_radius, - (ParamFloat) _param_com_spoolup_time + (ParamFloat) _param_rd_man_yaw_scale, + (ParamFloat) _param_rd_wheel_track, + (ParamFloat) _param_rd_p_gain_yaw_rate, + (ParamFloat) _param_rd_i_gain_yaw_rate, + (ParamFloat) _param_rd_max_speed, + (ParamFloat) _param_rd_max_yaw_rate, + (ParamInt) _param_r_rev ) }; diff --git a/src/modules/differential_drive/DifferentialDriveGuidance/CMakeLists.txt b/src/modules/rover_differential/RoverDifferentialGuidance/CMakeLists.txt similarity index 89% rename from src/modules/differential_drive/DifferentialDriveGuidance/CMakeLists.txt rename to src/modules/rover_differential/RoverDifferentialGuidance/CMakeLists.txt index fa833c50ae42..0fd7b68c394e 100644 --- a/src/modules/differential_drive/DifferentialDriveGuidance/CMakeLists.txt +++ b/src/modules/rover_differential/RoverDifferentialGuidance/CMakeLists.txt @@ -31,8 +31,9 @@ # ############################################################################ -px4_add_library(DifferentialDriveGuidance - DifferentialDriveGuidance.cpp +px4_add_library(RoverDifferentialGuidance + RoverDifferentialGuidance.cpp ) -target_include_directories(DifferentialDriveGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(RoverDifferentialGuidance PUBLIC pid) +target_include_directories(RoverDifferentialGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp new file mode 100644 index 000000000000..efad52e7b874 --- /dev/null +++ b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp @@ -0,0 +1,247 @@ +/**************************************************************************** + * + * Copyright (c) 2023-2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "RoverDifferentialGuidance.hpp" + +#include + +using namespace matrix; +using namespace time_literals; + +RoverDifferentialGuidance::RoverDifferentialGuidance(ModuleParams *parent) : ModuleParams(parent) +{ + updateParams(); + _rover_differential_guidance_status_pub.advertise(); + pid_init(&_pid_heading, PID_MODE_DERIVATIV_NONE, 0.001f); + pid_init(&_pid_throttle, PID_MODE_DERIVATIV_NONE, 0.001f); + +} + +void RoverDifferentialGuidance::updateParams() +{ + ModuleParams::updateParams(); + + _max_yaw_rate = _param_rd_max_yaw_rate.get() * M_DEG_TO_RAD_F; + pid_set_parameters(&_pid_heading, + _param_rd_p_gain_heading.get(), // Proportional gain + _param_rd_i_gain_heading.get(), // Integral gain + 0.f, // Derivative gain + _max_yaw_rate, // Integral limit + _max_yaw_rate); // Output limit + pid_set_parameters(&_pid_throttle, + _param_rd_p_gain_speed.get(), // Proportional gain + _param_rd_i_gain_speed.get(), // Integral gain + 0.f, // Derivative gain + 1.f, // Integral limit + 1.f); // Output limit +} + +RoverDifferentialGuidance::differential_setpoint RoverDifferentialGuidance::computeGuidance(const float yaw, + const float actual_speed, const int nav_state) +{ + // Initializations + bool mission_finished{false}; + float desired_speed{0.f}; + float desired_yaw_rate{0.f}; + hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + // uORB subscriber updates + if (_vehicle_global_position_sub.updated()) { + vehicle_global_position_s vehicle_global_position{}; + _vehicle_global_position_sub.copy(&vehicle_global_position); + _curr_pos = Vector2d(vehicle_global_position.lat, vehicle_global_position.lon); + } + + if (_local_position_sub.updated()) { + vehicle_local_position_s local_position{}; + _local_position_sub.copy(&local_position); + + if (!_global_ned_proj_ref.isInitialized() + || (_global_ned_proj_ref.getProjectionReferenceTimestamp() != local_position.ref_timestamp)) { + _global_ned_proj_ref.initReference(local_position.ref_lat, local_position.ref_lon, local_position.ref_timestamp); + } + + _curr_pos_ned = Vector2f(local_position.x, local_position.y); + } + + if (_position_setpoint_triplet_sub.updated()) { + updateWaypoints(); + } + + if (_mission_result_sub.updated()) { + mission_result_s mission_result{}; + _mission_result_sub.copy(&mission_result); + mission_finished = mission_result.finished; + } + + if (_home_position_sub.updated()) { + home_position_s home_position{}; + _home_position_sub.copy(&home_position); + _home_position = Vector2d(home_position.lat, home_position.lon); + } + + const float desired_heading = _pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, + math::max(actual_speed, 0.f)); + + const float heading_error = matrix::wrap_pi(desired_heading - yaw); + + const float distance_to_next_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), + _curr_wp(0), + _curr_wp(1)); + + if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL + && distance_to_next_wp < _param_nav_acc_rad.get()) { // Return to launch + mission_finished = true; + } + + // State machine + if (!mission_finished && distance_to_next_wp > _param_nav_acc_rad.get()) { + if (_currentState == GuidanceState::STOPPED) { + _currentState = GuidanceState::DRIVING; + } + + if (_currentState == GuidanceState::DRIVING && fabsf(heading_error) > _param_rd_trans_drv_trn.get()) { + pid_reset_integral(&_pid_heading); + _currentState = GuidanceState::SPOT_TURNING; + + } else if (_currentState == GuidanceState::SPOT_TURNING && fabsf(heading_error) < _param_rd_trans_trn_drv.get()) { + pid_reset_integral(&_pid_heading); + _currentState = GuidanceState::DRIVING; + } + + } else { // Mission finished or delay command + _currentState = GuidanceState::STOPPED; + } + + // Guidance logic + switch (_currentState) { + case GuidanceState::DRIVING: { + desired_speed = _param_rd_miss_spd_def.get(); + + if (_param_rd_max_jerk.get() > FLT_EPSILON && _param_rd_max_accel.get() > FLT_EPSILON) { + desired_speed = math::trajectory::computeMaxSpeedFromDistance(_param_rd_max_jerk.get(), + _param_rd_max_accel.get(), distance_to_next_wp, 0.0f); + desired_speed = math::constrain(desired_speed, -_param_rd_max_speed.get(), _param_rd_max_speed.get()); + } + + desired_yaw_rate = pid_calculate(&_pid_heading, heading_error, 0.f, 0.f, dt); + } break; + + case GuidanceState::SPOT_TURNING: + if (actual_speed < TURN_MAX_VELOCITY) { // Wait for the rover to stop + desired_yaw_rate = pid_calculate(&_pid_heading, heading_error, 0.f, 0.f, dt); // Turn on the spot + } + + break; + + case GuidanceState::STOPPED: + default: + desired_speed = 0.f; + desired_yaw_rate = 0.f; + break; + + } + + // Closed loop speed control + float throttle{0.f}; + + if (fabsf(desired_speed) < FLT_EPSILON) { + pid_reset_integral(&_pid_throttle); + + } else { + throttle = pid_calculate(&_pid_throttle, desired_speed, actual_speed, 0, + dt); + + if (_param_rd_max_speed.get() > FLT_EPSILON) { // Feed-forward term + throttle += math::interpolate(desired_speed, + 0.f, _param_rd_max_speed.get(), + 0.f, 1.f); + } + } + + // Publish differential controller status (logging) + _rover_differential_guidance_status.timestamp = _timestamp; + _rover_differential_guidance_status.desired_speed = desired_speed; + _rover_differential_guidance_status.pid_throttle_integral = _pid_throttle.integral; + _rover_differential_guidance_status.lookahead_distance = _pure_pursuit.getLookaheadDistance(); + _rover_differential_guidance_status.pid_heading_integral = _pid_heading.integral; + _rover_differential_guidance_status.heading_error_deg = M_RAD_TO_DEG_F * heading_error; + _rover_differential_guidance_status.state_machine = (uint8_t) _currentState; + _rover_differential_guidance_status_pub.publish(_rover_differential_guidance_status); + + // Return setpoints + differential_setpoint differential_setpoint_temp; + differential_setpoint_temp.throttle = math::constrain(throttle, 0.f, 1.f); + differential_setpoint_temp.yaw_rate = math::constrain(desired_yaw_rate, -_max_yaw_rate, + _max_yaw_rate); + differential_setpoint_temp.closed_loop_yaw_rate = true; + return differential_setpoint_temp; +} + +void RoverDifferentialGuidance::updateWaypoints() +{ + position_setpoint_triplet_s position_setpoint_triplet{}; + _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); + + // Global waypoint coordinates + if (position_setpoint_triplet.current.valid && PX4_ISFINITE(position_setpoint_triplet.current.lat) + && PX4_ISFINITE(position_setpoint_triplet.current.lon)) { + _curr_wp = Vector2d(position_setpoint_triplet.current.lat, position_setpoint_triplet.current.lon); + + } else { + _curr_wp = Vector2d(0, 0); + } + + if (position_setpoint_triplet.previous.valid && PX4_ISFINITE(position_setpoint_triplet.previous.lat) + && PX4_ISFINITE(position_setpoint_triplet.previous.lon)) { + _prev_wp = Vector2d(position_setpoint_triplet.previous.lat, position_setpoint_triplet.previous.lon); + + } else { + _prev_wp = _curr_pos; + } + + if (position_setpoint_triplet.next.valid && PX4_ISFINITE(position_setpoint_triplet.next.lat) + && PX4_ISFINITE(position_setpoint_triplet.next.lon)) { + _next_wp = Vector2d(position_setpoint_triplet.next.lat, position_setpoint_triplet.next.lon); + + } else { + _next_wp = _home_position; + } + + // NED waypoint coordinates + _curr_wp_ned = _global_ned_proj_ref.project(_curr_wp(0), _curr_wp(1)); + _prev_wp_ned = _global_ned_proj_ref.project(_prev_wp(0), _prev_wp(1)); + +} diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp new file mode 100644 index 000000000000..29a131f8d8e2 --- /dev/null +++ b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp @@ -0,0 +1,163 @@ +/**************************************************************************** + * + * Copyright (c) 2023-2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +// PX4 includes +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Standard libraries +#include +#include +#include +#include +#include +#include + +using namespace matrix; + +/** + * @brief Enum class for the different states of guidance. + */ +enum class GuidanceState { + SPOT_TURNING, // The vehicle is currently turning on the spot. + DRIVING, // The vehicle is currently driving. + STOPPED // The vehicle is stopped. +}; + +/** + * @brief Class for differential rover guidance. + */ +class RoverDifferentialGuidance : public ModuleParams +{ +public: + /** + * @brief Constructor for RoverDifferentialGuidance. + * @param parent The parent ModuleParams object. + */ + RoverDifferentialGuidance(ModuleParams *parent); + ~RoverDifferentialGuidance() = default; + + struct differential_setpoint { + float throttle{0.f}; + float yaw_rate{0.f}; + bool closed_loop_yaw_rate{false}; + }; + + /** + * @brief Compute guidance for the vehicle. + * @param yaw The yaw orientation of the vehicle in radians. + * @param actual_speed The velocity of the vehicle in m/s. + * @param dt The time step in seconds. + * @param nav_state Navigation state of the rover. + */ + RoverDifferentialGuidance::differential_setpoint computeGuidance(float yaw, float actual_speed, + int nav_state); + + /** + * @brief Update global/ned waypoint coordinates + */ + void updateWaypoints(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + // uORB subscriptions + uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; + uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; + uORB::Subscription _mission_result_sub{ORB_ID(mission_result)}; + uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _home_position_sub{ORB_ID(home_position)}; + + // uORB publications + uORB::Publication _rover_differential_guidance_status_pub{ORB_ID(rover_differential_guidance_status)}; + rover_differential_guidance_status_s _rover_differential_guidance_status{}; + + // Variables + MapProjection _global_ned_proj_ref{}; // Transform global to ned coordinates. + GuidanceState _currentState{GuidanceState::DRIVING}; // The current state of guidance. + PurePursuit _pure_pursuit{this}; // Pure pursuit library + hrt_abstime _timestamp{0}; + float _max_yaw_rate{0.f}; + + + // Waypoints + Vector2d _curr_pos{}; + Vector2f _curr_pos_ned{}; + Vector2d _prev_wp{}; + Vector2f _prev_wp_ned{}; + Vector2d _curr_wp{}; + Vector2f _curr_wp_ned{}; + Vector2d _next_wp{}; + Vector2d _home_position{}; + + // Controllers + PID_t _pid_heading; // The PID controller for the heading + PID_t _pid_throttle; // The PID controller for velocity + + // Constants + static constexpr float TURN_MAX_VELOCITY = 0.2f; // Velocity threshhold for starting the spot turn [m/s] + + // Parameters + DEFINE_PARAMETERS( + (ParamFloat) _param_rd_p_gain_heading, + (ParamFloat) _param_rd_i_gain_heading, + (ParamFloat) _param_rd_p_gain_speed, + (ParamFloat) _param_rd_i_gain_speed, + (ParamFloat) _param_rd_max_speed, + (ParamFloat) _param_nav_acc_rad, + (ParamFloat) _param_rd_max_jerk, + (ParamFloat) _param_rd_max_accel, + (ParamFloat) _param_rd_miss_spd_def, + (ParamFloat) _param_rd_max_yaw_rate, + (ParamFloat) _param_rd_trans_trn_drv, + (ParamFloat) _param_rd_trans_drv_trn + + ) +}; diff --git a/src/modules/differential_drive/module.yaml b/src/modules/rover_differential/module.yaml similarity index 55% rename from src/modules/differential_drive/module.yaml rename to src/modules/rover_differential/module.yaml index 50ab9bb837ca..be0a76c60e9b 100644 --- a/src/modules/differential_drive/module.yaml +++ b/src/modules/rover_differential/module.yaml @@ -1,11 +1,12 @@ -module_name: Differential Drive +module_name: Rover Differential parameters: - - group: Rover Differential Drive + - group: Rover Differential definitions: - RDD_WHEEL_BASE: + + RD_WHEEL_TRACK: description: - short: Wheel base + short: Wheel track long: Distance from the center of the right wheel to the center of the left wheel type: float unit: m @@ -14,55 +15,41 @@ parameters: increment: 0.001 decimal: 3 default: 0.5 - RDD_WHEEL_RADIUS: - description: - short: Wheel radius - long: Size of the wheel, half the diameter of the wheel - type: float - unit: m - min: 0.001 - max: 100 - increment: 0.001 - decimal: 3 - default: 0.1 - RDD_SPEED_SCALE: - description: - short: Manual speed scale - type: float - min: 0 - max: 1 - increment: 0.01 - decimal: 2 - default: 1 - RDD_ANG_SCALE: + + RD_MAN_YAW_SCALE: description: - short: Manual angular velocity scale + short: Manual yaw rate scale + long: | + In manual mode the setpoint for the yaw rate received from the rc remote + is scaled by this value. type: float - min: 0 + min: 0.01 max: 1 increment: 0.01 decimal: 2 default: 1 - RDD_WHEEL_SPEED: + + RD_HEADING_P: description: - short: Maximum wheel speed + short: Proportional gain for heading controller type: float - unit: rad/s min: 0 max: 100 increment: 0.01 decimal: 2 - default: 0.3 - RDD_P_HEADING: + default: 1 + + RD_HEADING_I: description: - short: Proportional gain for heading controller + short: Integral gain for heading controller type: float min: 0 max: 100 increment: 0.01 decimal: 2 - default: 1 - RDD_P_SPEED: + default: 0.1 + + RD_SPEED_P: description: short: Proportional gain for speed controller type: float @@ -71,7 +58,8 @@ parameters: increment: 0.01 decimal: 2 default: 1 - RDD_I_SPEED: + + RD_SPEED_I: description: short: Integral gain for ground speed controller type: float @@ -80,7 +68,8 @@ parameters: increment: 0.01 decimal: 2 default: 0 - RDD_P_ANG_VEL: + + RD_YAW_RATE_P: description: short: Proportional gain for angular velocity controller type: float @@ -89,7 +78,8 @@ parameters: increment: 0.01 decimal: 2 default: 1 - RDD_I_ANG_VEL: + + RD_YAW_RATE_I: description: short: Integral gain for angular velocity controller type: float @@ -98,7 +88,8 @@ parameters: increment: 0.01 decimal: 2 default: 0 - RDD_MAX_JERK: + + RD_MAX_JERK: description: short: Maximum jerk long: Limit for forwards acc/deceleration change. @@ -109,7 +100,8 @@ parameters: increment: 0.01 decimal: 2 default: 0.5 - RDD_MAX_ACCEL: + + RD_MAX_ACCEL: description: short: Maximum acceleration long: Maximum acceleration is used to limit the acceleration of the rover @@ -120,3 +112,61 @@ parameters: increment: 0.01 decimal: 2 default: 0.5 + + RD_MAX_SPEED: + description: + short: Maximum speed the rover can drive + long: This parameter is used to map desired speeds to normalized motor commands. + type: float + unit: m/s + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 7 + + RD_MAX_YAW_RATE: + description: + short: Maximum allowed yaw rate for the rover. + long: | + This parameter is used to cap desired yaw rates and map controller inputs to desired yaw rates in acro mode. + type: float + unit: deg/s + min: 0.01 + max: 1000 + increment: 0.01 + decimal: 2 + default: 90 + + RD_MISS_SPD_DEF: + description: + short: Default rover speed during a mission + type: float + unit: m/s + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 1 + + RD_TRANS_TRN_DRV: + description: + short: Heading error threshhold to switch from spot turning to driving + type: float + unit: rad + min: 0.001 + max: 3.14159 + increment: 0.01 + decimal: 3 + default: 0.0872665 + + RD_TRANS_DRV_TRN: + description: + short: Heading error threshhold to switch from driving to spot turning + type: float + unit: rad + min: 0.001 + max: 3.14159 + increment: 0.01 + decimal: 3 + default: 0.174533 diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index 24909280e99c..928ce13c6edb 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -122,8 +122,8 @@ RoverPositionControl::manual_control_setpoint_poll() if (_control_mode.flag_control_attitude_enabled) { // STABILIZED mode generate the attitude setpoint from manual user inputs - _att_sp.roll_body = 0.0; - _att_sp.pitch_body = 0.0; + float roll_body = 0.0; + float pitch_body = 0.0; /* reset yaw setpoint to current position if needed */ if (_reset_yaw_sp) { @@ -137,10 +137,10 @@ RoverPositionControl::manual_control_setpoint_poll() _manual_yaw_sp = wrap_pi(_manual_yaw_sp + _att_sp.yaw_sp_move_rate * dt); } - _att_sp.yaw_body = _manual_yaw_sp; + float yaw_body = _manual_yaw_sp; _att_sp.thrust_body[0] = _manual_control_setpoint.throttle; - Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); + const Quatf q(Eulerf(roll_body, pitch_body, yaw_body)); q.copyTo(_att_sp.q_d); _att_sp.timestamp = hrt_absolute_time(); diff --git a/src/modules/simulation/gz_bridge/CMakeLists.txt b/src/modules/simulation/gz_bridge/CMakeLists.txt index 003a712282f9..c7d36e36a5ad 100644 --- a/src/modules/simulation/gz_bridge/CMakeLists.txt +++ b/src/modules/simulation/gz_bridge/CMakeLists.txt @@ -93,6 +93,7 @@ if(gz-transport_FOUND) windy baylands lawn + rover ) # find corresponding airframes diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 3464c628694c..f7d80d32cac6 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -150,16 +150,31 @@ int GZBridge::init() // If PX4_GZ_STANDALONE has been set, you can try to connect but GZ_SIM_RESOURCE_PATH needs to be set correctly to work. else { - if (_node.Request(create_service, req, 1000, rep, result)) { - if (!rep.data() || !result) { - PX4_ERR("EntityFactory service call failed."); - return PX4_ERROR; - } - - } else { - PX4_ERR("Service call timed out. Check GZ_SIM_RESOURCE_PATH is set correctly."); + if (!callEntityFactoryService(create_service, req)) { return PX4_ERROR; } + + std::string scene_info_service = "/world/default/scene/info"; + bool scene_created = false; + + while (scene_created == false) { + if (!callSceneInfoMsgService(scene_info_service)) { + PX4_WARN("Service call timed out as Gazebo has not been detected."); + system_usleep(2000000); + + } else { + scene_created = true; + } + } + + gz::msgs::StringMsg follow_msg{}; + follow_msg.set_data(_model_name); + bool call_string_service = callStringMsgService("/gui/follow", follow_msg); + gz::msgs::Vector3d follow_offset_msg{}; + follow_offset_msg.set_x(-2.0); + follow_offset_msg.set_y(-2.0); + follow_offset_msg.set_z(2.0); + callVector3dService("/gui/follow/offset", follow_offset_msg); } } @@ -829,6 +844,93 @@ void GZBridge::laserScanCallback(const gz::msgs::LaserScan &scan) _obstacle_distance_pub.publish(obs); } +bool GZBridge::callEntityFactoryService(const std::string &service, const gz::msgs::EntityFactory &req) +{ + bool result; + gz::msgs::Boolean rep; + + if (_node.Request(service, req, 1000, rep, result)) { + if (!rep.data() || !result) { + PX4_ERR("EntityFactory service call failed."); + return false; + } + + } else { + PX4_ERR("Service call timed out. Check GZ_SIM_RESOURCE_PATH is set correctly."); + return false; + } + + return true; +} + +bool GZBridge::callSceneInfoMsgService(const std::string &service) +{ + bool result; + gz::msgs::Empty req; + gz::msgs::Scene rep; + + if (_node.Request(service, req, 1000, rep, result)) { + if (!result) { + PX4_ERR("Scene Info service call failed."); + return false; + + } else { + return true; + } + + } else { + PX4_ERR("Service call timed out. Check GZ_SIM_RESOURCE_PATH is set correctly."); + return false; + } + + return true; +} + +bool GZBridge::callStringMsgService(const std::string &service, const gz::msgs::StringMsg &req) +{ + bool result; + + gz::msgs::Boolean rep; + + if (_node.Request(service, req, 1000, rep, result)) { + if (!rep.data() || !result) { + PX4_ERR("String service call failed"); + return false; + + } + } + + else { + PX4_ERR("Service call timed out: %s", service.c_str()); + return false; + } + + return true; +} + +bool GZBridge::callVector3dService(const std::string &service, const gz::msgs::Vector3d &req) +{ + bool result; + + gz::msgs::Boolean rep; + + if (_node.Request(service, req, 1000, rep, result)) { + if (!rep.data() || !result) { + PX4_ERR("String service call failed"); + return false; + + } + } + + else { + PX4_ERR("Service call timed out: %s", service.c_str()); + return false; + } + + return true; +} + + void GZBridge::rotateQuaternion(gz::math::Quaterniond &q_FRD_to_NED, const gz::math::Quaterniond q_FLU_to_ENU) { // FLU (ROS) to FRD (PX4) static rotation diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index 8a832f7961b2..fc2cb3a6e9ad 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -69,6 +69,8 @@ #include #include #include +#include +#include using namespace time_literals; @@ -110,6 +112,45 @@ class GZBridge : public ModuleBase, public ModuleParams, public px4::S void navSatCallback(const gz::msgs::NavSat &nav_sat); void laserScanCallback(const gz::msgs::LaserScan &scan); + /** + * @brief Call Entityfactory service + * + * @param req + * @return true + * @return false + */ + bool callEntityFactoryService(const std::string &service, const gz::msgs::EntityFactory &req); + + + /** + * @brief Call scene info service + * + * @param service + * @param req + * @return true + * @return false + */ + bool callSceneInfoMsgService(const std::string &service); + + /** + * @brief Call String service + * + * @param service + * @param req + * @return true + * @return false + */ + bool callStringMsgService(const std::string &service, const gz::msgs::StringMsg &req); + + /** + * @brief Call Vector3d Service + * + * @param service + * @param req + * @return true + * @return false + */ + bool callVector3dService(const std::string &service, const gz::msgs::Vector3d &req); /** * * Convert a quaterion from FLU_to_ENU frames (ROS convention) diff --git a/src/modules/uuv_att_control/uuv_att_control.cpp b/src/modules/uuv_att_control/uuv_att_control.cpp index 58992ccb7803..52baccb8dde6 100644 --- a/src/modules/uuv_att_control/uuv_att_control.cpp +++ b/src/modules/uuv_att_control/uuv_att_control.cpp @@ -138,9 +138,10 @@ void UUVAttitudeControl::control_attitude_geo(const vehicle_attitude_s &attitude */ Eulerf euler_angles(matrix::Quatf(attitude.q)); - float roll_body = attitude_setpoint.roll_body; - float pitch_body = attitude_setpoint.pitch_body; - float yaw_body = attitude_setpoint.yaw_body; + const Eulerf setpoint_euler_angles(matrix::Quatf(attitude_setpoint.q_d)); + const float roll_body = setpoint_euler_angles(0); + const float pitch_body = setpoint_euler_angles(1); + const float yaw_body = setpoint_euler_angles(2); float roll_rate_desired = rates_setpoint.roll; float pitch_rate_desired = rates_setpoint.pitch; @@ -227,9 +228,8 @@ void UUVAttitudeControl::Run() _vehicle_rates_setpoint_sub.update(&_rates_setpoint); if (input_mode == 1) { // process manual data - _attitude_setpoint.roll_body = _param_direct_roll.get(); - _attitude_setpoint.pitch_body = _param_direct_pitch.get(); - _attitude_setpoint.yaw_body = _param_direct_yaw.get(); + Quatf attitude_setpoint(Eulerf(_param_direct_roll.get(), _param_direct_pitch.get(), _param_direct_yaw.get())); + attitude_setpoint.copyTo(_attitude_setpoint.q_d); _attitude_setpoint.thrust_body[0] = _param_direct_thrust.get(); _attitude_setpoint.thrust_body[1] = 0.f; _attitude_setpoint.thrust_body[2] = 0.f; diff --git a/src/modules/uuv_pos_control/uuv_pos_control.cpp b/src/modules/uuv_pos_control/uuv_pos_control.cpp index d402941b4a16..272ea56def8d 100644 --- a/src/modules/uuv_pos_control/uuv_pos_control.cpp +++ b/src/modules/uuv_pos_control/uuv_pos_control.cpp @@ -97,9 +97,8 @@ void UUVPOSControl::publish_attitude_setpoint(const float thrust_x, const float vehicle_attitude_setpoint_s vehicle_attitude_setpoint = {}; vehicle_attitude_setpoint.timestamp = hrt_absolute_time(); - vehicle_attitude_setpoint.roll_body = roll_des; - vehicle_attitude_setpoint.pitch_body = pitch_des; - vehicle_attitude_setpoint.yaw_body = yaw_des; + const Quatf attitude_setpoint(Eulerf(roll_des, pitch_des, yaw_des)); + attitude_setpoint.copyTo(vehicle_attitude_setpoint.q_d); vehicle_attitude_setpoint.thrust_body[0] = thrust_x; vehicle_attitude_setpoint.thrust_body[1] = thrust_y; diff --git a/src/modules/uxrce_dds_client/dds_topics.yaml b/src/modules/uxrce_dds_client/dds_topics.yaml index 67b1e9456cee..e9e67cff25ed 100644 --- a/src/modules/uxrce_dds_client/dds_topics.yaml +++ b/src/modules/uxrce_dds_client/dds_topics.yaml @@ -130,9 +130,6 @@ subscriptions: - topic: /fmu/in/vehicle_rates_setpoint type: px4_msgs::msg::VehicleRatesSetpoint - - topic: /fmu/in/differential_drive_setpoint - type: px4_msgs::msg::DifferentialDriveSetpoint - - topic: /fmu/in/vehicle_visual_odometry type: px4_msgs::msg::VehicleOdometry diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 84606aad4327..105ee1ee6efa 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -172,6 +172,11 @@ void Standard::update_transition_state() VtolType::update_transition_state(); + const Eulerf attitude_setpoint_euler(Quatf(_v_att_sp->q_d)); + float roll_body = attitude_setpoint_euler.phi(); + float pitch_body = attitude_setpoint_euler.theta(); + float yaw_body = attitude_setpoint_euler.psi(); + // we get attitude setpoint from a multirotor flighttask if climbrate is controlled. // in any other case the fixed wing attitude controller publishes attitude setpoint from manual stick input. if (_v_control_mode->flag_control_climb_rate_enabled) { @@ -181,7 +186,7 @@ void Standard::update_transition_state() } memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); - _v_att_sp->roll_body = _fw_virtual_att_sp->roll_body; + roll_body = Eulerf(Quatf(_fw_virtual_att_sp->q_d)).phi(); } else { // we need a recent incoming (fw virtual) attitude setpoint, otherwise return (means the previous setpoint stays active) @@ -227,21 +232,19 @@ void Standard::update_transition_state() } // ramp up FW_PSP_OFF - _v_att_sp->pitch_body = math::radians(_param_fw_psp_off.get()) * (1.0f - mc_weight); - + pitch_body = math::radians(_param_fw_psp_off.get()) * (1.0f - mc_weight); _v_att_sp->thrust_body[0] = _pusher_throttle; - - const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body)); + const Quatf q_sp(Eulerf(roll_body, pitch_body, yaw_body)); q_sp.copyTo(_v_att_sp->q_d); } else if (_vtol_mode == vtol_mode::TRANSITION_TO_MC) { if (_v_control_mode->flag_control_climb_rate_enabled) { // control backtransition deceleration using pitch. - _v_att_sp->pitch_body = update_and_get_backtransition_pitch_sp(); + pitch_body = update_and_get_backtransition_pitch_sp(); } - const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body)); + const Quatf q_sp(Eulerf(roll_body, pitch_body, yaw_body)); q_sp.copyTo(_v_att_sp->q_d); _pusher_throttle = 0.0f; diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index af95a05083c3..eaacbe68d611 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -179,7 +179,8 @@ void Tailsitter::update_transition_state() // the yaw setpoint and zero roll since we want wings level transition. // If for some reason the fw attitude setpoint is not recent then don't use it and assume 0 pitch if (_fw_virtual_att_sp->timestamp > (now - 1_s)) { - _q_trans_start = Eulerf(0.f, _fw_virtual_att_sp->pitch_body, yaw_sp); + const float pitch_body = Eulerf(Quatf(_fw_virtual_att_sp->q_d)).theta(); + _q_trans_start = Eulerf(0.f, pitch_body, yaw_sp); } else { _q_trans_start = Eulerf(0.f, 0.f, yaw_sp); @@ -191,7 +192,8 @@ void Tailsitter::update_transition_state() } else if (_vtol_mode == vtol_mode::TRANSITION_FRONT_P1) { // initial attitude setpoint for the transition should be with wings level - _q_trans_start = Eulerf(0.f, _mc_virtual_att_sp->pitch_body, _mc_virtual_att_sp->yaw_body); + const Eulerf setpoint_euler(Quatf(_mc_virtual_att_sp->q_d)); + _q_trans_start = Eulerf(0.f, setpoint_euler.theta(), setpoint_euler.psi()); Vector3f x = Dcmf(Quatf(_v_att->q)) * Vector3f(1.f, 0.f, 0.f); _trans_rot_axis = -x.cross(Vector3f(0.f, 0.f, -1.f)); } @@ -238,10 +240,6 @@ void Tailsitter::update_transition_state() _v_att_sp->timestamp = hrt_absolute_time(); const Eulerf euler_sp(_q_trans_sp); - _v_att_sp->roll_body = euler_sp.phi(); - _v_att_sp->pitch_body = euler_sp.theta(); - _v_att_sp->yaw_body = euler_sp.psi(); - _q_trans_sp.copyTo(_v_att_sp->q_d); } diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 0956e1003698..3a1904257993 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -203,6 +203,11 @@ void Tiltrotor::update_transition_state() const hrt_abstime now = hrt_absolute_time(); + const Eulerf attitude_setpoint_euler(Quatf(_v_att_sp->q_d)); + float roll_body = attitude_setpoint_euler.phi(); + float pitch_body = attitude_setpoint_euler.theta(); + float yaw_body = attitude_setpoint_euler.psi(); + // we get attitude setpoint from a multirotor flighttask if altitude is controlled. // in any other case the fixed wing attitude controller publishes attitude setpoint from manual stick input. if (_v_control_mode->flag_control_climb_rate_enabled) { @@ -212,7 +217,7 @@ void Tiltrotor::update_transition_state() } memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); - _v_att_sp->roll_body = _fw_virtual_att_sp->roll_body; + roll_body = Eulerf(Quatf(_fw_virtual_att_sp->q_d)).phi(); _thrust_transition = -_mc_virtual_att_sp->thrust_body[2]; } else { @@ -290,7 +295,7 @@ void Tiltrotor::update_transition_state() // control backtransition deceleration using pitch. if (_v_control_mode->flag_control_climb_rate_enabled) { - _v_att_sp->pitch_body = update_and_get_backtransition_pitch_sp(); + pitch_body = update_and_get_backtransition_pitch_sp(); } if (_time_since_trans_start < BACKTRANS_THROTTLE_DOWNRAMP_DUR_S) { @@ -321,7 +326,7 @@ void Tiltrotor::update_transition_state() _v_att_sp->thrust_body[2] = -_thrust_transition; - const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body)); + const Quatf q_sp(Eulerf(roll_body, pitch_body, yaw_body)); q_sp.copyTo(_v_att_sp->q_d); _mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f); diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index e43dd7c912ad..458b3927fb9c 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -563,10 +563,10 @@ float VtolType::pusher_assist() tilt_new = R_yaw_correction * tilt_new; // now extract roll and pitch setpoints - _v_att_sp->pitch_body = atan2f(tilt_new(0), tilt_new(2)); - _v_att_sp->roll_body = -asinf(tilt_new(1)); + const float pitch_body = atan2f(tilt_new(0), tilt_new(2)); + const float roll_body = -asinf(tilt_new(1)); - const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, euler_sp(2))); + const Quatf q_sp(Eulerf(roll_body, pitch_body, euler_sp(2))); q_sp.copyTo(_v_att_sp->q_d); } diff --git a/src/modules/zenoh/CMakeLists.txt b/src/modules/zenoh/CMakeLists.txt index 889a1994a427..e3a8166dff73 100644 --- a/src/modules/zenoh/CMakeLists.txt +++ b/src/modules/zenoh/CMakeLists.txt @@ -57,6 +57,7 @@ target_compile_options(zenohpico PUBLIC -Wno-cast-align -DZ_BATCH_SIZE_TX=512 -DZ_FRAG_MAX_SIZE=1024) +target_compile_options(zenohpico PRIVATE -Wno-missing-prototypes) if(CONFIG_PLATFORM_NUTTX) target_compile_options(zenohpico PRIVATE -DUNIX_NO_MULTICAST_IF) endif() @@ -72,7 +73,6 @@ px4_add_module( SRCS zenoh.cpp zenoh_config.cpp - zenoh.h publishers/zenoh_publisher.cpp subscribers/zenoh_subscriber.cpp MODULE_CONFIG @@ -96,6 +96,5 @@ px4_add_module( -Wno-double-promotion -Wno-unused -DZENOH_LINUX - -DZENOH_NO_STDATOMIC -D_Bool=int8_t ) diff --git a/src/modules/zenoh/Kconfig.topics b/src/modules/zenoh/Kconfig.topics index e1c2ea4ada5b..ee9fe7ce7362 100644 --- a/src/modules/zenoh/Kconfig.topics +++ b/src/modules/zenoh/Kconfig.topics @@ -573,6 +573,10 @@ menu "Zenoh publishers/subscribers" bool "rover_ackermann_guidance_status" default n + config ZENOH_PUBSUB_ROVER_ACKERMANN_STATUS + bool "rover_ackermann_status" + default n + config ZENOH_PUBSUB_RPM bool "rpm" default n @@ -1009,6 +1013,7 @@ config ZENOH_PUBSUB_ALL_SELECTION select ZENOH_PUBSUB_REGISTER_EXT_COMPONENT_REPLY select ZENOH_PUBSUB_REGISTER_EXT_COMPONENT_REQUEST select ZENOH_PUBSUB_ROVER_ACKERMANN_GUIDANCE_STATUS + select ZENOH_PUBSUB_ROVER_ACKERMANN_STATUS select ZENOH_PUBSUB_RPM select ZENOH_PUBSUB_RTL_STATUS select ZENOH_PUBSUB_RTL_TIME_ESTIMATE diff --git a/src/modules/zenoh/publishers/uorb_publisher.hpp b/src/modules/zenoh/publishers/uorb_publisher.hpp index b541bfd1dd7c..fd41e4f2e88c 100644 --- a/src/modules/zenoh/publishers/uorb_publisher.hpp +++ b/src/modules/zenoh/publishers/uorb_publisher.hpp @@ -51,7 +51,7 @@ class uORB_Zenoh_Publisher : public Zenoh_Publisher { public: uORB_Zenoh_Publisher(const orb_metadata *meta, const uint32_t *ops) : - Zenoh_Publisher(true), + Zenoh_Publisher(), _uorb_meta{meta}, _cdr_ops(ops) { diff --git a/src/modules/zenoh/publishers/zenoh_publisher.cpp b/src/modules/zenoh/publishers/zenoh_publisher.cpp index 1e312ffea502..8ce688d2b787 100644 --- a/src/modules/zenoh/publishers/zenoh_publisher.cpp +++ b/src/modules/zenoh/publishers/zenoh_publisher.cpp @@ -42,9 +42,8 @@ #include "zenoh_publisher.hpp" -Zenoh_Publisher::Zenoh_Publisher(bool rostopic) +Zenoh_Publisher::Zenoh_Publisher() { - this->_rostopic = rostopic; this->_topic[0] = 0x0; } @@ -59,24 +58,18 @@ int Zenoh_Publisher::undeclare_publisher() return 0; } -int Zenoh_Publisher::declare_publisher(z_session_t s, const char *keyexpr) +int Zenoh_Publisher::declare_publisher(z_owned_session_t s, const char *keyexpr) { - if (_rostopic) { - strncpy(this->_topic, (char *)_rt_prefix, _rt_prefix_offset); + strncpy(this->_topic, keyexpr, sizeof(this->_topic)); - if (keyexpr[0] == '/') { - strncpy(this->_topic + _rt_prefix_offset, keyexpr + 1, sizeof(this->_topic) - _rt_prefix_offset); + z_view_keyexpr_t ke; + z_view_keyexpr_from_str(&ke, this->_topic); - } else { - strncpy(this->_topic + _rt_prefix_offset, keyexpr, sizeof(this->_topic) - _rt_prefix_offset); - } - - } else { - strncpy(this->_topic, keyexpr, sizeof(this->_topic)); + if (z_declare_publisher(&_pub, z_loan(s), z_loan(ke), NULL) < 0) { + printf("Unable to declare publisher for key expression!\n"); + return -1; } - _pub = z_declare_publisher(s, z_keyexpr(this->_topic), NULL); - if (!z_publisher_check(&_pub)) { printf("Unable to declare publisher for key expression!\n"); return -1; @@ -87,9 +80,13 @@ int Zenoh_Publisher::declare_publisher(z_session_t s, const char *keyexpr) int8_t Zenoh_Publisher::publish(const uint8_t *buf, int size) { - z_publisher_put_options_t options = z_publisher_put_options_default(); - options.encoding = z_encoding(Z_ENCODING_PREFIX_APP_CUSTOM, NULL); - return z_publisher_put(z_publisher_loan(&_pub), buf, size, &options); + z_publisher_put_options_t options; + z_publisher_put_options_default(&options); + options.encoding = NULL; + + z_owned_bytes_t payload; + z_bytes_serialize_from_slice(&payload, buf, size); + return z_publisher_put(z_loan(_pub), z_move(payload), &options); } void Zenoh_Publisher::print() diff --git a/src/modules/zenoh/publishers/zenoh_publisher.hpp b/src/modules/zenoh/publishers/zenoh_publisher.hpp index 1d88a453e2f7..a12849c78682 100644 --- a/src/modules/zenoh/publishers/zenoh_publisher.hpp +++ b/src/modules/zenoh/publishers/zenoh_publisher.hpp @@ -51,10 +51,10 @@ class Zenoh_Publisher : public ListNode { public: - Zenoh_Publisher(bool rostopic = true); + Zenoh_Publisher(); virtual ~Zenoh_Publisher(); - virtual int declare_publisher(z_session_t s, const char *keyexpr); + virtual int declare_publisher(z_owned_session_t s, const char *keyexpr); virtual int undeclare_publisher(); @@ -66,11 +66,5 @@ class Zenoh_Publisher : public ListNode int8_t publish(const uint8_t *, int size); z_owned_publisher_t _pub; - char _topic[60]; // The Topic name is somewhere is the Zenoh stack as well but no good api to fetch it. - - // Indicates ROS2 Topic namespace - bool _rostopic; - const char *_rt_prefix = "rt/"; - const size_t _rt_prefix_offset = 3; // "rt/" are 3 chars }; diff --git a/src/modules/zenoh/subscribers/uorb_subscriber.hpp b/src/modules/zenoh/subscribers/uorb_subscriber.hpp index 550c1a3f4584..bd59adb84606 100644 --- a/src/modules/zenoh/subscribers/uorb_subscriber.hpp +++ b/src/modules/zenoh/subscribers/uorb_subscriber.hpp @@ -50,7 +50,7 @@ class uORB_Zenoh_Subscriber : public Zenoh_Subscriber { public: uORB_Zenoh_Subscriber(const orb_metadata *meta, const uint32_t *ops) : - Zenoh_Subscriber(true), + Zenoh_Subscriber(), _uorb_meta{meta}, _cdr_ops(ops) { @@ -61,11 +61,14 @@ class uORB_Zenoh_Subscriber : public Zenoh_Subscriber ~uORB_Zenoh_Subscriber() override = default; // Update the uORB Subscription and broadcast a Zenoh ROS2 message - void data_handler(const z_sample_t *sample) + void data_handler(const z_loaned_sample_t *sample) { char data[_uorb_meta->o_size]; - dds_istream_t is = {.m_buffer = (unsigned char *)(sample->payload.start), .m_size = static_cast(sample->payload.len), + const z_loaned_bytes_t *payload = z_sample_payload(sample); + size_t len = z_bytes_len(payload); + + dds_istream_t is = {.m_buffer = (unsigned char *)(payload), .m_size = static_cast(len), .m_index = 4, .m_xcdr_version = DDSI_RTPS_CDR_ENC_VERSION_2 }; dds_stream_read(&is, data, &dds_allocator, _cdr_ops); diff --git a/src/modules/zenoh/subscribers/zenoh_subscriber.cpp b/src/modules/zenoh/subscribers/zenoh_subscriber.cpp index aab66ee5bdba..79f37d89812a 100644 --- a/src/modules/zenoh/subscribers/zenoh_subscriber.cpp +++ b/src/modules/zenoh/subscribers/zenoh_subscriber.cpp @@ -41,22 +41,23 @@ #include "zenoh_subscriber.hpp" -static void data_handler_cb(const z_sample_t *sample, void *arg) +static void data_handler_cb(const z_loaned_sample_t *sample, void *arg) { static_cast(arg)->data_handler(sample); } -void Zenoh_Subscriber::data_handler(const z_sample_t *sample) +void Zenoh_Subscriber::data_handler(const z_loaned_sample_t *sample) { - z_owned_str_t keystr = z_keyexpr_to_string(sample->keyexpr); - printf(">> [Subscriber] Received ('%s' size '%d')\n", z_str_loan(&keystr), (int)sample->payload.len); - z_str_drop(z_str_move(&keystr)); + z_view_string_t keystr; + z_keyexpr_as_view_string(z_sample_keyexpr(sample), &keystr); + z_owned_slice_t value; + z_bytes_deserialize_into_slice(z_sample_payload(sample), &value); + printf(">> [Subscriber] Received ('%s' size '%d')\n", z_string_data(z_loan(keystr)), (int)z_slice_len(z_loan(value))); } -Zenoh_Subscriber::Zenoh_Subscriber(bool rostopic) +Zenoh_Subscriber::Zenoh_Subscriber() { - this->_rostopic = rostopic; this->_topic[0] = 0x0; } @@ -71,27 +72,21 @@ int Zenoh_Subscriber::undeclare_subscriber() return 0; } -int Zenoh_Subscriber::declare_subscriber(z_session_t s, const char *keyexpr) +int Zenoh_Subscriber::declare_subscriber(z_owned_session_t s, const char *keyexpr) { - z_owned_closure_sample_t callback = z_closure_sample(data_handler_cb, NULL, this); + z_owned_closure_sample_t callback; + z_closure_sample(&callback, data_handler_cb, NULL, this); - if (_rostopic) { - strncpy(this->_topic, (char *)_rt_prefix, _rt_prefix_offset); + strncpy(this->_topic, keyexpr, sizeof(this->_topic)); - if (keyexpr[0] == '/') { - strncpy(this->_topic + _rt_prefix_offset, keyexpr + 1, sizeof(this->_topic) - _rt_prefix_offset); + z_view_keyexpr_t ke; + z_view_keyexpr_from_str(&ke, this->_topic); - } else { - strncpy(this->_topic + _rt_prefix_offset, keyexpr, sizeof(this->_topic) - _rt_prefix_offset); - } - - } else { - strncpy(this->_topic, (char *)keyexpr, sizeof(this->_topic)); + if (z_declare_subscriber(&_sub, z_loan(s), z_loan(ke), z_closure_sample_move(&callback), NULL) < 0) { + printf("Unable to declare subscriber.\n"); + exit(-1); } - _sub = z_declare_subscriber(s, z_keyexpr(this->_topic), z_closure_sample_move(&callback), NULL); - - if (!z_subscriber_check(&_sub)) { printf("Unable to declare subscriber for key expression!\n %s\n", keyexpr); return -1; diff --git a/src/modules/zenoh/subscribers/zenoh_subscriber.hpp b/src/modules/zenoh/subscribers/zenoh_subscriber.hpp index e3f1200e9245..1b6d10922045 100644 --- a/src/modules/zenoh/subscribers/zenoh_subscriber.hpp +++ b/src/modules/zenoh/subscribers/zenoh_subscriber.hpp @@ -55,14 +55,14 @@ class Zenoh_Subscriber : public ListNode { public: - Zenoh_Subscriber(bool rostopic = true); + Zenoh_Subscriber(); virtual ~Zenoh_Subscriber(); - virtual int declare_subscriber(z_session_t s, const char *keyexpr); + virtual int declare_subscriber(z_owned_session_t s, const char *keyexpr); virtual int undeclare_subscriber(); - virtual void data_handler(const z_sample_t *sample); + virtual void data_handler(const z_loaned_sample_t *sample); virtual void print(); @@ -71,10 +71,4 @@ class Zenoh_Subscriber : public ListNode z_owned_subscriber_t _sub; char _topic[60]; // The Topic name is somewhere is the Zenoh stack as well but no good api to fetch it. - - - // Indicates ROS2 Topic namespace - bool _rostopic; - const char *_rt_prefix = "rt/"; - const size_t _rt_prefix_offset = 3; // "rt/" are 3 chars }; diff --git a/src/modules/zenoh/zenoh-pico b/src/modules/zenoh/zenoh-pico index 22bbfc215092..f093aa7955bb 160000 --- a/src/modules/zenoh/zenoh-pico +++ b/src/modules/zenoh/zenoh-pico @@ -1 +1 @@ -Subproject commit 22bbfc215092a051341c234fdcf68f5baa267dec +Subproject commit f093aa7955bb24dfa3e626de084583711e3bac5c diff --git a/src/modules/zenoh/zenoh.cpp b/src/modules/zenoh/zenoh.cpp index 0caf2c5eaa5f..6411734a2918 100644 --- a/src/modules/zenoh/zenoh.cpp +++ b/src/modules/zenoh/zenoh.cpp @@ -32,6 +32,8 @@ ****************************************************************************/ #include "zenoh.h" +#include "zenoh-pico/api/macros.h" +#include "zenoh-pico/api/primitives.h" #include #include #include @@ -78,27 +80,39 @@ void ZENOH::run() z_config.getNetworkConfig(mode, locator); - z_owned_config_t config = z_config_default(); - zp_config_insert(z_config_loan(&config), Z_CONFIG_MODE_KEY, z_string_make(mode)); + z_owned_config_t config; + z_config_default(&config); + zp_config_insert(z_loan_mut(config), Z_CONFIG_MODE_KEY, mode); if (locator[0] != 0) { - zp_config_insert(z_config_loan(&config), Z_CONFIG_PEER_KEY, z_string_make(locator)); + zp_config_insert(z_loan_mut(config), Z_CONFIG_CONNECT_KEY, locator); } else if (strcmp(Z_CONFIG_MODE_PEER, mode) == 0) { - zp_config_insert(z_config_loan(&config), Z_CONFIG_PEER_KEY, z_string_make(Z_CONFIG_MULTICAST_LOCATOR_DEFAULT)); + zp_config_insert(z_loan_mut(config), Z_CONFIG_CONNECT_KEY, Z_CONFIG_MULTICAST_LOCATOR_DEFAULT); } PX4_INFO("Opening session..."); - z_owned_session_t s = z_open(z_config_move(&config)); + z_owned_session_t s; + ret = z_open(&s, z_move(config)); + + if (ret < 0) { + PX4_ERR("Unable to open session, ret: %d", ret); + return; + } + + PX4_INFO("Checking session..."); if (!z_session_check(&s)) { - PX4_ERR("Unable to open session!"); + PX4_ERR("Unable to check session!"); return; } + PX4_INFO("Starting reading/writing tasks..."); + // Start read and lease tasks for zenoh-pico - if (zp_start_read_task(z_session_loan(&s), NULL) < 0 || zp_start_lease_task(z_session_loan(&s), NULL) < 0) { + if (zp_start_read_task(z_loan_mut(s), NULL) < 0 || zp_start_lease_task(z_loan_mut(s), NULL) < 0) { PX4_ERR("Unable to start read and lease tasks"); + z_close(z_move(s)); return; } @@ -114,7 +128,7 @@ void ZENOH::run() _zenoh_subscribers[i] = genSubscriber(type); if (_zenoh_subscribers[i] != 0) { - _zenoh_subscribers[i]->declare_subscriber(z_session_loan(&s), topic); + _zenoh_subscribers[i]->declare_subscriber(s, topic); } @@ -141,7 +155,7 @@ void ZENOH::run() _zenoh_publishers[i] = genPublisher(type); if (_zenoh_publishers[i] != 0) { - _zenoh_publishers[i]->declare_publisher(z_session_loan(&s), topic); + _zenoh_publishers[i]->declare_publisher(s, topic); _zenoh_publishers[i]->setPollFD(&pfds[i]); } } @@ -154,7 +168,7 @@ void ZENOH::run() if (_pub_count == 0) { // Nothing to publish but we don't want to stop this thread while (!should_exit()) { - sleep(2); + usleep(1000); } } @@ -194,8 +208,8 @@ void ZENOH::run() free(_zenoh_publishers); // Stop read and lease tasks for zenoh-pico - zp_stop_read_task(z_session_loan(&s)); - zp_stop_lease_task(z_session_loan(&s)); + zp_stop_read_task(z_session_loan_mut(&s)); + zp_stop_lease_task(z_session_loan_mut(&s)); z_close(z_session_move(&s)); exit_and_cleanup(); @@ -234,8 +248,8 @@ Zenoh demo bridge PX4_INFO_RAW(" addsubscriber Publish Zenoh topic to uORB\n"); PX4_INFO_RAW(" net Zenoh network mode\n"); PX4_INFO_RAW(" values: client|peer \n"); - PX4_INFO_RAW(" client: locator address for router\n"); - PX4_INFO_RAW(" peer: multicast address e.g. udp/224.0.0.225:7447#iface=eth0\n"); + PX4_INFO_RAW(" client: locator address e.g. tcp/10.41.10.1:7447#iface=eth0\n"); + PX4_INFO_RAW(" peer: multicast address e.g. udp/224.0.0.224:7446#iface=eth0\n"); return 0; } diff --git a/src/systemcmds/hardfault_log/CMakeLists.txt b/src/systemcmds/hardfault_log/CMakeLists.txt index fca7fc683929..8cd4ca9fea86 100644 --- a/src/systemcmds/hardfault_log/CMakeLists.txt +++ b/src/systemcmds/hardfault_log/CMakeLists.txt @@ -35,6 +35,7 @@ px4_add_module( MAIN hardfault_log COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable + STACK_MAIN 4096 SRCS hardfault_log.c DEPENDS diff --git a/src/systemcmds/hardfault_log/hardfault_log.c b/src/systemcmds/hardfault_log/hardfault_log.c index bad376b25edc..f8ff269ec6b8 100644 --- a/src/systemcmds/hardfault_log/hardfault_log.c +++ b/src/systemcmds/hardfault_log/hardfault_log.c @@ -1289,7 +1289,7 @@ static void print_usage(void) "Hardfault type: 0=divide by 0, 1=Assertion, 2=jump to 0x0, 3=write to 0x0 (default=0)", true); PRINT_MODULE_USAGE_COMMAND_DESCR("commit", - "Write uncommitted hardfault to " CONFIG_BOARD_ROOT_PATH "/fault_%i.txt (and rearm, but don't reset)"); + "Write uncommitted hardfault to /fs/microsd/fault_%i.txt (and rearm, but don't reset)"); PRINT_MODULE_USAGE_COMMAND_DESCR("count", "Read the reboot counter, counts the number of reboots of an uncommitted hardfault (returned as the exit code of the program)"); PRINT_MODULE_USAGE_COMMAND_DESCR("reset", "Reset the reboot counter"); diff --git a/src/systemcmds/param/CMakeLists.txt b/src/systemcmds/param/CMakeLists.txt index 0a27cf958949..056401daf75e 100644 --- a/src/systemcmds/param/CMakeLists.txt +++ b/src/systemcmds/param/CMakeLists.txt @@ -35,6 +35,7 @@ px4_add_module( MAIN param COMPILE_FLAGS -Wno-array-bounds + STACK_MAIN 4096 SRCS param.cpp DEPENDS