diff --git a/.github/workflows/dev_container.yml b/.github/workflows/dev_container.yml index f64d29b684..b3f9542ead 100644 --- a/.github/workflows/dev_container.yml +++ b/.github/workflows/dev_container.yml @@ -96,6 +96,6 @@ jobs: platforms: | linux/amd64 provenance: mode=max - push: true + push: ${{ github.event_name != 'pull_request' }} cache-from: type=s3,blobs_prefix=cache/${{ github.repository }}/,manifests_prefix=cache/${{ github.repository }}/,region=${{ env.RUNS_ON_AWS_REGION }},bucket=${{ env.RUNS_ON_S3_BUCKET_CACHE }} cache-to: type=s3,blobs_prefix=cache/${{ github.repository }}/,manifests_prefix=cache/${{ github.repository }}/,region=${{ env.RUNS_ON_AWS_REGION }},bucket=${{ env.RUNS_ON_S3_BUCKET_CACHE }},mode=max diff --git a/.github/workflows/flash_analysis.yml b/.github/workflows/flash_analysis.yml index 8ab97fdda8..da37dfa32d 100644 --- a/.github/workflows/flash_analysis.yml +++ b/.github/workflows/flash_analysis.yml @@ -1,5 +1,10 @@ name: FLASH usage analysis +permissions: + contents: read + pull-requests: write + issues: write + on: push: branches: @@ -83,59 +88,63 @@ jobs: echo '${{ steps.bloaty-step.outputs.bloaty-summary-map }}' >> $GITHUB_OUTPUT echo "$EOF" >> $GITHUB_OUTPUT - post_pr_comment: - name: Publish Results - runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",spot=false] - needs: [analyze_flash] - env: - V5X-SUMMARY-MAP-ABS: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v5x-bloaty-summary-map).vm-absolute) }} - V5X-SUMMARY-MAP-PERC: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v5x-bloaty-summary-map).vm-percentage) }} - V6X-SUMMARY-MAP-ABS: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v6x-bloaty-summary-map).vm-absolute) }} - V6X-SUMMARY-MAP-PERC: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v6x-bloaty-summary-map).vm-percentage) }} - if: ${{ github.event.pull_request }} - steps: - - name: Find Comment - uses: peter-evans/find-comment@v3 - id: fc - with: - issue-number: ${{ github.event.pull_request.number }} - comment-author: 'github-actions[bot]' - body-includes: FLASH Analysis + # TODO: + # This part of the workflow is causing errors, we should find a way to fix this and enable this test again + # Track this issue https://github.com/PX4/PX4-Autopilot/issues/24408 + # + #post_pr_comment: + #name: Publish Results + #runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",spot=false] + #needs: [analyze_flash] + #env: + #V5X-SUMMARY-MAP-ABS: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v5x-bloaty-summary-map).vm-absolute) }} + #V5X-SUMMARY-MAP-PERC: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v5x-bloaty-summary-map).vm-percentage) }} + #V6X-SUMMARY-MAP-ABS: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v6x-bloaty-summary-map).vm-absolute) }} + #V6X-SUMMARY-MAP-PERC: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v6x-bloaty-summary-map).vm-percentage) }} + #if: ${{ github.event.pull_request }} + #steps: + #- name: Find Comment + #uses: peter-evans/find-comment@v3 + #id: fc + #with: + #issue-number: ${{ github.event.pull_request.number }} + #comment-author: 'github-actions[bot]' + #body-includes: FLASH Analysis - - name: Set Build Time - id: bt - run: | - echo "timestamp=$(date +'%Y-%m-%dT%H:%M:%S')" >> $GITHUB_OUTPUT + #- name: Set Build Time + #id: bt + #run: | + #echo "timestamp=$(date +'%Y-%m-%dT%H:%M:%S')" >> $GITHUB_OUTPUT - - name: Create or update comment - # This can't be moved to the job-level conditions, as GH actions don't allow a job-level if condition to access the env. - if: | - steps.fc.outputs.comment-id != '' || - env.V5X-SUMMARY-MAP-ABS >= fromJSON(env.MIN_FLASH_POS_DIFF_FOR_COMMENT) || - env.V5X-SUMMARY-MAP-ABS <= fromJSON(env.MIN_FLASH_NEG_DIFF_FOR_COMMENT) || - env.V6X-SUMMARY-MAP-ABS >= fromJSON(env.MIN_FLASH_POS_DIFF_FOR_COMMENT) || - env.V6X-SUMMARY-MAP-ABS <= fromJSON(env.MIN_FLASH_NEG_DIFF_FOR_COMMENT) - uses: peter-evans/create-or-update-comment@v4 - with: - comment-id: ${{ steps.fc.outputs.comment-id }} - issue-number: ${{ github.event.pull_request.number }} - body: | - ## 🔎 FLASH Analysis -
- px4_fmu-v5x [Total VM Diff: ${{ env.V5X-SUMMARY-MAP-ABS }} byte (${{ env.V5X-SUMMARY-MAP-PERC}} %)] + #- name: Create or update comment + ## This can't be moved to the job-level conditions, as GH actions don't allow a job-level if condition to access the env. + #if: | + #steps.fc.outputs.comment-id != '' || + #env.V5X-SUMMARY-MAP-ABS >= fromJSON(env.MIN_FLASH_POS_DIFF_FOR_COMMENT) || + #env.V5X-SUMMARY-MAP-ABS <= fromJSON(env.MIN_FLASH_NEG_DIFF_FOR_COMMENT) || + #env.V6X-SUMMARY-MAP-ABS >= fromJSON(env.MIN_FLASH_POS_DIFF_FOR_COMMENT) || + #env.V6X-SUMMARY-MAP-ABS <= fromJSON(env.MIN_FLASH_NEG_DIFF_FOR_COMMENT) + #uses: peter-evans/create-or-update-comment@v4 + #with: + #comment-id: ${{ steps.fc.outputs.comment-id }} + #issue-number: ${{ github.event.pull_request.number }} + #body: | + ### 🔎 FLASH Analysis + #
+ #px4_fmu-v5x [Total VM Diff: ${{ env.V5X-SUMMARY-MAP-ABS }} byte (${{ env.V5X-SUMMARY-MAP-PERC}} %)] - ``` - ${{ needs.analyze_flash.outputs.px4_fmu-v5x-bloaty-output }} - ``` -
+ #``` + #${{ needs.analyze_flash.outputs.px4_fmu-v5x-bloaty-output }} + #``` + #
-
- px4_fmu-v6x [Total VM Diff: ${{ env.V6X-SUMMARY-MAP-ABS }} byte (${{ env.V6X-SUMMARY-MAP-PERC }} %)] + #
+ #px4_fmu-v6x [Total VM Diff: ${{ env.V6X-SUMMARY-MAP-ABS }} byte (${{ env.V6X-SUMMARY-MAP-PERC }} %)] - ``` - ${{ needs.analyze_flash.outputs.px4_fmu-v6x-bloaty-output }} - ``` -
+ #``` + #${{ needs.analyze_flash.outputs.px4_fmu-v6x-bloaty-output }} + #``` + #
- **Updated: _${{ steps.bt.outputs.timestamp }}_** - edit-mode: replace + #**Updated: _${{ steps.bt.outputs.timestamp }}_** + #edit-mode: replace diff --git a/CMakeLists.txt b/CMakeLists.txt index 55362b1425..75fc8afb67 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -99,7 +99,7 @@ # #============================================================================= -cmake_minimum_required(VERSION 3.9 FATAL_ERROR) +cmake_minimum_required(VERSION 3.10 FATAL_ERROR) set(PX4_SOURCE_DIR "${CMAKE_CURRENT_SOURCE_DIR}" CACHE FILEPATH "PX4 source directory" FORCE) set(PX4_BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}" CACHE FILEPATH "PX4 binary directory" FORCE) diff --git a/Jenkinsfile b/Jenkinsfile index 9e9f5787c8..fb092cd83d 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -235,9 +235,9 @@ pipeline { sh('rm -f px4_msgs/srv/*.srv') sh('rm -f px4_msgs/srv/versioned/*.srv') sh('cp msg/*.msg px4_msgs/msg/') - sh('mkdir -p px4_msgs/msg/versioned && cp msg/versioned/*.msg px4_msgs/msg/versioned/') + sh('cp msg/versioned/*.msg px4_msgs/msg/ || true') sh('cp srv/*.srv px4_msgs/srv/') - sh('mkdir -p px4_msgs/srv/versioned && cp srv/versioned/*.srv px4_msgs/srv/versioned/') + sh('cp srv/versioned/*.srv px4_msgs/srv/ || true') sh('cd px4_msgs; git status; git add .; git commit -a -m "Update message definitions `date`" || true') sh('cd px4_msgs; git push origin main || true') sh('rm -rf px4_msgs') diff --git a/Kconfig b/Kconfig index f9f2e5e8ec..20ee4a2266 100644 --- a/Kconfig +++ b/Kconfig @@ -73,13 +73,6 @@ menu "Toolchain" help relative path to the ROMFS root directory - config BOARD_ROOTFSDIR - string "Root directory" - depends on PLATFORM_POSIX - default "." - help - Configure the root directory in the file system for PX4 files - config BOARD_IO string "IO board name" default "px4_io-v2_default" diff --git a/MAINTAINERS.md b/MAINTAINERS.md new file mode 100644 index 0000000000..426852e7b2 --- /dev/null +++ b/MAINTAINERS.md @@ -0,0 +1,42 @@ +Maintainers +=========== + +See [the documentation on Maintainers](https://docs.px4.io/main/en/contribute/maintainers.html) to learn about the role of the maintainers and the process to become one. + +**Active Maintainers** + +| Name | Sector | GitHub | Chat | email +|-------------------------|--------|--------|------|---------------- +| Lorenz Meier | Founder | [LorenzMeier][LorenzMeier] | | +| Daniel Agar | Architecture | [dagar][dagar] | daniel_agar | +| Beat Küng | Architecture | [bkueng][bkueng] | beatkueng | +| Ramón Roche | CI / Testing | [mrpollo][mrpollo] | rroche | +| Mathieu Bresciani | State Estimation | [bresch][bresch] | mbresch | +| Paul Riseborough | State Estimation | [priseborough][priseborough] | | +| David Sidrane | RTOS / NuttX | [davids5][davids5] | david_s5 | +| Jayoung Lim | Simulation | [Jaeyoung-Lim][Jaeyoung-Lim] | jaeyounglim. | +| Beniamino Pozzan | ROS 2 | [beniaminopozzan][beniaminopozzan] | beniaminopozzan | +| Matthias Grob | Multirotor | [MaEtUgR][MaEtUgR] | maetugr | +| Silvan Fuhrer | Fixed-Wing / VTOL | [sfuhrer][sfuhrer] | sfuhrer | +| Christian Friedrich | Rover | [chfriedrich98][chfriedrich98] | christian982564 | +| Pedro Roque | Spacecraft | [Pedro-Roque][Pedro-Roque] | .pedroroque | + + +**Documentation Maintainers** + +| Name | GitHub | Chat | email +|------|--------|------|---------------------- +| Hamish Willee | [hamishwillee][hamishwillee] | hamishwillee | + +**Release Managers** + +| Name | GitHub | Chat | email +|------|--------|------|---------------------- +| Ramón Roche | [mrpollo][mrpollo] | rroche | +| Daniel Agar | [dagar][dagar] | daniel_agar | + +**Retired Maintainers** + +| Name | GitHub | Chat | email +|------|--------|------|---------------------- +| | | | diff --git a/Makefile b/Makefile index f549e4c8ce..cfd87b6e23 100644 --- a/Makefile +++ b/Makefile @@ -404,7 +404,7 @@ check_newlines: # Testing # -------------------------------------------------------------------- -.PHONY: tests tests_coverage tests_mission tests_mission_coverage tests_offboard tests_avoidance +.PHONY: tests tests_coverage tests_mission tests_mission_coverage tests_offboard .PHONY: rostest python_coverage tests: @@ -457,10 +457,6 @@ tests_offboard: rostest @"$(SRC_DIR)"/test/rostest_px4_run.sh mavros_posix_tests_offboard_posctl.test @"$(SRC_DIR)"/test/rostest_px4_run.sh mavros_posix_tests_offboard_rpyrt_ctl.test -tests_avoidance: rostest - @"$(SRC_DIR)"/test/rostest_avoidance_run.sh mavros_posix_test_avoidance.test - @"$(SRC_DIR)"/test/rostest_avoidance_run.sh mavros_posix_test_safe_landing.test - python_coverage: @mkdir -p "$(SRC_DIR)"/build/python_coverage @cd "$(SRC_DIR)"/build/python_coverage && cmake "$(SRC_DIR)" $(CMAKE_ARGS) -G"$(PX4_CMAKE_GENERATOR)" -DCONFIG=px4_sitl_default -DPYTHON_COVERAGE=ON diff --git a/README.md b/README.md index aef9e8f8a5..8353f3add6 100644 --- a/README.md +++ b/README.md @@ -17,17 +17,19 @@ PX4 is highly portable, OS-independent and supports Linux, NuttX and MacOS out o * [VTOL](https://docs.px4.io/main/en/frames_vtol/) * [Autogyro](https://docs.px4.io/main/en/frames_autogyro/) * [Rover](https://docs.px4.io/main/en/frames_rover/) - * many more experimental types (Blimps, Boats, Submarines, High altitude balloons, etc) + * many more experimental types (Blimps, Boats, Submarines, High Altitude Balloons, Spacecraft, etc) * Releases: [Downloads](https://github.com/PX4/PX4-Autopilot/releases) +## Releases + +Release notes and supporting information for PX4 releases can be found on the [Developer Guide](https://docs.px4.io/main/en/releases/). ## Building a PX4 based drone, rover, boat or robot -The [PX4 User Guide](https://docs.px4.io/main/en/) explains how to assemble [supported vehicles](https://docs.px4.io/main/en/airframes/airframe_reference.html) and fly drones with PX4. -See the [forum and chat](https://docs.px4.io/main/en/#getting-help) if you need help! +The [PX4 User Guide](https://docs.px4.io/main/en/) explains how to assemble [supported vehicles](https://docs.px4.io/main/en/airframes/airframe_reference.html) and fly drones with PX4. See the [forum and chat](https://docs.px4.io/main/en/#getting-help) if you need help! -## Changing code and contributing +## Changing Code and Contributing This [Developer Guide](https://docs.px4.io/main/en/development/development.html) is for software developers who want to modify the flight stack and middleware (e.g. to add new flight modes), hardware integrators who want to support new flight controller boards and peripherals, and anyone who wants to get PX4 working on a new (unsupported) airframe/vehicle. @@ -35,7 +37,7 @@ Developers should read the [Guide for Contributions](https://docs.px4.io/main/en See the [forum and chat](https://docs.px4.io/main/en/#getting-help) if you need help! -### Weekly Dev Call +## Weekly Dev Call The PX4 Dev Team syncs up on a [weekly dev call](https://docs.px4.io/main/en/contribute/). @@ -44,96 +46,17 @@ The PX4 Dev Team syncs up on a [weekly dev call](https://docs.px4.io/main/en/con ## Maintenance Team -Note: This is the source of truth for the active maintainers of PX4 ecosystem. +See the latest list of maintainers on [MAINTAINERS](MAINTAINERS.md) file at the root of the project. -| Sector | Maintainer | -|---|---| -| Founder | [Lorenz Meier](https://github.com/LorenzMeier) | -| Architecture | [Daniel Agar](https://github.com/dagar) / [Beat Küng](https://github.com/bkueng)| -| State Estimation | [Mathieu Bresciani](https://github.com/bresch) / [Paul Riseborough](https://github.com/priseborough) | -| OS/NuttX | [David Sidrane](https://github.com/davids5) | -| Drivers | [Daniel Agar](https://github.com/dagar) | -| Simulation | [Jaeyoung Lim](https://github.com/Jaeyoung-Lim) | -| ROS2 | [Beniamino Pozzan](https://github.com/beniaminopozzan) | -| Community QnA Call | [Ramon Roche](https://github.com/mrpollo) | -| [Documentation](https://docs.px4.io/main/en/) | [Hamish Willee](https://github.com/hamishwillee) | - -| Vehicle Type | Maintainer | -|---|---| -| Multirotor | [Matthias Grob](https://github.com/MaEtUgR) | -| Fixed Wing | [Thomas Stastny](https://github.com/tstastny) | -| Hybrid VTOL | [Silvan Fuhrer](https://github.com/sfuhrer) | -| Rover | [Christian Friedrich](https://github.com/chfriedrich98) | -| Boat | x | - - -See also [maintainers list](https://px4.io/community/maintainers/) (px4.io) and the [contributors list](https://github.com/PX4/PX4-Autopilot/graphs/contributors) (Github). However it may be not up to date. +For the latest stats on contributors please see the latest stats for the Dronecode ecosystem in our project dashboard under [LFX Insights](https://insights.lfx.linuxfoundation.org/foundation/dronecode). For information on how to update your profile and affiliations please see the following support link on how to [Complete Your LFX Profile](https://docs.linuxfoundation.org/lfx/my-profile/complete-your-lfx-profile). Dronecode publishes a yearly snapshot of contributions and achievements on its [website under the Reports section](https://dronecode.org). ## Supported Hardware -Pixhawk standard boards and proprietary boards are shown below (discontinued boards aren't listed). - -For the most up to date information, please visit [PX4 user Guide > Autopilot Hardware](https://docs.px4.io/main/en/flight_controller/). - -### Pixhawk Standard Boards - -These boards fully comply with Pixhawk Standard, and are maintained by the PX4-Autopilot maintainers and Dronecode team - -* FMUv6X and FMUv6C - * [CUAV Pixahwk V6X (FMUv6X)](https://docs.px4.io/main/en/flight_controller/cuav_pixhawk_v6x.html) - * [Holybro Pixhawk 6X (FMUv6X)](https://docs.px4.io/main/en/flight_controller/pixhawk6x.html) - * [Holybro Pixhawk 6C (FMUv6C)](https://docs.px4.io/main/en/flight_controller/pixhawk6c.html) - * [Holybro Pix32 v6 (FMUv6C)](https://docs.px4.io/main/en/flight_controller/holybro_pix32_v6.html) -* FMUv5 and FMUv5X (STM32F7, 2019/20) - * [Pixhawk 4 (FMUv5)](https://docs.px4.io/main/en/flight_controller/pixhawk4.html) - * [Pixhawk 4 mini (FMUv5)](https://docs.px4.io/main/en/flight_controller/pixhawk4_mini.html) - * [CUAV V5+ (FMUv5)](https://docs.px4.io/main/en/flight_controller/cuav_v5_plus.html) - * [CUAV V5 nano (FMUv5)](https://docs.px4.io/main/en/flight_controller/cuav_v5_nano.html) - * [Auterion Skynode (FMUv5X)](https://docs.auterion.com/avionics/skynode) -* FMUv4 (STM32F4, 2015) - * [Pixracer](https://docs.px4.io/main/en/flight_controller/pixracer.html) - * [Pixhawk 3 Pro](https://docs.px4.io/main/en/flight_controller/pixhawk3_pro.html) -* FMUv3 (STM32F4, 2014) - * [Pixhawk 2](https://docs.px4.io/main/en/flight_controller/pixhawk-2.html) - * [Pixhawk Mini](https://docs.px4.io/main/en/flight_controller/pixhawk_mini.html) - * [CUAV Pixhack v3](https://docs.px4.io/main/en/flight_controller/pixhack_v3.html) -* FMUv2 (STM32F4, 2013) - * [Pixhawk](https://docs.px4.io/main/en/flight_controller/pixhawk.html) - -### Manufacturer supported - -These boards are maintained to be compatible with PX4-Autopilot by the Manufacturers. - -* [ARK Electronics ARKV6X](https://docs.px4.io/main/en/flight_controller/ark_v6x.html) -* [CubePilot Cube Orange+](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_orangeplus.html) -* [CubePilot Cube Orange](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_orange.html) -* [CubePilot Cube Yellow](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_yellow.html) -* [Holybro Durandal](https://docs.px4.io/main/en/flight_controller/durandal.html) -* [Airmind MindPX V2.8](http://www.mindpx.net/assets/accessories/UserGuide_MindPX.pdf) -* [Airmind MindRacer V1.2](http://mindpx.net/assets/accessories/mindracer_user_guide_v1.2.pdf) -* [Holybro Kakute F7](https://docs.px4.io/main/en/flight_controller/kakutef7.html) - -### Community supported - -These boards don't fully comply industry standards, and thus is solely maintained by the PX4 public community members. - -### Experimental - -These boards are not maintained by PX4 team nor Manufacturer, and is not guaranteed to be compatible with up to date PX4 releases. - -* [Raspberry PI with Navio 2](https://docs.px4.io/main/en/flight_controller/raspberry_pi_navio2.html) -* [Bitcraze Crazyflie 2.0](https://docs.px4.io/main/en/complete_vehicles/crazyflie2.html) - -## Project Roadmap - -**Note: Outdated** - -A high level project roadmap is available [here](https://github.com/orgs/PX4/projects/25). +For the most up to date information, please visit [PX4 User Guide > Autopilot Hardware](https://docs.px4.io/main/en/flight_controller/). ## Project Governance The PX4 Autopilot project including all of its trademarks is hosted under [Dronecode](https://www.dronecode.org/), part of the Linux Foundation. -Dronecode Logo -Linux Foundation Logo +Dronecode Logo
 
diff --git a/src/modules/rover_mecanum/RoverMecanumGuidance/CMakeLists.txt b/ROMFS/performance-test/CMakeLists.txt similarity index 88% rename from src/modules/rover_mecanum/RoverMecanumGuidance/CMakeLists.txt rename to ROMFS/performance-test/CMakeLists.txt index a15f489245..56c6235ee2 100644 --- a/src/modules/rover_mecanum/RoverMecanumGuidance/CMakeLists.txt +++ b/ROMFS/performance-test/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2024 PX4 Development Team. All rights reserved. +# Copyright (c) 2025 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,8 +31,4 @@ # ############################################################################ -px4_add_library(RoverMecanumGuidance - RoverMecanumGuidance.cpp -) - -target_include_directories(RoverMecanumGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +add_subdirectory(init.d) diff --git a/src/lib/bezier/CMakeLists.txt b/ROMFS/performance-test/init.d/CMakeLists.txt similarity index 90% rename from src/lib/bezier/CMakeLists.txt rename to ROMFS/performance-test/init.d/CMakeLists.txt index f069fff591..8a08b20ab2 100644 --- a/src/lib/bezier/CMakeLists.txt +++ b/ROMFS/performance-test/init.d/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2018 PX4 Development Team. All rights reserved. +# Copyright (c) 2025 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,6 @@ # ############################################################################ -px4_add_library(bezier - BezierQuad.cpp - BezierN.cpp +px4_add_romfs_files( + rcS ) - -px4_add_unit_gtest(SRC BezierNTest.cpp LINKLIBS bezier) diff --git a/ROMFS/performance-test/init.d/rcS b/ROMFS/performance-test/init.d/rcS new file mode 100644 index 0000000000..d79fe58c76 --- /dev/null +++ b/ROMFS/performance-test/init.d/rcS @@ -0,0 +1,68 @@ +#!/bin/sh +# Un comment and use set +e to ignore and set -e to enable 'exit on error control' +set +e +# Un comment the line below to help debug scripts by printing a trace of the script commands +#set -x +# PX4FMU startup script. +# +# NOTE: environment variable references: +# If the dollar sign ('$') is followed by a left bracket ('{') then the +# variable name is terminated with the right bracket character ('}'). +# Otherwise, the variable name goes to the end of the argument. +# +# +# NOTE: COMMENT LINES ARE REMOVED BEFORE STORED IN ROMFS. +# +#------------------------------------------------------------------------------ +set R / + +# +# Print full system version. +# +ver all + +# +# Set the parameter file the board supports params on +# MTD device. +# +if mft query -q -k MTD -s MTD_PARAMETERS -v /fs/mtd_params +then + set PARAM_FILE /fs/mtd_params +fi + +# +# Load parameters. +# +# if the board has a storage for (factory) calibration data +if mft query -q -k MTD -s MTD_CALDATA -v /fs/mtd_caldata +then + param load /fs/mtd_caldata +fi + +# +# Load parameters. +# +param select $PARAM_FILE +if ! param load +then + param reset_all +fi + +# +# Try to mount the microSD card. +# +mount -t vfat /dev/mmcsd0 /fs/microsd +if [ $? = 0 ] +then + echo "SD card mounted at /fs/microsd" +else + echo "No SD card found" +fi + +unset R + +echo "" +echo "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" +echo "!!!!!! This is the PERFORMANCE TESTING firmware! WARNs and ERRORs are expected! !!!!!" +echo "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" +echo "" diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid b/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid deleted file mode 100644 index 8221969873..0000000000 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid +++ /dev/null @@ -1,32 +0,0 @@ -#!/bin/sh -# -# @name 3DR Iris Quadrotor SITL (Obstacle Avoidance) -# -# @type Quadrotor Wide -# - -. ${R}etc/init.d/rc.mc_defaults - - -param set-default CA_AIRFRAME 0 - -param set-default CA_ROTOR_COUNT 4 -param set-default CA_ROTOR0_PX 0.1515 -param set-default CA_ROTOR0_PY 0.245 -param set-default CA_ROTOR0_KM 0.05 -param set-default CA_ROTOR1_PX -0.1515 -param set-default CA_ROTOR1_PY -0.1875 -param set-default CA_ROTOR1_KM 0.05 -param set-default CA_ROTOR2_PX 0.1515 -param set-default CA_ROTOR2_PY -0.245 -param set-default CA_ROTOR2_KM -0.05 -param set-default CA_ROTOR3_PX -0.1515 -param set-default CA_ROTOR3_PY 0.1875 -param set-default CA_ROTOR3_KM -0.05 - -param set-default PWM_MAIN_FUNC1 101 -param set-default PWM_MAIN_FUNC2 102 -param set-default PWM_MAIN_FUNC3 103 -param set-default PWM_MAIN_FUNC4 104 - -param set-default COM_OBS_AVOID 1 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid.post b/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid.post deleted file mode 100644 index 64b8ce8b19..0000000000 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid.post +++ /dev/null @@ -1,2 +0,0 @@ -# shellcheck disable=SC2154 -mavlink start -x -u 14558 -r 4000000 -m onboard -o 14541 -p # add mavlink stream for SDK diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 b/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 index bca3feddfb..2add7a5cb4 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 @@ -13,10 +13,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500} param set-default SIM_GZ_EN 1 -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 0 -param set-default SENS_EN_MAGSIM 1 - param set-default CA_AIRFRAME 0 param set-default CA_ROTOR_COUNT 4 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna b/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna index 04fc1acfd8..96dd5fa151 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna @@ -12,13 +12,8 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=rc_cessna} param set-default SIM_GZ_EN 1 -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 FW_LND_ANG 8 param set-default NPFG_PERIOD 12 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol b/ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol index 96bb25d69a..5bd432abde 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol @@ -13,9 +13,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=standard_vtol} param set-default SIM_GZ_EN 1 -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 # TODO: Enable motor failure detection when the diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision b/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision index 65d290e0cc..4370a677ed 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision @@ -14,12 +14,7 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=px4vision} param set-default SIM_GZ_EN 1 -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 0 -param set-default SENS_EN_MAGSIM 1 - # Commander Parameters -param set-default COM_OBS_AVOID 0 param set-default COM_DISARM_LAND 0.5 # EKF2 parameters diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4008_gz_advanced_plane b/ROMFS/px4fmu_common/init.d-posix/airframes/4008_gz_advanced_plane index 5d5c50df33..8ac407bac4 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4008_gz_advanced_plane +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4008_gz_advanced_plane @@ -11,8 +11,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=advanced_plane} param set-default SIM_GZ_EN 1 -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_MAGSIM 1 param set-default SENS_EN_ARSPDSIM 1 param set-default FW_LND_ANG 8 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 3f96dad5f8..2d36aed7ce 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 @@ -11,35 +11,39 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=r1_rover} param set-default SIM_GZ_EN 1 # Gazebo bridge -# Rover parameters +param set-default NAV_ACC_RAD 0.5 + +# Differential Parameters param set-default RD_WHEEL_TRACK 0.3 -param set-default RD_MAX_ACCEL 5 -param set-default RD_MAX_DECEL 10 -param set-default RD_MAX_JERK 30 param set-default RD_MAX_THR_YAW_R 1.5 -param set-default RD_YAW_RATE_P 0.25 -param set-default RD_YAW_RATE_I 0.01 -param set-default RD_YAW_P 5 -param set-default RD_YAW_I 0.1 -param set-default RD_MAX_SPEED 2 -param set-default RD_MAX_THR_SPD 2.15 -param set-default RD_SPEED_P 0.1 -param set-default RD_SPEED_I 0.01 -param set-default RD_MAX_YAW_RATE 180 param set-default RD_TRANS_DRV_TRN 0.349066 param set-default RD_TRANS_TRN_DRV 0.174533 -param set-default RD_MAX_YAW_ACCEL 1000 -# Pure pursuit parameters -param set-default PP_LOOKAHD_MAX 30 -param set-default PP_LOOKAHD_MIN 2 +# Rover Control Parameters +param set-default RO_ACCEL_LIM 5 +param set-default RO_DECEL_LIM 10 +param set-default RO_JERK_LIM 30 +param set-default RO_MAX_THR_SPEED 2.1 + +# Rover Rate Control Parameters +param set-default RO_YAW_RATE_I 0.01 +param set-default RO_YAW_RATE_P 0.25 +param set-default RO_YAW_RATE_LIM 180 +param set-default RO_YAW_ACCEL_LIM 120 +param set-default RO_YAW_DECEL_LIM 1000 + +# Rover Attitude Control Parameters +param set-default RO_YAW_P 5 + +# Rover Position Control Parameters +param set-default RO_SPEED_LIM 2 +param set-default RO_SPEED_I 0.5 +param set-default RO_SPEED_P 1 + +# Pure Pursuit parameters 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 0 +param set-default PP_LOOKAHD_MAX 10 +param set-default PP_LOOKAHD_MIN 1 # 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 0fec76b5c4..83f387b56e 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower @@ -11,11 +11,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=lawnmower} param set-default SIM_GZ_EN 1 # Gazebo bridge -# 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 0 # We can arm and drive in manual mode when it slides and GPS check fails: param set-default COM_ARM_WO_GPS 1 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann b/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann index cd870caabb..9c24991fcf 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann @@ -44,12 +44,6 @@ param set-default PP_LOOKAHD_GAIN 1 param set-default PP_LOOKAHD_MAX 10 param set-default PP_LOOKAHD_MIN 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 0 - # Wheels param set-default SIM_GZ_WH_FUNC1 101 param set-default SIM_GZ_WH_MIN1 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum b/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum index e252b3cfa0..45f80383e8 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum @@ -11,34 +11,41 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=r1_rover_mecanum} param set-default SIM_GZ_EN 1 # Gazebo bridge -# Rover parameters -param set-default RM_WHEEL_TRACK 0.3 -param set-default RM_YAW_RATE_I 0.1 -param set-default RM_YAW_RATE_P 0.1 -param set-default RM_MAX_ACCEL 3 -param set-default RM_MAX_DECEL 5 -param set-default RM_MAX_JERK 5 -param set-default RM_MAX_SPEED 2 -param set-default RM_MAX_THR_SPD 2.2 -param set-default RM_MAX_THR_YAW_R 1.2 -param set-default RM_YAW_P 5 -param set-default RM_YAW_I 0.1 -param set-default RM_MAX_YAW_RATE 120 -param set-default RM_MAX_YAW_ACCEL 240 -param set-default RM_MISS_VEL_GAIN 1 -param set-default RM_SPEED_I 0.01 -param set-default RM_SPEED_P 0.1 +param set-default NAV_ACC_RAD 0.5 -# Pure pursuit parameters +# Mecanum Parameters +param set-default RM_WHEEL_TRACK 0.3 +param set-default RM_MAX_THR_YAW_R 1.2 +param set-default RM_MISS_SPD_GAIN 1 + +# Rover Control Parameters +param set-default RO_ACCEL_LIM 3 +param set-default RO_DECEL_LIM 5 +param set-default RO_JERK_LIM 30 +param set-default RO_MAX_THR_SPEED 2.1 + +# Rover Rate Control Parameters +param set-default RO_YAW_RATE_I 0.1 +param set-default RO_YAW_RATE_P 0.1 +param set-default RO_YAW_RATE_LIM 120 +param set-default RO_YAW_ACCEL_LIM 240 +param set-default RO_YAW_DECEL_LIM 1000 + +# Rover Attitude Control Parameters +param set-default RO_YAW_P 5 + +# Rover Position Control Parameters +param set-default RO_SPEED_LIM 2 +param set-default RO_SPEED_I 0.5 +param set-default RO_SPEED_P 1 + +# Pure Pursuit parameters +param set-default PP_LOOKAHD_GAIN 0.5 param set-default PP_LOOKAHD_MAX 10 param set-default PP_LOOKAHD_MIN 1 -param set-default PP_LOOKAHD_GAIN 0.5 # 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 0 # Actuator mapping param set-default SIM_GZ_WH_FUNC1 102 # right wheel front diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4018_gz_quadtailsitter b/ROMFS/px4fmu_common/init.d-posix/airframes/4018_gz_quadtailsitter index 9856b1bf27..bced062c1e 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4018_gz_quadtailsitter +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4018_gz_quadtailsitter @@ -13,12 +13,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=quadtailsitter} 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 0 - - param set-default MAV_TYPE 20 param set-default CA_AIRFRAME 4 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor b/ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor index c798650b54..ff7805cb77 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor @@ -13,11 +13,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=tiltrotor} 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 0 - param set-default MAV_TYPE 21 param set-default CA_AIRFRAME 3 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4021_gz_x500_flow b/ROMFS/px4fmu_common/init.d-posix/airframes/4021_gz_x500_flow new file mode 100644 index 0000000000..5f0aababbd --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4021_gz_x500_flow @@ -0,0 +1,15 @@ +#!/bin/sh +# +# @name Gazebo x500 with downward optical flow and distance sensor +# +# @type Quadrotor +# + +PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500_flow} + +. ${R}etc/init.d-posix/airframes/4001_gz_x500 + +echo "Disabling Sim GPS" +param set-default SYS_HAS_GPS 0 +param set-default SIM_GPS_USED 0 +param set-default EKF2_GPS_CTRL 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d b/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d index 480454d72b..4cc0a02e41 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d @@ -15,8 +15,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=spacecraft_2d} param set-default SIM_GZ_EN 1 -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 1 param set-default SENS_EN_MAGSIM 1 param set-default COM_ARM_CHK_ESCS 0 # We don't have ESCs param set-default FD_ESCS_EN 0 # We don't have ESCs - but maybe we need this later? diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/8011_gz_omnicopter b/ROMFS/px4fmu_common/init.d-posix/airframes/8011_gz_omnicopter index b43b61025c..be7a32431b 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/8011_gz_omnicopter +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/8011_gz_omnicopter @@ -83,9 +83,6 @@ param set-default CA_ROTOR7_AY -0.211325 param set-default CA_ROTOR7_AZ -0.57735 param set-default SIM_GZ_EN 1 -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 0 -param set-default SENS_EN_MAGSIM 1 param set-default SIM_GZ_EC_FUNC1 101 param set-default SIM_GZ_EC_FUNC2 102 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index 94f2f4d39a..b4e107799f 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -39,8 +39,6 @@ px4_add_romfs_files( 1012_gazebo-classic_iris_rplidar 1013_gazebo-classic_iris_vision 1013_gazebo-classic_iris_vision.post - 1014_gazebo-classic_iris_obs_avoid - 1014_gazebo-classic_iris_obs_avoid.post 1015_gazebo-classic_iris_depth_camera 1016_gazebo-classic_iris_downward_depth_camera 1017_gazebo-classic_iris_opt_flow_mockup @@ -93,6 +91,7 @@ px4_add_romfs_files( 4018_gz_quadtailsitter 4019_gz_x500_gimbal 4020_gz_tiltrotor + 4021_gz_x500_flow 6011_gazebo-classic_typhoon_h480 6011_gazebo-classic_typhoon_h480.post diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator index fd0954744e..b396115491 100644 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator @@ -73,13 +73,13 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then exit 1 fi - # look for running ${gz_command} gazebo world + # Look for an already running world gz_world=$( ${gz_command} topic -l | grep -m 1 -e "^/world/.*/clock" | sed 's/\/world\///g; s/\/clock//g' ) # shellcheck disable=SC2153 if [ -z "${gz_world}" ] && [ -n "${PX4_GZ_WORLD}" ]; then - # source generated gz_env.sh for GZ_SIM_RESOURCE_PATH + # Setup gz environment variables if [ -f ./gz_env.sh ]; then . ./gz_env.sh @@ -87,62 +87,125 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then . ../gz_env.sh fi - echo "INFO [init] starting gazebo with world: ${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" + echo "INFO [init] Starting gazebo with world: ${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" - ${gz_command} ${gz_sub_command} --verbose=1 -r -s "${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" & + ${gz_command} ${gz_sub_command} --verbose=${GZ_VERBOSE:=1} -r -s "${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" & if [ -z "${HEADLESS}" ]; then - # HEADLESS not set, starting gui - ${gz_command} ${gz_sub_command} -g & + echo "INFO [init] Starting gz gui" + ${gz_command} ${gz_sub_command} -g > /dev/null 2>&1 & fi - else - # Gazebo is already running, do not start the simulator, nor the GUI + # Gazebo is already running echo "INFO [init] gazebo already running world: ${gz_world}" PX4_GZ_WORLD=${gz_world} fi + else echo "INFO [init] Standalone PX4 launch, waiting for Gazebo" fi - # start gz_bridge + # Start gz_bridge - either spawn a model or connect to existing one if [ -n "${PX4_SIM_MODEL#*gz_}" ] && [ -z "${PX4_GZ_MODEL_NAME}" ]; then - # model specified, gz_bridge will spawn model + # Spawn a model + MODEL_NAME="${PX4_SIM_MODEL#*gz_}" + MODEL_NAME_INSTANCE="${MODEL_NAME}_${px4_instance}" + POSE_ARG="" if [ -n "${PX4_GZ_MODEL_POSE}" ]; then - # model pose provided: [x, y, z, roll, pitch, yaw] + pos_x=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $1}') + pos_y=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $2}') + pos_z=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $3}') + pos_x=${pos_x:-0} + pos_y=${pos_y:-0} + pos_z=${pos_z:-0} - # Clean potential input line formatting. - model_pose="$( echo "${PX4_GZ_MODEL_POSE}" | sed -e 's/^[ \t]*//; s/[ \t]*$//; s/,/ /g; s/ / /g; s/ /,/g' )" - echo "INFO [init] PX4_GZ_MODEL_POSE set, spawning at: ${model_pose}" - - else - # model pose not provided, origin will be used - - echo "WARN [init] PX4_GZ_MODEL_POSE not set, spawning at origin." - model_pose="0,0,0,0,0,0" + POSE_ARG=", pose: { position: { x: ${pos_x}, y: ${pos_y}, z: ${pos_z} } }" + echo "INFO [init] Spawning model at position: ${pos_x} ${pos_y} ${pos_z}" fi - # start gz bridge with pose arg. - if ! gz_bridge start -p "${model_pose}" -m "${PX4_SIM_MODEL#*gz_}" -w "${PX4_GZ_WORLD}" -i "${px4_instance}"; then + echo "INFO [init] Spawning model" + # Spawn model + ${gz_command} service -s "/world/${PX4_GZ_WORLD}/create" --reqtype gz.msgs.EntityFactory \ + --reptype gz.msgs.Boolean --timeout 5000 \ + --req "sdf_filename: \"${PX4_GZ_MODELS}/${MODEL_NAME}\", name: \"${MODEL_NAME_INSTANCE}\", allow_renaming: false${POSE_ARG}" > /dev/null 2>&1 + + # Start gz_bridge + if ! gz_bridge start -w "${PX4_GZ_WORLD}" -n "${MODEL_NAME_INSTANCE}"; then echo "ERROR [init] gz_bridge failed to start and spawn model" exit 1 fi - elif [ -n "${PX4_GZ_MODEL_NAME}" ]; then - # model name specificed, gz_bridge will attach to existing model + # Set physics parameters for faster-than-realtime simulation if needed + check_scene_info() { + SERVICE_INFO=$(${gz_command} service -i --service "/world/${PX4_GZ_WORLD}/scene/info" 2>&1) + if echo "$SERVICE_INFO" | grep -q "Service providers"; then + return 0 + else + return 1 + fi + } + ATTEMPTS=30 + while [ $ATTEMPTS -gt 0 ]; do + if check_scene_info; then + echo "INFO [init] Gazebo world is ready" + break + fi + ATTEMPTS=$((ATTEMPTS-1)) + if [ $ATTEMPTS -eq 0 ]; then + echo "ERROR [init] Timed out waiting for Gazebo world" + exit 1 + fi + echo "INFO [init] Waiting for Gazebo world..." + sleep 1 + done + + if [ -n "${PX4_SIM_SPEED_FACTOR}" ]; then + echo "INFO [init] Setting simulation speed factor: ${PX4_SIM_SPEED_FACTOR}" + ${gz_command} service -s "/world/${PX4_GZ_WORLD}/set_physics" --reqtype gz.msgs.Physics \ + --reptype gz.msgs.Boolean --timeout 5000 \ + --req "real_time_factor: ${PX4_SIM_SPEED_FACTOR}" > /dev/null 2>&1 + fi + + + # Set up camera to follow the model if requested + if [ -n "${PX4_GZ_FOLLOW}" ]; then + + # Wait for model to spawn + sleep 1 + + echo "INFO [init] Setting camera to follow ${MODEL_NAME_INSTANCE}" + + # Set camera to follow the model + ${gz_command} service -s "/gui/follow" --reqtype gz.msgs.StringMsg \ + --reptype gz.msgs.Boolean --timeout 5000 \ + --req "data: \"${MODEL_NAME_INSTANCE}\"" > /dev/null 2>&1 + + # Set default camera offset if not specified + follow_x=${PX4_GZ_FOLLOW_OFFSET_X:--2.0} + follow_y=${PX4_GZ_FOLLOW_OFFSET_Y:--2.0} + follow_z=${PX4_GZ_FOLLOW_OFFSET_Z:-2.0} + + # Set camera offset + ${gz_command} service -s "/gui/follow/offset" --reqtype gz.msgs.Vector3d \ + --reptype gz.msgs.Boolean --timeout 5000 \ + --req "x: ${follow_x}, y: ${follow_y}, z: ${follow_z}" > /dev/null 2>&1 + + echo "INFO [init] Camera follow offset set to ${follow_x}, ${follow_y}, ${follow_z}" + fi + + elif [ -n "${PX4_GZ_MODEL_NAME}" ]; then + # Connect to existing model echo "INFO [init] PX4_GZ_MODEL_NAME set, PX4 will attach to existing model" - if ! gz_bridge start -n "${PX4_GZ_MODEL_NAME}" -w "${PX4_GZ_WORLD}"; then + if ! gz_bridge start -w "${PX4_GZ_WORLD}" -n "${PX4_GZ_MODEL_NAME}"; then echo "ERROR [init] gz_bridge failed to start and attach to existing model" exit 1 fi - else - echo "ERROR [init] failed to pass only PX4_GZ_MODEL_NAME or PX4_SIM_MODEL" + echo "ERROR [init] failed to pass either PX4_GZ_MODEL_NAME or PX4_SIM_MODEL" exit 1 fi - # Start the sensor simulator modules if param compare -s SENS_EN_BAROSIM 1 then diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index ef2907deb7..c53dbbd46a 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -337,6 +337,11 @@ then payload_deliverer start fi +if param compare -s ICE_EN 1 +then + internal_combustion_engine_control start +fi + #user defined mavlink streams for instances can be in PATH . px4-rc.mavlink diff --git a/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision b/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision index d60de46e7b..9b86703c31 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision +++ b/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision @@ -12,7 +12,6 @@ . ${R}etc/init.d/rc.mc_defaults # Commander Parameters -param set-default COM_OBS_AVOID 1 param set-default COM_DISARM_LAND 0.5 # EKF2 parameters diff --git a/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 b/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 index c9c3e5843a..e63a9eef01 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 +++ b/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 @@ -12,7 +12,6 @@ . ${R}etc/init.d/rc.mc_defaults # Commander Parameters -param set-default COM_OBS_AVOID 1 param set-default COM_DISARM_LAND 0.5 # EKF2 parameters diff --git a/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover b/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover index 57a3644dae..45b35ffeaa 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover +++ b/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover @@ -21,26 +21,37 @@ param set-default RBCLW_FUNC1 101 param set-default RBCLW_FUNC2 102 param set-default RBCLW_REV 1 # reverse right wheels -# Rover parameters + +param set-default NAV_ACC_RAD 0.5 + +# Differential Parameters param set-default RD_WHEEL_TRACK 0.3 -param set-default RD_MAX_ACCEL 3 -param set-default RD_MAX_DECEL 4 -param set-default RD_MAX_JERK 5 -param set-default RD_MAX_SPEED 1.6 -param set-default RD_MAX_THR_SPD 1.9 param set-default RD_MAX_THR_YAW_R 0.7 -param set-default RD_MAX_YAW_ACCEL 600 -param set-default RD_MAX_YAW_RATE 250 -param set-default RD_SPEED_P 0.1 -param set-default RD_SPEED_I 0.01 param set-default RD_TRANS_DRV_TRN 0.785398 param set-default RD_TRANS_TRN_DRV 0.139626 -param set-default RD_YAW_P 5 -param set-default RD_YAW_I 0.1 -param set-default RD_YAW_RATE_P 0.1 -param set-default RD_YAW_RATE_I 0.01 -# Pure pursuit parameters +# Rover Control Parameters +param set-default RO_ACCEL_LIM 3 +param set-default RO_DECEL_LIM 4 +param set-default RO_JERK_LIM 5 +param set-default RO_MAX_THR_SPEED 1.9 + +# Rover Rate Control Parameters +param set-default RO_YAW_RATE_I 0.01 +param set-default RO_YAW_RATE_P 0.1 +param set-default RO_YAW_RATE_LIM 250 +param set-default RO_YAW_ACCEL_LIM 600 +param set-default RO_YAW_DECEL_LIM 600 + +# Rover Attitude Control Parameters +param set-default RO_YAW_P 5 + +# Rover Position Control Parameters +param set-default RO_SPEED_LIM 1.6 +param set-default RO_SPEED_I 0.01 +param set-default RO_SPEED_P 0.1 + +# Pure Pursuit parameters +param set-default PP_LOOKAHD_GAIN 1 param set-default PP_LOOKAHD_MAX 10 param set-default PP_LOOKAHD_MIN 1 -param set-default PP_LOOKAHD_GAIN 1 diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 807a26ed03..9131f83e96 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -488,11 +488,14 @@ else rc_input start $RC_INPUT_ARGS # Manages USB interface - if ! cdcacm_autostart start + if param greater -s SYS_USB_AUTO -1 then - sercon - echo "Starting MAVLink on /dev/ttyACM0" - mavlink start -d /dev/ttyACM0 + if ! cdcacm_autostart start + then + sercon + echo "Starting MAVLink on /dev/ttyACM0" + mavlink start -d /dev/ttyACM0 + fi fi # @@ -555,6 +558,11 @@ else payload_deliverer start fi + if param compare -s ICE_EN 1 + then + internal_combustion_engine_control start + fi + # # Optional board supplied extras: rc.board_extras # diff --git a/Tools/HIL/run_tests.py b/Tools/HIL/run_tests.py index 3e79c70227..0d9c5f5d1b 100755 --- a/Tools/HIL/run_tests.py +++ b/Tools/HIL/run_tests.py @@ -136,9 +136,6 @@ class TestHardwareMethods(unittest.TestCase): def test_atomic_bitset(self): self.assertTrue(do_test(self.TEST_DEVICE, self.TEST_BAUDRATE, "atomic_bitset")) - def test_bezier(self): - self.assertTrue(do_test(self.TEST_DEVICE, self.TEST_BAUDRATE, "bezier")) - def test_bitset(self): self.assertTrue(do_test(self.TEST_DEVICE, self.TEST_BAUDRATE, "bitset")) diff --git a/Tools/ci/check_msg_versioning.sh b/Tools/ci/check_msg_versioning.sh index c10d4819c2..37c593d277 100755 --- a/Tools/ci/check_msg_versioning.sh +++ b/Tools/ci/check_msg_versioning.sh @@ -26,6 +26,15 @@ do # - Incrementing the version # - An old .msg version exists # - A translation header exists and is included + + # Ignore changes to comments or constants + content_a=$(git show "${BASE_COMMIT}:${file}" | grep -o '^[^#]*' | grep -v =) + content_b=$(git show "${HEAD_COMMIT}:${file}" | grep -o '^[^#]*' | grep -v =) + if [ "${content_a}" == "${content_b}" ]; then + echo "No version update required for ${file}" + continue + fi + diff=$(git --no-pager diff --no-color --diff-filter=M "${BASE_COMMIT}"..."${HEAD_COMMIT}" -- "${file}") old_version=$(echo "${diff}" | sed -n 's/^-uint32 MESSAGE_VERSION = \([0-9]*\).*/\1/p') new_version=$(echo "${diff}" | sed -n 's/^+uint32 MESSAGE_VERSION = \([0-9]*\).*/\1/p') diff --git a/Tools/ci/generate_board_targets_json.py b/Tools/ci/generate_board_targets_json.py index 218862cecd..5749a988d1 100755 --- a/Tools/ci/generate_board_targets_json.py +++ b/Tools/ci/generate_board_targets_json.py @@ -69,7 +69,7 @@ def process_target(px4board_file, target_name): group = None if px4board_file.endswith("default.px4board") or \ - px4board_file.endswith("recovery.px4board") or \ + px4board_file.endswith("performance-test.px4board") or \ px4board_file.endswith("bootloader.px4board"): kconf.load_config(px4board_file, replace=True) else: # Merge config with default.px4board diff --git a/Tools/copy_to_ros_ws.sh b/Tools/copy_to_ros_ws.sh index ef663aaaa9..48fcaa0d1f 100755 --- a/Tools/copy_to_ros_ws.sh +++ b/Tools/copy_to_ros_ws.sh @@ -29,6 +29,5 @@ then rm -rf "${PX4_MSGS_DIR}"/srv/*.srv fi cp -ar "${PX4_SRC_DIR}"/msg/*.msg "${PX4_MSGS_DIR}"/msg -mkdir -p "${PX4_MSGS_DIR}"/msg/versioned -cp -ar "${PX4_SRC_DIR}"/msg/versioned/*.msg "${PX4_MSGS_DIR}"/msg/versioned +cp -ar "${PX4_SRC_DIR}"/msg/versioned/*.msg "${PX4_MSGS_DIR}"/msg cp -ar "${PX4_SRC_DIR}"/srv/*.srv "${PX4_MSGS_DIR}"/srv diff --git a/Tools/docker_run.sh b/Tools/docker_run.sh index c78b9cc7e1..46888cd066 100755 --- a/Tools/docker_run.sh +++ b/Tools/docker_run.sh @@ -47,6 +47,7 @@ CCACHE_DIR=${HOME}/.ccache mkdir -p "${CCACHE_DIR}" docker run -it --rm -w "${SRC_DIR}" \ + --user="$(id -u):$(id -g)" \ --env=AWS_ACCESS_KEY_ID \ --env=AWS_SECRET_ACCESS_KEY \ --env=BRANCH_NAME \ @@ -54,7 +55,6 @@ docker run -it --rm -w "${SRC_DIR}" \ --env=CI \ --env=CODECOV_TOKEN \ --env=COVERALLS_REPO_TOKEN \ - --env=LOCAL_USER_ID="$(id -u)" \ --env=PX4_ASAN \ --env=PX4_MSAN \ --env=PX4_TSAN \ diff --git a/Tools/kconfig/allyesconfig.py b/Tools/kconfig/allyesconfig.py index 2c0f58d75a..e432c52ec0 100644 --- a/Tools/kconfig/allyesconfig.py +++ b/Tools/kconfig/allyesconfig.py @@ -74,6 +74,7 @@ exception_list_sitl = [ 'SYSTEMCMDS_I2CDETECT', # Not supported in SITL 'SYSTEMCMDS_DMESG', # Not supported in SITL 'SYSTEMCMDS_USB_CONNECTED', # Not supported in SITL + 'SYSTEMCMDS_MFT_CFG', # Not supported in SITL 'MODULES_SPACECRAFT', # Clashes with Control Allocation (mom's spaghetti code) ] diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 34f3be05ca..6d8c7c69dc 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -708,7 +708,7 @@ class uploader: # https://github.com/PX4/Firmware/blob/master/src/drivers/boards/common/stm32/board_mcu_version.c#L125-L144 if self.fw_maxsize > fw.property('image_maxsize') and not force: - raise RuntimeError(f"Board can accept larger flash images ({self.fw_maxsize} bytes) than board config ({fw.property('image_maxsize')} bytes). Please use the correct board configuration to avoid lacking critical functionality.") + print(f"WARNING: Board can accept larger flash images ({self.fw_maxsize} bytes) than board config ({fw.property('image_maxsize')} bytes)") else: # If we're still on bootloader v4 on a Pixhawk, we don't know if we # have the silicon errata and therefore need to flash px4_fmu-v2 diff --git a/Tools/setup/docker-entrypoint.sh b/Tools/setup/docker-entrypoint.sh index 1094587c1c..0412558902 100755 --- a/Tools/setup/docker-entrypoint.sh +++ b/Tools/setup/docker-entrypoint.sh @@ -1,6 +1,5 @@ #!/bin/bash -echo "[docker-entrypoint.sh] Starting entrypoint" # Start virtual X server in the background # - DISPLAY default is :99, set in dockerfile # - Users can override with `-e DISPLAY=` in `docker run` command to avoid @@ -17,17 +16,4 @@ if [ -n "${ROS_DISTRO}" ]; then source "/opt/ros/$ROS_DISTRO/setup.bash" fi -echo "[docker-entrypoint.sh] Working Directory: ${pwd}" - -# Use the LOCAL_USER_ID if passed in at runtime -if [ -n "${LOCAL_USER_ID}" ]; then - echo "[docker-entrypoint.sh] Starting with: $LOCAL_USER_ID:user" - # modify existing user's id - usermod -u $LOCAL_USER_ID user - - # run as user - # exec gosu user "$@" - exec "$@" -else - exec "$@" -fi +exec "$@" diff --git a/Tools/setup/ubuntu.sh b/Tools/setup/ubuntu.sh index 434b2a81cd..e641a82da3 100755 --- a/Tools/setup/ubuntu.sh +++ b/Tools/setup/ubuntu.sh @@ -116,6 +116,7 @@ if [[ $INSTALL_NUTTX == "true" ]]; then binutils-dev \ bison \ build-essential \ + curl \ flex \ g++-multilib \ gcc-arm-none-eabi \ diff --git a/Tools/simulation/gz b/Tools/simulation/gz index db4af69088..6c18846a4c 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit db4af69088cccef4549cf3a5c195d5cd97d6b36a +Subproject commit 6c18846a4c7f9fe786840a29bf4e3237f908611b diff --git a/boards/ark/can-gps/init/rc.board_defaults b/boards/ark/can-gps/init/rc.board_defaults index 13e16a24f1..3d085fe527 100644 --- a/boards/ark/can-gps/init/rc.board_defaults +++ b/boards/ark/can-gps/init/rc.board_defaults @@ -4,7 +4,7 @@ #------------------------------------------------------------------------------ param set-default CBRK_IO_SAFETY 0 -param set-default MBE_ENABLE 1 +param set-default MBE_ENABLE 0 param set-default SENS_IMU_CLPNOTI 0 safety_button start diff --git a/boards/ark/can-rtk-gps/init/rc.board_defaults b/boards/ark/can-rtk-gps/init/rc.board_defaults index 99b30c747f..85d6a92063 100644 --- a/boards/ark/can-rtk-gps/init/rc.board_defaults +++ b/boards/ark/can-rtk-gps/init/rc.board_defaults @@ -7,7 +7,7 @@ param set-default CBRK_IO_SAFETY 0 param set-default CANNODE_SUB_MBD 1 param set-default CANNODE_SUB_RTCM 1 param set-default GPS_1_GNSS 63 -param set-default MBE_ENABLE 1 +param set-default MBE_ENABLE 0 param set-default SENS_IMU_CLPNOTI 0 safety_button start diff --git a/boards/ark/cannode/init/rc.board_defaults b/boards/ark/cannode/init/rc.board_defaults index 7d149ce5ad..9fe7db23e0 100644 --- a/boards/ark/cannode/init/rc.board_defaults +++ b/boards/ark/cannode/init/rc.board_defaults @@ -3,6 +3,7 @@ # board specific defaults #------------------------------------------------------------------------------ +param set-default MBE_ENABLE 0 param set-default SENS_IMU_CLPNOTI 0 pwm_out start diff --git a/boards/ark/pi6x/nuttx-config/bootloader/defconfig b/boards/ark/pi6x/nuttx-config/bootloader/defconfig index 745251793b..bba1ad3f39 100644 --- a/boards/ark/pi6x/nuttx-config/bootloader/defconfig +++ b/boards/ark/pi6x/nuttx-config/bootloader/defconfig @@ -32,7 +32,7 @@ CONFIG_BOARD_LOOPSPERMSEC=95150 CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_CDCACM=y CONFIG_CDCACM_IFLOWCONTROL=y -CONFIG_CDCACM_PRODUCTID=0x0039 +CONFIG_CDCACM_PRODUCTID=0x003A CONFIG_CDCACM_PRODUCTSTR="ARK BL Pi6X.x" CONFIG_CDCACM_RXBUFSIZE=600 CONFIG_CDCACM_TXBUFSIZE=12000 diff --git a/boards/ark/pi6x/nuttx-config/nsh/defconfig b/boards/ark/pi6x/nuttx-config/nsh/defconfig index 0e944e53c9..57ff449302 100644 --- a/boards/ark/pi6x/nuttx-config/nsh/defconfig +++ b/boards/ark/pi6x/nuttx-config/nsh/defconfig @@ -76,7 +76,7 @@ CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_CDCACM=y CONFIG_CDCACM_IFLOWCONTROL=y -CONFIG_CDCACM_PRODUCTID=0x0039 +CONFIG_CDCACM_PRODUCTID=0x003A CONFIG_CDCACM_PRODUCTSTR="ARK Pi6X.x" CONFIG_CDCACM_RXBUFSIZE=600 CONFIG_CDCACM_TXBUFSIZE=12000 diff --git a/boards/ark/septentrio-gps/init/rc.board_defaults b/boards/ark/septentrio-gps/init/rc.board_defaults index 0db1234ce9..6ae5883882 100644 --- a/boards/ark/septentrio-gps/init/rc.board_defaults +++ b/boards/ark/septentrio-gps/init/rc.board_defaults @@ -6,7 +6,7 @@ param set-default CBRK_IO_SAFETY 0 param set-default CANNODE_SUB_MBD 1 param set-default CANNODE_SUB_RTCM 1 -param set-default MBE_ENABLE 1 +param set-default MBE_ENABLE 0 param set-default SENS_IMU_CLPNOTI 0 safety_button start diff --git a/boards/ark/teseo-gps/init/rc.board_defaults b/boards/ark/teseo-gps/init/rc.board_defaults index 610b5b418f..c765117f40 100644 --- a/boards/ark/teseo-gps/init/rc.board_defaults +++ b/boards/ark/teseo-gps/init/rc.board_defaults @@ -6,7 +6,7 @@ param set-default CBRK_IO_SAFETY 0 param set-default CANNODE_SUB_MBD 1 param set-default CANNODE_SUB_RTCM 1 -param set-default MBE_ENABLE 1 +param set-default MBE_ENABLE 0 param set-default SENS_IMU_CLPNOTI 0 tone_alarm start diff --git a/boards/bitcraze/crazyflie/syslink/syslink_main.h b/boards/bitcraze/crazyflie/syslink/syslink_main.h index a54859508d..8bf97b1c6e 100644 --- a/boards/bitcraze/crazyflie/syslink/syslink_main.h +++ b/boards/bitcraze/crazyflie/syslink/syslink_main.h @@ -139,7 +139,7 @@ private: // nrf chip schedules battery updates with SYSLINK_SEND_PERIOD_MS static constexpr uint32_t SYSLINK_BATTERY_STATUS_INTERVAL_US = 10_ms; - Battery _battery{1, nullptr, SYSLINK_BATTERY_STATUS_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE}; + Battery _battery{1, nullptr, SYSLINK_BATTERY_STATUS_INTERVAL_US, battery_status_s::SOURCE_POWER_MODULE}; int32_t _rssi; battery_state _bstate; diff --git a/boards/modalai/voxl2-slpi/default.px4board b/boards/modalai/voxl2-slpi/default.px4board index db58d4e28a..1aec34124a 100644 --- a/boards/modalai/voxl2-slpi/default.px4board +++ b/boards/modalai/voxl2-slpi/default.px4board @@ -13,6 +13,7 @@ CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y CONFIG_DRIVERS_QSHELL_QURT=y CONFIG_DRIVERS_RC_CRSF_RC=y +CONFIG_DRIVERS_VOXL2_IO=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_EKF2=y diff --git a/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/ghst_rc.cpp b/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/ghst_rc.cpp index e213179922..9ad45c1389 100644 --- a/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/ghst_rc.cpp +++ b/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/ghst_rc.cpp @@ -207,15 +207,15 @@ void GhstRc::Run() if (new_bytes > 0) { _bytes_rx += new_bytes; - int8_t ghst_rssi = -1; - bool rc_updated = ghst_parse(cycle_timestamp, &_rcs_buf[0], new_bytes, &_raw_rc_values[0], &ghst_rssi, + ghstLinkStatistics_t link_stats = { .rssi_pct = -1, .rssi_dbm = NAN, .link_quality = 0 }; + bool rc_updated = ghst_parse(cycle_timestamp, &_rcs_buf[0], new_bytes, &_raw_rc_values[0], &link_stats, &_raw_rc_count, GHST_MAX_NUM_CHANNELS); if (rc_updated) { _last_packet_seen = time_now_us; // we have a new GHST frame. Publish it. _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST; - fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0, ghst_rssi); + fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0, link_stats.rssi_pct); // ghst telemetry works on fmu-v5 // on other Pixhawk (-related) boards we cannot write to the RC UART diff --git a/boards/modalai/voxl2/default.px4board b/boards/modalai/voxl2/default.px4board index 20860e3fce..63023ff4b9 100644 --- a/boards/modalai/voxl2/default.px4board +++ b/boards/modalai/voxl2/default.px4board @@ -1,7 +1,7 @@ CONFIG_PLATFORM_POSIX=y CONFIG_BOARD_LINUX_TARGET=y CONFIG_BOARD_TOOLCHAIN="aarch64-linux-gnu" -CONFIG_BOARD_ROOTFSDIR="/data/px4" +CONFIG_BOARD_ROOT_PATH="/data/px4" CONFIG_DRIVERS_ACTUATORS_VOXL_ESC=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_OSD_MSP_OSD=y diff --git a/boards/px4/fmu-v5x/performance-test.px4board b/boards/px4/fmu-v5x/performance-test.px4board new file mode 100644 index 0000000000..5e69dd7ae3 --- /dev/null +++ b/boards/px4/fmu-v5x/performance-test.px4board @@ -0,0 +1,31 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ETHERNET=y +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS7" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1" +CONFIG_BOARD_SERIAL_EXT2="/dev/ttyS3" +CONFIG_BOARD_ROMFSROOT="performance-test" +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_SYSTEMCMDS_DMESG=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_MFT_CFG=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=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_UORB=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/px4/fmu-v6x/performance-test.px4board b/boards/px4/fmu-v6x/performance-test.px4board new file mode 100644 index 0000000000..5e69dd7ae3 --- /dev/null +++ b/boards/px4/fmu-v6x/performance-test.px4board @@ -0,0 +1,31 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ETHERNET=y +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS7" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1" +CONFIG_BOARD_SERIAL_EXT2="/dev/ttyS3" +CONFIG_BOARD_ROMFSROOT="performance-test" +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_SYSTEMCMDS_DMESG=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_MFT_CFG=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=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_UORB=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index b0b7a8a2b3..8eb8ebec8c 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -50,7 +50,9 @@ CONFIG_MODULES_ROVER_MECANUM=y CONFIG_MODULES_ROVER_POS_CONTROL=y CONFIG_MODULES_SENSORS=y CONFIG_COMMON_SIMULATION=y +CONFIG_MODULES_SIMULATION_GZ_MSGS=y CONFIG_MODULES_SIMULATION_GZ_BRIDGE=y +CONFIG_MODULES_SIMULATION_GZ_PLUGINS=y CONFIG_MODULES_SIMULATION_SENSOR_AGP_SIM=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y CONFIG_MODULES_UUV_ATT_CONTROL=y diff --git a/cmake/kconfig.cmake b/cmake/kconfig.cmake index 7eaaa00d67..a5c79ad131 100644 --- a/cmake/kconfig.cmake +++ b/cmake/kconfig.cmake @@ -35,7 +35,7 @@ if(EXISTS ${BOARD_DEFCONFIG}) # Depend on BOARD_DEFCONFIG so that we reconfigure on config change set_property(DIRECTORY APPEND PROPERTY CMAKE_CONFIGURE_DEPENDS ${BOARD_DEFCONFIG}) - if(${LABEL} MATCHES "default" OR ${LABEL} MATCHES "recovery" OR ${LABEL} MATCHES "bootloader" OR ${LABEL} MATCHES "canbootloader") + if(${LABEL} MATCHES "default" OR ${LABEL} MATCHES "performance-test" OR ${LABEL} MATCHES "bootloader" OR ${LABEL} MATCHES "canbootloader") # Generate boardconfig from saved defconfig execute_process( COMMAND ${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS} diff --git a/integrationtests/python_src/px4_it/mavros/missions/avoidance.plan b/integrationtests/python_src/px4_it/mavros/missions/avoidance.plan deleted file mode 100644 index 1698c6b2bc..0000000000 --- a/integrationtests/python_src/px4_it/mavros/missions/avoidance.plan +++ /dev/null @@ -1,88 +0,0 @@ -{ - "fileType": "Plan", - "geoFence": { - "circles": [ - ], - "polygons": [ - ], - "version": 2 - }, - "groundStation": "QGroundControl", - "mission": { - "cruiseSpeed": 15, - "firmwareType": 12, - "hoverSpeed": 5, - "items": [ - { - "AMSLAltAboveTerrain": null, - "Altitude": 4, - "AltitudeMode": 0, - "autoContinue": true, - "command": 22, - "doJumpId": 1, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.3977432, - 8.5456085, - 4 - ], - "type": "SimpleItem" - }, - { - "AMSLAltAboveTerrain": null, - "Altitude": 4, - "AltitudeMode": 0, - "autoContinue": true, - "command": 16, - "doJumpId": 2, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.3977432, - 8.5458765, - 4 - ], - "type": "SimpleItem" - }, - { - "AMSLAltAboveTerrain": null, - "Altitude": -1, - "AltitudeMode": 0, - "autoContinue": true, - "command": 21, - "doJumpId": 3, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.3977432, - 8.5458812, - -1 - ], - "type": "SimpleItem" - } - ], - "plannedHomePosition": [ - 47.3977419, - 8.5458854, - 487.923 - ], - "vehicleType": 2, - "version": 2 - }, - "rallyPoints": { - "points": [ - ], - "version": 2 - }, - "version": 1 -} diff --git a/msg/Airspeed.msg b/msg/Airspeed.msg index aaed7b72ca..bc673cf0aa 100644 --- a/msg/Airspeed.msg +++ b/msg/Airspeed.msg @@ -5,6 +5,4 @@ float32 indicated_airspeed_m_s # indicated airspeed in m/s float32 true_airspeed_m_s # true filtered airspeed in m/s -float32 air_temperature_celsius # air temperature in degrees Celsius, -1000 if unknown - float32 confidence # confidence value from 0 to 1 for this sensor diff --git a/msg/Buffer128.msg b/msg/Buffer128.msg deleted file mode 100644 index 4ff939a87d..0000000000 --- a/msg/Buffer128.msg +++ /dev/null @@ -1,8 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -uint8 len # length of data -uint32 MAX_BUFLEN = 128 - -uint8[128] data # data - -# TOPICS voxl2_io_data diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 51c9325413..8ef4407504 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -48,7 +48,6 @@ set(msg_files AirspeedValidated.msg AirspeedWind.msg AutotuneAttitudeControlStatus.msg - Buffer128.msg ButtonEvent.msg CameraCapture.msg CameraStatus.msg @@ -114,6 +113,7 @@ set(msg_files HeaterStatus.msg HoverThrustEstimate.msg InputRc.msg + InternalCombustionEngineControl.msg InternalCombustionEngineStatus.msg IridiumsbdStatus.msg IrlockReport.msg @@ -173,17 +173,11 @@ set(msg_files RcParameterMap.msg RoverAttitudeSetpoint.msg RoverAttitudeStatus.msg - RoverVelocityStatus.msg RoverRateSetpoint.msg RoverRateStatus.msg RoverSteeringSetpoint.msg RoverThrottleSetpoint.msg - RoverDifferentialGuidanceStatus.msg - RoverDifferentialSetpoint.msg - RoverDifferentialStatus.msg - RoverMecanumGuidanceStatus.msg - RoverMecanumSetpoint.msg - RoverMecanumStatus.msg + RoverVelocityStatus.msg Rpm.msg RtlStatus.msg RtlTimeEstimate.msg @@ -214,8 +208,6 @@ set(msg_files TelemetryStatus.msg TiltrotorExtraControls.msg TimesyncStatus.msg - TrajectoryBezier.msg - TrajectoryWaypoint.msg TransponderReport.msg TuneControl.msg UavcanParameterRequest.msg @@ -235,8 +227,6 @@ set(msg_files VehicleRoi.msg VehicleThrustSetpoint.msg VehicleTorqueSetpoint.msg - VehicleTrajectoryBezier.msg - VehicleTrajectoryWaypoint.msg VelocityLimits.msg WheelEncoders.msg Wind.msg diff --git a/msg/GimbalManagerSetAttitude.msg b/msg/GimbalManagerSetAttitude.msg index d88acca8b6..e1a174fd0b 100644 --- a/msg/GimbalManagerSetAttitude.msg +++ b/msg/GimbalManagerSetAttitude.msg @@ -20,3 +20,5 @@ float32[4] q float32 angular_velocity_x float32 angular_velocity_y float32 angular_velocity_z + +uint8 ORB_QUEUE_LENGTH = 2 diff --git a/msg/InternalCombustionEngineControl.msg b/msg/InternalCombustionEngineControl.msg new file mode 100644 index 0000000000..08b4198e01 --- /dev/null +++ b/msg/InternalCombustionEngineControl.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) + +bool ignition_on # activate/deactivate ignition (Spark Plug) +float32 throttle_control # [0,1] - Motor should idle with 0. Includes slew rate if enabled. +float32 choke_control # [0,1] - 1 fully closes the air inlet. +float32 starter_engine_control # [0,1] - control value for electric starter motor. + +uint8 user_request # user intent for the ICE being on/off diff --git a/msg/MavlinkTunnel.msg b/msg/MavlinkTunnel.msg index 16934a9522..140bca5a1b 100644 --- a/msg/MavlinkTunnel.msg +++ b/msg/MavlinkTunnel.msg @@ -18,3 +18,6 @@ uint8 target_system # System ID (can be 0 for broadcast, but this is discou uint8 target_component # Component ID (can be 0 for broadcast, but this is discouraged) uint8 payload_length # Length of the data transported in payload uint8[128] payload # Data itself + +# Topic aliases for known payload types +# TOPICS mavlink_tunnel esc_serial_passthru diff --git a/msg/PositionControllerStatus.msg b/msg/PositionControllerStatus.msg index 7237351fd0..44ec5412ba 100644 --- a/msg/PositionControllerStatus.msg +++ b/msg/PositionControllerStatus.msg @@ -7,6 +7,4 @@ float32 target_bearing # Bearing angle from aircraft to current target [rad] float32 xtrack_error # Signed track error [m] float32 wp_dist # Distance to active (next) waypoint [m] float32 acceptance_radius # Current horizontal acceptance radius [m] -float32 yaw_acceptance # Yaw acceptance error[rad] -float32 altitude_acceptance # Current vertical acceptance error [m] uint8 type # Current (applied) position setpoint type (see PositionSetpoint.msg) diff --git a/msg/RoverDifferentialGuidanceStatus.msg b/msg/RoverDifferentialGuidanceStatus.msg deleted file mode 100644 index ce3d375111..0000000000 --- a/msg/RoverDifferentialGuidanceStatus.msg +++ /dev/null @@ -1,7 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 lookahead_distance # [m] Lookahead distance of pure the pursuit controller -float32 heading_error_deg # [deg] Heading error of the pure pursuit controller -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/RoverDifferentialSetpoint.msg b/msg/RoverDifferentialSetpoint.msg deleted file mode 100644 index 100cc6b3cd..0000000000 --- a/msg/RoverDifferentialSetpoint.msg +++ /dev/null @@ -1,9 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 forward_speed_setpoint # [m/s] Desired forward speed for the rover -float32 forward_speed_setpoint_normalized # [-1, 1] Normalized forward speed for the rover -float32 yaw_rate_setpoint # [rad/s] Desired yaw rate for the rover (Overriden by yaw controller if yaw_setpoint is used) -float32 speed_diff_setpoint_normalized # [-1, 1] Normalized speed difference between the left and right wheels -float32 yaw_setpoint # [rad] Desired yaw (heading) for the rover - -# TOPICS rover_differential_setpoint diff --git a/msg/RoverDifferentialStatus.msg b/msg/RoverDifferentialStatus.msg deleted file mode 100644 index 767474fb5b..0000000000 --- a/msg/RoverDifferentialStatus.msg +++ /dev/null @@ -1,14 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 measured_forward_speed # [m/s] Measured speed in body x direction. Forwards: positiv, Backwards: negativ -float32 adjusted_forward_speed_setpoint # [m/s] Speed setpoint after applying slew rate -float32 measured_yaw # [rad] Measured yaw -float32 adjusted_yaw_setpoint # [rad] Yaw setpoint after applying slew rate -float32 clyaw_yaw_rate_setpoint # [rad/s] Yaw rate setpoint output by the closed loop yaw controller -float32 measured_yaw_rate # [rad/s] Measured yaw rate -float32 adjusted_yaw_rate_setpoint # [rad/s] Yaw rate setpoint from the closed loop yaw controller -float32 pid_yaw_integral # Integral of the PID for the closed loop yaw controller -float32 pid_yaw_rate_integral # Integral of the PID for the closed loop yaw rate controller -float32 pid_throttle_integral # Integral of the PID for the closed loop speed controller - -# TOPICS rover_differential_status diff --git a/msg/RoverMecanumGuidanceStatus.msg b/msg/RoverMecanumGuidanceStatus.msg deleted file mode 100644 index fba920033b..0000000000 --- a/msg/RoverMecanumGuidanceStatus.msg +++ /dev/null @@ -1,7 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 lookahead_distance # [m] Lookahead distance of pure the pursuit controller -float32 heading_error # [rad] Heading error of the pure pursuit controller -float32 desired_speed # [m/s] Desired velocity magnitude (speed) - -# TOPICS rover_mecanum_guidance_status diff --git a/msg/RoverMecanumSetpoint.msg b/msg/RoverMecanumSetpoint.msg deleted file mode 100644 index 0cc9415be1..0000000000 --- a/msg/RoverMecanumSetpoint.msg +++ /dev/null @@ -1,11 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 forward_speed_setpoint # [m/s] Desired forward speed -float32 forward_speed_setpoint_normalized # [-1, 1] Desired normalized forward speed -float32 lateral_speed_setpoint # [m/s] Desired lateral speed -float32 lateral_speed_setpoint_normalized # [-1, 1] Desired normalized lateral speed -float32 yaw_rate_setpoint # [rad/s] Desired yaw rate -float32 speed_diff_setpoint_normalized # [-1, 1] Normalized speed difference between the left and right wheels -float32 yaw_setpoint # [rad] Desired yaw (heading) - -# TOPICS rover_mecanum_setpoint diff --git a/msg/RoverMecanumStatus.msg b/msg/RoverMecanumStatus.msg deleted file mode 100644 index 7547fd5be6..0000000000 --- a/msg/RoverMecanumStatus.msg +++ /dev/null @@ -1,17 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 measured_forward_speed # [m/s] Measured speed in body x direction. Positiv: forwards, Negativ: backwards -float32 adjusted_forward_speed_setpoint # [m/s] Speed setpoint after applying slew rate -float32 measured_lateral_speed # [m/s] Measured speed in body y direction. Positiv: right, Negativ: left -float32 adjusted_lateral_speed_setpoint # [m/s] Speed setpoint after applying slew rate -float32 measured_yaw_rate # [rad/s] Measured yaw rate -float32 clyaw_yaw_rate_setpoint # [rad/s] Yaw rate setpoint output by the closed loop yaw controller -float32 adjusted_yaw_rate_setpoint # [rad/s] Yaw rate setpoint from the closed loop yaw controller -float32 measured_yaw # [rad] Measured yaw -float32 adjusted_yaw_setpoint # [rad] Yaw setpoint after applying slew rate -float32 pid_yaw_rate_integral # Integral of the PID for the closed loop yaw rate controller -float32 pid_yaw_integral # Integral of the PID for the closed loop yaw controller -float32 pid_forward_throttle_integral # Integral of the PID for the closed loop forward speed controller -float32 pid_lateral_throttle_integral # Integral of the PID for the closed loop lateral speed controller - -# TOPICS rover_mecanum_status diff --git a/msg/SensorOpticalFlow.msg b/msg/SensorOpticalFlow.msg index ce7e8bf08c..ed6a3cfefb 100644 --- a/msg/SensorOpticalFlow.msg +++ b/msg/SensorOpticalFlow.msg @@ -3,7 +3,7 @@ uint64 timestamp_sample uint32 device_id # unique device ID for the sensor that does not change between power cycles -float32[2] pixel_flow # (radians) optical flow in radians where a positive value is produced by a RH rotation about the body axis +float32[2] pixel_flow # (radians) optical flow in radians where a positive value is produced by a RH rotation of the sensor about the body axis float32[3] delta_angle # (radians) accumulated gyro radians where a positive value is produced by a RH rotation about the body axis. Set to NaN if flow sensor does not have 3-axis gyro data. bool delta_angle_available diff --git a/msg/TelemetryStatus.msg b/msg/TelemetryStatus.msg index 48d85f934d..b2b3920a36 100644 --- a/msg/TelemetryStatus.msg +++ b/msg/TelemetryStatus.msg @@ -51,13 +51,11 @@ bool heartbeat_type_open_drone_id # MAV_TYPE_ODID bool heartbeat_component_telemetry_radio # MAV_COMP_ID_TELEMETRY_RADIO bool heartbeat_component_log # MAV_COMP_ID_LOG bool heartbeat_component_osd # MAV_COMP_ID_OSD -bool heartbeat_component_obstacle_avoidance # MAV_COMP_ID_OBSTACLE_AVOIDANCE bool heartbeat_component_vio # MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY bool heartbeat_component_pairing_manager # MAV_COMP_ID_PAIRING_MANAGER bool heartbeat_component_udp_bridge # MAV_COMP_ID_UDP_BRIDGE bool heartbeat_component_uart_bridge # MAV_COMP_ID_UART_BRIDGE # Misc component health -bool avoidance_system_healthy bool open_drone_id_system_healthy bool parachute_system_healthy diff --git a/msg/TrajectoryBezier.msg b/msg/TrajectoryBezier.msg deleted file mode 100644 index e3d9d4e0ff..0000000000 --- a/msg/TrajectoryBezier.msg +++ /dev/null @@ -1,8 +0,0 @@ -# Bezier Trajectory description. See also Mavlink TRAJECTORY msg -# The topic trajectory_bezier describe each waypoint defined in vehicle_trajectory_bezier - -uint64 timestamp # time since system start (microseconds) - -float32[3] position # local position x,y,z (metres) -float32 yaw # yaw angle (rad) -float32 delta # time it should take to get to this waypoint, if this is the final waypoint (seconds) diff --git a/msg/TrajectoryWaypoint.msg b/msg/TrajectoryWaypoint.msg deleted file mode 100644 index 6ea9bae4d3..0000000000 --- a/msg/TrajectoryWaypoint.msg +++ /dev/null @@ -1,13 +0,0 @@ -# Waypoint Trajectory description. See also Mavlink TRAJECTORY msg -# The topic trajectory_waypoint describe each waypoint defined in vehicle_trajectory_waypoint - -uint64 timestamp # time since system start (microseconds) - -float32[3] position -float32[3] velocity -float32[3] acceleration -float32 yaw -float32 yaw_speed - -bool point_valid -uint8 type diff --git a/msg/VehicleAirData.msg b/msg/VehicleAirData.msg index 59ca5e5c8d..41e5358ae8 100644 --- a/msg/VehicleAirData.msg +++ b/msg/VehicleAirData.msg @@ -6,10 +6,10 @@ uint64 timestamp_sample # the timestamp of the raw data (microseconds) uint32 baro_device_id # unique device ID for the selected barometer float32 baro_alt_meter # Altitude above MSL calculated from temperature compensated baro sensor data using an ISA corrected for sea level pressure SENS_BARO_QNH. -float32 baro_temp_celcius # Temperature in degrees Celsius float32 baro_pressure_pa # Absolute pressure in Pascals +float32 ambient_temperature # Abient temperature in degrees Celsius +uint8 temperature_source # Source of temperature data: 0: Default Temperature (15°C), 1: External Baro, 2: Airspeed float32 rho # air density -float32 eas2tas # equivalent airspeed to true airspeed conversion factor uint8 calibration_count # Calibration changed counter. Monotonically increases whenever calibration changes. diff --git a/msg/VehicleOpticalFlow.msg b/msg/VehicleOpticalFlow.msg index 13bdb57bbf..23472b4c97 100644 --- a/msg/VehicleOpticalFlow.msg +++ b/msg/VehicleOpticalFlow.msg @@ -7,7 +7,7 @@ uint32 device_id # unique device ID for the sensor that does not c float32[2] pixel_flow # (radians) accumulated optical flow in radians where a positive value is produced by a RH rotation about the body axis -float32[3] delta_angle # (radians) accumulated gyro radians where a positive value is produced by a RH rotation about the body axis. (NAN if unavailable) +float32[3] delta_angle # (radians) accumulated gyro radians where a positive value is produced by a RH rotation of the sensor about the body axis. (NAN if unavailable) float32 distance_m # (meters) Distance to the center of the flow field (NAN if unavailable) diff --git a/msg/VehicleTrajectoryBezier.msg b/msg/VehicleTrajectoryBezier.msg deleted file mode 100644 index d4bf99b469..0000000000 --- a/msg/VehicleTrajectoryBezier.msg +++ /dev/null @@ -1,18 +0,0 @@ -# Vehicle Waypoints Trajectory description. See also MAVLink MAV_TRAJECTORY_REPRESENTATION msg -# The topic vehicle_trajectory_bezier is used to send a smooth flight path from the -# companion computer / avoidance module to the position controller. - -uint64 timestamp # time since system start (microseconds) - -uint8 POINT_0 = 0 -uint8 POINT_1 = 1 -uint8 POINT_2 = 2 -uint8 POINT_3 = 3 -uint8 POINT_4 = 4 - -uint8 NUMBER_POINTS = 5 - -TrajectoryBezier[5] control_points -uint8 bezier_order - -# TOPICS vehicle_trajectory_bezier diff --git a/msg/VehicleTrajectoryWaypoint.msg b/msg/VehicleTrajectoryWaypoint.msg deleted file mode 100644 index 6bff1cec84..0000000000 --- a/msg/VehicleTrajectoryWaypoint.msg +++ /dev/null @@ -1,21 +0,0 @@ -# Vehicle Waypoints Trajectory description. See also MAVLink MAV_TRAJECTORY_REPRESENTATION msg -# The topic vehicle_trajectory_waypoint_desired is used to send the user desired waypoints from the position controller to the companion computer / avoidance module. -# The topic vehicle_trajectory_waypoint is used to send the adjusted waypoints from the companion computer / avoidance module to the position controller. - -uint64 timestamp # time since system start (microseconds) - -uint8 MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS = 0 - -uint8 type # Type from MAV_TRAJECTORY_REPRESENTATION enum. - -uint8 POINT_0 = 0 -uint8 POINT_1 = 1 -uint8 POINT_2 = 2 -uint8 POINT_3 = 3 -uint8 POINT_4 = 4 - -uint8 NUMBER_POINTS = 5 - -TrajectoryWaypoint[5] waypoints - -# TOPICS vehicle_trajectory_waypoint vehicle_trajectory_waypoint_desired diff --git a/msg/px4_msgs_old/msg/VehicleStatusV0.msg b/msg/px4_msgs_old/msg/VehicleStatusV0.msg new file mode 100644 index 0000000000..a347dfce3d --- /dev/null +++ b/msg/px4_msgs_old/msg/VehicleStatusV0.msg @@ -0,0 +1,140 @@ +# Encodes the system state of the vehicle published by commander + +uint32 MESSAGE_VERSION = 0 + +uint64 timestamp # time since system start (microseconds) + +uint64 armed_time # Arming timestamp (microseconds) +uint64 takeoff_time # Takeoff timestamp (microseconds) + +uint8 arming_state +uint8 ARMING_STATE_DISARMED = 1 +uint8 ARMING_STATE_ARMED = 2 + +uint8 latest_arming_reason +uint8 latest_disarming_reason +uint8 ARM_DISARM_REASON_TRANSITION_TO_STANDBY = 0 +uint8 ARM_DISARM_REASON_STICK_GESTURE = 1 +uint8 ARM_DISARM_REASON_RC_SWITCH = 2 +uint8 ARM_DISARM_REASON_COMMAND_INTERNAL = 3 +uint8 ARM_DISARM_REASON_COMMAND_EXTERNAL = 4 +uint8 ARM_DISARM_REASON_MISSION_START = 5 +uint8 ARM_DISARM_REASON_SAFETY_BUTTON = 6 +uint8 ARM_DISARM_REASON_AUTO_DISARM_LAND = 7 +uint8 ARM_DISARM_REASON_AUTO_DISARM_PREFLIGHT = 8 +uint8 ARM_DISARM_REASON_KILL_SWITCH = 9 +uint8 ARM_DISARM_REASON_LOCKDOWN = 10 +uint8 ARM_DISARM_REASON_FAILURE_DETECTOR = 11 +uint8 ARM_DISARM_REASON_SHUTDOWN = 12 +uint8 ARM_DISARM_REASON_UNIT_TEST = 13 + +uint64 nav_state_timestamp # time when current nav_state activated + +uint8 nav_state_user_intention # Mode that the user selected (might be different from nav_state in a failsafe situation) + +uint8 nav_state # Currently active mode +uint8 NAVIGATION_STATE_MANUAL = 0 # Manual mode +uint8 NAVIGATION_STATE_ALTCTL = 1 # Altitude control mode +uint8 NAVIGATION_STATE_POSCTL = 2 # Position control mode +uint8 NAVIGATION_STATE_AUTO_MISSION = 3 # Auto mission mode +uint8 NAVIGATION_STATE_AUTO_LOITER = 4 # Auto loiter mode +uint8 NAVIGATION_STATE_AUTO_RTL = 5 # Auto return to launch mode +uint8 NAVIGATION_STATE_POSITION_SLOW = 6 +uint8 NAVIGATION_STATE_FREE5 = 7 +uint8 NAVIGATION_STATE_FREE4 = 8 +uint8 NAVIGATION_STATE_FREE3 = 9 +uint8 NAVIGATION_STATE_ACRO = 10 # Acro mode +uint8 NAVIGATION_STATE_FREE2 = 11 +uint8 NAVIGATION_STATE_DESCEND = 12 # Descend mode (no position control) +uint8 NAVIGATION_STATE_TERMINATION = 13 # Termination mode +uint8 NAVIGATION_STATE_OFFBOARD = 14 +uint8 NAVIGATION_STATE_STAB = 15 # Stabilized mode +uint8 NAVIGATION_STATE_FREE1 = 16 +uint8 NAVIGATION_STATE_AUTO_TAKEOFF = 17 # Takeoff +uint8 NAVIGATION_STATE_AUTO_LAND = 18 # Land +uint8 NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19 # Auto Follow +uint8 NAVIGATION_STATE_AUTO_PRECLAND = 20 # Precision land with landing target +uint8 NAVIGATION_STATE_ORBIT = 21 # Orbit in a circle +uint8 NAVIGATION_STATE_AUTO_VTOL_TAKEOFF = 22 # Takeoff, transition, establish loiter +uint8 NAVIGATION_STATE_EXTERNAL1 = 23 +uint8 NAVIGATION_STATE_EXTERNAL2 = 24 +uint8 NAVIGATION_STATE_EXTERNAL3 = 25 +uint8 NAVIGATION_STATE_EXTERNAL4 = 26 +uint8 NAVIGATION_STATE_EXTERNAL5 = 27 +uint8 NAVIGATION_STATE_EXTERNAL6 = 28 +uint8 NAVIGATION_STATE_EXTERNAL7 = 29 +uint8 NAVIGATION_STATE_EXTERNAL8 = 30 +uint8 NAVIGATION_STATE_MAX = 31 + +uint8 executor_in_charge # Current mode executor in charge (0=Autopilot) + +uint32 valid_nav_states_mask # Bitmask for all valid nav_state values +uint32 can_set_nav_states_mask # Bitmask for all modes that a user can select + +# Bitmask of detected failures +uint16 failure_detector_status +uint16 FAILURE_NONE = 0 +uint16 FAILURE_ROLL = 1 # (1 << 0) +uint16 FAILURE_PITCH = 2 # (1 << 1) +uint16 FAILURE_ALT = 4 # (1 << 2) +uint16 FAILURE_EXT = 8 # (1 << 3) +uint16 FAILURE_ARM_ESC = 16 # (1 << 4) +uint16 FAILURE_BATTERY = 32 # (1 << 5) +uint16 FAILURE_IMBALANCED_PROP = 64 # (1 << 6) +uint16 FAILURE_MOTOR = 128 # (1 << 7) + +uint8 hil_state +uint8 HIL_STATE_OFF = 0 +uint8 HIL_STATE_ON = 1 + +# If it's a VTOL, then the value will be VEHICLE_TYPE_ROTARY_WING while flying as a multicopter, and VEHICLE_TYPE_FIXED_WING when flying as a fixed-wing +uint8 vehicle_type +uint8 VEHICLE_TYPE_UNKNOWN = 0 +uint8 VEHICLE_TYPE_ROTARY_WING = 1 +uint8 VEHICLE_TYPE_FIXED_WING = 2 +uint8 VEHICLE_TYPE_ROVER = 3 +uint8 VEHICLE_TYPE_AIRSHIP = 4 + +uint8 FAILSAFE_DEFER_STATE_DISABLED = 0 +uint8 FAILSAFE_DEFER_STATE_ENABLED = 1 +uint8 FAILSAFE_DEFER_STATE_WOULD_FAILSAFE = 2 # Failsafes deferred, but would trigger a failsafe + +bool failsafe # true if system is in failsafe state (e.g.:RTL, Hover, Terminate, ...) +bool failsafe_and_user_took_over # true if system is in failsafe state but the user took over control +uint8 failsafe_defer_state # one of FAILSAFE_DEFER_STATE_* + +# Link loss +bool gcs_connection_lost # datalink to GCS lost +uint8 gcs_connection_lost_counter # counts unique GCS connection lost events +bool high_latency_data_link_lost # Set to true if the high latency data link (eg. RockBlock Iridium 9603 telemetry module) is lost + +# VTOL flags +bool is_vtol # True if the system is VTOL capable +bool is_vtol_tailsitter # True if the system performs a 90° pitch down rotation during transition from MC to FW +bool in_transition_mode # True if VTOL is doing a transition +bool in_transition_to_fw # True if VTOL is doing a transition from MC to FW + +# MAVLink identification +uint8 system_type # system type, contains mavlink MAV_TYPE +uint8 system_id # system id, contains MAVLink's system ID field +uint8 component_id # subsystem / component id, contains MAVLink's component ID field + +bool safety_button_available # Set to true if a safety button is connected +bool safety_off # Set to true if safety is off + +bool power_input_valid # set if input power is valid +bool usb_connected # set to true (never cleared) once telemetry received from usb link + +bool open_drone_id_system_present +bool open_drone_id_system_healthy + +bool parachute_system_present +bool parachute_system_healthy + +bool avoidance_system_required # Set to true if avoidance system is enabled via COM_OBS_AVOID parameter +bool avoidance_system_valid # Status of the obstacle avoidance system + +bool rc_calibration_in_progress +bool calibration_enabled + +bool pre_flight_checks_pass # true if all checks necessary to arm pass diff --git a/msg/translation_node/translations/all_translations.h b/msg/translation_node/translations/all_translations.h index 2b3c2030b4..4369a3c58a 100644 --- a/msg/translation_node/translations/all_translations.h +++ b/msg/translation_node/translations/all_translations.h @@ -9,3 +9,5 @@ //#include "example_translation_direct_v1.h" //#include "example_translation_multi_v2.h" //#include "example_translation_service_v1.h" + +#include "translation_vehicle_status_v1.h" diff --git a/msg/translation_node/translations/translation_vehicle_status_v1.h b/msg/translation_node/translations/translation_vehicle_status_v1.h new file mode 100644 index 0000000000..2041b013d8 --- /dev/null +++ b/msg/translation_node/translations/translation_vehicle_status_v1.h @@ -0,0 +1,110 @@ +/**************************************************************************** + * Copyright (c) 2025 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +// Translate VehicleStatus v0 <--> v1 +#include +#include + +class VehicleStatusV1Translation { +public: + using MessageOlder = px4_msgs_old::msg::VehicleStatusV0; + static_assert(MessageOlder::MESSAGE_VERSION == 0); + + using MessageNewer = px4_msgs::msg::VehicleStatus; + static_assert(MessageNewer::MESSAGE_VERSION == 1); + + static constexpr const char* kTopic = "fmu/out/vehicle_status"; + + static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) { + // Set msg_newer from msg_older + msg_newer.timestamp = msg_older.timestamp; + msg_newer.armed_time = msg_older.armed_time; + msg_newer.takeoff_time = msg_older.takeoff_time; + msg_newer.arming_state = msg_older.arming_state; + msg_newer.latest_arming_reason = msg_older.latest_arming_reason; + msg_newer.latest_disarming_reason = msg_older.latest_disarming_reason; + msg_newer.nav_state_timestamp = msg_older.nav_state_timestamp; + msg_newer.nav_state_user_intention = msg_older.nav_state_user_intention; + msg_newer.nav_state = msg_older.nav_state; + msg_newer.executor_in_charge = msg_older.executor_in_charge; + msg_newer.valid_nav_states_mask = msg_older.valid_nav_states_mask; + msg_newer.can_set_nav_states_mask = msg_older.can_set_nav_states_mask; + msg_newer.failure_detector_status = msg_older.failure_detector_status; + msg_newer.hil_state = msg_older.hil_state; + msg_newer.vehicle_type = msg_older.vehicle_type; + msg_newer.failsafe = msg_older.failsafe; + msg_newer.failsafe_and_user_took_over = msg_older.failsafe_and_user_took_over; + msg_newer.failsafe_defer_state = msg_older.failsafe_defer_state; + msg_newer.gcs_connection_lost = msg_older.gcs_connection_lost; + msg_newer.gcs_connection_lost_counter = msg_older.gcs_connection_lost_counter; + msg_newer.high_latency_data_link_lost = msg_older.high_latency_data_link_lost; + msg_newer.is_vtol = msg_older.is_vtol; + msg_newer.is_vtol_tailsitter = msg_older.is_vtol_tailsitter; + msg_newer.in_transition_mode = msg_older.in_transition_mode; + msg_newer.in_transition_to_fw = msg_older.in_transition_to_fw; + msg_newer.system_type = msg_older.system_type; + msg_newer.system_id = msg_older.system_id; + msg_newer.component_id = msg_older.component_id; + msg_newer.safety_button_available = msg_older.safety_button_available; + msg_newer.safety_off = msg_older.safety_off; + msg_newer.power_input_valid = msg_older.power_input_valid; + msg_newer.usb_connected = msg_older.usb_connected; + msg_newer.open_drone_id_system_present = msg_older.open_drone_id_system_present; + msg_newer.open_drone_id_system_healthy = msg_older.open_drone_id_system_healthy; + msg_newer.parachute_system_present = msg_older.parachute_system_present; + msg_newer.parachute_system_healthy = msg_older.parachute_system_healthy; + msg_newer.rc_calibration_in_progress = msg_older.rc_calibration_in_progress; + msg_newer.calibration_enabled = msg_older.calibration_enabled; + msg_newer.pre_flight_checks_pass = msg_older.pre_flight_checks_pass; + } + + static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) { + // Set msg_older from msg_newer + msg_older.timestamp = msg_newer.timestamp; + msg_older.armed_time = msg_newer.armed_time; + msg_older.takeoff_time = msg_newer.takeoff_time; + msg_older.arming_state = msg_newer.arming_state; + msg_older.latest_arming_reason = msg_newer.latest_arming_reason; + msg_older.latest_disarming_reason = msg_newer.latest_disarming_reason; + msg_older.nav_state_timestamp = msg_newer.nav_state_timestamp; + msg_older.nav_state_user_intention = msg_newer.nav_state_user_intention; + msg_older.nav_state = msg_newer.nav_state; + msg_older.executor_in_charge = msg_newer.executor_in_charge; + msg_older.valid_nav_states_mask = msg_newer.valid_nav_states_mask; + msg_older.can_set_nav_states_mask = msg_newer.can_set_nav_states_mask; + msg_older.failure_detector_status = msg_newer.failure_detector_status; + msg_older.hil_state = msg_newer.hil_state; + msg_older.vehicle_type = msg_newer.vehicle_type; + msg_older.failsafe = msg_newer.failsafe; + msg_older.failsafe_and_user_took_over = msg_newer.failsafe_and_user_took_over; + msg_older.failsafe_defer_state = msg_newer.failsafe_defer_state; + msg_older.gcs_connection_lost = msg_newer.gcs_connection_lost; + msg_older.gcs_connection_lost_counter = msg_newer.gcs_connection_lost_counter; + msg_older.high_latency_data_link_lost = msg_newer.high_latency_data_link_lost; + msg_older.is_vtol = msg_newer.is_vtol; + msg_older.is_vtol_tailsitter = msg_newer.is_vtol_tailsitter; + msg_older.in_transition_mode = msg_newer.in_transition_mode; + msg_older.in_transition_to_fw = msg_newer.in_transition_to_fw; + msg_older.system_type = msg_newer.system_type; + msg_older.system_id = msg_newer.system_id; + msg_older.component_id = msg_newer.component_id; + msg_older.safety_button_available = msg_newer.safety_button_available; + msg_older.safety_off = msg_newer.safety_off; + msg_older.power_input_valid = msg_newer.power_input_valid; + msg_older.usb_connected = msg_newer.usb_connected; + msg_older.open_drone_id_system_present = msg_newer.open_drone_id_system_present; + msg_older.open_drone_id_system_healthy = msg_newer.open_drone_id_system_healthy; + msg_older.parachute_system_present = msg_newer.parachute_system_present; + msg_older.parachute_system_healthy = msg_newer.parachute_system_healthy; + msg_older.avoidance_system_required = false; + msg_older.avoidance_system_valid = false; + msg_older.rc_calibration_in_progress = msg_newer.rc_calibration_in_progress; + msg_older.calibration_enabled = msg_newer.calibration_enabled; + msg_older.pre_flight_checks_pass = msg_newer.pre_flight_checks_pass; + } +}; + +REGISTER_TOPIC_TRANSLATION_DIRECT(VehicleStatusV1Translation); diff --git a/msg/versioned/ArmingCheckReply.msg b/msg/versioned/ArmingCheckReply.msg index 1a1aaa5183..4c1616d849 100644 --- a/msg/versioned/ArmingCheckReply.msg +++ b/msg/versioned/ArmingCheckReply.msg @@ -6,7 +6,6 @@ uint8 request_id uint8 registration_id uint8 HEALTH_COMPONENT_INDEX_NONE = 0 -uint8 HEALTH_COMPONENT_INDEX_AVOIDANCE = 19 uint8 health_component_index # HEALTH_COMPONENT_INDEX_* bool health_component_is_present diff --git a/msg/versioned/BatteryStatus.msg b/msg/versioned/BatteryStatus.msg index 9838786f3f..852db501df 100644 --- a/msg/versioned/BatteryStatus.msg +++ b/msg/versioned/BatteryStatus.msg @@ -12,9 +12,9 @@ float32 time_remaining_s # predicted time in seconds remaining until battery i float32 temperature # Temperature of the battery in degrees Celcius, NaN if unknown uint8 cell_count # Number of cells, 0 if unknown -uint8 BATTERY_SOURCE_POWER_MODULE = 0 -uint8 BATTERY_SOURCE_EXTERNAL = 1 -uint8 BATTERY_SOURCE_ESCS = 2 +uint8 SOURCE_POWER_MODULE = 0 +uint8 SOURCE_EXTERNAL = 1 +uint8 SOURCE_ESCS = 2 uint8 source # Battery source uint8 priority # Zero based priority is the connection on the Power Controller V1..Vn AKA BrickN-1 uint16 capacity # actual capacity of the battery @@ -34,26 +34,26 @@ bool is_powering_off # Power off event imminent indication, false if unknown bool is_required # Set if the battery is explicitly required before arming -uint8 BATTERY_WARNING_NONE = 0 # no battery low voltage warning active -uint8 BATTERY_WARNING_LOW = 1 # warning of low voltage -uint8 BATTERY_WARNING_CRITICAL = 2 # critical voltage, return / abort immediately -uint8 BATTERY_WARNING_EMERGENCY = 3 # immediate landing required -uint8 BATTERY_WARNING_FAILED = 4 # the battery has failed completely -uint8 BATTERY_STATE_UNHEALTHY = 6 # Battery is diagnosed to be defective or an error occurred, usage is discouraged / prohibited. Possible causes (faults) are listed in faults field. -uint8 BATTERY_STATE_CHARGING = 7 # Battery is charging +uint8 WARNING_NONE = 0 # no battery low voltage warning active +uint8 WARNING_LOW = 1 # warning of low voltage +uint8 WARNING_CRITICAL = 2 # critical voltage, return / abort immediately +uint8 WARNING_EMERGENCY = 3 # immediate landing required +uint8 WARNING_FAILED = 4 # the battery has failed completely +uint8 STATE_UNHEALTHY = 6 # Battery is diagnosed to be defective or an error occurred, usage is discouraged / prohibited. Possible causes (faults) are listed in faults field. +uint8 STATE_CHARGING = 7 # Battery is charging -uint8 BATTERY_FAULT_DEEP_DISCHARGE = 0 # Battery has deep discharged -uint8 BATTERY_FAULT_SPIKES = 1 # Voltage spikes -uint8 BATTERY_FAULT_CELL_FAIL= 2 # One or more cells have failed -uint8 BATTERY_FAULT_OVER_CURRENT = 3 # Over-current -uint8 BATTERY_FAULT_OVER_TEMPERATURE = 4 # Over-temperature -uint8 BATTERY_FAULT_UNDER_TEMPERATURE = 5 # Under-temperature fault -uint8 BATTERY_FAULT_INCOMPATIBLE_VOLTAGE = 6 # Vehicle voltage is not compatible with this battery (batteries on same power rail should have similar voltage). -uint8 BATTERY_FAULT_INCOMPATIBLE_FIRMWARE = 7 # Battery firmware is not compatible with current autopilot firmware -uint8 BATTERY_FAULT_INCOMPATIBLE_MODEL = 8 # Battery model is not supported by the system -uint8 BATTERY_FAULT_HARDWARE_FAILURE = 9 # hardware problem -uint8 BATTERY_FAULT_FAILED_TO_ARM = 10 # Battery had a problem while arming -uint8 BATTERY_FAULT_COUNT = 11 # Counter - keep it as last element! +uint8 FAULT_DEEP_DISCHARGE = 0 # Battery has deep discharged +uint8 FAULT_SPIKES = 1 # Voltage spikes +uint8 FAULT_CELL_FAIL= 2 # One or more cells have failed +uint8 FAULT_OVER_CURRENT = 3 # Over-current +uint8 FAULT_OVER_TEMPERATURE = 4 # Over-temperature +uint8 FAULT_UNDER_TEMPERATURE = 5 # Under-temperature fault +uint8 FAULT_INCOMPATIBLE_VOLTAGE = 6 # Vehicle voltage is not compatible with this battery (batteries on same power rail should have similar voltage). +uint8 FAULT_INCOMPATIBLE_FIRMWARE = 7 # Battery firmware is not compatible with current autopilot firmware +uint8 FAULT_INCOMPATIBLE_MODEL = 8 # Battery model is not supported by the system +uint8 FAULT_HARDWARE_FAILURE = 9 # hardware problem +uint8 FAULT_FAILED_TO_ARM = 10 # Battery had a problem while arming +uint8 FAULT_COUNT = 11 # Counter - keep it as last element! uint16 faults # Smart battery supply status/fault flags (bitmask) for health indication. uint8 warning # Current battery warning diff --git a/msg/versioned/VehicleStatus.msg b/msg/versioned/VehicleStatus.msg index 6f3938a0af..7ad58bc785 100644 --- a/msg/versioned/VehicleStatus.msg +++ b/msg/versioned/VehicleStatus.msg @@ -1,6 +1,6 @@ # Encodes the system state of the vehicle published by commander -uint32 MESSAGE_VERSION = 0 +uint32 MESSAGE_VERSION = 1 uint64 timestamp # time since system start (microseconds) @@ -87,15 +87,14 @@ uint8 hil_state uint8 HIL_STATE_OFF = 0 uint8 HIL_STATE_ON = 1 -# If it's a VTOL, then the value will be VEHICLE_TYPE_ROTARY_WING while flying as a multicopter, and VEHICLE_TYPE_FIXED_WING when flying as a fixed-wing +# Current vehicle locomotion method. A vehicle can have different methods (e.g. VTOL transitions from RW to FW method) uint8 vehicle_type -uint8 VEHICLE_TYPE_UNKNOWN = 0 -uint8 VEHICLE_TYPE_ROTARY_WING = 1 -uint8 VEHICLE_TYPE_FIXED_WING = 2 -uint8 VEHICLE_TYPE_ROVER = 3 -uint8 VEHICLE_TYPE_AIRSHIP = 4 +uint8 VEHICLE_TYPE_ROTARY_WING = 0 +uint8 VEHICLE_TYPE_FIXED_WING = 1 +uint8 VEHICLE_TYPE_ROVER = 2 uint8 VEHICLE_TYPE_BOAT = 5 + uint8 FAILSAFE_DEFER_STATE_DISABLED = 0 uint8 FAILSAFE_DEFER_STATE_ENABLED = 1 uint8 FAILSAFE_DEFER_STATE_WOULD_FAILSAFE = 2 # Failsafes deferred, but would trigger a failsafe @@ -132,9 +131,6 @@ bool open_drone_id_system_healthy bool parachute_system_present bool parachute_system_healthy -bool avoidance_system_required # Set to true if avoidance system is enabled via COM_OBS_AVOID parameter -bool avoidance_system_valid # Status of the obstacle avoidance system - bool rc_calibration_in_progress bool calibration_enabled diff --git a/platforms/common/include/px4_platform_common/defines.h b/platforms/common/include/px4_platform_common/defines.h index 3b420f95f1..eba5e944d9 100644 --- a/platforms/common/include/px4_platform_common/defines.h +++ b/platforms/common/include/px4_platform_common/defines.h @@ -97,7 +97,7 @@ __BEGIN_DECLS extern long PX4_TICKS_PER_SEC; __END_DECLS -#define PX4_ROOTFSDIR CONFIG_BOARD_ROOTFSDIR +#define PX4_ROOTFSDIR CONFIG_BOARD_ROOT_PATH // Qurt doesn't have an SD card for storage #ifndef __PX4_QURT diff --git a/platforms/nuttx/NuttX/nuttx b/platforms/nuttx/NuttX/nuttx index 9a213327fb..205b3100f8 160000 --- a/platforms/nuttx/NuttX/nuttx +++ b/platforms/nuttx/NuttX/nuttx @@ -1 +1 @@ -Subproject commit 9a213327fbb2dab252185d468fb29348e1833d8c +Subproject commit 205b3100f8f63944a45faa5cfb5d3f86e904ee59 diff --git a/platforms/posix/cmake/sitl_tests.cmake b/platforms/posix/cmake/sitl_tests.cmake index bf69b6a7ce..fdd8a5300e 100644 --- a/platforms/posix/cmake/sitl_tests.cmake +++ b/platforms/posix/cmake/sitl_tests.cmake @@ -5,7 +5,6 @@ # tests command arguments set(tests atomic_bitset - bezier bitset bson dataman diff --git a/src/drivers/actuators/voxl_esc/voxl_esc.cpp b/src/drivers/actuators/voxl_esc/voxl_esc.cpp index f5568d7eeb..89aebcbbaf 100644 --- a/src/drivers/actuators/voxl_esc/voxl_esc.cpp +++ b/src/drivers/actuators/voxl_esc/voxl_esc.cpp @@ -47,7 +47,7 @@ VoxlEsc::VoxlEsc() : _mixing_output{"VOXL_ESC", VOXL_ESC_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}, _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")), _output_update_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": output update interval")), - _battery(1, nullptr, _battery_report_interval, battery_status_s::BATTERY_SOURCE_POWER_MODULE) + _battery(1, nullptr, _battery_report_interval, battery_status_s::SOURCE_POWER_MODULE) { _device = VOXL_ESC_DEFAULT_PORT; @@ -90,13 +90,13 @@ VoxlEsc::~VoxlEsc() int VoxlEsc::init() { - PX4_INFO("VOXL_ESC: Starting VOXL ESC driver"); + PX4_ERR("Starting VOXL ESC driver"); /* Getting initial parameter values */ int ret = update_params(); if (ret != OK) { - PX4_ERR("VOXL_ESC: Failed to update params during init"); + PX4_ERR("Failed to update params during init"); return ret; } @@ -124,13 +124,13 @@ int VoxlEsc::device_init() // Open serial port if (!_uart_port.isOpen()) { - PX4_INFO("VOXL_ESC: Opening UART ESC device %s, baud rate %" PRIi32, _device, _parameters.baud_rate); + PX4_ERR("Opening UART ESC device %s, baud rate %" PRIi32, _device, _parameters.baud_rate); #ifndef __PX4_QURT //warn user that unless DMA is enabled for UART RX, data can be lost due to high frequency of per char cpu interrupts //at least at 2mbit, there are definitely losses, did not test other baud rates to find the cut off if (_parameters.baud_rate > 250000) { - PX4_WARN("VOXL_ESC: Baud rate is too high for non-DMA based UART, this can lead to loss of RX data"); + PX4_WARN("Baud rate is too high for non-DMA based UART, this can lead to loss of RX data"); } #endif @@ -165,7 +165,7 @@ int VoxlEsc::device_init() } // Detect ESCs - PX4_INFO("VOXL_ESC: Detecting ESCs..."); + PX4_ERR("Detecting ESCs..."); qc_esc_packet_init(&_fb_packet); //request extended version info from each ESC and wait for reply @@ -174,7 +174,7 @@ int VoxlEsc::device_init() cmd.len = qc_esc_create_extended_version_request_packet(esc_id, cmd.buf, sizeof(cmd.buf)); if (_uart_port.write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("VOXL_ESC: Could not write version request packet to UART port"); + PX4_ERR("Could not write version request packet to UART port"); return -1; } @@ -201,17 +201,17 @@ int VoxlEsc::device_init() QC_ESC_EXTENDED_VERSION_INFO ver; memcpy(&ver, _fb_packet.buffer, packet_size); - PX4_INFO("VOXL_ESC: \tESC ID : %i", ver.id); - PX4_INFO("VOXL_ESC: \tBoard Type : %i: %s", ver.hw_version, board_id_to_name(ver.hw_version)); + PX4_ERR("\tESC ID : %i", ver.id); + PX4_ERR("\tBoard Type : %i: %s", ver.hw_version, board_id_to_name(ver.hw_version)); uint8_t *u = &ver.unique_id[0]; - PX4_INFO("VOXL_ESC: \tUnique ID : 0x%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X", - u[11], u[10], u[9], u[8], u[7], u[6], u[5], u[4], u[3], u[2], u[1], u[0]); + PX4_ERR("\tUnique ID : 0x%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X", + u[11], u[10], u[9], u[8], u[7], u[6], u[5], u[4], u[3], u[2], u[1], u[0]); - PX4_INFO("VOXL_ESC: \tFirmware : version %4d, hash %.12s", ver.sw_version, ver.firmware_git_version); - PX4_INFO("VOXL_ESC: \tBootloader : version %4d, hash %.12s", ver.bootloader_version, ver.bootloader_git_version); - PX4_INFO("VOXL_ESC: \tReply time : %" PRIu32 "us", (uint32_t)response_time); - PX4_INFO("VOXL_ESC:"); + PX4_ERR("\tFirmware : version %4d, hash %.12s", ver.sw_version, ver.firmware_git_version); + PX4_ERR("\tBootloader : version %4d, hash %.12s", ver.bootloader_version, ver.bootloader_git_version); + PX4_ERR("\tReply time : %" PRIu32 "us", (uint32_t)response_time); + PX4_INFO(""); if (ver.id == esc_id) { memcpy(&_version_info[esc_id], &ver, sizeof(ver)); @@ -223,7 +223,7 @@ int VoxlEsc::device_init() } if (!got_response) { - PX4_ERR("VOXL_ESC: ESC %d version info response timeout", esc_id); + PX4_ERR("ESC %d version info response timeout", esc_id); } } @@ -232,7 +232,7 @@ int VoxlEsc::device_init() for (int esc_id = 0; esc_id < VOXL_ESC_OUTPUT_CHANNELS; esc_id++) { if (_version_info[esc_id].sw_version == 0) { - PX4_ERR("VOXL_ESC: ESC ID %d was not detected", esc_id); + PX4_ERR("ESC ID %d was not detected", esc_id); esc_detection_fault = true; } } @@ -240,7 +240,7 @@ int VoxlEsc::device_init() //check the firmware hashes to make sure they are the same. Firmware hash has 8 chars plus optional "*" for (int esc_id = 1; esc_id < VOXL_ESC_OUTPUT_CHANNELS; esc_id++) { if (strncmp(_version_info[0].firmware_git_version, _version_info[esc_id].firmware_git_version, 9) != 0) { - PX4_ERR("VOXL_ESC: ESC %d Firmware hash does not match ESC 0 firmware hash: (%.12s) != (%.12s)", + PX4_ERR("ESC %d Firmware hash does not match ESC 0 firmware hash: (%.12s) != (%.12s)", esc_id, _version_info[esc_id].firmware_git_version, _version_info[0].firmware_git_version); esc_detection_fault = true; } @@ -256,13 +256,13 @@ int VoxlEsc::device_init() } if (esc_detection_fault) { - PX4_ERR("VOXL_ESC: Critical error during ESC initialization"); + PX4_ERR("Critical error during ESC initialization"); return -1; } - PX4_INFO("VOXL_ESC: Use extened rpm packet : %d", _extended_rpm); + PX4_ERR("Use extened rpm packet : %d", _extended_rpm); - PX4_INFO("VOXL_ESC: All ESCs successfully detected"); + PX4_ERR("All ESCs successfully detected"); _device_initialized = true; @@ -309,45 +309,53 @@ int VoxlEsc::load_params(voxl_esc_params_t *params, ch_assign_t *map) param_get(param_find("VOXL_ESC_T_WARN"), ¶ms->esc_warn_temp_threshold); param_get(param_find("VOXL_ESC_T_OVER"), ¶ms->esc_over_temp_threshold); + param_get(param_find("VOXL_ESC_GPIO_CH"), ¶ms->gpio_ctl_channel); + if (params->rpm_min >= params->rpm_max) { - PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_RPM_MIN. Please verify parameters."); + PX4_ERR("Invalid parameter VOXL_ESC_RPM_MIN. Please verify parameters."); params->rpm_min = 0; ret = PX4_ERROR; } if (params->turtle_motor_percent < 0 || params->turtle_motor_percent > 100) { - PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_T_PERC. Please verify parameters."); + PX4_ERR("Invalid parameter VOXL_ESC_T_PERC. Please verify parameters."); params->turtle_motor_percent = 0; ret = PX4_ERROR; } if (params->turtle_motor_deadband < 0 || params->turtle_motor_deadband > 100) { - PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_T_DEAD. Please verify parameters."); + PX4_ERR("Invalid parameter VOXL_ESC_T_DEAD. Please verify parameters."); params->turtle_motor_deadband = 0; ret = PX4_ERROR; } if (params->turtle_motor_expo < 0 || params->turtle_motor_expo > 100) { - PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_T_EXPO. Please verify parameters."); + PX4_ERR("Invalid parameter VOXL_ESC_T_EXPO. Please verify parameters."); params->turtle_motor_expo = 0; ret = PX4_ERROR; } if (params->turtle_stick_minf < 0.0f || params->turtle_stick_minf > 100.0f) { - PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_T_MINF. Please verify parameters."); + PX4_ERR("Invalid parameter VOXL_ESC_T_MINF. Please verify parameters."); params->turtle_stick_minf = 0.0f; ret = PX4_ERROR; } if (params->turtle_cosphi < 0.0f || params->turtle_cosphi > 100.0f) { - PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_T_COSP. Please verify parameters."); + PX4_ERR("Invalid parameter VOXL_ESC_T_COSP. Please verify parameters."); params->turtle_cosphi = 0.0f; ret = PX4_ERROR; } + if (params->gpio_ctl_channel < 0 || params->gpio_ctl_channel > 6) { + PX4_ERR("Invalid parameter VOXL_ESC_GPIO_CH. Please verify parameters."); + params->gpio_ctl_channel = 0; + ret = PX4_ERROR; + } + for (int i = 0; i < VOXL_ESC_OUTPUT_CHANNELS; i++) { if (params->function_map[i] < (int)OutputFunction::Motor1 || params->function_map[i] > (int)OutputFunction::Motor4) { - PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_FUNCX. Only supports motors 1-4. Please verify parameters."); + PX4_ERR("Invalid parameter VOXL_ESC_FUNCX. Only supports motors 1-4. Please verify parameters."); params->function_map[i] = 0; ret = PX4_ERROR; @@ -365,7 +373,7 @@ int VoxlEsc::load_params(voxl_esc_params_t *params, ch_assign_t *map) if (params->motor_map[i] == VOXL_ESC_OUTPUT_DISABLED || params->motor_map[i] < -(VOXL_ESC_OUTPUT_CHANNELS) || params->motor_map[i] > VOXL_ESC_OUTPUT_CHANNELS) { - PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_MOTORX. Please verify parameters."); + PX4_ERR("Invalid parameter VOXL_ESC_MOTORX. Please verify parameters."); params->motor_map[i] = 0; ret = PX4_ERROR; } @@ -471,9 +479,8 @@ int VoxlEsc::parse_response(uint8_t *buf, uint8_t len, bool print_feedback) uint32_t voltage = fb.voltage; int32_t current = fb.current * 8; int32_t temperature = fb.temperature / 100; - PX4_INFO("VOXL_ESC: [%" PRId64 "] ID_RAW=%d ID=%d, RPM=%5d, PWR=%3d%%, V=%5dmV, I=%+5dmA, T=%+3dC", tnow, (int)id, - motor_idx + 1, - (int)rpm, (int)power, (int)voltage, (int)current, (int)temperature); + PX4_ERR("[%" PRId64 "] ID_RAW=%d ID=%d, RPM=%5d, PWR=%3d%%, V=%5dmV, I=%+5dmA, T=%+3dC", tnow, (int)id, + motor_idx + 1, (int)rpm, (int)power, (int)voltage, (int)current, (int)temperature); } _esc_chans[id].rate_meas = fb.rpm; @@ -541,24 +548,24 @@ int VoxlEsc::parse_response(uint8_t *buf, uint8_t len, bool print_feedback) QC_ESC_VERSION_INFO ver; memcpy(&ver, _fb_packet.buffer, packet_size); - PX4_INFO("VOXL_ESC: ESC ID: %i", ver.id); - PX4_INFO("VOXL_ESC: HW Version: %i", ver.hw_version); - PX4_INFO("VOXL_ESC: SW Version: %i", ver.sw_version); - PX4_INFO("VOXL_ESC: Unique ID: %i", (int)ver.unique_id); + PX4_ERR("ESC ID: %i", ver.id); + PX4_ERR("HW Version: %i", ver.hw_version); + PX4_ERR("SW Version: %i", ver.sw_version); + PX4_ERR("Unique ID: %i", (int)ver.unique_id); } else if (packet_type == ESC_PACKET_TYPE_VERSION_EXT_RESPONSE && packet_size == sizeof(QC_ESC_EXTENDED_VERSION_INFO)) { QC_ESC_EXTENDED_VERSION_INFO ver; memcpy(&ver, _fb_packet.buffer, packet_size); - PX4_INFO("VOXL_ESC: \tESC ID : %i", ver.id); - PX4_INFO("VOXL_ESC: \tBoard : %i", ver.hw_version); - PX4_INFO("VOXL_ESC: \tSW Version : %i", ver.sw_version); + PX4_ERR("\tESC ID : %i", ver.id); + PX4_ERR("\tBoard : %i", ver.hw_version); + PX4_ERR("\tSW Version : %i", ver.sw_version); uint8_t *u = &ver.unique_id[0]; - PX4_INFO("VOXL_ESC: \tUnique ID : 0x%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X", - u[11], u[10], u[9], u[8], u[7], u[6], u[5], u[4], u[3], u[2], u[1], u[0]); + PX4_ERR("\tUnique ID : 0x%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X", + u[11], u[10], u[9], u[8], u[7], u[6], u[5], u[4], u[3], u[2], u[1], u[0]); - PX4_INFO("VOXL_ESC: \tFirmware : version %4d, hash %.12s", ver.sw_version, ver.firmware_git_version); - PX4_INFO("VOXL_ESC: \tBootloader : version %4d, hash %.12s", ver.bootloader_version, ver.bootloader_git_version); + PX4_ERR("\tFirmware : version %4d, hash %.12s", ver.sw_version, ver.firmware_git_version); + PX4_ERR("\tBootloader : version %4d, hash %.12s", ver.bootloader_version, ver.bootloader_git_version); } else if (packet_type == ESC_PACKET_TYPE_FB_POWER_STATUS && packet_size == sizeof(QC_ESC_FB_POWER_STATUS)) { QC_ESC_FB_POWER_STATUS packet; @@ -671,7 +678,7 @@ int VoxlEsc::custom_command(int argc, char *argv[]) } if (!is_running()) { - PX4_INFO("VOXL_ESC:Not running"); + PX4_INFO("Not running"); return -1; } @@ -730,7 +737,7 @@ int VoxlEsc::custom_command(int argc, char *argv[]) if (!strcmp(verb, "reset")) { if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) { - PX4_INFO("VOXL_ESC: Reset ESC: %i", esc_id); + PX4_ERR("Reset ESC: %i", esc_id); cmd.len = qc_esc_create_reset_packet(esc_id, cmd.buf, sizeof(cmd.buf)); cmd.response = false; return get_instance()->send_cmd_thread_safe(&cmd); @@ -742,7 +749,7 @@ int VoxlEsc::custom_command(int argc, char *argv[]) } else if (!strcmp(verb, "version")) { if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) { - PX4_INFO("VOXL_ESC: Request version for ESC: %i", esc_id); + PX4_ERR("Request version for ESC: %i", esc_id); cmd.len = qc_esc_create_version_request_packet(esc_id, cmd.buf, sizeof(cmd.buf)); cmd.response = true; cmd.resp_delay_us = 2000; @@ -755,7 +762,7 @@ int VoxlEsc::custom_command(int argc, char *argv[]) } else if (!strcmp(verb, "version-ext")) { if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) { - PX4_INFO("VOXL_ESC: Request extended version for ESC: %i", esc_id); + PX4_ERR("Request extended version for ESC: %i", esc_id); cmd.len = qc_esc_create_extended_version_request_packet(esc_id, cmd.buf, sizeof(cmd.buf)); cmd.response = true; cmd.resp_delay_us = 5000; @@ -768,7 +775,7 @@ int VoxlEsc::custom_command(int argc, char *argv[]) } else if (!strcmp(verb, "tone")) { if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) { - PX4_INFO("VOXL_ESC: Request tone for ESC mask: %i", esc_id); + PX4_ERR("Request tone for ESC mask: %i", esc_id); cmd.len = qc_esc_create_sound_packet(period, duration, power, esc_id, cmd.buf, sizeof(cmd.buf)); cmd.response = false; return get_instance()->send_cmd_thread_safe(&cmd); @@ -782,7 +789,7 @@ int VoxlEsc::custom_command(int argc, char *argv[]) if (led_mask <= 0x0FFF) { get_instance()->_led_rsc.test = true; get_instance()->_led_rsc.breath_en = false; - PX4_INFO("VOXL_ESC: Request LED control for ESCs with mask: %i", led_mask); + PX4_ERR("Request LED control for ESCs with mask: %i", led_mask); get_instance()->_esc_chans[0].led = (led_mask & 0x0007); get_instance()->_esc_chans[1].led = (led_mask & 0x0038) >> 3; @@ -797,7 +804,7 @@ int VoxlEsc::custom_command(int argc, char *argv[]) } else if (!strcmp(verb, "rpm")) { if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) { - PX4_INFO("VOXL_ESC: Request RPM for ESC ID: %i - RPM: %i", esc_id, rate); + PX4_ERR("Request RPM for ESC ID: %i - RPM: %i", esc_id, rate); int16_t rate_req[VOXL_ESC_OUTPUT_CHANNELS] = {0, 0, 0, 0}; uint8_t id_fb = 0; @@ -831,8 +838,8 @@ int VoxlEsc::custom_command(int argc, char *argv[]) cmd.repeat_delay_us = repeat_delay_us; cmd.print_feedback = true; - PX4_INFO("VOXL_ESC: Feedback id debug: %i", id_fb); - PX4_INFO("VOXL_ESC: Sending UART ESC RPM command %i", rate); + PX4_ERR("Feedback id debug: %i", id_fb); + PX4_ERR("Sending UART ESC RPM command %i", rate); return get_instance()->send_cmd_thread_safe(&cmd); @@ -843,7 +850,7 @@ int VoxlEsc::custom_command(int argc, char *argv[]) } else if (!strcmp(verb, "pwm")) { if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) { - PX4_INFO("VOXL_ESC: Request PWM for ESC ID: %i - PWM: %i", esc_id, rate); + PX4_ERR("Request PWM for ESC ID: %i - PWM: %i", esc_id, rate); int16_t rate_req[VOXL_ESC_OUTPUT_CHANNELS] = {0, 0, 0, 0}; uint8_t id_fb = 0; @@ -876,8 +883,8 @@ int VoxlEsc::custom_command(int argc, char *argv[]) cmd.repeat_delay_us = repeat_delay_us; cmd.print_feedback = true; - PX4_INFO("VOXL_ESC: Feedback id debug: %i", id_fb); - PX4_INFO("VOXL_ESC: Sending UART ESC power command %i", rate); + PX4_ERR("Feedback id debug: %i", id_fb); + PX4_ERR("Sending UART ESC power command %i", rate); return get_instance()->send_cmd_thread_safe(&cmd); @@ -1242,14 +1249,58 @@ bool VoxlEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], _extended_rpm); if (_uart_port.write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("VOXL_ESC: Failed to send packet"); + PX4_ERR("Failed to send packet"); return false; } + // Track and manage gpio command writes + bool write_gpio_command = false; + + if (_gpio_ctl_en) { + if (_gpio_ctl_high != _prev_gpio_ctl_high) { + _gpio_write_counter = 0; + } + + if (_gpio_write_counter < 10) { + write_gpio_command = true; + _gpio_write_counter++; + } + + _prev_gpio_ctl_high = _gpio_ctl_high; + + if (write_gpio_command) { + Command gpio_cmd; + const int ESC_PACKET_TYPE_GPIO_CMD = 15; + uint8_t data[5]; + + int esc_id = 0; // In future un-hardcode + int val = 0; + + if (_gpio_ctl_high) { + val = 1; + } + + data[0] = esc_id; // esc id + data[1] = 80; // 01010000 : pin F0 + data[2] = 0; // 0: output, 1: input + data[3] = val; //cmd LSB + data[4] = 0; // cmd MSB + + // type, data, size + gpio_cmd.len = qc_esc_create_packet(ESC_PACKET_TYPE_GPIO_CMD, (uint8_t *) & (data[0]), 5, gpio_cmd.buf, + sizeof(gpio_cmd.buf)); + + if (_uart_port.write(gpio_cmd.buf, gpio_cmd.len) != gpio_cmd.len) { + PX4_ERR("Failed to send gpio packet"); + return false; + } + } + + } + // increment ESC id from which to request feedback in round robin order _fb_idx = (_fb_idx + 1) % VOXL_ESC_OUTPUT_CHANNELS; - /* * Here we read and parse response from ESCs. Since the latest command has just been sent out, * the response packet we may read here is probabaly from previous iteration, but it is totally ok. @@ -1283,20 +1334,18 @@ bool VoxlEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], _esc_status_pub.publish(_esc_status); - // If any extra external modal io data has been received then - // send it over as well - while (_voxl2_io_data_sub.updated()) { - buffer128_s io_data{}; - _voxl2_io_data_sub.copy(&io_data); + uint8_t num_writes = 0; - // PX4_INFO("Got Modal IO data: %u bytes", io_data.len); - // PX4_INFO(" 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x", - // io_data.data[0], io_data.data[1], io_data.data[2], io_data.data[3], - // io_data.data[4], io_data.data[5], io_data.data[6], io_data.data[7]); - if (_uart_port.write(io_data.data, io_data.len) != io_data.len) { - PX4_ERR("VOXL_ESC: Failed to send modal io data to esc"); + while (_esc_serial_passthru_sub.updated() && (num_writes < 4)) { + mavlink_tunnel_s uart_passthru{}; + _esc_serial_passthru_sub.copy(&uart_passthru); + + if (_uart_port.write(uart_passthru.payload, uart_passthru.payload_length) != uart_passthru.payload_length) { + PX4_ERR("Failed to send mavlink tunnel data to esc"); return false; } + + num_writes++; } perf_count(_output_update_perf); @@ -1308,7 +1357,7 @@ bool VoxlEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], void VoxlEsc::Run() { if (should_exit()) { - PX4_ERR("VOXL_ESC: Stopping the module"); + PX4_ERR("Stopping the module"); ScheduleClear(); _mixing_output.unregister(); @@ -1328,12 +1377,12 @@ void VoxlEsc::Run() int dev_init_ret = device_init(); if (dev_init_ret != 0) { - PX4_WARN("VOXL_ESC: Failed to initialize device, retries left %d", retries_left); + PX4_WARN("Failed to initialize device, retries left %d", retries_left); } } if (!_device_initialized) { - PX4_ERR("VOXL_ESC: Failed to initialize device, exiting the module"); + PX4_ERR("Failed to initialize device, exiting the module"); ScheduleClear(); _mixing_output.unregister(); exit_and_cleanup(); @@ -1374,11 +1423,12 @@ void VoxlEsc::Run() update_leds(_led_rsc.mode, _led_rsc.control); } - if (_parameters.mode > 0) { - /* if turtle mode enabled, we go straight to the sticks, no mix */ - if (_manual_control_setpoint_sub.updated()) { + /* check whether sticks have been updated */ + if (_manual_control_setpoint_sub.updated()) { + _manual_control_setpoint_sub.copy(&_manual_control_setpoint); - _manual_control_setpoint_sub.copy(&_manual_control_setpoint); + // if turtle mode enabled, we go straight to the sticks, no mix + if (_parameters.mode > 0) { if (!_outputs_on) { @@ -1400,6 +1450,45 @@ void VoxlEsc::Run() } } + // check if gpio control is enabled + if (_parameters.gpio_ctl_channel > 0) { + + _gpio_ctl_en = true; + float gpio_setpoint = VOXL_ESC_GPIO_CTL_DISABLED_SETPOINT; + + switch (_parameters.gpio_ctl_channel) { + case VOXL_ESC_GPIO_CTL_AUX1: + gpio_setpoint = _manual_control_setpoint.aux1; + break; + + case VOXL_ESC_GPIO_CTL_AUX2: + gpio_setpoint = _manual_control_setpoint.aux2; + break; + + case VOXL_ESC_GPIO_CTL_AUX3: + gpio_setpoint = _manual_control_setpoint.aux3; + break; + + case VOXL_ESC_GPIO_CTL_AUX4: + gpio_setpoint = _manual_control_setpoint.aux4; + break; + + case VOXL_ESC_GPIO_CTL_AUX5: + gpio_setpoint = _manual_control_setpoint.aux5; + break; + + case VOXL_ESC_GPIO_CTL_AUX6: + gpio_setpoint = _manual_control_setpoint.aux6; + break; + } + + if (gpio_setpoint > VOXL_ESC_GPIO_CTL_THRESHOLD) { + _gpio_ctl_high = false; + + } else { + _gpio_ctl_high = true; + } + } } if (!_outputs_on) { @@ -1431,19 +1520,19 @@ void VoxlEsc::Run() } else { if (_current_cmd.retries == 0) { _current_cmd.clear(); - PX4_ERR("VOXL_ESC: Failed to send command, errno: %i", errno); + PX4_ERR("Failed to send command, errno: %i", errno); } else { _current_cmd.retries--; - PX4_ERR("VOXL_ESC: Failed to send command, errno: %i", errno); + PX4_ERR("Failed to send command, errno: %i", errno); } } px4_usleep(_current_cmd.repeat_delay_us); } while (_current_cmd.repeats-- > 0); - PX4_INFO("VOXL_ESC: RX packet count: %d", (int)_rx_packet_count); - PX4_INFO("VOXL_ESC: CRC error count: %d", (int)_rx_crc_error_count); + PX4_ERR("RX packet count: %d", (int)_rx_packet_count); + PX4_ERR("CRC error count: %d", (int)_rx_crc_error_count); } else { Command *new_cmd = _pending_cmd.load(); @@ -1547,9 +1636,11 @@ void VoxlEsc::print_params() PX4_INFO("Params: VOXL_ESC_VLOG: %" PRId32, _parameters.verbose_logging); PX4_INFO("Params: VOXL_ESC_PUB_BST: %" PRId32, _parameters.publish_battery_status); - + PX4_INFO("Params: VOXL_ESC_T_WARN: %" PRId32, _parameters.esc_warn_temp_threshold); PX4_INFO("Params: VOXL_ESC_T_OVER: %" PRId32, _parameters.esc_over_temp_threshold); + + PX4_INFO("Params: VOXL_ESC_GPIO_CH: %" PRId32, _parameters.gpio_ctl_channel); } int VoxlEsc::print_status() diff --git a/src/drivers/actuators/voxl_esc/voxl_esc.hpp b/src/drivers/actuators/voxl_esc/voxl_esc.hpp index d894cb9326..c28d1e1ab9 100644 --- a/src/drivers/actuators/voxl_esc/voxl_esc.hpp +++ b/src/drivers/actuators/voxl_esc/voxl_esc.hpp @@ -49,7 +49,7 @@ #include #include #include -#include +#include #include @@ -129,17 +129,24 @@ private: static constexpr uint32_t VOXL_ESC_MODE_TURTLE_AUX1 = 1; static constexpr uint32_t VOXL_ESC_MODE_TURTLE_AUX2 = 2; - static constexpr uint16_t VOXL_ESC_EXT_RPM = - 39; // minimum firmware version for extended RPM command support - static constexpr uint16_t VOXL_ESC_RPM_MAX = INT16_MAX - - 1; // 32K, Limit max standard range RPM to prevent overflow (rpm packet packing function accepts int32_t) - static constexpr uint16_t VOXL_ESC_RPM_MAX_EXT = UINT16_MAX - - 5; // 65K, Limit max extended range RPM to prevent overflow (rpm packet packing function accepts int32_t) + // minimum firmware version for extended RPM command support + static constexpr uint16_t VOXL_ESC_EXT_RPM = 39; + // 32K, Limit max standard range RPM to prevent overflow (rpm packet packing function accepts int32_t) + static constexpr uint16_t VOXL_ESC_RPM_MAX = INT16_MAX - 1; + // 65K, Limit max extended range RPM to prevent overflow (rpm packet packing function accepts int32_t) + static constexpr uint16_t VOXL_ESC_RPM_MAX_EXT = UINT16_MAX - 5; static constexpr uint16_t VOXL_ESC_NUM_INIT_RETRIES = 3; - //static constexpr uint16_t max_pwm(uint16_t pwm) { return math::min(pwm, VOXL_ESC_PWM_MAX); } - //static constexpr uint16_t max_rpm(uint16_t rpm) { return math::min(rpm, VOXL_ESC_RPM_MAX); } + static constexpr float VOXL_ESC_GPIO_CTL_DISABLED_SETPOINT = -0.1f; + static constexpr float VOXL_ESC_GPIO_CTL_THRESHOLD = 0.0f; + + static constexpr uint32_t VOXL_ESC_GPIO_CTL_AUX1 = 1; + static constexpr uint32_t VOXL_ESC_GPIO_CTL_AUX2 = 2; + static constexpr uint32_t VOXL_ESC_GPIO_CTL_AUX3 = 3; + static constexpr uint32_t VOXL_ESC_GPIO_CTL_AUX4 = 4; + static constexpr uint32_t VOXL_ESC_GPIO_CTL_AUX5 = 5; + static constexpr uint32_t VOXL_ESC_GPIO_CTL_AUX6 = 6; Serial _uart_port{}; @@ -161,6 +168,7 @@ private: int32_t publish_battery_status{0}; int32_t esc_warn_temp_threshold{0}; int32_t esc_over_temp_threshold{0}; + int32_t gpio_ctl_channel{0}; } voxl_esc_params_t; struct EscChan { @@ -205,7 +213,7 @@ private: uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _actuator_test_sub{ORB_ID(actuator_test)}; uORB::Subscription _led_update_sub{ORB_ID(led_control)}; - uORB::Subscription _voxl2_io_data_sub{ORB_ID(voxl2_io_data)}; + uORB::Subscription _esc_serial_passthru_sub{ORB_ID(esc_serial_passthru)}; uORB::Publication _outputs_debug_pub{ORB_ID(actuator_outputs_debug)}; uORB::Publication _esc_status_pub{ORB_ID(esc_status)}; @@ -224,6 +232,11 @@ private: int32_t _rpm_fullscale{0}; manual_control_setpoint_s _manual_control_setpoint{}; + int32_t _gpio_write_counter{0}; + bool _gpio_ctl_en{false}; + bool _gpio_ctl_high{true}; + bool _prev_gpio_ctl_high{true}; + uint16_t _cmd_id{0}; Command _current_cmd; px4::atomic _pending_cmd{nullptr}; diff --git a/src/drivers/actuators/voxl_esc/voxl_esc_params.c b/src/drivers/actuators/voxl_esc/voxl_esc_params.c index 2752d9d744..b0526ecf1f 100644 --- a/src/drivers/actuators/voxl_esc/voxl_esc_params.c +++ b/src/drivers/actuators/voxl_esc/voxl_esc_params.c @@ -262,3 +262,17 @@ PARAM_DEFINE_INT32(VOXL_ESC_T_WARN, 0); * @max 200 */ PARAM_DEFINE_INT32(VOXL_ESC_T_OVER, 0); + + +/** + * GPIO Control Channel + * + * + * @reboot_required true + * + * @group VOXL ESC + * @value 0 - Disabled + * @min 0 + * @max 6 + */ +PARAM_DEFINE_INT32(VOXL_ESC_GPIO_CH, 0); diff --git a/src/drivers/adc/ads1115/ADS1115.cpp b/src/drivers/adc/ads1115/ADS1115.cpp index 5ce4ae3504..25c8e92956 100644 --- a/src/drivers/adc/ads1115/ADS1115.cpp +++ b/src/drivers/adc/ads1115/ADS1115.cpp @@ -42,18 +42,7 @@ int ADS1115::init() return ret; } - uint8_t config[2] = {}; - config[0] = CONFIG_HIGH_OS_NOACT | CONFIG_HIGH_MUX_P0NG | CONFIG_HIGH_PGA_6144 | CONFIG_HIGH_MODE_SS; - config[1] = CONFIG_LOW_DR_250SPS | CONFIG_LOW_COMP_MODE_TRADITIONAL | CONFIG_LOW_COMP_POL_RESET | - CONFIG_LOW_COMP_LAT_NONE | CONFIG_LOW_COMP_QU_DISABLE; - ret = writeReg(ADDRESSPOINTER_REG_CONFIG, config, 2); - - if (ret != PX4_OK) { - PX4_ERR("writeReg failed (%i)", ret); - return ret; - } - - setChannel(ADS1115::A0); // prepare for the first measure. + readChannel(Channel::A0); // prepare for the first measure. ScheduleOnInterval(SAMPLE_INTERVAL / 4, SAMPLE_INTERVAL / 4); @@ -62,152 +51,69 @@ int ADS1115::init() int ADS1115::probe() { - uint8_t buf[2] = {}; - int ret = readReg(ADDRESSPOINTER_REG_CONFIG, buf, 2); + // The ADS1115 has no ID register, so we read out the threshold registers + // and check their default values. We cannot use the config register, as + // this is changed by this driver. Note the default value is in BE. + static constexpr uint32_t DEFAULT{0xFF7F0080}; + union { + struct { + uint8_t low[2]; + uint8_t high[2]; + } parts; + uint32_t threshold{}; + }; + int ret = readReg(ADDRESSPOINTER_REG_LO_THRESH, parts.low, 2); if (ret != PX4_OK) { - DEVICE_DEBUG("readReg failed (%i)", ret); + DEVICE_DEBUG("lo_thresh read failed (%i)", ret); return ret; } - if (buf[0] != CONFIG_RESET_VALUE_HIGH || buf[1] != CONFIG_RESET_VALUE_LOW) { - DEVICE_DEBUG("ADS1115 not found"); - return PX4_ERROR; + ret = readReg(ADDRESSPOINTER_REG_HI_THRESH, parts.high, 2); + + if (ret != PX4_OK) { + DEVICE_DEBUG("hi_thresh read failed (%i)", ret); + return ret; } - return PX4_OK; + if (threshold == DEFAULT) { + return PX4_OK; + } + + DEVICE_DEBUG("ADS1115 not found"); + return PX4_ERROR; } -int ADS1115::setChannel(ADS1115::ChannelSelection ch) +int ADS1115::readChannel(ADS1115::Channel ch) { - uint8_t buf[2] = {}; - uint8_t next_mux_reg = CONFIG_HIGH_MUX_P0NG; - - switch (ch) { - case A0: - next_mux_reg = CONFIG_HIGH_MUX_P0NG; - break; - - case A1: - next_mux_reg = CONFIG_HIGH_MUX_P1NG; - break; - - case A2: - next_mux_reg = CONFIG_HIGH_MUX_P2NG; - break; - - case A3: - next_mux_reg = CONFIG_HIGH_MUX_P3NG; - break; - - default: - assert(false); - break; - } - - buf[0] = CONFIG_HIGH_OS_START_SINGLE | next_mux_reg | CONFIG_HIGH_PGA_6144 | CONFIG_HIGH_MODE_SS; + uint8_t buf[2]; + buf[0] = CONFIG_HIGH_OS_START_SINGLE | uint8_t(ch) | CONFIG_HIGH_PGA_6144 | CONFIG_HIGH_MODE_SS; buf[1] = CONFIG_LOW_DR_250SPS | CONFIG_LOW_COMP_MODE_TRADITIONAL | CONFIG_LOW_COMP_POL_RESET | CONFIG_LOW_COMP_LAT_NONE | CONFIG_LOW_COMP_QU_DISABLE; return writeReg(ADDRESSPOINTER_REG_CONFIG, buf, 2); // must write whole register to take effect } -bool ADS1115::isSampleReady() +int ADS1115::isSampleReady() { uint8_t buf[1] = {0x00}; - if (readReg(ADDRESSPOINTER_REG_CONFIG, buf, 1) != 0) { return false; } // Pull config register + if (readReg(ADDRESSPOINTER_REG_CONFIG, buf, 1) != PX4_OK) { return -1; } // Pull config register - return (buf[0] & (uint8_t) 0x80); + return (buf[0] & (uint8_t) 0x80) ? 1 : 0; } -ADS1115::ChannelSelection ADS1115::getMeasurement(int16_t *value) +ADS1115::Channel ADS1115::getMeasurement(int16_t *value) { uint8_t buf[2] = {0x00}; - readReg(ADDRESSPOINTER_REG_CONFIG, buf, 1); // Pull config register - ChannelSelection channel; - switch ((buf[0] & (uint8_t) 0x70) >> 4) { - case 0x04: - channel = A0; - break; + if (readReg(ADDRESSPOINTER_REG_CONFIG, buf, 1) != PX4_OK) { return Channel::Invalid; } - case 0x05: - channel = A1; - break; + const auto channel{Channel(buf[0] & CONFIG_HIGH_MUX_P3NG)}; - case 0x06: - channel = A2; - break; + if (readReg(ADDRESSPOINTER_REG_CONVERSATION, buf, 2) != PX4_OK) { return Channel::Invalid; } - case 0x07: - channel = A3; - break; + *value = int16_t((buf[0] << 8) | buf[1]); - default: - return Invalid; - } - - readReg(ADDRESSPOINTER_REG_CONVERSATION, buf, 2); - uint16_t raw_adc_val = buf[0] * 256 + buf[1]; - - if (raw_adc_val & (uint16_t) 0x8000) { // Negetive value - raw_adc_val = ~raw_adc_val + 1; // 2's complement - *value = -raw_adc_val; - - } else { - *value = raw_adc_val; - } - - return channel; -} - -ADS1115::ChannelSelection ADS1115::cycleMeasure(int16_t *value) -{ - uint8_t buf[2] = {0x00}; - readReg(ADDRESSPOINTER_REG_CONFIG, buf, 1); // Pull config register - ChannelSelection channel; - uint8_t next_mux_reg = CONFIG_HIGH_MUX_P0NG; - - switch ((buf[0] & (uint8_t) 0x70) >> 4) { - case 0x04: - channel = A0; - next_mux_reg = CONFIG_HIGH_MUX_P1NG; - break; - - case 0x05: - channel = A1; - next_mux_reg = CONFIG_HIGH_MUX_P2NG; - break; - - case 0x06: - channel = A2; - next_mux_reg = CONFIG_HIGH_MUX_P3NG; - break; - - case 0x07: - channel = A3; - next_mux_reg = CONFIG_HIGH_MUX_P0NG; - break; - - default: - return Invalid; - } - - readReg(ADDRESSPOINTER_REG_CONVERSATION, buf, 2); - uint16_t raw_adc_val = buf[0] * 256 + buf[1]; - - if (raw_adc_val & (uint16_t) 0x8000) { // Negetive value - raw_adc_val = ~raw_adc_val + 1; // 2's complement - *value = -raw_adc_val; - - } else { - *value = raw_adc_val; - } - - buf[0] = CONFIG_HIGH_OS_START_SINGLE | next_mux_reg | CONFIG_HIGH_PGA_6144 | CONFIG_HIGH_MODE_SS; - buf[1] = CONFIG_LOW_DR_250SPS | CONFIG_LOW_COMP_MODE_TRADITIONAL | CONFIG_LOW_COMP_POL_RESET | - CONFIG_LOW_COMP_LAT_NONE | CONFIG_LOW_COMP_QU_DISABLE; - writeReg(ADDRESSPOINTER_REG_CONFIG, buf, 2); // must write whole register to take effect return channel; } diff --git a/src/drivers/adc/ads1115/ADS1115.h b/src/drivers/adc/ads1115/ADS1115.h index 79a31198c9..634a659df8 100644 --- a/src/drivers/adc/ads1115/ADS1115.h +++ b/src/drivers/adc/ads1115/ADS1115.h @@ -93,9 +93,6 @@ #define CONFIG_LOW_COMP_QU_AFTER4 0x02 #define CONFIG_LOW_COMP_QU_DISABLE 0x03 -#define CONFIG_RESET_VALUE_HIGH 0x85 -#define CONFIG_RESET_VALUE_LOW 0x83 - using namespace time_literals; /* @@ -127,35 +124,39 @@ protected: private: - uORB::PublicationMulti _to_adc_report{ORB_ID(adc_report)}; + uORB::PublicationMulti _to_adc_report{ORB_ID(adc_report)}; - static const hrt_abstime SAMPLE_INTERVAL{50_ms}; + static const hrt_abstime SAMPLE_INTERVAL{50_ms}; adc_report_s _adc_report{}; - perf_counter_t _cycle_perf; + perf_counter_t _cycle_perf; + perf_counter_t _comms_errors; - int _channel_cycle_count{0}; + uint8_t _channel_cycle_mask{0}; - bool _reported_ready_last_cycle{false}; + static constexpr uint8_t MAX_READY_COUNTER{20}; + uint8_t _ready_counter{MAX_READY_COUNTER}; // ADS1115 logic part - enum ChannelSelection { - Invalid = -1, A0 = 0, A1, A2, A3 + enum class Channel : uint8_t { + A0 = CONFIG_HIGH_MUX_P0NG, + A1 = CONFIG_HIGH_MUX_P1NG, + A2 = CONFIG_HIGH_MUX_P2NG, + A3 = CONFIG_HIGH_MUX_P3NG, + Invalid = 0xff, }; - /* set multiplexer to specific channel */ - int setChannel(ChannelSelection ch); - /* return true if sample result is valid */ - bool isSampleReady(); + constexpr unsigned ch2u(Channel ch) { return (unsigned(ch) >> 4) & 0b11u; } + constexpr Channel u2ch(unsigned ch) { return Channel((ch << 4) | CONFIG_HIGH_MUX_P0NG); } + // Set the channel and start a conversion + int readChannel(Channel ch); + // return 1 if sample result is valid else 0 or -1 if I2C transaction failed + int isSampleReady(); /* * get adc sample. return the channel being measured. * Invalid indicates sample failure. */ - ChannelSelection getMeasurement(int16_t *value); - /* - * get adc sample and automatically switch to next channel and start another measurement - */ - ChannelSelection cycleMeasure(int16_t *value); + Channel getMeasurement(int16_t *value); int readReg(uint8_t addr, uint8_t *buf, size_t len); diff --git a/src/drivers/adc/ads1115/ads1115_main.cpp b/src/drivers/adc/ads1115/ads1115_main.cpp index 2132082c66..1967b58263 100644 --- a/src/drivers/adc/ads1115/ads1115_main.cpp +++ b/src/drivers/adc/ads1115/ads1115_main.cpp @@ -45,7 +45,8 @@ ADS1115::ADS1115(const I2CSPIDriverConfig &config) : I2C(config), I2CSPIDriver(config), - _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": single-sample")) + _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": single-sample")), + _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms errors")) { _adc_report.device_id = this->get_device_id(); _adc_report.resolution = 32768; @@ -61,6 +62,7 @@ ADS1115::~ADS1115() { ScheduleClear(); perf_free(_cycle_perf); + perf_free(_comms_errors); } void ADS1115::exit_and_cleanup() @@ -77,56 +79,55 @@ void ADS1115::RunImpl() perf_begin(_cycle_perf); - _adc_report.timestamp = hrt_absolute_time(); + const int ready = isSampleReady(); - if (isSampleReady()) { // whether ADS1115 is ready to be read or not - if (!_reported_ready_last_cycle) { - PX4_INFO("ADS1115: reported ready"); - _reported_ready_last_cycle = true; + if (ready == 1) { + // I2C transaction success and status register reported conversion as finished + if (_ready_counter == 0) { PX4_INFO("ADS1115: reported ready"); } + + if (_ready_counter < MAX_READY_COUNTER) { _ready_counter++; } + + int16_t value; + Channel ch = getMeasurement(&value); + + if (ch != Channel::Invalid) { + // Store current readings and mark channel as read + const unsigned index{ch2u(ch)}; + _adc_report.channel_id[index] = index; + _adc_report.raw_data[index] = value; + _channel_cycle_mask |= 1u << index; + + } else { + // we will retry the same channel again + perf_count(_comms_errors); } - int16_t buf; - ADS1115::ChannelSelection ch = cycleMeasure(&buf); - ++_channel_cycle_count; + // Find the next unread channel in the bitmask + uint8_t next_index{0}; - switch (ch) { - case ADS1115::A0: - _adc_report.channel_id[0] = 0; - _adc_report.raw_data[0] = buf; - break; + for (; next_index < 4 && (_channel_cycle_mask & (1u << next_index)); next_index++) {} - case ADS1115::A1: - _adc_report.channel_id[1] = 1; - _adc_report.raw_data[1] = buf; - break; + readChannel(u2ch(next_index)); - case ADS1115::A2: - _adc_report.channel_id[2] = 2; - _adc_report.raw_data[2] = buf; - break; - - case ADS1115::A3: - _adc_report.channel_id[3] = 3; - _adc_report.raw_data[3] = buf; - break; - - default: - PX4_DEBUG("ADS1115: undefined behaviour"); - setChannel(ADS1115::A0); - --_channel_cycle_count; - break; - } - - if (_channel_cycle_count == 4) { // ADS1115 has 4 channels - _channel_cycle_count = 0; + if (_channel_cycle_mask == 0b1111) { + _channel_cycle_mask = 0; _to_adc_report.publish(_adc_report); } - } else { - if (_reported_ready_last_cycle) { - _reported_ready_last_cycle = false; - PX4_ERR("ADS1115: not ready. Device lost?"); - } + } else if (ready == 0) { + // I2C transaction success but status register reported conversion still in progress + perf_count(_comms_errors); + // Reset the channel to unstick the device + readChannel(Channel::A0); + + } else if (ready == -1) { + if (_ready_counter == 1) { PX4_ERR("ADS1115: device lost"); } + + if (_ready_counter > 0) { _ready_counter--; } + + perf_count(_comms_errors); + // Reset the channel to unstick the device + readChannel(Channel::A0); } perf_end(_cycle_perf); @@ -151,7 +152,7 @@ parameter, and is disabled by default. If enabled, internal ADCs are not used. )DESCR_STR"); - + PRINT_MODULE_USAGE_NAME("ads1115", "driver"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); @@ -163,6 +164,7 @@ void ADS1115::print_status() { I2CSPIDriverBase::print_status(); perf_print_counter(_cycle_perf); + perf_print_counter(_comms_errors); } extern "C" int ads1115_main(int argc, char *argv[]) diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index b9059f5445..da47a50905 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -172,19 +172,19 @@ void BATT_SMBUS::RunImpl() // Check if max lifetime voltage delta is greater than allowed. if (_lifetime_max_delta_cell_voltage > BATT_CELL_VOLTAGE_THRESHOLD_FAILED) { - new_report.warning = battery_status_s::BATTERY_WARNING_CRITICAL; + new_report.warning = battery_status_s::WARNING_CRITICAL; } else if (new_report.remaining > _low_thr) { - new_report.warning = battery_status_s::BATTERY_WARNING_NONE; + new_report.warning = battery_status_s::WARNING_NONE; } else if (new_report.remaining > _crit_thr) { - new_report.warning = battery_status_s::BATTERY_WARNING_LOW; + new_report.warning = battery_status_s::WARNING_LOW; } else if (new_report.remaining > _emergency_thr) { - new_report.warning = battery_status_s::BATTERY_WARNING_CRITICAL; + new_report.warning = battery_status_s::WARNING_CRITICAL; } else { - new_report.warning = battery_status_s::BATTERY_WARNING_EMERGENCY; + new_report.warning = battery_status_s::WARNING_EMERGENCY; } new_report.interface_error = perf_event_count(_interface->_interface_errors); diff --git a/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp b/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp index 614d408a5e..825cbdf97f 100644 --- a/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp +++ b/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp @@ -143,7 +143,7 @@ private: ) uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; - typeof(px4::msg::VehicleStatus::vehicle_type) _vehicle_type{px4::msg::VehicleStatus::VEHICLE_TYPE_UNKNOWN}; + typeof(px4::msg::VehicleStatus::vehicle_type) _vehicle_type{px4::msg::VehicleStatus::VEHICLE_TYPE_ROTARY_WING}; uORB::Subscription _dist_sense_mode_change_sub{ORB_ID(distance_sensor_mode_change_request)}; typeof(px4::msg::DistanceSensorModeChangeRequest::request_on_off) _req_mode{px4::msg::DistanceSensorModeChangeRequest::REQUEST_OFF}; bool _restriction{false}; diff --git a/src/drivers/distance_sensor/ulanding_radar/AerotennaULanding.cpp b/src/drivers/distance_sensor/ulanding_radar/AerotennaULanding.cpp index 6558aef348..ffd2c81a7a 100644 --- a/src/drivers/distance_sensor/ulanding_radar/AerotennaULanding.cpp +++ b/src/drivers/distance_sensor/ulanding_radar/AerotennaULanding.cpp @@ -125,10 +125,15 @@ int AerotennaULanding::collect() index--; } + + } else { + return -EAGAIN; } if (!checksum_passed) { - return -EAGAIN; + perf_count(_comms_errors); + perf_end(_sample_perf); + return -EBADMSG; } _px4_rangefinder.update(timestamp_sample, distance_m); diff --git a/src/drivers/distance_sensor/ulanding_radar/AerotennaULanding.hpp b/src/drivers/distance_sensor/ulanding_radar/AerotennaULanding.hpp index dc133254bf..98d1c0ab90 100644 --- a/src/drivers/distance_sensor/ulanding_radar/AerotennaULanding.hpp +++ b/src/drivers/distance_sensor/ulanding_radar/AerotennaULanding.hpp @@ -56,7 +56,7 @@ using namespace time_literals; -#define ULANDING_MEASURE_INTERVAL 10_ms +#define ULANDING_MEASURE_INTERVAL 12_ms #define ULANDING_MAX_DISTANCE 50.0f #define ULANDING_MIN_DISTANCE 0.315f #define ULANDING_VERSION 1 diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index aeeba755e3..ba6f24070f 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -547,8 +547,13 @@ void GPS::handleInjectDataTopic() for (int instance = 0; instance < _orb_inject_data_sub.size(); instance++) { const bool exists = _orb_inject_data_sub[instance].advertised(); - if (exists) { - if (_orb_inject_data_sub[instance].copy(&msg)) { + if (exists && _orb_inject_data_sub[instance].copy(&msg)) { + /* Don't select the own RTCM instance. In case it has a lower + * instance number, it will be selected and will be rejected + * later in the code, resulting in no RTCM injection at all. + */ + if (msg.device_id != get_device_id()) { + // Only use the message if it is up to date if ((hrt_absolute_time() - msg.timestamp) < 5_s) { // Remember that we already did a copy on this instance. already_copied = true; diff --git a/src/drivers/imu/invensense/mpu6000/InvenSense_MPU6000_registers.hpp b/src/drivers/imu/invensense/mpu6000/InvenSense_MPU6000_registers.hpp index 768f4a3de9..b53a6dab43 100644 --- a/src/drivers/imu/invensense/mpu6000/InvenSense_MPU6000_registers.hpp +++ b/src/drivers/imu/invensense/mpu6000/InvenSense_MPU6000_registers.hpp @@ -41,6 +41,7 @@ #pragma once #include +#include // TODO: move to a central header static constexpr uint8_t Bit0 = (1 << 0); diff --git a/src/drivers/power_monitor/ina220/ina220.cpp b/src/drivers/power_monitor/ina220/ina220.cpp index 305d1a61a2..c8f52bf71c 100644 --- a/src/drivers/power_monitor/ina220/ina220.cpp +++ b/src/drivers/power_monitor/ina220/ina220.cpp @@ -52,7 +52,7 @@ INA220::INA220(const I2CSPIDriverConfig &config, int battery_index) : _collection_errors(perf_alloc(PC_COUNT, "ina220_collection_err")), _measure_errors(perf_alloc(PC_COUNT, "ina220_measurement_err")), _ch_type((PM_CH_TYPE)config.custom2), - _battery(battery_index, this, INA220_SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE) + _battery(battery_index, this, INA220_SAMPLE_INTERVAL_US, battery_status_s::SOURCE_POWER_MODULE) { float fvalue = MAX_CURRENT; _max_current = fvalue; diff --git a/src/drivers/power_monitor/ina226/ina226.cpp b/src/drivers/power_monitor/ina226/ina226.cpp index 2c117caf1d..f61a7c7b79 100644 --- a/src/drivers/power_monitor/ina226/ina226.cpp +++ b/src/drivers/power_monitor/ina226/ina226.cpp @@ -49,7 +49,7 @@ INA226::INA226(const I2CSPIDriverConfig &config, int battery_index) : _comms_errors(perf_alloc(PC_COUNT, "ina226_com_err")), _collection_errors(perf_alloc(PC_COUNT, "ina226_collection_err")), _measure_errors(perf_alloc(PC_COUNT, "ina226_measurement_err")), - _battery(battery_index, this, INA226_SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE) + _battery(battery_index, this, INA226_SAMPLE_INTERVAL_US, battery_status_s::SOURCE_POWER_MODULE) { float fvalue = MAX_CURRENT; _max_current = fvalue; diff --git a/src/drivers/power_monitor/ina228/ina228.cpp b/src/drivers/power_monitor/ina228/ina228.cpp index 8c0207b3f2..6505a7d65e 100644 --- a/src/drivers/power_monitor/ina228/ina228.cpp +++ b/src/drivers/power_monitor/ina228/ina228.cpp @@ -49,7 +49,7 @@ INA228::INA228(const I2CSPIDriverConfig &config, int battery_index) : _comms_errors(perf_alloc(PC_COUNT, "ina228_com_err")), _collection_errors(perf_alloc(PC_COUNT, "ina228_collection_err")), _measure_errors(perf_alloc(PC_COUNT, "ina228_measurement_err")), - _battery(battery_index, this, INA228_SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE) + _battery(battery_index, this, INA228_SAMPLE_INTERVAL_US, battery_status_s::SOURCE_POWER_MODULE) { float fvalue = MAX_CURRENT; _max_current = fvalue; diff --git a/src/drivers/power_monitor/ina238/ina238.cpp b/src/drivers/power_monitor/ina238/ina238.cpp index b46b277a84..ff3f193385 100644 --- a/src/drivers/power_monitor/ina238/ina238.cpp +++ b/src/drivers/power_monitor/ina238/ina238.cpp @@ -45,7 +45,7 @@ INA238::INA238(const I2CSPIDriverConfig &config, int battery_index) : _sample_perf(perf_alloc(PC_ELAPSED, "ina238_read")), _comms_errors(perf_alloc(PC_COUNT, "ina238_com_err")), _collection_errors(perf_alloc(PC_COUNT, "ina238_collection_err")), - _battery(battery_index, this, INA238_SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE) + _battery(battery_index, this, INA238_SAMPLE_INTERVAL_US, battery_status_s::SOURCE_POWER_MODULE) { float fvalue = DEFAULT_MAX_CURRENT; _max_current = fvalue; diff --git a/src/drivers/power_monitor/voxlpm/voxlpm.cpp b/src/drivers/power_monitor/voxlpm/voxlpm.cpp index 5fa109d57d..51a496bd6d 100644 --- a/src/drivers/power_monitor/voxlpm/voxlpm.cpp +++ b/src/drivers/power_monitor/voxlpm/voxlpm.cpp @@ -54,7 +54,7 @@ VOXLPM::VOXLPM(const I2CSPIDriverConfig &config) : _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": sample")), _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms_errors")), _ch_type((VOXLPM_CH_TYPE)config.custom1), - _battery(1, this, _meas_interval_us, battery_status_s::BATTERY_SOURCE_POWER_MODULE) + _battery(1, this, _meas_interval_us, battery_status_s::SOURCE_POWER_MODULE) { } diff --git a/src/drivers/rc/ghst_rc/GhstRc.cpp b/src/drivers/rc/ghst_rc/GhstRc.cpp index 1960c84ce6..1aa3c23e89 100644 --- a/src/drivers/rc/ghst_rc/GhstRc.cpp +++ b/src/drivers/rc/ghst_rc/GhstRc.cpp @@ -34,6 +34,7 @@ #include "GhstRc.hpp" #include +#include GhstRc::GhstRc(const char *device) : ModuleParams(nullptr), @@ -174,16 +175,18 @@ void GhstRc::Run() if (newBytes > 0) { uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {}; uint16_t raw_rc_count = 0; - int8_t ghst_rssi = -1; + ghstLinkStatistics_t link_stats = { .rssi_pct = -1, .rssi_dbm = NAN, .link_quality = 0 }; - if (ghst_parse(cycle_timestamp, &rcs_buf[0], newBytes, &raw_rc_values[0], &ghst_rssi, + if (ghst_parse(cycle_timestamp, &rcs_buf[0], newBytes, &raw_rc_values[0], &link_stats, &raw_rc_count, input_rc_s::RC_INPUT_MAX_CHANNELS) ) { // we have a new GHST frame. Publish it. input_rc_s input_rc{}; input_rc.timestamp_last_signal = cycle_timestamp; input_rc.channel_count = math::constrain(raw_rc_count, (uint16_t)0, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS); - input_rc.rssi = ghst_rssi; + input_rc.rssi = link_stats.rssi_pct; + input_rc.link_quality = link_stats.link_quality; + input_rc.rssi_dbm = link_stats.rssi_dbm; input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST; unsigned valid_chans = 0; @@ -200,13 +203,11 @@ void GhstRc::Run() if (valid_chans == 0) { input_rc.rssi = 0; + // can't force link quality to zero here, receiver takes care of this } input_rc.rc_lost = (valid_chans == 0); - input_rc.link_quality = -1; - input_rc.rssi_dbm = NAN; - input_rc.timestamp = hrt_absolute_time(); _input_rc_pub.publish(input_rc); perf_count(_publish_interval_perf); diff --git a/src/drivers/rc_input/RCInput.cpp b/src/drivers/rc_input/RCInput.cpp index 12cd1ab0bf..538cb9d8ed 100644 --- a/src/drivers/rc_input/RCInput.cpp +++ b/src/drivers/rc_input/RCInput.cpp @@ -762,14 +762,15 @@ void RCInput::Run() // parse new data if (newBytes > 0) { - int8_t ghst_rssi = -1; - rc_updated = ghst_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &ghst_rssi, + ghstLinkStatistics_t link_stats = { .rssi_pct = -1, .rssi_dbm = NAN, .link_quality = 0 }; + + rc_updated = ghst_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &link_stats, &_raw_rc_count, input_rc_s::RC_INPUT_MAX_CHANNELS); if (rc_updated) { // we have a new GHST frame. Publish it. _input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST; - int32_t valid_chans = fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0, ghst_rssi); + int32_t valid_chans = fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0, link_stats.rssi_pct); // ghst telemetry works on fmu-v5 // on other Pixhawk (-related) boards we cannot write to the RC UART diff --git a/src/drivers/rpm_capture/rpm_capture_params.c b/src/drivers/rpm_capture/rpm_capture_params.c index 13bd3fb7a7..5402d089ab 100644 --- a/src/drivers/rpm_capture/rpm_capture_params.c +++ b/src/drivers/rpm_capture/rpm_capture_params.c @@ -32,9 +32,9 @@ ****************************************************************************/ /** - * RPM Capture Enable + * RPM capture enable * - * Enables the RPM capture module on FMU channel 5. + * Enables the RPM capture module to estimate RPM from pulses detected on a PWM pin configured as "RPM Input". * * @boolean * @group System diff --git a/src/drivers/smart_battery/batmon/batmon.cpp b/src/drivers/smart_battery/batmon/batmon.cpp index 9061a6d3e4..ed424c1e18 100644 --- a/src/drivers/smart_battery/batmon/batmon.cpp +++ b/src/drivers/smart_battery/batmon/batmon.cpp @@ -199,19 +199,19 @@ void Batmon::RunImpl() // TODO: This critical setting should be set with BMS info or through a paramter // Setting a hard coded BATT_CELL_VOLTAGE_THRESHOLD_FAILED may not be appropriate //if (_lifetime_max_delta_cell_voltage > BATT_CELL_VOLTAGE_THRESHOLD_FAILED) { - // new_report.warning = battery_status_s::BATTERY_WARNING_CRITICAL; + // new_report.warning = battery_status_s::WARNING_CRITICAL; if (new_report.remaining > _low_thr) { - new_report.warning = battery_status_s::BATTERY_WARNING_NONE; + new_report.warning = battery_status_s::WARNING_NONE; } else if (new_report.remaining > _crit_thr) { - new_report.warning = battery_status_s::BATTERY_WARNING_LOW; + new_report.warning = battery_status_s::WARNING_LOW; } else if (new_report.remaining > _emergency_thr) { - new_report.warning = battery_status_s::BATTERY_WARNING_CRITICAL; + new_report.warning = battery_status_s::WARNING_CRITICAL; } else { - new_report.warning = battery_status_s::BATTERY_WARNING_EMERGENCY; + new_report.warning = battery_status_s::WARNING_EMERGENCY; } new_report.interface_error = perf_event_count(_interface->_interface_errors); diff --git a/src/drivers/telemetry/hott/messages.cpp b/src/drivers/telemetry/hott/messages.cpp index 3be05d87a3..364d5005a7 100644 --- a/src/drivers/telemetry/hott/messages.cpp +++ b/src/drivers/telemetry/hott/messages.cpp @@ -150,7 +150,7 @@ build_eam_response(uint8_t *buffer, size_t *size) msg.eam_sensor_id = EAM_SENSOR_ID; msg.sensor_text_id = EAM_SENSOR_TEXT_ID; - msg.temperature1 = (uint8_t)(airdata.baro_temp_celcius + 20); + msg.temperature1 = (uint8_t)(airdata.ambient_temperature + 20); msg.temperature2 = msg.temperature1 - BOARD_TEMP_OFFSET_DEG; msg.main_voltage_L = (uint8_t)(battery.voltage_v * 10); diff --git a/src/drivers/uavcan/sensors/airspeed.cpp b/src/drivers/uavcan/sensors/airspeed.cpp index e62b29d5ea..2127817fd0 100644 --- a/src/drivers/uavcan/sensors/airspeed.cpp +++ b/src/drivers/uavcan/sensors/airspeed.cpp @@ -104,7 +104,6 @@ UavcanAirspeedBridge::ias_sub_cb(const report.timestamp = hrt_absolute_time(); report.indicated_airspeed_m_s = msg.indicated_airspeed; report.true_airspeed_m_s = _last_tas_m_s; - report.air_temperature_celsius = _last_outside_air_temp_k + atmosphere::kAbsoluteNullCelsius; publish(msg.getSrcNodeID().get(), &report); } diff --git a/src/drivers/uavcan/sensors/battery.cpp b/src/drivers/uavcan/sensors/battery.cpp index 77698cff16..0c8528670f 100644 --- a/src/drivers/uavcan/sensors/battery.cpp +++ b/src/drivers/uavcan/sensors/battery.cpp @@ -44,7 +44,7 @@ UavcanBatteryBridge::UavcanBatteryBridge(uavcan::INode &node) : ModuleParams(nullptr), _sub_battery(node), _sub_battery_aux(node), - _warning(battery_status_s::BATTERY_WARNING_NONE), + _warning(battery_status_s::WARNING_NONE), _last_timestamp(0) { } @@ -215,14 +215,14 @@ void UavcanBatteryBridge::determineWarning(float remaining) { // propagate warning state only if the state is higher, otherwise remain in current warning state - if (remaining < _param_bat_emergen_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_EMERGENCY)) { - _warning = battery_status_s::BATTERY_WARNING_EMERGENCY; + if (remaining < _param_bat_emergen_thr.get() || (_warning == battery_status_s::WARNING_EMERGENCY)) { + _warning = battery_status_s::WARNING_EMERGENCY; - } else if (remaining < _param_bat_crit_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_CRITICAL)) { - _warning = battery_status_s::BATTERY_WARNING_CRITICAL; + } else if (remaining < _param_bat_crit_thr.get() || (_warning == battery_status_s::WARNING_CRITICAL)) { + _warning = battery_status_s::WARNING_CRITICAL; - } else if (remaining < _param_bat_low_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_LOW)) { - _warning = battery_status_s::BATTERY_WARNING_LOW; + } else if (remaining < _param_bat_low_thr.get() || (_warning == battery_status_s::WARNING_LOW)) { + _warning = battery_status_s::WARNING_LOW; } } diff --git a/src/drivers/uavcan/sensors/battery.hpp b/src/drivers/uavcan/sensors/battery.hpp index ceb6222da0..a27ad64505 100644 --- a/src/drivers/uavcan/sensors/battery.hpp +++ b/src/drivers/uavcan/sensors/battery.hpp @@ -107,10 +107,10 @@ private: static_assert(battery_status_s::MAX_INSTANCES <= BATTERY_INDEX_4, "Battery array too big"); - Battery battery1 = {BATTERY_INDEX_1, this, SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_EXTERNAL}; - Battery battery2 = {BATTERY_INDEX_2, this, SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_EXTERNAL}; - Battery battery3 = {BATTERY_INDEX_3, this, SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_EXTERNAL}; - Battery battery4 = {BATTERY_INDEX_4, this, SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_EXTERNAL}; + Battery battery1 = {BATTERY_INDEX_1, this, SAMPLE_INTERVAL_US, battery_status_s::SOURCE_EXTERNAL}; + Battery battery2 = {BATTERY_INDEX_2, this, SAMPLE_INTERVAL_US, battery_status_s::SOURCE_EXTERNAL}; + Battery battery3 = {BATTERY_INDEX_3, this, SAMPLE_INTERVAL_US, battery_status_s::SOURCE_EXTERNAL}; + Battery battery4 = {BATTERY_INDEX_4, this, SAMPLE_INTERVAL_US, battery_status_s::SOURCE_EXTERNAL}; Battery *_battery[battery_status_s::MAX_INSTANCES] = { &battery1, &battery2, &battery3, &battery4 }; }; diff --git a/src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp b/src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp index 129bd32c21..134caea21a 100644 --- a/src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp +++ b/src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp @@ -100,10 +100,12 @@ public: } // reading_type - if (dist.current_distance >= dist.max_distance) { + const float tolerance = 1e-6; + + if (dist.current_distance > dist.max_distance) { range_sensor.reading_type = uavcan::equipment::range_sensor::Measurement::READING_TYPE_TOO_FAR; - } else if (dist.current_distance <= dist.min_distance) { + } else if (dist.current_distance < dist.min_distance - tolerance) { range_sensor.reading_type = uavcan::equipment::range_sensor::Measurement::READING_TYPE_TOO_CLOSE; } else if (dist.signal_quality != 0) { diff --git a/src/drivers/voxl2_io/CMakeLists.txt b/src/drivers/voxl2_io/CMakeLists.txt index 57a5f4af21..b6e3d8a3c2 100644 --- a/src/drivers/voxl2_io/CMakeLists.txt +++ b/src/drivers/voxl2_io/CMakeLists.txt @@ -37,15 +37,12 @@ px4_add_module( SRCS voxl2_io_crc16.c voxl2_io_crc16.h - voxl2_io_serial.cpp - voxl2_io_serial.hpp voxl2_io_packet.c voxl2_io_packet.h voxl2_io_packet_types.h voxl2_io.cpp voxl2_io.hpp DEPENDS - rc px4_work_queue mixer_module MODULE_CONFIG diff --git a/src/drivers/voxl2_io/module.yaml b/src/drivers/voxl2_io/module.yaml index 021305e955..86dc48ca58 100644 --- a/src/drivers/voxl2_io/module.yaml +++ b/src/drivers/voxl2_io/module.yaml @@ -1,12 +1,12 @@ module_name: VOXL2 IO Output actuator_output: - config_parameters: - - param: 'VOXL2_IO_MIN' - label: 'PWM min value' - - param: 'VOXL2_IO_MAX' - label: 'PWM max value' +# config_parameters: +# - param: 'VOXL2_IO_MIN' +# label: 'PWM min value' +# - param: 'VOXL2_IO_MAX' +# label: 'PWM max value' output_groups: - param_prefix: VOXL2_IO group_label: 'PWMs' channel_label: 'PWM Channel' - num_channels: 4 + num_channels: 8 diff --git a/src/drivers/voxl2_io/voxl2_io.cpp b/src/drivers/voxl2_io/voxl2_io.cpp index 352b474906..3461c99a4b 100644 --- a/src/drivers/voxl2_io/voxl2_io.cpp +++ b/src/drivers/voxl2_io/voxl2_io.cpp @@ -33,8 +33,6 @@ #include "voxl2_io.hpp" -#include - Voxl2IO::Voxl2IO() : OutputModuleInterface(MODULE_NAME, px4::serial_port_to_wq(VOXL2_IO_DEFAULT_PORT)), @@ -43,69 +41,81 @@ Voxl2IO::Voxl2IO() : _output_update_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": output update interval")) { _mixing_output.setMaxNumOutputs(VOXL2_IO_OUTPUT_CHANNELS); - _uart_port = new Voxl2IoSerial(); - voxl2_io_packet_init(&_sbus_packet); + voxl2_io_packet_init(&_voxl2_io_packet); + + //set low rate scheduling interval to 200hz so that RC can be updated even if all actuators are disabled + //otherwise, the default low rate scheduling interval is 300ms and RC packets are delayed and lost + _mixing_output.setLowrateSchedulingInterval(5_ms); } Voxl2IO::~Voxl2IO() { - /* make sure servos are off */ - stop_all_pwms(); - - if (_uart_port) { - _uart_port->uart_close(); - _uart_port = nullptr; - } + _uart_port.close(); perf_free(_cycle_perf); perf_free(_output_update_perf); } - int Voxl2IO::init() { + PX4_INFO("VOXL2_IO: Driver starting"); + int ret = PX4_OK; - /* Open serial port in this thread */ - if (!_uart_port->is_open()) { - if (_uart_port->uart_open((const char *)_device, _parameters.baud_rate) == PX4_OK) { - /* Send PWM config to M0065... pwm_min and pwm_max */ - PX4_INFO("Opened UART connection to M0065 device on port %s", _device); - - } else { - PX4_ERR("Failed opening device"); - return PX4_ERROR; - } - } - - /* Verify connectivity and protocol version number */ - if (get_version_info() < 0) { - PX4_ERR("Failed to detect voxl2_io protocol version."); - return PX4_ERROR; - - } else { - if (_version_info.sw_version == VOXL2_IO_SW_PROTOCOL_VERSION - && _version_info.hw_version == VOXL2_IO_HW_PROTOCOL_VERSION) { - PX4_INFO("Detected M0065 protocol version. SW: %u HW: %u", _version_info.sw_version, _version_info.hw_version); - - } else { - PX4_ERR("Detected incorrect M0065 protocol version. SW: %u HW: %u", _version_info.sw_version, _version_info.hw_version); - return PX4_ERROR; - } - } - - /* Getting initial parameter values */ + // Getting initial parameter values ret = update_params(); if (ret != OK) { + PX4_ERR("VOXL2_IO: Failed to update params during init"); return ret; } - /* Send PWM MIN/MAX to M0065 */ - update_pwm_config(); + // Print initial param values + print_params(); - ScheduleOnInterval(_current_update_interval); - // ScheduleNow(); + PX4_INFO("VOXL2_IO: "); + + // Open serial port + if (!_uart_port.isOpen()) { + PX4_INFO("VOXL2_IO: Opening UART ESC device %s, baud rate %" PRIi32, _device, _parameters.baud_rate); + + // Configure UART port + if (! _uart_port.setPort(_device)) { + PX4_ERR("Error configuring serial device on port %s", _device); + return -1; + } + + if (! _uart_port.setBaudrate(_parameters.baud_rate)) { + PX4_ERR("Error setting baudrate to %d on %s", (int) _parameters.baud_rate, _device); + return -1; + } + + // Open the UART. If this is successful then the UART is ready to use. + if (! _uart_port.open()) { + PX4_ERR("Error opening serial device %s", _device); + return -1; + } + } + + // Detect M0065 board + ret = get_version_info(); + + if (ret != 0) { + PX4_ERR("VOXL2_IO: Could not detect the board"); + PX4_ERR("VOXL2_IO: Driver initialization failed. Exiting"); + + if (_uart_port.open()) { + PX4_INFO("VOXL2_IO: Closing uart port"); + _uart_port.close(); + } + + return -1; + } + + //ScheduleOnInterval(_current_update_interval); + ScheduleNow(); + + PX4_INFO("VOXL2_IO: Driver initialization succeeded"); return ret; } @@ -117,11 +127,10 @@ int Voxl2IO::update_params() ret = load_params(&_parameters); if (ret == PX4_OK) { - _mixing_output.setAllDisarmedValues(VOXL2_IO_MIXER_DISARMED); + _mixing_output.setAllDisarmedValues(_parameters.pwm_dis); _mixing_output.setAllFailsafeValues(VOXL2_IO_MIXER_FAILSAFE); - _mixing_output.setAllMinValues(VOXL2_IO_MIXER_MIN); - _mixing_output.setAllMaxValues(VOXL2_IO_MIXER_MAX); - _pwm_fullscale = _parameters.pwm_max - _parameters.pwm_min; + _mixing_output.setAllMinValues(_parameters.pwm_min); + _mixing_output.setAllMaxValues(_parameters.pwm_max); } return ret; @@ -130,196 +139,159 @@ int Voxl2IO::update_params() int Voxl2IO::load_params(voxl2_io_params_t *params) { int ret = PX4_OK; - int32_t max = params->pwm_max; - int32_t min = params->pwm_min; // initialize out for (int i = 0; i < VOXL2_IO_OUTPUT_CHANNELS; i++) { params->function_map[i] = (int)OutputFunction::Disabled; } - /* UART config, PWM mode, and RC protocol*/ + // UART config, PWM mode, and RC protocol param_get(param_find("VOXL2_IO_BAUD"), ¶ms->baud_rate); - param_get(param_find("RC_INPUT_PROTO"), ¶ms->param_rc_input_proto); + //param_get(param_find("RC_INPUT_PROTO"), ¶ms->param_rc_input_proto); - /* PWM min, max, and failsafe values*/ + // PWM min, max, and failsafe values param_get(param_find("VOXL2_IO_MIN"), ¶ms->pwm_min); param_get(param_find("VOXL2_IO_MAX"), ¶ms->pwm_max); + param_get(param_find("VOXL2_IO_DIS"), ¶ms->pwm_dis); + param_get(param_find("VOXL2_IO_CMIN"), ¶ms->pwm_cal_min); + param_get(param_find("VOXL2_IO_CMAX"), ¶ms->pwm_cal_max); - /* PWM output functions */ + // PWM output functions + //0: disabled, 1: constant min, 2: constant max + //101-112: motors, 201-208: servos, 402: RC Roll, 403: RC Pitch, 404: RC Throttle, + //405: RC Yaw, 406: RC Flaps, 407-412: RC AUX 1-6, 420-422: Gimbal RPY param_get(param_find("VOXL2_IO_FUNC1"), ¶ms->function_map[0]); param_get(param_find("VOXL2_IO_FUNC2"), ¶ms->function_map[1]); param_get(param_find("VOXL2_IO_FUNC3"), ¶ms->function_map[2]); param_get(param_find("VOXL2_IO_FUNC4"), ¶ms->function_map[3]); + param_get(param_find("VOXL2_IO_FUNC5"), ¶ms->function_map[4]); + param_get(param_find("VOXL2_IO_FUNC6"), ¶ms->function_map[5]); + param_get(param_find("VOXL2_IO_FUNC7"), ¶ms->function_map[6]); + param_get(param_find("VOXL2_IO_FUNC8"), ¶ms->function_map[7]); - /* Validate PWM min and max values */ + // Validate PWM min and max values if (params->pwm_min > params->pwm_max) { - PX4_ERR("Invalid parameter VOXL2_IO_MIN. Please verify parameters."); + PX4_ERR("VOXL2_IO: Invalid parameter VOXL2_IO_MIN. Please verify parameters."); params->pwm_min = 0; ret = PX4_ERROR; } - if (ret == PX4_OK && _uart_port->is_open() && (max != params->pwm_max || min != params->pwm_min)) { - PX4_INFO("Updating PWM params load_params"); - update_pwm_config(); - } - return ret; } -void Voxl2IO::update_pwm_config() -{ - Command cmd; - uint8_t data[VOXL2_IO_BOARD_CONFIG_SIZE] = {static_cast((_parameters.pwm_min & 0xFF00) >> 8), static_cast(_parameters.pwm_min & 0xFF), - static_cast((_parameters.pwm_max & 0xFF00) >> 8), static_cast(_parameters.pwm_max & 0xFF) - }; - cmd.len = voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_CONFIG_BOARD_REQUEST, data, VOXL2_IO_BOARD_CONFIG_SIZE, cmd.buf, - sizeof(cmd.buf)); - - if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("Failed to send config packet"); - - } else { - _bytes_sent += cmd.len; - _packets_sent++; - } -} - int Voxl2IO::get_version_info() { - int res = 0 ; - int header = -1 ; - int info_packet = -1; - int read_retries = 3; - int read_succeeded = 0; + PX4_INFO("VOXL2_IO: Detecting M0065 board..."); + voxl2_io_packet_init(&_voxl2_io_packet); + Command cmd; + cmd.len = voxl2_io_create_extended_version_request_packet(0, cmd.buf, sizeof(cmd.buf)); - /* Request protocol version info from M0065 */ - cmd.len = voxl2_io_create_version_request_packet(0, cmd.buf, VOXL2_IO_VERSION_INFO_SIZE); + int retries_left = _board_detect_retries; + bool got_response = false; - if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("Failed to send version info packet"); + while ((got_response == false) && (retries_left > 0)) { + retries_left--; + + //send the version request command to the board + if (_uart_port.write(cmd.buf, cmd.len) != cmd.len) { + PX4_ERR("VOXL2_IO: Could not write version request packet to UART port"); + return -1; + } - } else { _bytes_sent += cmd.len; _packets_sent++; - } - /* Read response */ - px4_usleep(10000); - memset(&_read_buf, 0x00, READ_BUF_SIZE); - res = _uart_port->uart_read(_read_buf, READ_BUF_SIZE); + hrt_abstime t_request = hrt_absolute_time(); + hrt_abstime t_timeout = 50000; //50ms timeout for version info response - while (read_retries) { - if (res) { - /* Get index of packer header */ - for (int index = 0; index < READ_BUF_SIZE; ++index) { - if (_read_buf[index] == VOXL2_IO_PACKET_TYPE_VERSION_RESPONSE) { - info_packet = index; - break; - } - if (_read_buf[index] == VOXL2_IO_PACKET_HEADER) { - header = index; + //wait for the response to come back + while ((!got_response) && (hrt_elapsed_time(&t_request) < t_timeout)) { + px4_usleep(500); //sleep a bit while waiting for the board to respond + + int nread = _uart_port.read(_read_buf, sizeof(_read_buf)); + + for (int i = 0; i < nread; i++) { + int16_t parse_ret = voxl2_io_packet_process_char(_read_buf[i], &_voxl2_io_packet); + + if (parse_ret > 0) { + hrt_abstime response_time = hrt_elapsed_time(&t_request); + //PX4_INFO("got packet of length %i",ret); + _packets_received++; + uint8_t packet_type = voxl2_io_packet_get_type(&_voxl2_io_packet); + uint8_t packet_size = voxl2_io_packet_get_size(&_voxl2_io_packet); + + if (packet_type == VOXL2_IO_PACKET_TYPE_VERSION_EXT_RESPONSE && packet_size == sizeof(VOXL2_IO_EXTENDED_VERSION_INFO)) { + VOXL2_IO_EXTENDED_VERSION_INFO ver; + memcpy(&ver, _voxl2_io_packet.buffer, packet_size); + + PX4_INFO("VOXL2_IO: \tVOXL2_IO ID: %i", ver.id); + PX4_INFO("VOXL2_IO: \tBoard Type : %i: %s", ver.hw_version, board_id_to_name(ver.hw_version).c_str()); + + uint8_t *u = &ver.unique_id[0]; + PX4_INFO("VOXL2_IO: \tUnique ID : 0x%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X", + u[11], u[10], u[9], u[8], u[7], u[6], u[5], u[4], u[3], u[2], u[1], u[0]); + + PX4_INFO("VOXL2_IO: \tFirmware : version %4d, hash %.12s", ver.sw_version, ver.firmware_git_version); + PX4_INFO("VOXL2_IO: \tBootloader : version %4d, hash %.12s", ver.bootloader_version, ver.bootloader_git_version); + PX4_INFO("VOXL2_IO: \tReply time : %uus", (uint32_t)response_time); + + //we requested response from ID 0, so it should match + if (ver.id != 0) { + PX4_ERR("VOXL2_IO: Invalid id: %d", ver.id); + } + + //check HW (board version) + else if (ver.hw_version != VOXL2_IO_HW_VERSION) { + PX4_ERR("VOXL2_IO: Invalid HW version : %d (expected %d)", ver.hw_version, VOXL2_IO_HW_VERSION); + return -1; + } + + //check firmware version running on the board + else if (ver.sw_version != VOXL2_IO_SW_VERSION) { + PX4_ERR("VOXL2_IO: Invalid FW version : %d (expected %d)", ver.sw_version, VOXL2_IO_SW_VERSION); + return -1; + + } else { + got_response = true; + memcpy(&_version_info, &ver, sizeof(_version_info)); //store the version info only if it is valid + } + } } } - /* Try again in a bit if packet header not present yet... */ - if (header == -1 || info_packet == -1) { - if (_debug && header == -1) { PX4_ERR("Failed to find voxl2_io packet header, trying again... retries left: %i", read_retries); } - - if (_debug && info_packet == -1) { PX4_ERR("Failed to find version info packet header, trying again... retries left: %i", read_retries); } - - read_retries--; - flush_uart_rx(); - - if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("Failed to send version info packet"); - - } else { - _bytes_sent += cmd.len; - _packets_sent++; - px4_usleep(2000); - } - - continue; - } - - /* Check if we got a valid packet...*/ - if (parse_response(&_read_buf[header], (uint8_t)VOXL2_IO_VERSION_INFO_SIZE)) { - if (_debug) { - PX4_ERR("Error parsing version info packet"); - PX4_INFO_RAW("[%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x]\n", - _read_buf[header + 0], _read_buf[header + 1], _read_buf[header + 2], _read_buf[header + 3], _read_buf[header + 4], - _read_buf[header + 5], - _read_buf[header + 6], _read_buf[header + 7], _read_buf[header + 8], _read_buf[header + 9], _read_buf[header + 10], - _read_buf[header + 11], - _read_buf[header + 12], _read_buf[header + 13], _read_buf[header + 14], _read_buf[header + 15], _read_buf[header + 16], - _read_buf[header + 17], - _read_buf[header + 18], _read_buf[header + 19], _read_buf[header + 20], _read_buf[header + 21], _read_buf[header + 22], - _read_buf[header + 23], - _read_buf[header + 24], _read_buf[header + 25], _read_buf[header + 26], _read_buf[header + 27], _read_buf[header + 28], - _read_buf[header + 29] - ); - } - - read_retries--; - flush_uart_rx(); - - if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("Failed to send version info packet"); - - } else { - _bytes_sent += cmd.len; - _packets_sent++; - px4_usleep(2000); - } - - break; - - } else { - memcpy(&_version_info, &_read_buf[header], sizeof(VOXL2_IO_VERSION_INFO)); - read_succeeded = 1; + //break out of the loop waiting for a response + if (got_response) { break; } + } - } else { - read_retries--; - if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("Failed to send version info packet"); - - } else { - _bytes_sent += cmd.len; - _packets_sent++; - px4_usleep(2000); - } + if (!got_response) { + PX4_ERR("VOXL2_IO: Board version info response timeout (%d retries left)", retries_left); } } - if (! read_succeeded) { - return -EIO; - } - - return 0; + return (got_response == true ? 0 : -1); } bool Voxl2IO::updateOutputs(bool stop_motors, uint16_t outputs[input_rc_s::RC_INPUT_MAX_CHANNELS], unsigned num_outputs, unsigned num_control_groups_updated) { - /* Stop Mixer while ESCs are being calibrated */ + // Stop Mixer while ESCs are being calibrated if (_outputs_disabled) { return 0; } + //PX4_INFO("VOXL2_IO: Mixer output: %u, %u, %u, %u", outputs[0], outputs[1], outputs[2], outputs[3]); + //in Run() we call _mixing_output.update(), which calls MixingOutput::limitAndUpdateOutputs which calls _interface.updateOutputs (this function) //So, if Run() is blocked by a custom command, this function will not be called until Run is running again - int16_t _rate_req[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0}; - uint8_t _led_req[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0}; - int32_t _fb_idx = -1; + uint32_t output_cmds[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0, 0, 0, 0, 0}; if (num_outputs != VOXL2_IO_OUTPUT_CHANNELS) { - PX4_ERR("Num outputs != VOXL2_IO_OUTPUT_CHANNELS!"); + PX4_ERR("VOXL2_IO: Num outputs != VOXL2_IO_OUTPUT_CHANNELS!"); return false; } @@ -333,43 +305,20 @@ bool Voxl2IO::updateOutputs(bool stop_motors, uint16_t outputs[input_rc_s::RC_IN _pwm_on = true; } - if (!_pwm_on || stop_motors) { - _rate_req[i] = 0; + //Do we even need this condition? mixer should handle stopping motors anyway by sending the disable command, right? + if (0) { //(!_pwm_on || stop_motors) { + output_cmds[i] = _parameters.pwm_dis * MIXER_OUTPUT_TO_CMD_SCALE; //0; //convert to ns } else { - _rate_req[i] = outputs[i]; + output_cmds[i] = ((uint32_t)outputs[i]) * MIXER_OUTPUT_TO_CMD_SCALE; //convert to ns } - - _pwm_values[i] = _rate_req[i]; } Command cmd; - cmd.len = voxl2_io_create_pwm_packet4_fb(_rate_req[0], _rate_req[1], _rate_req[2], _rate_req[3], - _led_req[0], _led_req[1], _led_req[2], _led_req[3], - _fb_idx, cmd.buf, sizeof(cmd.buf)); + cmd.len = voxl2_io_create_hires_pwm_packet(output_cmds, VOXL2_IO_OUTPUT_CHANNELS, cmd.buf, sizeof(cmd.buf)); - if (_pwm_on && _debug) { - PX4_INFO("Mixer outputs"); - PX4_INFO("[%u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u]", - outputs[0], outputs[1], outputs[2], outputs[3], outputs[4], outputs[5], - outputs[6], outputs[7], outputs[8], outputs[9], outputs[10], outputs[11], - outputs[12], outputs[13], outputs[14], outputs[15], outputs[16], outputs[17] - ); - - // Debug messages for PWM 400Hz values sent to M0065 - uint16_t tics_1 = (_parameters.pwm_min + (_pwm_fullscale * ((double)outputs[0] / VOXL2_IO_MIXER_MAX))) * VOXL2_IO_TICS; - PX4_INFO("\tPWM CH1: %hu::%uus::%u tics", outputs[0], tics_1 / 24, tics_1); - uint16_t tics_2 = (_parameters.pwm_min + (_pwm_fullscale * ((double)outputs[1] / VOXL2_IO_MIXER_MAX))) * VOXL2_IO_TICS; - PX4_INFO("\tPWM CH2: %u::%uus::%u tics", outputs[1], tics_2 / 24, tics_2); - uint16_t tics_3 = (_parameters.pwm_min + (_pwm_fullscale * ((double)outputs[2] / VOXL2_IO_MIXER_MAX))) * VOXL2_IO_TICS; - PX4_INFO("\tPWM CH3: %u::%uus::%u tics", outputs[2], tics_3 / 24, tics_3); - uint16_t tics_4 = (_parameters.pwm_min + (_pwm_fullscale * ((double)outputs[3] / VOXL2_IO_MIXER_MAX))) * VOXL2_IO_TICS; - PX4_INFO("\tPWM CH4: %u::%uus::%u tics", outputs[3], tics_4 / 24, tics_4); - PX4_INFO(""); - } - - if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("Failed to send packet"); + if (_uart_port.write(cmd.buf, cmd.len) != cmd.len) { + PX4_ERR("VOXL2_IO: Failed to send packet"); return false; } else { @@ -377,40 +326,39 @@ bool Voxl2IO::updateOutputs(bool stop_motors, uint16_t outputs[input_rc_s::RC_IN _packets_sent++; } + //if (_pwm_on && _debug){ + if (_debug) { + PX4_INFO("VOXL2_IO: Mixer outputs: [%u, %u, %u, %u, %u, %u, %u, %u]", + outputs[0], outputs[1], outputs[2], outputs[3], + outputs[4], outputs[5], outputs[6], outputs[7]); + } + perf_count(_output_update_perf); return true; } -int Voxl2IO::flush_uart_rx() -{ - while (_uart_port->uart_read(_read_buf, sizeof(_read_buf)) > 0) {} - - return 0; -} - static bool valid_port(int port) { - if (port == 2 || port == 6 || port == 7) { return true; } + if (port == 2 || port == 6 || port == 7) { + return true; + } return false; } +/* int Voxl2IO::parse_response(uint8_t *buf, uint8_t len) { for (int i = 0; i < len; i++) { - int16_t ret = voxl2_io_packet_process_char(buf[i], &_sbus_packet); + int16_t ret = voxl2_io_packet_process_char(buf[i], &_voxl2_io_packet); if (ret > 0) { - uint8_t packet_type = voxl2_io_packet_get_type(&_sbus_packet); - uint8_t packet_size = voxl2_io_packet_get_size(&_sbus_packet); + uint8_t packet_type = voxl2_io_packet_get_type(&_voxl2_io_packet); + uint8_t packet_size = voxl2_io_packet_get_size(&_voxl2_io_packet); if (packet_type == VOXL2_IO_PACKET_TYPE_RC_DATA_RAW && packet_size == VOXL2_IO_SBUS_FRAME_SIZE) { return 0; - - } else if (packet_type == VOXL2_IO_PACKET_TYPE_VERSION_RESPONSE && packet_size == sizeof(VOXL2_IO_VERSION_INFO)) { - return 0; - } else { return -1; } @@ -418,36 +366,31 @@ int Voxl2IO::parse_response(uint8_t *buf, uint8_t len) } else { //parser error switch (ret) { case VOXL2_IO_ERROR_BAD_CHECKSUM: - if (_pwm_on && _debug) { PX4_WARN("BAD packet checksum"); } - + if(_pwm_on && _debug) PX4_WARN("VOXL2_IO: BAD packet checksum"); break; case VOXL2_IO_ERROR_BAD_LENGTH: - if (_pwm_on && _debug) { PX4_WARN("BAD packet length"); } - + if(_pwm_on && _debug) PX4_WARN("VOXL2_IO: BAD packet length"); break; case VOXL2_IO_ERROR_BAD_HEADER: - if (_pwm_on && _debug) { PX4_WARN("BAD packet header"); } - + if(_pwm_on && _debug) PX4_WARN("VOXL2_IO: BAD packet header"); break; case VOXL2_IO_NO_PACKET: - // if(_pwm_on) PX4_WARN("NO packet"); + // if(_pwm_on) PX4_WARN("VOXL2_IO: NO packet"); break; default: - if (_pwm_on && _debug) { PX4_WARN("Unknown error: %i", ret); } - + if(_pwm_on && _debug) PX4_WARN("VOXL2_IO: Unknown error: %i", ret); break; } - - return ret; } } return 0; } +*/ void Voxl2IO::fill_rc_in(uint16_t raw_rc_count_local, uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS], @@ -455,6 +398,8 @@ void Voxl2IO::fill_rc_in(uint16_t raw_rc_count_local, unsigned frame_drops, int rssi, input_rc_s &input_rc) { // fill rc_in struct for publishing + memset(&input_rc, 0, sizeof(input_rc)); //zero out the struct first + input_rc.channel_count = raw_rc_count_local; if (input_rc.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) { @@ -474,9 +419,9 @@ void Voxl2IO::fill_rc_in(uint16_t raw_rc_count_local, _raw_rc_values[i] = UINT16_MAX; } - input_rc.timestamp = now; + input_rc.timestamp = now; input_rc.timestamp_last_signal = input_rc.timestamp; - input_rc.rc_ppm_frame_length = 0; + input_rc.rc_ppm_frame_length = 0; /* fake rssi if no value was provided */ if (rssi == -1) { @@ -494,131 +439,98 @@ void Voxl2IO::fill_rc_in(uint16_t raw_rc_count_local, _sbus_frame_drops++; } - input_rc.rc_failsafe = failsafe; - input_rc.rc_lost = input_rc.rc_failsafe; - input_rc.rc_lost_frame_count = _sbus_frame_drops; + input_rc.rssi_dbm = 0.0f; + input_rc.rc_failsafe = failsafe; + input_rc.rc_lost = input_rc.rc_failsafe; + input_rc.rc_lost_frame_count = _sbus_frame_drops; input_rc.rc_total_frame_count = ++_sbus_total_frames; } -int Voxl2IO::receive_sbus() + +int Voxl2IO::parse_sbus_packet(uint8_t *raw_data, uint32_t data_len) { - int res = 0; - int header = -1; - int read_retries = 3; - int read_succeeded = 0; - voxl2_io_packet_init(&_sbus_packet); + input_rc_s input_rc; + uint16_t num_values; + bool sbus_failsafe = false; + bool sbus_frame_drop = false; + uint16_t max_channels = sizeof(_raw_rc_values) / sizeof(_raw_rc_values[0]); + hrt_abstime t_now = hrt_absolute_time(); - while (read_retries) { - memset(&_read_buf, 0x00, READ_BUF_SIZE); - res = _uart_port->uart_read(_read_buf, READ_BUF_SIZE); - if (res) { - /* Get index of packer header */ - for (int index = 0; index < READ_BUF_SIZE; ++index) { - if (_read_buf[index] == VOXL2_IO_PACKET_HEADER) { - header = index; - break; - } - } + bool rc_updated = sbus_parse(t_now, raw_data, data_len, _raw_rc_values, &num_values, + &sbus_failsafe, &sbus_frame_drop, &_sbus_frame_drops, max_channels); - /* Try again in a bit if packet header not present yet... */ - if (header == -1) { - if (_debug) { PX4_ERR("Failed to find SBUS packet header, trying again... retries left: %i", read_retries); } + if (rc_updated) { + //if (_pwm_on && _debug){ + if (_debug) { + //PX4_INFO("VOXL2_IO: Decoded packet, header pos: %i", header); + PX4_INFO("VOXL2_IO: [%u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u]", + _raw_rc_values[0], _raw_rc_values[1], _raw_rc_values[2], + _raw_rc_values[3], _raw_rc_values[4], _raw_rc_values[5], + _raw_rc_values[6], _raw_rc_values[7], _raw_rc_values[8], + _raw_rc_values[9], _raw_rc_values[10], _raw_rc_values[11], + _raw_rc_values[12], _raw_rc_values[13], _raw_rc_values[14], + _raw_rc_values[15], _raw_rc_values[16], _raw_rc_values[17] + ); - read_retries--; - continue; - } - - /* Check if we got a valid packet...*/ - if (parse_response(&_read_buf[header], (uint8_t)VOXL2_IO_SBUS_FRAME_SIZE)) { - if (_pwm_on && _debug) { - PX4_ERR("Error parsing QC RAW SBUS packet"); - PX4_INFO_RAW("[%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x]\n", - _read_buf[header + 0], _read_buf[header + 1], _read_buf[header + 2], _read_buf[header + 3], _read_buf[header + 4], - _read_buf[header + 5], - _read_buf[header + 6], _read_buf[header + 7], _read_buf[header + 8], _read_buf[header + 9], _read_buf[header + 10], - _read_buf[header + 11], - _read_buf[header + 12], _read_buf[header + 13], _read_buf[header + 14], _read_buf[header + 15], _read_buf[header + 16], - _read_buf[header + 17], - _read_buf[header + 18], _read_buf[header + 19], _read_buf[header + 20], _read_buf[header + 21], _read_buf[header + 22], - _read_buf[header + 23], - _read_buf[header + 24], _read_buf[header + 25], _read_buf[header + 26], _read_buf[header + 27], _read_buf[header + 28], - _read_buf[header + 29] - ); - } - - read_retries--; - break; - } - - input_rc_s input_rc; - uint16_t num_values; - bool sbus_failsafe = false; - bool sbus_frame_drop = false; - uint16_t max_channels = sizeof(_raw_rc_values) / sizeof(_raw_rc_values[0]); - hrt_abstime now = hrt_absolute_time(); - bool rc_updated = sbus_parse(now, &_read_buf[header + SBUS_PAYLOAD], SBUS_FRAME_SIZE, _raw_rc_values, &num_values, - &sbus_failsafe, &sbus_frame_drop, &_sbus_frame_drops, max_channels); - - if (rc_updated) { - if (_pwm_on && _debug) { - PX4_INFO("Decoded packet, header pos: %i", header); - PX4_INFO("[%u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u]", - _raw_rc_values[0], _raw_rc_values[1], _raw_rc_values[2], - _raw_rc_values[3], _raw_rc_values[4], _raw_rc_values[5], - _raw_rc_values[6], _raw_rc_values[7], _raw_rc_values[8], - _raw_rc_values[9], _raw_rc_values[10], _raw_rc_values[11], - _raw_rc_values[12], _raw_rc_values[13], _raw_rc_values[14], - _raw_rc_values[15], _raw_rc_values[16], _raw_rc_values[17] - ); - } - - input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS; - fill_rc_in(num_values, _raw_rc_values, now, sbus_frame_drop, sbus_failsafe, _sbus_frame_drops, -1, input_rc); - - if (!input_rc.rc_lost && !input_rc.rc_failsafe) { - _rc_last_valid = input_rc.timestamp; - } - - input_rc.timestamp_last_signal = _rc_last_valid; - _rc_pub.publish(input_rc); - - _bytes_received += res; - _packets_received++; - read_succeeded = 1; - break; - - } else if (_pwm_on && _debug) { - PX4_ERR("Failed to decode SBUS packet, header pos: %i", header); - - if (sbus_frame_drop) { - PX4_WARN("SBUS frame dropped"); - } - - PX4_ERR("[%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x]", - _read_buf[header + 0], _read_buf[header + 1], _read_buf[header + 2], _read_buf[header + 3], _read_buf[header + 4], - _read_buf[header + 5], - _read_buf[header + 6], _read_buf[header + 7], _read_buf[header + 8], _read_buf[header + 9], _read_buf[header + 10], - _read_buf[header + 11], - _read_buf[header + 12], _read_buf[header + 13], _read_buf[header + 14], _read_buf[header + 15], _read_buf[header + 16], - _read_buf[header + 17], - _read_buf[header + 18], _read_buf[header + 19], _read_buf[header + 20], _read_buf[header + 21], _read_buf[header + 22], - _read_buf[header + 23], - _read_buf[header + 24], _read_buf[header + 25], _read_buf[header + 26], _read_buf[header + 27], _read_buf[header + 28], - _read_buf[header + 29] - ); + if (sbus_frame_drop) { + PX4_WARN("VOXL2_IO: SBUS frame dropped"); } } - read_retries--; + input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS; + fill_rc_in(num_values, _raw_rc_values, t_now, sbus_frame_drop, sbus_failsafe, _sbus_frame_drops, -1, input_rc); + + if (!input_rc.rc_lost && !input_rc.rc_failsafe) { + _rc_last_valid_time = input_rc.timestamp; + } + + input_rc.timestamp_last_signal = _rc_last_valid_time; + _rc_pub.publish(input_rc); + + if (_rc_mode == RC_MODE::SCAN) { + PX4_INFO("VOXL2_IO: Detected VOXL2 IO SBUS RC"); + _rc_mode = RC_MODE::SBUS; + } } - if (! read_succeeded) { - _new_packet = false; - return -EIO; + return 0; +} + + +int Voxl2IO::receive_uart_packets() +{ + int nread = _uart_port.read(_read_buf, READ_BUF_SIZE); + + if (nread > 0) { + if (_debug) { + PX4_INFO("VOXL2_IO: receive_uart_packets read %d bytes", nread); + } + + _bytes_received += nread; + + for (int i = 0; i < nread; i++) { + int16_t parse_ret = voxl2_io_packet_process_char(_read_buf[i], &_voxl2_io_packet); + + if (parse_ret > 0) { + _packets_received++; + + uint8_t packet_type = voxl2_io_packet_get_type(&_voxl2_io_packet); + uint8_t packet_size = voxl2_io_packet_get_size(&_voxl2_io_packet); + + if (packet_type == VOXL2_IO_PACKET_TYPE_RC_DATA_RAW && packet_size == VOXL2_IO_SBUS_FRAME_SIZE) { + + //parse SBUS packet only if configured to do so + if ((_rc_mode == RC_MODE::SCAN) || (_rc_mode == RC_MODE::SBUS)) { + parse_sbus_packet(&_voxl2_io_packet.buffer[SBUS_PAYLOAD], SBUS_FRAME_SIZE); + } + } + + //parse other packets (future use) + } + } } - _new_packet = true; return 0; } @@ -635,26 +547,14 @@ void Voxl2IO::Run() perf_begin(_cycle_perf); - /* Handle RC */ - if (_rc_mode == RC_MODE::SCAN) { - if (receive_sbus() == PX4_OK) { - PX4_INFO("Found M0065 SBUS RC."); - _rc_mode = RC_MODE::SBUS; - } // Add more cases here for other protocols in the future.. - - } else if (_rc_mode == RC_MODE::SBUS) { - receive_sbus(); - } - - /* Only update outputs if we have new values from RC */ - if (_new_packet || _rc_mode == RC_MODE::EXTERNAL) { - _mixing_output.update(); //calls MixingOutput::limitAndUpdateOutputs which calls updateOutputs in this module - _new_packet = false; - } + _mixing_output.update(); //calls MixingOutput::limitAndUpdateOutputs which calls updateOutputs in this module /* update PWM status if armed or if disarmed PWM values are set */ _pwm_on = _mixing_output.armed().armed; + //receive packets from voxl_io board + receive_uart_packets(); + /* check for parameter updates */ if (!_pwm_on && _parameter_update_sub.updated()) { /* clear update */ @@ -665,48 +565,6 @@ void Voxl2IO::Run() update_params(); } - /* Don't process commands if pwm on */ - if (!_pwm_on) { - if (_current_cmd.valid()) { - PX4_INFO("sending %d commands with delay %dus", _current_cmd.repeats, _current_cmd.repeat_delay_us); - flush_uart_rx(); - - do { - PX4_INFO("CMDs left %d", _current_cmd.repeats); - - if (_uart_port->uart_write(_current_cmd.buf, _current_cmd.len) == _current_cmd.len) { - if (_current_cmd.repeats == 0) { - _current_cmd.clear(); - } - - - } else { - _bytes_sent += _current_cmd.len; - _packets_sent++; - - if (_current_cmd.retries == 0) { - _current_cmd.clear(); - PX4_ERR("Failed to send command, errno: %i", errno); - - } else { - _current_cmd.retries--; - PX4_ERR("Failed to send command, errno: %i", errno); - } - } - - px4_usleep(_current_cmd.repeat_delay_us); - } while (_current_cmd.repeats-- > 0); - - } else { - Command *new_cmd = _pending_cmd.load(); - - if (new_cmd) { - _current_cmd = *new_cmd; - _pending_cmd.store(nullptr); - } - } - } - /* check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread) */ _mixing_output.updateSubscriptions(true); perf_end(_cycle_perf); @@ -728,17 +586,17 @@ int Voxl2IO::task_spawn(int argc, char *argv[]) while ((ch = px4_getopt(argc - 1, argv, "vdep:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'v': - PX4_INFO("Verbose mode enabled"); + PX4_INFO("VOXL2_IO: Verbose mode enabled"); get_instance()->_debug = true; break; case 'd': - PX4_INFO("M0065 PWM outputs disabled"); + PX4_INFO("VOXL2_IO: M0065 PWM outputs disabled"); get_instance()->_outputs_disabled = true; break; case 'e': - PX4_INFO("M0065 using external RC"); + PX4_INFO("VOXL2_IO: M0065 using external RC"); get_instance()->_rc_mode = RC_MODE::EXTERNAL; break; @@ -747,7 +605,7 @@ int Voxl2IO::task_spawn(int argc, char *argv[]) snprintf(get_instance()->_device, 2, "%s", myoptarg); } else { - PX4_ERR("Bad UART port number: %s (must be 2, 6, or 7).", myoptarg); + PX4_ERR("VOXL2_IO: Bad UART port number: %s (must be 2, 6, or 7).", myoptarg); _object.store(nullptr); _task_id = -1; return PX4_ERROR; @@ -756,7 +614,7 @@ int Voxl2IO::task_spawn(int argc, char *argv[]) break; default: - print_usage("Unknown command, parsing flags"); + print_usage("VOXL2_IO: Unknown command, parsing flags"); break; } } @@ -766,7 +624,7 @@ int Voxl2IO::task_spawn(int argc, char *argv[]) } } else { - PX4_ERR("alloc failed"); + PX4_ERR("VOXL2_IO: alloc failed"); } _object.store(nullptr); @@ -776,280 +634,131 @@ int Voxl2IO::task_spawn(int argc, char *argv[]) } -bool Voxl2IO::stop_all_pwms() -{ - int16_t _rate_req[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0}; - int16_t _led_req[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0}; - uint8_t _fb_idx = 0; - - Command cmd; - cmd.len = voxl2_io_create_pwm_packet4_fb(_rate_req[0], _rate_req[1], _rate_req[2], _rate_req[3], - _led_req[0], _led_req[1], _led_req[2], _led_req[3], - _fb_idx, cmd.buf, sizeof(cmd.buf)); - - if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("Failed to send packet"); - return false; - - } else { - _bytes_sent += cmd.len; - _packets_sent++; - } - - return true; -} - -int Voxl2IO::send_cmd_thread_safe(Command *cmd) -{ - cmd->id = _cmd_id++; - _pending_cmd.store(cmd); - - /* wait until main thread processed it */ - while (_pending_cmd.load()) { - px4_usleep(1000); - } - - return 0; -} - int Voxl2IO::calibrate_escs() { - /* Disable outputs so Mixer isn't being a PITA while we calibrate */ + // Disable outputs so Mixer isn't being a PITA while we calibrate _outputs_disabled = true; Command cmd; - int32_t fb_idx = -1; - uint8_t data[VOXL2_IO_ESC_CAL_SIZE] {0}; - cmd.len = voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_TUNE_CONFIG, data, VOXL2_IO_ESC_CAL_SIZE, cmd.buf, - sizeof(cmd.buf)); - if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("ESC Calibration failed: Failed to send PWM OFF packet"); - _outputs_disabled = false; - return -1; + PX4_INFO("VOXL2_IO: PWM ESC calibration is starting!"); + + // Give user 10 seconds to plug in PWM cable for ESCs + PX4_INFO("VOXL2_IO: MIN PWM used for ESC Calibration : %" PRId32, _parameters.pwm_cal_min); + PX4_INFO("VOXL2_IO: MAX PWM used for ESC Calibration : %" PRId32, _parameters.pwm_cal_max); + PX4_INFO("VOXL2_IO:"); + PX4_INFO("VOXL2_IO: Connect your ESCs! (Calibration will start in ~10 seconds)"); + + px4_usleep(10000000); + + uint32_t max_pwm_cmds[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0, 0, 0, 0, 0}; + uint32_t min_pwm_cmds[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0, 0, 0, 0, 0}; + + for (int idx = 0; idx < VOXL2_IO_OUTPUT_CHANNELS; idx++) { + //only send out calibration pulse if the actuator is a motor + if ((_parameters.function_map[idx] >= 101) && (_parameters.function_map[idx] <= 112)) { + max_pwm_cmds[idx] = _parameters.pwm_cal_max * MIXER_OUTPUT_TO_CMD_SCALE; + min_pwm_cmds[idx] = _parameters.pwm_cal_min * MIXER_OUTPUT_TO_CMD_SCALE; + } } - /* Give user 10 seconds to plug in PWM cable for ESCs */ - PX4_INFO("Disconnected and reconnect your ESCs! (Calibration will start in ~10 seconds)"); - hrt_abstime start_cal = hrt_absolute_time(); - - while (hrt_elapsed_time(&start_cal) < 10000000) { - continue; + if (_debug) { + PX4_INFO("VOXL2_IO: Scaled max pwms: %u %u %u %u %u %u %u %u", + max_pwm_cmds[0], max_pwm_cmds[1], max_pwm_cmds[2], max_pwm_cmds[3], + max_pwm_cmds[4], max_pwm_cmds[5], max_pwm_cmds[6], max_pwm_cmds[7]); } - /* PWM MAX 3 seconds */ - PX4_INFO("Writing PWM MAX for 3 seconds!"); - int16_t max_pwm[4] {VOXL2_IO_MIXER_MAX, VOXL2_IO_MIXER_MAX, VOXL2_IO_MIXER_MAX, VOXL2_IO_MIXER_MAX}; + hrt_abstime start; - if (_debug) { PX4_INFO("%i %i %i %i", max_pwm[0], max_pwm[1], max_pwm[2], max_pwm[3]); } + // Send PWM max every 2.5ms for 5 seconds + PX4_INFO("VOXL2_IO: Sending PWM MAX (%dus) for 5 seconds!", _parameters.pwm_cal_max); + cmd.len = voxl2_io_create_hires_pwm_packet(max_pwm_cmds, VOXL2_IO_OUTPUT_CHANNELS, cmd.buf, sizeof(cmd.buf)); + start = hrt_absolute_time(); - int16_t led_cmd[4] {0, 0, 0, 0}; - cmd.len = voxl2_io_create_pwm_packet4_fb(max_pwm[0], max_pwm[1], max_pwm[2], max_pwm[3], - led_cmd[0], led_cmd[1], led_cmd[2], led_cmd[3], - fb_idx, cmd.buf, sizeof(cmd.buf)); + while (hrt_elapsed_time(&start) < 5000000) { + if (_uart_port.write(cmd.buf, cmd.len) != cmd.len) { + PX4_ERR("VOXL2_IO: ESC Calibration failed: Failed to send PWM MAX packet"); + _outputs_disabled = false; + return -1; + } - if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("ESC Calibration failed: Failed to send PWM MAX packet"); - _outputs_disabled = false; - return -1; - - } else { - cmd.clear(); + px4_usleep(2500); } - hrt_abstime start_pwm_max = hrt_absolute_time(); + // Send PWM min every 2.5ms for 5 seconds + PX4_INFO("VOXL2_IO: Sending PWM MIN (%dus) for 5 seconds!", _parameters.pwm_cal_min); - while (hrt_elapsed_time(&start_pwm_max) < 3000000) { - continue; + if (_debug) { + PX4_INFO("VOXL2_IO: Scaled min pwms: %u %u %u %u %u %u %u %u", + min_pwm_cmds[0], min_pwm_cmds[1], min_pwm_cmds[2], min_pwm_cmds[3], + min_pwm_cmds[4], min_pwm_cmds[5], min_pwm_cmds[6], min_pwm_cmds[7]); } - /* PWM MIN 4 seconds */ - PX4_INFO("Writing PWM MIN for 4 seconds!"); - int16_t min_pwm[4] {VOXL2_IO_MIXER_MIN, VOXL2_IO_MIXER_MIN, VOXL2_IO_MIXER_MIN, VOXL2_IO_MIXER_MIN}; + cmd.len = voxl2_io_create_hires_pwm_packet(min_pwm_cmds, VOXL2_IO_OUTPUT_CHANNELS, cmd.buf, sizeof(cmd.buf)); - if (_debug) { PX4_INFO("%i %i %i %i", min_pwm[0], min_pwm[1], min_pwm[2], min_pwm[3]); } - cmd.len = voxl2_io_create_pwm_packet4_fb(min_pwm[0], min_pwm[1], min_pwm[2], min_pwm[3], - led_cmd[0], led_cmd[1], led_cmd[2], led_cmd[3], - fb_idx, cmd.buf, sizeof(cmd.buf)); + start = hrt_absolute_time(); - if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("ESC Calibration failed: Failed to send PWM MIN packet"); - _outputs_disabled = false; - return -1; + while (hrt_elapsed_time(&start) < 5000000) { + if (_uart_port.write(cmd.buf, cmd.len) != cmd.len) { + PX4_ERR("VOXL2_IO: ESC Calibration failed: Failed to send PWM MIN packet"); + _outputs_disabled = false; + return -1; + } + + px4_usleep(2500); } - hrt_abstime start_pwm_min = hrt_absolute_time(); - - while (hrt_elapsed_time(&start_pwm_min) < 4000000) { - continue; - } - - PX4_INFO("ESC Calibration complete"); + PX4_INFO("VOXL2_IO: Waiting 5 seconds to finish the calibration (no PWM output)"); + px4_usleep(5000000); + PX4_INFO("VOXL2_IO: ESC Calibration complete"); _outputs_disabled = false; return 0; } int Voxl2IO::custom_command(int argc, char *argv[]) { - int myoptind = 0; - int ch; - const char *myoptarg = nullptr; - - Command cmd; - uint8_t output_channel = 0xF; - int16_t rate = 0; - - uint32_t repeat_count = 100; - uint32_t repeat_delay_us = 10000; - const char *verb = argv[argc - 1]; - if ((strcmp(verb, "pwm")) == 0 && argc < 3) { - return print_usage("pwm command: missing args"); - - } else if (argc < 1) { + if (argc < 1) { return print_usage("unknown command: missing args"); } - PX4_INFO("Executing the following command: %s", verb); + PX4_INFO("VOXL2_IO: Executing the following command: %s", verb); - /* start the FMU if not running */ + /* start if not running */ if (!strcmp(verb, "start")) { if (!is_running()) { return Voxl2IO::task_spawn(argc, argv); } - } - if (!strcmp(verb, "status")) { - if (!is_running()) { - PX4_INFO("Not running"); - return -1; - } - - return get_instance()->print_status(); + PX4_INFO("VOXL2_IO: Already running"); + return 0; } if (!is_running()) { - PX4_INFO("Not running"); + PX4_INFO("VOXL2_IO: Not running"); return -1; } + if (!strcmp(verb, "status")) { + return get_instance()->print_status(); + } + + if (!strcmp(verb, "calibrate_escs")) { if (get_instance()->_outputs_disabled) { - PX4_WARN("Can't calibrate ESCs while outputs are disabled."); + PX4_WARN("VOXL2_IO: Can't calibrate ESCs while outputs are disabled."); return -1; } return get_instance()->calibrate_escs(); } - while ((ch = px4_getopt(argc, argv, "c:n:t:r:p:", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'c': - output_channel = atoi(myoptarg); - - if (output_channel > VOXL2_IO_OUTPUT_CHANNELS - 1) { - char reason[50]; - sprintf(reason, "Bad channel value: %d. Must be 0-%d.", output_channel, VOXL2_IO_OUTPUT_CHANNELS - 1); - print_usage(reason); - return 0; - } - - break; - - case 'n': - repeat_count = atoi(myoptarg); - - if (repeat_count < 1) { - print_usage("bad repeat_count"); - return 0; - } - - break; - - case 't': - repeat_delay_us = atoi(myoptarg); - - if (repeat_delay_us < 1) { - print_usage("bad repeat delay"); - return 0; - } - - break; - - case 'r': - rate = atoi(myoptarg); - break; - - case 'p': - if (valid_port(atoi(myoptarg))) { - snprintf(get_instance()->_device, 2, "%s", myoptarg); - - } else { - PX4_ERR("Bad UART port number: %s (must be 2, 6, or 7).", myoptarg); - return 0; - } - - break; - - default: - print_usage("Unknown command, parsing flags"); - return 0; - } - } - - if (!strcmp(verb, "pwm")) { - PX4_INFO("Output channel: %i", output_channel); - PX4_INFO("Repeat count: %i", repeat_count); - PX4_INFO("Repeat delay (us): %i", repeat_delay_us); - PX4_INFO("Rate: %i", rate); - - if (output_channel < VOXL2_IO_OUTPUT_CHANNELS) { - PX4_INFO("Request PWM for Output Channel: %i - PWM: %i", output_channel, rate); - int16_t rate_req[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0}; - uint8_t id_fb = 0; - - if (output_channel == - 0xFF) { //WARNING: this condition is not possible due to check 'if (esc_id < MODAL_IO_OUTPUT_CHANNELS)'. - rate_req[0] = rate; - rate_req[1] = rate; - rate_req[2] = rate; - rate_req[3] = rate; - - } else { - rate_req[output_channel] = rate; - id_fb = output_channel; - } - - cmd.len = voxl2_io_create_pwm_packet4_fb(rate_req[0], rate_req[1], rate_req[2], rate_req[3], - 0, 0, 0, 0, - id_fb, cmd.buf, sizeof(cmd.buf)); - - cmd.response = false; - cmd.repeats = repeat_count; - cmd.resp_delay_us = 1000; - cmd.repeat_delay_us = repeat_delay_us; - cmd.print_feedback = false; - - PX4_INFO("feedback id debug: %i", id_fb); - PX4_INFO("Sending UART M0065 power command %i", rate); - - if (get_instance()->_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("Failed to send packet: stop PWMs"); - return -1; - - } else { - get_instance()->_bytes_sent += cmd.len; - get_instance()->_packets_sent++; - } - - } else { - print_usage("Invalid Output Channel, use 0-3"); - return 0; - } + if (!strcmp(verb, "enable_debug")) { + get_instance()->_debug = true; } return print_usage("unknown custom command"); @@ -1057,22 +766,20 @@ int Voxl2IO::custom_command(int argc, char *argv[]) int Voxl2IO::print_status() { - PX4_INFO("Max update rate: %u Hz", 1000000 / _current_update_interval); - PX4_INFO("PWM Rate: 400 Hz"); // Only support 400 Hz for now - PX4_INFO("Outputs on: %s", _pwm_on ? "yes" : "no"); - PX4_INFO("FW version: v%u.%u", _version_info.sw_version, _version_info.hw_version); - PX4_INFO("RC Type: SBUS"); // Only support SBUS through M0065 for now - PX4_INFO("RC Connected: %s", hrt_absolute_time() - _rc_last_valid > 500000 ? "no" : "yes"); - PX4_INFO("RC Packets Received: %" PRIu16, _sbus_total_frames); - PX4_INFO("UART port: %s", _device); - PX4_INFO("UART open: %s", _uart_port->is_open() ? "yes" : "no"); - PX4_INFO("Packets sent: %" PRIu32, _packets_sent); - PX4_INFO(""); - PX4_INFO("Params: VOXL2_IO_BAUD: %" PRId32, _parameters.baud_rate); - PX4_INFO("Params: VOXL2_IO_FUNC1: %" PRId32, _parameters.function_map[0]); - PX4_INFO("Params: VOXL2_IO_FUNC2: %" PRId32, _parameters.function_map[1]); - PX4_INFO("Params: VOXL2_IO_FUNC3: %" PRId32, _parameters.function_map[2]); - PX4_INFO("Params: VOXL2_IO_FUNC4: %" PRId32, _parameters.function_map[3]); + //PX4_INFO("VOXL2_IO: Max update rate: %u Hz", 1000000/_current_update_interval); + //PX4_INFO("VOXL2_IO: PWM Rate: 400 Hz"); // Only support 400 Hz for now + PX4_INFO("VOXL2_IO: Outputs on : %s", _pwm_on ? "yes" : "no"); + PX4_INFO("VOXL2_IO: SW version : %u", _version_info.sw_version); + PX4_INFO("VOXL2_IO: HW version : %u: %s", _version_info.hw_version, + board_id_to_name(_version_info.hw_version).c_str()); + PX4_INFO("VOXL2_IO: RC Type : SBUS"); // Only support SBUS through M0065 for now + PX4_INFO("VOXL2_IO: RC Connected : %s", hrt_absolute_time() - _rc_last_valid_time > 500000 ? "no" : "yes"); + PX4_INFO("VOXL2_IO: RC Packets Rxd : %" PRIu16, _sbus_total_frames); + PX4_INFO("VOXL2_IO: UART port : %s", _device); + PX4_INFO("VOXL2_IO: UART open : %s", _uart_port.open() ? "yes" : "no"); + PX4_INFO("VOXL2_IO: Packets sent : %" PRIu32, _packets_sent); + PX4_INFO("VOXL2_IO: "); + print_params(); perf_print_counter(_cycle_perf); perf_print_counter(_output_update_perf); @@ -1103,17 +810,49 @@ px4io driver is used for main ones. PRINT_MODULE_USAGE_PARAM_FLAG('e', "Disable RC", false); PRINT_MODULE_USAGE_PARAM_INT('p', 2, 2, 7, "UART port", false); PRINT_MODULE_USAGE_COMMAND_DESCR("calibrate_escs", "Calibrate ESCs min/max range"); - PRINT_MODULE_USAGE_COMMAND_DESCR("calibrate_escs", "Calibrate ESCs min/max range"); - PRINT_MODULE_USAGE_COMMAND_DESCR("pwm", "Open-Loop PWM test control request"); - PRINT_MODULE_USAGE_PARAM_INT('c', 0, 0, 3, "PWM OUTPUT Channel, 0-3", false); - PRINT_MODULE_USAGE_PARAM_INT('r', 0, 0, 800, "Duty Cycle value, 0 to 800", false); - PRINT_MODULE_USAGE_PARAM_INT('n', 100, 0, 1<<31, "Command repeat count, 0 to INT_MAX", false); - PRINT_MODULE_USAGE_PARAM_INT('t', 10000, 0, 1<<31, "Delay between repeated commands (microseconds), 0 to INT_MAX", false); + PRINT_MODULE_USAGE_COMMAND_DESCR("enable_debug", "Enables driver debugging"); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; } +std::string Voxl2IO::board_id_to_name(int board_id) +{ + switch(board_id){ + case 31: return std::string("ModalAi 4-in-1 ESC V2 RevB (M0049)"); + case 32: return std::string("Blheli32 4-in-1 ESC Type A (Tmotor F55A PRO F051)"); + case 33: return std::string("Blheli32 4-in-1 ESC Type B (Tmotor F55A PRO G071)"); + case 34: return std::string("ModalAi 4-in-1 ESC (M0117-1)"); + case 35: return std::string("ModalAi I/O Expander (M0065)"); + case 36: return std::string("ModalAi 4-in-1 ESC (M0117-3)"); + case 37: return std::string("ModalAi 4-in-1 ESC (M0134-1)"); + case 38: return std::string("ModalAi 4-in-1 ESC (M0134-3)"); + case 39: return std::string("ModalAi 4-in-1 ESC (M0129-1)"); + case 40: return std::string("ModalAi 4-in-1 ESC (M0129-3)"); + case 41: return std::string("ModalAi 4-in-1 ESC (M0134-6)"); + case 42: return std::string("ModalAi 4-in-1 ESC (M0138-1)"); + default: return std::string("Unknown Board"); + } +} + +void Voxl2IO::print_params() +{ + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_BAUD : %" PRId32, _parameters.baud_rate); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_FUNC1 : %" PRId32, _parameters.function_map[0]); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_FUNC2 : %" PRId32, _parameters.function_map[1]); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_FUNC3 : %" PRId32, _parameters.function_map[2]); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_FUNC4 : %" PRId32, _parameters.function_map[3]); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_FUNC5 : %" PRId32, _parameters.function_map[4]); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_FUNC6 : %" PRId32, _parameters.function_map[5]); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_FUNC7 : %" PRId32, _parameters.function_map[6]); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_FUNC8 : %" PRId32, _parameters.function_map[7]); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_DIS : %" PRId32, _parameters.pwm_dis); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_MIN : %" PRId32, _parameters.pwm_min); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_MAX : %" PRId32, _parameters.pwm_max); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_CMIN : %" PRId32, _parameters.pwm_cal_min); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_CMAX : %" PRId32, _parameters.pwm_cal_max); +} + extern "C" __EXPORT int voxl2_io_main(int argc, char *argv[]); int voxl2_io_main(int argc, char *argv[]) diff --git a/src/drivers/voxl2_io/voxl2_io.hpp b/src/drivers/voxl2_io/voxl2_io.hpp index 6ac46251bc..b3458456a0 100644 --- a/src/drivers/voxl2_io/voxl2_io.hpp +++ b/src/drivers/voxl2_io/voxl2_io.hpp @@ -35,6 +35,7 @@ #include #include +#include #include @@ -49,16 +50,17 @@ #include #include #include +#include #include #include #include #include -#include "voxl2_io_serial.hpp" - #include "voxl2_io_packet.h" #include "voxl2_io_packet_types.h" +using namespace device; + using namespace time_literals; class Voxl2IO final : public ModuleBase, public OutputModuleInterface @@ -85,8 +87,8 @@ public: virtual int init(); - void update_pwm_config(); int get_version_info(); + void print_params(); struct Command { uint16_t id = 0; @@ -105,8 +107,8 @@ public: void clear() { len = 0; } }; - int send_cmd_thread_safe(Command *cmd); - int receive_sbus(); + int receive_uart_packets(); + int parse_sbus_packet(uint8_t *raw_data, uint32_t data_len); void fill_rc_in(uint16_t raw_rc_count_local, uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS], @@ -114,53 +116,42 @@ public: unsigned frame_drops, int rssi, input_rc_s &input_rc); private: void Run() override; - bool stop_all_pwms(); /* PWM Parameters */ - static constexpr uint32_t VOXL2_IO_CONFIG = 0; // Default to off - static constexpr uint32_t VOXL2_IO_BOARD_CONFIG_SIZE = 4; // PWM_MIN, PWM_MAX, 4 bytes - static constexpr uint32_t VOXL2_IO_ESC_CAL_SIZE = 1; - static constexpr uint32_t VOXL2_IO_DEFAULT_BAUD = 921600; - static constexpr uint16_t VOXL2_IO_OUTPUT_CHANNELS = 4; - static constexpr uint16_t VOXL2_IO_OUTPUT_DISABLED = 0; - - static constexpr uint32_t VOXL2_IO_WRITE_WAIT_US = 200; - static constexpr uint32_t VOXL2_IO_DISCONNECT_TIMEOUT_US = 500000; - - static constexpr uint16_t DISARMED_VALUE = 0; - - static constexpr uint16_t VOXL2_IO_MIXER_MIN = 0; - static constexpr uint16_t VOXL2_IO_MIXER_MAX = 800; - static constexpr uint16_t VOXL2_IO_MIXER_FAILSAFE = 0; - static constexpr uint16_t VOXL2_IO_MIXER_DISARMED = 0; - - static constexpr int32_t VOXL2_IO_DEFAULT_MIN = 1000; - static constexpr int32_t VOXL2_IO_DEFAULT_MAX = 2000; - static constexpr int32_t VOXL2_IO_DEFAULT_FAILSAFE = 900; - static constexpr int32_t VOXL2_IO_TICS = 24; // 24 tics per us on M0065 timer clks + static constexpr uint32_t VOXL2_IO_DEFAULT_BAUD = 921600; + static constexpr uint16_t VOXL2_IO_OUTPUT_CHANNELS = 8; + static constexpr uint16_t VOXL2_IO_MIXER_FAILSAFE = + UINT16_MAX; //this will tell mixer to use the default failsafe depending on the function /* SBUS */ static constexpr uint16_t VOXL2_IO_SBUS_FRAME_SIZE = 30; static constexpr uint16_t SBUS_PAYLOAD = 3; + //packet packing function accepts pulse width in nanoseconds + //assuming the mixer outputs in microseconds, need to convert + static constexpr uint32_t MIXER_OUTPUT_TO_CMD_SCALE = 1000; //standard pwm + //static constexpr uint32_t MIXER_OUTPUT_TO_CMD_SCALE = 125; //oneshot125 + /* M0065 version info */ - static constexpr uint16_t VOXL2_IO_VERSION_INFO_SIZE = 6; - static constexpr uint16_t VOXL2_IO_SW_PROTOCOL_VERSION = 1; - static constexpr uint16_t VOXL2_IO_HW_PROTOCOL_VERSION = 35; - VOXL2_IO_VERSION_INFO _version_info; + static constexpr uint16_t VOXL2_IO_SW_VERSION = 2; + static constexpr uint16_t VOXL2_IO_HW_VERSION = 35; //this is the version of the HW + int _board_detect_retries{3}; + VOXL2_IO_EXTENDED_VERSION_INFO _version_info; /* Module update interval */ static constexpr unsigned _current_update_interval{4000}; // 250 Hz typedef struct { - int32_t config{VOXL2_IO_CONFIG}; int32_t baud_rate{VOXL2_IO_DEFAULT_BAUD}; - int32_t pwm_min{VOXL2_IO_DEFAULT_MIN}; - int32_t pwm_max{VOXL2_IO_DEFAULT_MAX}; - int32_t pwm_failsafe{VOXL2_IO_DEFAULT_FAILSAFE}; - int32_t param_rc_input_proto{0}; + int32_t pwm_min{0}; + int32_t pwm_max{0}; + int32_t pwm_dis{0}; + int32_t pwm_cal_min{0}; + int32_t pwm_cal_max{0}; + //int32_t pwm_failsafe{VOXL2_IO_DEFAULT_FAILSAFE}; + //int32_t param_rc_input_proto{0}; int32_t param_rc_rssi_pwm_chan{0}; - int32_t function_map[VOXL2_IO_OUTPUT_CHANNELS] {0, 0, 0, 0}; + int32_t function_map[VOXL2_IO_OUTPUT_CHANNELS] {0, 0, 0, 0, 0, 0, 0, 0}; int32_t verbose_logging{0}; } voxl2_io_params_t; voxl2_io_params_t _parameters; @@ -179,20 +170,18 @@ private: SCAN } _rc_mode{RC_MODE::SCAN}; - /* QUP7, VOXL2 J19, /dev/slpi-uart-7*/ char _device[10] {VOXL2_IO_DEFAULT_PORT}; - Voxl2IoSerial *_uart_port; + Serial _uart_port{}; /* Mixer output */ MixingOutput _mixing_output; /* RC input */ - VOXL2_IOPacket _sbus_packet; - uint64_t _rc_last_valid; + VOXL2_IOPacket _voxl2_io_packet; + uint64_t _rc_last_valid_time; uint16_t _raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {UINT16_MAX}; - unsigned _sbus_frame_drops{0}; - uint16_t _sbus_total_frames{0}; - bool _new_packet{false}; + unsigned int _sbus_frame_drops{0}; + uint32_t _sbus_total_frames{0}; /* Publications */ uORB::PublicationMulti _rc_pub{ORB_ID(input_rc)}; @@ -201,17 +190,12 @@ private: uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; bool _pwm_on{false}; - int32_t _pwm_fullscale{0}; - int16_t _pwm_values[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0}; bool _outputs_disabled{false}; perf_counter_t _cycle_perf; perf_counter_t _output_update_perf; bool _debug{false}; - uint16_t _cmd_id{0}; - Command _current_cmd; - px4::atomic _pending_cmd{nullptr}; static const uint8_t READ_BUF_SIZE = 128; uint8_t _read_buf[READ_BUF_SIZE]; @@ -220,9 +204,8 @@ private: uint32_t _packets_sent{0}; uint32_t _packets_received{0}; - int parse_response(uint8_t *buf, uint8_t len); int load_params(voxl2_io_params_t *params); int update_params(); - int flush_uart_rx(); int calibrate_escs(); + std::string board_id_to_name(int board_id); }; diff --git a/src/drivers/voxl2_io/voxl2_io_packet.c b/src/drivers/voxl2_io/voxl2_io_packet.c index 3de26acbfe..99340fa974 100644 --- a/src/drivers/voxl2_io/voxl2_io_packet.c +++ b/src/drivers/voxl2_io/voxl2_io_packet.c @@ -150,6 +150,32 @@ int32_t voxl2_io_create_rpm_packet4_fb(int16_t rpm0, int16_t rpm1, int16_t rpm2, return voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_RPM_CMD, (uint8_t *) & (data[0]), 10, out, out_size); } +typedef struct { + uint8_t command_type; + //uint8_t channel_offset; + uint16_t vals[8]; //could be 1,2,4,6,8 +} __attribute__((__packed__)) pwm_hires_cmd_t; + +pwm_hires_cmd_t hires_cmd; + +int32_t voxl2_io_create_hires_pwm_packet(uint32_t *pwm_val_ns, uint32_t cmd_cnt, uint8_t *out, uint16_t out_size) +{ + if (cmd_cnt > 8) { + return -1; + } + + hires_cmd.command_type = 0; + //hires_cmd.channel_offset = 0; + + //resolution of commands in the packet is 0.05us = 50ns + for (uint32_t idx = 0; idx < cmd_cnt; idx++) { + hires_cmd.vals[idx] = pwm_val_ns[idx] / 50; + } + + return voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_PWM_HIRES_CMD, (uint8_t *) &hires_cmd, (cmd_cnt * 2 + 1), out, + out_size); +} + int32_t voxl2_io_create_packet(uint8_t type, uint8_t *data, uint16_t size, uint8_t *out, uint16_t out_size) { uint16_t packet_size = size + 5; diff --git a/src/drivers/voxl2_io/voxl2_io_packet.h b/src/drivers/voxl2_io/voxl2_io_packet.h index 7e6237017f..1c4d242814 100644 --- a/src/drivers/voxl2_io/voxl2_io_packet.h +++ b/src/drivers/voxl2_io/voxl2_io_packet.h @@ -213,6 +213,9 @@ int32_t voxl2_io_create_rpm_packet4(int16_t rpm0, int16_t rpm1, int16_t rpm2, in uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3, uint8_t *out, uint16_t out_size); +int32_t voxl2_io_create_hires_pwm_packet(uint32_t *pwm_val_ns, uint32_t cmd_cnt, uint8_t *out, uint16_t out_size); + + // Create a packet for sending closed-loop RPM command and LED command to 4 ESCs, also request feedback from one ESC (with id=fb_id) // Return value is the length of generated packet (if positive), otherwise error code int32_t voxl2_io_create_rpm_packet4_fb(int16_t rpm0, int16_t rpm1, int16_t rpm2, int16_t rpm3, diff --git a/src/drivers/voxl2_io/voxl2_io_packet_types.h b/src/drivers/voxl2_io/voxl2_io_packet_types.h index 150982eca4..12766fb0ef 100644 --- a/src/drivers/voxl2_io/voxl2_io_packet_types.h +++ b/src/drivers/voxl2_io/voxl2_io_packet_types.h @@ -46,6 +46,7 @@ #define VOXL2_IO_PACKET_TYPE_SOUND_CMD 3 #define VOXL2_IO_PACKET_TYPE_STEP_CMD 4 #define VOXL2_IO_PACKET_TYPE_LED_CMD 5 +#define VOXL2_IO_PACKET_TYPE_PWM_HIRES_CMD 6 #define VOXL2_IO_PACKET_TYPE_RESET_CMD 10 #define VOXL2_IO_PACKET_TYPE_SET_ID_CMD 11 #define VOXL2_IO_PACKET_TYPE_SET_DIR_CMD 12 diff --git a/src/drivers/voxl2_io/voxl2_io_params.c b/src/drivers/voxl2_io/voxl2_io_params.c index 704cca989e..ac99723b8d 100644 --- a/src/drivers/voxl2_io/voxl2_io_params.c +++ b/src/drivers/voxl2_io/voxl2_io_params.c @@ -32,34 +32,73 @@ ****************************************************************************/ /** - * UART M0065 baud rate + * VOXL2_IO UART baud rate * - * Default rate is 921600, which is used for communicating with M0065. + * Default rate is 921600, which is used for communicating with VOXL2_IO board * * @group VOXL2 IO * @unit bit/s */ PARAM_DEFINE_INT32(VOXL2_IO_BAUD, 921600); + /** - * M0065 PWM Min + * VOXL2_IO Disabled PWM * - * Minimum duration (microseconds) for M0065 PWM + * Pulse duration in disabled state (microseconds) for VOXL2_IO board * * @min 0 * @max 2000 * @group VOXL2 IO * @unit us */ -PARAM_DEFINE_INT32(VOXL2_IO_MIN, 1000); +PARAM_DEFINE_INT32(VOXL2_IO_DIS, 1000); /** - * M0065 PWM Max + * VOXL2_IO Min PWM * - * Maximum duration (microseconds) for M0065 PWM + * Minimum duration (microseconds) for VOXL2_IO board + * + * @min 0 + * @max 2000 + * @group VOXL2 IO + * @unit us + */ + +PARAM_DEFINE_INT32(VOXL2_IO_MIN, 1100); + +/** + * VOXL2_IO Max PWM + * + * Maximum duration (microseconds) for VOXL2_IO board * @min 0 * @max 2000 * @group VOXL2 IO * @unit us */ PARAM_DEFINE_INT32(VOXL2_IO_MAX, 2000); + + +/** + * VOXL2_IO Calibration Min PWM + * + * Minimum duration (microseconds) for VOXL2_IO board + * + * @min 0 + * @max 2000 + * @group VOXL2 IO + * @unit us + */ + +PARAM_DEFINE_INT32(VOXL2_IO_CMIN, 1050); + +/** + * VOXL2_IO Calibration Max PWM + * + * Maximum duration (microseconds) for VOXL2_IO board + * @min 0 + * @max 2000 + * @group VOXL2 IO + * @unit us + */ +PARAM_DEFINE_INT32(VOXL2_IO_CMAX, 2000); diff --git a/src/drivers/voxl2_io/voxl2_io_serial.cpp b/src/drivers/voxl2_io/voxl2_io_serial.cpp deleted file mode 100644 index cee0923ece..0000000000 --- a/src/drivers/voxl2_io/voxl2_io_serial.cpp +++ /dev/null @@ -1,191 +0,0 @@ -/**************************************************************************** - * - * 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 "string.h" -#include "voxl2_io_serial.hpp" - -Voxl2IoSerial::Voxl2IoSerial() -{ -} - -Voxl2IoSerial::~Voxl2IoSerial() -{ - if (_uart_fd >= 0) { - uart_close(); - } -} - -int Voxl2IoSerial::uart_open(const char *dev, speed_t speed) -{ - if (_uart_fd >= 0) { - PX4_ERR("Port in use: %s (%i)", dev, errno); - return -1; - } - - /* Open UART */ -#ifdef __PX4_QURT - _uart_fd = qurt_uart_open(dev, speed); -#else - _uart_fd = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK); -#endif - - if (_uart_fd < 0) { - PX4_ERR("Error opening port: %s (%i)", dev, errno); - return -1; - } - -#ifndef __PX4_QURT - /* Back up the original UART configuration to restore it after exit */ - int termios_state; - - if ((termios_state = tcgetattr(_uart_fd, &_orig_cfg)) < 0) { - PX4_ERR("Error configuring port: tcgetattr %s: %d", dev, termios_state); - uart_close(); - return -1; - } - - /* Fill the struct for the new configuration */ - tcgetattr(_uart_fd, &_cfg); - - /* Disable output post-processing */ - _cfg.c_oflag &= ~OPOST; - - _cfg.c_cflag |= (CLOCAL | CREAD); /* ignore modem controls */ - _cfg.c_cflag &= ~CSIZE; - _cfg.c_cflag |= CS8; /* 8-bit characters */ - _cfg.c_cflag &= ~PARENB; /* no parity bit */ - _cfg.c_cflag &= ~CSTOPB; /* only need 1 stop bit */ - _cfg.c_cflag &= ~CRTSCTS; /* no hardware flowcontrol */ - - /* setup for non-canonical mode */ - _cfg.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON); - _cfg.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN); - - if (cfsetispeed(&_cfg, speed) < 0 || cfsetospeed(&_cfg, speed) < 0) { - PX4_ERR("Error configuring port: %s: %d (cfsetispeed, cfsetospeed)", dev, termios_state); - uart_close(); - return -1; - } - - if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &_cfg)) < 0) { - PX4_ERR("Error configuring port: %s (tcsetattr)", dev); - uart_close(); - return -1; - } - -#endif - - _speed = speed; - - return 0; -} - -int Voxl2IoSerial::uart_set_baud(speed_t speed) -{ -#ifndef __PX4_QURT - - if (_uart_fd < 0) { - return -1; - } - - if (cfsetispeed(&_cfg, speed) < 0) { - return -1; - } - - if (tcsetattr(_uart_fd, TCSANOW, &_cfg) < 0) { - return -1; - } - - _speed = speed; - - return 0; -#endif - - return -1; -} - -int Voxl2IoSerial::uart_close() -{ -#ifndef __PX4_QURT - - if (_uart_fd < 0) { - PX4_ERR("invalid state for closing"); - return -1; - } - - if (tcsetattr(_uart_fd, TCSANOW, &_orig_cfg)) { - PX4_ERR("failed restoring uart to original state"); - } - - if (close(_uart_fd)) { - PX4_ERR("error closing uart"); - } - -#endif - - _uart_fd = -1; - - return 0; -} - -int Voxl2IoSerial::uart_write(FAR void *buf, size_t len) -{ - if (_uart_fd < 0 || buf == NULL) { - PX4_ERR("invalid state for writing or buffer"); - return -1; - } - -#ifdef __PX4_QURT - return qurt_uart_write(_uart_fd, (const char *) buf, len); -#else - return write(_uart_fd, buf, len); -#endif -} - -int Voxl2IoSerial::uart_read(FAR void *buf, size_t len) -{ - if (_uart_fd < 0 || buf == NULL) { - PX4_ERR("invalid state for reading or buffer"); - return -1; - } - -#ifdef __PX4_QURT -#define ASYNC_UART_READ_WAIT_US 2000 - // The UART read on SLPI is via an asynchronous service so specify a timeout - // for the return. The driver will poll periodically until the read comes in - // so this may block for a while. However, it will timeout if no read comes in. - return qurt_uart_read(_uart_fd, (char *) buf, len, ASYNC_UART_READ_WAIT_US); -#else - return read(_uart_fd, buf, len); -#endif -} diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 1075ad4cd6..a9396caa1f 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -34,9 +34,7 @@ add_subdirectory(adsb EXCLUDE_FROM_ALL) add_subdirectory(airspeed EXCLUDE_FROM_ALL) add_subdirectory(atmosphere EXCLUDE_FROM_ALL) -add_subdirectory(avoidance EXCLUDE_FROM_ALL) add_subdirectory(battery EXCLUDE_FROM_ALL) -add_subdirectory(bezier EXCLUDE_FROM_ALL) add_subdirectory(button EXCLUDE_FROM_ALL) add_subdirectory(cdev EXCLUDE_FROM_ALL) add_subdirectory(cdrstream EXCLUDE_FROM_ALL) diff --git a/src/lib/avoidance/ObstacleAvoidance.cpp b/src/lib/avoidance/ObstacleAvoidance.cpp deleted file mode 100644 index f818f068a0..0000000000 --- a/src/lib/avoidance/ObstacleAvoidance.cpp +++ /dev/null @@ -1,271 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 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 ObstacleAvoidance.cpp - */ - -#include "ObstacleAvoidance.hpp" -#include "bezier/BezierN.hpp" - -using namespace matrix; -using namespace time_literals; - -/** Timeout in us for trajectory data to get considered invalid */ -static constexpr uint64_t TRAJECTORY_STREAM_TIMEOUT_US = 500_ms; -/** If Flighttask fails, keep 0.5 seconds the current setpoint before going into failsafe land */ -static constexpr uint64_t TIME_BEFORE_FAILSAFE = 500_ms; -static constexpr uint64_t Z_PROGRESS_TIMEOUT_US = 2_s; - -ObstacleAvoidance::ObstacleAvoidance(ModuleParams *parent) : - ModuleParams(parent) -{ - _desired_waypoint = empty_trajectory_waypoint; - _failsafe_position.setNaN(); - _avoidance_point_not_valid_hysteresis.set_hysteresis_time_from(false, TIME_BEFORE_FAILSAFE); - _no_progress_z_hysteresis.set_hysteresis_time_from(false, Z_PROGRESS_TIMEOUT_US); - -} - -void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel_sp, float &yaw_sp, - float &yaw_speed_sp) -{ - _sub_vehicle_status.update(); - _sub_vehicle_trajectory_waypoint.update(); - _sub_vehicle_trajectory_bezier.update(); - - const auto &wp_msg = _sub_vehicle_trajectory_waypoint.get(); - const auto &bezier_msg = _sub_vehicle_trajectory_bezier.get(); - - const bool wp_msg_timeout = hrt_elapsed_time((hrt_abstime *)&wp_msg.timestamp) > TRAJECTORY_STREAM_TIMEOUT_US; - const bool bezier_msg_timeout = hrt_elapsed_time((hrt_abstime *)&bezier_msg.timestamp) > hrt_abstime( - bezier_msg.control_points[bezier_msg.bezier_order - 1].delta * 1e6f); - const bool avoidance_data_timeout = wp_msg_timeout && bezier_msg_timeout; - - const bool avoidance_point_valid = wp_msg.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid; - const bool avoidance_bezier_valid = bezier_msg.bezier_order > 0; - - _avoidance_point_not_valid_hysteresis.set_state_and_update(!avoidance_point_valid - && !avoidance_bezier_valid, hrt_absolute_time()); - - const bool avoidance_invalid = (avoidance_data_timeout || _avoidance_point_not_valid_hysteresis.get_state()); - - if ((_sub_vehicle_status.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) && avoidance_invalid) { - // if in nav_state LOITER and avoidance isn't healthy, don't inject setpoints from avoidance system - return; - } - - if (avoidance_invalid) { - if (_avoidance_activated) { - // Invalid point received: deactivate - PX4_WARN("Obstacle Avoidance system failed, loitering"); - _publishVehicleCmdDoLoiter(); - _avoidance_activated = false; - } - - if (!_failsafe_position.isAllFinite()) { - // save vehicle position when entering failsafe - _failsafe_position = _position; - } - - pos_sp = _failsafe_position; - vel_sp.setNaN(); - yaw_sp = NAN; - yaw_speed_sp = NAN; - - // Do nothing further - wait until activation - return; - - } else if (!_avoidance_activated) { - // First setpoint has been received: activate - PX4_INFO("Obstacle Avoidance system activated"); - _failsafe_position.setNaN(); - _avoidance_activated = true; - } - - if (avoidance_point_valid && !wp_msg_timeout) { - const auto &point0 = wp_msg.waypoints[vehicle_trajectory_waypoint_s::POINT_0]; - pos_sp = Vector3f(point0.position); - vel_sp = Vector3f(point0.velocity); - - if (!_ext_yaw_active) { - // inject yaw setpoints only if weathervane isn't active - yaw_sp = point0.yaw; - yaw_speed_sp = point0.yaw_speed; - } - - } else if (avoidance_bezier_valid && !bezier_msg_timeout) { - float yaw = NAN, yaw_speed = NAN; - _generateBezierSetpoints(pos_sp, vel_sp, yaw, yaw_speed); - - if (!_ext_yaw_active) { - // inject yaw setpoints only if weathervane isn't active - yaw_sp = yaw; - yaw_speed_sp = yaw_speed; - } - } -} - -void ObstacleAvoidance::_generateBezierSetpoints(matrix::Vector3f &position, matrix::Vector3f &velocity, - float &yaw, float &yaw_velocity) -{ - const auto &msg = _sub_vehicle_trajectory_bezier.get(); - int bezier_order = msg.bezier_order; - matrix::Vector3f bezier_points[bezier_order]; - float bezier_yaws[bezier_order]; - - for (int i = 0; i < bezier_order; i++) { - bezier_points[i] = Vector3f(msg.control_points[i].position); - bezier_yaws[i] = msg.control_points[i].yaw; - } - - const float duration_s = msg.control_points[bezier_order - 1].delta; - const hrt_abstime now = hrt_absolute_time(); - const hrt_abstime start = msg.timestamp; - const hrt_abstime end = start + hrt_abstime(duration_s * 1e6f); - - float T = NAN; - - if (bezier::calculateT(start, end, now, T) && - bezier::calculateBezierPosVel(bezier_points, bezier_order, T, position, velocity) && - bezier::calculateBezierYaw(bezier_yaws, bezier_order, T, yaw, yaw_velocity) - ) { - // translate bezier velocities T [0;1] into real velocities m/s - yaw_velocity /= duration_s; - velocity /= duration_s; - - } else { - PX4_WARN("Obstacle Avoidance system failed, bad trajectory"); - } -} - - -void ObstacleAvoidance::updateAvoidanceDesiredWaypoints(const Vector3f &curr_wp, const float curr_yaw, - const float curr_yawspeed, const Vector3f &next_wp, const float next_yaw, const float next_yawspeed, - const bool ext_yaw_active, const int wp_type) -{ - _desired_waypoint.timestamp = hrt_absolute_time(); - _desired_waypoint.type = vehicle_trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS; - _curr_wp = curr_wp; - _ext_yaw_active = ext_yaw_active; - - curr_wp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].position); - Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].velocity); - Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].acceleration); - - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw = curr_yaw; - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw_speed = curr_yawspeed; - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].point_valid = true; - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].type = wp_type; - - next_wp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].position); - Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].velocity); - Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].acceleration); - - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw = next_yaw; - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw_speed = next_yawspeed; - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].point_valid = true; -} - -void ObstacleAvoidance::updateAvoidanceDesiredSetpoints(const Vector3f &pos_sp, const Vector3f &vel_sp, const int type) -{ - pos_sp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position); - vel_sp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity); - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].type = type; - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid = true; - - _pub_traj_wp_avoidance_desired.publish(_desired_waypoint); - - _desired_waypoint = empty_trajectory_waypoint; -} - -void ObstacleAvoidance::checkAvoidanceProgress(const Vector3f &pos, const Vector3f &prev_wp, - float target_acceptance_radius, const Vector2f &closest_pt) -{ - _position = pos; - position_controller_status_s pos_control_status = {}; - pos_control_status.timestamp = hrt_absolute_time(); - - // vector from previous triplet to current target - Vector2f prev_to_target = Vector2f(_curr_wp - prev_wp); - // vector from previous triplet to the vehicle projected position on the line previous-target triplet - Vector2f prev_to_closest_pt = closest_pt - Vector2f(prev_wp); - // fraction of the previous-tagerget line that has been flown - const float prev_curr_travelled = prev_to_closest_pt.length() / prev_to_target.length(); - - Vector2f pos_to_target = Vector2f(_curr_wp - _position); - - if (prev_curr_travelled > 1.0f) { - // if the vehicle projected position on the line previous-target is past the target waypoint, - // increase the target acceptance radius such that navigator will update the triplets - pos_control_status.acceptance_radius = pos_to_target.length() + 0.5f; - } - - const float pos_to_target_z = fabsf(_curr_wp(2) - _position(2)); - - bool no_progress_z = (pos_to_target_z > _prev_pos_to_target_z); - _no_progress_z_hysteresis.set_state_and_update(no_progress_z, hrt_absolute_time()); - - if (pos_to_target.length() < target_acceptance_radius && pos_to_target_z > _param_nav_mc_alt_rad.get() - && _no_progress_z_hysteresis.get_state()) { - // vehicle above or below the target waypoint - pos_control_status.altitude_acceptance = pos_to_target_z + 0.5f; - } - - _prev_pos_to_target_z = pos_to_target_z; - - // do not check for waypoints yaw acceptance in navigator - pos_control_status.yaw_acceptance = NAN; - - _pub_pos_control_status.publish(pos_control_status); -} - -void ObstacleAvoidance::_publishVehicleCmdDoLoiter() -{ - vehicle_command_s command{}; - command.timestamp = hrt_absolute_time(); - command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE; - command.param1 = (float)1; // base mode - command.param3 = (float)0; // sub mode - command.target_system = 1; - command.target_component = 1; - command.source_system = 1; - command.source_component = 1; - command.confirmation = false; - command.from_external = false; - command.param2 = (float)PX4_CUSTOM_MAIN_MODE_AUTO; - command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - - // publish the vehicle command - _pub_vehicle_command.publish(command); -} diff --git a/src/lib/avoidance/ObstacleAvoidance.hpp b/src/lib/avoidance/ObstacleAvoidance.hpp deleted file mode 100644 index 8a27ffbbf1..0000000000 --- a/src/lib/avoidance/ObstacleAvoidance.hpp +++ /dev/null @@ -1,152 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 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 ObstacleAvoidance.hpp - * This class is used to inject the setpoints of an obstacle avoidance system - * into the FlightTasks - * - * @author Martina Rivizzigno - */ - -#pragma once - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include - -const vehicle_trajectory_waypoint_s empty_trajectory_waypoint = {0, 0, {0, 0, 0, 0, 0, 0, 0}, - { {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}}, - {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}}, - {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}}, - {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}}, - {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}} - } -}; - -class ObstacleAvoidance : public ModuleParams -{ -public: - ObstacleAvoidance(ModuleParams *parent); - ~ObstacleAvoidance() = default; - - /** - * Inject setpoints from obstacle avoidance system into FlightTasks. - * @param pos_sp, position setpoint - * @param vel_sp, velocity setpoint - * @param yaw_sp, yaw setpoint - * @param yaw_speed_sp, yaw speed setpoint - */ - void injectAvoidanceSetpoints(matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, float &yaw_sp, float &yaw_speed_sp); - - /** - * Updates the desired waypoints to send to the obstacle avoidance system. These messages don't have any direct impact on the flight. - * @param curr_wp, current position triplet - * @param curr_yaw, current yaw triplet - * @param curr_yawspeed, current yaw speed triplet - * @param next_wp, next position triplet - * @param next_yaw, next yaw triplet - * @param next_yawspeed, next yaw speed triplet - * @param wp_type, current triplet type - */ - void updateAvoidanceDesiredWaypoints(const matrix::Vector3f &curr_wp, const float curr_yaw, const float curr_yawspeed, - const matrix::Vector3f &next_wp, const float next_yaw, const float next_yawspeed, const bool ext_yaw_active, - const int wp_type); - /** - * Updates the desired setpoints to send to the obstacle avoidance system. - * @param pos_sp, desired position setpoint computed by the active FlightTask - * @param vel_sp, desired velocity setpoint computed by the active FlightTask - * @param type, current triplet type - */ - void updateAvoidanceDesiredSetpoints(const matrix::Vector3f &pos_sp, const matrix::Vector3f &vel_sp, const int type); - - /** - * Checks the vehicle progress between previous and current position waypoint of the triplet. - * @param pos, vehicle position - * @param prev_wp, previous position triplet - * @param target_acceptance_radius, current position triplet xy acceptance radius - * @param closest_pt, closest point to the vehicle on the line previous-current position triplet - */ - void checkAvoidanceProgress(const matrix::Vector3f &pos, const matrix::Vector3f &prev_wp, - float target_acceptance_radius, const matrix::Vector2f &closest_pt); - -protected: - - uORB::SubscriptionData _sub_vehicle_trajectory_bezier{ORB_ID(vehicle_trajectory_bezier)}; /**< vehicle trajectory waypoint subscription */ - uORB::SubscriptionData _sub_vehicle_trajectory_waypoint{ORB_ID(vehicle_trajectory_waypoint)}; /**< vehicle trajectory waypoint subscription */ - uORB::SubscriptionData _sub_vehicle_status{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ - - vehicle_trajectory_waypoint_s _desired_waypoint{}; /**< desired vehicle trajectory waypoint to be sent to OA */ - - uORB::Publication _pub_traj_wp_avoidance_desired{ORB_ID(vehicle_trajectory_waypoint_desired)}; /**< trajectory waypoint desired publication */ - uORB::Publication _pub_pos_control_status{ORB_ID(position_controller_status)}; /**< position controller status publication */ - uORB::Publication _pub_vehicle_command{ORB_ID(vehicle_command)}; /**< vehicle command do publication */ - - matrix::Vector3f _curr_wp = {}; /**< current position triplet */ - matrix::Vector3f _position = {}; /**< current vehicle position */ - matrix::Vector3f _failsafe_position = {}; /**< vehicle position when entered in failsafe */ - - bool _avoidance_activated{false}; /**< true after the first avoidance setpoint is received */ - - systemlib::Hysteresis _avoidance_point_not_valid_hysteresis{false}; /**< becomes true if the companion doesn't start sending valid setpoints */ - systemlib::Hysteresis _no_progress_z_hysteresis{false}; /**< becomes true if the vehicle is not making progress towards the z component of the goal */ - - float _prev_pos_to_target_z = -1.f; /**< z distance to the goal */ - - bool _ext_yaw_active = false; /**< true, if external yaw handling is active */ - - /** - * Publishes vehicle command. - */ - void _publishVehicleCmdDoLoiter(); - void _generateBezierSetpoints(matrix::Vector3f &position, matrix::Vector3f &velocity, float &yaw, float &yaw_velocity); - - DEFINE_PARAMETERS( - (ParamFloat) _param_nav_mc_alt_rad /**< Acceptance radius for multicopter altitude */ - ); - -}; diff --git a/src/lib/avoidance/ObstacleAvoidanceTest.cpp b/src/lib/avoidance/ObstacleAvoidanceTest.cpp deleted file mode 100644 index 008a8758a9..0000000000 --- a/src/lib/avoidance/ObstacleAvoidanceTest.cpp +++ /dev/null @@ -1,242 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2019 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 - - -using namespace matrix; -// to run: make tests TESTFILTER=ObstacleAvoidance - -class ObstacleAvoidanceTest : public ::testing::Test -{ -public: - Vector3f pos_sp; - Vector3f vel_sp; - float yaw_sp = 0.123f; - float yaw_speed_sp = NAN; - void SetUp() override - - { - param_control_autosave(false); - param_reset_all(); - pos_sp = Vector3f(1.f, 1.2f, 0.1f); - vel_sp = Vector3f(0.3f, 0.4f, 0.1f); - } -}; - -class TestObstacleAvoidance : public ::ObstacleAvoidance -{ -public: - TestObstacleAvoidance() : ObstacleAvoidance(nullptr) {} - void paramsChanged() {ObstacleAvoidance::updateParamsImpl();} - void test_setPosition(Vector3f &pos) {_position = pos;} -}; - -TEST_F(ObstacleAvoidanceTest, instantiation) { ObstacleAvoidance oa(nullptr); } - -TEST_F(ObstacleAvoidanceTest, oa_enabled_healthy) -{ - // GIVEN: the flight controller setpoints from FlightTaskAuto and a vehicle_trajectory_waypoint message coming - // from offboard - TestObstacleAvoidance oa; - - vehicle_trajectory_waypoint_s message = empty_trajectory_waypoint; - message.timestamp = hrt_absolute_time(); - message.type = vehicle_trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS; - message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[0] = 2.6f; - message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[1] = 2.4f; - message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[2] = 2.7f; - message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw = 0.23f; - message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid = true; - - // GIVEN: and we publish the vehicle_trajectory_waypoint message and vehicle status message - uORB::Publication vehicle_trajectory_waypoint_pub{ORB_ID(vehicle_trajectory_waypoint)}; - vehicle_trajectory_waypoint_pub.publish(message); - - vehicle_status_s vehicle_status{}; - vehicle_status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; - uORB::Publication vehicle_status_pub{ORB_ID(vehicle_status)}; - vehicle_status_pub.publish(vehicle_status); - - // WHEN: we inject the vehicle_trajectory_waypoint in the interface - oa.injectAvoidanceSetpoints(pos_sp, vel_sp, yaw_sp, yaw_speed_sp); - - // THEN: the setpoints should be injected - EXPECT_FLOAT_EQ(message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[0], pos_sp(0)); - EXPECT_FLOAT_EQ(message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[1], pos_sp(1)); - EXPECT_FLOAT_EQ(message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[2], pos_sp(2)); - EXPECT_TRUE(vel_sp.isAllNan()); - EXPECT_FLOAT_EQ(message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw, yaw_sp); - EXPECT_FALSE(PX4_ISFINITE(yaw_speed_sp)); -} - -TEST_F(ObstacleAvoidanceTest, oa_enabled_healthy_bezier) -{ - // GIVEN: the flight controller setpoints from FlightTaskAuto and a vehicle_trajectory_waypoint message coming - // from offboard - TestObstacleAvoidance oa; - - vehicle_trajectory_bezier_s message {}; - message.timestamp = hrt_absolute_time(); - message.bezier_order = 2; - message.control_points[vehicle_trajectory_bezier_s::POINT_0].position[0] = 2.6f; - message.control_points[vehicle_trajectory_bezier_s::POINT_0].position[1] = 2.4f; - message.control_points[vehicle_trajectory_bezier_s::POINT_0].position[2] = 2.7f; - message.control_points[vehicle_trajectory_bezier_s::POINT_0].yaw = 0.23f; - message.control_points[vehicle_trajectory_bezier_s::POINT_0].delta = NAN; - message.control_points[vehicle_trajectory_bezier_s::POINT_1].position[0] = 2.6f; - message.control_points[vehicle_trajectory_bezier_s::POINT_1].position[1] = 2.4f; - message.control_points[vehicle_trajectory_bezier_s::POINT_1].position[2] = 3.7f; - message.control_points[vehicle_trajectory_bezier_s::POINT_1].yaw = 0.23f; - message.control_points[vehicle_trajectory_bezier_s::POINT_1].delta = 0.5f; - - // GIVEN: and we publish the vehicle_trajectory_waypoint message and vehicle status message - uORB::Publication vehicle_trajectory_bezier_pub{ORB_ID(vehicle_trajectory_bezier)}; - vehicle_trajectory_bezier_pub.publish(message); - - vehicle_status_s vehicle_status{}; - vehicle_status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; - uORB::Publication vehicle_status_pub{ORB_ID(vehicle_status)}; - vehicle_status_pub.publish(vehicle_status); - - // WHEN: we inject the vehicle_trajectory_waypoint in the interface - oa.injectAvoidanceSetpoints(pos_sp, vel_sp, yaw_sp, yaw_speed_sp); - - // THEN: the setpoints should be injected - EXPECT_FLOAT_EQ(2.6f, pos_sp(0)); - EXPECT_FLOAT_EQ(2.4f, pos_sp(1)); - EXPECT_LT(2.7f, pos_sp(2)); - EXPECT_GT(2.8f, pos_sp(2)); // probably only a tiny bit above 2.7, but let's not have flakey tests - EXPECT_FLOAT_EQ(vel_sp.xy().norm(), 0); - EXPECT_FLOAT_EQ(vel_sp(2), (3.7f - 2.7f) / 0.5f); - EXPECT_FLOAT_EQ(0.23, yaw_sp); - EXPECT_FLOAT_EQ(yaw_speed_sp, 0); -} - -TEST_F(ObstacleAvoidanceTest, oa_enabled_not_healthy) -{ - // GIVEN: the flight controller setpoints from FlightTaskAuto and a vehicle_trajectory_waypoint message - TestObstacleAvoidance oa; - - vehicle_trajectory_waypoint_s message = empty_trajectory_waypoint; - Vector3f pos(3.1f, 4.7f, 5.2f); - oa.test_setPosition(pos); - - // GIVEN: and we publish the vehicle_trajectory_waypoint message and vehicle_status - uORB::Publication vehicle_trajectory_waypoint_pub{ORB_ID(vehicle_trajectory_waypoint)}; - vehicle_trajectory_waypoint_pub.publish(message); - - vehicle_status_s vehicle_status{}; - vehicle_status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; - uORB::Publication vehicle_status_pub{ORB_ID(vehicle_status)}; - vehicle_status_pub.publish(vehicle_status); - - // WHEN: we inject the vehicle_trajectory_waypoint in the interface - oa.injectAvoidanceSetpoints(pos_sp, vel_sp, yaw_sp, yaw_speed_sp); - - // THEN: the setpoints shouldn't be injected - EXPECT_FLOAT_EQ(pos(0), pos_sp(0)); - EXPECT_FLOAT_EQ(pos(1), pos_sp(1)); - EXPECT_FLOAT_EQ(pos(2), pos_sp(2)); - EXPECT_TRUE(vel_sp.isAllNan()); - EXPECT_FALSE(PX4_ISFINITE(yaw_sp)); - EXPECT_FALSE(PX4_ISFINITE(yaw_speed_sp)); -} - -TEST_F(ObstacleAvoidanceTest, oa_desired) -{ - // GIVEN: the flight controller setpoints from FlightTaskAuto and the waypoints from FLightTaskAuto - TestObstacleAvoidance oa; - - pos_sp = Vector3f(1.f, 1.2f, NAN); - vel_sp = Vector3f(NAN, NAN, 0.7f); - int type = 4; - Vector3f curr_wp(1.f, 1.2f, 5.0f); - float curr_yaw = 1.02f; - float curr_yawspeed = NAN; - Vector3f next_wp(11.f, 1.2f, 5.0f); - float next_yaw = 0.82f; - float next_yawspeed = NAN; - bool ext_yaw_active = false; - - // WHEN: we inject the setpoints and waypoints in the interface - oa.updateAvoidanceDesiredWaypoints(curr_wp, curr_yaw, curr_yawspeed, next_wp, next_yaw, next_yawspeed, ext_yaw_active, - type); - oa.updateAvoidanceDesiredSetpoints(pos_sp, vel_sp, type); - - // WHEN: we subscribe to the uORB message out of the interface - uORB::SubscriptionData _sub_traj_wp_avoidance_desired{ORB_ID(vehicle_trajectory_waypoint_desired)}; - - // THEN: we expect the setpoints in POINT_0 and waypoints in POINT_1 and POINT_2 - EXPECT_FLOAT_EQ(pos_sp(0), - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[0]); - EXPECT_FLOAT_EQ(pos_sp(1), - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[1]); - EXPECT_FALSE(PX4_ISFINITE( - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[2])); - EXPECT_FALSE(PX4_ISFINITE( - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[0])); - EXPECT_FALSE(PX4_ISFINITE( - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[1])); - EXPECT_FLOAT_EQ(vel_sp(2), - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[2]); - EXPECT_EQ(type, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].type); - EXPECT_TRUE(_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid); - - EXPECT_FLOAT_EQ(curr_wp(0), - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].position[0]); - EXPECT_FLOAT_EQ(curr_wp(1), - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].position[1]); - EXPECT_FLOAT_EQ(curr_wp(2), - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].position[2]); - EXPECT_FLOAT_EQ(curr_yaw, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw); - EXPECT_FALSE(PX4_ISFINITE( - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw_speed)); - EXPECT_EQ(type, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].type); - EXPECT_TRUE(_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].point_valid); - - EXPECT_FLOAT_EQ(next_wp(0), - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].position[0]); - EXPECT_FLOAT_EQ(next_wp(1), - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].position[1]); - EXPECT_FLOAT_EQ(next_wp(2), - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].position[2]); - EXPECT_FLOAT_EQ(next_yaw, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw); - EXPECT_FALSE(PX4_ISFINITE( - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw_speed)); - EXPECT_EQ(UINT8_MAX, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].type); - EXPECT_TRUE(_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].point_valid); - -} diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp index 21421d592d..10dbb93a68 100644 --- a/src/lib/battery/battery.cpp +++ b/src/lib/battery/battery.cpp @@ -307,16 +307,16 @@ void Battery::estimateStateOfCharge() uint8_t Battery::determineWarning(float state_of_charge) { if (state_of_charge < _params.emergen_thr) { - return battery_status_s::BATTERY_WARNING_EMERGENCY; + return battery_status_s::WARNING_EMERGENCY; } else if (state_of_charge < _params.crit_thr) { - return battery_status_s::BATTERY_WARNING_CRITICAL; + return battery_status_s::WARNING_CRITICAL; } else if (state_of_charge < _params.low_thr) { - return battery_status_s::BATTERY_WARNING_LOW; + return battery_status_s::WARNING_LOW; } else { - return battery_status_s::BATTERY_WARNING_NONE; + return battery_status_s::WARNING_NONE; } } @@ -327,7 +327,7 @@ uint16_t Battery::determineFaults() if ((_params.n_cells > 0) && (_voltage_v > (_params.n_cells * _params.v_charged * 1.05f))) { // Reported as a "spike" since "over-voltage" does not exist in MAV_BATTERY_FAULT - faults |= (1 << battery_status_s::BATTERY_FAULT_SPIKES); + faults |= (1 << battery_status_s::FAULT_SPIKES); } return faults; diff --git a/src/lib/battery/battery.h b/src/lib/battery/battery.h index 64649c3c08..baf6e5ecd6 100644 --- a/src/lib/battery/battery.h +++ b/src/lib/battery/battery.h @@ -175,7 +175,7 @@ private: float _state_of_charge_volt_based{-1.f}; // [0,1] float _state_of_charge{-1.f}; // [0,1] float _scale{1.f}; - uint8_t _warning{battery_status_s::BATTERY_WARNING_NONE}; + uint8_t _warning{battery_status_s::WARNING_NONE}; hrt_abstime _last_timestamp{0}; bool _armed{false}; bool _vehicle_status_is_fw{false}; diff --git a/src/lib/bezier/BezierN.cpp b/src/lib/bezier/BezierN.cpp deleted file mode 100644 index 0d30aebd88..0000000000 --- a/src/lib/bezier/BezierN.cpp +++ /dev/null @@ -1,163 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ -/** - * @file BezierN.cpp - * Bezier function - * - * @author Julian Kent - */ - -#include -#include - -namespace -{ - -/* - * Generic in-place bezier implementation. Leaves result in first element. - * - */ -template -void calculateBezier(matrix::Vector *positions, int N, Scalar t, Scalar one_minus_t) -{ - for (int bezier_order = 1; bezier_order < N; bezier_order++) { - for (int i = 0; i < N - bezier_order; i++) { - positions[i] = positions[i] * one_minus_t + positions[i + 1] * t; - } - } -} -} - -namespace bezier -{ - -bool calculateBezierPosVel(const matrix::Vector3f *positions, int N, float t, - matrix::Vector3f &position, matrix::Vector3f &velocity) -{ - if (positions == nullptr || N == 0 || t < 0 || t > 1) { - return false; - } - - using Df = matrix::Dual; - using Vector3Df = matrix::Vector3; - - Vector3Df intermediates[N]; - - for (int i = 0; i < N; i++) { - for (int j = 0; j < 3; j++) { - intermediates[i](j) = positions[i](j); - } - } - - Df dual_t(t, 0); // derivative with respect to time - calculateBezier(intermediates, N, dual_t, Df(1) - dual_t); - - position = matrix::collectReals(intermediates[0]); - velocity = matrix::collectDerivatives(intermediates[0]); - - return true; -} - -bool calculateBezierPosVelAcc(const matrix::Vector3f *positions, int N, float t, - matrix::Vector3f &position, matrix::Vector3f &velocity, matrix::Vector3f &acceleration) -{ - if (positions == nullptr || N == 0 || t < 0 || t > 1) { - return false; - } - - using Df = matrix::Dual; - using DDf = matrix::Dual; - using Vector3DDf = matrix::Vector3; - - Vector3DDf intermediates[N]; - - for (int i = 0; i < N; i++) { - for (int j = 0; j < 3; j++) { - intermediates[i](j) = Df(positions[i](j)); - } - } - - DDf dual_t(Df(t, 0), 0); // 1st and 2nd derivative with respect to time - calculateBezier(intermediates, N, dual_t, Df(1) - dual_t); - - position = matrix::collectReals(matrix::collectReals(intermediates[0])); - velocity = matrix::collectReals(matrix::collectDerivatives(intermediates[0])); - acceleration = matrix::collectDerivatives(matrix::collectDerivatives(intermediates[0])); - - return true; -} - -bool calculateBezierYaw(const float *setpoints, int N, float t, float &yaw_setpoint, float &yaw_vel_setpoint) -{ - if (setpoints == nullptr || N == 0 || t < 0 || t > 1) { - return false; - } - - - using Df = matrix::Dual; - using Vector1Df = matrix::Vector; - - Vector1Df intermediates[N]; - - // all yaw setpoints are wrapped relative to the starting yaw - const float offset = setpoints[0]; - - for (int i = 0; i < N; i++) { - intermediates[i](0) = matrix::wrap_pi(setpoints[i] - offset); - } - - Df dual_t (t, 0); // derivative with respect to time - calculateBezier(intermediates, N, dual_t, Df(1) - dual_t); - - Df result = intermediates[0](0); - yaw_setpoint = matrix::wrap_pi(result.value + offset); - yaw_vel_setpoint = result.derivative(0); - - return true; -} - -bool calculateT(int64_t start_time, int64_t end_time, int64_t now, float &T) -{ - if (now < start_time || end_time < now) { - return false; - } - - int64_t total_duration = end_time - start_time; - int64_t elapsed_duration = now - start_time; - - T = (float) elapsed_duration / (float) total_duration; - - return true; -} - -} diff --git a/src/lib/bezier/BezierNTest.cpp b/src/lib/bezier/BezierNTest.cpp deleted file mode 100644 index b08a40fd62..0000000000 --- a/src/lib/bezier/BezierNTest.cpp +++ /dev/null @@ -1,325 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2019 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. - * - ****************************************************************************/ - -/** - * Test code for the Velocity Smoothing library - * Run this test only using make tests TESTFILTER=BezierN - * - * @author Julian Kent - */ - -#include -#include - -#include "BezierN.hpp" - -TEST(BezierN_calculateBezier, checks_validity) -{ - matrix::Vector3f points[10]; - matrix::Vector3f a, b; - EXPECT_FALSE(bezier::calculateBezierPosVel(nullptr, 10, 0.5f, a, b)); - EXPECT_FALSE(bezier::calculateBezierPosVel(points, 0, 0.5f, a, b)); - EXPECT_FALSE(bezier::calculateBezierPosVel(points, 10, -0.5f, a, b)); - EXPECT_FALSE(bezier::calculateBezierPosVel(points, 10, 1.5f, a, b)); -} - -TEST(BezierN_calculateBezier, checks_validity_accel) -{ - matrix::Vector3f points[10]; - matrix::Vector3f a, b, c; - EXPECT_FALSE(bezier::calculateBezierPosVelAcc(nullptr, 10, 0.5f, a, b, c)); - EXPECT_FALSE(bezier::calculateBezierPosVelAcc(points, 0, 0.5f, a, b, c)); - EXPECT_FALSE(bezier::calculateBezierPosVelAcc(points, 10, -0.5f, a, b, c)); - EXPECT_FALSE(bezier::calculateBezierPosVelAcc(points, 10, 1.5f, a, b, c)); -} - -TEST(BezierN_calculateBezier, work_1_point) -{ - // GIVEN: a single point bezier curve - matrix::Vector3f points[2] = {matrix::Vector3f(1, 2, 3), matrix::Vector3f(NAN, NAN, NAN)}; - matrix::Vector3f pos, vel; - pos *= NAN; - vel *= NAN; - - // WHEN: we get the half-way point - EXPECT_TRUE(bezier::calculateBezierPosVel(points, 1, 0.5f, pos, vel)); - - // THEN: it should be the same as the point, and the velocity should be 0 - EXPECT_EQ((pos - points[0]).norm(), 0.f); - EXPECT_EQ(vel.norm(), 0.f); -} - -TEST(BezierN_calculateBezier, works_2_points) -{ - // GIVEN: a 2-point bezier curve - matrix::Vector3f points[3] = {matrix::Vector3f(1, 2, 3), matrix::Vector3f(5, 0, 1), matrix::Vector3f(NAN, NAN, NAN)}; - matrix::Vector3f pos, vel; - pos *= NAN; - vel *= NAN; - - // WHEN: we get the half-way point - EXPECT_TRUE(bezier::calculateBezierPosVel(points, 2, 0.5f, pos, vel)); - - // THEN: the position should be the mid-point between the start and end, and velocity should be the length - EXPECT_EQ((pos - matrix::Vector3f(3, 1, 2)).norm(), 0.f); - EXPECT_FLOAT_EQ((vel - (points[1] - points[0])).norm(), 0.f); - - // WHEN: we get the beginning point - EXPECT_TRUE(bezier::calculateBezierPosVel(points, 2, 0.f, pos, vel)); - - // THEN: the position should be the first point, and the velocity should still be the length - EXPECT_EQ((pos - points[0]).norm(), 0.f); - EXPECT_FLOAT_EQ((vel - (points[1] - points[0])).norm(), 0.f); - - // WHEN: we get the end point - EXPECT_TRUE(bezier::calculateBezierPosVel(points, 2, 1.f, pos, vel)); - - // THEN: the position should be the first point, and the velocity should still be the length - EXPECT_EQ((pos - points[1]).norm(), 0.f); - EXPECT_FLOAT_EQ((vel - (points[1] - points[0])).norm(), 0.f); -} - -TEST(BezierN_calculateBezier, works_3_points_zero_accel) -{ - // GIVEN: 3 points bezier, evenly spaced in a straight line - matrix::Vector3f points[4] = {matrix::Vector3f(1, 2, 3), matrix::Vector3f(5, 0, 1), matrix::Vector3f(9, -2, -1), matrix::Vector3f(NAN, NAN, NAN)}; - matrix::Vector3f pos, vel; - pos *= NAN; - vel *= NAN; - - // WHEN: we get the half-way point - EXPECT_TRUE(bezier::calculateBezierPosVel(points, 3, 0.5f, pos, vel)); - - // THEN: it should be the middle point, with velocity of 1st to last - EXPECT_FLOAT_EQ((pos - points[1]).norm(), 0.f); - EXPECT_FLOAT_EQ((vel - (points[2] - points[0])).norm(), 0.f); - - matrix::Vector3f pos2, vel2, accel2; - - // WHEN: we use the accel interface - EXPECT_TRUE(bezier::calculateBezierPosVelAcc(points, 3, 0.5f, pos2, vel2, accel2)); - - // THEN: it should give same position, velocity as the non-accel interface, and zero accel (since this curve is 0 accel) - EXPECT_FLOAT_EQ((pos2 - pos).norm(), 0.f); - EXPECT_FLOAT_EQ((vel2 - vel).norm(), 0.f); - EXPECT_FLOAT_EQ(accel2.norm(), 0.f); - - // WHEN: we check at the beginning - EXPECT_TRUE(bezier::calculateBezierPosVel(points, 3, 0.f, pos, vel)); - EXPECT_TRUE(bezier::calculateBezierPosVelAcc(points, 3, 0.f, pos2, vel2, accel2)); - - // THEN: it should be the starting point and same velocity - EXPECT_FLOAT_EQ((pos - points[0]).norm(), 0.f); - EXPECT_FLOAT_EQ((vel - (points[2] - points[0])).norm(), 0.f); - EXPECT_FLOAT_EQ((pos2 - pos).norm(), 0.f); - EXPECT_FLOAT_EQ((vel2 - vel).norm(), 0.f); - EXPECT_FLOAT_EQ(accel2.norm(), 0.f); - - // WHEN: we check at the end - EXPECT_TRUE(bezier::calculateBezierPosVel(points, 3, 1.f, pos, vel)); - EXPECT_TRUE(bezier::calculateBezierPosVelAcc(points, 3, 1.f, pos2, vel2, accel2)); - - // THEN: it should be the ending point and same velocity - EXPECT_FLOAT_EQ((pos - points[2]).norm(), 0.f); - EXPECT_FLOAT_EQ((vel - (points[2] - points[0])).norm(), 0.f); - EXPECT_FLOAT_EQ((pos2 - pos).norm(), 0.f); - EXPECT_FLOAT_EQ((vel2 - vel).norm(), 0.f); - EXPECT_FLOAT_EQ(accel2.norm(), 0.f); -} - -TEST(BezierN_calculateBezier, works_3_points_accel) -{ - // GIVEN: 3 points bezier, in a curve - matrix::Vector3f points[4] = {matrix::Vector3f(1, 2, 3), matrix::Vector3f(5, 0, 1), matrix::Vector3f(19, -8, 1), matrix::Vector3f(NAN, NAN, NAN)}; - matrix::Vector3f pos, vel; - pos *= NAN; - vel *= NAN; - - matrix::Vector3f pos2; - pos2 *= NAN; - - matrix::Vector3f accel_start, accel_mid, accel_end; - matrix::Vector3f vel_start, vel_mid, vel_end; - - - // WHEN: we check at the beginning - EXPECT_TRUE(bezier::calculateBezierPosVel(points, 3, 0.f, pos, vel)); - EXPECT_TRUE(bezier::calculateBezierPosVelAcc(points, 3, 0.f, pos2, vel_start, accel_start)); - - // THEN: it should give same position, velocity as the non-accel interface, and non-zero accel - EXPECT_FLOAT_EQ((pos2 - pos).norm(), 0.f); - EXPECT_FLOAT_EQ((vel_start - vel).norm(), 0.f); - EXPECT_GT(accel_start.norm(), 0.f); - - // WHEN: we use the accel interface to get the half-way point - EXPECT_TRUE(bezier::calculateBezierPosVel(points, 3, 0.5f, pos, vel)); - EXPECT_TRUE(bezier::calculateBezierPosVelAcc(points, 3, 0.5f, pos2, vel_mid, accel_mid)); - - // THEN: the values should matche between accel and non-accel version - EXPECT_FLOAT_EQ((pos2 - pos).norm(), 0.f); - EXPECT_FLOAT_EQ((vel_mid - vel).norm(), 0.f); - - // AND: the accel should be the same as the start - EXPECT_FLOAT_EQ((accel_mid - accel_start).norm(), 0.f); - - - // WHEN: we check at the end - EXPECT_TRUE(bezier::calculateBezierPosVel(points, 3, 1.f, pos, vel)); - EXPECT_TRUE(bezier::calculateBezierPosVelAcc(points, 3, 1.f, pos2, vel_end, accel_end)); - - // THEN: it should be the ending point, and accel should match - EXPECT_FLOAT_EQ((pos - points[2]).norm(), 0.f); - EXPECT_FLOAT_EQ((pos2 - pos).norm(), 0.f); - EXPECT_FLOAT_EQ((vel_end - vel).norm(), 0.f); - EXPECT_FLOAT_EQ((accel_end - accel_start).norm(), 0.f); - - // FINALLY: mid point velocity should be average of start and end velocity - EXPECT_FLOAT_EQ((vel_mid - 0.5f * (vel_start + vel_end)).norm(), 0.f); -} - -TEST(BezierN_calculateBezierYaw, checks_validity) -{ - float points[10]; - float a, b; - EXPECT_FALSE(bezier::calculateBezierYaw(nullptr, 10, 0.5f, a, b)); - EXPECT_FALSE(bezier::calculateBezierYaw(points, 0, 0.5f, a, b)); - EXPECT_FALSE(bezier::calculateBezierYaw(points, 10, -0.5f, a, b)); - EXPECT_FALSE(bezier::calculateBezierYaw(points, 10, 1.5f, a, b)); -} - -TEST(BezierN_calculateBezierYaw, work_1_point) -{ - // GIVEN: a single yaw point - float points[2] = {M_PI / 2, NAN}; - float yaw, yaw_speed; - - // WHEN: we use it as a 1-point bezier curve - EXPECT_TRUE(bezier::calculateBezierYaw(points, 1, 0.5f, yaw, yaw_speed)); - - // THEN: it should have that same value, and the velocity should be 0 - EXPECT_FLOAT_EQ(yaw, M_PI / 2); - EXPECT_FLOAT_EQ(yaw_speed, 0); -} - -TEST(BezierN_calculateBezierYaw, work_2_points) -{ - // GIVEN: a single yaw point - float points[3] = {0, M_PI / 2, NAN}; - float yaw, yaw_speed; - - // WHEN: we get the beginning - EXPECT_TRUE(bezier::calculateBezierYaw(points, 2, 0.f, yaw, yaw_speed)); - - // THEN: it should have the beginning value, and the velocity should be the difference between first and last - EXPECT_FLOAT_EQ(yaw, 0); - EXPECT_FLOAT_EQ(yaw_speed, M_PI / 2); - - // WHEN: we get the middle - EXPECT_TRUE(bezier::calculateBezierYaw(points, 2, 0.5f, yaw, yaw_speed)); - - // THEN: it should have the beginning value, and the velocity should be the difference between first and last - EXPECT_FLOAT_EQ(yaw, M_PI / 4); - EXPECT_FLOAT_EQ(yaw_speed, M_PI / 2); - - // WHEN: we get the end - EXPECT_TRUE(bezier::calculateBezierYaw(points, 2, 1.f, yaw, yaw_speed)); - - // THEN: it should have the beginning value, and the velocity should be the difference between first and last - EXPECT_FLOAT_EQ(yaw, M_PI / 2); - EXPECT_FLOAT_EQ(yaw_speed, M_PI / 2); -} - -TEST(BezierN_calculateBezierYaw, work_2_points_wrap) -{ - // GIVEN: 2 yaw points on either side of the +- PI wrap line - float points[3] = {-M_PI + 0.1, M_PI - 0.1, NAN}; - float yaw, yaw_speed; - - // WHEN: we get the beginning - EXPECT_TRUE(bezier::calculateBezierYaw(points, 2, 0.f, yaw, yaw_speed)); - - // THEN: it should have the beginning value, and the velocity should be the wrapped distance between first and last - EXPECT_FLOAT_EQ(yaw, -M_PI + 0.1); - EXPECT_NEAR(yaw_speed, -0.2, 1e-6f); - - // WHEN: we get the middle - EXPECT_TRUE(bezier::calculateBezierYaw(points, 2, 0.5f, yaw, yaw_speed)); - - // THEN: it should have the wrapped middle value, and the velocity should be the wrapped distance between first and last - EXPECT_FLOAT_EQ(matrix::wrap_pi(yaw - float(M_PI)), 0); - EXPECT_NEAR(yaw_speed, -0.2, 1e-6f); - - // WHEN: we get the end - EXPECT_TRUE(bezier::calculateBezierYaw(points, 2, 1.f, yaw, yaw_speed)); - - // THEN: it should have the end value, and the velocity should be the wrapped distance between first and last - EXPECT_FLOAT_EQ(yaw, M_PI - 0.1); - EXPECT_NEAR(yaw_speed, -0.2, 1e-6f); -} - - -TEST(BezierN_calculateT, rejects_bad_timestamps) -{ - float f = NAN; - EXPECT_FALSE(bezier::calculateT(100, 1000, 99, f)); - EXPECT_FALSE(bezier::calculateT(100, 1000, 1001, f)); - EXPECT_FALSE(bezier::calculateT(1001, 1000, 1001, f)); -} - - -TEST(BezierN_calculateT, begin_middle_end) -{ - float f = NAN; - EXPECT_TRUE(bezier::calculateT(100, 1000, 100, f)); - EXPECT_FLOAT_EQ(f, 0.f); - - EXPECT_TRUE(bezier::calculateT(100, 1000, 550, f)); - EXPECT_FLOAT_EQ(f, 0.5f); - - EXPECT_TRUE(bezier::calculateT(100, 1000, 1000, f)); - EXPECT_FLOAT_EQ(f, 1.f); -} - -TEST(BezierN_calculateT, giant_offset) -{ - int64_t offset = 0xFFFFFFFFFFFF; // 48 bit max - float f = NAN; - EXPECT_TRUE(bezier::calculateT(offset + 100, offset + 1000, offset + 100, f)); - EXPECT_FLOAT_EQ(f, 0.f); - - EXPECT_TRUE(bezier::calculateT(offset + 100, offset + 1000, offset + 550, f)); - EXPECT_FLOAT_EQ(f, 0.5f); - - EXPECT_TRUE(bezier::calculateT(offset + 100, offset + 1000, offset + 1000, f)); - EXPECT_FLOAT_EQ(f, 1.f); -} diff --git a/src/lib/bezier/BezierQuad.cpp b/src/lib/bezier/BezierQuad.cpp deleted file mode 100644 index 61309cc496..0000000000 --- a/src/lib/bezier/BezierQuad.cpp +++ /dev/null @@ -1,223 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018 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 BezierQuad.cpp - * Bezier function - * - * @author Dennis Mannhart - */ - -// The header includes this implementation, so in order to have this library shared with other -// .cpp files and avoid duplication of constants, we need to prevent including it twice -#ifndef BEZIER_QUAD_CPP -#define BEZIER_QUAD_CPP - - -#include "BezierQuad.hpp" - -namespace bezier -{ -static constexpr double GOLDEN_RATIO = 1.6180339887; //(sqrt(5)+1)/2 -static constexpr double RESOLUTION = 0.0001; //End criterion for golden section search - -template -void BezierQuad::setBezier(const Vector3_t &pt0, const Vector3_t &ctrl, const Vector3_t &pt1, - Tp duration) -{ - _pt0 = pt0; - _ctrl = ctrl; - _pt1 = pt1; - _duration = duration; - _cached_resolution = (Tp)(-1); -} - -template -void BezierQuad::getBezier(Vector3_t &pt0, Vector3_t &ctrl, Vector3_t &pt1) -{ - pt0 = _pt0; - ctrl = _ctrl; - pt1 = _pt1; -} - -template -matrix::Vector BezierQuad::getPoint(const Tp t) -{ - return (_pt0 * ((Tp)1 - t / _duration) * ((Tp)1 - t / _duration) + _ctrl * (Tp)2 * (( - Tp)1 - t / _duration) * t / _duration + _pt1 * - t / _duration * t / _duration); -} - -template -matrix::Vector BezierQuad::getVelocity(const Tp t) -{ - return (((_ctrl - _pt0) * _duration + (_pt0 - _ctrl * (Tp)2 + _pt1) * t) * (Tp)2 / (_duration * _duration)); -} - -template -matrix::Vector BezierQuad::getAcceleration() -{ - return ((_pt0 - _ctrl * (Tp)2 + _pt1) * (Tp)2 / (_duration * _duration)); -} - -template -void BezierQuad::getStates(Vector3_t &point, Vector3_t &vel, Vector3_t &acc, const Tp time) -{ - point = getPoint(time); - vel = getVelocity(time); - acc = getAcceleration(); -} - -template -void BezierQuad::getStatesClosest(Vector3_t &point, Vector3_t &vel, Vector3_t &acc, - const Vector3_t pose) -{ - // get t that corresponds to point closest on bezier point - Tp t = _goldenSectionSearch(pose); - - // get states corresponding to t - getStates(point, vel, acc, t); - -} - -template -void BezierQuad::setBezFromVel(const Vector3_t &ctrl, const Vector3_t &vel0, const Vector3_t &vel1, - const Tp duration) -{ - // update bezier points - _ctrl = ctrl; - _duration = duration; - _pt0 = _ctrl - vel0 * _duration / (Tp)2; - _pt1 = _ctrl + vel1 * _duration / (Tp)2; - _cached_resolution = (Tp)(-1); -} - -template -Tp BezierQuad::getArcLength(const Tp resolution) -{ - // we don't need to recompute arc length if: - // 1. _cached_resolution is up to date; 2. _cached_resolution is smaller than desired resolution (= more accurate) - if ((_cached_resolution > (Tp)0) && (_cached_resolution <= resolution)) { - return _cached_arc_length; - } - - // get number of elements - int n = (int)(roundf(_duration / resolution)); - - // check if n is even - if (n % 2 == 1) { - n += 1; - } - - // step size - Tp h = (_duration) / n; - - // get integration - _cached_arc_length = (Tp)0; - Vector3_t y; - - for (int i = 1; i < n; i++) { - - y = getVelocity(h * i); - - if (i % 2 == 1) { - _cached_arc_length += (Tp)4 * y.length(); - - } else { - _cached_arc_length += (Tp)2 * y.length(); - } - } - - // velocity length at start and end points - Tp y0 = getVelocity((Tp)0).length(); - Tp yn = getVelocity(_duration).length(); - - // 1/3 simpsons rule - _cached_arc_length = h / (Tp)3 * (y0 + yn + _cached_arc_length); - - // update cache - _cached_resolution = resolution; - - return _cached_arc_length; -} - -template -Tp BezierQuad::getDistToClosestPoint(const Vector3_t &pose) -{ - // get t that corresponds to point closest on bezier point - Tp t = _goldenSectionSearch(pose); - - // get closest point - Vector3_t point = getPoint(t); - return (pose - point).length(); -} - -template -Tp BezierQuad::_goldenSectionSearch(const Vector3_t &pose) -{ - Tp a, b, c, d; - a = (Tp)0; // represents most left point - b = _duration * (Tp)1; // represents most right point - - c = b - (b - a) / GOLDEN_RATIO; - d = a + (b - a) / GOLDEN_RATIO; - - while (fabsf(c - d) > RESOLUTION) { - if (_getDistanceSquared(c, pose) < _getDistanceSquared(d, pose)) { - b = d; - - } else { - a = c; - } - - c = b - (b - a) / GOLDEN_RATIO; - d = a + (b - a) / GOLDEN_RATIO; - } - - return (b + a) / (Tp)2; -} - -template -Tp BezierQuad::_getDistanceSquared(const Tp t, const Vector3_t &pose) -{ - // get point on bezier - Vector3_t vec = getPoint(t); - - // get vector from point to pose - vec = vec - pose; - - // norm squared - return (vec * vec); -} -} - -#endif diff --git a/src/lib/bezier/BezierQuad.hpp b/src/lib/bezier/BezierQuad.hpp deleted file mode 100644 index 5aebe94b7b..0000000000 --- a/src/lib/bezier/BezierQuad.hpp +++ /dev/null @@ -1,210 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2018 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 BezierQuad.hpp - * - * Quadratic bezier lib - * - * A quadratic bezier function/spline is completely defined by three 3D points in space and a time scaling factor. - * pt0 and pt1 define the start and end points of the spline. ctrl point is a point in space that effects the curvature - * of the spline. The time scaling factor (= duration) defines the time it takes to travel along the spline from pt0 to - * pt1. - * A bezier spline is a continuous function from which position, velocity and acceleration can be extracted. For a given spline, - * acceleration stays constant. - */ - - -#pragma once - -#include - -namespace bezier -{ -template -class BezierQuad -{ -public: - - using Vector3_t = matrix::Vector; - - /** - * Empty constructor - */ - BezierQuad() : - _pt0(Vector3_t()), _ctrl(Vector3_t()), _pt1(Vector3_t()), _duration(1.0f) {} - - /** - * Constructor from array - */ - BezierQuad(const Tp pt0[3], const Tp ctrl[3], const Tp pt1[3], Tp duration = 1.0f) : - _pt0(Vector3_t(pt0)), _ctrl(Vector3_t(ctrl)), _pt1(Vector3_t(pt1)), _duration(duration) {} - - /** - * Constructor from vector - */ - BezierQuad(const Vector3_t &pt0, const Vector3_t &ctrl, const Vector3_t &pt1, - Tp duration = 1.0f): - _pt0(pt0), _ctrl(ctrl), _pt1(pt1), _duration(duration) {} - - - /* - * Get bezier points - */ - void getBezier(Vector3_t &pt0, Vector3_t &ctrl, Vector3_t &pt1); - - /* - * Return pt0 - */ - Vector3_t getPt0() {return _pt0;} - - /* - * Return ctrl - */ - Vector3_t getCtrl() {return _ctrl;} - - /* - * Return pt1 - */ - Vector3_t getPt1() {return _pt1;} - - /** - * Set new bezier points and duration - */ - void setBezier(const Vector3_t &pt0, const Vector3_t &ctrl, const Vector3_t &pt1, - Tp duration = (Tp)1); - - /* - * Set duration - * - * @param time is the total time it takes to travel along the bezier spline. - */ - void setDuration(const Tp time) {_duration = time;} - - /** - * Return point on bezier point corresponding to time t - * - * @param t is a time in seconds in between [0, duration] - * @return a point on bezier - */ - Vector3_t getPoint(const Tp t); - - /* - * Distance to closest point given a position - * - * @param pose is a position in 3D space from which distance to bezier is computed. - * @return distance to closest point on bezier - */ - Tp getDistToClosestPoint(const Vector3_t &pose); - - /* - * Return velocity on bezier corresponding to time t - * - * @param t is a time in seconds in between [0, duration] - * @return velocity vector at time t - */ - Vector3_t getVelocity(const Tp t); - - /* - * Return acceleration on bezier corresponding to time t - * - * @return constant acceleration of bezier - */ - Vector3_t getAcceleration(); - - /* - * Get all states on bezier corresponding to time t - */ - void getStates(Vector3_t &point, Vector3_t &vel, Vector3_t &acc, const Tp t); - - /* - * Get states on bezier which are closest to pose in space - * - * @param point is a posiiton on the spline that is closest to a given pose - * @param vel is the velocity at that given point - * @param acc is the acceleration for that spline - * @param pose represent a position in space from which closest point is computed - */ - void getStatesClosest(Vector3_t &point, Vector3_t &vel, Vector3_t &acc, - const Vector3_t pose); - - /* - * Compute bezier from velocity at bezier end points and ctrl point - * - * The bezier end points are fully defined by a given control point ctrl, the duration and - * the desired velocity vectors at the end points. - */ - void setBezFromVel(const Vector3_t &ctrl, const Vector3_t &vel0, const Vector3_t &vel1, - const Tp duration = (Tp)1); - - /* - * Return the arc length of a bezier spline - * - * The arc length is computed with simpsons integration. - * @param resolution in meters. - */ - Tp getArcLength(const Tp resolution); - -private: - - Vector3_t _pt0; /**< Bezier starting point */ - Vector3_t _ctrl; /**< Bezier control point */ - Vector3_t _pt1; /**< bezier end point */ - Tp _duration = (Tp)1; /**< Total time to travle along spline */ - - Tp _cached_arc_length = (Tp)0; /**< The saved arc length of the spline */ - Tp _cached_resolution = (Tp)(-1); /**< The resolution used to compute the arc length. - Negative number means that cache is not up to date. */ - - /* - * Golden section search - */ - Tp _goldenSectionSearch(const Vector3_t &pose); - - /* - * Get squared distance from 3D pose in space and a point on bezier. - * - * @param t is the time in between [0, duration] that defines a point on the bezier. - * @param pose is a 3D pose in space. - */ - Tp _getDistanceSquared(const Tp t, const Vector3_t &pose); - - -}; - -using BezierQuad_f = BezierQuad; -using BezierQuad_d = BezierQuad; -} - -// include implementation -#include "BezierQuad.cpp" diff --git a/src/lib/drivers/device/qurt/I2C.cpp b/src/lib/drivers/device/qurt/I2C.cpp index c2b70f66d3..a459dbbe81 100644 --- a/src/lib/drivers/device/qurt/I2C.cpp +++ b/src/lib/drivers/device/qurt/I2C.cpp @@ -55,7 +55,16 @@ I2C::_config_i2c_bus_func_t I2C::_config_i2c_bus = NULL; I2C::_set_i2c_address_func_t I2C::_set_i2c_address = NULL; I2C::_i2c_transfer_func_t I2C::_i2c_transfer = NULL; -pthread_mutex_t I2C::_mutex = PTHREAD_MUTEX_INITIALIZER; +// There is a mutex per I2C bus. A mutex isn't required with normal +// use since per bus I2C accesses are made via work item tied to a per bus thread. +// But it is here anyways in case someone decides to use the bus in some different +// custom code that has a separate thread. +struct I2C::_bus_mutex_t I2C::_bus_mutex[I2C::MAX_I2C_BUS] = { + {1, PTHREAD_MUTEX_INITIALIZER}, + {2, PTHREAD_MUTEX_INITIALIZER}, + {4, PTHREAD_MUTEX_INITIALIZER}, + {5, PTHREAD_MUTEX_INITIALIZER} +}; I2C::I2C(uint8_t device_type, const char *name, const int bus, const uint16_t address, const uint32_t frequency) : CDev(name, nullptr), @@ -91,10 +100,27 @@ I2C::init() goto out; } - pthread_mutex_lock(&_mutex); + if (_mutex == nullptr) { + for (int i = 0; i < MAX_I2C_BUS; i++) { + if (get_device_bus() == _bus_mutex[i]._bus) { + _mutex = &_bus_mutex[i]._mutex; + break; + } + } + + if (_mutex == nullptr) { + PX4_ERR("NULL i2c bus mutex"); + goto out; + + } else { + PX4_INFO("Set up I2C bus mutex for bus %d", get_device_bus()); + } + } + + pthread_mutex_lock(_mutex); // Open the actual I2C device _i2c_fd = _config_i2c_bus(get_device_bus(), get_device_address(), _frequency); - pthread_mutex_unlock(&_mutex); + pthread_mutex_unlock(_mutex); if (_i2c_fd == PX4_ERROR) { PX4_ERR("i2c init failed"); @@ -131,9 +157,9 @@ I2C::set_device_address(int address) if ((_i2c_fd != PX4_ERROR) && (_set_i2c_address != NULL)) { PX4_INFO("Set i2c address 0x%x, fd %d", address, _i2c_fd); - pthread_mutex_lock(&_mutex); + pthread_mutex_lock(_mutex); _set_i2c_address(_i2c_fd, address); - pthread_mutex_unlock(&_mutex); + pthread_mutex_unlock(_mutex); Device::set_device_address(address); } @@ -150,9 +176,9 @@ I2C::transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const do { // PX4_INFO("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len); - pthread_mutex_lock(&_mutex); + pthread_mutex_lock(_mutex); ret = _i2c_transfer(_i2c_fd, send, send_len, recv, recv_len); - pthread_mutex_unlock(&_mutex); + pthread_mutex_unlock(_mutex); if (ret != PX4_ERROR) { break; } diff --git a/src/lib/drivers/device/qurt/I2C.hpp b/src/lib/drivers/device/qurt/I2C.hpp index 7f1d59bc0a..ccf5e1f5df 100644 --- a/src/lib/drivers/device/qurt/I2C.hpp +++ b/src/lib/drivers/device/qurt/I2C.hpp @@ -124,10 +124,18 @@ protected: private: uint32_t _frequency{0}; int _i2c_fd{-1}; + pthread_mutex_t *_mutex{nullptr}; + + static const int MAX_I2C_BUS{4}; + static _config_i2c_bus_func_t _config_i2c_bus; static _set_i2c_address_func_t _set_i2c_address; static _i2c_transfer_func_t _i2c_transfer; - static pthread_mutex_t _mutex; + + static struct _bus_mutex_t { + int _bus; + pthread_mutex_t _mutex{PTHREAD_MUTEX_INITIALIZER}; + } _bus_mutex[MAX_I2C_BUS]; }; } // namespace device diff --git a/src/lib/mixer_module/CMakeLists.txt b/src/lib/mixer_module/CMakeLists.txt index 783dfe3dde..3a1581118b 100644 --- a/src/lib/mixer_module/CMakeLists.txt +++ b/src/lib/mixer_module/CMakeLists.txt @@ -48,6 +48,7 @@ px4_add_library(mixer_module functions/FunctionConstantMax.hpp functions/FunctionConstantMin.hpp functions/FunctionGimbal.hpp + functions/FunctionICEngineControl.hpp functions/FunctionLandingGear.hpp functions/FunctionManualRC.hpp functions/FunctionMotors.hpp diff --git a/src/lib/avoidance/ObstacleAvoidance_dummy.hpp b/src/lib/mixer_module/functions/FunctionICEngineControl.hpp similarity index 54% rename from src/lib/avoidance/ObstacleAvoidance_dummy.hpp rename to src/lib/mixer_module/functions/FunctionICEngineControl.hpp index 7831c919fd..0609f5cc94 100644 --- a/src/lib/avoidance/ObstacleAvoidance_dummy.hpp +++ b/src/lib/mixer_module/functions/FunctionICEngineControl.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2025 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,63 +31,53 @@ * ****************************************************************************/ -/** - * @file ObstacleAvoidance_dummy.hpp - * This is a dummy class to reduce flash space for when obstacle avoidance is not required - * - * @author Julian Kent - */ - #pragma once -#include -#include +#include "FunctionProviderBase.hpp" +#include -#include - - -class ObstacleAvoidance +/** + * Functions: ICE... + */ +class FunctionICEControl : public FunctionProviderBase { public: - ObstacleAvoidance(void *) {} // takes void* argument to be compatible with ModuleParams constructor - - - void injectAvoidanceSetpoints(matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, float &yaw_sp, - float &yaw_speed_sp) + FunctionICEControl() { - notify_dummy(); - }; - - void updateAvoidanceDesiredWaypoints(const matrix::Vector3f &curr_wp, const float curr_yaw, - const float curr_yawspeed, - const matrix::Vector3f &next_wp, const float next_yaw, const float next_yawspeed, const bool ext_yaw_active, - const int wp_type) - { - notify_dummy(); - }; - - void updateAvoidanceDesiredSetpoints(const matrix::Vector3f &pos_sp, const matrix::Vector3f &vel_sp, - const int type) - { - notify_dummy(); + resetAllToDisarmedValue(); } + static FunctionProviderBase *allocate(const Context &context) { return new FunctionICEControl(); } - void checkAvoidanceProgress(const matrix::Vector3f &pos, const matrix::Vector3f &prev_wp, - float target_acceptance_radius, const matrix::Vector2f &closest_pt) + void update() override { - notify_dummy(); - }; + internal_combustion_engine_control_s internal_combustion_engine_control; -protected: - - bool _logged_error = false; - void notify_dummy() - { - if (!_logged_error) { - PX4_ERR("Dummy avoidance library called!"); - _logged_error = true; + // map [0, 1] to [-1, 1] which is the interface for non-motor PWM channels + if (_internal_combustion_engine_control_sub.update(&internal_combustion_engine_control)) { + _data[0] = internal_combustion_engine_control.ignition_on * 2.f - 1.f; + _data[1] = internal_combustion_engine_control.throttle_control * 2.f - 1.f; + _data[2] = internal_combustion_engine_control.choke_control * 2.f - 1.f; + _data[3] = internal_combustion_engine_control.starter_engine_control * 2.f - 1.f; } } + + float value(OutputFunction func) override { return _data[(int)func - (int)OutputFunction::IC_Engine_Ignition]; } + +private: + static constexpr int num_data_points = 4; + + void resetAllToDisarmedValue() + { + for (int i = 0; i < num_data_points; ++i) { + _data[i] = NAN; + } + } + + static_assert(num_data_points == (int)OutputFunction::IC_Engine_Starter - (int)OutputFunction::IC_Engine_Ignition + 1, + "number of functions mismatch"); + + uORB::Subscription _internal_combustion_engine_control_sub{ORB_ID(internal_combustion_engine_control)}; + float _data[num_data_points] {}; }; diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index 45f56ab540..56a2d61356 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -64,6 +64,7 @@ static const FunctionProvider all_function_providers[] = { {OutputFunction::Gripper, &FunctionGripper::allocate}, {OutputFunction::RC_Roll, OutputFunction::RC_AUXMax, &FunctionManualRC::allocate}, {OutputFunction::Gimbal_Roll, OutputFunction::Gimbal_Yaw, &FunctionGimbal::allocate}, + {OutputFunction::IC_Engine_Ignition, OutputFunction::IC_Engine_Starter, &FunctionICEControl::allocate}, }; MixingOutput::MixingOutput(const char *param_prefix, uint8_t max_num_outputs, OutputModuleInterface &interface, diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index ff400ca6d5..c55b72da4e 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -40,6 +40,7 @@ #include "functions/FunctionConstantMin.hpp" #include "functions/FunctionGimbal.hpp" #include "functions/FunctionGripper.hpp" +#include "functions/FunctionICEngineControl.hpp" #include "functions/FunctionLandingGear.hpp" #include "functions/FunctionLandingGearWheel.hpp" #include "functions/FunctionManualRC.hpp" diff --git a/src/lib/mixer_module/output_functions.yaml b/src/lib/mixer_module/output_functions.yaml index a9965a5454..52543add97 100644 --- a/src/lib/mixer_module/output_functions.yaml +++ b/src/lib/mixer_module/output_functions.yaml @@ -38,6 +38,11 @@ functions: Landing_Gear_Wheel: 440 + IC_Engine_Ignition: 450 + IC_Engine_Throttle: 451 + IC_Engine_Choke: 452 + IC_Engine_Starter: 453 + # Add your own here: #MyCustomFunction: 10000 diff --git a/src/lib/parameters/CMakeLists.txt b/src/lib/parameters/CMakeLists.txt index b87e652cad..81b609080a 100644 --- a/src/lib/parameters/CMakeLists.txt +++ b/src/lib/parameters/CMakeLists.txt @@ -62,6 +62,10 @@ if(DISABLE_PARAMS_MODULE_SCOPING) # allow those timer configurations to be skipped. if ((${PX4_BOARD_NAME} MATCHES "MODALAI_VOXL2") AND (file_path MATCHES pwm_out)) message(STATUS "Skipping pwm file path ${file_path} for VOXL2") + # Spacecraft has duplicate parameter names which kills the VOXL 2 build. VOXL 2 does not + # support the spacecraft module so we skip adding the parameters. + elseif ((${PX4_BOARD_NAME} MATCHES "MODALAI_VOXL2") AND (file_path MATCHES spacecraft)) + message(STATUS "Skipping spacecraft file path ${file_path} for VOXL2") else() list(APPEND module_config_files "${file_path}") endif() diff --git a/src/lib/parameters/flashparams/flashfs32.c b/src/lib/parameters/flashparams/flashfs32.c index 848623b261..25e97d2563 100644 --- a/src/lib/parameters/flashparams/flashfs32.c +++ b/src/lib/parameters/flashparams/flashfs32.c @@ -1128,6 +1128,11 @@ int parameter_flashfs_init(sector_descriptor_t *fconfig, uint8_t *buffer, uint16 if (pf == NULL) { // Parameters can't be found, assume sector is corrupt or empty rv = parameter_flashfs_erase(); + + // A positive return value means flash space has been erased successfully. + if (rv > 0) { + rv = 0; + } } return rv; diff --git a/src/lib/rc/ghst.cpp b/src/lib/rc/ghst.cpp index a0bce68718..306004059c 100644 --- a/src/lib/rc/ghst.cpp +++ b/src/lib/rc/ghst.cpp @@ -56,6 +56,7 @@ #include #include #include +#include // TODO: include RSSI dBm to percentage conversion for ghost receiver #include "spektrum_rssi.h" @@ -77,8 +78,8 @@ enum class ghst_parser_state_t : uint8_t { synced }; -// only RSSI frame contains value of RSSI, if it is not received, send last received RSSI -static int8_t ghst_rssi = -1; +// only RSSI frame contains value of RSSI, if it is not received, send last received RSSI/LQ +static ghstLinkStatistics_t last_link_stats = { .rssi_pct = -1, .rssi_dbm = NAN, .link_quality = 0 }; static ghst_frame_t &ghst_frame = rc_decode_buf.ghst_frame; static uint32_t current_frame_position = 0U; @@ -89,7 +90,8 @@ static uint16_t prev_rc_vals[GHST_MAX_NUM_CHANNELS]; /** * parse the current ghst_frame buffer */ -static bool ghst_parse_buffer(uint16_t *values, int8_t *rssi, uint16_t *num_values, uint16_t max_channels); +static bool ghst_parse_buffer(uint16_t *values, ghstLinkStatistics_t *link_stats, uint16_t *num_values, + uint16_t max_channels); int ghst_config(int uart_fd) { @@ -114,7 +116,7 @@ static uint16_t convert_channel_value(unsigned chan_value); bool ghst_parse(const uint64_t now, const uint8_t *frame, unsigned len, uint16_t *values, - int8_t *rssi, uint16_t *num_values, uint16_t max_channels) + ghstLinkStatistics_t *link_stats, uint16_t *num_values, uint16_t max_channels) { bool success = false; uint8_t *ghst_frame_ptr = (uint8_t *)&ghst_frame; @@ -145,7 +147,7 @@ bool ghst_parse(const uint64_t now, const uint8_t *frame, unsigned len, uint16_t len -= current_len; frame += current_len; - if (ghst_parse_buffer(values, rssi, num_values, max_channels)) { + if (ghst_parse_buffer(values, link_stats, num_values, max_channels)) { success = true; } } @@ -182,7 +184,8 @@ static uint16_t convert_channel_value(unsigned int chan_value) return converted_chan_value; } -static bool ghst_parse_buffer(uint16_t *values, int8_t *rssi, uint16_t *num_values, uint16_t max_channels) +static bool ghst_parse_buffer(uint16_t *values, ghstLinkStatistics_t *link_stats, uint16_t *num_values, + uint16_t max_channels) { uint8_t *ghst_frame_ptr = (uint8_t *)&ghst_frame; @@ -299,13 +302,16 @@ static bool ghst_parse_buffer(uint16_t *values, int8_t *rssi, uint16_t *num_valu } else if (ghst_frame.type == static_cast(ghstFrameType::frameTypeRssi)) { const ghstPayloadRssi_t *const rssiValues = (ghstPayloadRssi_t *)&ghst_frame.payload; // TODO: call function for RSSI dBm to percentage conversion for ghost receiver - ghst_rssi = spek_dbm_to_percent(static_cast(rssiValues->rssidBm)); + last_link_stats.rssi_pct = spek_dbm_to_percent(static_cast + (rssiValues->rssidBm)); // rssidBm sign inverted (90 = -90dBm) + last_link_stats.rssi_dbm = -rssiValues->rssidBm; + last_link_stats.link_quality = rssiValues->lq; // 0 - 100 } else { GHST_DEBUG("Frame type: %u", ghst_frame.type); } - *rssi = ghst_rssi; + *link_stats = last_link_stats; memcpy(prev_rc_vals, values, sizeof(uint16_t) * GHST_MAX_NUM_CHANNELS); diff --git a/src/lib/rc/ghst.hpp b/src/lib/rc/ghst.hpp index 6f5a4f4664..c8fbc407c5 100644 --- a/src/lib/rc/ghst.hpp +++ b/src/lib/rc/ghst.hpp @@ -106,6 +106,13 @@ typedef struct { int txPowerdBm: 8; // Tx power [dBm] } __attribute__((__packed__)) ghstPayloadRssi_t; +// Link statistics for internal transport +typedef struct { + int8_t rssi_pct; + float rssi_dbm; + int8_t link_quality; +} ghstLinkStatistics_t; + /** * Configure an UART port to be used for GHST * @param uart_fd UART file descriptor @@ -127,7 +134,7 @@ __EXPORT int ghst_config(int uart_fd); * @return true if channels successfully decoded */ __EXPORT bool ghst_parse(const uint64_t now, const uint8_t *frame, unsigned len, uint16_t *values, - int8_t *rssi, uint16_t *num_values, uint16_t max_channels); + ghstLinkStatistics_t *link_stats, uint16_t *num_values, uint16_t max_channels); /** diff --git a/src/lib/rc/rc_tests/RCTest.cpp b/src/lib/rc/rc_tests/RCTest.cpp index 1997c19e85..4612673b92 100644 --- a/src/lib/rc/rc_tests/RCTest.cpp +++ b/src/lib/rc/rc_tests/RCTest.cpp @@ -159,7 +159,7 @@ bool RCTest::ghstTest() uint16_t rc_values[max_channels]; uint16_t num_values = 0; int line_counter = 1; - int8_t ghst_rssi = -1; + ghstLinkStatistics_t link_stats; ghst_config(uart_fd); while (fgets(line, line_size, fp) != nullptr) { @@ -186,7 +186,7 @@ bool RCTest::ghstTest() // Pipe the data into the parser hrt_abstime now = hrt_absolute_time(); - bool result = ghst_parse(now, frame, frame_len, rc_values, &ghst_rssi, &num_values, max_channels); + bool result = ghst_parse(now, frame, frame_len, rc_values, &link_stats, &num_values, max_channels); if (result) { has_decoded_values = true; diff --git a/src/lib/rover_control/RoverControl.cpp b/src/lib/rover_control/RoverControl.cpp index 7b0864a428..0eafcaa9f8 100644 --- a/src/lib/rover_control/RoverControl.cpp +++ b/src/lib/rover_control/RoverControl.cpp @@ -48,26 +48,24 @@ float throttleControl(SlewRate &motor_setpoint, const float throttle_setp if (accelerating && max_accel > FLT_EPSILON && max_thr_spd > FLT_EPSILON) { // Acceleration slew rate motor_setpoint.setSlewRate(max_accel / max_thr_spd); + motor_setpoint.update(throttle_setpoint, dt); // Reinitialize slew rate if current value is closer to setpoint than post slew rate value if (fabsf(motor_setpoint.getState() - current_motor_setpoint) > fabsf(throttle_setpoint - current_motor_setpoint)) { - motor_setpoint.setForcedValue(current_motor_setpoint); + motor_setpoint.setForcedValue(throttle_setpoint); } - motor_setpoint.update(throttle_setpoint, dt); - } else if (!accelerating && max_decel > FLT_EPSILON && max_thr_spd > FLT_EPSILON) { // Deceleration slew rate motor_setpoint.setSlewRate(max_decel / max_thr_spd); + motor_setpoint.update(throttle_setpoint, dt); // Reinitialize slew rate if current value is closer to setpoint than post slew rate value if (fabsf(motor_setpoint.getState() - current_motor_setpoint) > fabsf(throttle_setpoint - current_motor_setpoint)) { - motor_setpoint.setForcedValue(current_motor_setpoint); + motor_setpoint.setForcedValue(throttle_setpoint); } - motor_setpoint.update(throttle_setpoint, dt); - } else { // Fallthrough if slew rate parameters are not configured motor_setpoint.setForcedValue(throttle_setpoint); } @@ -88,14 +86,13 @@ float attitudeControl(SlewRateYaw &adjusted_yaw_setpoint, PID &pid_yaw, if (yaw_slew_rate > FLT_EPSILON) { // Apply slew rate if configured adjusted_yaw_setpoint.setSlewRate(yaw_slew_rate); + adjusted_yaw_setpoint.update(yaw_setpoint, dt); // Reinitialize slew rate if current value is closer to setpoint than post slew rate value if (fabsf(wrap_pi(adjusted_yaw_setpoint.getState() - vehicle_yaw)) > fabsf(wrap_pi(yaw_setpoint - vehicle_yaw))) { - adjusted_yaw_setpoint.setForcedValue(vehicle_yaw); + adjusted_yaw_setpoint.setForcedValue(yaw_setpoint); } - adjusted_yaw_setpoint.update(yaw_setpoint, dt); - } else { adjusted_yaw_setpoint.setForcedValue(yaw_setpoint); } @@ -118,26 +115,24 @@ float speedControl(SlewRate &speed_with_rate_limit, PID &pid_speed, const // Apply acceleration and deceleration limit if (fabsf(speed_setpoint) >= fabsf(vehicle_speed) && max_accel > FLT_EPSILON) { speed_with_rate_limit.setSlewRate(max_accel); + speed_with_rate_limit.update(speed_setpoint, dt); // Reinitialize slew rate if current value is closer to setpoint than post slew rate value if (fabsf(speed_with_rate_limit.getState() - vehicle_speed) > fabsf( speed_setpoint - vehicle_speed)) { - speed_with_rate_limit.setForcedValue(vehicle_speed); + speed_with_rate_limit.setForcedValue(speed_setpoint); } - speed_with_rate_limit.update(speed_setpoint, dt); - } else if (fabsf(speed_setpoint) < fabsf(vehicle_speed) && max_decel > FLT_EPSILON) { speed_with_rate_limit.setSlewRate(max_decel); + speed_with_rate_limit.update(speed_setpoint, dt); // Reinitialize slew rate if current value is closer to setpoint than post slew rate value if (fabsf(speed_with_rate_limit.getState() - vehicle_speed) > fabsf( speed_setpoint - vehicle_speed)) { - speed_with_rate_limit.setForcedValue(vehicle_speed); + speed_with_rate_limit.setForcedValue(speed_setpoint); } - speed_with_rate_limit.update(speed_setpoint, dt); - } else { // Fallthrough if slew rate is not configured speed_with_rate_limit.setForcedValue(speed_setpoint); } @@ -153,12 +148,72 @@ float speedControl(SlewRate &speed_with_rate_limit, PID &pid_speed, const } // Feedback control - pid_speed.setSetpoint(speed_with_rate_limit.getState()); - forward_speed_normalized += pid_speed.update(vehicle_speed, dt); + if (fabsf(speed_with_rate_limit.getState()) > FLT_EPSILON) { + pid_speed.setSetpoint(speed_with_rate_limit.getState()); + forward_speed_normalized += pid_speed.update(vehicle_speed, dt); + + } else { + pid_speed.resetIntegral(); + } + return math::constrain(forward_speed_normalized, -1.f, 1.f); } +float rateControl(SlewRate &adjusted_yaw_rate_setpoint, PID &pid_yaw_rate, const float yaw_rate_setpoint, + const float vehicle_yaw_rate, const float max_thr_yaw_r, const float max_yaw_accel, const float max_yaw_decel, + const float wheel_track, const float dt) +{ + // Apply acceleration and deceleration limit + if (fabsf(yaw_rate_setpoint) >= fabsf(vehicle_yaw_rate) && max_yaw_accel > FLT_EPSILON) { + adjusted_yaw_rate_setpoint.setSlewRate(max_yaw_accel); + adjusted_yaw_rate_setpoint.update(yaw_rate_setpoint, dt); + + // Reinitialize slew rate if current value is closer to setpoint than post slew rate value + if (fabsf(adjusted_yaw_rate_setpoint.getState() - vehicle_yaw_rate) > fabsf( + yaw_rate_setpoint - vehicle_yaw_rate)) { + adjusted_yaw_rate_setpoint.setForcedValue(yaw_rate_setpoint); + } + + + } else if (fabsf(yaw_rate_setpoint) < fabsf(vehicle_yaw_rate) && max_yaw_decel > FLT_EPSILON) { + adjusted_yaw_rate_setpoint.setSlewRate(max_yaw_decel); + adjusted_yaw_rate_setpoint.update(yaw_rate_setpoint, dt); + + // Reinitialize slew rate if current value is closer to setpoint than post slew rate value + if (fabsf(adjusted_yaw_rate_setpoint.getState() - vehicle_yaw_rate) > fabsf( + yaw_rate_setpoint - vehicle_yaw_rate)) { + adjusted_yaw_rate_setpoint.setForcedValue(yaw_rate_setpoint); + } + + + } else { // Fallthrough if slew rate is not configured + adjusted_yaw_rate_setpoint.setForcedValue(yaw_rate_setpoint); + } + + // Transform yaw rate into speed difference + float speed_diff_normalized{0.f}; + + if (wheel_track > FLT_EPSILON && max_thr_yaw_r > FLT_EPSILON) { // Feedforward + const float speed_diff = adjusted_yaw_rate_setpoint.getState() * wheel_track / + 2.f; + speed_diff_normalized = math::interpolate(speed_diff, -max_thr_yaw_r, + max_thr_yaw_r, -1.f, 1.f); + } + + // Feedback control + if (fabsf(adjusted_yaw_rate_setpoint.getState()) > FLT_EPSILON) { + pid_yaw_rate.setSetpoint(adjusted_yaw_rate_setpoint.getState()); + speed_diff_normalized += pid_yaw_rate.update(vehicle_yaw_rate, dt); + + } else { + pid_yaw_rate.resetIntegral(); + } + + + return math::constrain(speed_diff_normalized, -1.f, 1.f); +} + void globalToLocalSetpointTriplet(Vector2f &curr_wp_ned, Vector2f &prev_wp_ned, Vector2f &next_wp_ned, position_setpoint_triplet_s position_setpoint_triplet, Vector2f &curr_pos_ned, Vector2d &home_pos, MapProjection &global_ned_proj_ref) diff --git a/src/lib/rover_control/RoverControl.hpp b/src/lib/rover_control/RoverControl.hpp index 7f76710225..1196e04650 100644 --- a/src/lib/rover_control/RoverControl.hpp +++ b/src/lib/rover_control/RoverControl.hpp @@ -92,6 +92,25 @@ float attitudeControl(SlewRateYaw &adjusted_yaw_setpoint, PID &pid_yaw, f float speedControl(SlewRate &speed_with_rate_limit, PID &pid_speed, float speed_setpoint, float vehicle_speed, float max_accel, float max_decel, float max_thr_speed, float dt); +/** + * Applies yaw acceleration slew rate to a yaw rate setpoint and calculates the necessary speed diff setpoint + * using a feedforward term and/or a PID controller. + * Note: This function is only for rovers that control the rate through a speed difference between the left/right wheels. + * @param adjusted_yaw_rate_setpoint Yaw rate setpoint with applied slew rate [-1, 1] (Updated by this function). + * @param pid_yaw_rate Yaw rate PID (Updated by this function). + * @param yaw_rate_setpoint Yaw rate setpoint [rad/s]. + * @param vehicle_yaw_rate Measured vehicle yaw rate [rad/s]. + * @param max_thr_yaw_r Yaw rate turning left/right wheels at max speed in opposite directions [m/s]. + * @param max_yaw_accel Maximum allowed yaw acceleration [rad/s^2]. + * @param max_yaw_decel Maximum allowed yaw deceleration [rad/s^2]. + * @param wheel_track Distance from the center of the right wheel to the center of the left wheel [m]. + * @param dt Time since last update [s]. + * @return Normalized speed difference setpoint [-1, 1]. + */ +float rateControl(SlewRate &adjusted_yaw_rate_setpoint, PID &pid_yaw_rate, + float yaw_rate_setpoint, float vehicle_yaw_rate, float max_thr_yaw_r, float max_yaw_accel, float max_yaw_decel, + float wheel_track, float dt); + /** * Projects positionSetpointTriplet waypoints from global to ned frame. * @param curr_wp_ned Current waypoint in NED frame (Updated by this function) diff --git a/src/lib/rover_control/module.yaml b/src/lib/rover_control/module.yaml deleted file mode 100644 index 8450a03b06..0000000000 --- a/src/lib/rover_control/module.yaml +++ /dev/null @@ -1,191 +0,0 @@ -module_name: Rover Control - -parameters: - - group: Rover Control - definitions: - RO_MAX_THR_SPEED: - description: - short: Speed the rover drives at maximum throttle - long: Used to linearly map speeds [m/s] to throttle values [-1. 1]. - type: float - unit: m/s - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 0 - - RO_ACCEL_LIM: - description: - short: Acceleration limit - long: | - Set to -1 to disable. - For mecanum rovers this limit is used for longitudinal and lateral acceleration. - type: float - unit: m/s^2 - min: -1 - max: 100 - increment: 0.01 - decimal: 2 - default: -1 - - RO_DECEL_LIM: - description: - short: Deceleration limit - long: | - Set to -1 to disable. - Note that if it is disabled the rover will not slow down when approaching waypoints in auto modes. - For mecanum rovers this limit is used for longitudinal and lateral deceleration. - type: float - unit: m/s^2 - min: -1 - max: 100 - increment: 0.01 - decimal: 2 - default: -1 - - RO_JERK_LIM: - description: - short: Jerk limit - long: | - Set to -1 to disable. - Note that if it is disabled the rover will not slow down when approaching waypoints in auto modes. - For mecanum rovers this limit is used for longitudinal and lateral jerk. - type: float - unit: m/s^3 - min: -1 - max: 100 - increment: 0.01 - decimal: 2 - default: -1 - - RO_YAW_RATE_TH: - description: - short: Yaw rate measurement threshold - long: The minimum threshold for the yaw rate measurement not to be interpreted as zero. - type: float - unit: deg/s - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 3 - - RO_SPEED_TH: - description: - short: Speed measurement threshold - long: The minimum threshold for the speed measurement not to be interpreted as zero. - type: float - unit: m/s - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 0.1 - - RO_YAW_STICK_DZ: - description: - short: Yaw stick deadzone - long: Percentage of stick input range that will be interpreted as zero around the stick centered value. - type: float - min: 0 - max: 1 - increment: 0.01 - decimal: 2 - default: 0.1 - - - group: Rover Rate Control - definitions: - RO_YAW_RATE_P: - description: - short: Proportional gain for closed loop yaw rate controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 3 - default: 0 - - RO_YAW_RATE_I: - description: - short: Integral gain for closed loop yaw rate controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 3 - default: 0 - - RO_YAW_RATE_LIM: - description: - short: Yaw rate limit - long: | - Used to cap yaw rate setpoints and map controller inputs to yaw rate setpoints - in Acro, Stabilized and Position mode. - type: float - unit: deg/s - min: 0 - max: 10000 - increment: 0.01 - decimal: 2 - default: 0 - - RO_YAW_ACCEL_LIM: - description: - short: Yaw acceleration limit - long: Set to -1 to disable. - type: float - unit: deg/s^2 - min: -1 - max: 10000 - increment: 0.01 - decimal: 2 - default: -1 - - - group: Rover Attitude Control - definitions: - RO_YAW_P: - description: - short: Proportional gain for closed loop yaw controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 3 - default: 0 - - - group: Rover Velocity Control - definitions: - RO_SPEED_P: - description: - short: Proportional gain for ground speed controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 0 - - RO_SPEED_I: - description: - short: Integral gain for ground speed controller - type: float - min: 0 - max: 100 - increment: 0.001 - decimal: 3 - default: 0 - - RO_SPEED_LIM: - description: - short: Speed limit - long: | - Used to cap speed setpoints and map controller inputs to speed setpoints - in Position mode. - type: float - unit: m/s - min: -1 - max: 100 - increment: 0.01 - decimal: 2 - default: -1 diff --git a/src/lib/rover_control/rovercontrol_params.c b/src/lib/rover_control/rovercontrol_params.c new file mode 100644 index 0000000000..adee1a69d5 --- /dev/null +++ b/src/lib/rover_control/rovercontrol_params.c @@ -0,0 +1,255 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 rovercontrol_params.c + * + * Parameters defined by the rover control lib. + */ + +/** + * Yaw stick deadzone + * + * Percentage of stick input range that will be interpreted as zero around the stick centered value. + * + * @min 0 + * @max 1 + * @increment 0.01 + * @decimal 2 + * @group Rover Rate Control + */ +PARAM_DEFINE_FLOAT(RO_YAW_STICK_DZ, 0.1f); + +/** + * Yaw rate measurement threshold + * + * The minimum threshold for the yaw rate measurement not to be interpreted as zero. + * + * @unit deg/s + * @min 0 + * @max 100 + * @increment 0.01 + * @decimal 2 + * @group Rover Rate Control + */ +PARAM_DEFINE_FLOAT(RO_YAW_RATE_TH, 3.f); + +/** + * Proportional gain for closed loop yaw rate controller + * + * @min 0 + * @max 100 + * @increment 0.01 + * @decimal 3 + * @group Rover Rate Control + */ +PARAM_DEFINE_FLOAT(RO_YAW_RATE_P, 0.f); + +/** + * Integral gain for closed loop yaw rate controller + * + * @min 0 + * @max 100 + * @increment 0.01 + * @decimal 3 + * @group Rover Rate Control + */ +PARAM_DEFINE_FLOAT(RO_YAW_RATE_I, 0.f); + +/** + * Yaw rate limit + * + * Used to cap yaw rate setpoints and map controller inputs to yaw rate setpoints + * in Acro, Stabilized and Position mode. + * + * @unit deg/s + * @min 0 + * @max 10000 + * @increment 0.01 + * @decimal 2 + * @group Rover Rate Control + */ +PARAM_DEFINE_FLOAT(RO_YAW_RATE_LIM, 0.f); + +/** + * Yaw acceleration limit + * + * Used to cap how quickly the magnitude of yaw rate setpoints can increase. + * Set to -1 to disable. + * + * @unit deg/s^2 + * @min -1 + * @max 10000 + * @increment 0.01 + * @decimal 2 + * @group Rover Rate Control + */ +PARAM_DEFINE_FLOAT(RO_YAW_ACCEL_LIM, -1.f); + +/** + * Yaw deceleration limit + * + * Used to cap how quickly the magnitude of yaw rate setpoints can decrease. + * Set to -1 to disable. + * + * @unit deg/s^2 + * @min -1 + * @max 10000 + * @increment 0.01 + * @decimal 2 + * @group Rover Rate Control + */ +PARAM_DEFINE_FLOAT(RO_YAW_DECEL_LIM, -1.f); + +/** + * Proportional gain for closed loop yaw controller + * + * @min 0 + * @max 100 + * @increment 0.01 + * @decimal 3 + * @group Rover Attitude Control + */ +PARAM_DEFINE_FLOAT(RO_YAW_P, 0.f); + +/** + * Speed the rover drives at maximum throttle + * + * Used to linearly map speeds [m/s] to throttle values [-1. 1]. + * + * @min 0 + * @max 100 + * @unit m/s + * @increment 0.01 + * @decimal 2 + * @group Rover Velocity Control + */ +PARAM_DEFINE_FLOAT(RO_MAX_THR_SPEED, 0.f); + +/** + * Proportional gain for ground speed controller + * + * @min 0 + * @max 100 + * @increment 0.01 + * @decimal 2 + * @group Rover Velocity Control + */ +PARAM_DEFINE_FLOAT(RO_SPEED_P, 0.f); + +/** + * Integral gain for ground speed controller + * + * @min 0 + * @max 100 + * @increment 0.001 + * @decimal 3 + * @group Rover Velocity Control + */ +PARAM_DEFINE_FLOAT(RO_SPEED_I, 0.f); + +/** + * Speed limit + * + * Used to cap speed setpoints and map controller inputs to speed setpoints in Position mode. + * + * @unit m/s + * @min -1 + * @max 100 + * @increment 0.01 + * @decimal 2 + * @group Rover Velocity Control + */ +PARAM_DEFINE_FLOAT(RO_SPEED_LIM, -1.f); + +/** + * Acceleration limit + * + * Set to -1 to disable. + * For mecanum rovers this limit is used for longitudinal and lateral acceleration. + * + * @unit m/s^2 + * @min -1 + * @max 100 + * @increment 0.01 + * @decimal 2 + * @group Rover Velocity Control + */ +PARAM_DEFINE_FLOAT(RO_ACCEL_LIM, -1.f); + +/** + * Deceleration limit + * + * Set to -1 to disable. + * Note that if it is disabled the rover will not slow down when approaching waypoints in auto modes. + * For mecanum rovers this limit is used for longitudinal and lateral deceleration. + * + * @unit m/s^2 + * @min -1 + * @max 100 + * @increment 0.01 + * @decimal 2 + * @group Rover Velocity Control + */ +PARAM_DEFINE_FLOAT(RO_DECEL_LIM, -1.f); + +/** + * Jerk limit + * + * Set to -1 to disable. + * Note that if it is disabled the rover will not slow down when approaching waypoints in auto modes. + * For mecanum rovers this limit is used for longitudinal and lateral jerk. + * + * @unit m/s^3 + * @min -1 + * @max 100 + * @increment 0.01 + * @decimal 2 + * @group Rover Velocity Control + */ +PARAM_DEFINE_FLOAT(RO_JERK_LIM, -1.f); + +/** + * Speed measurement threshold + * + * Set to -1 to disable. + * The minimum threshold for the speed measurement not to be interpreted as zero. + * + * @unit m/s + * @min 0 + * @max 100 + * @increment 0.01 + * @decimal 2 + * @group Rover Velocity Control + */ +PARAM_DEFINE_FLOAT(RO_SPEED_TH, 0.1f); diff --git a/src/lib/rtl/rtl_time_estimator.h b/src/lib/rtl/rtl_time_estimator.h index 8c8519cd9b..7ca79fba1f 100644 --- a/src/lib/rtl/rtl_time_estimator.h +++ b/src/lib/rtl/rtl_time_estimator.h @@ -116,7 +116,7 @@ private: float _time_estimate{0.f}; /**< Accumulated time estimate [s] */ bool _is_valid{false}; /**< Checks if time estimate is valid */ - uint8_t _vehicle_type{vehicle_status_s::VEHICLE_TYPE_UNKNOWN}; /**< the defined vehicle type to use for the calculation*/ + uint8_t _vehicle_type{vehicle_status_s::VEHICLE_TYPE_ROTARY_WING}; /**< the defined vehicle type to use for the calculation*/ DEFINE_PARAMETERS( (ParamFloat) _param_rtl_time_factor, /**< Safety factory for safe time estimate */ diff --git a/src/lib/sensor_calibration/Magnetometer.hpp b/src/lib/sensor_calibration/Magnetometer.hpp index 8d328e7a7d..5f9dd72d43 100644 --- a/src/lib/sensor_calibration/Magnetometer.hpp +++ b/src/lib/sensor_calibration/Magnetometer.hpp @@ -85,6 +85,7 @@ public: int8_t calibration_index() const { return _calibration_index; } uint32_t device_id() const { return _device_id; } bool enabled() const { return (_priority > 0); } + void disable() { _priority = 0; } bool external() const { return _external; } const matrix::Vector3f &offset() const { return _offset; } const int32_t &priority() const { return _priority; } diff --git a/src/lib/systemlib/system_params.c b/src/lib/systemlib/system_params.c index d396039f3f..644e849d9d 100644 --- a/src/lib/systemlib/system_params.c +++ b/src/lib/systemlib/system_params.c @@ -240,6 +240,18 @@ PARAM_DEFINE_INT32(SYS_HAS_NUM_ASPD, 0); */ PARAM_DEFINE_INT32(SYS_HAS_NUM_DIST, 0); +/** + * Number of optical flow sensors required to be available + * + * The preflight check will fail if fewer than this number of optical flow sensors with valid data are present. + * + * @group System + * @min 0 + * @max 1 + */ + +PARAM_DEFINE_INT32(SYS_HAS_NUM_OF, 0); + /** * Enable factory calibration mode * diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index f8012529e1..03b9d48574 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -388,7 +388,7 @@ AirspeedModule::Run() input_data.airspeed_indicated_raw = airspeed_raw.indicated_airspeed_m_s; input_data.airspeed_true_raw = airspeed_raw.true_airspeed_m_s; input_data.airspeed_timestamp = airspeed_raw.timestamp; - input_data.air_temperature_celsius = airspeed_raw.air_temperature_celsius; + input_data.air_temperature_celsius = _vehicle_air_data.ambient_temperature; if (_in_takeoff_situation) { // set flag to false if either speed is above stall speed, diff --git a/src/modules/battery_status/battery_status.cpp b/src/modules/battery_status/battery_status.cpp index 1d104e4087..8d92ad251b 100644 --- a/src/modules/battery_status/battery_status.cpp +++ b/src/modules/battery_status/battery_status.cpp @@ -137,9 +137,9 @@ private: BatteryStatus::BatteryStatus() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default), - _battery1(1, this, SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0), + _battery1(1, this, SAMPLE_INTERVAL_US, battery_status_s::SOURCE_POWER_MODULE, 0), #if BOARD_NUMBER_BRICKS > 1 - _battery2(2, this, SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE, 1), + _battery2(2, this, SAMPLE_INTERVAL_US, battery_status_s::SOURCE_POWER_MODULE, 1), #endif _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME)) { diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index e303077a90..6d89c3f45e 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -574,8 +574,7 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_ if (!_vehicle_control_mode.flag_control_climb_rate_enabled && !_failsafe_flags.manual_control_signal_lost && !_is_throttle_low - && _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROVER - && _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_BOAT) { + && !is_ground_vehicle(_vehicle_status)) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied: high throttle\t"); events::send(events::ID("commander_arm_denied_throttle_high"), {events::Log::Critical, events::LogInternal::Info}, @@ -700,7 +699,7 @@ Commander::Commander() : _vehicle_status.system_id = 1; _vehicle_status.component_id = 1; _vehicle_status.system_type = 0; - _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_UNKNOWN; + _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; _vehicle_status.nav_state = _user_mode_intention.get(); _vehicle_status.nav_state_user_intention = _user_mode_intention.get(); _vehicle_status.nav_state_timestamp = hrt_absolute_time(); @@ -1708,7 +1707,7 @@ void Commander::executeActionRequest(const action_request_s &action_request) case action_request_s::ACTION_SWITCH_MODE: - if (!_user_mode_intention.change(action_request.mode, ModeChangeSource::User, true)) { + if (!_user_mode_intention.change(action_request.mode, ModeChangeSource::User, false)) { printRejectMode(action_request.mode); } @@ -1729,8 +1728,6 @@ void Commander::updateParameters() _vehicle_status.system_type = value_int32; } - _vehicle_status.avoidance_system_required = _param_com_obs_avoid.get(); - _auto_disarm_killed.set_hysteresis_time_from(false, _param_com_kill_disarm.get() * 1_s); const bool is_rotary = is_rotary_wing(_vehicle_status) || (is_vtol(_vehicle_status) @@ -1813,9 +1810,6 @@ void Commander::run() _status_changed = true; } - /* Update OA parameter */ - _vehicle_status.avoidance_system_required = _param_com_obs_avoid.get(); - handlePowerButtonState(); systemPowerUpdate(); @@ -1908,8 +1902,7 @@ void Commander::run() _actuator_armed.armed = isArmed(); _actuator_armed.prearmed = getPrearmState(); _actuator_armed.ready_to_arm = _vehicle_status.pre_flight_checks_pass || isArmed(); - _actuator_armed.lockdown = ((_vehicle_status.nav_state == _vehicle_status.NAVIGATION_STATE_TERMINATION) - || (_vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON) + _actuator_armed.lockdown = ((_vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON) || _multicopter_throw_launch.isThrowLaunchInProgress()); // _actuator_armed.manual_lockdown // action_request_s::ACTION_KILL _actuator_armed.force_failsafe = (_vehicle_status.nav_state == _vehicle_status.NAVIGATION_STATE_TERMINATION); @@ -2212,13 +2205,13 @@ void Commander::updateTunes() } else if (!_vehicle_status.usb_connected && (_vehicle_status.hil_state != vehicle_status_s::HIL_STATE_ON) && - (_battery_warning == battery_status_s::BATTERY_WARNING_CRITICAL)) { + (_battery_warning == battery_status_s::WARNING_CRITICAL)) { /* play tune on battery critical */ set_tune(tune_control_s::TUNE_ID_BATTERY_WARNING_FAST); } else if ((_vehicle_status.hil_state != vehicle_status_s::HIL_STATE_ON) && - (_battery_warning == battery_status_s::BATTERY_WARNING_LOW)) { + (_battery_warning == battery_status_s::WARNING_LOW)) { /* play tune on battery warning */ set_tune(tune_control_s::TUNE_ID_BATTERY_WARNING_SLOW); @@ -2512,10 +2505,10 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning) if (_vehicle_status.failsafe) { led_color = led_control_s::COLOR_PURPLE; - } else if (battery_warning == battery_status_s::BATTERY_WARNING_LOW) { + } else if (battery_warning == battery_status_s::WARNING_LOW) { led_color = led_control_s::COLOR_AMBER; - } else if (battery_warning == battery_status_s::BATTERY_WARNING_CRITICAL) { + } else if (battery_warning == battery_status_s::WARNING_CRITICAL) { led_color = led_control_s::COLOR_RED; } else { @@ -2828,16 +2821,6 @@ void Commander::dataLinkCheck() _vehicle_status.open_drone_id_system_present = true; _vehicle_status.open_drone_id_system_healthy = healthy; } - - if (telemetry.heartbeat_component_obstacle_avoidance) { - if (_avoidance_system_lost) { - _avoidance_system_lost = false; - _status_changed = true; - } - - _datalink_last_heartbeat_avoidance_system = telemetry.timestamp; - _vehicle_status.avoidance_system_valid = telemetry.avoidance_system_healthy; - } } } @@ -2889,17 +2872,6 @@ void Commander::dataLinkCheck() _open_drone_id_system_lost = true; _status_changed = true; } - - // AVOIDANCE SYSTEM state check (only if it is enabled) - if (_vehicle_status.avoidance_system_required && !_onboard_controller_lost) { - // if heartbeats stop - if (!_avoidance_system_lost && (_datalink_last_heartbeat_avoidance_system > 0) - && (hrt_elapsed_time(&_datalink_last_heartbeat_avoidance_system) > 5_s)) { - - _avoidance_system_lost = true; - _vehicle_status.avoidance_system_valid = false; - } - } } void Commander::battery_status_check() @@ -2907,7 +2879,7 @@ void Commander::battery_status_check() // Handle shutdown request from emergency battery action if (_battery_warning != _failsafe_flags.battery_warning) { - if (_failsafe_flags.battery_warning == battery_status_s::BATTERY_WARNING_EMERGENCY) { + if (_failsafe_flags.battery_warning == battery_status_s::WARNING_EMERGENCY) { #if defined(BOARD_HAS_POWER_CONTROL) if (!isArmed() && (px4_shutdown_request(60_s) == 0)) { diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index f4d1b5eaae..fbfdce886e 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -241,7 +241,6 @@ private: Hysteresis _auto_disarm_killed{false}; hrt_abstime _datalink_last_heartbeat_open_drone_id_system{0}; - hrt_abstime _datalink_last_heartbeat_avoidance_system{0}; hrt_abstime _datalink_last_heartbeat_gcs{0}; hrt_abstime _datalink_last_heartbeat_onboard_controller{0}; hrt_abstime _datalink_last_heartbeat_parachute_system{0}; @@ -263,12 +262,11 @@ private: hrt_abstime _last_health_and_arming_check{0}; - uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE}; + uint8_t _battery_warning{battery_status_s::WARNING_NONE}; bool _failsafe_user_override_request{false}; ///< override request due to stick movements bool _open_drone_id_system_lost{true}; - bool _avoidance_system_lost{false}; bool _onboard_controller_lost{false}; bool _parachute_system_lost{true}; @@ -343,7 +341,6 @@ private: (ParamBool) _param_com_force_safety, (ParamFloat) _param_com_kill_disarm, (ParamBool) _param_com_mot_test_en, - (ParamBool) _param_com_obs_avoid, (ParamFloat) _param_com_obc_loss_t, (ParamInt) _param_com_prearm_mode, (ParamInt) _param_com_rc_override, diff --git a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt index 7265e042b4..d62d577ca4 100644 --- a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt +++ b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt @@ -42,6 +42,7 @@ px4_add_library(health_and_arming_checks checks/batteryCheck.cpp checks/cpuResourceCheck.cpp checks/distanceSensorChecks.cpp + checks/opticalFlowCheck.cpp checks/escCheck.cpp checks/estimatorCheck.cpp checks/failureDetectorCheck.cpp diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp index d04b8d088e..d4f7136f1d 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp @@ -46,6 +46,7 @@ #include "checks/baroCheck.hpp" #include "checks/cpuResourceCheck.hpp" #include "checks/distanceSensorChecks.hpp" +#include "checks/opticalFlowCheck.hpp" #include "checks/escCheck.hpp" #include "checks/estimatorCheck.hpp" #include "checks/failureDetectorCheck.hpp" @@ -130,6 +131,7 @@ private: BaroChecks _baro_checks; CpuResourceChecks _cpu_resource_checks; DistanceSensorChecks _distance_sensor_checks; + OpticalFlowCheck _optical_flow_check; EscChecks _esc_checks; EstimatorChecks _estimator_checks; FailureDetectorChecks _failure_detector_checks; @@ -169,6 +171,7 @@ private: &_baro_checks, &_cpu_resource_checks, &_distance_sensor_checks, + &_optical_flow_check, &_esc_checks, &_estimator_checks, &_failure_detector_checks, diff --git a/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp index 85a19cf550..c20919ba56 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp @@ -39,7 +39,7 @@ using namespace time_literals; using battery_fault_reason_t = events::px4::enums::battery_fault_reason_t; -static_assert(battery_status_s::BATTERY_FAULT_COUNT == (static_cast(battery_fault_reason_t::_max) + 1) +static_assert(battery_status_s::FAULT_COUNT == (static_cast(battery_fault_reason_t::_max) + 1) , "Battery fault flags mismatch!"); static constexpr const char *battery_fault_reason_str(battery_fault_reason_t battery_fault_reason) @@ -78,7 +78,7 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) // Reset related failsafe flags otherwise failures from before disabling the check cause failsafes without reported reason reporter.failsafeFlags().battery_unhealthy = false; reporter.failsafeFlags().battery_low_remaining_time = false; - reporter.failsafeFlags().battery_warning = battery_status_s::BATTERY_WARNING_NONE; + reporter.failsafeFlags().battery_warning = battery_status_s::WARNING_NONE; return; } @@ -86,7 +86,7 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) // There are possibly multiple batteries, and we can't know which ones serve which purpose. So the safest // option is to check if ANY of them have a warning, and specifically find which one has the most // urgent warning. - uint8_t worst_warning = battery_status_s::BATTERY_WARNING_NONE; + uint8_t worst_warning = battery_status_s::WARNING_NONE; float worst_battery_remaining = 1.f; // To make sure that all connected batteries are being regularly reported, we check which one has the // oldest timestamp. @@ -132,7 +132,7 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) } // trigger a battery failsafe action if a battery disconnects in flight - worst_warning = battery_status_s::BATTERY_WARNING_CRITICAL; + worst_warning = battery_status_s::WARNING_CRITICAL; } if (battery.connected) { @@ -195,13 +195,13 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) reporter.failsafeFlags().battery_warning = worst_warning; } - const bool battery_warning = reporter.failsafeFlags().battery_warning > battery_status_s::BATTERY_WARNING_NONE - && reporter.failsafeFlags().battery_warning < battery_status_s::BATTERY_WARNING_FAILED; + const bool battery_warning = reporter.failsafeFlags().battery_warning > battery_status_s::WARNING_NONE + && reporter.failsafeFlags().battery_warning < battery_status_s::WARNING_FAILED; const bool configured_arm_threshold_in_use = !context.isArmed() && (_param_com_arm_bat_min.get() >= -FLT_EPSILON); const bool below_configured_arm_threshold = (worst_battery_remaining < _param_com_arm_bat_min.get()); if (battery_warning || (configured_arm_threshold_in_use && below_configured_arm_threshold)) { - const bool critical_or_higher = reporter.failsafeFlags().battery_warning >= battery_status_s::BATTERY_WARNING_CRITICAL; + const bool critical_or_higher = reporter.failsafeFlags().battery_warning >= battery_status_s::WARNING_CRITICAL; NavModes affected_modes = (!configured_arm_threshold_in_use && critical_or_higher) || (configured_arm_threshold_in_use && below_configured_arm_threshold) ? NavModes::All : NavModes::None; events::LogLevel log_level = critical_or_higher || below_configured_arm_threshold @@ -209,7 +209,7 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) switch (reporter.failsafeFlags().battery_warning) { default: - case battery_status_s::BATTERY_WARNING_LOW: + case battery_status_s::WARNING_LOW: /* EVENT * @description * The lowest battery state of charge is below the low threshold. @@ -227,7 +227,7 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) break; - case battery_status_s::BATTERY_WARNING_CRITICAL: + case battery_status_s::WARNING_CRITICAL: /* EVENT * @description * The lowest battery state of charge is below the critical threshold. @@ -245,7 +245,7 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) break; - case battery_status_s::BATTERY_WARNING_EMERGENCY: + case battery_status_s::WARNING_EMERGENCY: /* EVENT * @description * The lowest battery state of charge is below the emergency threshold. @@ -275,7 +275,7 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) || is_required_battery_missing // No currently-connected batteries have any fault || battery_has_fault - || reporter.failsafeFlags().battery_warning == battery_status_s::BATTERY_WARNING_FAILED; + || reporter.failsafeFlags().battery_warning == battery_status_s::WARNING_FAILED; if (reporter.failsafeFlags().battery_unhealthy && !is_required_battery_missing && !battery_has_fault) { // missing batteries and faults are reported above already diff --git a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp index bbba418000..2efa6896c3 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp @@ -40,9 +40,6 @@ #include #include -static_assert((1ull << arming_check_reply_s::HEALTH_COMPONENT_INDEX_AVOIDANCE) == (uint64_t) - health_component_t::avoidance, "enum definition missmatch"); - class ExternalChecks : public HealthAndArmingCheckBase { public: diff --git a/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.cpp index 6716a8e06a..ac35a61803 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.cpp @@ -47,9 +47,6 @@ void MagnetometerChecks::checkAndReport(const Context &context, Report &reporter int num_enabled_and_valid_calibration = 0; for (int instance = 0; instance < _sensor_mag_sub.size(); instance++) { - bool is_mag_fault = false; - const bool is_required = instance == 0 || isMagRequired(instance, is_mag_fault); - const bool exists = _sensor_mag_sub[instance].advertised(); bool is_valid = false; bool is_calibration_valid = false; @@ -80,7 +77,9 @@ void MagnetometerChecks::checkAndReport(const Context &context, Report &reporter } // Do not raise errors if a mag is not required - if (!is_required) { + bool is_mag_fault = false; + + if (!isMagRequired(instance, is_mag_fault)) { continue; } diff --git a/src/lib/bezier/BezierN.hpp b/src/modules/commander/HealthAndArmingChecks/checks/opticalFlowCheck.cpp similarity index 56% rename from src/lib/bezier/BezierN.hpp rename to src/modules/commander/HealthAndArmingChecks/checks/opticalFlowCheck.cpp index 7f635e94b5..c01c8cf565 100644 --- a/src/lib/bezier/BezierN.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/opticalFlowCheck.cpp @@ -1,8 +1,6 @@ - - /**************************************************************************** * - * Copyright (C) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2025 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,44 +31,38 @@ * ****************************************************************************/ -/** - * @file BezierN.hpp - * - * @author Julian Kent - * - * N-order Bezier library designed for time-aware trajectory tracking - */ +#include "opticalFlowCheck.hpp" -#pragma once -#include - -namespace bezier +void OpticalFlowCheck::checkAndReport(const Context &context, Report &reporter) { + if (!_param_sys_has_num_of.get()) { + return; + } -/* - * Calculates the location and velocity with respect to T on a given bezier curve of any order. - * - */ -bool calculateBezierPosVel(const matrix::Vector3f *positions, int N, float t, - matrix::Vector3f &position, matrix::Vector3f &velocity); + const bool exists = _vehicle_optical_flow_sub.advertised(); -/* - * Calculates the position, velocity and acceleration with respect to T on a given bezier curve of any order. - * - */ -bool calculateBezierPosVelAcc(const matrix::Vector3f *positions, int N, float t, - matrix::Vector3f &position, matrix::Vector3f &velocity, matrix::Vector3f &acceleration); + if (exists) { + vehicle_optical_flow_s flow_sens; + const bool valid = _vehicle_optical_flow_sub.copy(&flow_sens) && (hrt_elapsed_time(&flow_sens.timestamp) < 1_s); + reporter.setIsPresent(health_component_t::optical_flow); -/* - * Calculates the position and velocity of yaw with respect to t on a bezier curve. - * All yaw setpoints are wrapped relative to the starting yaw. - * - */ -bool calculateBezierYaw(const float *setpoints, int N, float t, float &yaw_setpoint, float &yaw_vel_setpoint); - -/* - * Calculates the fraction between the begin and end time which can be used for fast bezier curve lookups - */ -bool calculateT(int64_t start_time, int64_t end_time, int64_t now, float &T); + if (!valid) { + /* EVENT + */ + reporter.healthFailure(NavModes::All, health_component_t::optical_flow, + events::ID("check_optical_flow_sensor_invalid"), + events::Log::Error, "No valid data from optical flow sensor"); + } + } else { + /* EVENT + * @description + * + * This check can be configured via SYS_HAS_NUM_OF parameter. + * + */ + reporter.healthFailure(NavModes::All, health_component_t::optical_flow, + events::ID("check_optical_sensor_missing"), + events::Log::Error, "Optical flow sensor missing"); + } } diff --git a/src/drivers/voxl2_io/voxl2_io_serial.hpp b/src/modules/commander/HealthAndArmingChecks/checks/opticalFlowCheck.hpp similarity index 71% rename from src/drivers/voxl2_io/voxl2_io_serial.hpp rename to src/modules/commander/HealthAndArmingChecks/checks/opticalFlowCheck.hpp index 638bdef288..4eaa47f1f4 100644 --- a/src/drivers/voxl2_io/voxl2_io_serial.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/opticalFlowCheck.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2025 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,37 +33,23 @@ #pragma once -#include -#include -#include -#include +#include "../Common.hpp" -#ifdef __PX4_QURT -#include -#define FAR -#endif +#include +#include -class Voxl2IoSerial +class OpticalFlowCheck : public HealthAndArmingCheckBase { public: - Voxl2IoSerial(); - virtual ~Voxl2IoSerial(); + OpticalFlowCheck() = default; + ~OpticalFlowCheck() = default; - int uart_open(const char *dev, speed_t speed); - int uart_set_baud(speed_t speed); - int uart_close(); - int uart_write(FAR void *buf, size_t len); - int uart_read(FAR void *buf, size_t len); - bool is_open() { return _uart_fd >= 0; }; - int uart_get_baud() {return _speed; } + void checkAndReport(const Context &context, Report &reporter) override; private: - int _uart_fd = -1; + uORB::Subscription _vehicle_optical_flow_sub{ORB_ID::vehicle_optical_flow}; -#if ! defined(__PX4_QURT) - struct termios _orig_cfg; - struct termios _cfg; -#endif - - int _speed = -1; + DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, + (ParamInt) _param_sys_has_num_of + ) }; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.cpp index 69afd1c12c..a27d17f1be 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.cpp @@ -36,6 +36,14 @@ using namespace time_literals; +PowerChecks::PowerChecks() +{ + _voltage_low_hysteresis.set_hysteresis_time_from(false, 0_s); + _voltage_low_hysteresis.set_hysteresis_time_from(true, 15_s); + _voltage_high_hysteresis.set_hysteresis_time_from(false, 0_s); + _voltage_high_hysteresis.set_hysteresis_time_from(true, 15_s); +} + void PowerChecks::checkAndReport(const Context &context, Report &reporter) { if (circuit_breaker_enabled_by_val(_param_cbrk_supply_chk.get(), CBRK_SUPPLY_CHK_KEY)) { @@ -77,7 +85,11 @@ void PowerChecks::checkAndReport(const Context &context, Report &reporter) const float low_error_threshold = 4.7f; const float high_error_threshold = 5.4f; - if (avionics_power_rail_voltage < low_error_threshold) { + const auto now = hrt_absolute_time(); + _voltage_low_hysteresis.set_state_and_update(avionics_power_rail_voltage < low_error_threshold, now); + _voltage_high_hysteresis.set_state_and_update(avionics_power_rail_voltage > high_error_threshold, now); + + if (_voltage_low_hysteresis.get_state()) { /* EVENT * @description @@ -96,7 +108,7 @@ void PowerChecks::checkAndReport(const Context &context, Report &reporter) (double)avionics_power_rail_voltage); } - } else if (avionics_power_rail_voltage > high_error_threshold) { + } else if (_voltage_high_hysteresis.get_state()) { /* EVENT * @description * Check the voltage supply to the FMU, it must be below {2:.2} Volt. diff --git a/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.hpp index b81fadaee0..76d8b9aae0 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.hpp @@ -35,13 +35,14 @@ #include "../Common.hpp" +#include #include #include class PowerChecks : public HealthAndArmingCheckBase { public: - PowerChecks() = default; + PowerChecks(); ~PowerChecks() = default; void checkAndReport(const Context &context, Report &reporter) override; @@ -49,6 +50,8 @@ public: private: uORB::Subscription _system_power_sub{ORB_ID(system_power)}; bool _overcurrent_warning_sent{false}; + systemlib::Hysteresis _voltage_low_hysteresis{false}; + systemlib::Hysteresis _voltage_high_hysteresis{false}; DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, (ParamInt) _param_cbrk_supply_chk, diff --git a/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp index 32beaba9af..1f2592c9ed 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp @@ -121,27 +121,6 @@ void SystemChecks::checkAndReport(const Context &context, Report &reporter) } } - // avoidance system - if (context.status().avoidance_system_required) { - if (context.status().avoidance_system_valid) { - reporter.setIsPresent(health_component_t::avoidance); - - } else { - /* EVENT - * @description - * - * This check can be configured via COM_OBS_AVOID parameter. - * - */ - reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_system_avoidance_not_ready"), - events::Log::Error, "Avoidance system not ready"); - - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Avoidance system not ready"); - } - } - } - // VTOL in transition if (context.status().is_vtol && !context.isArmed()) { if (context.status().in_transition_mode) { diff --git a/src/modules/commander/UserModeIntention.cpp b/src/modules/commander/UserModeIntention.cpp index 02b4a7f3f7..3a766640ec 100644 --- a/src/modules/commander/UserModeIntention.cpp +++ b/src/modules/commander/UserModeIntention.cpp @@ -45,7 +45,6 @@ bool UserModeIntention::change(uint8_t user_intended_nav_state, ModeChangeSource bool force) { _ever_had_mode_change = true; - _had_mode_change = true; if (_handler) { // If a replacement mode is selected, select the internal one instead. The replacement will be selected after. diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 37dcbdc9a9..44c2f07987 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -640,14 +640,6 @@ PARAM_DEFINE_INT32(COM_RCL_EXCEPT, 0); */ PARAM_DEFINE_INT32(COM_ACT_FAIL_ACT, 0); -/** - * Flag to enable obstacle avoidance. - * - * @boolean - * @group Commander - */ -PARAM_DEFINE_INT32(COM_OBS_AVOID, 0); - /** * Expect and require a healthy MAVLink parachute system * diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index 3ba1131d97..6e6aee83b2 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -192,16 +192,16 @@ FailsafeBase::ActionOptions Failsafe::fromBatteryWarningActParam(int param_value switch (battery_warning) { default: - case battery_status_s::BATTERY_WARNING_NONE: + case battery_status_s::WARNING_NONE: options.action = Action::None; break; - case battery_status_s::BATTERY_WARNING_LOW: + case battery_status_s::WARNING_LOW: options.action = Action::Warn; options.cause = Cause::BatteryLow; break; - case battery_status_s::BATTERY_WARNING_CRITICAL: + case battery_status_s::WARNING_CRITICAL: options.action = Action::Warn; options.cause = Cause::BatteryCritical; @@ -222,7 +222,7 @@ FailsafeBase::ActionOptions Failsafe::fromBatteryWarningActParam(int param_value break; - case battery_status_s::BATTERY_WARNING_EMERGENCY: + case battery_status_s::WARNING_EMERGENCY: options.action = Action::Warn; options.cause = Cause::BatteryEmergency; @@ -550,21 +550,21 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, _param_com_low_bat_act.get() : (int32_t)LowBatteryAction::Warning; switch (status_flags.battery_warning) { - case battery_status_s::BATTERY_WARNING_LOW: + case battery_status_s::WARNING_LOW: _last_state_battery_warning_low = checkFailsafe(_caller_id_battery_warning_low, _last_state_battery_warning_low, - true, fromBatteryWarningActParam(low_battery_action, battery_status_s::BATTERY_WARNING_LOW)); + true, fromBatteryWarningActParam(low_battery_action, battery_status_s::WARNING_LOW)); break; - case battery_status_s::BATTERY_WARNING_CRITICAL: + case battery_status_s::WARNING_CRITICAL: _last_state_battery_warning_critical = checkFailsafe(_caller_id_battery_warning_critical, _last_state_battery_warning_critical, - true, fromBatteryWarningActParam(low_battery_action, battery_status_s::BATTERY_WARNING_CRITICAL)); + true, fromBatteryWarningActParam(low_battery_action, battery_status_s::WARNING_CRITICAL)); break; - case battery_status_s::BATTERY_WARNING_EMERGENCY: + case battery_status_s::WARNING_EMERGENCY: _last_state_battery_warning_emergency = checkFailsafe(_caller_id_battery_warning_emergency, _last_state_battery_warning_emergency, - true, fromBatteryWarningActParam(low_battery_action, battery_status_s::BATTERY_WARNING_EMERGENCY)); + true, fromBatteryWarningActParam(low_battery_action, battery_status_s::WARNING_EMERGENCY)); break; default: diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index a72f0fcadb..7aeadeab55 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -903,6 +903,15 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma bool param_save = false; bool failed = true; + bool external_mag_available = false; + + for (unsigned cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { + if (worker_data.calibration[cur_mag].external() && worker_data.calibration[cur_mag].enabled()) { + external_mag_available = true; + break; + } + } + for (unsigned cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { calibration::Magnetometer ¤t_cal = worker_data.calibration[cur_mag]; @@ -921,6 +930,11 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma current_cal.set_offdiagonal(offdiag[cur_mag]); } + if (external_mag_available && !current_cal.external()) { + // automatically disable the internal mags as they should not be used for navigation + current_cal.disable(); + } + current_cal.PrintStatus(); if (current_cal.ParametersSave(cur_mag, true)) { diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp index 23dee70563..46fea5f33e 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp @@ -69,15 +69,11 @@ void ActuatorEffectivenessControlSurfaces::updateParams() { ModuleParams::updateParams(); - int32_t count = 0; - - if (param_get(_count_handle, &count) != 0) { + if (param_get(_count_handle, &_count) != PX4_OK) { PX4_ERR("param_get failed"); return; } - _count = count; - for (int i = 0; i < _count; i++) { param_get(_param_handles[i].type, (int32_t *)&_params[i].type); diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp index 7047363cc8..5e64e5a738 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp @@ -107,7 +107,7 @@ private: param_t _count_handle; Params _params[MAX_COUNT] {}; - int _count{0}; + int32_t _count{0}; SlewRate _flaps_setpoint_with_slewrate; SlewRate _spoilers_setpoint_with_slewrate; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp index 643b218c47..967706a6d8 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp @@ -65,6 +65,7 @@ ActuatorEffectivenessHelicopter::ActuatorEffectivenessHelicopter(ModuleParams *p _param_handles.yaw_throttle_scale = param_find("CA_HELI_YAW_TH_S"); _param_handles.yaw_ccw = param_find("CA_HELI_YAW_CCW"); _param_handles.spoolup_time = param_find("COM_SPOOLUP_TIME"); + _param_handles.max_servo_throw = param_find("CA_MAX_SVO_THROW"); updateParams(); } @@ -73,14 +74,13 @@ void ActuatorEffectivenessHelicopter::updateParams() { ModuleParams::updateParams(); - int32_t count = 0; - - if (param_get(_param_handles.num_swash_plate_servos, &count) != 0) { + if (param_get(_param_handles.num_swash_plate_servos, &_geometry.num_swash_plate_servos) != PX4_OK) { PX4_ERR("param_get failed"); return; } - _geometry.num_swash_plate_servos = math::constrain((int)count, 3, NUM_SWASH_PLATE_SERVOS_MAX); + _geometry.num_swash_plate_servos = math::constrain(_geometry.num_swash_plate_servos, + (int32_t)3, (int32_t)NUM_SWASH_PLATE_SERVOS_MAX); for (int i = 0; i < _geometry.num_swash_plate_servos; ++i) { float angle_deg{}; @@ -102,6 +102,21 @@ void ActuatorEffectivenessHelicopter::updateParams() int32_t yaw_ccw = 0; param_get(_param_handles.yaw_ccw, &yaw_ccw); _geometry.yaw_sign = (yaw_ccw == 1) ? -1.f : 1.f; + float max_servo_throw_deg = 0.f; + param_get(_param_handles.max_servo_throw, &max_servo_throw_deg); + + if (max_servo_throw_deg > 0.f) { + // linearization feature enabled + _geometry.linearize_servos = 1; + const float max_servo_throw = math::radians(max_servo_throw_deg); + _geometry.max_servo_height = sinf(max_servo_throw); + _geometry.inverse_max_servo_throw = 1.f / max_servo_throw; + + } else { + // handle any undefined behaviour if disabled + _geometry.linearize_servos = 0; + _geometry.max_servo_height = _geometry.inverse_max_servo_throw = 0.f; + } } bool ActuatorEffectivenessHelicopter::getEffectivenessMatrix(Configuration &configuration, @@ -169,6 +184,11 @@ void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector", "Storage file", true); +#endif PRINT_MODULE_USAGE_PARAM_FLAG('r', "Use RAM backend (NOT persistent)", true); +#ifdef CONFIG_DATAMAN_PERSISTENT_STORAGE PRINT_MODULE_USAGE_PARAM_COMMENT("The options -f and -r are mutually exclusive. If nothing is specified, a file 'dataman' is used"); +#endif PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } @@ -930,9 +956,14 @@ dataman_main(int argc, char *argv[]) return -1; } +#ifdef CONFIG_DATAMAN_PERSISTENT_STORAGE backend = BACKEND_FILE; k_data_manager_device_path = strdup(dmoptarg); PX4_INFO("dataman file set to: %s", k_data_manager_device_path); +#else + backend = BACKEND_RAM; + PX4_WARN("dataman does not support persistent storage. Falling back to RAM."); +#endif break; case 'r': @@ -951,16 +982,22 @@ dataman_main(int argc, char *argv[]) } if (backend == BACKEND_NONE) { +#ifdef CONFIG_DATAMAN_PERSISTENT_STORAGE backend = BACKEND_FILE; k_data_manager_device_path = strdup(default_device_path); +#else + backend = BACKEND_RAM; +#endif } start(); if (!is_running()) { PX4_ERR("dataman start failed"); +#ifdef CONFIG_DATAMAN_PERSISTENT_STORAGE free(k_data_manager_device_path); k_data_manager_device_path = nullptr; +#endif return -1; } @@ -976,8 +1013,10 @@ dataman_main(int argc, char *argv[]) if (!strcmp(argv[1], "stop")) { stop(); +#ifdef CONFIG_DATAMAN_PERSISTENT_STORAGE free(k_data_manager_device_path); k_data_manager_device_path = nullptr; +#endif } else if (!strcmp(argv[1], "status")) { status(); diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 112b920ba2..181978792c 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -38,6 +38,7 @@ execute_process( COMMAND ${PYTHON_EXECUTABLE} -m symforce.symbolic RESULT_VARIABLE PYTHON_SYMFORCE_EXIT_CODE OUTPUT_QUIET + ERROR_QUIET ) # for now only provide symforce target helper if derivation.py generation isn't default diff --git a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp index 3307dd56b9..675bffdd3c 100644 --- a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp +++ b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp @@ -104,9 +104,20 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed _state = State::starting; if (ekf.global_origin_valid()) { - ekf.enableControlStatusAuxGpos(); - _reset_counters.lat_lon = sample.lat_lon_reset_counter; - _state = State::active; + const bool fused = ekf.fuseHorizontalPosition(aid_src); + bool reset = false; + + if (!fused && !ekf.isOtherSourceOfHorizontalPositionAidingThan(ekf.control_status_flags().aux_gpos)) { + ekf.resetGlobalPositionTo(sample.latitude, sample.longitude, sample.altitude_amsl, pos_var, sq(sample.epv)); + ekf.resetAidSourceStatusZeroInnovation(aid_src); + reset = true; + } + + if (fused || reset) { + ekf.enableControlStatusAuxGpos(); + _reset_counters.lat_lon = sample.lat_lon_reset_counter; + _state = State::active; + } } else { // Try to initialize using measurement @@ -128,12 +139,18 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed if (isTimedOut(aid_src.time_last_fuse, imu_delayed.time_us, ekf._params.no_aid_timeout_max) || (_reset_counters.lat_lon != sample.lat_lon_reset_counter)) { + if (ekf.isOnlyActiveSourceOfHorizontalPositionAiding(ekf.control_status_flags().aux_gpos)) { - ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance)); + ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance)); - ekf.resetAidSourceStatusZeroInnovation(aid_src); + ekf.resetAidSourceStatusZeroInnovation(aid_src); - _reset_counters.lat_lon = sample.lat_lon_reset_counter; + _reset_counters.lat_lon = sample.lat_lon_reset_counter; + + } else { + ekf.disableControlStatusAuxGpos(); + _state = State::stopped; + } } } else { diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 84ced94c80..22ccf958f0 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -600,12 +600,32 @@ bool EstimatorInterface::isOtherSourceOfHorizontalAidingThan(const bool aiding_f } int EstimatorInterface::getNumberOfActiveHorizontalAidingSources() const +{ + return getNumberOfActiveHorizontalPositionAidingSources() + getNumberOfActiveHorizontalVelocityAidingSources(); +} + +bool EstimatorInterface::isOnlyActiveSourceOfHorizontalPositionAiding(const bool aiding_flag) const +{ + return aiding_flag && !isOtherSourceOfHorizontalPositionAidingThan(aiding_flag); +} + +bool EstimatorInterface::isOtherSourceOfHorizontalPositionAidingThan(const bool aiding_flag) const +{ + const int nb_sources = getNumberOfActiveHorizontalPositionAidingSources(); + return aiding_flag ? nb_sources > 1 : nb_sources > 0; +} + +int EstimatorInterface::getNumberOfActiveHorizontalPositionAidingSources() const { return int(_control_status.flags.gps) - + int(_control_status.flags.opt_flow) + int(_control_status.flags.ev_pos) + + int(_control_status.flags.aux_gpos); +} + +int EstimatorInterface::getNumberOfActiveHorizontalVelocityAidingSources() const +{ + return int(_control_status.flags.opt_flow) + int(_control_status.flags.ev_vel) - + int(_control_status.flags.aux_gpos) // Combined airspeed and sideslip fusion allows sustained wind relative dead reckoning // and so is treated as a single aiding source. + int(_control_status.flags.fuse_aspd && _control_status.flags.fuse_beta); diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 1212300278..93632bc109 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -207,8 +207,8 @@ public: // set air density used by the multi-rotor specific drag force fusion void set_air_density(float air_density) { _air_density = air_density; } - // the flags considered are opt_flow, gps, ev_vel and ev_pos bool isOnlyActiveSourceOfHorizontalAiding(bool aiding_flag) const; + bool isOnlyActiveSourceOfHorizontalPositionAiding(bool aiding_flag) const; /* * Check if there are any other active source of horizontal aiding @@ -221,6 +221,7 @@ public: * @return true if an other source than aiding_flag is active */ bool isOtherSourceOfHorizontalAidingThan(bool aiding_flag) const; + bool isOtherSourceOfHorizontalPositionAidingThan(bool aiding_flag) const; // Return true if at least one source of horizontal aiding is active // the flags considered are opt_flow, gps, ev_vel and ev_pos @@ -228,6 +229,8 @@ public: bool isVerticalAidingActive() const; int getNumberOfActiveHorizontalAidingSources() const; + int getNumberOfActiveHorizontalPositionAidingSources() const; + int getNumberOfActiveHorizontalVelocityAidingSources() const; bool isOtherSourceOfVerticalPositionAidingThan(bool aiding_flag) const; bool isVerticalPositionAidingActive() const; diff --git a/src/modules/esc_battery/EscBattery.cpp b/src/modules/esc_battery/EscBattery.cpp index 0fb4a3efad..78d1c2c71f 100644 --- a/src/modules/esc_battery/EscBattery.cpp +++ b/src/modules/esc_battery/EscBattery.cpp @@ -40,7 +40,7 @@ using namespace time_literals; EscBattery::EscBattery() : ModuleParams(nullptr), WorkItem(MODULE_NAME, px4::wq_configurations::lp_default), - _battery(1, this, ESC_BATTERY_INTERVAL_US, battery_status_s::BATTERY_SOURCE_ESCS) + _battery(1, this, ESC_BATTERY_INTERVAL_US, battery_status_s::SOURCE_ESCS) { } diff --git a/src/modules/events/set_leds.cpp b/src/modules/events/set_leds.cpp index 20541f6fbb..88c2ed43d0 100644 --- a/src/modules/events/set_leds.cpp +++ b/src/modules/events/set_leds.cpp @@ -101,12 +101,12 @@ void StatusDisplay::set_leds() } // handle battery warnings, once a state is reached it can not be reset - if (_battery_status_sub.get().warning == battery_status_s::BATTERY_WARNING_CRITICAL || _critical_battery) { + if (_battery_status_sub.get().warning == battery_status_s::WARNING_CRITICAL || _critical_battery) { _led_control.color = led_control_s::COLOR_RED; _led_control.mode = led_control_s::MODE_BLINK_FAST; _critical_battery = true; - } else if (_battery_status_sub.get().warning == battery_status_s::BATTERY_WARNING_LOW || _low_battery) { + } else if (_battery_status_sub.get().warning == battery_status_s::WARNING_LOW || _low_battery) { _led_control.color = led_control_s::COLOR_RED; _led_control.mode = led_control_s::MODE_FLASH; _low_battery = true; diff --git a/src/modules/flight_mode_manager/tasks/Auto/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/Auto/CMakeLists.txt index 2fadf2754d..905e08e80b 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/CMakeLists.txt +++ b/src/modules/flight_mode_manager/tasks/Auto/CMakeLists.txt @@ -35,5 +35,5 @@ px4_add_library(FlightTaskAuto FlightTaskAuto.cpp ) -target_link_libraries(FlightTaskAuto PUBLIC avoidance FlightTask FlightTaskUtility WeatherVane) +target_link_libraries(FlightTaskAuto PUBLIC FlightTask FlightTaskUtility WeatherVane) target_include_directories(FlightTaskAuto PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 8e19ffd5a3..5cb75222a0 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -156,17 +156,11 @@ bool FlightTaskAuto::update() break; } - if (_param_com_obs_avoid.get()) { - _obstacle_avoidance.updateAvoidanceDesiredSetpoints(_position_setpoint, _velocity_setpoint, (int)_type); - _obstacle_avoidance.injectAvoidanceSetpoints(_position_setpoint, _velocity_setpoint, _yaw_setpoint, - _yawspeed_setpoint); - } - _checkEmergencyBraking(); Vector3f waypoints[] = {_prev_wp, _position_setpoint, _next_wp}; if (isTargetModified()) { - // In case object avoidance has injected a new setpoint, we take this as the next waypoints + // In case the target has been modified, we take this as the next waypoints waypoints[2] = _position_setpoint; } @@ -259,9 +253,15 @@ void FlightTaskAuto::_prepareLandSetpoints() // Stick full up -1 -> stop, stick full down 1 -> double the speed vertical_speed *= (1 - _sticks.getThrottleZeroCenteredExpo()); + Vector2f sticks_xy = _sticks.getPitchRollExpo(); + + if (sticks_xy.longerThan(FLT_EPSILON)) { + // Ensure no unintended yawing when nudging horizontally during initial heading alignment + _land_heading = _yaw_sp_prev; + } + rcHelpModifyYaw(_land_heading); - Vector2f sticks_xy = _sticks.getPitchRollExpo(); Vector2f sticks_ne = sticks_xy; Sticks::rotateIntoHeadingFrameXY(sticks_ne, _yaw, _land_heading); @@ -477,17 +477,6 @@ bool FlightTaskAuto::_evaluateTriplets() _updateInternalWaypoints(); } - if (_param_com_obs_avoid.get() - && _sub_vehicle_status.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - _obstacle_avoidance.updateAvoidanceDesiredWaypoints(_triplet_target, _yaw_setpoint, _yawspeed_setpoint, - _triplet_next_wp, - _sub_triplet_setpoint.get().next.yaw, - (float)NAN, - _weathervane.isActive(), _sub_triplet_setpoint.get().current.type); - _obstacle_avoidance.checkAvoidanceProgress( - _position, _triplet_prev_wp, _target_acceptance_radius, Vector2f(_closest_pt)); - } - // set heading _weathervane.update(); diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp index 90a98cac23..6c49dd8726 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp @@ -53,13 +53,6 @@ #include "StickAccelerationXY.hpp" #include "StickYaw.hpp" -// TODO: make this switchable in the board config, like a module -#if CONSTRAINED_FLASH -#include -#else -#include -#endif - /** * This enum has to agree with position_setpoint_s type definition * The only reason for not using the struct position_setpoint is because @@ -147,8 +140,6 @@ protected: AlphaFilter _yawspeed_filter; bool _yaw_sp_aligned{false}; - ObstacleAvoidance _obstacle_avoidance{this}; /**< class adjusting setpoints according to external avoidance module's input */ - PositionSmoothing _position_smoothing; Vector3f _unsmoothed_velocity_setpoint; Sticks _sticks{this}; @@ -165,7 +156,6 @@ protected: (ParamFloat) _param_nav_mc_alt_rad, //vertical acceptance radius at which waypoints are updated (ParamInt) _param_mpc_yaw_mode, // defines how heading is executed, - (ParamInt) _param_com_obs_avoid, // obstacle avoidance active (ParamFloat) _param_mpc_yawrauto_max, (ParamFloat) _param_mis_yaw_err, // yaw-error threshold (ParamFloat) _param_mpc_acc_hor, // acceleration in flight diff --git a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp index 7b1c24f1dd..dd2ca1da59 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp @@ -39,6 +39,7 @@ #include using namespace time_literals; +using namespace matrix; bool FlightTaskManualAccelerationSlow::update() { @@ -128,7 +129,29 @@ bool FlightTaskManualAccelerationSlow::update() FlightTaskManualAltitude::_velocity_constraint_down = velocity_down; FlightTaskManualAcceleration::_stick_yaw.setYawspeedConstraint(yaw_rate); - return FlightTaskManualAcceleration::update(); + bool ret = FlightTaskManualAcceleration::update(); + + // Optimize input-to-video latency gimbal control + if (_gimbal.checkForTelemetry(_time_stamp_current) && haveTakenOff()) { + _gimbal.acquireGimbalControlIfNeeded(); + + // the exact same _yawspeed_setpoint is setpoint for the gimbal and vehicle feed-forward + const float pitchrate_setpoint = getInputFromSanitizedAuxParameterIndex(_param_mc_slow_map_pitch.get()) * yaw_rate; + _yawspeed_setpoint = _sticks.getYaw() * yaw_rate; + + _gimbal.publishGimbalManagerSetAttitude(Gimbal::FLAGS_ALL_AXES_LOCKED, Quatf(NAN, NAN, NAN, NAN), + Vector3f(NAN, pitchrate_setpoint, _yawspeed_setpoint)); + + if (_gimbal.allAxesLockedConfirmed()) { + // but the vehicle makes sure it stays alligned with the gimbal absolute yaw + _yaw_setpoint = _gimbal.getTelemetryYaw(); + } + + } else { + _gimbal.releaseGimbalControlIfNeeded(); + } + + return ret; } float FlightTaskManualAccelerationSlow::getInputFromSanitizedAuxParameterIndex(int parameter_value) @@ -136,3 +159,11 @@ float FlightTaskManualAccelerationSlow::getInputFromSanitizedAuxParameterIndex(i const int sanitized_index = math::constrain(parameter_value - 1, 0, 5); return _sticks.getAux()(sanitized_index); } + +bool FlightTaskManualAccelerationSlow::haveTakenOff() +{ + takeoff_status_s takeoff_status{}; + _takeoff_status_sub.copy(&takeoff_status); + + return takeoff_status.takeoff_state == takeoff_status_s::TAKEOFF_STATE_FLIGHT; +} diff --git a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.hpp b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.hpp index b54d9b765d..66b2de4a8f 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.hpp @@ -42,6 +42,7 @@ #include "FlightTaskManualAcceleration.hpp" #include "StickAccelerationXY.hpp" +#include "Gimbal.hpp" #include #include @@ -67,9 +68,15 @@ private: bool _velocity_limits_received_before{false}; + // Gimbal control + Gimbal _gimbal{this}; + uORB::Subscription _velocity_limits_sub{ORB_ID(velocity_limits)}; velocity_limits_s _velocity_limits{}; + uORB::Subscription _takeoff_status_sub{ORB_ID(takeoff_status)}; + bool haveTakenOff(); + DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualAcceleration, (ParamInt) _param_mc_slow_map_hvel, (ParamInt) _param_mc_slow_map_vvel, @@ -83,6 +90,7 @@ private: (ParamFloat) _param_mpc_vel_manual, (ParamFloat) _param_mpc_z_vel_max_up, (ParamFloat) _param_mpc_z_vel_max_dn, - (ParamFloat) _param_mpc_man_y_max + (ParamFloat) _param_mpc_man_y_max, + (ParamInt) _param_mc_slow_map_pitch ) }; diff --git a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/flight_task_acceleration_slow_params.c b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/flight_task_acceleration_slow_params.c index 4c643d87d8..32558d98ea 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/flight_task_acceleration_slow_params.c +++ b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/flight_task_acceleration_slow_params.c @@ -156,3 +156,17 @@ PARAM_DEFINE_FLOAT(MC_SLOW_DEF_VVEL, 1.f); * @group Multicopter Position Slow Mode */ PARAM_DEFINE_FLOAT(MC_SLOW_DEF_YAWR, 45.f); + +/** + * RC_MAP_AUX{N} to allow for gimbal pitch rate control in position slow mode + * + * @value 0 No pitch rate input + * @value 1 AUX1 + * @value 2 AUX2 + * @value 3 AUX3 + * @value 4 AUX4 + * @value 5 AUX5 + * @value 6 AUX6 + * @group Multicopter Position Slow Mode + */ +PARAM_DEFINE_INT32(MC_SLOW_MAP_PTCH, 0); diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 7ea9289cb6..ed7676296f 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -129,6 +129,7 @@ void FlightTaskManualAltitude::_updateAltitudeLock() } else { _position_setpoint(2) = _position(2); + _dist_to_ground_lock = NAN; } } diff --git a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp index fb7f5dd4e5..b1f05092d3 100644 --- a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp @@ -68,5 +68,5 @@ private: uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */ WeatherVane _weathervane{this}; /**< weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */ - CollisionPrevention _collision_prevention{this}; /**< collision avoidance setpoint amendment */ + CollisionPrevention _collision_prevention{this}; /**< collision prevention setpoint amendment */ }; diff --git a/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt index 911da11d26..44a5e1dba1 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt +++ b/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt @@ -36,9 +36,10 @@ px4_add_library(FlightTaskUtility StickAccelerationXY.cpp StickTiltXY.cpp StickYaw.cpp + Gimbal.cpp ) -target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis bezier SlewRate motion_planning mathlib) +target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis SlewRate motion_planning mathlib) target_include_directories(FlightTaskUtility PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) px4_add_functional_gtest(SRC StickTiltXYTest.cpp LINKLIBS FlightTaskUtility) diff --git a/src/modules/flight_mode_manager/tasks/Utility/Gimbal.cpp b/src/modules/flight_mode_manager/tasks/Utility/Gimbal.cpp new file mode 100644 index 0000000000..21e97f5fc3 --- /dev/null +++ b/src/modules/flight_mode_manager/tasks/Utility/Gimbal.cpp @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 "Gimbal.hpp" +#include + +using namespace time_literals; +using namespace matrix; + +Gimbal::Gimbal(ModuleParams *parent) : + ModuleParams(parent) +{} + +Gimbal::~Gimbal() +{ + releaseGimbalControlIfNeeded(); +} + +bool Gimbal::checkForTelemetry(const hrt_abstime now) +{ + if (_gimbal_device_attitude_status_sub.updated()) { + gimbal_device_attitude_status_s gimbal_device_attitude_status{}; + + if (_gimbal_device_attitude_status_sub.copy(&gimbal_device_attitude_status)) { + _telemtry_timestamp = gimbal_device_attitude_status.timestamp; + _telemetry_flags = gimbal_device_attitude_status.device_flags; + _telemetry_yaw = Eulerf(Quatf(gimbal_device_attitude_status.q)).psi(); + } + } + + return now < _telemtry_timestamp + 2_s; +} + +void Gimbal::acquireGimbalControlIfNeeded() +{ + if (!_have_gimbal_control) { + _have_gimbal_control = true; + + vehicle_command_s vehicle_command{}; + vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE; + vehicle_command.param1 = _param_mav_sys_id.get(); + vehicle_command.param2 = _param_mav_comp_id.get(); + vehicle_command.param3 = -1.0f; // Leave unchanged. + vehicle_command.param4 = -1.0f; // Leave unchanged. + vehicle_command.timestamp = hrt_absolute_time(); + vehicle_command.source_system = _param_mav_sys_id.get(); + vehicle_command.source_component = _param_mav_comp_id.get(); + vehicle_command.target_system = _param_mav_sys_id.get(); + vehicle_command.target_component = _param_mav_sys_id.get(); + vehicle_command.from_external = false; + _vehicle_command_pub.publish(vehicle_command); + } +} + +void Gimbal::releaseGimbalControlIfNeeded() +{ + if (_have_gimbal_control) { + _have_gimbal_control = false; + + // Restore default flags, setting rate setpoints to NAN lead to unexpected behavior + publishGimbalManagerSetAttitude(FLAGS_ROLL_PITCH_LOCKED, + Quatf(NAN, NAN, NAN, NAN), + Vector3f(NAN, 0.f, 0.f)); + + // Release gimbal + vehicle_command_s vehicle_command{}; + vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE; + vehicle_command.param1 = -3.0f; // Remove control if it had it. + vehicle_command.param2 = -3.0f; // Remove control if it had it. + vehicle_command.param3 = -1.0f; // Leave unchanged. + vehicle_command.param4 = -1.0f; // Leave unchanged. + vehicle_command.timestamp = hrt_absolute_time(); + vehicle_command.from_external = false; + vehicle_command.source_system = _param_mav_sys_id.get(); + vehicle_command.source_component = _param_mav_comp_id.get(); + vehicle_command.target_system = _param_mav_sys_id.get(); + vehicle_command.target_component = _param_mav_comp_id.get(); + _vehicle_command_pub.publish(vehicle_command); + } +} + +void Gimbal::publishGimbalManagerSetAttitude(const uint16_t gimbal_flags, + const matrix::Quatf &q_gimbal_setpoint, + const matrix::Vector3f &gimbal_rates) +{ + gimbal_manager_set_attitude_s gimbal_setpoint{}; + gimbal_setpoint.origin_sysid = _param_mav_sys_id.get(); + gimbal_setpoint.origin_compid = _param_mav_comp_id.get(); + gimbal_setpoint.flags = gimbal_flags; + q_gimbal_setpoint.copyTo(gimbal_setpoint.q); + gimbal_setpoint.angular_velocity_x = gimbal_rates(0); + gimbal_setpoint.angular_velocity_y = gimbal_rates(1); + gimbal_setpoint.angular_velocity_z = gimbal_rates(2); + gimbal_setpoint.timestamp = hrt_absolute_time(); + _gimbal_manager_set_attitude_pub.publish(gimbal_setpoint); +} diff --git a/src/modules/flight_mode_manager/tasks/Utility/Gimbal.hpp b/src/modules/flight_mode_manager/tasks/Utility/Gimbal.hpp new file mode 100644 index 0000000000..938ef32aa2 --- /dev/null +++ b/src/modules/flight_mode_manager/tasks/Utility/Gimbal.hpp @@ -0,0 +1,90 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 Gimbal.hpp + * @brief Gimbal interface with flight tasks + * @author Pernilla Wikström + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "Sticks.hpp" + + +class Gimbal : public ModuleParams +{ +public: + Gimbal(ModuleParams *parent); + ~Gimbal(); + + bool checkForTelemetry(const hrt_abstime now); + void publishGimbalManagerSetAttitude(const uint16_t gimbal_flags, + const matrix::Quatf &q_gimbal_setpoint, + const matrix::Vector3f &gimbal_rates); + void acquireGimbalControlIfNeeded(); + void releaseGimbalControlIfNeeded(); + float getTelemetryYaw() { return _telemetry_yaw; } + bool allAxesLockedConfirmed() { return _telemetry_flags == FLAGS_ALL_AXES_LOCKED; } + + static constexpr uint16_t FLAGS_ALL_AXES_LOCKED = gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_ROLL_LOCK + | gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_PITCH_LOCK + | gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_YAW_LOCK; + static constexpr uint16_t FLAGS_ROLL_PITCH_LOCKED = + gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_ROLL_LOCK + | gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_PITCH_LOCK; + +private: + bool _have_gimbal_control{false}; + + uORB::Subscription _gimbal_device_attitude_status_sub{ORB_ID(gimbal_device_attitude_status)}; + hrt_abstime _telemtry_timestamp{}; + uint16_t _telemetry_flags{}; + float _telemetry_yaw{}; + + uORB::Publication _gimbal_manager_set_attitude_pub{ORB_ID(gimbal_manager_set_attitude)}; + uORB::Publication _vehicle_command_pub{ORB_ID(vehicle_command)}; + + DEFINE_PARAMETERS( + (ParamInt) _param_mav_sys_id, + (ParamInt) _param_mav_comp_id + ) +}; diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 8baddd55e6..e939a56f54 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -487,8 +487,6 @@ FixedwingPositionControl::status_publish() pos_ctrl_status.wp_dist = get_distance_to_next_waypoint(_current_latitude, _current_longitude, _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon); - pos_ctrl_status.yaw_acceptance = NAN; - pos_ctrl_status.timestamp = hrt_absolute_time(); pos_ctrl_status.type = _position_sp_type; @@ -1318,7 +1316,15 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons } else { // continue straight until vehicle has sufficient altitude + // keep flaps in landing configuration if the airspeed is below the min airspeed (keep deployed if airspeed not valid) roll_body = 0.0f; + + if (!_airspeed_valid || _airspeed_eas < _performance_model.getMinimumCalibratedAirspeed()) { + _flaps_setpoint = _param_fw_flaps_lnd_scl.get(); + + } else { + _flaps_setpoint = 0.f; + } } is_low_height = true; // In low-height flight, TECS will control altitude tighter diff --git a/src/modules/gimbal/input_mavlink.cpp b/src/modules/gimbal/input_mavlink.cpp index e20bf6c552..aef348ca53 100644 --- a/src/modules/gimbal/input_mavlink.cpp +++ b/src/modules/gimbal/input_mavlink.cpp @@ -43,6 +43,8 @@ #include #include +using namespace matrix; + namespace gimbal { @@ -936,21 +938,19 @@ InputMavlinkGimbalV2::UpdateResult InputMavlinkGimbalV2::_process_set_manual_con if (set_manual_control.origin_sysid == control_data.sysid_primary_control && set_manual_control.origin_compid == control_data.compid_primary_control) { - const matrix::Quatf q = - (PX4_ISFINITE(set_manual_control.pitch) && PX4_ISFINITE(set_manual_control.yaw)) - ? - matrix::Quatf( - matrix::Eulerf(0.0f, set_manual_control.pitch, set_manual_control.yaw)) - : - matrix::Quatf(NAN, NAN, NAN, NAN); + Quatf q(NAN, NAN, NAN, NAN); - const matrix::Vector3f angular_velocity = - (PX4_ISFINITE(set_manual_control.pitch_rate) && - PX4_ISFINITE(set_manual_control.yaw_rate)) ? - matrix::Vector3f(0.0f, - math::radians(_parameters.mnt_rate_pitch) * set_manual_control.pitch_rate, - math::radians(_parameters.mnt_rate_yaw) * set_manual_control.yaw_rate) : - matrix::Vector3f(NAN, NAN, NAN); + if (PX4_ISFINITE(set_manual_control.pitch) && PX4_ISFINITE(set_manual_control.yaw)) { + q = Quatf(matrix::Eulerf(0.0f, set_manual_control.pitch, set_manual_control.yaw)); + } + + Vector3f angular_velocity(NAN, NAN, NAN); + + if (PX4_ISFINITE(set_manual_control.pitch_rate) && PX4_ISFINITE(set_manual_control.yaw_rate)) { + angular_velocity = Vector3f(0.0f, + set_manual_control.pitch_rate * math::radians(_parameters.mnt_rate_pitch), + set_manual_control.yaw_rate * math::radians(_parameters.mnt_rate_yaw)); + } _set_control_data_from_set_attitude(control_data, set_manual_control.flags, q, angular_velocity, set_manual_control.timestamp); diff --git a/src/modules/internal_combustion_engine_control/CMakeLists.txt b/src/modules/internal_combustion_engine_control/CMakeLists.txt new file mode 100644 index 0000000000..15df6ca368 --- /dev/null +++ b/src/modules/internal_combustion_engine_control/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# Copyright (c) 2025 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. +# +############################################################################ +px4_add_module( + MODULE modules__internal_combustion_engine_control + MAIN internal_combustion_engine_control + COMPILE_FLAGS + #-DDEBUG_BUILD + SRCS + InternalCombustionEngineControl.cpp + InternalCombustionEngineControl.hpp + MODULE_CONFIG + module.yaml + DEPENDS + px4_work_queue + SlewRate +) diff --git a/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.cpp b/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.cpp new file mode 100644 index 0000000000..1e8a1b1a35 --- /dev/null +++ b/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.cpp @@ -0,0 +1,418 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 "InternalCombustionEngineControl.hpp" + +#include + +using namespace time_literals; + +namespace internal_combustion_engine_control +{ + +InternalCombustionEngineControl::InternalCombustionEngineControl() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) +{ + _internal_combustion_engine_control_pub.advertise(); + _internal_combustion_engine_status_pub.advertise(); +} + +InternalCombustionEngineControl::~InternalCombustionEngineControl() +{ + +} + +int InternalCombustionEngineControl::task_spawn(int argc, char *argv[]) +{ + InternalCombustionEngineControl *obj = new InternalCombustionEngineControl(); + + if (!obj) { + PX4_ERR("alloc failed"); + return -1; + } + + _object.store(obj); + _task_id = task_id_is_work_queue; + + /* Schedule a cycle to start things. */ + obj->start(); + + return 0; +} + +void InternalCombustionEngineControl::start() +{ + ScheduleOnInterval(20_ms); // 50 Hz +} + +void InternalCombustionEngineControl::Run() +{ + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + } + + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + updateParams(); + _throttle_control_slew_rate.setSlewRate(_param_ice_thr_slew.get()); + } + + + manual_control_setpoint_s manual_control_setpoint; + _manual_control_setpoint_sub.copy(&manual_control_setpoint); + + vehicle_status_s vehicle_status; + _vehicle_status_sub.copy(&vehicle_status); + + actuator_motors_s actuator_motors; + _actuator_motors.copy(&actuator_motors); + + const float throttle_in = actuator_motors.control[0]; + + const hrt_abstime now = hrt_absolute_time(); + + UserOnOffRequest user_request = UserOnOffRequest::Off; + + switch (static_cast(_param_ice_on_source.get())) { + case ICESource::ArmingState: { + if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + user_request = UserOnOffRequest::On; + } + } + break; + + case ICESource::Aux1: { + if (manual_control_setpoint.aux1 > 0.5f) { + user_request = UserOnOffRequest::On; + } + } + break; + + case ICESource::Aux2: { + if (manual_control_setpoint.aux2 > 0.5f) { + user_request = UserOnOffRequest::On; + } + } + break; + } + + switch (_state) { + case State::Stopped: { + controlEngineStop(); + + if (user_request == UserOnOffRequest::On && !maximumAttemptsReached()) { + + _state = State::Starting; + _state_start_time = now; + PX4_INFO("ICE: Starting"); + } + } + break; + + case State::Starting: { + + if (user_request == UserOnOffRequest::Off) { + _state = State::Stopped; + _starting_retry_cycle = 0; + PX4_INFO("ICE: Stopped"); + + } else { + + switch (_starting_sub_state) { + case SubState::Rest: { + if (isStartingPermitted(now)) { + _state_start_time = now; + _starting_sub_state = SubState::Run; + } + } + break; + + case SubState::Run: + default: { + controlEngineStartup(now); + + if (isEngineRunning(now)) { + _state = State::Running; + PX4_INFO("ICE: Starting finished"); + + } else { + + if (maximumAttemptsReached()) { + _state = State::Fault; + PX4_WARN("ICE: Fault"); + + } else if (!isStartingPermitted(now)) { + controlEngineStop(); + _starting_sub_state = SubState::Rest; + } + } + + break; + } + + } + } + + } + break; + + case State::Running: { + controlEngineRunning(throttle_in); + + if (user_request == UserOnOffRequest::Off) { + _state = State::Stopped; + _starting_retry_cycle = 0; + PX4_INFO("ICE: Stopped"); + + } else if (!isEngineRunning(now) && _param_ice_running_fault_detection.get()) { + // without RPM feedback we assume the engine is running after the + // starting procedure but only switch state if fault detection is enabled + _state = State::Starting; + _state_start_time = now; + _starting_retry_cycle = 0; + PX4_WARN("ICE: Running Fault detected"); + events::send(events::ID("internal_combustion_engine_control_fault"), events::Log::Critical, + "IC engine fault detected"); + } + } + + break; + + case State::Fault: { + + // do nothing + if (user_request == UserOnOffRequest::Off) { + _state = State::Stopped; + _starting_retry_cycle = 0; + PX4_INFO("ICE: Stopped"); + + } else { + controlEngineFault(); + } + } + + + break; + } + + const float control_interval = math::constrain(static_cast((now - _last_time_run) * 1e-6f), 0.01f, 0.1f); + + _last_time_run = now; + + // slew rate limit throttle control if it's finite, otherwise just pass it through (0 throttle = NAN = disarmed) + if (PX4_ISFINITE(_throttle_control)) { + _throttle_control = _throttle_control_slew_rate.update(_throttle_control, control_interval); + + } else { + _throttle_control_slew_rate.setForcedValue(0.f); + } + + publishControl(now, user_request); +} + +void InternalCombustionEngineControl::publishControl(const hrt_abstime now, const UserOnOffRequest user_request) +{ + internal_combustion_engine_control_s ice_control{}; + ice_control.timestamp = now; + ice_control.choke_control = _choke_control; + ice_control.ignition_on = _ignition_on; + ice_control.starter_engine_control = _starter_engine_control; + ice_control.throttle_control = _throttle_control; + ice_control.user_request = static_cast(user_request); + _internal_combustion_engine_control_pub.publish(ice_control); + + internal_combustion_engine_status_s ice_status; + ice_status.state = static_cast(_state); + ice_status.timestamp = now; + _internal_combustion_engine_status_pub.publish(ice_status); +} + +bool InternalCombustionEngineControl::isEngineRunning(const hrt_abstime now) +{ + rpm_s rpm; + + if (_rpm_sub.copy(&rpm)) { + const hrt_abstime rpm_timestamp = rpm.timestamp; + + return (_param_ice_min_run_rpm.get() > FLT_EPSILON && (now < rpm_timestamp + 2_s) + && rpm.rpm_estimate > _param_ice_min_run_rpm.get()); + } + + return false; +} + +void InternalCombustionEngineControl::controlEngineRunning(float throttle_in) +{ + _ignition_on = true; + _choke_control = 0.f; + _starter_engine_control = 0.f; + _throttle_control = throttle_in; + +} + +void InternalCombustionEngineControl::controlEngineStop() +{ + _ignition_on = false; + _choke_control = _param_ice_stop_choke.get() ? 1.f : 0.f; + _starter_engine_control = 0.f; + _throttle_control = 0.f; +} + +void InternalCombustionEngineControl::controlEngineFault() +{ + _ignition_on = false; + _choke_control = _param_ice_stop_choke.get() ? 1.f : 0.f; + _starter_engine_control = 0.f; + _throttle_control = 0.f; +} + +void InternalCombustionEngineControl::controlEngineStartup(const hrt_abstime now) +{ + float ignition_delay = 0.f; + float choke_duration = 0.f; + const float starter_duration = _param_ice_strt_dur.get(); + + if (_starting_retry_cycle == 0) { + ignition_delay = math::max(_param_ice_ign_delay.get(), 0.f); + + if (_param_ice_choke_st_dur.get() > FLT_EPSILON) { + choke_duration = _param_ice_choke_st_dur.get(); + } + } + + _ignition_on = true; + _throttle_control = _param_ice_strt_thr.get(); + _choke_control = now < _state_start_time + (choke_duration + ignition_delay) * 1_s ? 1.f : 0.f; + _starter_engine_control = now > _state_start_time + (ignition_delay * 1_s) ? 1.f : 0.f; + const hrt_abstime cycle_timeout_duration = (ignition_delay + choke_duration + starter_duration) * 1_s; + + if (now > _state_start_time + cycle_timeout_duration) { + // start resting timer if engine is not running + _starting_rest_time = now; + _starting_retry_cycle++; + PX4_INFO("ICE: starting attempt %i finished", _starting_retry_cycle); + } +} + +bool InternalCombustionEngineControl::isStartingPermitted(const hrt_abstime now) +{ + return now > _starting_rest_time + DELAY_BEFORE_RESTARTING * 1_s; +} + +bool InternalCombustionEngineControl::maximumAttemptsReached() +{ + // First and only attempt + if (_param_ice_strt_attempts.get() == 0) { + return _starting_retry_cycle > 0; + } + + return _starting_retry_cycle >= _param_ice_strt_attempts.get(); +} + +int InternalCombustionEngineControl::print_usage(const char *reason) +{ + if (reason) { + PX4_ERR("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description + +The module controls internal combustion engine (ICE) features including: +ignition (on/off), throttle and choke level, starter engine delay, and user request. + +### Enabling + +This feature is not enabled by default needs to be configured in the +build target for your board together with the rpm capture driver: + +``` +CONFIG_MODULES_INTERNAL_COMBUSTION_ENGINE_CONTROL=y +CONFIG_DRIVERS_RPM_CAPTURE=y +``` + +Additionally, to enable the module: + +- Set [ICE_EN](../advanced_config/parameter_reference.md#ICE_EN) +to true and adjust the other `ICE_` module parameters according to your needs. +- Set [RPM_CAP_ENABLE](../advanced_config/parameter_reference.md#RPM_CAP_ENABLE) to true. + +The module outputs control signals for ignition, throttle, and choke, +and takes inputs from an RPM sensor. +These must be mapped to AUX outputs/inputs in the [Actuator configuration](../config/actuators.md), +similar to the setup shown below. + +![Actuator setup for ICE](../../assets/hardware/ice/ice_actuator_setup.png) + +### Implementation + +The ICE is implemented with a (4) state machine: + +![Architecture](../../assets/hardware/ice/ice_control_state_machine.png) + +The state machine: + +- Checks if [Rpm.msg](../msg_docs/Rpm.md) is updated to know if the engine is running +- Allows for user inputs from: + - AUX{N} + - Arming state in [VehicleStatus.msg](../msg_docs/VehicleStatus.md) + +The module publishes [InternalCombustionEngineControl.msg](../msg_docs/InternalCombustionEngineControl.md). + +The architecture is as shown below: + +![Architecture](../../assets/hardware/ice/ice_control_diagram.png) + + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("internal_combustion_engine_control", "system"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + return 0; +} + +extern "C" __EXPORT int internal_combustion_engine_control_main(int argc, char *argv[]) +{ + return InternalCombustionEngineControl::main(argc, argv); +} + +} // namespace internal_combustion_engine_control diff --git a/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.hpp b/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.hpp new file mode 100644 index 0000000000..01884e4967 --- /dev/null +++ b/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.hpp @@ -0,0 +1,159 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 + +#include + +namespace internal_combustion_engine_control +{ + +class InternalCombustionEngineControl : public ModuleBase, public ModuleParams, + public px4::ScheduledWorkItem +{ +public: + InternalCombustionEngineControl(); + ~InternalCombustionEngineControl() override; + + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]) + { + return print_usage("unknown command"); + } + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + void start(); + +private: + void Run() override; + + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + uORB::Subscription _rpm_sub{ORB_ID(rpm)}; + uORB::Subscription _actuator_motors{ORB_ID(actuator_motors)}; + + uORB::Publication _internal_combustion_engine_control_pub{ORB_ID(internal_combustion_engine_control)}; + uORB::Publication _internal_combustion_engine_status_pub{ORB_ID(internal_combustion_engine_status)}; + + + // has to mirror internal_combustion_engine_status_s::State + enum class State { + Stopped, + Starting, + Running, + Fault + } _state{State::Stopped}; + + enum class SubState { + Run, + Rest + }; + + enum class UserOnOffRequest { + Off, + On + }; + + enum class ICESource { + ArmingState, + Aux1, + Aux2, + }; + + hrt_abstime _state_start_time{0}; + hrt_abstime _last_time_run{0}; + + bool _ignition_on{false}; + float _choke_control{1.f}; + float _starter_engine_control{0.f}; + float _throttle_control{0.f}; + + SlewRate _throttle_control_slew_rate; + + bool isEngineRunning(const hrt_abstime now); + void controlEngineRunning(float throttle_in); + void controlEngineStop(); + void controlEngineStartup(const hrt_abstime now); + void controlEngineFault(); + bool maximumAttemptsReached(); + void publishControl(const hrt_abstime now, const UserOnOffRequest user_request); + + // Starting state specifics + static constexpr float DELAY_BEFORE_RESTARTING{1.f}; + int _starting_retry_cycle{0}; + hrt_abstime _starting_rest_time{0}; + SubState _starting_sub_state{SubState::Run}; + + /** + * @brief Currently the ICE starting state is permitted after resting + * DELAY_BEFORE_RESTARTING s to reduce wear on the starter motor. + * @param now current time + * @return if true, otherwise false + */ + bool isStartingPermitted(const hrt_abstime now); + + DEFINE_PARAMETERS( + (ParamInt) _param_ice_on_source, + (ParamFloat) _param_ice_choke_st_dur, + (ParamFloat) _param_ice_strt_dur, + (ParamFloat) _param_ice_min_run_rpm, + (ParamInt) _param_ice_strt_attempts, + (ParamInt) _param_ice_running_fault_detection, + (ParamFloat) _param_ice_strt_thr, + (ParamInt) _param_ice_stop_choke, + (ParamFloat) _param_ice_thr_slew, + (ParamFloat) _param_ice_ign_delay + ) +}; + +} // namespace internal_combustion_engine_control diff --git a/src/modules/internal_combustion_engine_control/Kconfig b/src/modules/internal_combustion_engine_control/Kconfig new file mode 100644 index 0000000000..c0d0e9049f --- /dev/null +++ b/src/modules/internal_combustion_engine_control/Kconfig @@ -0,0 +1,5 @@ +menuconfig MODULES_INTERNAL_COMBUSTION_ENGINE_CONTROL + bool "internal_combustion_engine_control" + default n + ---help--- + Enable support for internal_combustion_engine_control diff --git a/src/modules/internal_combustion_engine_control/module.yaml b/src/modules/internal_combustion_engine_control/module.yaml new file mode 100644 index 0000000000..517fc89fee --- /dev/null +++ b/src/modules/internal_combustion_engine_control/module.yaml @@ -0,0 +1,120 @@ +module_name: Internal Combustion Engine Control + +parameters: + - group: ICE + definitions: + + ICE_EN: + description: + short: Enable internal combustion engine + type: boolean + default: true + + ICE_ON_SOURCE: + description: + short: Engine start/stop input source + type: enum + default: 0 + values: + 0: On arming - disarming + 1: Aux1 + 2: Aux2 + + ICE_CHOKE_ST_DUR: + description: + short: Duration of choking during startup + type: float + unit: s + min: 0 + max: 10 + decimal: 1 + increment: 0.1 + default: 5 + + ICE_STRT_DUR: + description: + short: Duration of single starting attempt (excl. choking) + long: | + Maximum expected time for startup before declaring timeout. + type: float + unit: s + min: 0 + max: 10 + decimal: 1 + increment: 0.1 + default: 5 + + ICE_MIN_RUN_RPM: + description: + short: Minimum RPM for engine to be declared running + type: float + unit: rpm + min: 0 + max: 10000 + decimal: 0 + increment: 1 + default: 2000 + + ICE_STRT_ATTEMPT: + description: + short: Number attempts for starting the engine + long: | + Number of accepted attempts for starting the engine before declaring a fault. + type: int32 + min: 0 + max: 10 + default: 3 + + ICE_RUN_FAULT_D: + description: + short: Fault detection if it stops in running state + long: | + Enables restart if a fault is detected during the running state. Otherwise + commands continues in running state until given an user request off. + type: boolean + default: true + + ICE_STRT_THR: + description: + short: Throttle value for starting engine + long: | + During the choking and the starting phase, the throttle value is set to this value. + type: float + unit: norm + min: 0 + max: 1 + decimal: 0 + increment: 0.01 + default: 0.1 + + ICE_STOP_CHOKE: + description: + short: Apply choke when stopping engine + type: boolean + default: true + + ICE_THR_SLEW: + description: + short: Throttle slew rate + long: | + Maximum rate of change of throttle value per second. + type: float + unit: 1/s + min: 0 + max: 1 + decimal: 2 + increment: 0.01 + default: 0.5 + + ICE_IGN_DELAY: + description: + short: Cold-start delay after ignition before engaging starter + long: | + In case that the ignition takes a moment to be up and running, this parameter can be set to account for that. + type: float + unit: s + min: 0 + max: 10 + decimal: 1 + increment: 0.1 + default: 0 diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 57b2a429de..84d8e3eadd 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -79,6 +79,7 @@ void LoggedTopics::add_default_topics() add_topic("home_position"); add_topic("hover_thrust_estimate", 100); add_topic("input_rc", 500); + add_optional_topic("internal_combustion_engine_control", 10); add_optional_topic("internal_combustion_engine_status", 10); add_optional_topic("iridiumsbd_status", 1000); add_optional_topic("irlock_report", 1000); @@ -104,17 +105,11 @@ void LoggedTopics::add_default_topics() add_topic("radio_status"); add_optional_topic("rover_attitude_setpoint", 100); add_optional_topic("rover_attitude_status", 100); - add_optional_topic("rover_velocity_status", 100); add_optional_topic("rover_rate_setpoint", 100); add_optional_topic("rover_rate_status", 100); add_optional_topic("rover_steering_setpoint", 100); add_optional_topic("rover_throttle_setpoint", 100); - add_optional_topic("rover_differential_guidance_status", 100); - add_optional_topic("rover_differential_setpoint", 100); - add_optional_topic("rover_differential_status", 100); - add_optional_topic("rover_mecanum_guidance_status", 100); - add_optional_topic("rover_mecanum_setpoint", 100); - add_optional_topic("rover_mecanum_status", 100); + add_optional_topic("rover_velocity_status", 100); add_topic("rtl_time_estimate", 1000); add_topic("rtl_status", 2000); add_optional_topic("sensor_airflow", 100); @@ -335,8 +330,6 @@ void LoggedTopics::add_vision_and_avoidance_topics() add_topic("obstacle_distance_fused"); add_topic("obstacle_distance"); add_topic("vehicle_mocap_odometry", 30); - add_topic("vehicle_trajectory_waypoint", 200); - add_topic("vehicle_trajectory_waypoint_desired", 200); add_topic("vehicle_visual_odometry", 30); } diff --git a/src/modules/logger/params.c b/src/modules/logger/params.c index 3d7e873854..e1bbd5145f 100644 --- a/src/modules/logger/params.c +++ b/src/modules/logger/params.c @@ -123,7 +123,7 @@ PARAM_DEFINE_INT32(SDLOG_MISSION, 0); * 4 : Full rates for analysis of fast maneuvers (RC, attitude, rates and actuators) * 5 : Debugging topics (debug_*.msg topics, for custom code) * 6 : Topics for sensor comparison (low rate raw IMU, Baro and magnetometer data) - * 7 : Topics for computer vision and collision avoidance + * 7 : Topics for computer vision and collision prevention * 8 : Raw FIFO high-rate IMU (Gyro) * 9 : Raw FIFO high-rate IMU (Accel) * 10: Logging of mavlink tunnel message (useful for payload communication debugging) diff --git a/src/modules/mavlink/mavlink b/src/modules/mavlink/mavlink index 619947d8bc..8690e10164 160000 --- a/src/modules/mavlink/mavlink +++ b/src/modules/mavlink/mavlink @@ -1 +1 @@ -Subproject commit 619947d8bc29e80eecff18b0f4fecc42d9e171dd +Subproject commit 8690e10164da864d7d36a3daad5547662e7a4103 diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 6e81849600..724391cf86 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1525,7 +1525,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("SYS_STATUS", 5.0f); configure_stream_local("SYSTEM_TIME", 1.0f); configure_stream_local("TIME_ESTIMATE_TO_TARGET", 1.0f); - configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f); configure_stream_local("VFR_HUD", 10.0f); configure_stream_local("VIBRATION", 0.5f); configure_stream_local("WIND_COV", 10.0f); @@ -1592,7 +1591,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("RC_CHANNELS", 5.0f); configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f); configure_stream_local("SYS_STATUS", 5.0f); - configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f); configure_stream_local("VFR_HUD", 4.0f); configure_stream_local("VIBRATION", 0.5f); configure_stream_local("WIND_COV", 1.0f); @@ -1784,7 +1782,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("SYS_STATUS", 5.0f); configure_stream_local("SYSTEM_TIME", 2.0f); configure_stream_local("TIME_ESTIMATE_TO_TARGET", 1.0f); - configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f); configure_stream_local("VFR_HUD", 4.0f); configure_stream_local("VIBRATION", 0.5f); configure_stream_local("WIND_COV", 1.0f); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 72b5b49123..7d5e23bb6e 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -116,7 +116,6 @@ #include "streams/SYSTEM_TIME.hpp" #include "streams/TIME_ESTIMATE_TO_TARGET.hpp" #include "streams/TIMESYNC.hpp" -#include "streams/TRAJECTORY_REPRESENTATION_WAYPOINTS.hpp" #include "streams/VFR_HUD.hpp" #include "streams/VIBRATION.hpp" #include "streams/WIND_COV.hpp" @@ -382,9 +381,6 @@ static const StreamListItem streams_list[] = { #if defined(MANUAL_CONTROL_HPP) create_stream_list_item(), #endif // MANUAL_CONTROL_HPP -#if defined(TRAJECTORY_REPRESENTATION_WAYPOINTS_HPP) - create_stream_list_item(), -#endif // TRAJECTORY_REPRESENTATION_WAYPOINTS_HPP #if defined(OPTICAL_FLOW_RAD_HPP) create_stream_list_item(), #endif // OPTICAL_FLOW_RAD_HPP diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index aa8b50d01a..39b626b4d2 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -256,14 +256,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_tunnel(msg); break; - case MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER: - handle_message_trajectory_representation_bezier(msg); - break; - - case MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS: - handle_message_trajectory_representation_waypoints(msg); - break; - case MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS: handle_message_onboard_computer_status(msg); break; @@ -1822,13 +1814,13 @@ MavlinkReceiver::handle_message_battery_status(mavlink_message_t *msg) // Set the battery warning based on remaining charge. // Note: Smallest values must come first in evaluation. if (battery_status.remaining < _param_bat_emergen_thr.get()) { - battery_status.warning = battery_status_s::BATTERY_WARNING_EMERGENCY; + battery_status.warning = battery_status_s::WARNING_EMERGENCY; } else if (battery_status.remaining < _param_bat_crit_thr.get()) { - battery_status.warning = battery_status_s::BATTERY_WARNING_CRITICAL; + battery_status.warning = battery_status_s::WARNING_CRITICAL; } else if (battery_status.remaining < _param_bat_low_thr.get()) { - battery_status.warning = battery_status_s::BATTERY_WARNING_LOW; + battery_status.warning = battery_status_s::WARNING_LOW; } _battery_pub.publish(battery_status); @@ -1984,66 +1976,17 @@ MavlinkReceiver::handle_message_tunnel(mavlink_message_t *msg) memcpy(tunnel.payload, mavlink_tunnel.payload, sizeof(tunnel.payload)); static_assert(sizeof(tunnel.payload) == sizeof(mavlink_tunnel.payload), "mavlink_tunnel.payload size mismatch"); - _mavlink_tunnel_pub.publish(tunnel); + switch (mavlink_tunnel.payload_type) { + case MAV_TUNNEL_PAYLOAD_TYPE_MODALAI_ESC_UART_PASSTHRU: + _esc_serial_passthru_pub.publish(tunnel); + break; -} - -void -MavlinkReceiver::handle_message_trajectory_representation_bezier(mavlink_message_t *msg) -{ - mavlink_trajectory_representation_bezier_t trajectory; - mavlink_msg_trajectory_representation_bezier_decode(msg, &trajectory); - - vehicle_trajectory_bezier_s trajectory_bezier{}; - - trajectory_bezier.timestamp = _mavlink_timesync.sync_stamp(trajectory.time_usec); - - for (int i = 0; i < vehicle_trajectory_bezier_s::NUMBER_POINTS; ++i) { - trajectory_bezier.control_points[i].position[0] = trajectory.pos_x[i]; - trajectory_bezier.control_points[i].position[1] = trajectory.pos_y[i]; - trajectory_bezier.control_points[i].position[2] = trajectory.pos_z[i]; - - trajectory_bezier.control_points[i].delta = trajectory.delta[i]; - trajectory_bezier.control_points[i].yaw = trajectory.pos_yaw[i]; + default: + _mavlink_tunnel_pub.publish(tunnel); + break; } - trajectory_bezier.bezier_order = math::min(trajectory.valid_points, vehicle_trajectory_bezier_s::NUMBER_POINTS); - _trajectory_bezier_pub.publish(trajectory_bezier); -} -void -MavlinkReceiver::handle_message_trajectory_representation_waypoints(mavlink_message_t *msg) -{ - mavlink_trajectory_representation_waypoints_t trajectory; - mavlink_msg_trajectory_representation_waypoints_decode(msg, &trajectory); - - vehicle_trajectory_waypoint_s trajectory_waypoint{}; - - const int number_valid_points = math::min(trajectory.valid_points, vehicle_trajectory_waypoint_s::NUMBER_POINTS); - - for (int i = 0; i < number_valid_points; ++i) { - trajectory_waypoint.waypoints[i].position[0] = trajectory.pos_x[i]; - trajectory_waypoint.waypoints[i].position[1] = trajectory.pos_y[i]; - trajectory_waypoint.waypoints[i].position[2] = trajectory.pos_z[i]; - - trajectory_waypoint.waypoints[i].velocity[0] = trajectory.vel_x[i]; - trajectory_waypoint.waypoints[i].velocity[1] = trajectory.vel_y[i]; - trajectory_waypoint.waypoints[i].velocity[2] = trajectory.vel_z[i]; - - trajectory_waypoint.waypoints[i].acceleration[0] = trajectory.acc_x[i]; - trajectory_waypoint.waypoints[i].acceleration[1] = trajectory.acc_y[i]; - trajectory_waypoint.waypoints[i].acceleration[2] = trajectory.acc_z[i]; - - trajectory_waypoint.waypoints[i].yaw = trajectory.pos_yaw[i]; - trajectory_waypoint.waypoints[i].yaw_speed = trajectory.vel_yaw[i]; - - trajectory_waypoint.waypoints[i].point_valid = true; - - trajectory_waypoint.waypoints[i].type = UINT8_MAX; - } - - trajectory_waypoint.timestamp = hrt_absolute_time(); - _trajectory_waypoint_pub.publish(trajectory_waypoint); } void @@ -2259,11 +2202,6 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) _heartbeat_component_osd = now; break; - case MAV_COMP_ID_OBSTACLE_AVOIDANCE: - _heartbeat_component_obstacle_avoidance = now; - _mavlink.telemetry_status().avoidance_system_healthy = (hb.system_status == MAV_STATE_ACTIVE); - break; - case MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY: _heartbeat_component_visual_inertial_odometry = now; break; @@ -2669,7 +2607,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) airspeed.timestamp_sample = timestamp_sample; airspeed.indicated_airspeed_m_s = hil_state.ind_airspeed * 1e-2f; airspeed.true_airspeed_m_s = hil_state.true_airspeed * 1e-2f; - airspeed.air_temperature_celsius = 15.f; airspeed.timestamp = hrt_absolute_time(); _airspeed_pub.publish(airspeed); } @@ -2976,7 +2913,6 @@ void MavlinkReceiver::CheckHeartbeats(const hrt_abstime &t, bool force) tstatus.heartbeat_component_telemetry_radio = (t <= TIMEOUT + _heartbeat_component_telemetry_radio); tstatus.heartbeat_component_log = (t <= TIMEOUT + _heartbeat_component_log); tstatus.heartbeat_component_osd = (t <= TIMEOUT + _heartbeat_component_osd); - tstatus.heartbeat_component_obstacle_avoidance = (t <= TIMEOUT + _heartbeat_component_obstacle_avoidance); tstatus.heartbeat_component_vio = (t <= TIMEOUT + _heartbeat_component_visual_inertial_odometry); tstatus.heartbeat_component_pairing_manager = (t <= TIMEOUT + _heartbeat_component_pairing_manager); tstatus.heartbeat_component_udp_bridge = (t <= TIMEOUT + _heartbeat_component_udp_bridge); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 326d0ee19e..6c7b3065e2 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -110,8 +110,6 @@ #include #include #include -#include -#include #include #if !defined(CONSTRAINED_FLASH) @@ -201,8 +199,6 @@ private: void handle_message_set_position_target_local_ned(mavlink_message_t *msg); void handle_message_statustext(mavlink_message_t *msg); void handle_message_tunnel(mavlink_message_t *msg); - void handle_message_trajectory_representation_bezier(mavlink_message_t *msg); - void handle_message_trajectory_representation_waypoints(mavlink_message_t *msg); void handle_message_utm_global_position(mavlink_message_t *msg); #if defined(MAVLINK_MSG_ID_SET_VELOCITY_LIMITS) // For now only defined if development.xml is used void handle_message_set_velocity_limits(mavlink_message_t *msg); @@ -311,6 +307,7 @@ private: uORB::Publication _landing_target_pose_pub{ORB_ID(landing_target_pose)}; uORB::Publication _log_message_pub{ORB_ID(log_message)}; uORB::Publication _mavlink_tunnel_pub{ORB_ID(mavlink_tunnel)}; + uORB::Publication _esc_serial_passthru_pub{ORB_ID(esc_serial_passthru)}; uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance)}; uORB::Publication _offboard_control_mode_pub{ORB_ID(offboard_control_mode)}; uORB::Publication _onboard_computer_status_pub{ORB_ID(onboard_computer_status)}; @@ -329,8 +326,6 @@ private: uORB::Publication _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)}; uORB::Publication _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; uORB::Publication _rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; - uORB::Publication _trajectory_bezier_pub{ORB_ID(vehicle_trajectory_bezier)}; - uORB::Publication _trajectory_waypoint_pub{ORB_ID(vehicle_trajectory_waypoint)}; #if !defined(CONSTRAINED_FLASH) uORB::Publication _debug_array_pub {ORB_ID(debug_array)}; @@ -400,7 +395,6 @@ private: hrt_abstime _heartbeat_component_telemetry_radio{0}; hrt_abstime _heartbeat_component_log{0}; hrt_abstime _heartbeat_component_osd{0}; - hrt_abstime _heartbeat_component_obstacle_avoidance{0}; hrt_abstime _heartbeat_component_visual_inertial_odometry{0}; hrt_abstime _heartbeat_component_pairing_manager{0}; hrt_abstime _heartbeat_component_udp_bridge{0}; diff --git a/src/modules/mavlink/streams/BATTERY_STATUS.hpp b/src/modules/mavlink/streams/BATTERY_STATUS.hpp index 633807a86e..5824c534eb 100644 --- a/src/modules/mavlink/streams/BATTERY_STATUS.hpp +++ b/src/modules/mavlink/streams/BATTERY_STATUS.hpp @@ -81,31 +81,31 @@ private: math::max((int)battery_status.time_remaining_s, 1) : 0; switch (battery_status.warning) { - case (battery_status_s::BATTERY_WARNING_NONE): + case (battery_status_s::WARNING_NONE): bat_msg.charge_state = MAV_BATTERY_CHARGE_STATE_OK; break; - case (battery_status_s::BATTERY_WARNING_LOW): + case (battery_status_s::WARNING_LOW): bat_msg.charge_state = MAV_BATTERY_CHARGE_STATE_LOW; break; - case (battery_status_s::BATTERY_WARNING_CRITICAL): + case (battery_status_s::WARNING_CRITICAL): bat_msg.charge_state = MAV_BATTERY_CHARGE_STATE_CRITICAL; break; - case (battery_status_s::BATTERY_WARNING_EMERGENCY): + case (battery_status_s::WARNING_EMERGENCY): bat_msg.charge_state = MAV_BATTERY_CHARGE_STATE_EMERGENCY; break; - case (battery_status_s::BATTERY_WARNING_FAILED): + case (battery_status_s::WARNING_FAILED): bat_msg.charge_state = MAV_BATTERY_CHARGE_STATE_FAILED; break; - case (battery_status_s::BATTERY_STATE_UNHEALTHY): + case (battery_status_s::STATE_UNHEALTHY): bat_msg.charge_state = MAV_BATTERY_CHARGE_STATE_UNHEALTHY; break; - case (battery_status_s::BATTERY_STATE_CHARGING): + case (battery_status_s::STATE_CHARGING): bat_msg.charge_state = MAV_BATTERY_CHARGE_STATE_CHARGING; break; diff --git a/src/modules/mavlink/streams/DISTANCE_SENSOR.hpp b/src/modules/mavlink/streams/DISTANCE_SENSOR.hpp index 9042403a48..ddabecfba6 100644 --- a/src/modules/mavlink/streams/DISTANCE_SENSOR.hpp +++ b/src/modules/mavlink/streams/DISTANCE_SENSOR.hpp @@ -94,6 +94,22 @@ private: msg.min_distance = dist_sensor.min_distance * 1e2f; // m to cm msg.orientation = dist_sensor.orientation; msg.covariance = dist_sensor.variance * 1e4f; // m^2 to cm^2 + msg.horizontal_fov = dist_sensor.h_fov; + msg.vertical_fov = dist_sensor.v_fov; + msg.quaternion[0] = dist_sensor.q[0]; + msg.quaternion[1] = dist_sensor.q[1]; + msg.quaternion[2] = dist_sensor.q[2]; + msg.quaternion[3] = dist_sensor.q[3]; + + if (dist_sensor.signal_quality < 0) { + msg.signal_quality = 0; + + } else if (dist_sensor.signal_quality == 0) { + msg.signal_quality = 1; + + } else { + msg.signal_quality = dist_sensor.signal_quality; + } mavlink_msg_distance_sensor_send_struct(_mavlink->get_channel(), &msg); diff --git a/src/modules/mavlink/streams/HIGHRES_IMU.hpp b/src/modules/mavlink/streams/HIGHRES_IMU.hpp index a6682213bf..7a3d96cb38 100644 --- a/src/modules/mavlink/streams/HIGHRES_IMU.hpp +++ b/src/modules/mavlink/streams/HIGHRES_IMU.hpp @@ -193,7 +193,7 @@ private: msg.abs_pressure = air_data.baro_pressure_pa; msg.diff_pressure = differential_pressure.differential_pressure_pa; msg.pressure_alt = air_data.baro_alt_meter; - msg.temperature = air_data.baro_temp_celcius; + msg.temperature = air_data.ambient_temperature; msg.fields_updated = fields_updated; mavlink_msg_highres_imu_send_struct(_mavlink->get_channel(), &msg); diff --git a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp index 6aca9f7d98..8723f14131 100644 --- a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp +++ b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp @@ -57,6 +57,7 @@ #include #include #include +#include #include @@ -284,7 +285,7 @@ private: updated = true; _batteries[i].connected = battery.connected; - if (battery.warning > battery_status_s::BATTERY_WARNING_LOW) { + if (battery.warning > battery_status_s::WARNING_LOW) { msg->failure_flags |= HL_FAILURE_FLAG_BATTERY; } } @@ -521,6 +522,7 @@ private: update_gps(); update_vehicle_status(); update_wind(); + update_vehicle_air_data(); } void update_airspeed() @@ -529,7 +531,6 @@ private: if (_airspeed_sub.update(&airspeed)) { _airspeed.add_value(airspeed.indicated_airspeed_m_s, _update_rate_filtered); - _temperature.add_value(airspeed.air_temperature_celsius, _update_rate_filtered); } } @@ -610,6 +611,15 @@ private: } } + void update_vehicle_air_data() + { + vehicle_air_data_s air_data; + + if (_vehicle_air_data_sub.update(&air_data)) { + _temperature.add_value(air_data.ambient_temperature, _update_rate_filtered); + } + } + void set_default_values(mavlink_high_latency2_t &msg) const { msg.airspeed = 0; @@ -659,6 +669,7 @@ private: uORB::Subscription _tecs_status_sub{ORB_ID(tecs_status)}; uORB::Subscription _wind_sub{ORB_ID(wind)}; uORB::Subscription _health_report_sub{ORB_ID(health_report)}; + uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; SimpleAnalyzer _airspeed; SimpleAnalyzer _airspeed_sp; diff --git a/src/modules/mavlink/streams/SYS_STATUS.hpp b/src/modules/mavlink/streams/SYS_STATUS.hpp index c875eaceaa..7a89501793 100644 --- a/src/modules/mavlink/streams/SYS_STATUS.hpp +++ b/src/modules/mavlink/streams/SYS_STATUS.hpp @@ -144,7 +144,6 @@ private: fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_BATTERY, health_component_t::battery, msg); fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS, health_component_t::motors_escs, msg); fillOutComponent(health_report, MAV_SYS_STATUS_RECOVERY_SYSTEM, health_component_t::parachute, msg); - fillOutComponent(health_report, MAV_SYS_STATUS_OBSTACLE_AVOIDANCE, health_component_t::avoidance, msg); fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_3D_ACCEL, health_component_t::accel, msg); fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_3D_GYRO, health_component_t::gyro, msg); fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_3D_MAG, health_component_t::magnetometer, msg); diff --git a/src/modules/mavlink/streams/TRAJECTORY_REPRESENTATION_WAYPOINTS.hpp b/src/modules/mavlink/streams/TRAJECTORY_REPRESENTATION_WAYPOINTS.hpp deleted file mode 100644 index 174c633fd2..0000000000 --- a/src/modules/mavlink/streams/TRAJECTORY_REPRESENTATION_WAYPOINTS.hpp +++ /dev/null @@ -1,124 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ - -#ifndef TRAJECTORY_REPRESENTATION_WAYPOINTS_HPP -#define TRAJECTORY_REPRESENTATION_WAYPOINTS_HPP - -#include - -class MavlinkStreamTrajectoryRepresentationWaypoints: public MavlinkStream -{ -public: - static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamTrajectoryRepresentationWaypoints(mavlink); } - - const char *get_name() const override { return get_name_static(); } - uint16_t get_id() override { return get_id_static(); } - - static constexpr const char *get_name_static() { return "TRAJECTORY_REPRESENTATION_WAYPOINTS"; } - static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS; } - - unsigned get_size() override - { - if (_traj_wp_avoidance_sub.advertised()) { - return MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; - } - - return 0; - } - -private: - explicit MavlinkStreamTrajectoryRepresentationWaypoints(Mavlink *mavlink) : MavlinkStream(mavlink) {} - - uORB::Subscription _traj_wp_avoidance_sub{ORB_ID(vehicle_trajectory_waypoint_desired)}; - - bool send() override - { - vehicle_trajectory_waypoint_s traj_wp_avoidance_desired; - - if (_traj_wp_avoidance_sub.update(&traj_wp_avoidance_desired)) { - mavlink_trajectory_representation_waypoints_t msg{}; - - msg.time_usec = traj_wp_avoidance_desired.timestamp; - int number_valid_points = 0; - - for (int i = 0; i < vehicle_trajectory_waypoint_s::NUMBER_POINTS; ++i) { - msg.pos_x[i] = traj_wp_avoidance_desired.waypoints[i].position[0]; - msg.pos_y[i] = traj_wp_avoidance_desired.waypoints[i].position[1]; - msg.pos_z[i] = traj_wp_avoidance_desired.waypoints[i].position[2]; - - msg.vel_x[i] = traj_wp_avoidance_desired.waypoints[i].velocity[0]; - msg.vel_y[i] = traj_wp_avoidance_desired.waypoints[i].velocity[1]; - msg.vel_z[i] = traj_wp_avoidance_desired.waypoints[i].velocity[2]; - - msg.acc_x[i] = traj_wp_avoidance_desired.waypoints[i].acceleration[0]; - msg.acc_y[i] = traj_wp_avoidance_desired.waypoints[i].acceleration[1]; - msg.acc_z[i] = traj_wp_avoidance_desired.waypoints[i].acceleration[2]; - - msg.pos_yaw[i] = traj_wp_avoidance_desired.waypoints[i].yaw; - msg.vel_yaw[i] = traj_wp_avoidance_desired.waypoints[i].yaw_speed; - - switch (traj_wp_avoidance_desired.waypoints[i].type) { - case position_setpoint_s::SETPOINT_TYPE_TAKEOFF: - msg.command[i] = vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF; - break; - - case position_setpoint_s::SETPOINT_TYPE_LOITER: - msg.command[i] = vehicle_command_s::VEHICLE_CMD_NAV_LOITER_UNLIM; - break; - - case position_setpoint_s::SETPOINT_TYPE_LAND: - msg.command[i] = vehicle_command_s::VEHICLE_CMD_NAV_LAND; - break; - - default: - msg.command[i] = UINT16_MAX; - } - - if (traj_wp_avoidance_desired.waypoints[i].point_valid) { - number_valid_points++; - } - - } - - msg.valid_points = number_valid_points; - - mavlink_msg_trajectory_representation_waypoints_send_struct(_mavlink->get_channel(), &msg); - - return true; - } - - return false; - } -}; - -#endif // TRAJECTORY_REPRESENTATION_WAYPOINTS_HPP diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 995ee2cb10..71a6bddf2a 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1143,17 +1143,11 @@ float Navigator::get_altitude_acceptance_radius() } else { float alt_acceptance_radius = _param_nav_mc_alt_rad.get(); - - const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get(); - const position_setpoint_s &curr_sp = get_position_setpoint_triplet()->current; if (PX4_ISFINITE(curr_sp.alt_acceptance_radius) && curr_sp.alt_acceptance_radius > FLT_EPSILON) { alt_acceptance_radius = curr_sp.alt_acceptance_radius; - } else if ((pos_ctrl_status.timestamp > _pos_sp_triplet.timestamp) - && pos_ctrl_status.altitude_acceptance > alt_acceptance_radius) { - alt_acceptance_radius = pos_ctrl_status.altitude_acceptance; } return alt_acceptance_radius; @@ -1215,14 +1209,6 @@ bool Navigator::get_yaw_to_be_accepted(float mission_item_yaw) { float yaw = mission_item_yaw; - const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get(); - - // if yaw_acceptance from position controller is NaN overwrite the mission item yaw such that - // the waypoint can be reached from any direction - if ((pos_ctrl_status.timestamp > _pos_sp_triplet.timestamp) && !PX4_ISFINITE(pos_ctrl_status.yaw_acceptance)) { - yaw = pos_ctrl_status.yaw_acceptance; - } - return PX4_ISFINITE(yaw); } diff --git a/src/modules/replay/Replay.cpp b/src/modules/replay/Replay.cpp index a49c005bb5..8329477649 100644 --- a/src/modules/replay/Replay.cpp +++ b/src/modules/replay/Replay.cpp @@ -721,10 +721,6 @@ Replay::nextDataMessage(std::ifstream &file, Subscription &subscription, int msg } switch (message_header.msg_type) { - case (int)ULogMessageType::ADD_LOGGED_MSG: - readAndAddSubscription(file, message_header.msg_size); - break; - case (int)ULogMessageType::DATA: file.read((char *)&file_msg_id, sizeof(file_msg_id)); @@ -751,6 +747,7 @@ Replay::nextDataMessage(std::ifstream &file, Subscription &subscription, int msg break; case (int)ULogMessageType::REMOVE_LOGGED_MSG: //skip these + case (int)ULogMessageType::ADD_LOGGED_MSG: case (int)ULogMessageType::PARAMETER: case (int)ULogMessageType::DROPOUT: case (int)ULogMessageType::INFO: @@ -907,19 +904,26 @@ Replay::run() ulog_message_header_s message_header; replay_file.seekg(_data_section_start); - //we know the next message must be an ADD_LOGGED_MSG - ReadAndAndAddSubResult res; - - do { + while (true) { + //we are in the Definition & Data Section Message Header section replay_file.read((char *)&message_header, ULOG_MSG_HEADER_LEN); - res = readAndAddSubscription(replay_file, message_header.msg_size); - if (res == ReadAndAndAddSubResult::kFailure) { - PX4_ERR("Failed to read subscription"); - return; + if (!replay_file) { + break; } - } while (res != ReadAndAndAddSubResult::kSuccess); + if (message_header.msg_type == (int)ULogMessageType::ADD_LOGGED_MSG) { + readAndAddSubscription(replay_file, message_header.msg_size); + + } else if (message_header.msg_type == (int)ULogMessageType::DATA) { + // End of Definition & Data Section Message Header section + break; + + } else { + // Not important for now, skip + replay_file.seekg(message_header.msg_size, ios::cur); + } + } const uint64_t timestamp_offset = getTimestampOffset(); uint32_t nr_published_messages = 0; diff --git a/src/modules/rover_ackermann/CMakeLists.txt b/src/modules/rover_ackermann/CMakeLists.txt index 93269c29b9..9ad139d6e5 100644 --- a/src/modules/rover_ackermann/CMakeLists.txt +++ b/src/modules/rover_ackermann/CMakeLists.txt @@ -50,5 +50,3 @@ px4_add_module( MODULE_CONFIG module.yaml ) - -set_property(GLOBAL APPEND PROPERTY PX4_MODULE_CONFIG_FILES ${CMAKE_CURRENT_SOURCE_DIR}/../../lib/rover_control/module.yaml) diff --git a/src/modules/rover_ackermann/Kconfig b/src/modules/rover_ackermann/Kconfig index a6acd00251..bfcff4a895 100644 --- a/src/modules/rover_ackermann/Kconfig +++ b/src/modules/rover_ackermann/Kconfig @@ -2,4 +2,4 @@ menuconfig MODULES_ROVER_ACKERMANN bool "rover_ackermann" default n ---help--- - Enable support for rover_ackermann + Enable support for ackermann rovers diff --git a/src/modules/rover_differential/CMakeLists.txt b/src/modules/rover_differential/CMakeLists.txt index 8805893160..8fafc30bab 100644 --- a/src/modules/rover_differential/CMakeLists.txt +++ b/src/modules/rover_differential/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2024 PX4 Development Team. All rights reserved. +# Copyright (c) 2025 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,8 +31,9 @@ # ############################################################################ -add_subdirectory(RoverDifferentialGuidance) -add_subdirectory(RoverDifferentialControl) +add_subdirectory(DifferentialRateControl) +add_subdirectory(DifferentialAttControl) +add_subdirectory(DifferentialPosVelControl) px4_add_module( MODULE modules__rover_differential @@ -41,10 +42,11 @@ px4_add_module( RoverDifferential.cpp RoverDifferential.hpp DEPENDS - RoverDifferentialGuidance - RoverDifferentialControl + DifferentialRateControl + DifferentialAttControl + DifferentialPosVelControl px4_work_queue - pure_pursuit + rover_control MODULE_CONFIG module.yaml ) diff --git a/src/modules/rover_mecanum/RoverMecanumControl/CMakeLists.txt b/src/modules/rover_differential/DifferentialAttControl/CMakeLists.txt similarity index 85% rename from src/modules/rover_mecanum/RoverMecanumControl/CMakeLists.txt rename to src/modules/rover_differential/DifferentialAttControl/CMakeLists.txt index 61b64980f1..456b21489d 100644 --- a/src/modules/rover_mecanum/RoverMecanumControl/CMakeLists.txt +++ b/src/modules/rover_differential/DifferentialAttControl/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2024 PX4 Development Team. All rights reserved. +# Copyright (c) 2025 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,9 @@ # ############################################################################ -px4_add_library(RoverMecanumControl - RoverMecanumControl.cpp +px4_add_library(DifferentialAttControl + DifferentialAttControl.cpp ) -target_link_libraries(RoverMecanumControl PUBLIC PID) -target_include_directories(RoverMecanumControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(DifferentialAttControl PUBLIC PID) +target_include_directories(DifferentialAttControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp b/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp new file mode 100644 index 0000000000..a36ed41ada --- /dev/null +++ b/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp @@ -0,0 +1,208 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 "DifferentialAttControl.hpp" + +using namespace time_literals; + +DifferentialAttControl::DifferentialAttControl(ModuleParams *parent) : ModuleParams(parent) +{ + _rover_rate_setpoint_pub.advertise(); + _rover_throttle_setpoint_pub.advertise(); + _rover_attitude_setpoint_pub.advertise(); + _rover_attitude_status_pub.advertise(); + updateParams(); +} + +void DifferentialAttControl::updateParams() +{ + ModuleParams::updateParams(); + + if (_param_ro_yaw_rate_limit.get() > FLT_EPSILON) { + _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + } + + _pid_yaw.setGains(_param_ro_yaw_p.get(), 0.f, 0.f); + _pid_yaw.setIntegralLimit(_max_yaw_rate); + _pid_yaw.setOutputLimit(_max_yaw_rate); + _adjusted_yaw_setpoint.setSlewRate(_max_yaw_rate); +} + +void DifferentialAttControl::updateAttControl() +{ + const hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + if (_vehicle_control_mode_sub.updated()) { + _vehicle_control_mode_sub.copy(&_vehicle_control_mode); + } + + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude{}; + _vehicle_attitude_sub.copy(&vehicle_attitude); + matrix::Quatf vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q); + _vehicle_yaw = matrix::Eulerf(vehicle_attitude_quaternion).psi(); + } + + if (_vehicle_control_mode.flag_control_attitude_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) { + + if (_vehicle_control_mode.flag_control_manual_enabled || _vehicle_control_mode.flag_control_offboard_enabled) { + generateAttitudeSetpoint(); + } + + generateRateSetpoint(); + + } else { // Reset pid and slew rate when attitude control is not active + _pid_yaw.resetIntegral(); + _adjusted_yaw_setpoint.setForcedValue(0.f); + } + + // Publish attitude controller status (logging only) + rover_attitude_status_s rover_attitude_status; + rover_attitude_status.timestamp = _timestamp; + rover_attitude_status.measured_yaw = _vehicle_yaw; + rover_attitude_status.adjusted_yaw_setpoint = _adjusted_yaw_setpoint.getState(); + _rover_attitude_status_pub.publish(rover_attitude_status); + +} + +void DifferentialAttControl::generateAttitudeSetpoint() +{ + const bool stab_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled + && !_vehicle_control_mode.flag_control_position_enabled && _vehicle_control_mode.flag_control_attitude_enabled; + + if (stab_mode_enabled && _manual_control_setpoint_sub.updated()) { // Stab Mode + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = _timestamp; + rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + rover_throttle_setpoint.throttle_body_y = 0.f; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + + const float yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.roll, + _param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + + if (fabsf(yaw_rate_setpoint) > FLT_EPSILON + || fabsf(rover_throttle_setpoint.throttle_body_x) < FLT_EPSILON) { // Closed loop yaw rate control + _stab_yaw_ctl = false; + _adjusted_yaw_setpoint.setForcedValue(0.f); + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = yaw_rate_setpoint; + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + + } else { // Closed loop yaw control if the yaw rate input is zero (keep current yaw) + if (!_stab_yaw_ctl) { + _stab_yaw_setpoint = _vehicle_yaw; + _stab_yaw_ctl = true; + } + + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = _stab_yaw_setpoint; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + } + + + } + + } else if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard attitude control + trajectory_setpoint_s trajectory_setpoint{}; + _trajectory_setpoint_sub.copy(&trajectory_setpoint); + + if (_offboard_control_mode_sub.updated()) { + _offboard_control_mode_sub.copy(&_offboard_control_mode); + } + + const bool offboard_att_control = _offboard_control_mode.attitude && !_offboard_control_mode.position + && !_offboard_control_mode.velocity; + + if (offboard_att_control && PX4_ISFINITE(trajectory_setpoint.yaw)) { + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = trajectory_setpoint.yaw; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + } + } +} + +void DifferentialAttControl::generateRateSetpoint() +{ + if (_rover_attitude_setpoint_sub.updated()) { + _rover_attitude_setpoint_sub.copy(&_rover_attitude_setpoint); + } + + if (_rover_rate_setpoint_sub.updated()) { + _rover_rate_setpoint_sub.copy(&_rover_rate_setpoint); + } + + // Check if a new rate setpoint was already published from somewhere else + if (_rover_rate_setpoint.timestamp > _last_rate_setpoint_update + && _rover_rate_setpoint.timestamp > _rover_attitude_setpoint.timestamp) { + return; + } + + const float yaw_rate_setpoint = RoverControl::attitudeControl(_adjusted_yaw_setpoint, _pid_yaw, _max_yaw_rate, + _vehicle_yaw, _rover_attitude_setpoint.yaw_setpoint, _dt); + + _last_rate_setpoint_update = _timestamp; + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = math::constrain(yaw_rate_setpoint, -_max_yaw_rate, _max_yaw_rate); + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); +} + +bool DifferentialAttControl::runSanityChecks() +{ + bool ret = true; + + if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) { + ret = false; + } + + if (_param_ro_yaw_p.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("differential_att_control_conf_invalid_yaw_p"), events::Log::Error, + "Invalid configuration of necessary parameter RO_YAW_P", _param_ro_yaw_p.get()); + } + } + + _prev_param_check_passed = ret; + return ret; +} diff --git a/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.hpp b/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.hpp new file mode 100644 index 0000000000..e4198384c0 --- /dev/null +++ b/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.hpp @@ -0,0 +1,143 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 + +// Libraries +#include +#include +#include +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * @brief Class for differential attitude control. + */ +class DifferentialAttControl : public ModuleParams +{ +public: + /** + * @brief Constructor for DifferentialAttControl. + * @param parent The parent ModuleParams object. + */ + DifferentialAttControl(ModuleParams *parent); + ~DifferentialAttControl() = default; + + /** + * @brief Update attitude controller. + */ + void updateAttControl(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + /** + * @brief Generate and publish roverAttitudeSetpoint from manualControlSetpoint (Stab Mode) + * or trajectorySetpoint (Offboard attitude control). + */ + void generateAttitudeSetpoint(); + + /** + * @brief Generate and publish roverRateSetpoint from roverAttitudeSetpoint. + */ + void generateRateSetpoint(); + + /** + * @brief Check if the necessary parameters are set. + * @return True if all checks pass. + */ + bool runSanityChecks(); + + // uORB subscriptions + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; + uORB::Subscription _rover_attitude_setpoint_sub{ORB_ID(rover_attitude_setpoint)}; + uORB::Subscription _rover_rate_setpoint_sub{ORB_ID(rover_rate_setpoint)}; + vehicle_control_mode_s _vehicle_control_mode{}; + rover_attitude_setpoint_s _rover_attitude_setpoint{}; + rover_rate_setpoint_s _rover_rate_setpoint{}; + offboard_control_mode_s _offboard_control_mode{}; + + // uORB publications + uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; + uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; + uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; + uORB::Publication _rover_attitude_status_pub{ORB_ID(rover_attitude_status)}; + + // Variables + hrt_abstime _timestamp{0}; + hrt_abstime _last_rate_setpoint_update{0}; + float _vehicle_yaw{0.f}; + float _dt{0.f}; + float _max_yaw_rate{0.f}; + float _stab_yaw_setpoint{0.f}; // Yaw setpoint if rover is doing yaw control in stab mode + bool _stab_yaw_ctl{false}; // Indicates if rover is doing yaw control in stab mode + bool _prev_param_check_passed{true}; + + // Controllers + PID _pid_yaw; + SlewRateYaw _adjusted_yaw_setpoint; + + // Parameters + DEFINE_PARAMETERS( + (ParamFloat) _param_ro_yaw_rate_limit, + (ParamFloat) _param_ro_yaw_p, + (ParamFloat) _param_ro_yaw_stick_dz + ) +}; diff --git a/src/modules/rover_differential/RoverDifferentialControl/CMakeLists.txt b/src/modules/rover_differential/DifferentialPosVelControl/CMakeLists.txt similarity index 85% rename from src/modules/rover_differential/RoverDifferentialControl/CMakeLists.txt rename to src/modules/rover_differential/DifferentialPosVelControl/CMakeLists.txt index 7464de4f5b..a4a4795be7 100644 --- a/src/modules/rover_differential/RoverDifferentialControl/CMakeLists.txt +++ b/src/modules/rover_differential/DifferentialPosVelControl/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2024 PX4 Development Team. All rights reserved. +# Copyright (c) 2025 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,9 @@ # ############################################################################ -px4_add_library(RoverDifferentialControl - RoverDifferentialControl.cpp +px4_add_library(DifferentialPosVelControl + DifferentialPosVelControl.cpp ) -target_link_libraries(RoverDifferentialControl PUBLIC PID) -target_include_directories(RoverDifferentialControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(DifferentialPosVelControl PUBLIC PID) +target_include_directories(DifferentialPosVelControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.cpp b/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.cpp new file mode 100644 index 0000000000..32532f4c56 --- /dev/null +++ b/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.cpp @@ -0,0 +1,406 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 "DifferentialPosVelControl.hpp" + +using namespace time_literals; + +DifferentialPosVelControl::DifferentialPosVelControl(ModuleParams *parent) : ModuleParams(parent) +{ + _rover_rate_setpoint_pub.advertise(); + _rover_throttle_setpoint_pub.advertise(); + _rover_attitude_setpoint_pub.advertise(); + _rover_velocity_status_pub.advertise(); + updateParams(); +} + +void DifferentialPosVelControl::updateParams() +{ + ModuleParams::updateParams(); + _pid_speed.setGains(_param_ro_speed_p.get(), _param_ro_speed_i.get(), 0.f); + _pid_speed.setIntegralLimit(1.f); + _pid_speed.setOutputLimit(1.f); + _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + + if (_param_ro_accel_limit.get() > FLT_EPSILON) { + _speed_setpoint.setSlewRate(_param_ro_accel_limit.get()); + } +} + +void DifferentialPosVelControl::updatePosVelControl() +{ + const hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + updateSubscriptions(); + + if ((_vehicle_control_mode.flag_control_position_enabled || _vehicle_control_mode.flag_control_velocity_enabled) + && _vehicle_control_mode.flag_armed && runSanityChecks()) { + generateAttitudeSetpoint(); + + if (_param_ro_max_thr_speed.get() > FLT_EPSILON) { + + const float speed_body_x_setpoint_normalized = math::interpolate(_speed_body_x_setpoint, + -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f); + + if (_rover_steering_setpoint_sub.updated()) { + _rover_steering_setpoint_sub.copy(&_rover_steering_setpoint); + } + + if (fabsf(speed_body_x_setpoint_normalized) > 1.f - + fabsf(_rover_steering_setpoint.normalized_speed_diff)) { // Adjust speed setpoint if it is infeasible due to the desired speed difference of the left/right wheels + _speed_body_x_setpoint = sign(_speed_body_x_setpoint) * + math::interpolate(1.f - fabsf(_rover_steering_setpoint.normalized_speed_diff), -1.f, 1.f, + -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get()); + } + } + + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = _timestamp; + _speed_body_x_setpoint = fabsf(_speed_body_x_setpoint) > _param_ro_speed_th.get() ? _speed_body_x_setpoint : 0.f; + rover_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_speed_setpoint, _pid_speed, + _speed_body_x_setpoint, _vehicle_speed_body_x, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(), + _param_ro_max_thr_speed.get(), _dt); + rover_throttle_setpoint.throttle_body_y = 0.f; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + + } else { // Reset controller and slew rate when position control is not active + _pid_speed.resetIntegral(); + _speed_setpoint.setForcedValue(0.f); + } + + // Publish position controller status (logging only) + rover_velocity_status_s rover_velocity_status; + rover_velocity_status.timestamp = _timestamp; + rover_velocity_status.measured_speed_body_x = _vehicle_speed_body_x; + rover_velocity_status.speed_body_x_setpoint = _speed_body_x_setpoint; + rover_velocity_status.adjusted_speed_body_x_setpoint = _speed_setpoint.getState(); + rover_velocity_status.measured_speed_body_y = _vehicle_speed_body_y; + rover_velocity_status.speed_body_y_setpoint = NAN; + rover_velocity_status.adjusted_speed_body_y_setpoint = NAN; + rover_velocity_status.pid_throttle_body_x_integral = _pid_speed.getIntegral(); + rover_velocity_status.pid_throttle_body_y_integral = NAN; + _rover_velocity_status_pub.publish(rover_velocity_status); +} + +void DifferentialPosVelControl::updateSubscriptions() +{ + if (_vehicle_control_mode_sub.updated()) { + _vehicle_control_mode_sub.copy(&_vehicle_control_mode); + } + + 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); + + if (!_global_ned_proj_ref.isInitialized() + || (_global_ned_proj_ref.getProjectionReferenceTimestamp() != vehicle_local_position.ref_timestamp)) { + _global_ned_proj_ref.initReference(vehicle_local_position.ref_lat, vehicle_local_position.ref_lon, + vehicle_local_position.ref_timestamp); + } + + _curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y); + const Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); + const Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame); + _vehicle_speed_body_x = fabsf(velocity_in_body_frame(0)) > _param_ro_speed_th.get() ? velocity_in_body_frame(0) : 0.f; + _vehicle_speed_body_y = fabsf(velocity_in_body_frame(1)) > _param_ro_speed_th.get() ? velocity_in_body_frame(1) : 0.f; + } + + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status; + _vehicle_status_sub.copy(&vehicle_status); + _nav_state = vehicle_status.nav_state; + } + +} + +void DifferentialPosVelControl::generateAttitudeSetpoint() +{ + if (_vehicle_control_mode.flag_control_manual_enabled + && _vehicle_control_mode.flag_control_position_enabled) { // Position Mode + manualPositionMode(); + + } else if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard Control + if (_offboard_control_mode_sub.updated()) { + _offboard_control_mode_sub.copy(&_offboard_control_mode); + } + + if (_offboard_control_mode.position) { + offboardPositionMode(); + + } else if (_offboard_control_mode.velocity) { + offboardVelocityMode(); + } + + } else if (_vehicle_control_mode.flag_control_auto_enabled) { // Auto Mode + autoPositionMode(); + } +} + +void DifferentialPosVelControl::manualPositionMode() +{ + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + _speed_body_x_setpoint = math::interpolate(manual_control_setpoint.throttle, + -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get()); + const float yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.roll, + _param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + + if (fabsf(yaw_rate_setpoint) > FLT_EPSILON + || fabsf(_speed_body_x_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control + _course_control = false; + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = yaw_rate_setpoint; + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + + } else { // Course control if the steering input is zero (keep driving on a straight line) + if (!_course_control) { + _pos_ctl_course_direction = Vector2f(cos(_vehicle_yaw), sin(_vehicle_yaw)); + _pos_ctl_start_position_ned = _curr_pos_ned; + _course_control = true; + } + + // Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover + const float vector_scaling = sqrtf(powf(_param_pp_lookahd_max.get(), + 2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceOnLineSegment(); + const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign(_speed_body_x_setpoint) * + vector_scaling * _pos_ctl_course_direction; + float yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _pos_ctl_start_position_ned, + _curr_pos_ned, fabsf(_speed_body_x_setpoint)); + yaw_setpoint = _speed_body_x_setpoint > FLT_EPSILON ? yaw_setpoint : matrix::wrap_pi(yaw_setpoint + M_PI_F); + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = yaw_setpoint; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + } + } +} + +void DifferentialPosVelControl::offboardPositionMode() +{ + trajectory_setpoint_s trajectory_setpoint{}; + _trajectory_setpoint_sub.copy(&trajectory_setpoint); + + // Translate trajectory setpoint to rover setpoints + const Vector2f target_waypoint_ned(trajectory_setpoint.position[0], trajectory_setpoint.position[1]); + const float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm(); + + if (target_waypoint_ned.isAllFinite() && distance_to_target > _param_nav_acc_rad.get()) { + const float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(), + _param_ro_decel_limit.get(), distance_to_target, 0.f); + _speed_body_x_setpoint = math::min(speed_setpoint, _param_ro_speed_limit.get()); + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _curr_pos_ned, + _curr_pos_ned, fabsf(_speed_body_x_setpoint)); + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + + } else { + _speed_body_x_setpoint = 0.f; + } +} + +void DifferentialPosVelControl::offboardVelocityMode() +{ + trajectory_setpoint_s trajectory_setpoint{}; + _trajectory_setpoint_sub.copy(&trajectory_setpoint); + + const Vector2f velocity_in_local_frame(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]); + + if (velocity_in_local_frame.isAllFinite()) { + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = atan2f(velocity_in_local_frame(1), velocity_in_local_frame(0)); + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + _speed_body_x_setpoint = velocity_in_local_frame.norm(); + } +} + +void DifferentialPosVelControl::autoPositionMode() +{ + updateAutoSubscriptions(); + + // Distances to waypoints + const float distance_to_curr_wp = sqrt(powf(_curr_pos_ned(0) - _curr_wp_ned(0), + 2) + powf(_curr_pos_ned(1) - _curr_wp_ned(1), 2)); + + if (_nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { // Check RTL arrival + _mission_finished = distance_to_curr_wp < _param_nav_acc_rad.get(); + } + + // State machine + float yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, + fabsf(_vehicle_speed_body_x)); + const float heading_error = matrix::wrap_pi(yaw_setpoint - _vehicle_yaw); + + if (!_mission_finished && distance_to_curr_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()) { + _currentState = GuidanceState::SPOT_TURNING; + + } else if (_currentState == GuidanceState::SPOT_TURNING && fabsf(heading_error) < _param_rd_trans_trn_drv.get()) { + _currentState = GuidanceState::DRIVING; + } + + } else { // Mission finished or delay command + _currentState = GuidanceState::STOPPED; + } + + // Guidance logic + switch (_currentState) { + case GuidanceState::DRIVING: { + // Calculate desired speed in body x direction + _speed_body_x_setpoint = calcSpeedSetpoint(_cruising_speed, distance_to_curr_wp, _param_ro_decel_limit.get(), + _param_ro_jerk_limit.get(), _waypoint_transition_angle, _param_ro_speed_limit.get(), _param_rd_trans_drv_trn.get(), + _param_rd_miss_spd_gain.get()); + + } break; + + case GuidanceState::SPOT_TURNING: + if (fabsf(_vehicle_speed_body_x) > 0.f) { + yaw_setpoint = _vehicle_yaw; // Wait for the rover to stop + + } + + _speed_body_x_setpoint = 0.f; + break; + + case GuidanceState::STOPPED: + default: + yaw_setpoint = _vehicle_yaw; + _speed_body_x_setpoint = 0.f; + break; + + } + + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = yaw_setpoint; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); +} + +void DifferentialPosVelControl::updateAutoSubscriptions() +{ + 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); + } + + if (_position_setpoint_triplet_sub.updated()) { + position_setpoint_triplet_s position_setpoint_triplet{}; + _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); + + RoverControl::globalToLocalSetpointTriplet(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, position_setpoint_triplet, + _curr_pos_ned, _home_position, _global_ned_proj_ref); + + _waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(_prev_wp_ned, _curr_wp_ned, _next_wp_ned); + + // Waypoint cruising speed + _cruising_speed = position_setpoint_triplet.current.cruising_speed > 0.f ? math::constrain( + position_setpoint_triplet.current.cruising_speed, 0.f, _param_ro_speed_limit.get()) : _param_ro_speed_limit.get(); + } + + if (_mission_result_sub.updated()) { + mission_result_s mission_result{}; + _mission_result_sub.copy(&mission_result); + _mission_finished = mission_result.finished; + } +} + +float DifferentialPosVelControl::calcSpeedSetpoint(const float cruising_speed, const float distance_to_curr_wp, + const float max_decel, const float max_jerk, const float waypoint_transition_angle, const float max_speed, + const float trans_drv_trn, const float miss_spd_gain) +{ + float speed_body_x_setpoint = cruising_speed; + + if (_waypoint_transition_angle < M_PI_F - trans_drv_trn && max_jerk > FLT_EPSILON && max_decel > FLT_EPSILON) { + speed_body_x_setpoint = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_decel, distance_to_curr_wp, 0.0f); + + } else if (_waypoint_transition_angle >= M_PI_F - trans_drv_trn && max_jerk > FLT_EPSILON && max_decel > FLT_EPSILON + && miss_spd_gain > FLT_EPSILON) { + const float speed_reduction = math::constrain(miss_spd_gain * math::interpolate(M_PI_F - _waypoint_transition_angle, + 0.f, M_PI_F, 0.f, 1.f), 0.f, 1.f); + speed_body_x_setpoint = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_decel, distance_to_curr_wp, + max_speed * (1.f - speed_reduction)); + } + + return math::constrain(speed_body_x_setpoint, -cruising_speed, cruising_speed); + +} + +bool DifferentialPosVelControl::runSanityChecks() +{ + bool ret = true; + + if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) { + ret = false; + } + + if (_param_ro_speed_limit.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("differential_posVel_control_conf_invalid_speed_lim"), events::Log::Error, + "Invalid configuration of necessary parameter RO_SPEED_LIM", _param_ro_speed_limit.get()); + } + + } + + if (_param_ro_max_thr_speed.get() < FLT_EPSILON && _param_ro_speed_p.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("differential_posVel_control_conf_invalid_speed_control"), events::Log::Error, + "Invalid configuration for speed control: Neither feed forward (RO_MAX_THR_SPEED) nor feedback (RO_SPEED_P) is setup", + _param_ro_max_thr_speed.get(), + _param_ro_speed_p.get()); + } + } + + _prev_param_check_passed = ret; + return ret; +} diff --git a/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.hpp b/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.hpp new file mode 100644 index 0000000000..0ae8a24eae --- /dev/null +++ b/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.hpp @@ -0,0 +1,243 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 + +// Libraries +#include +#include +#include +#include +#include +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#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 position/velocity control. + */ +class DifferentialPosVelControl : public ModuleParams +{ +public: + /** + * @brief Constructor for DifferentialPosVelControl. + * @param parent The parent ModuleParams object. + */ + DifferentialPosVelControl(ModuleParams *parent); + ~DifferentialPosVelControl() = default; + + /** + * @brief Update position controller. + */ + void updatePosVelControl(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + /** + * @brief Update uORB subscriptions used in position controller. + */ + void updateSubscriptions(); + + /** + * @brief Generate and publish roverAttitudeSetpoint from manualControlSetpoint (Position Mode) or + * trajcetorySetpoint (Offboard position or velocity control) or + * positionSetpointTriplet (Auto Mode). + */ + void generateAttitudeSetpoint(); + + /** + * @brief Generate and publish roverThrottleSetpoint and roverAttitudeSetpoint or roverRateSetpoint from manualControlSetpoint. + */ + void manualPositionMode(); + + /** + * @brief Generate and publish roverAttitudeSetpoint from position of trajectorySetpoint. + */ + void offboardPositionMode(); + + /** + * @brief Generate and publish roverAttitudeSetpoint from velocity of trajectorySetpoint. + */ + void offboardVelocityMode(); + + /** + * @brief Generate and publish roverThrottleSetpoint and roverAttitudeSetpoint from positionSetpointTriplet. + */ + void autoPositionMode(); + + /** + * @brief Update uORB subscriptions used for auto modes. + */ + void updateAutoSubscriptions(); + + /** + * @brief Calculate the speed setpoint. During waypoint transition the speed is restricted to + * Maximum_speed * (1 - normalized_transition_angle * RM_MISS_VEL_GAIN). + * On straight lines it is based on a speed trajectory such that the rover will arrive at the next waypoint transition + * with the desired waypoiny transition speed under consideration of the maximum deceleration and jerk. + * @param cruising_speed Cruising speed [m/s]. + * @param distance_to_curr_wp Distance to the current waypoint [m]. + * @param max_decel Maximum allowed deceleration [m/s^2]. + * @param max_jerk Maximum allowed jerk [m/s^3]. + * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad] + * @param max_speed Maximum speed setpoint [m/s] + * @param trans_drv_trn Heading error threshold to switch from driving to turning [rad]. + * @param miss_spd_gain Tuning parameter for the speed reduction during waypoint transition. + * @return Speed setpoint [m/s]. + */ + float calcSpeedSetpoint(float cruising_speed, float distance_to_curr_wp, float max_decel, float max_jerk, + float waypoint_transition_angle, float max_speed, float trans_drv_trn, float miss_spd_gain); + + + /** + * @brief Check if the necessary parameters are set. + * @return True if all checks pass. + */ + bool runSanityChecks(); + + // uORB subscriptions + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _mission_result_sub{ORB_ID(mission_result)}; + uORB::Subscription _home_position_sub{ORB_ID(home_position)}; + uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)}; + vehicle_control_mode_s _vehicle_control_mode{}; + offboard_control_mode_s _offboard_control_mode{}; + rover_steering_setpoint_s _rover_steering_setpoint{}; + + // uORB publications + uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; + uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; + uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; + uORB::Publication _rover_velocity_status_pub{ORB_ID(rover_velocity_status)}; + uORB::Publication _position_controller_status_pub{ORB_ID(position_controller_status)}; + + // Variables + hrt_abstime _timestamp{0}; + Quatf _vehicle_attitude_quaternion{}; + Vector2f _curr_pos_ned{}; + Vector2f _pos_ctl_course_direction{}; + Vector2f _pos_ctl_start_position_ned{}; + float _vehicle_speed_body_x{0.f}; + float _vehicle_speed_body_y{0.f}; + float _vehicle_yaw{0.f}; + float _max_yaw_rate{0.f}; + float _speed_body_x_setpoint{0.f}; + float _dt{0.f}; + int _nav_state{0}; + bool _course_control{false}; // Indicates if the rover is doing course control in manual position mode. + bool _mission_finished{false}; + + // Waypoint variables + Vector2d _home_position{}; + Vector2f _curr_wp_ned{}; + Vector2f _prev_wp_ned{}; + Vector2f _next_wp_ned{}; + float _cruising_speed{0.f}; + float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad] + bool _prev_param_check_passed{true}; + + // Controllers + PID _pid_speed; + SlewRate _speed_setpoint; + + // Class Instances + PurePursuit _posctl_pure_pursuit{this}; // Pure pursuit library + MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates + GuidanceState _currentState{GuidanceState::DRIVING}; // The current state of the guidance. + + DEFINE_PARAMETERS( + (ParamFloat) _param_rd_trans_trn_drv, + (ParamFloat) _param_rd_trans_drv_trn, + (ParamFloat) _param_rd_miss_spd_gain, + (ParamFloat) _param_ro_max_thr_speed, + (ParamFloat) _param_ro_speed_p, + (ParamFloat) _param_ro_speed_i, + (ParamFloat) _param_ro_yaw_stick_dz, + (ParamFloat) _param_ro_accel_limit, + (ParamFloat) _param_ro_decel_limit, + (ParamFloat) _param_ro_jerk_limit, + (ParamFloat) _param_ro_speed_limit, + (ParamFloat) _param_ro_speed_th, + (ParamFloat) _param_pp_lookahd_max, + (ParamFloat) _param_ro_yaw_rate_limit, + (ParamFloat) _param_nav_acc_rad + + ) +}; diff --git a/src/modules/rover_differential/DifferentialRateControl/CMakeLists.txt b/src/modules/rover_differential/DifferentialRateControl/CMakeLists.txt new file mode 100644 index 0000000000..0182e89d30 --- /dev/null +++ b/src/modules/rover_differential/DifferentialRateControl/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2025 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. +# +############################################################################ + +px4_add_library(DifferentialRateControl + DifferentialRateControl.cpp +) + +target_link_libraries(DifferentialRateControl PUBLIC PID) +target_include_directories(DifferentialRateControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp new file mode 100644 index 0000000000..fe62328cd9 --- /dev/null +++ b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp @@ -0,0 +1,190 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 "DifferentialRateControl.hpp" + +using namespace time_literals; + +DifferentialRateControl::DifferentialRateControl(ModuleParams *parent) : ModuleParams(parent) +{ + _rover_rate_setpoint_pub.advertise(); + _rover_throttle_setpoint_pub.advertise(); + _rover_steering_setpoint_pub.advertise(); + _rover_rate_status_pub.advertise(); + updateParams(); +} + +void DifferentialRateControl::updateParams() +{ + ModuleParams::updateParams(); + _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + _max_yaw_accel = _param_ro_yaw_accel_limit.get() * M_DEG_TO_RAD_F; + _max_yaw_decel = _param_ro_yaw_decel_limit.get() * M_DEG_TO_RAD_F; + _pid_yaw_rate.setGains(_param_ro_yaw_rate_p.get(), _param_ro_yaw_rate_i.get(), 0.f); + _pid_yaw_rate.setIntegralLimit(1.f); + _pid_yaw_rate.setOutputLimit(1.f); + _adjusted_yaw_rate_setpoint.setSlewRate(_max_yaw_accel); +} + +void DifferentialRateControl::updateRateControl() +{ + const hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + if (_vehicle_control_mode_sub.updated()) { + _vehicle_control_mode_sub.copy(&_vehicle_control_mode); + } + + if (_vehicle_angular_velocity_sub.updated()) { + vehicle_angular_velocity_s vehicle_angular_velocity{}; + _vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity); + _vehicle_yaw_rate = fabsf(vehicle_angular_velocity.xyz[2]) > _param_ro_yaw_rate_th.get() * M_DEG_TO_RAD_F ? + vehicle_angular_velocity.xyz[2] : 0.f; + } + + if (_vehicle_control_mode.flag_control_rates_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) { + if (_vehicle_control_mode.flag_control_manual_enabled || _vehicle_control_mode.flag_control_offboard_enabled) { + generateRateSetpoint(); + } + + generateSteeringSetpoint(); + + } else { // Reset controller and slew rate when rate control is not active + _pid_yaw_rate.resetIntegral(); + _adjusted_yaw_rate_setpoint.setForcedValue(0.f); + } + + // Publish rate controller status (logging only) + rover_rate_status_s rover_rate_status; + rover_rate_status.timestamp = _timestamp; + rover_rate_status.measured_yaw_rate = _vehicle_yaw_rate; + rover_rate_status.adjusted_yaw_rate_setpoint = _adjusted_yaw_rate_setpoint.getState(); + rover_rate_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral(); + _rover_rate_status_pub.publish(rover_rate_status); + +} + +void DifferentialRateControl::generateRateSetpoint() +{ + const bool acro_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled + && !_vehicle_control_mode.flag_control_position_enabled && !_vehicle_control_mode.flag_control_attitude_enabled; + + if (acro_mode_enabled && _manual_control_setpoint_sub.updated()) { // Acro Mode + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = _timestamp; + rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + rover_throttle_setpoint.throttle_body_y = 0.f; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = math::interpolate (manual_control_setpoint.roll, -1.f, 1.f, + -_max_yaw_rate, _max_yaw_rate); + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + } + + } else if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard rate control + trajectory_setpoint_s trajectory_setpoint{}; + _trajectory_setpoint_sub.copy(&trajectory_setpoint); + + if (_offboard_control_mode_sub.updated()) { + _offboard_control_mode_sub.copy(&_offboard_control_mode); + } + + const bool offboard_rate_control = _offboard_control_mode.body_rate && !_offboard_control_mode.position + && !_offboard_control_mode.velocity && !_offboard_control_mode.attitude; + + if (offboard_rate_control && PX4_ISFINITE(trajectory_setpoint.yawspeed)) { + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = trajectory_setpoint.yawspeed; + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + } + } +} + +void DifferentialRateControl::generateSteeringSetpoint() +{ + if (_rover_rate_setpoint_sub.updated()) { + _rover_rate_setpoint_sub.copy(&_rover_rate_setpoint); + + } + + float speed_diff_normalized{0.f}; + + if (PX4_ISFINITE(_rover_rate_setpoint.yaw_rate_setpoint) && PX4_ISFINITE(_vehicle_yaw_rate)) { + const float yaw_rate_setpoint = fabsf(_rover_rate_setpoint.yaw_rate_setpoint) > _param_ro_yaw_rate_th.get() * + M_DEG_TO_RAD_F ? + _rover_rate_setpoint.yaw_rate_setpoint : 0.f; + speed_diff_normalized = RoverControl::rateControl(_adjusted_yaw_rate_setpoint, _pid_yaw_rate, + yaw_rate_setpoint, _vehicle_yaw_rate, _param_rd_max_thr_yaw_r.get(), _max_yaw_accel, + _max_yaw_decel, _param_rd_wheel_track.get(), _dt); + } + + rover_steering_setpoint_s rover_steering_setpoint{}; + rover_steering_setpoint.timestamp = _timestamp; + rover_steering_setpoint.normalized_speed_diff = speed_diff_normalized; + _rover_steering_setpoint_pub.publish(rover_steering_setpoint); +} + +bool DifferentialRateControl::runSanityChecks() +{ + bool ret = true; + + if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("differential_rate_control_conf_invalid_yaw_rate_lim"), events::Log::Error, + "Invalid configuration of necessary parameter RO_YAW_RATE_LIM", _param_ro_yaw_rate_limit.get()); + } + + } + + if ((_param_rd_wheel_track.get() < FLT_EPSILON || _param_rd_max_thr_yaw_r.get() < FLT_EPSILON) + && _param_ro_yaw_rate_p.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("differential_rate_control_conf_invalid_rate_control"), events::Log::Error, + "Invalid configuration for rate control: Neither feed forward nor feedback is setup", _param_rd_wheel_track.get(), + _param_rd_max_thr_yaw_r.get(), _param_ro_yaw_rate_p.get()); + } + } + + _prev_param_check_passed = ret; + return ret; +} diff --git a/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp new file mode 100644 index 0000000000..5b9bd6d632 --- /dev/null +++ b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp @@ -0,0 +1,144 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 + +// Libraries +#include +#include +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * @brief Class for differential rate control. + */ +class DifferentialRateControl : public ModuleParams +{ +public: + /** + * @brief Constructor for DifferentialRateControl. + * @param parent The parent ModuleParams object. + */ + DifferentialRateControl(ModuleParams *parent); + ~DifferentialRateControl() = default; + + /** + * @brief Update rate controller. + */ + void updateRateControl(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + + /** + * @brief Generate and publish roverRateSetpoint from manualControlSetpoint (Acro Mode) + * or trajectorySetpoint (Offboard rate control). + */ + void generateRateSetpoint(); + + /** + * @brief Generate and publish roverSteeringSetpoint from RoverRateSetpoint. + */ + void generateSteeringSetpoint(); + + /** + * @brief Check if the necessary parameters are set. + * @return True if all checks pass. + */ + bool runSanityChecks(); + + // uORB subscriptions + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; + uORB::Subscription _rover_rate_setpoint_sub{ORB_ID(rover_rate_setpoint)}; + uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; + uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; + vehicle_control_mode_s _vehicle_control_mode{}; + offboard_control_mode_s _offboard_control_mode{}; + rover_rate_setpoint_s _rover_rate_setpoint{}; + + // uORB publications + uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; + uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; + uORB::Publication _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)}; + uORB::Publication _rover_rate_status_pub{ORB_ID(rover_rate_status)}; + + // Variables + hrt_abstime _timestamp{0}; + float _max_yaw_rate{0.f}; + float _max_yaw_accel{0.f}; + float _max_yaw_decel{0.f}; + float _vehicle_yaw_rate{0.f}; + float _dt{0.f}; // Time since last update [s]. + bool _prev_param_check_passed{true}; + + // Controllers + PID _pid_yaw_rate; + SlewRate _adjusted_yaw_rate_setpoint{0.f}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_rd_wheel_track, + (ParamFloat) _param_rd_max_thr_yaw_r, + (ParamFloat) _param_ro_yaw_rate_limit, + (ParamFloat) _param_ro_yaw_rate_th, + (ParamFloat) _param_ro_yaw_rate_p, + (ParamFloat) _param_ro_yaw_rate_i, + (ParamFloat) _param_ro_yaw_accel_limit, + (ParamFloat) _param_ro_yaw_decel_limit + ) +}; diff --git a/src/modules/rover_differential/Kconfig b/src/modules/rover_differential/Kconfig index 840e2cdbf9..ed62f619b7 100644 --- a/src/modules/rover_differential/Kconfig +++ b/src/modules/rover_differential/Kconfig @@ -1,6 +1,5 @@ menuconfig MODULES_ROVER_DIFFERENTIAL bool "rover_differential" default n - depends on MODULES_CONTROL_ALLOCATOR ---help--- - Enable support for control of differential rovers + Enable support for differential rovers diff --git a/src/modules/rover_differential/RoverDifferential.cpp b/src/modules/rover_differential/RoverDifferential.cpp index c5fe294cd3..5ad4439597 100644 --- a/src/modules/rover_differential/RoverDifferential.cpp +++ b/src/modules/rover_differential/RoverDifferential.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * Copyright (c) 2025 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,12 +33,15 @@ #include "RoverDifferential.hpp" +using namespace time_literals; + RoverDifferential::RoverDifferential() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) { + _rover_throttle_setpoint_pub.advertise(); + _rover_steering_setpoint_pub.advertise(); updateParams(); - _rover_differential_setpoint_pub.advertise(); } bool RoverDifferential::init() @@ -50,219 +53,103 @@ bool RoverDifferential::init() void RoverDifferential::updateParams() { ModuleParams::updateParams(); - _max_yaw_rate = _param_rd_max_yaw_rate.get() * M_DEG_TO_RAD_F; + + if (_param_ro_accel_limit.get() > FLT_EPSILON && _param_ro_max_thr_speed.get() > FLT_EPSILON) { + _throttle_body_x_setpoint.setSlewRate(_param_ro_accel_limit.get() / _param_ro_max_thr_speed.get()); + } } void RoverDifferential::Run() { - if (should_exit()) { - ScheduleClear(); - exit_and_cleanup(); - return; - } - - updateSubscriptions(); - - // Generate and publish attitude, rate and speed setpoints - hrt_abstime timestamp = hrt_absolute_time(); - - 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)) { - rover_differential_setpoint_s rover_differential_setpoint{}; - rover_differential_setpoint.timestamp = timestamp; - rover_differential_setpoint.forward_speed_setpoint = NAN; - rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle; - rover_differential_setpoint.yaw_setpoint = NAN; - - if (_max_yaw_rate > FLT_EPSILON && _param_rd_max_thr_yaw_r.get() > FLT_EPSILON) { - const float scaled_yaw_rate_input = math::interpolate(manual_control_setpoint.roll, - -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); - const float speed_diff = scaled_yaw_rate_input * _param_rd_wheel_track.get() / 2.f; - rover_differential_setpoint.speed_diff_setpoint_normalized = math::interpolate(speed_diff, - -_param_rd_max_thr_yaw_r.get(), _param_rd_max_thr_yaw_r.get(), -1.f, 1.f); - - } else { - rover_differential_setpoint.speed_diff_setpoint_normalized = manual_control_setpoint.roll; - - } - - rover_differential_setpoint.yaw_rate_setpoint = NAN; - _rover_differential_setpoint_pub.publish(rover_differential_setpoint); - } - } break; - - case vehicle_status_s::NAVIGATION_STATE_ACRO: { - manual_control_setpoint_s manual_control_setpoint{}; - - if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - rover_differential_setpoint_s rover_differential_setpoint{}; - rover_differential_setpoint.timestamp = timestamp; - rover_differential_setpoint.forward_speed_setpoint = NAN; - rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle; - rover_differential_setpoint.yaw_rate_setpoint = math::interpolate(manual_control_setpoint.roll, - -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); - rover_differential_setpoint.speed_diff_setpoint_normalized = NAN; - rover_differential_setpoint.yaw_setpoint = NAN; - _rover_differential_setpoint_pub.publish(rover_differential_setpoint); - } - - } break; - - case vehicle_status_s::NAVIGATION_STATE_STAB: { - manual_control_setpoint_s manual_control_setpoint{}; - - if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - rover_differential_setpoint_s rover_differential_setpoint{}; - rover_differential_setpoint.timestamp = timestamp; - rover_differential_setpoint.forward_speed_setpoint = NAN; - rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle; - rover_differential_setpoint.yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.roll, - STICK_DEADZONE), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); - rover_differential_setpoint.speed_diff_setpoint_normalized = NAN; - rover_differential_setpoint.yaw_setpoint = NAN; - - if (fabsf(rover_differential_setpoint.yaw_rate_setpoint) > FLT_EPSILON - || fabsf(rover_differential_setpoint.forward_speed_setpoint_normalized) < FLT_EPSILON) { // Closed loop yaw rate control - _yaw_ctl = false; - - - } else { // Closed loop yaw control if the yaw rate input is zero (keep current yaw) - if (!_yaw_ctl) { - _stab_desired_yaw = _vehicle_yaw; - _yaw_ctl = true; - } - - rover_differential_setpoint.yaw_setpoint = _stab_desired_yaw; - rover_differential_setpoint.yaw_rate_setpoint = NAN; - - } - - _rover_differential_setpoint_pub.publish(rover_differential_setpoint); - } - - } break; - - case vehicle_status_s::NAVIGATION_STATE_POSCTL: { - manual_control_setpoint_s manual_control_setpoint{}; - - if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - rover_differential_setpoint_s rover_differential_setpoint{}; - rover_differential_setpoint.timestamp = timestamp; - rover_differential_setpoint.forward_speed_setpoint = math::interpolate(manual_control_setpoint.throttle, - -1.f, 1.f, -_param_rd_max_speed.get(), _param_rd_max_speed.get()); - rover_differential_setpoint.forward_speed_setpoint_normalized = NAN; - rover_differential_setpoint.yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.roll, - STICK_DEADZONE), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); - rover_differential_setpoint.speed_diff_setpoint_normalized = NAN; - rover_differential_setpoint.yaw_setpoint = NAN; - - if (fabsf(rover_differential_setpoint.yaw_rate_setpoint) > FLT_EPSILON - || fabsf(rover_differential_setpoint.forward_speed_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control - _yaw_ctl = false; - - - } else { // Course control if the yaw rate input is zero (keep driving on a straight line) - if (!_yaw_ctl) { - _pos_ctl_course_direction = Vector2f(cos(_vehicle_yaw), sin(_vehicle_yaw)); - _pos_ctl_start_position_ned = _curr_pos_ned; - _yaw_ctl = true; - } - - // Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover - const float vector_scaling = sqrtf(powf(_param_pp_lookahd_max.get(), - 2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceOnLineSegment(); - const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign( - rover_differential_setpoint.forward_speed_setpoint) * - vector_scaling * _pos_ctl_course_direction; - // Calculate yaw setpoint - const float yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, - _pos_ctl_start_position_ned, _curr_pos_ned, fabsf(_vehicle_forward_speed)); - rover_differential_setpoint.yaw_setpoint = sign(rover_differential_setpoint.forward_speed_setpoint) >= 0 ? - yaw_setpoint : matrix::wrap_pi(M_PI_F + yaw_setpoint); // Flip yaw setpoint when driving backwards - rover_differential_setpoint.yaw_rate_setpoint = NAN; - - } - - _rover_differential_setpoint_pub.publish(rover_differential_setpoint); - } - - } break; - - case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: - case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: - _rover_differential_guidance.computeGuidance(_vehicle_yaw, _vehicle_forward_speed, _nav_state); - break; - - default: // Unimplemented nav states will stop the rover - _rover_differential_control.resetControllers(); - _yaw_ctl = false; - rover_differential_setpoint_s rover_differential_setpoint{}; - rover_differential_setpoint.forward_speed_setpoint = NAN; - rover_differential_setpoint.forward_speed_setpoint_normalized = 0.f; - rover_differential_setpoint.yaw_rate_setpoint = NAN; - rover_differential_setpoint.speed_diff_setpoint_normalized = 0.f; - rover_differential_setpoint.yaw_setpoint = NAN; - _rover_differential_setpoint_pub.publish(rover_differential_setpoint); - break; - } - - if (!_armed) { // Reset when disarmed - _rover_differential_control.resetControllers(); - _yaw_ctl = false; - } - - _rover_differential_control.computeMotorCommands(_vehicle_yaw, _vehicle_yaw_rate, _vehicle_forward_speed); - -} - -void RoverDifferential::updateSubscriptions() -{ - 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); + const hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; - if (vehicle_status.nav_state != _nav_state) { // Reset on mode change - _rover_differential_control.resetControllers(); - _yaw_ctl = false; - } + _differential_pos_vel_control.updatePosVelControl(); + _differential_att_control.updateAttControl(); + _differential_rate_control.updateRateControl(); - _nav_state = vehicle_status.nav_state; - _armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED; + if (_vehicle_control_mode_sub.updated()) { + _vehicle_control_mode_sub.copy(&_vehicle_control_mode); } - if (_vehicle_angular_velocity_sub.updated()) { - vehicle_angular_velocity_s vehicle_angular_velocity{}; - _vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity); - _vehicle_yaw_rate = fabsf(vehicle_angular_velocity.xyz[2]) > YAW_RATE_THRESHOLD ? vehicle_angular_velocity.xyz[2] : 0.f; + const bool full_manual_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled + && !_vehicle_control_mode.flag_control_position_enabled && !_vehicle_control_mode.flag_control_attitude_enabled + && !_vehicle_control_mode.flag_control_rates_enabled; + + if (full_manual_mode_enabled) { // Manual mode + generateSteeringSetpoint(); } - 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_control_mode.flag_armed) { + generateActuatorSetpoint(); + } - if (_vehicle_local_position_sub.updated()) { - vehicle_local_position_s vehicle_local_position{}; - _vehicle_local_position_sub.copy(&vehicle_local_position); - _curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y); - 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 = fabsf(velocity_in_body_frame(0)) > SPEED_THRESHOLD ? velocity_in_body_frame(0) : 0.f; +} + +void RoverDifferential::generateSteeringSetpoint() +{ + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + rover_steering_setpoint_s rover_steering_setpoint{}; + rover_steering_setpoint.timestamp = _timestamp; + rover_steering_setpoint.normalized_speed_diff = manual_control_setpoint.roll; + _rover_steering_setpoint_pub.publish(rover_steering_setpoint); + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = _timestamp; + rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + rover_throttle_setpoint.throttle_body_y = 0.f; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); } } +void RoverDifferential::generateActuatorSetpoint() +{ + if (_rover_throttle_setpoint_sub.updated()) { + _rover_throttle_setpoint_sub.copy(&_rover_throttle_setpoint); + } + + if (_actuator_motors_sub.updated()) { + actuator_motors_s actuator_motors{}; + _actuator_motors_sub.copy(&actuator_motors); + _current_throttle_body_x = (actuator_motors.control[0] + actuator_motors.control[1]) / 2.f; + } + + if (_rover_steering_setpoint_sub.updated()) { + _rover_steering_setpoint_sub.copy(&_rover_steering_setpoint); + } + + const float throttle_body_x = RoverControl::throttleControl(_throttle_body_x_setpoint, + _rover_throttle_setpoint.throttle_body_x, _current_throttle_body_x, _param_ro_accel_limit.get(), + _param_ro_decel_limit.get(), _param_ro_max_thr_speed.get(), _dt); + actuator_motors_s actuator_motors{}; + actuator_motors.reversible_flags = _param_r_rev.get(); + computeInverseKinematics(throttle_body_x, + _rover_steering_setpoint.normalized_speed_diff).copyTo(actuator_motors.control); + actuator_motors.timestamp = _timestamp; + _actuator_motors_pub.publish(actuator_motors); +} + +Vector2f RoverDifferential::computeInverseKinematics(float throttle_body_x, const float speed_diff_normalized) +{ + float max_motor_command = fabsf(throttle_body_x) + fabsf(speed_diff_normalized); + + if (max_motor_command > 1.0f) { // Prioritize yaw rate if a normalized motor command exceeds limit of 1 + float excess = fabsf(max_motor_command - 1.0f); + throttle_body_x -= sign(throttle_body_x) * excess; + } + + // Calculate the left and right wheel speeds + return Vector2f(throttle_body_x - speed_diff_normalized, + throttle_body_x + speed_diff_normalized); +} + int RoverDifferential::task_spawn(int argc, char *argv[]) { RoverDifferential *instance = new RoverDifferential(); @@ -288,7 +175,7 @@ int RoverDifferential::task_spawn(int argc, char *argv[]) int RoverDifferential::custom_command(int argc, char *argv[]) { - return print_usage("unk_timestampn command"); + return print_usage("unknown command"); } int RoverDifferential::print_usage(const char *reason) @@ -300,7 +187,7 @@ int RoverDifferential::print_usage(const char *reason) PRINT_MODULE_DESCRIPTION( R"DESCR_STR( ### Description -Rover Differential controller. +Rover differential module. )DESCR_STR"); PRINT_MODULE_USAGE_NAME("rover_differential", "controller"); diff --git a/src/modules/rover_differential/RoverDifferential.hpp b/src/modules/rover_differential/RoverDifferential.hpp index 3fa8f4891c..9c943627b0 100644 --- a/src/modules/rover_differential/RoverDifferential.hpp +++ b/src/modules/rover_differential/RoverDifferential.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * Copyright (c) 2025 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 @@ -39,40 +39,34 @@ #include #include #include -#include + +// Libraries +#include +#include // uORB includes -#include #include -#include +#include +#include #include -#include -#include -#include -#include -#include - -// Standard libraries -#include +#include +#include +#include +#include +#include // Local includes -#include "RoverDifferentialGuidance/RoverDifferentialGuidance.hpp" -#include "RoverDifferentialControl/RoverDifferentialControl.hpp" - -using namespace time_literals; - -// Constants -static constexpr float STICK_DEADZONE = - 0.1f; // [0, 1] Percentage of stick input range that will be interpreted as zero around the stick centered value -static constexpr float YAW_RATE_THRESHOLD = - 0.02f; // [rad/s] The minimum threshold for the yaw rate measurement not to be interpreted as zero -static constexpr float SPEED_THRESHOLD = - 0.1f; // [m/s] The minimum threshold for the speed measurement not to be interpreted as zero +#include "DifferentialRateControl/DifferentialRateControl.hpp" +#include "DifferentialAttControl/DifferentialAttControl.hpp" +#include "DifferentialPosVelControl/DifferentialPosVelControl.hpp" class RoverDifferential : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: + /** + * @brief Constructor for RoverDifferential + */ RoverDifferential(); ~RoverDifferential() override = default; @@ -88,51 +82,66 @@ public: bool init(); protected: + /** + * @brief Update the parameters of the module. + */ void updateParams() override; private: void Run() override; /** - * @brief Update uORB subscriptions. + * @brief Generate and publish roverSteeringSetpoint from manualControlSetpoint (Manual Mode). */ - void updateSubscriptions(); + void generateSteeringSetpoint(); - // uORB Subscriptions - uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + /** + * @brief Generate and publish actuatorMotors setpoints from roverThrottleSetpoint/roverSteeringSetpoint. + */ + void generateActuatorSetpoint(); + + /** + * @brief Compute normalized motor commands based on normalized setpoints. + * @param throttle_body_x Normalized speed in body x direction [-1, 1]. + * @param speed_diff_normalized Speed difference between left and right wheels [-1, 1]. + * @return Motor speeds for the right and left motors [-1, 1]. + */ + Vector2f computeInverseKinematics(float throttle_body_x, float speed_diff_normalized); + + // uORB subscriptions uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; - 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::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)}; + uORB::Subscription _rover_throttle_setpoint_sub{ORB_ID(rover_throttle_setpoint)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; + vehicle_control_mode_s _vehicle_control_mode{}; + rover_steering_setpoint_s _rover_steering_setpoint{}; + rover_throttle_setpoint_s _rover_throttle_setpoint{}; - // uORB Publications - uORB::Publication _rover_differential_setpoint_pub{ORB_ID(rover_differential_setpoint)}; + // uORB publications + uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; + uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; + uORB::Publication _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)}; - // Instances - RoverDifferentialGuidance _rover_differential_guidance{this}; - RoverDifferentialControl _rover_differential_control{this}; - PurePursuit _posctl_pure_pursuit{this}; // Pure pursuit library + // Class instances + DifferentialRateControl _differential_rate_control{this}; + DifferentialAttControl _differential_att_control{this}; + DifferentialPosVelControl _differential_pos_vel_control{this}; // Variables - Vector2f _curr_pos_ned{}; - matrix::Quatf _vehicle_attitude_quaternion{}; - float _vehicle_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}; - bool _armed{false}; - bool _yaw_ctl{false}; // Indicates if the rover is doing yaw or yaw rate control in Stabilized and Position mode - float _stab_desired_yaw{0.f}; // Yaw setpoint for Stabilized mode - Vector2f _pos_ctl_course_direction{}; // Course direction for Position mode - Vector2f _pos_ctl_start_position_ned{}; // Initial rover position for course control in Position mode + hrt_abstime _timestamp{0}; + float _dt{0.f}; + float _current_throttle_body_x{0.f}; + // Controllers + SlewRate _throttle_body_x_setpoint{0.f}; + + // Parameters DEFINE_PARAMETERS( - (ParamFloat) _param_rd_wheel_track, - (ParamFloat) _param_rd_max_yaw_rate, - (ParamFloat) _param_rd_max_thr_yaw_r, - (ParamFloat) _param_rd_max_speed, - (ParamFloat) _param_pp_lookahd_max + (ParamInt) _param_r_rev, + (ParamFloat) _param_ro_accel_limit, + (ParamFloat) _param_ro_decel_limit, + (ParamFloat) _param_ro_max_thr_speed ) }; diff --git a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp deleted file mode 100644 index a378de3b94..0000000000 --- a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp +++ /dev/null @@ -1,277 +0,0 @@ -/**************************************************************************** - * - * 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 "RoverDifferentialControl.hpp" - -#include - -using namespace matrix; -using namespace time_literals; - -RoverDifferentialControl::RoverDifferentialControl(ModuleParams *parent) : ModuleParams(parent) -{ - updateParams(); - _rover_differential_status_pub.advertise(); -} - -void RoverDifferentialControl::updateParams() -{ - ModuleParams::updateParams(); - _max_yaw_rate = _param_rd_max_yaw_rate.get() * M_DEG_TO_RAD_F; - _max_yaw_accel = _param_rd_max_yaw_accel.get() * M_DEG_TO_RAD_F; - - // Update PID - _pid_yaw_rate.setGains(_param_rd_yaw_rate_p.get(), _param_rd_yaw_rate_i.get(), 0.f); - _pid_yaw_rate.setIntegralLimit(1.f); - _pid_yaw_rate.setOutputLimit(1.f); - _pid_throttle.setGains(_param_rd_p_gain_speed.get(), _param_rd_i_gain_speed.get(), 0.f); - _pid_throttle.setIntegralLimit(1.f); - _pid_throttle.setOutputLimit(1.f); - _pid_yaw.setGains(_param_rd_p_gain_yaw.get(), _param_rd_i_gain_yaw.get(), 0.f); - _pid_yaw.setIntegralLimit(_max_yaw_rate); - _pid_yaw.setOutputLimit(_max_yaw_rate); - - // Update slew rates - if (_max_yaw_rate > FLT_EPSILON) { - _yaw_setpoint_with_yaw_rate_limit.setSlewRate(_max_yaw_rate); - } - - if (_max_yaw_accel > FLT_EPSILON) { - _yaw_rate_with_accel_limit.setSlewRate(_max_yaw_accel); - } - -} - -void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, const float vehicle_yaw_rate, - const float vehicle_forward_speed) -{ - // Timestamps - hrt_abstime timestamp_prev = _timestamp; - _timestamp = hrt_absolute_time(); - const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; - - // Update differential setpoint - _rover_differential_setpoint_sub.update(&_rover_differential_setpoint); - - // Closed loop yaw control (Overrides yaw rate setpoint) - if (PX4_ISFINITE(_rover_differential_setpoint.yaw_setpoint)) { - _yaw_setpoint_with_yaw_rate_limit.update(matrix::wrap_pi(_rover_differential_setpoint.yaw_setpoint), dt); - _rover_differential_status.adjusted_yaw_setpoint = matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState()); - _pid_yaw.setSetpoint( - matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState() - - vehicle_yaw)); // error as setpoint to take care of wrapping - _rover_differential_setpoint.yaw_rate_setpoint = _pid_yaw.update(0.f, dt); - _rover_differential_status.clyaw_yaw_rate_setpoint = _rover_differential_setpoint.yaw_rate_setpoint; - - } else { - _yaw_setpoint_with_yaw_rate_limit.setForcedValue(vehicle_yaw); - } - - // Yaw rate control - float speed_diff_normalized{0.f}; - - if (PX4_ISFINITE(_rover_differential_setpoint.yaw_rate_setpoint)) { // Closed loop yaw rate control - speed_diff_normalized = calcNormalizedSpeedDiff(_rover_differential_setpoint.yaw_rate_setpoint, vehicle_yaw_rate, - _param_rd_max_thr_yaw_r.get(), _max_yaw_accel, _param_rd_wheel_track.get(), dt, _yaw_rate_with_accel_limit, - _pid_yaw_rate, false); - - } else { // Use normalized setpoint - speed_diff_normalized = calcNormalizedSpeedDiff(_rover_differential_setpoint.speed_diff_setpoint_normalized, - vehicle_yaw_rate, - _param_rd_max_thr_yaw_r.get(), _max_yaw_accel, _param_rd_wheel_track.get(), dt, _yaw_rate_with_accel_limit, - _pid_yaw_rate, true); - } - - // Speed control - float forward_speed_normalized{0.f}; - - if (PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint)) { - if (_param_rd_max_thr_spd.get() > FLT_EPSILON) { - - forward_speed_normalized = math::interpolate(_rover_differential_setpoint.forward_speed_setpoint, - -_param_rd_max_thr_spd.get(), _param_rd_max_thr_spd.get(), -1.f, 1.f); - - if (fabsf(forward_speed_normalized) > 1.f - - fabsf(speed_diff_normalized)) { // Adjust forward speed setpoint if it is infeasible due to the desired speed difference of the left/right wheels - _rover_differential_setpoint.forward_speed_setpoint = sign(_rover_differential_setpoint.forward_speed_setpoint) * - math::interpolate(1.f - fabsf(speed_diff_normalized), -1.f, 1.f, - -_param_rd_max_thr_spd.get(), _param_rd_max_thr_spd.get()); - } - } - - - forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_differential_setpoint.forward_speed_setpoint, - vehicle_forward_speed, _param_rd_max_thr_spd.get(), _forward_speed_setpoint_with_accel_limit, _pid_throttle, - _param_rd_max_accel.get(), _param_rd_max_decel.get(), dt, false); - - } else if (PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint_normalized)) { // Use normalized setpoint - forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_differential_setpoint.forward_speed_setpoint_normalized, - vehicle_forward_speed, _param_rd_max_thr_spd.get(), _forward_speed_setpoint_with_accel_limit, _pid_throttle, - _param_rd_max_accel.get(), _param_rd_max_decel.get(), dt, true); - - } - - // Publish rover differential status (logging) - _rover_differential_status.timestamp = _timestamp; - _rover_differential_status.measured_forward_speed = vehicle_forward_speed; - _rover_differential_status.measured_yaw = vehicle_yaw; - _rover_differential_status.measured_yaw_rate = vehicle_yaw_rate; - _rover_differential_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral(); - _rover_differential_status.pid_throttle_integral = _pid_throttle.getIntegral(); - _rover_differential_status.pid_yaw_integral = _pid_yaw.getIntegral(); - _rover_differential_status_pub.publish(_rover_differential_status); - - // Publish to motors - actuator_motors_s actuator_motors{}; - actuator_motors.reversible_flags = _param_r_rev.get(); - computeInverseKinematics(forward_speed_normalized, speed_diff_normalized).copyTo(actuator_motors.control); - actuator_motors.timestamp = _timestamp; - _actuator_motors_pub.publish(actuator_motors); -} - -float RoverDifferentialControl::calcNormalizedSpeedDiff(const float yaw_rate_setpoint, const float vehicle_yaw_rate, - const float max_thr_yaw_r, - const float max_yaw_accel, const float wheel_track, const float dt, SlewRate &yaw_rate_with_accel_limit, - PID &pid_yaw_rate, const bool normalized) -{ - float slew_rate_normalization{1.f}; - - if (normalized) { // Slew rate needs to be normalized if the setpoint is normalized - slew_rate_normalization = max_thr_yaw_r > FLT_EPSILON ? max_thr_yaw_r : 0.f; - } - - if (max_yaw_accel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) { - yaw_rate_with_accel_limit.setSlewRate(max_yaw_accel / slew_rate_normalization); - yaw_rate_with_accel_limit.update(yaw_rate_setpoint, dt); - - } else { - yaw_rate_with_accel_limit.setForcedValue(yaw_rate_setpoint); - } - - // Transform yaw rate into speed difference - float speed_diff_normalized{0.f}; - - if (normalized) { - speed_diff_normalized = yaw_rate_with_accel_limit.getState(); - - } else { - _rover_differential_status.adjusted_yaw_rate_setpoint = yaw_rate_with_accel_limit.getState(); - - if (wheel_track > FLT_EPSILON && max_thr_yaw_r > FLT_EPSILON) { // Feedforward - const float speed_diff = yaw_rate_with_accel_limit.getState() * wheel_track / - 2.f; - speed_diff_normalized = math::interpolate(speed_diff, -max_thr_yaw_r, - max_thr_yaw_r, -1.f, 1.f); - } - - pid_yaw_rate.setSetpoint(yaw_rate_with_accel_limit.getState()); - speed_diff_normalized += pid_yaw_rate.update(vehicle_yaw_rate, dt); - } - - return math::constrain(speed_diff_normalized, -1.f, 1.f); - -} - -float RoverDifferentialControl::calcNormalizedSpeedSetpoint(const float forward_speed_setpoint, - const float vehicle_forward_speed, const float max_thr_spd, SlewRate &forward_speed_setpoint_with_accel_limit, - PID &pid_throttle, const float max_accel, const float max_decel, const float dt, const bool normalized) -{ - float slew_rate_normalization{1.f}; - - if (normalized) { // Slew rate needs to be normalized if the setpoint is normalized - slew_rate_normalization = max_thr_spd > FLT_EPSILON ? max_thr_spd : 0.f; - } - - // Apply acceleration and deceleration limit - if (fabsf(forward_speed_setpoint) >= fabsf(forward_speed_setpoint_with_accel_limit.getState())) { - if (max_accel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) { - forward_speed_setpoint_with_accel_limit.setSlewRate(max_accel / slew_rate_normalization); - forward_speed_setpoint_with_accel_limit.update(forward_speed_setpoint, dt); - - } else { - forward_speed_setpoint_with_accel_limit.setForcedValue(forward_speed_setpoint); - - } - - } else if (max_decel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) { - forward_speed_setpoint_with_accel_limit.setSlewRate(max_decel / slew_rate_normalization); - forward_speed_setpoint_with_accel_limit.update(forward_speed_setpoint, dt); - - } else { - forward_speed_setpoint_with_accel_limit.setForcedValue(forward_speed_setpoint); - } - - // Calculate normalized forward speed setpoint - float forward_speed_normalized{0.f}; - - if (normalized) { - forward_speed_normalized = _forward_speed_setpoint_with_accel_limit.getState(); - - } else { // Closed loop speed control - _rover_differential_status.adjusted_forward_speed_setpoint = _forward_speed_setpoint_with_accel_limit.getState(); - - if (_param_rd_max_thr_spd.get() > FLT_EPSILON) { // Feedforward - forward_speed_normalized = math::interpolate(_forward_speed_setpoint_with_accel_limit.getState(), - -max_thr_spd, max_thr_spd, - -1.f, 1.f); - } - - pid_throttle.setSetpoint(_forward_speed_setpoint_with_accel_limit.getState()); - forward_speed_normalized += pid_throttle.update(vehicle_forward_speed, dt); - } - - return math::constrain(forward_speed_normalized, -1.f, 1.f); - -} - -matrix::Vector2f RoverDifferentialControl::computeInverseKinematics(float forward_speed_normalized, - const float speed_diff_normalized) -{ - float max_motor_command = fabsf(forward_speed_normalized) + fabsf(speed_diff_normalized); - - if (max_motor_command > 1.0f) { // Prioritize yaw rate if a normalized motor command exceeds limit of 1 - float excess = fabsf(max_motor_command - 1.0f); - forward_speed_normalized -= sign(forward_speed_normalized) * excess; - } - - // Calculate the left and right wheel speeds - return Vector2f(forward_speed_normalized - speed_diff_normalized, - forward_speed_normalized + speed_diff_normalized); -} - -void RoverDifferentialControl::resetControllers() -{ - _pid_throttle.resetIntegral(); - _pid_yaw_rate.resetIntegral(); - _pid_yaw.resetIntegral(); -} diff --git a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp deleted file mode 100644 index e08153356b..0000000000 --- a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp +++ /dev/null @@ -1,169 +0,0 @@ -/**************************************************************************** - * - * 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 - -// PX4 includes -#include - -// uORB includes -#include -#include -#include -#include -#include -#include -#include -#include - -// Standard libraries -#include -#include -#include -#include -#include - -using namespace matrix; - -/** - * @brief Class for differential rover guidance. - */ -class RoverDifferentialControl : public ModuleParams -{ -public: - /** - * @brief Constructor for RoverDifferentialControl. - * @param parent The parent ModuleParams object. - */ - RoverDifferentialControl(ModuleParams *parent); - ~RoverDifferentialControl() = default; - - /** - * @brief Compute motor commands based on setpoints - */ - void computeMotorCommands(float vehicle_yaw, float vehicle_yaw_rate, float vehicle_forward_speed); - - /** - * @brief Reset PID controllers - */ - void resetControllers(); - -protected: - /** - * @brief Update the parameters of the module. - */ - void updateParams() override; - -private: - - /** - * @brief Compute normalized speed diff setpoint between the left and right wheels and apply slew rates. - * @param yaw_rate_setpoint Yaw rate setpoint [rad/s or normalized [-1, 1]]. - * @param vehicle_yaw_rate Measured yaw rate [rad/s]. - * @param max_thr_yaw_r Yaw rate turning left/right wheels at max speed in opposite directions [m/s]. - * @param max_yaw_accel Maximum allowed yaw acceleration for the rover [rad/s^2]. - * @param wheel_track Wheel track [m]. - * @param dt Time since last update [s]. - * @param yaw_rate_with_accel_limit Yaw rate slew rate. - * @param pid_yaw_rate Yaw rate PID - * @param normalized Indicates if the forward speed setpoint is already normalized. - * @return Normalized speed differece setpoint with applied slew rates [-1, 1]. - */ - float calcNormalizedSpeedDiff(float yaw_rate_setpoint, float vehicle_yaw_rate, float max_thr_yaw_r, float max_yaw_accel, - float wheel_track, float dt, SlewRate &yaw_rate_with_accel_limit, PID &pid_yaw_rate, bool normalized); - /** - * @brief Compute normalized forward speed setpoint and apply slew rates. - * to the forward speed setpoint and doing closed loop speed control if enabled. - * @param forward_speed_setpoint Forward speed setpoint [m/s]. - * @param vehicle_forward_speed Actual forward speed [m/s]. - * @param max_thr_spd Speed the rover drives at maximum throttle [m/s]. - * @param forward_speed_setpoint_with_accel_limit Speed slew rate. - * @param pid_throttle Throttle PID - * @param max_accel Maximum acceleration [m/s^2] - * @param max_decel Maximum deceleration [m/s^2] - * @param dt Time since last update [s]. - * @param normalized Indicates if the forward speed setpoint is already normalized. - * @return Normalized forward speed setpoint with applied slew rates [-1, 1]. - */ - float calcNormalizedSpeedSetpoint(float forward_speed_setpoint, float vehicle_forward_speed, float max_thr_spd, - SlewRate &forward_speed_setpoint_with_accel_limit, PID &pid_throttle, float max_accel, float max_decel, - float dt, bool normalized); - - /** - * @brief Compute normalized motor commands based on normalized setpoints. - * @param forward_speed_normalized Normalized forward speed [-1, 1]. - * @param speed_diff_normalized Speed difference between left and right wheels [-1, 1]. - * @return matrix::Vector2f Motor velocities for the right and left motors [-1, 1]. - */ - matrix::Vector2f computeInverseKinematics(float forward_speed_normalized, const float speed_diff_normalized); - - // uORB subscriptions - uORB::Subscription _rover_differential_setpoint_sub{ORB_ID(rover_differential_setpoint)}; - - // uORB publications - uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; - uORB::Publication _rover_differential_status_pub{ORB_ID(rover_differential_status)}; - rover_differential_status_s _rover_differential_status{}; - - // Variables - rover_differential_setpoint_s _rover_differential_setpoint{}; - hrt_abstime _timestamp{0}; - float _max_yaw_rate{0.f}; - float _max_yaw_accel{0.f}; - - // Controllers - PID _pid_throttle; // Closed loop speed control - PID _pid_yaw; // Closed loop yaw control - PID _pid_yaw_rate; // Closed loop yaw rate control - SlewRate _forward_speed_setpoint_with_accel_limit{0.f}; - SlewRate _yaw_rate_with_accel_limit{0.f}; - SlewRateYaw _yaw_setpoint_with_yaw_rate_limit; - - // Parameters - DEFINE_PARAMETERS( - (ParamFloat) _param_rd_wheel_track, - (ParamFloat) _param_rd_max_thr_spd, - (ParamFloat) _param_rd_max_accel, - (ParamFloat) _param_rd_max_decel, - (ParamFloat) _param_rd_max_thr_yaw_r, - (ParamFloat) _param_rd_max_yaw_rate, - (ParamFloat) _param_rd_max_yaw_accel, - (ParamFloat) _param_rd_yaw_rate_p, - (ParamFloat) _param_rd_yaw_rate_i, - (ParamFloat) _param_rd_p_gain_speed, - (ParamFloat) _param_rd_i_gain_speed, - (ParamFloat) _param_rd_p_gain_yaw, - (ParamFloat) _param_rd_i_gain_yaw, - (ParamInt) _param_r_rev - ) -}; diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp deleted file mode 100644 index bb45e6b7b9..0000000000 --- a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp +++ /dev/null @@ -1,234 +0,0 @@ -/**************************************************************************** - * - * 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 "RoverDifferentialGuidance.hpp" - -#include - -using namespace matrix; - -RoverDifferentialGuidance::RoverDifferentialGuidance(ModuleParams *parent) : ModuleParams(parent) -{ - updateParams(); - _max_forward_speed = _param_rd_max_speed.get(); - _rover_differential_guidance_status_pub.advertise(); -} - -void RoverDifferentialGuidance::updateParams() -{ - ModuleParams::updateParams(); -} - -void RoverDifferentialGuidance::computeGuidance(const float vehicle_yaw, const float forward_speed, const int nav_state) -{ - updateSubscriptions(); - - // Catch return to launch - const float distance_to_curr_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) { - _mission_finished = distance_to_curr_wp < _param_nav_acc_rad.get(); - } - - // State machine - float desired_yaw = _pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, - math::max(forward_speed, 0.f)); - const float heading_error = matrix::wrap_pi(desired_yaw - vehicle_yaw); - - if (!_mission_finished && distance_to_curr_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()) { - _currentState = GuidanceState::SPOT_TURNING; - - } else if (_currentState == GuidanceState::SPOT_TURNING && fabsf(heading_error) < _param_rd_trans_trn_drv.get()) { - _currentState = GuidanceState::DRIVING; - } - - } else { // Mission finished or delay command - _currentState = GuidanceState::STOPPED; - } - - // Guidance logic - float desired_forward_speed{0.f}; - - switch (_currentState) { - case GuidanceState::DRIVING: { - // Calculate desired forward speed - desired_forward_speed = _max_forward_speed; - - if (_waypoint_transition_angle < M_PI_F - _param_rd_trans_drv_trn.get()) { - if (_param_rd_max_jerk.get() > FLT_EPSILON && _param_rd_max_decel.get() > FLT_EPSILON) { - desired_forward_speed = math::trajectory::computeMaxSpeedFromDistance(_param_rd_max_jerk.get(), - _param_rd_max_decel.get(), distance_to_curr_wp, 0.0f); - desired_forward_speed = math::constrain(desired_forward_speed, -_max_forward_speed, _max_forward_speed); - } - - } else if (_param_rd_max_jerk.get() > FLT_EPSILON && _param_rd_max_decel.get() > FLT_EPSILON - && _param_rd_miss_spd_gain.get() > FLT_EPSILON) { - const float speed_reduction = math::constrain(_param_rd_miss_spd_gain.get() * math::interpolate( - M_PI_F - _waypoint_transition_angle, 0.f, - M_PI_F, 0.f, 1.f), 0.f, 1.f); - desired_forward_speed = math::trajectory::computeMaxSpeedFromDistance(_param_rd_max_jerk.get(), - _param_rd_max_decel.get(), distance_to_curr_wp, _max_forward_speed * (1.f - speed_reduction)); - desired_forward_speed = math::constrain(desired_forward_speed, -_max_forward_speed, - _max_forward_speed); - } - - } break; - - case GuidanceState::SPOT_TURNING: - if (forward_speed > TURN_MAX_VELOCITY) { - desired_yaw = vehicle_yaw; // Wait for the rover to stop - - } - - break; - - case GuidanceState::STOPPED: - default: - desired_yaw = vehicle_yaw; - break; - - } - - // Publish differential guidance status (logging) - hrt_abstime timestamp = hrt_absolute_time(); - rover_differential_guidance_status_s rover_differential_guidance_status{}; - rover_differential_guidance_status.timestamp = timestamp; - rover_differential_guidance_status.lookahead_distance = _pure_pursuit.getLookaheadDistance(); - 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); - - // Publish setpoints - rover_differential_setpoint_s rover_differential_setpoint{}; - rover_differential_setpoint.timestamp = timestamp; - rover_differential_setpoint.forward_speed_setpoint = desired_forward_speed; - rover_differential_setpoint.forward_speed_setpoint_normalized = NAN; - rover_differential_setpoint.yaw_rate_setpoint = NAN; - rover_differential_setpoint.speed_diff_setpoint_normalized = NAN; - rover_differential_setpoint.yaw_setpoint = desired_yaw; - _rover_differential_setpoint_pub.publish(rover_differential_setpoint); -} - -void RoverDifferentialGuidance::updateSubscriptions() -{ - 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); - } -} - -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)); - _next_wp_ned = _global_ned_proj_ref.project(_next_wp(0), _next_wp(1)); - - // Distances - const Vector2f curr_to_next_wp_ned = _next_wp_ned - _curr_wp_ned; - const Vector2f curr_to_prev_wp_ned = _prev_wp_ned - _curr_wp_ned; - 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 - _waypoint_transition_angle = acosf(cosin); - - // Waypoint cruising speed - if (position_setpoint_triplet.current.cruising_speed > 0.f) { - _max_forward_speed = math::constrain(position_setpoint_triplet.current.cruising_speed, 0.f, _param_rd_max_speed.get()); - - } else { - _max_forward_speed = _param_rd_max_speed.get(); - } -} diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp deleted file mode 100644 index cc1b033635..0000000000 --- a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp +++ /dev/null @@ -1,151 +0,0 @@ -/**************************************************************************** - * - * 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 - -// PX4 includes -#include -#include - -// uORB includes -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// Standard libraries -#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; - - /** - * @brief Compute and publish attitude and velocity setpoints based on the mission plan. - * @param vehicle_yaw The yaw of the vehicle [rad]. - * @param forward_speed The forward speed of the vehicle [m/s]. - * @param nav_state Navigation state of the rover. - */ - void computeGuidance(float vehicle_yaw, float forward_speed, 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 - */ - void updateWaypoints(); - - // 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_setpoint_pub{ORB_ID(rover_differential_setpoint)}; - uORB::Publication _rover_differential_guidance_status_pub{ORB_ID(rover_differential_guidance_status)}; - - // Variables - MapProjection _global_ned_proj_ref{}; // Transform global to ned coordinates. - GuidanceState _currentState{GuidanceState::DRIVING}; // The current state of the guidance. - PurePursuit _pure_pursuit{this}; // Pure pursuit library - bool _mission_finished{false}; - - // 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{}; - Vector2f _next_wp_ned{}; - Vector2d _home_position{}; - float _max_forward_speed{0.f}; // Maximum forward speed for the rover [m/s] - float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad] - - // Constants - static constexpr float TURN_MAX_VELOCITY = 0.2f; // Velocity threshhold for starting the spot turn [m/s] - - // Parameters - DEFINE_PARAMETERS( - (ParamFloat) _param_nav_acc_rad, - (ParamFloat) _param_rd_max_jerk, - (ParamFloat) _param_rd_max_decel, - (ParamFloat) _param_rd_max_speed, - (ParamFloat) _param_rd_trans_trn_drv, - (ParamFloat) _param_rd_trans_drv_trn, - (ParamFloat) _param_rd_miss_spd_gain - ) -}; diff --git a/src/modules/rover_differential/module.yaml b/src/modules/rover_differential/module.yaml index 6297f3aafd..eca54e798e 100644 --- a/src/modules/rover_differential/module.yaml +++ b/src/modules/rover_differential/module.yaml @@ -3,176 +3,17 @@ module_name: Rover Differential parameters: - group: Rover Differential definitions: - RD_WHEEL_TRACK: - description: - short: Wheel track - long: Distance from the center of the right wheel to the center of the left wheel - type: float - unit: m - min: 0.001 - max: 100 - increment: 0.001 - decimal: 3 - default: 0.5 - - RD_YAW_P: - description: - short: Proportional gain for closed loop yaw controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 3 - default: 1 - - RD_YAW_I: - description: - short: Integral gain for closed loop yaw controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 3 - default: 0 - - RD_SPEED_P: - description: - short: Proportional gain for closed loop forward speed controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 3 - default: 1 - - RD_SPEED_I: - description: - short: Integral gain for closed loop forward speed controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 3 - default: 0 - - RD_YAW_RATE_P: - description: - short: Proportional gain for closed loop yaw rate controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 3 - default: 1 - - RD_YAW_RATE_I: - description: - short: Integral gain for closed loop yaw rate controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 3 - default: 0 - - RD_MAX_JERK: - description: - short: Maximum jerk - long: Limit for forwards acc/deceleration change. - type: float - unit: m/s^3 - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 0.5 - - RD_MAX_ACCEL: - description: - short: Maximum acceleration - long: Maximum acceleration is used to limit the acceleration of the rover - type: float - unit: m/s^2 - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 0.5 - - RD_MAX_DECEL: - description: - short: Maximum deceleration - long: | - Maximum decelaration is used to limit the deceleration of the rover. - Set to -1 to disable, causing the rover to decelerate as fast as possible. - Caution: This disables the slow down effect in auto modes. - type: float - unit: m/s^2 - min: -1 - max: 100 - increment: 0.01 - decimal: 2 - default: -1 - - RD_MAX_SPEED: - description: - short: Maximum speed setpoint - long: | - This parameter is used to cap desired forward speed and map controller inputs to desired speeds in Position mode. - type: float - unit: m/s - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 1 - - RD_MAX_THR_SPD: - description: - short: Speed the rover drives at maximum throttle - long: | - This parameter is used to calculate the feedforward term of the closed loop speed control which linearly - maps desired speeds to normalized motor commands [-1. 1]. - A good starting point is the observed ground speed when the rover drives at maximum throttle in manual mode. - Increase this parameter if the rover is faster than the setpoint, and decrease if the rover is slower. - type: float - unit: m/s - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 1 - - 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,Stabilized and Position mode. - type: float - unit: deg/s - min: 0.01 - max: 1000 - increment: 0.01 - decimal: 2 - default: 90 - - RD_MAX_YAW_ACCEL: - description: - short: Maximum allowed yaw acceleration for the rover - long: | - This parameter is used to cap desired yaw acceleration. This is used to adjust incoming yaw rate setpoints - to a feasible yaw rate setpoint based on the physical limitation on how fast the yaw rate can change. - This leads to a smooth setpoint trajectory for the closed loop yaw rate controller to track. - Set to -1 to disable. - type: float - unit: deg/s^2 - min: -1 - max: 1000 - increment: 0.01 - decimal: 2 - default: -1 + description: + short: Wheel track + long: Distance from the center of the right wheel to the center of the left wheel. + type: float + unit: m + min: 0 + max: 100 + increment: 0.001 + decimal: 3 + default: 0 RD_MAX_THR_YAW_R: description: @@ -181,7 +22,7 @@ parameters: This parameter is used to calculate the feedforward term of the closed loop yaw rate control. The controller first calculates the required speed difference between the left and right motor to achieve the desired yaw rate. This desired speed difference is then linearly mapped to normalized motor commands. - A good starting point is twice the speed the rover drives at maximum throttle (RD_MAX_THR_SPD)). + A good starting point is half the speed the rover drives at maximum throttle (RD_MAX_THR_SPD)). Increase this parameter if the rover turns faster than the setpoint, and decrease if the rover turns slower. type: float unit: m/s @@ -189,11 +30,14 @@ parameters: max: 100 increment: 0.01 decimal: 2 - default: 2 + default: 0 RD_TRANS_TRN_DRV: description: short: Yaw error threshhold to switch from spot turning to driving + long: | + This threshold is used for the state machine to switch from turning to driving based on the + error between the desired and actual yaw. type: float unit: rad min: 0.001 @@ -227,9 +71,10 @@ parameters: The normalized transition angle is the angle between the line segment from prev-curr WP and curr-next WP interpolated from [0, 180] -> [0, 1]. Higher value -> More speed reduction during waypoint transitions. + Set to -1 to disable any speed reduction during waypoint transition. type: float - min: 0.05 + min: -1 max: 100 increment: 0.01 decimal: 2 - default: 1 + default: -1 diff --git a/src/modules/rover_mecanum/CMakeLists.txt b/src/modules/rover_mecanum/CMakeLists.txt index 8eea90fb2b..cf5e5b9187 100644 --- a/src/modules/rover_mecanum/CMakeLists.txt +++ b/src/modules/rover_mecanum/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2024 PX4 Development Team. All rights reserved. +# Copyright (c) 2025 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,8 +31,9 @@ # ############################################################################ -add_subdirectory(RoverMecanumGuidance) -add_subdirectory(RoverMecanumControl) +add_subdirectory(MecanumRateControl) +add_subdirectory(MecanumAttControl) +add_subdirectory(MecanumPosVelControl) px4_add_module( MODULE modules__rover_mecanum @@ -41,10 +42,11 @@ px4_add_module( RoverMecanum.cpp RoverMecanum.hpp DEPENDS - RoverMecanumGuidance - RoverMecanumControl + MecanumRateControl + MecanumAttControl + MecanumPosVelControl px4_work_queue - pure_pursuit + rover_control MODULE_CONFIG module.yaml ) diff --git a/src/modules/rover_mecanum/Kconfig b/src/modules/rover_mecanum/Kconfig index 46a24b3fc8..5c1778c67c 100644 --- a/src/modules/rover_mecanum/Kconfig +++ b/src/modules/rover_mecanum/Kconfig @@ -1,6 +1,5 @@ menuconfig MODULES_ROVER_MECANUM bool "rover_mecanum" default n - depends on MODULES_CONTROL_ALLOCATOR ---help--- - Enable support for control of mecanum rovers + Enable support for mecanum rovers diff --git a/src/lib/avoidance/CMakeLists.txt b/src/modules/rover_mecanum/MecanumAttControl/CMakeLists.txt similarity index 86% rename from src/lib/avoidance/CMakeLists.txt rename to src/modules/rover_mecanum/MecanumAttControl/CMakeLists.txt index c1da2a76dc..34bad46d91 100644 --- a/src/lib/avoidance/CMakeLists.txt +++ b/src/modules/rover_mecanum/MecanumAttControl/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2020 PX4 Development Team. All rights reserved. +# Copyright (c) 2025 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,9 @@ # ############################################################################ -px4_add_library(avoidance - ObstacleAvoidance.cpp +px4_add_library(MecanumAttControl + MecanumAttControl.cpp ) -target_link_libraries(avoidance PUBLIC hysteresis bezier) -px4_add_functional_gtest(SRC ObstacleAvoidanceTest.cpp LINKLIBS avoidance) +target_link_libraries(MecanumAttControl PUBLIC PID) +target_include_directories(MecanumAttControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp b/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp new file mode 100644 index 0000000000..0582c5f299 --- /dev/null +++ b/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp @@ -0,0 +1,215 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 "MecanumAttControl.hpp" + +using namespace time_literals; + +MecanumAttControl::MecanumAttControl(ModuleParams *parent) : ModuleParams(parent) +{ + _rover_rate_setpoint_pub.advertise(); + _rover_throttle_setpoint_pub.advertise(); + _rover_attitude_setpoint_pub.advertise(); + _rover_attitude_status_pub.advertise(); + updateParams(); +} + +void MecanumAttControl::updateParams() +{ + ModuleParams::updateParams(); + + if (_param_ro_yaw_rate_limit.get() > FLT_EPSILON) { + _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + } + + _pid_yaw.setGains(_param_ro_yaw_p.get(), 0.f, 0.f); + _pid_yaw.setIntegralLimit(_max_yaw_rate); + _pid_yaw.setOutputLimit(_max_yaw_rate); + _adjusted_yaw_setpoint.setSlewRate(_max_yaw_rate); +} + +void MecanumAttControl::updateAttControl() +{ + const hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + if (_vehicle_control_mode_sub.updated()) { + _vehicle_control_mode_sub.copy(&_vehicle_control_mode); + } + + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude{}; + _vehicle_attitude_sub.copy(&vehicle_attitude); + matrix::Quatf vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q); + _vehicle_yaw = matrix::Eulerf(vehicle_attitude_quaternion).psi(); + } + + if (_vehicle_control_mode.flag_control_attitude_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) { + + if (_vehicle_control_mode.flag_control_manual_enabled || _vehicle_control_mode.flag_control_offboard_enabled) { + generateAttitudeSetpoint(); + } + + generateRateSetpoint(); + + } else { // Reset pid and slew rate when attitude control is not active + _pid_yaw.resetIntegral(); + _adjusted_yaw_setpoint.setForcedValue(0.f); + } + + // Publish attitude controller status (logging only) + rover_attitude_status_s rover_attitude_status; + rover_attitude_status.timestamp = _timestamp; + rover_attitude_status.measured_yaw = _vehicle_yaw; + rover_attitude_status.adjusted_yaw_setpoint = _adjusted_yaw_setpoint.getState(); + _rover_attitude_status_pub.publish(rover_attitude_status); + +} + +void MecanumAttControl::generateAttitudeSetpoint() +{ + const bool stab_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled + && !_vehicle_control_mode.flag_control_position_enabled && _vehicle_control_mode.flag_control_attitude_enabled; + + if (stab_mode_enabled && _manual_control_setpoint_sub.updated()) { // Stab Mode + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = _timestamp; + rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + rover_throttle_setpoint.throttle_body_y = manual_control_setpoint.roll; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + + const float yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.yaw, + _param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + + if (fabsf(yaw_rate_setpoint) > FLT_EPSILON) { // Closed loop yaw rate control + _stab_yaw_setpoint = NAN; + _adjusted_yaw_setpoint.setForcedValue(0.f); + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = yaw_rate_setpoint; + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + + } else if (fabsf(rover_throttle_setpoint.throttle_body_x) > FLT_EPSILON + || fabsf(rover_throttle_setpoint.throttle_body_y) > + FLT_EPSILON) { // Closed loop yaw control if the yaw rate input is zero (keep current yaw) + if (!PX4_ISFINITE(_stab_yaw_setpoint)) { + _stab_yaw_setpoint = _vehicle_yaw; + } + + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = _stab_yaw_setpoint; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + + } else { // Reset yaw control and yaw rate setpoint + _stab_yaw_setpoint = NAN; + _adjusted_yaw_setpoint.setForcedValue(0.f); + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = 0.f; + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + } + + + } + + } else if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard attitude control + trajectory_setpoint_s trajectory_setpoint{}; + _trajectory_setpoint_sub.copy(&trajectory_setpoint); + + if (_offboard_control_mode_sub.updated()) { + _offboard_control_mode_sub.copy(&_offboard_control_mode); + } + + const bool offboard_att_control = _offboard_control_mode.attitude; + + if (offboard_att_control && PX4_ISFINITE(trajectory_setpoint.yaw)) { + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = trajectory_setpoint.yaw; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + } + } +} + +void MecanumAttControl::generateRateSetpoint() +{ + if (_rover_attitude_setpoint_sub.updated()) { + _rover_attitude_setpoint_sub.copy(&_rover_attitude_setpoint); + } + + if (_rover_rate_setpoint_sub.updated()) { + _rover_rate_setpoint_sub.copy(&_rover_rate_setpoint); + } + + // Check if a new rate setpoint was already published from somewhere else + if (_rover_rate_setpoint.timestamp > _last_rate_setpoint_update + && _rover_rate_setpoint.timestamp > _rover_attitude_setpoint.timestamp) { + return; + } + + const float yaw_rate_setpoint = RoverControl::attitudeControl(_adjusted_yaw_setpoint, _pid_yaw, _max_yaw_rate, + _vehicle_yaw, _rover_attitude_setpoint.yaw_setpoint, _dt); + + _last_rate_setpoint_update = _timestamp; + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = math::constrain(yaw_rate_setpoint, -_max_yaw_rate, _max_yaw_rate); + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); +} + +bool MecanumAttControl::runSanityChecks() +{ + bool ret = true; + + if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) { + ret = false; + } + + if (_param_ro_yaw_p.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("mecanum_att_control_conf_invalid_yaw_p"), events::Log::Error, + "Invalid configuration of necessary parameter RO_YAW_P", _param_ro_yaw_p.get()); + } + } + + _prev_param_check_passed = ret; + return ret; +} diff --git a/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp b/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp new file mode 100644 index 0000000000..c6e9a3353e --- /dev/null +++ b/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp @@ -0,0 +1,142 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 + +// Libraries +#include +#include +#include +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * @brief Class for mecanum attitude control. + */ +class MecanumAttControl : public ModuleParams +{ +public: + /** + * @brief Constructor for MecanumAttControl. + * @param parent The parent ModuleParams object. + */ + MecanumAttControl(ModuleParams *parent); + ~MecanumAttControl() = default; + + /** + * @brief Update attitude controller. + */ + void updateAttControl(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + /** + * @brief Generate and publish roverAttitudeSetpoint from manualControlSetpoint (Stab Mode) + * or trajectorySetpoint (Offboard attitude control). + */ + void generateAttitudeSetpoint(); + + /** + * @brief Generate and publish roverRateSetpoint from roverAttitudeSetpoint. + */ + void generateRateSetpoint(); + + /** + * @brief Check if the necessary parameters are set. + * @return True if all checks pass. + */ + bool runSanityChecks(); + + // uORB subscriptions + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; + uORB::Subscription _rover_attitude_setpoint_sub{ORB_ID(rover_attitude_setpoint)}; + uORB::Subscription _rover_rate_setpoint_sub{ORB_ID(rover_rate_setpoint)}; + vehicle_control_mode_s _vehicle_control_mode{}; + rover_attitude_setpoint_s _rover_attitude_setpoint{}; + rover_rate_setpoint_s _rover_rate_setpoint{}; + offboard_control_mode_s _offboard_control_mode{}; + + // uORB publications + uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; + uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; + uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; + uORB::Publication _rover_attitude_status_pub{ORB_ID(rover_attitude_status)}; + + // Variables + hrt_abstime _timestamp{0}; + hrt_abstime _last_rate_setpoint_update{0}; + float _vehicle_yaw{0.f}; + float _dt{0.f}; + float _max_yaw_rate{0.f}; + float _stab_yaw_setpoint{0.f}; // Yaw setpoint for stab mode, NAN if yaw rate is manually controlled [rad] + bool _prev_param_check_passed{true}; + + // Controllers + PID _pid_yaw; + SlewRateYaw _adjusted_yaw_setpoint; + + // Parameters + DEFINE_PARAMETERS( + (ParamFloat) _param_ro_yaw_rate_limit, + (ParamFloat) _param_ro_yaw_p, + (ParamFloat) _param_ro_yaw_stick_dz + ) +}; diff --git a/src/modules/rover_mecanum/MecanumPosVelControl/CMakeLists.txt b/src/modules/rover_mecanum/MecanumPosVelControl/CMakeLists.txt new file mode 100644 index 0000000000..692e4e661b --- /dev/null +++ b/src/modules/rover_mecanum/MecanumPosVelControl/CMakeLists.txt @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2025 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. +# +############################################################################ + +px4_add_library(MecanumPosVelControl + MecanumPosVelControl.cpp +) + +target_link_libraries(MecanumPosVelControl PUBLIC PID) +target_link_libraries(MecanumPosVelControl PUBLIC pure_pursuit) +target_include_directories(MecanumPosVelControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.cpp b/src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.cpp new file mode 100644 index 0000000000..a5ee63496e --- /dev/null +++ b/src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.cpp @@ -0,0 +1,426 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 "MecanumPosVelControl.hpp" + +using namespace time_literals; + +MecanumPosVelControl::MecanumPosVelControl(ModuleParams *parent) : ModuleParams(parent) +{ + _rover_rate_setpoint_pub.advertise(); + _rover_throttle_setpoint_pub.advertise(); + _rover_attitude_setpoint_pub.advertise(); + _rover_velocity_status_pub.advertise(); + updateParams(); +} + +void MecanumPosVelControl::updateParams() +{ + ModuleParams::updateParams(); + _pid_speed_x.setGains(_param_ro_speed_p.get(), _param_ro_speed_i.get(), 0.f); + _pid_speed_x.setIntegralLimit(1.f); + _pid_speed_x.setOutputLimit(1.f); + _pid_speed_y.setGains(_param_ro_speed_p.get(), _param_ro_speed_i.get(), 0.f); + _pid_speed_y.setIntegralLimit(1.f); + _pid_speed_y.setOutputLimit(1.f); + _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + + if (_param_ro_accel_limit.get() > FLT_EPSILON) { + _speed_x_setpoint.setSlewRate(_param_ro_accel_limit.get()); + _speed_y_setpoint.setSlewRate(_param_ro_accel_limit.get()); + } +} + +void MecanumPosVelControl::updatePosControl() +{ + const hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + updateSubscriptions(); + + if ((_vehicle_control_mode.flag_control_position_enabled || _vehicle_control_mode.flag_control_velocity_enabled) + && _vehicle_control_mode.flag_armed && runSanityChecks()) { + generateAttitudeSetpoint(); + + if (_param_ro_max_thr_speed.get() > FLT_EPSILON) { // Adjust speed setpoints if infeasible + if (_rover_steering_setpoint_sub.updated()) { + _rover_steering_setpoint_sub.copy(&_rover_steering_setpoint); + } + + float speed_body_x_setpoint_normalized = math::interpolate(_speed_body_x_setpoint, + -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f); + + float speed_body_y_setpoint_normalized = math::interpolate(_speed_body_y_setpoint, + -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f); + + const float total_speed = fabsf(speed_body_x_setpoint_normalized) + fabsf(speed_body_y_setpoint_normalized) + fabsf( + _rover_steering_setpoint.normalized_speed_diff); + + if (total_speed > 1.f) { + const float theta = atan2f(fabsf(speed_body_y_setpoint_normalized), fabsf(speed_body_x_setpoint_normalized)); + const float magnitude = (1.f - fabsf(_rover_steering_setpoint.normalized_speed_diff)) / (sinf(theta) + cosf(theta)); + const float normalization = 1.f / (sqrtf(powf(speed_body_x_setpoint_normalized, + 2.f) + powf(speed_body_y_setpoint_normalized, 2.f))); + speed_body_x_setpoint_normalized *= magnitude * normalization; + speed_body_y_setpoint_normalized *= magnitude * normalization; + _speed_body_x_setpoint = math::interpolate(speed_body_x_setpoint_normalized, -1.f, 1.f, + -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get()); + _speed_body_y_setpoint = math::interpolate(speed_body_y_setpoint_normalized, -1.f, 1.f, + -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get()); + } + } + + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = _timestamp; + _speed_body_x_setpoint = fabsf(_speed_body_x_setpoint) > _param_ro_speed_th.get() ? _speed_body_x_setpoint : 0.f; + _speed_body_y_setpoint = fabsf(_speed_body_y_setpoint) > _param_ro_speed_th.get() ? _speed_body_y_setpoint : 0.f; + rover_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_speed_x_setpoint, _pid_speed_x, + _speed_body_x_setpoint, _vehicle_speed_body_x, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(), + _param_ro_max_thr_speed.get(), _dt); + rover_throttle_setpoint.throttle_body_y = RoverControl::speedControl(_speed_y_setpoint, _pid_speed_y, + _speed_body_y_setpoint, _vehicle_speed_body_y, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(), + _param_ro_max_thr_speed.get(), _dt); + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + + } else { // Reset controller and slew rate when position control is not active + _pid_speed_x.resetIntegral(); + _speed_x_setpoint.setForcedValue(0.f); + _pid_speed_y.resetIntegral(); + _speed_y_setpoint.setForcedValue(0.f); + } + + // Publish position controller status (logging only) + rover_velocity_status_s rover_velocity_status; + rover_velocity_status.timestamp = _timestamp; + rover_velocity_status.measured_speed_body_x = _vehicle_speed_body_x; + rover_velocity_status.speed_body_x_setpoint = _speed_body_x_setpoint; + rover_velocity_status.adjusted_speed_body_x_setpoint = _speed_x_setpoint.getState(); + rover_velocity_status.measured_speed_body_y = _vehicle_speed_body_y; + rover_velocity_status.speed_body_y_setpoint = _speed_body_y_setpoint; + rover_velocity_status.adjusted_speed_body_y_setpoint = _speed_y_setpoint.getState(); + rover_velocity_status.pid_throttle_body_x_integral = _pid_speed_x.getIntegral(); + rover_velocity_status.pid_throttle_body_y_integral = _pid_speed_y.getIntegral(); + _rover_velocity_status_pub.publish(rover_velocity_status); +} + +void MecanumPosVelControl::updateSubscriptions() +{ + if (_vehicle_control_mode_sub.updated()) { + _vehicle_control_mode_sub.copy(&_vehicle_control_mode); + } + + 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); + + if (!_global_ned_proj_ref.isInitialized() + || (_global_ned_proj_ref.getProjectionReferenceTimestamp() != vehicle_local_position.ref_timestamp)) { + _global_ned_proj_ref.initReference(vehicle_local_position.ref_lat, vehicle_local_position.ref_lon, + vehicle_local_position.ref_timestamp); + } + + _curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y); + 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_speed_body_x = fabsf(velocity_in_body_frame(0)) > _param_ro_speed_th.get() ? velocity_in_body_frame(0) : 0.f; + _vehicle_speed_body_y = fabsf(velocity_in_body_frame(1)) > _param_ro_speed_th.get() ? velocity_in_body_frame(1) : 0.f; + } + + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status; + _vehicle_status_sub.copy(&vehicle_status); + _nav_state = vehicle_status.nav_state; + } + +} + +void MecanumPosVelControl::generateAttitudeSetpoint() +{ + if (_vehicle_control_mode.flag_control_manual_enabled + && _vehicle_control_mode.flag_control_position_enabled) { // Position Mode + manualPositionMode(); + + } else if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard Control + if (_offboard_control_mode_sub.updated()) { + _offboard_control_mode_sub.copy(&_offboard_control_mode); + } + + if (_offboard_control_mode.position) { + offboardPositionMode(); + + } else if (_offboard_control_mode.velocity) { + offboardVelocityMode(); + } + + } else if (_vehicle_control_mode.flag_control_auto_enabled) { // Auto Mode + autoPositionMode(); + } +} + +void MecanumPosVelControl::manualPositionMode() +{ + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + _speed_body_x_setpoint = math::interpolate(manual_control_setpoint.throttle, + -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get()); + _speed_body_y_setpoint = math::interpolate(manual_control_setpoint.roll, + -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get()); + const float yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.yaw, + _param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + + if (fabsf(yaw_rate_setpoint) > FLT_EPSILON) { // Closed loop yaw rate control + _pos_ctl_yaw_setpoint = NAN; + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = yaw_rate_setpoint; + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + + } else if ((fabsf(_speed_body_x_setpoint) > FLT_EPSILON + || fabsf(_speed_body_y_setpoint) > + FLT_EPSILON)) { // Course control if the steering input is zero (keep driving on a straight line) + const Vector3f velocity = Vector3f(_speed_body_x_setpoint, _speed_body_y_setpoint, 0.f); + const float velocity_magnitude_setpoint = velocity.norm(); + const Vector3f pos_ctl_course_direction_local = _vehicle_attitude_quaternion.rotateVector(velocity.normalized()); + const Vector2f pos_ctl_course_direction_temp = Vector2f(pos_ctl_course_direction_local(0), + pos_ctl_course_direction_local(1)); + + // Reset course control if course direction change is above threshold + if (fabsf(asinf(pos_ctl_course_direction_temp % _pos_ctl_course_direction)) > _param_rm_course_ctl_th.get()) { + _pos_ctl_yaw_setpoint = NAN; + } + + if (!PX4_ISFINITE(_pos_ctl_yaw_setpoint)) { + _pos_ctl_start_position_ned = _curr_pos_ned; + _pos_ctl_yaw_setpoint = _vehicle_yaw; + _pos_ctl_course_direction = pos_ctl_course_direction_temp; + } + + // Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover + const float vector_scaling = sqrtf(powf(_param_pp_lookahd_max.get(), + 2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceOnLineSegment(); + const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + vector_scaling * _pos_ctl_course_direction; + const float bearing_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _pos_ctl_start_position_ned, + _curr_pos_ned, velocity_magnitude_setpoint); + const float bearing_setpoint_body_frame = matrix::wrap_pi(bearing_setpoint - _vehicle_yaw); + _speed_body_x_setpoint = velocity_magnitude_setpoint * cosf(bearing_setpoint_body_frame); + _speed_body_y_setpoint = velocity_magnitude_setpoint * sinf(bearing_setpoint_body_frame); + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = _pos_ctl_yaw_setpoint; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + + } else { // Reset course control and yaw rate setpoint + _pos_ctl_yaw_setpoint = NAN; + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = 0.f; + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + } + } +} + +void MecanumPosVelControl::offboardPositionMode() +{ + trajectory_setpoint_s trajectory_setpoint{}; + _trajectory_setpoint_sub.copy(&trajectory_setpoint); + + // Translate trajectory setpoint to rover setpoints + const Vector2f target_waypoint_ned(trajectory_setpoint.position[0], trajectory_setpoint.position[1]); + const float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm(); + + if (target_waypoint_ned.isAllFinite() && distance_to_target > _param_nav_acc_rad.get()) { + const float velocity_magnitude_setpoint = math::min(math::trajectory::computeMaxSpeedFromDistance( + _param_ro_jerk_limit.get(), + _param_ro_decel_limit.get(), distance_to_target, 0.f), _param_ro_speed_limit.get()); + const float bearing_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _curr_pos_ned, + _curr_pos_ned, fabsf(_speed_body_x_setpoint)); + const float bearing_setpoint_body_frame = matrix::wrap_pi(bearing_setpoint - _vehicle_yaw); + _speed_body_x_setpoint = velocity_magnitude_setpoint * cosf(bearing_setpoint_body_frame); + _speed_body_y_setpoint = velocity_magnitude_setpoint * sinf(bearing_setpoint_body_frame); + + } else { + _speed_body_x_setpoint = 0.f; + _speed_body_y_setpoint = 0.f; + } +} + +void MecanumPosVelControl::offboardVelocityMode() +{ + trajectory_setpoint_s trajectory_setpoint{}; + _trajectory_setpoint_sub.copy(&trajectory_setpoint); + + const Vector3f velocity_in_local_frame(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1], + trajectory_setpoint.velocity[2]); + const Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame); + + if (velocity_in_body_frame.isAllFinite()) { + _speed_body_x_setpoint = velocity_in_body_frame(0); + _speed_body_y_setpoint = velocity_in_body_frame(1); + + } else { + _speed_body_x_setpoint = 0.f; + _speed_body_y_setpoint = 0.f; + } +} + +void MecanumPosVelControl::autoPositionMode() +{ + updateAutoSubscriptions(); + + const float distance_to_curr_wp = sqrt(powf(_curr_pos_ned(0) - _curr_wp_ned(0), + 2) + powf(_curr_pos_ned(1) - _curr_wp_ned(1), 2)); + + if (_nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { // Check RTL arrival + _mission_finished = distance_to_curr_wp < _param_nav_acc_rad.get(); + } + + const float velocity_magnitude = calcVelocityMagnitude(_auto_speed, distance_to_curr_wp, _param_ro_decel_limit.get(), + _param_ro_jerk_limit.get(), _waypoint_transition_angle, _param_ro_speed_limit.get(), _param_rm_miss_spd_gain.get(), + _nav_state); + const float bearing_setpoint = _posctl_pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, + velocity_magnitude); + const float bearing_setpoint_body_frame = matrix::wrap_pi(bearing_setpoint - _vehicle_yaw); + Vector2f desired_velocity(0.f, 0.f); + _speed_body_x_setpoint = _mission_finished ? 0.f : velocity_magnitude * cosf(bearing_setpoint_body_frame); + _speed_body_y_setpoint = _mission_finished ? 0.f : velocity_magnitude * sinf(bearing_setpoint_body_frame); + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = _auto_yaw; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); +} + +void MecanumPosVelControl::updateAutoSubscriptions() +{ + 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); + } + + if (_position_setpoint_triplet_sub.updated()) { + position_setpoint_triplet_s position_setpoint_triplet{}; + _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); + + RoverControl::globalToLocalSetpointTriplet(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, position_setpoint_triplet, + _curr_pos_ned, _home_position, _global_ned_proj_ref); + + _waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(_prev_wp_ned, _curr_wp_ned, _next_wp_ned); + + // Waypoint cruising speed + _auto_speed = position_setpoint_triplet.current.cruising_speed > 0.f ? math::constrain( + position_setpoint_triplet.current.cruising_speed, 0.f, _param_ro_speed_limit.get()) : _param_ro_speed_limit.get(); + + // Waypoint yaw setpoint + if (PX4_ISFINITE(position_setpoint_triplet.current.yaw)) { + _auto_yaw = position_setpoint_triplet.current.yaw; + + } else { + _auto_yaw = _vehicle_yaw; + } + } + + if (_mission_result_sub.updated()) { + mission_result_s mission_result{}; + _mission_result_sub.copy(&mission_result); + _mission_finished = mission_result.finished; + } +} + +float MecanumPosVelControl::calcVelocityMagnitude(const float auto_speed, const float distance_to_curr_wp, + const float max_decel, const float max_jerk, const float waypoint_transition_angle, const float max_speed, + const float miss_spd_gain, const int nav_state) +{ + float velocity_magnitude{auto_speed}; + + if (max_jerk > FLT_EPSILON && max_decel > FLT_EPSILON + && miss_spd_gain > FLT_EPSILON) { + float max_velocity_magnitude = velocity_magnitude; + + if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { + max_velocity_magnitude = math::trajectory::computeMaxSpeedFromDistance(max_jerk, + max_decel, distance_to_curr_wp, 0.f); + + } else if (PX4_ISFINITE(waypoint_transition_angle)) { + const float speed_reduction = math::constrain(miss_spd_gain * math::interpolate(M_PI_F - waypoint_transition_angle, 0.f, + M_PI_F, 0.f, 1.f), 0.f, 1.f); + max_velocity_magnitude = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_decel, distance_to_curr_wp, + max_speed * (1.f - speed_reduction)); + } + + velocity_magnitude = math::constrain(max_velocity_magnitude, -auto_speed, auto_speed); + } + + return velocity_magnitude; +} + +bool MecanumPosVelControl::runSanityChecks() +{ + bool ret = true; + + if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) { + ret = false; + } + + if (_param_ro_speed_limit.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("mecanum_posVel_control_conf_invalid_speed_lim"), events::Log::Error, + "Invalid configuration of necessary parameter RO_SPEED_LIM", _param_ro_speed_limit.get()); + } + + } + + if (_param_ro_max_thr_speed.get() < FLT_EPSILON && _param_ro_speed_p.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("mecanum_posVel_control_conf_invalid_speed_control"), events::Log::Error, + "Invalid configuration for speed control: Neither feed forward (RO_MAX_THR_SPD) nor feedback (RO_SPEED_P) is setup", + _param_ro_max_thr_speed.get(), + _param_ro_speed_p.get()); + } + } + + _prev_param_check_passed = ret; + return ret; +} diff --git a/src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.hpp b/src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.hpp new file mode 100644 index 0000000000..4659588fdf --- /dev/null +++ b/src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.hpp @@ -0,0 +1,233 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 + +// Libraries +#include +#include +#include +#include +#include +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace matrix; + +/** + * @brief Class for mecanum position/velocity control. + */ +class MecanumPosVelControl : public ModuleParams +{ +public: + /** + * @brief Constructor for MecanumPosVelControl. + * @param parent The parent ModuleParams object. + */ + MecanumPosVelControl(ModuleParams *parent); + ~MecanumPosVelControl() = default; + + /** + * @brief Update position controller. + */ + void updatePosControl(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + /** + * @brief Update uORB subscriptions used in position controller. + */ + void updateSubscriptions(); + + /** + * @brief Generate and publish roverAttitudeSetpoint from manualControlSetpoint (Position Mode) or + * trajcetorySetpoint (Offboard position or velocity control) or + * positionSetpointTriplet (Auto Mode). + */ + void generateAttitudeSetpoint(); + + /** + * @brief Generate and publish roverThrottleSetpoint and roverAttitudeSetpoint or roverRateSetpoint from manualControlSetpoint. + */ + void manualPositionMode(); + + /** + * @brief Generate and publish roverAttitudeSetpoint from position of trajectorySetpoint. + */ + void offboardPositionMode(); + + /** + * @brief Generate and publish roverAttitudeSetpoint from velocity of trajectorySetpoint. + */ + void offboardVelocityMode(); + + /** + * @brief Generate and publish roverThrottleSetpoint and roverAttitudeSetpoint from positionSetpointTriplet. + */ + void autoPositionMode(); + + /** + * @brief Update uORB subscriptions used for auto modes. + */ + void updateAutoSubscriptions(); + + /** + * @brief Calculate the velocity magnitude setpoint. During waypoint transition the speed is restricted to + * Maximum_speed * (1 - normalized_transition_angle * RM_MISS_VEL_GAIN). + * On straight lines it is based on a speed trajectory such that the rover will arrive at the next waypoint transition + * with the desired waypoiny transition speed under consideration of the maximum deceleration and jerk. + * @param auto_speed Default auto speed [m/s]. + * @param distance_to_curr_wp Distance to the current waypoint [m]. + * @param max_decel Maximum allowed deceleration [m/s^2]. + * @param max_jerk Maximum allowed jerk [m/s^3]. + * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad] + * @param max_speed Maximum velocity magnitude setpoint [m/s] + * @param miss_spd_gain Tuning parameter for the speed reduction during waypoint transition. + * @param nav_state Vehicle navigation state + * @return Velocity magnitude setpoint [m/s]. + */ + float calcVelocityMagnitude(float auto_speed, float distance_to_curr_wp, float max_decel, float max_jerk, + float waypoint_transition_angle, float max_speed, float miss_spd_gain, int nav_state); + + /** + * @brief Check if the necessary parameters are set. + * @return True if all checks pass. + */ + bool runSanityChecks(); + + // uORB subscriptions + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _mission_result_sub{ORB_ID(mission_result)}; + uORB::Subscription _home_position_sub{ORB_ID(home_position)}; + uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)}; + vehicle_control_mode_s _vehicle_control_mode{}; + offboard_control_mode_s _offboard_control_mode{}; + rover_steering_setpoint_s _rover_steering_setpoint{}; + + // uORB publications + uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; + uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; + uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; + uORB::Publication _rover_velocity_status_pub{ORB_ID(rover_velocity_status)}; + uORB::Publication _position_controller_status_pub{ORB_ID(position_controller_status)}; + + // Variables + hrt_abstime _timestamp{0}; + Quatf _vehicle_attitude_quaternion{}; + Vector2d _home_position{}; + Vector2f _curr_pos_ned{}; + Vector2f _pos_ctl_course_direction{}; + Vector2f _pos_ctl_start_position_ned{}; + Vector2f _curr_wp_ned{}; + Vector2f _prev_wp_ned{}; + Vector2f _next_wp_ned{}; + float _vehicle_speed_body_x{0.f}; + float _vehicle_speed_body_y{0.f}; + float _vehicle_yaw{0.f}; + float _max_yaw_rate{0.f}; + float _speed_body_x_setpoint{0.f}; + float _speed_body_y_setpoint{0.f}; + float _pos_ctl_yaw_setpoint{0.f}; // Yaw setpoint for manual position mode, NAN if yaw rate is manually controlled [rad] + float _dt{0.f}; + float _auto_speed{0.f}; + float _auto_yaw{0.f}; + float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad] + int _nav_state{0}; + bool _mission_finished{false}; + bool _prev_param_check_passed{true}; + + // Controllers + PID _pid_speed_x; + PID _pid_speed_y; + SlewRate _speed_x_setpoint; + SlewRate _speed_y_setpoint; + + // Class Instances + PurePursuit _posctl_pure_pursuit{this}; // Pure pursuit library + MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates + + DEFINE_PARAMETERS( + (ParamFloat) _param_rm_miss_spd_gain, + (ParamFloat) _param_rm_course_ctl_th, + (ParamFloat) _param_ro_max_thr_speed, + (ParamFloat) _param_ro_speed_p, + (ParamFloat) _param_ro_speed_i, + (ParamFloat) _param_ro_yaw_stick_dz, + (ParamFloat) _param_ro_accel_limit, + (ParamFloat) _param_ro_decel_limit, + (ParamFloat) _param_ro_jerk_limit, + (ParamFloat) _param_ro_speed_limit, + (ParamFloat) _param_ro_speed_th, + (ParamFloat) _param_pp_lookahd_max, + (ParamFloat) _param_ro_yaw_rate_limit, + (ParamFloat) _param_nav_acc_rad + + ) +}; diff --git a/src/modules/rover_mecanum/MecanumRateControl/CMakeLists.txt b/src/modules/rover_mecanum/MecanumRateControl/CMakeLists.txt new file mode 100644 index 0000000000..5a876fab55 --- /dev/null +++ b/src/modules/rover_mecanum/MecanumRateControl/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2025 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. +# +############################################################################ + +px4_add_library(MecanumRateControl + MecanumRateControl.cpp +) + +target_link_libraries(MecanumRateControl PUBLIC PID) +target_include_directories(MecanumRateControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.cpp b/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.cpp new file mode 100644 index 0000000000..49f1ae5a8c --- /dev/null +++ b/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.cpp @@ -0,0 +1,190 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 "MecanumRateControl.hpp" + +using namespace time_literals; + +MecanumRateControl::MecanumRateControl(ModuleParams *parent) : ModuleParams(parent) +{ + _rover_rate_setpoint_pub.advertise(); + _rover_throttle_setpoint_pub.advertise(); + _rover_steering_setpoint_pub.advertise(); + _rover_rate_status_pub.advertise(); + updateParams(); +} + +void MecanumRateControl::updateParams() +{ + ModuleParams::updateParams(); + _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + _max_yaw_accel = _param_ro_yaw_accel_limit.get() * M_DEG_TO_RAD_F; + _max_yaw_decel = _param_ro_yaw_decel_limit.get() * M_DEG_TO_RAD_F; + _pid_yaw_rate.setGains(_param_ro_yaw_rate_p.get(), _param_ro_yaw_rate_i.get(), 0.f); + _pid_yaw_rate.setIntegralLimit(1.f); + _pid_yaw_rate.setOutputLimit(1.f); + _adjusted_yaw_rate_setpoint.setSlewRate(_max_yaw_accel); +} + +void MecanumRateControl::updateRateControl() +{ + const hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + if (_vehicle_control_mode_sub.updated()) { + _vehicle_control_mode_sub.copy(&_vehicle_control_mode); + } + + if (_vehicle_angular_velocity_sub.updated()) { + vehicle_angular_velocity_s vehicle_angular_velocity{}; + _vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity); + _vehicle_yaw_rate = fabsf(vehicle_angular_velocity.xyz[2]) > _param_ro_yaw_rate_th.get() * M_DEG_TO_RAD_F ? + vehicle_angular_velocity.xyz[2] : 0.f; + } + + if (_vehicle_control_mode.flag_control_rates_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) { + if (_vehicle_control_mode.flag_control_manual_enabled || _vehicle_control_mode.flag_control_offboard_enabled) { + generateRateSetpoint(); + } + + generateSteeringSetpoint(); + + } else { // Reset controller and slew rate when rate control is not active + _pid_yaw_rate.resetIntegral(); + _adjusted_yaw_rate_setpoint.setForcedValue(0.f); + } + + // Publish rate controller status (logging only) + rover_rate_status_s rover_rate_status; + rover_rate_status.timestamp = _timestamp; + rover_rate_status.measured_yaw_rate = _vehicle_yaw_rate; + rover_rate_status.adjusted_yaw_rate_setpoint = _adjusted_yaw_rate_setpoint.getState(); + rover_rate_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral(); + _rover_rate_status_pub.publish(rover_rate_status); + +} + +void MecanumRateControl::generateRateSetpoint() +{ + const bool acro_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled + && !_vehicle_control_mode.flag_control_position_enabled && !_vehicle_control_mode.flag_control_attitude_enabled; + + if (acro_mode_enabled && _manual_control_setpoint_sub.updated()) { // Acro Mode + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = _timestamp; + rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + rover_throttle_setpoint.throttle_body_y = manual_control_setpoint.roll; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = math::interpolate (manual_control_setpoint.yaw, -1.f, 1.f, + -_max_yaw_rate, _max_yaw_rate); + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + } + + } else if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard rate control + trajectory_setpoint_s trajectory_setpoint{}; + _trajectory_setpoint_sub.copy(&trajectory_setpoint); + + if (_offboard_control_mode_sub.updated()) { + _offboard_control_mode_sub.copy(&_offboard_control_mode); + } + + const bool offboard_rate_control = _offboard_control_mode.body_rate && !_offboard_control_mode.attitude; + + if (offboard_rate_control && PX4_ISFINITE(trajectory_setpoint.yawspeed)) { + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = trajectory_setpoint.yawspeed; + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + } + } +} + +void MecanumRateControl::generateSteeringSetpoint() +{ + if (_rover_rate_setpoint_sub.updated()) { + _rover_rate_setpoint_sub.copy(&_rover_rate_setpoint); + + } + + float speed_diff_normalized{0.f}; + + if (PX4_ISFINITE(_rover_rate_setpoint.yaw_rate_setpoint) && PX4_ISFINITE(_vehicle_yaw_rate)) { + const float yaw_rate_setpoint = fabsf(_rover_rate_setpoint.yaw_rate_setpoint) > _param_ro_yaw_rate_th.get() * + M_DEG_TO_RAD_F ? + _rover_rate_setpoint.yaw_rate_setpoint : 0.f; + speed_diff_normalized = RoverControl::rateControl(_adjusted_yaw_rate_setpoint, _pid_yaw_rate, + yaw_rate_setpoint, _vehicle_yaw_rate, _param_rm_max_thr_yaw_r.get(), _max_yaw_accel, + _max_yaw_decel, _param_rm_wheel_track.get(), _dt); + } + + rover_steering_setpoint_s rover_steering_setpoint{}; + rover_steering_setpoint.timestamp = _timestamp; + rover_steering_setpoint.normalized_speed_diff = speed_diff_normalized; + _rover_steering_setpoint_pub.publish(rover_steering_setpoint); +} + +bool MecanumRateControl::runSanityChecks() +{ + bool ret = true; + + if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("mecanum_rate_control_conf_invalid_yaw_rate_lim"), events::Log::Error, + "Invalid configuration of necessary parameter RO_YAW_RATE_LIM", _param_ro_yaw_rate_limit.get()); + } + + } + + if ((_param_rm_wheel_track.get() < FLT_EPSILON || _param_rm_max_thr_yaw_r.get() < FLT_EPSILON) + && _param_ro_yaw_rate_p.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("mecanum_rate_control_conf_invalid_rate_control"), events::Log::Error, + "Invalid configuration for rate control: Neither feed forward (RM_MAX_THR_YAW_R) nor feedback (RO_YAW_RATE_P) is setup", + _param_rm_wheel_track.get(), + _param_rm_max_thr_yaw_r.get(), _param_ro_yaw_rate_p.get()); + } + } + + _prev_param_check_passed = ret; + return ret; +} diff --git a/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.hpp b/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.hpp new file mode 100644 index 0000000000..3e61c4cb1b --- /dev/null +++ b/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.hpp @@ -0,0 +1,144 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 + +// Libraries +#include +#include +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * @brief Class for mecanum rate control. + */ +class MecanumRateControl : public ModuleParams +{ +public: + /** + * @brief Constructor for MecanumRateControl. + * @param parent The parent ModuleParams object. + */ + MecanumRateControl(ModuleParams *parent); + ~MecanumRateControl() = default; + + /** + * @brief Update rate controller. + */ + void updateRateControl(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + + /** + * @brief Generate and publish roverRateSetpoint from manualControlSetpoint (Acro Mode) + * or trajectorySetpoint (Offboard rate control). + */ + void generateRateSetpoint(); + + /** + * @brief Generate and publish roverSteeringSetpoint from RoverRateSetpoint. + */ + void generateSteeringSetpoint(); + + /** + * @brief Check if the necessary parameters are set. + * @return True if all checks pass. + */ + bool runSanityChecks(); + + // uORB subscriptions + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; + uORB::Subscription _rover_rate_setpoint_sub{ORB_ID(rover_rate_setpoint)}; + uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; + uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; + vehicle_control_mode_s _vehicle_control_mode{}; + offboard_control_mode_s _offboard_control_mode{}; + rover_rate_setpoint_s _rover_rate_setpoint{}; + + // uORB publications + uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; + uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; + uORB::Publication _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)}; + uORB::Publication _rover_rate_status_pub{ORB_ID(rover_rate_status)}; + + // Variables + hrt_abstime _timestamp{0}; + float _max_yaw_rate{0.f}; + float _max_yaw_accel{0.f}; + float _max_yaw_decel{0.f}; + float _vehicle_yaw_rate{0.f}; + float _dt{0.f}; // Time since last update [s]. + bool _prev_param_check_passed{true}; + + // Controllers + PID _pid_yaw_rate; + SlewRate _adjusted_yaw_rate_setpoint{0.f}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_rm_wheel_track, + (ParamFloat) _param_rm_max_thr_yaw_r, + (ParamFloat) _param_ro_yaw_rate_limit, + (ParamFloat) _param_ro_yaw_rate_th, + (ParamFloat) _param_ro_yaw_rate_p, + (ParamFloat) _param_ro_yaw_rate_i, + (ParamFloat) _param_ro_yaw_accel_limit, + (ParamFloat) _param_ro_yaw_decel_limit + ) +}; diff --git a/src/modules/rover_mecanum/RoverMecanum.cpp b/src/modules/rover_mecanum/RoverMecanum.cpp index b0729acc02..f881fcdc5d 100644 --- a/src/modules/rover_mecanum/RoverMecanum.cpp +++ b/src/modules/rover_mecanum/RoverMecanum.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2023-2024 PX4 Development Team. All rights reserved. + * Copyright (c) 2025 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 @@ -32,15 +32,16 @@ ****************************************************************************/ #include "RoverMecanum.hpp" -using namespace matrix; + using namespace time_literals; RoverMecanum::RoverMecanum() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) { + _rover_throttle_setpoint_pub.advertise(); + _rover_steering_setpoint_pub.advertise(); updateParams(); - _rover_mecanum_setpoint_pub.advertise(); } bool RoverMecanum::init() @@ -53,266 +54,117 @@ void RoverMecanum::updateParams() { ModuleParams::updateParams(); - _max_yaw_rate = _param_rm_max_yaw_rate.get() * M_DEG_TO_RAD_F; + if (_param_ro_accel_limit.get() > FLT_EPSILON && _param_ro_max_thr_speed.get() > FLT_EPSILON) { + _throttle_body_x_setpoint.setSlewRate(_param_ro_accel_limit.get() / _param_ro_max_thr_speed.get()); + _throttle_body_y_setpoint.setSlewRate(_param_ro_accel_limit.get() / _param_ro_max_thr_speed.get()); + } } void RoverMecanum::Run() -{ - if (should_exit()) { - ScheduleClear(); - exit_and_cleanup(); - return; - } - - updateSubscriptions(); - - // Generate and publish attitude and velocity setpoints - hrt_abstime timestamp = hrt_absolute_time(); - - switch (_nav_state) { - case vehicle_status_s::NAVIGATION_STATE_MANUAL: { - manual_control_setpoint_s manual_control_setpoint{}; - rover_mecanum_setpoint_s rover_mecanum_setpoint{}; - - if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - rover_mecanum_setpoint.timestamp = timestamp; - rover_mecanum_setpoint.forward_speed_setpoint = NAN; - rover_mecanum_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle; - rover_mecanum_setpoint.lateral_speed_setpoint = NAN; - rover_mecanum_setpoint.lateral_speed_setpoint_normalized = manual_control_setpoint.roll; - rover_mecanum_setpoint.yaw_rate_setpoint = NAN; - - if (_max_yaw_rate > FLT_EPSILON && _param_rm_max_thr_yaw_r.get() > FLT_EPSILON) { - const float scaled_yaw_rate_input = math::interpolate(manual_control_setpoint.yaw, - -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); - const float speed_diff = scaled_yaw_rate_input * _param_rm_wheel_track.get() / 2.f; - rover_mecanum_setpoint.speed_diff_setpoint_normalized = math::interpolate(speed_diff, - -_param_rm_max_thr_yaw_r.get(), _param_rm_max_thr_yaw_r.get(), -1.f, 1.f); - - } else { - rover_mecanum_setpoint.speed_diff_setpoint_normalized = manual_control_setpoint.yaw; - - } - - rover_mecanum_setpoint.yaw_setpoint = NAN; - _rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint); - - } - } break; - - case vehicle_status_s::NAVIGATION_STATE_ACRO: { - manual_control_setpoint_s manual_control_setpoint{}; - rover_mecanum_setpoint_s rover_mecanum_setpoint{}; - - if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - rover_mecanum_setpoint.timestamp = timestamp; - rover_mecanum_setpoint.forward_speed_setpoint = NAN; - rover_mecanum_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle; - rover_mecanum_setpoint.lateral_speed_setpoint = NAN; - rover_mecanum_setpoint.lateral_speed_setpoint_normalized = manual_control_setpoint.roll; - rover_mecanum_setpoint.yaw_rate_setpoint = math::interpolate(manual_control_setpoint.yaw, - -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); - rover_mecanum_setpoint.speed_diff_setpoint_normalized = NAN; - rover_mecanum_setpoint.yaw_setpoint = NAN; - _rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint); - } - } break; - - case vehicle_status_s::NAVIGATION_STATE_STAB: { - manual_control_setpoint_s manual_control_setpoint{}; - rover_mecanum_setpoint_s rover_mecanum_setpoint{}; - - if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - rover_mecanum_setpoint.timestamp = timestamp; - rover_mecanum_setpoint.forward_speed_setpoint = NAN; - rover_mecanum_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle; - rover_mecanum_setpoint.lateral_speed_setpoint = NAN; - rover_mecanum_setpoint.lateral_speed_setpoint_normalized = manual_control_setpoint.roll; - rover_mecanum_setpoint.yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.yaw, - STICK_DEADZONE), - -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); - rover_mecanum_setpoint.speed_diff_setpoint_normalized = NAN; - rover_mecanum_setpoint.yaw_setpoint = NAN; - - if (fabsf(rover_mecanum_setpoint.yaw_rate_setpoint) > FLT_EPSILON - || (fabsf(rover_mecanum_setpoint.forward_speed_setpoint_normalized) < FLT_EPSILON - && fabsf(rover_mecanum_setpoint.lateral_speed_setpoint_normalized) < FLT_EPSILON)) { // Closed loop yaw rate control - _yaw_ctl = false; - - } else { // Closed loop yaw control - - if (!_yaw_ctl) { - _desired_yaw = _vehicle_yaw; - _yaw_ctl = true; - } - - rover_mecanum_setpoint.yaw_setpoint = _desired_yaw; - rover_mecanum_setpoint.yaw_rate_setpoint = NAN; - } - - _rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint); - } - } break; - - case vehicle_status_s::NAVIGATION_STATE_POSCTL: { - manual_control_setpoint_s manual_control_setpoint{}; - rover_mecanum_setpoint_s rover_mecanum_setpoint{}; - - if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - rover_mecanum_setpoint.timestamp = timestamp; - rover_mecanum_setpoint.forward_speed_setpoint = math::interpolate(manual_control_setpoint.throttle, - -1.f, 1.f, -_param_rm_max_speed.get(), _param_rm_max_speed.get());; - rover_mecanum_setpoint.forward_speed_setpoint_normalized = NAN; - rover_mecanum_setpoint.lateral_speed_setpoint = math::interpolate(manual_control_setpoint.roll, - -1.f, 1.f, -_param_rm_max_speed.get(), _param_rm_max_speed.get()); - rover_mecanum_setpoint.lateral_speed_setpoint_normalized = NAN; - rover_mecanum_setpoint.yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.yaw, - STICK_DEADZONE), - -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); - rover_mecanum_setpoint.speed_diff_setpoint_normalized = NAN; - rover_mecanum_setpoint.yaw_setpoint = NAN; - - // Reset cruise control if the manual input changes - if (_yaw_ctl && (!(fabsf(manual_control_setpoint.throttle - _prev_throttle) > STICK_DEADZONE) - || !(fabsf(manual_control_setpoint.roll - _prev_roll) > STICK_DEADZONE))) { - _yaw_ctl = false; - _prev_throttle = manual_control_setpoint.throttle; - _prev_roll = manual_control_setpoint.roll; - - } else if (!_yaw_ctl) { - _prev_throttle = manual_control_setpoint.throttle; - _prev_roll = manual_control_setpoint.roll; - } - - - if (fabsf(rover_mecanum_setpoint.yaw_rate_setpoint) > FLT_EPSILON - || (fabsf(rover_mecanum_setpoint.forward_speed_setpoint) < FLT_EPSILON - && fabsf(rover_mecanum_setpoint.lateral_speed_setpoint) < FLT_EPSILON)) { // Closed loop yaw rate control - rover_mecanum_setpoint.yaw_rate_setpoint = math::interpolate(manual_control_setpoint.yaw, - -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); - rover_mecanum_setpoint.yaw_setpoint = NAN; - _yaw_ctl = false; - - } else { // Cruise control - const Vector3f velocity = Vector3f(rover_mecanum_setpoint.forward_speed_setpoint, - rover_mecanum_setpoint.lateral_speed_setpoint, 0.f); - const float desired_velocity_magnitude = velocity.norm(); - - if (!_yaw_ctl) { - _desired_yaw = _vehicle_yaw; - _yaw_ctl = true; - _pos_ctl_start_position_ned = _curr_pos_ned; - const Vector3f pos_ctl_course_direction_local = _vehicle_attitude_quaternion.rotateVector(velocity.normalized()); - _pos_ctl_course_direction = Vector2f(pos_ctl_course_direction_local(0), pos_ctl_course_direction_local(1)); - - } - - // Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover - const float vector_scaling = sqrtf(powf(_param_pp_lookahd_max.get(), - 2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceOnLineSegment(); - const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + vector_scaling * _pos_ctl_course_direction; - const float desired_heading = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _pos_ctl_start_position_ned, - _curr_pos_ned, desired_velocity_magnitude); - const float heading_error = matrix::wrap_pi(desired_heading - _vehicle_yaw); - const Vector2f desired_velocity = desired_velocity_magnitude * Vector2f(cosf(heading_error), sinf(heading_error)); - rover_mecanum_setpoint.forward_speed_setpoint = desired_velocity(0); - rover_mecanum_setpoint.lateral_speed_setpoint = desired_velocity(1); - rover_mecanum_setpoint.yaw_setpoint = _desired_yaw; - rover_mecanum_setpoint.yaw_rate_setpoint = NAN; - } - - _rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint); - } - } break; - - case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: - case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: - _rover_mecanum_guidance.computeGuidance(_vehicle_yaw, _nav_state); - break; - - default: // Unimplemented nav states will stop the rover - rover_mecanum_setpoint_s rover_mecanum_setpoint{}; - rover_mecanum_setpoint.timestamp = timestamp; - rover_mecanum_setpoint.forward_speed_setpoint = NAN; - rover_mecanum_setpoint.forward_speed_setpoint_normalized = 0.f; - rover_mecanum_setpoint.lateral_speed_setpoint = NAN; - rover_mecanum_setpoint.lateral_speed_setpoint_normalized = 0.f; - rover_mecanum_setpoint.yaw_rate_setpoint = NAN; - rover_mecanum_setpoint.speed_diff_setpoint_normalized = 0.f; - rover_mecanum_setpoint.yaw_setpoint = NAN; - _rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint); - break; - } - - if (!_armed) { // Reset when disarmed - _rover_mecanum_control.resetControllers(); - _yaw_ctl = false; - } - - _rover_mecanum_control.computeMotorCommands(_vehicle_yaw, _vehicle_yaw_rate, _vehicle_forward_speed, - _vehicle_lateral_speed); - -} - -void RoverMecanum::updateSubscriptions() { 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); + const hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; - if (vehicle_status.nav_state != _nav_state) { - _rover_mecanum_control.resetControllers(); - _yaw_ctl = false; + _mecanum_pos_vel_control.updatePosControl(); + _mecanum_att_control.updateAttControl(); + _mecanum_rate_control.updateRateControl(); - if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION - || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { - _rover_mecanum_guidance.setDesiredYaw(_vehicle_yaw); - } - } - - _nav_state = vehicle_status.nav_state; - _armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED; + if (_vehicle_control_mode_sub.updated()) { + _vehicle_control_mode_sub.copy(&_vehicle_control_mode); } - if (_vehicle_local_position_sub.updated()) { - vehicle_local_position_s vehicle_local_position{}; - _vehicle_local_position_sub.copy(&vehicle_local_position); - _curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y); - 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); - // Apply threshold to the velocity measurement to cut off measurement noise when standing still - _vehicle_forward_speed = fabsf(velocity_in_body_frame(0)) > SPEED_THRESHOLD ? velocity_in_body_frame(0) : 0.f; - _vehicle_lateral_speed = fabsf(velocity_in_body_frame(1)) > SPEED_THRESHOLD ? velocity_in_body_frame(1) : 0.f; + const bool full_manual_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled + && !_vehicle_control_mode.flag_control_position_enabled && !_vehicle_control_mode.flag_control_attitude_enabled + && !_vehicle_control_mode.flag_control_rates_enabled; + + if (full_manual_mode_enabled) { // Manual mode + generateSteeringSetpoint(); } - if (_vehicle_angular_velocity_sub.updated()) { - vehicle_angular_velocity_s vehicle_angular_velocity{}; - _vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity); - - // Apply threshold to the yaw rate measurement if the rover is standing still to avoid stuttering due to closed loop yaw(rate) control - if ((fabsf(_vehicle_forward_speed) > FLT_EPSILON && fabsf(_vehicle_lateral_speed) > FLT_EPSILON) - || fabsf(vehicle_angular_velocity.xyz[2]) > YAW_RATE_THRESHOLD) { - _vehicle_yaw_rate = vehicle_angular_velocity.xyz[2]; - - } else { - _vehicle_yaw_rate = 0.f; - } + if (_vehicle_control_mode.flag_armed) { + generateActuatorSetpoint(); } - 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(); +} + +void RoverMecanum::generateSteeringSetpoint() +{ + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + rover_steering_setpoint_s rover_steering_setpoint{}; + rover_steering_setpoint.timestamp = _timestamp; + rover_steering_setpoint.normalized_speed_diff = manual_control_setpoint.yaw; + _rover_steering_setpoint_pub.publish(rover_steering_setpoint); + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = _timestamp; + rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + rover_throttle_setpoint.throttle_body_y = manual_control_setpoint.roll; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + } +} + +void RoverMecanum::generateActuatorSetpoint() +{ + if (_rover_throttle_setpoint_sub.updated()) { + _rover_throttle_setpoint_sub.copy(&_rover_throttle_setpoint); } + if (_actuator_motors_sub.updated()) { + actuator_motors_s actuator_motors{}; + _actuator_motors_sub.copy(&actuator_motors); + _current_throttle_body_x = (actuator_motors.control[0] + actuator_motors.control[1]) / 2.f; + _current_throttle_body_y = (actuator_motors.control[2] - actuator_motors.control[0]) / 2.f; + } + + if (_rover_steering_setpoint_sub.updated()) { + _rover_steering_setpoint_sub.copy(&_rover_steering_setpoint); + } + + const float throttle_body_x = RoverControl::throttleControl(_throttle_body_x_setpoint, + _rover_throttle_setpoint.throttle_body_x, _current_throttle_body_x, _param_ro_accel_limit.get(), + _param_ro_decel_limit.get(), _param_ro_max_thr_speed.get(), _dt); + const float throttle_body_y = RoverControl::throttleControl(_throttle_body_y_setpoint, + _rover_throttle_setpoint.throttle_body_y, _current_throttle_body_y, _param_ro_accel_limit.get(), + _param_ro_decel_limit.get(), _param_ro_max_thr_speed.get(), _dt); + actuator_motors_s actuator_motors{}; + actuator_motors.reversible_flags = _param_r_rev.get(); + computeInverseKinematics(throttle_body_x, throttle_body_y, + _rover_steering_setpoint.normalized_speed_diff).copyTo(actuator_motors.control); + actuator_motors.timestamp = _timestamp; + _actuator_motors_pub.publish(actuator_motors); + + +} + +Vector4f RoverMecanum::computeInverseKinematics(float throttle_body_x, float throttle_body_y, + const float speed_diff_normalized) +{ + const float total_speed = fabsf(throttle_body_x) + fabsf(throttle_body_y) + fabsf(speed_diff_normalized); + + if (total_speed > 1.f) { // Adjust speed setpoints if infeasible + const float theta = atan2f(fabsf(throttle_body_y), fabsf(throttle_body_x)); + const float magnitude = (1.f - fabsf(speed_diff_normalized)) / (sinf(theta) + cosf(theta)); + const float normalization = 1.f / (sqrtf(powf(throttle_body_x, 2.f) + powf(throttle_body_y, 2.f))); + throttle_body_x *= magnitude * normalization; + throttle_body_y *= magnitude * normalization; + + } + + // Calculate motor commands + const float input_data[3] = {throttle_body_x, throttle_body_y, speed_diff_normalized}; + const Matrix input(input_data); + const float m_data[12] = {1.f, -1.f, -1.f, 1.f, 1.f, 1.f, 1.f, 1.f, -1.f, 1.f, -1.f, 1.f}; + const Matrix m(m_data); + const Vector4f motor_commands = m * input; + + return motor_commands; } int RoverMecanum::task_spawn(int argc, char *argv[]) @@ -340,7 +192,7 @@ int RoverMecanum::task_spawn(int argc, char *argv[]) int RoverMecanum::custom_command(int argc, char *argv[]) { - return print_usage("unk_timestampn command"); + return print_usage("unknown command"); } int RoverMecanum::print_usage(const char *reason) @@ -352,7 +204,7 @@ int RoverMecanum::print_usage(const char *reason) PRINT_MODULE_DESCRIPTION( R"DESCR_STR( ### Description -Rover Mecanum controller. +Rover mecanum module. )DESCR_STR"); PRINT_MODULE_USAGE_NAME("rover_mecanum", "controller"); diff --git a/src/modules/rover_mecanum/RoverMecanum.hpp b/src/modules/rover_mecanum/RoverMecanum.hpp index 7c57a1138f..1812e1c7c4 100644 --- a/src/modules/rover_mecanum/RoverMecanum.hpp +++ b/src/modules/rover_mecanum/RoverMecanum.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * Copyright (c) 2025 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 @@ -39,41 +39,34 @@ #include #include #include -#include + +// Libraries +#include +#include // uORB includes -#include #include -#include +#include +#include #include -#include -#include -#include -#include -#include - -// Standard libraries -#include -#include +#include +#include +#include +#include +#include // Local includes -#include "RoverMecanumGuidance/RoverMecanumGuidance.hpp" -#include "RoverMecanumControl/RoverMecanumControl.hpp" - -// Constants -static constexpr float YAW_RATE_THRESHOLD = - 0.02f; // [rad/s] Threshold for the yaw rate measurement to avoid stuttering when the rover is standing still -static constexpr float SPEED_THRESHOLD = - 0.1f; // [m/s] Threshold for the speed measurement to cut off measurement noise when the rover is standing still -static constexpr float STICK_DEADZONE = - 0.1f; // [0, 1] Percentage of stick input range that will be interpreted as zero around the stick centered value - -using namespace time_literals; +#include "MecanumRateControl/MecanumRateControl.hpp" +#include "MecanumAttControl/MecanumAttControl.hpp" +#include "MecanumPosVelControl/MecanumPosVelControl.hpp" class RoverMecanum : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: + /** + * @brief Constructor for RoverMecanum + */ RoverMecanum(); ~RoverMecanum() override = default; @@ -89,54 +82,69 @@ public: bool init(); protected: + /** + * @brief Update the parameters of the module. + */ void updateParams() override; private: void Run() override; /** - * @brief Update uORB subscriptions. + * @brief Generate and publish roverSteeringSetpoint from manualControlSetpoint (Manual Mode). */ - void updateSubscriptions(); + void generateSteeringSetpoint(); - // uORB Subscriptions - uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + /** + * @brief Generate and publish actuatorMotors setpoints from roverThrottleSetpoint/roverSteeringSetpoint. + */ + void generateActuatorSetpoint(); + + /** + * @brief Compute normalized motor commands based on normalized setpoints. + * @param throttle_body_x Normalized speed in body x direction [-1, 1]. + * @param throttle_body_y Normalized speed in body y direction [-1, 1]. + * @param speed_diff_normalized Speed difference between left and right wheels [-1, 1]. + * @return Motor speeds [-1, 1]. + */ + Vector4f computeInverseKinematics(float throttle_body_x, float throttle_body_y, const float speed_diff_normalized); + + // uORB subscriptions uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; - 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::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)}; + uORB::Subscription _rover_throttle_setpoint_sub{ORB_ID(rover_throttle_setpoint)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; + vehicle_control_mode_s _vehicle_control_mode{}; + rover_steering_setpoint_s _rover_steering_setpoint{}; + rover_throttle_setpoint_s _rover_throttle_setpoint{}; - // uORB Publications - uORB::Publication _rover_mecanum_setpoint_pub{ORB_ID(rover_mecanum_setpoint)}; + // uORB publications + uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; + uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; + uORB::Publication _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)}; - // Instances - RoverMecanumGuidance _rover_mecanum_guidance{this}; - RoverMecanumControl _rover_mecanum_control{this}; - PurePursuit _posctl_pure_pursuit{this}; // Pure pursuit library + // Class instances + MecanumRateControl _mecanum_rate_control{this}; + MecanumAttControl _mecanum_att_control{this}; + MecanumPosVelControl _mecanum_pos_vel_control{this}; // Variables - matrix::Quatf _vehicle_attitude_quaternion{}; - float _vehicle_yaw_rate{0.f}; - float _vehicle_forward_speed{0.f}; - float _vehicle_lateral_speed{0.f}; - float _vehicle_yaw{0.f}; - float _max_yaw_rate{0.f}; - int _nav_state{0}; - bool _yaw_ctl{false}; // Indicates if the rover is doing yaw or yaw rate control in position mode - float _desired_yaw{0.f}; // Yaw setpoint for position mode - Vector2f _pos_ctl_start_position_ned{}; - Vector2f _pos_ctl_course_direction{}; - Vector2f _curr_pos_ned{}; - float _prev_throttle{0.f}; - float _prev_roll{0.f}; - bool _armed{false}; + hrt_abstime _timestamp{0}; + float _dt{0.f}; + float _current_throttle_body_x{0.f}; + float _current_throttle_body_y{0.f}; + // Controllers + SlewRate _throttle_body_x_setpoint{0.f}; + SlewRate _throttle_body_y_setpoint{0.f}; + + // Parameters DEFINE_PARAMETERS( - (ParamFloat) _param_rm_max_speed, - (ParamFloat) _param_rm_wheel_track, - (ParamFloat) _param_rm_max_thr_yaw_r, - (ParamFloat) _param_rm_max_yaw_rate, - (ParamFloat) _param_pp_lookahd_max + (ParamInt) _param_r_rev, + (ParamFloat) _param_ro_accel_limit, + (ParamFloat) _param_ro_decel_limit, + (ParamFloat) _param_ro_max_thr_speed ) }; diff --git a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp deleted file mode 100644 index c581236784..0000000000 --- a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp +++ /dev/null @@ -1,323 +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 "RoverMecanumControl.hpp" - -#include - -using namespace matrix; -using namespace time_literals; - -RoverMecanumControl::RoverMecanumControl(ModuleParams *parent) : ModuleParams(parent) -{ - updateParams(); - _rover_mecanum_status_pub.advertise(); -} - -void RoverMecanumControl::updateParams() -{ - ModuleParams::updateParams(); - - _pid_yaw_rate.setGains(_param_rm_yaw_rate_p.get(), _param_rm_yaw_rate_i.get(), 0.f); - _pid_yaw_rate.setIntegralLimit(1.f); - _pid_yaw_rate.setOutputLimit(1.f); - - _pid_forward_throttle.setGains(_param_rm_p_gain_speed.get(), _param_rm_i_gain_speed.get(), 0.f); - _pid_forward_throttle.setIntegralLimit(1.f); - _pid_forward_throttle.setOutputLimit(1.f); - - _pid_lateral_throttle.setGains(_param_rm_p_gain_speed.get(), _param_rm_i_gain_speed.get(), 0.f); - _pid_lateral_throttle.setIntegralLimit(1.f); - _pid_lateral_throttle.setOutputLimit(1.f); - - _max_yaw_rate = _param_rm_max_yaw_rate.get() * M_DEG_TO_RAD_F; - _max_yaw_accel = _param_rm_max_yaw_accel.get() * M_DEG_TO_RAD_F; - _pid_yaw.setGains(_param_rm_p_gain_yaw.get(), _param_rm_i_gain_yaw.get(), 0.f); - _pid_yaw.setIntegralLimit(_max_yaw_rate); - _pid_yaw.setOutputLimit(_max_yaw_rate); - - // Update slew rates - if (_max_yaw_rate > FLT_EPSILON) { - _yaw_setpoint_with_yaw_rate_limit.setSlewRate(_max_yaw_rate); - } - - if (_max_yaw_accel > FLT_EPSILON) { - _yaw_rate_with_accel_limit.setSlewRate(_max_yaw_accel); - } -} - -void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const float vehicle_yaw_rate, - const float vehicle_forward_speed, const float vehicle_lateral_speed) -{ - // Timestamps - hrt_abstime timestamp_prev = _timestamp; - _timestamp = hrt_absolute_time(); - const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; - - // Update mecanum setpoint - _rover_mecanum_setpoint_sub.update(&_rover_mecanum_setpoint); - - // Closed loop yaw control - if (PX4_ISFINITE(_rover_mecanum_setpoint.yaw_setpoint)) { - _yaw_setpoint_with_yaw_rate_limit.update(matrix::wrap_pi(_rover_mecanum_setpoint.yaw_setpoint), dt); - _rover_mecanum_status.adjusted_yaw_setpoint = matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState()); - _pid_yaw.setSetpoint( - matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState() - - vehicle_yaw)); // error as setpoint to take care of wrapping - _rover_mecanum_setpoint.yaw_rate_setpoint = _pid_yaw.update(0.f, dt); - _rover_mecanum_status.clyaw_yaw_rate_setpoint = _rover_mecanum_setpoint.yaw_rate_setpoint; - - } else { - _pid_yaw.resetIntegral(); - _yaw_setpoint_with_yaw_rate_limit.setForcedValue(vehicle_yaw); - } - - // Yaw rate control - float speed_diff_normalized{0.f}; - - if (PX4_ISFINITE(_rover_mecanum_setpoint.yaw_rate_setpoint)) { // Closed loop yaw rate control - speed_diff_normalized = calcNormalizedSpeedDiff(_rover_mecanum_setpoint.yaw_rate_setpoint, vehicle_yaw_rate, - _param_rm_max_thr_yaw_r.get(), _max_yaw_accel, _param_rm_wheel_track.get(), dt, _yaw_rate_with_accel_limit, - _pid_yaw_rate, false); - _rover_mecanum_status.adjusted_yaw_rate_setpoint = _yaw_rate_with_accel_limit.getState(); - - } else { // Use normalized setpoint - speed_diff_normalized = calcNormalizedSpeedDiff(_rover_mecanum_setpoint.speed_diff_setpoint_normalized, - vehicle_yaw_rate, - _param_rm_max_thr_yaw_r.get(), _max_yaw_accel, _param_rm_wheel_track.get(), dt, _yaw_rate_with_accel_limit, - _pid_yaw_rate, true); - } - - // Speed control - float forward_speed_normalized{0.f}; - float lateral_speed_normalized{0.f}; - - if (PX4_ISFINITE(_rover_mecanum_setpoint.forward_speed_setpoint) - && PX4_ISFINITE(_rover_mecanum_setpoint.lateral_speed_setpoint)) { // Closed loop speed control - - if (_param_rm_max_thr_spd.get() > FLT_EPSILON) { - - forward_speed_normalized = math::interpolate(_rover_mecanum_setpoint.forward_speed_setpoint, - -_param_rm_max_thr_spd.get(), _param_rm_max_thr_spd.get(), -1.f, 1.f); - - lateral_speed_normalized = math::interpolate(_rover_mecanum_setpoint.lateral_speed_setpoint, - -_param_rm_max_thr_spd.get(), _param_rm_max_thr_spd.get(), -1.f, 1.f); - - const float total_speed = fabsf(forward_speed_normalized) + fabsf(lateral_speed_normalized) + fabsf( - speed_diff_normalized); - - if (total_speed > 1.f) { // Adjust speed setpoints if infeasible - const float theta = atan2f(fabsf(lateral_speed_normalized), fabsf(forward_speed_normalized)); - const float magnitude = (1.f - fabsf(speed_diff_normalized)) / (sinf(theta) + cosf(theta)); - const float normalization = 1.f / (sqrtf(powf(forward_speed_normalized, 2.f) + powf(lateral_speed_normalized, 2.f))); - forward_speed_normalized *= magnitude * normalization; - lateral_speed_normalized *= magnitude * normalization; - _rover_mecanum_setpoint.forward_speed_setpoint = math::interpolate(forward_speed_normalized, -1.f, 1.f, - -_param_rm_max_thr_spd.get(), _param_rm_max_thr_spd.get()); - _rover_mecanum_setpoint.lateral_speed_setpoint = math::interpolate(lateral_speed_normalized, -1.f, 1.f, - -_param_rm_max_thr_spd.get(), _param_rm_max_thr_spd.get()); - } - } - - forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_mecanum_setpoint.forward_speed_setpoint, - vehicle_forward_speed, _param_rm_max_thr_spd.get(), _forward_speed_setpoint_with_accel_limit, _pid_forward_throttle, - _param_rm_max_accel.get(), _param_rm_max_decel.get(), dt, false); - lateral_speed_normalized = calcNormalizedSpeedSetpoint(_rover_mecanum_setpoint.lateral_speed_setpoint, - vehicle_lateral_speed, _param_rm_max_thr_spd.get(), _lateral_speed_setpoint_with_accel_limit, _pid_lateral_throttle, - _param_rm_max_accel.get(), _param_rm_max_decel.get(), dt, false); - _rover_mecanum_status.adjusted_forward_speed_setpoint = _forward_speed_setpoint_with_accel_limit.getState(); - _rover_mecanum_status.adjusted_lateral_speed_setpoint = _lateral_speed_setpoint_with_accel_limit.getState(); - - - } else if (PX4_ISFINITE(_rover_mecanum_setpoint.forward_speed_setpoint_normalized) - && PX4_ISFINITE(_rover_mecanum_setpoint.lateral_speed_setpoint_normalized)) { // Use normalized setpoint - forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_mecanum_setpoint.forward_speed_setpoint_normalized, - vehicle_forward_speed, _param_rm_max_thr_spd.get(), _forward_speed_setpoint_with_accel_limit, _pid_forward_throttle, - _param_rm_max_accel.get(), _param_rm_max_decel.get(), dt, true); - lateral_speed_normalized = calcNormalizedSpeedSetpoint(_rover_mecanum_setpoint.lateral_speed_setpoint_normalized, - vehicle_lateral_speed, _param_rm_max_thr_spd.get(), _lateral_speed_setpoint_with_accel_limit, _pid_lateral_throttle, - _param_rm_max_accel.get(), _param_rm_max_decel.get(), dt, true); - - } - - // Publish rover mecanum status (logging) - _rover_mecanum_status.timestamp = _timestamp; - _rover_mecanum_status.measured_forward_speed = vehicle_forward_speed; - _rover_mecanum_status.measured_lateral_speed = vehicle_lateral_speed; - _rover_mecanum_status.measured_yaw_rate = vehicle_yaw_rate; - _rover_mecanum_status.measured_yaw = vehicle_yaw; - _rover_mecanum_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral(); - _rover_mecanum_status.pid_yaw_integral = _pid_yaw.getIntegral(); - _rover_mecanum_status.pid_forward_throttle_integral = _pid_forward_throttle.getIntegral(); - _rover_mecanum_status.pid_lateral_throttle_integral = _pid_lateral_throttle.getIntegral(); - _rover_mecanum_status_pub.publish(_rover_mecanum_status); - - // Publish to motors - actuator_motors_s actuator_motors{}; - actuator_motors.reversible_flags = _param_r_rev.get(); - computeInverseKinematics(forward_speed_normalized, lateral_speed_normalized, - speed_diff_normalized).copyTo(actuator_motors.control); - actuator_motors.timestamp = _timestamp; - _actuator_motors_pub.publish(actuator_motors); - -} - -float RoverMecanumControl::calcNormalizedSpeedDiff(const float yaw_rate_setpoint, const float vehicle_yaw_rate, - const float max_thr_yaw_r, - const float max_yaw_accel, const float wheel_track, const float dt, SlewRate &yaw_rate_with_accel_limit, - PID &pid_yaw_rate, const bool normalized) -{ - float slew_rate_normalization{1.f}; - - if (normalized) { // Slew rate needs to be normalized if the setpoint is normalized - slew_rate_normalization = max_thr_yaw_r > FLT_EPSILON ? max_thr_yaw_r : 0.f; - } - - if (max_yaw_accel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) { - yaw_rate_with_accel_limit.setSlewRate(max_yaw_accel / slew_rate_normalization); - yaw_rate_with_accel_limit.update(yaw_rate_setpoint, dt); - - } else { - yaw_rate_with_accel_limit.setForcedValue(yaw_rate_setpoint); - } - - // Transform yaw rate into speed difference - float speed_diff_normalized{0.f}; - - if (normalized) { - speed_diff_normalized = yaw_rate_with_accel_limit.getState(); - - } else { - if (wheel_track > FLT_EPSILON && max_thr_yaw_r > FLT_EPSILON) { // Feedforward - const float speed_diff = yaw_rate_with_accel_limit.getState() * wheel_track / - 2.f; - speed_diff_normalized = math::interpolate(speed_diff, -max_thr_yaw_r, - max_thr_yaw_r, -1.f, 1.f); - } - - _pid_yaw_rate.setSetpoint(yaw_rate_with_accel_limit.getState()); - speed_diff_normalized = math::constrain(speed_diff_normalized + - _pid_yaw_rate.update(vehicle_yaw_rate, dt), - -1.f, 1.f); // Feedback - - - } - - return math::constrain(speed_diff_normalized, -1.f, 1.f); - -} - -float RoverMecanumControl::calcNormalizedSpeedSetpoint(const float speed_setpoint, - const float vehicle_speed, const float max_thr_spd, SlewRate &speed_setpoint_with_accel_limit, - PID &pid_throttle, const float max_accel, const float max_decel, const float dt, const bool normalized) -{ - float slew_rate_normalization{1.f}; - - if (normalized) { // Slew rate needs to be normalized if the setpoint is normalized - slew_rate_normalization = max_thr_spd > FLT_EPSILON ? max_thr_spd : 0.f; - } - - // Apply acceleration and deceleration limit - if (fabsf(speed_setpoint) >= fabsf(speed_setpoint_with_accel_limit.getState())) { - if (max_accel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) { - speed_setpoint_with_accel_limit.setSlewRate(max_accel / slew_rate_normalization); - speed_setpoint_with_accel_limit.update(speed_setpoint, dt); - - } else { - speed_setpoint_with_accel_limit.setForcedValue(speed_setpoint); - - } - - } else if (max_decel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) { - speed_setpoint_with_accel_limit.setSlewRate(max_decel / slew_rate_normalization); - speed_setpoint_with_accel_limit.update(speed_setpoint, dt); - - } else { - speed_setpoint_with_accel_limit.setForcedValue(speed_setpoint); - } - - // Calculate normalized forward speed setpoint - float forward_speed_normalized{0.f}; - - if (normalized) { - forward_speed_normalized = speed_setpoint_with_accel_limit.getState(); - - } else { // Closed loop speed control - - if (_param_rm_max_thr_spd.get() > FLT_EPSILON) { // Feedforward - forward_speed_normalized = math::interpolate(speed_setpoint_with_accel_limit.getState(), - -max_thr_spd, max_thr_spd, - -1.f, 1.f); - } - - pid_throttle.setSetpoint(speed_setpoint_with_accel_limit.getState()); - forward_speed_normalized += pid_throttle.update(vehicle_speed, dt); // Feedback - - } - - return math::constrain(forward_speed_normalized, -1.f, 1.f); - -} - -matrix::Vector4f RoverMecanumControl::computeInverseKinematics(float forward_speed_normalized, - float lateral_speed_normalized, - float speed_diff) -{ - const float total_speed = fabsf(forward_speed_normalized) + fabsf(lateral_speed_normalized) + fabsf(speed_diff); - - if (total_speed > 1.f) { // Adjust speed setpoints if infeasible - const float theta = atan2f(fabsf(lateral_speed_normalized), fabsf(forward_speed_normalized)); - const float magnitude = (1.f - fabsf(speed_diff)) / (sinf(theta) + cosf(theta)); - const float normalization = 1.f / (sqrtf(powf(forward_speed_normalized, 2.f) + powf(lateral_speed_normalized, 2.f))); - forward_speed_normalized *= magnitude * normalization; - lateral_speed_normalized *= magnitude * normalization; - - } - - // Calculate motor commands - const float input_data[3] = {forward_speed_normalized, lateral_speed_normalized, speed_diff}; - const Matrix input(input_data); - const float m_data[12] = {1.f, -1.f, -1.f, 1.f, 1.f, 1.f, 1.f, 1.f, -1.f, 1.f, -1.f, 1.f}; - const Matrix m(m_data); - const Vector4f motor_commands = m * input; - - return motor_commands; -} - -void RoverMecanumControl::resetControllers() -{ - _pid_forward_throttle.resetIntegral(); - _pid_lateral_throttle.resetIntegral(); - _pid_yaw_rate.resetIntegral(); - _pid_yaw.resetIntegral(); -} diff --git a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp deleted file mode 100644 index 53ae266383..0000000000 --- a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp +++ /dev/null @@ -1,177 +0,0 @@ -/**************************************************************************** - * - * 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 - -// PX4 includes -#include - -// uORB includes -#include -#include -#include -#include -#include -#include -#include -#include - -// Standard libraries -#include -#include -#include -#include -#include - -using namespace matrix; - -/** - * @brief Class for mecanum rover guidance. - */ -class RoverMecanumControl : public ModuleParams -{ -public: - /** - * @brief Constructor for RoverMecanumControl. - * @param parent The parent ModuleParams object. - */ - RoverMecanumControl(ModuleParams *parent); - ~RoverMecanumControl() = default; - - /** - * @brief Compute motor commands based on setpoints - * @param vehicle_yaw Yaw of the vehicle [rad] - * @param vehicle_yaw_rate Yaw rate of the vehicle [rad/s] - * @param vehicle_forward_speed Forward speed of the vehicle [m/s] - * @param vehicle_lateral_speed Lateral speed of the vehicle [m/s] - */ - void computeMotorCommands(float vehicle_yaw, float vehicle_yaw_rate, float vehicle_forward_speed, - float vehicle_lateral_speed); - - /** - * @brief Reset PID controllers - */ - void resetControllers(); - -protected: - /** - * @brief Update the parameters of the module. - */ - void updateParams() override; - -private: - /** - * @brief Compute normalized speed diff setpoint between the left and right wheels and apply slew rates. - * @param yaw_rate_setpoint Yaw rate setpoint [rad/s or normalized [-1, 1]]. - * @param vehicle_yaw_rate Measured yaw rate [rad/s]. - * @param max_thr_yaw_r Yaw rate turning left/right wheels at max speed in opposite directions [m/s]. - * @param max_yaw_accel Maximum allowed yaw acceleration for the rover [rad/s^2]. - * @param wheel_track Wheel track [m]. - * @param dt Time since last update [s]. - * @param yaw_rate_with_accel_limit Yaw rate slew rate. - * @param pid_yaw_rate Yaw rate PID - * @param normalized Indicates if the forward speed setpoint is already normalized. - * @return Normalized speed differece setpoint with applied slew rates [-1, 1]. - */ - float calcNormalizedSpeedDiff(float yaw_rate_setpoint, float vehicle_yaw_rate, float max_thr_yaw_r, float max_yaw_accel, - float wheel_track, float dt, SlewRate &yaw_rate_with_accel_limit, PID &pid_yaw_rate, bool normalized); - /** - * @brief Compute normalized speed setpoint and apply slew rates. - * to the speed setpoint and doing closed loop speed control if enabled. - * @param speed_setpoint Speed setpoint [m/s]. - * @param vehicle_speed Actual speed [m/s]. - * @param max_thr_spd Speed the rover drives at maximum throttle [m/s]. - * @param speed_setpoint_with_accel_limit Speed slew rate. - * @param pid_throttle Throttle PID - * @param max_accel Maximum acceleration [m/s^2] - * @param max_decel Maximum deceleration [m/s^2] - * @param dt Time since last update [s]. - * @param normalized Indicates if the speed setpoint is already normalized. - * @return Normalized speed setpoint with applied slew rates [-1, 1]. - */ - float calcNormalizedSpeedSetpoint(float speed_setpoint, float vehicle_forward_speed, float max_thr_spd, - SlewRate &speed_setpoint_with_accel_limit, PID &pid_throttle, float max_accel, float max_decel, - float dt, bool normalized); - - /** - * @brief Turn normalized speed setpoints into normalized motor commands. - * - * @param forward_speed Normalized linear speed in body forward direction [-1, 1]. - * @param lateral_speed Normalized linear speed in body lateral direction [-1, 1]. - * @param speed_diff Normalized speed difference between left and right wheels [-1, 1]. - * @return matrix::Vector4f: Normalized motor inputs [-1, 1] - */ - matrix::Vector4f computeInverseKinematics(float forward_speed, float lateral_speed, float speed_diff); - - // uORB subscriptions - uORB::Subscription _rover_mecanum_setpoint_sub{ORB_ID(rover_mecanum_setpoint)}; - - // uORB publications - uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; - uORB::Publication _rover_mecanum_status_pub{ORB_ID(rover_mecanum_status)}; - rover_mecanum_status_s _rover_mecanum_status{}; - - // Variables - rover_mecanum_setpoint_s _rover_mecanum_setpoint{}; - hrt_abstime _timestamp{0}; - float _max_yaw_rate{0.f}; - float _max_yaw_accel{0.f}; - - // Controllers - PID _pid_forward_throttle; // PID for the closed loop forward speed control - PID _pid_lateral_throttle; // PID for the closed loop lateral speed control - PID _pid_yaw; // PID for the closed loop yaw control - PID _pid_yaw_rate; // PID for the closed loop yaw rate control - SlewRate _forward_speed_setpoint_with_accel_limit{0.f}; - SlewRate _lateral_speed_setpoint_with_accel_limit{0.f}; - SlewRate _yaw_rate_with_accel_limit{0.f}; - SlewRateYaw _yaw_setpoint_with_yaw_rate_limit; - - // Parameters - DEFINE_PARAMETERS( - (ParamFloat) _param_rm_wheel_track, - (ParamFloat) _param_rm_max_thr_spd, - (ParamFloat) _param_rm_max_accel, - (ParamFloat) _param_rm_max_decel, - (ParamFloat) _param_rm_max_thr_yaw_r, - (ParamFloat) _param_rm_max_yaw_rate, - (ParamFloat) _param_rm_max_yaw_accel, - (ParamFloat) _param_rm_yaw_rate_p, - (ParamFloat) _param_rm_yaw_rate_i, - (ParamFloat) _param_rm_p_gain_speed, - (ParamFloat) _param_rm_i_gain_speed, - (ParamFloat) _param_rm_p_gain_yaw, - (ParamFloat) _param_rm_i_gain_yaw, - (ParamInt) _param_r_rev - ) -}; diff --git a/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp b/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp deleted file mode 100644 index b72478d746..0000000000 --- a/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp +++ /dev/null @@ -1,206 +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 "RoverMecanumGuidance.hpp" - -#include - -using namespace matrix; -using namespace time_literals; - -RoverMecanumGuidance::RoverMecanumGuidance(ModuleParams *parent) : ModuleParams(parent) -{ - updateParams(); - _max_velocity_magnitude = _param_rm_max_speed.get(); - _rover_mecanum_guidance_status_pub.advertise(); -} - -void RoverMecanumGuidance::updateParams() -{ - ModuleParams::updateParams(); -} - -void RoverMecanumGuidance::computeGuidance(const float yaw, const int nav_state) -{ - updateSubscriptions(); - - // Calculate desired velocity magnitude - float desired_velocity_magnitude{_max_velocity_magnitude}; - const float distance_to_curr_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), - _curr_wp(0), _curr_wp(1)); - - if (_param_rm_max_jerk.get() > FLT_EPSILON && _param_rm_max_accel.get() > FLT_EPSILON - && _param_rm_miss_vel_gain.get() > FLT_EPSILON) { - const float speed_reduction = math::constrain(_param_rm_miss_vel_gain.get() * math::interpolate( - M_PI_F - _waypoint_transition_angle, 0.f, - M_PI_F, 0.f, 1.f), 0.f, 1.f); - desired_velocity_magnitude = math::trajectory::computeMaxSpeedFromDistance(_param_rm_max_jerk.get(), - _param_rm_max_accel.get(), distance_to_curr_wp, _max_velocity_magnitude * (1.f - speed_reduction)); - desired_velocity_magnitude = math::constrain(desired_velocity_magnitude, -_max_velocity_magnitude, - _max_velocity_magnitude); - } - - - // Calculate heading error - const float desired_heading = _pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, - desired_velocity_magnitude); - const float heading_error = matrix::wrap_pi(desired_heading - yaw); - - // Catch return to launch - const float distance_to_next_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), - _next_wp(0), _next_wp(1)); - - if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { - _mission_finished = distance_to_next_wp < _param_nav_acc_rad.get(); - } - - // Guidance logic - Vector2f desired_velocity(0.f, 0.f); - - if (!_mission_finished && distance_to_curr_wp > _param_nav_acc_rad.get()) { - desired_velocity = desired_velocity_magnitude * Vector2f(cosf(heading_error), sinf(heading_error)); - } - - // Timestamp - hrt_abstime timestamp = hrt_absolute_time(); - - // Publish mecanum controller status (logging) - rover_mecanum_guidance_status_s rover_mecanum_guidance_status{}; - rover_mecanum_guidance_status.timestamp = timestamp; - rover_mecanum_guidance_status.lookahead_distance = _pure_pursuit.getLookaheadDistance(); - rover_mecanum_guidance_status.heading_error = heading_error; - rover_mecanum_guidance_status.desired_speed = desired_velocity_magnitude; - _rover_mecanum_guidance_status_pub.publish(rover_mecanum_guidance_status); - - // Publish setpoints - rover_mecanum_setpoint_s rover_mecanum_setpoint{}; - rover_mecanum_setpoint.timestamp = timestamp; - rover_mecanum_setpoint.forward_speed_setpoint = desired_velocity(0); - rover_mecanum_setpoint.forward_speed_setpoint_normalized = NAN; - rover_mecanum_setpoint.lateral_speed_setpoint = desired_velocity(1); - rover_mecanum_setpoint.lateral_speed_setpoint_normalized = NAN; - rover_mecanum_setpoint.yaw_rate_setpoint = NAN; - rover_mecanum_setpoint.speed_diff_setpoint_normalized = NAN; - rover_mecanum_setpoint.yaw_setpoint = _desired_yaw; - _rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint); -} - -void RoverMecanumGuidance::updateSubscriptions() -{ - 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); - } -} - -void RoverMecanumGuidance::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)); - _next_wp_ned = _global_ned_proj_ref.project(_next_wp(0), _next_wp(1)); - - // Distances - const Vector2f curr_to_next_wp_ned = _next_wp_ned - _curr_wp_ned; - const Vector2f curr_to_prev_wp_ned = _prev_wp_ned - _curr_wp_ned; - 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 - _waypoint_transition_angle = acosf(cosin); - - // Waypoint cruising speed - _max_velocity_magnitude = position_setpoint_triplet.current.cruising_speed > FLT_EPSILON ? math::constrain( - position_setpoint_triplet.current.cruising_speed, 0.f, - _param_rm_max_speed.get()) : _param_rm_max_speed.get(); - - // Waypoint yaw setpoint - if (PX4_ISFINITE(position_setpoint_triplet.current.yaw)) { - _desired_yaw = position_setpoint_triplet.current.yaw; - } -} diff --git a/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.hpp b/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.hpp deleted file mode 100644 index 0970b83646..0000000000 --- a/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.hpp +++ /dev/null @@ -1,139 +0,0 @@ -/**************************************************************************** - * - * 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 - -// PX4 includes -#include -#include - -// uORB includes -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// Standard libraries -#include -#include -#include -#include -#include - -using namespace matrix; - -/** - * @brief Class for mecanum rover guidance. - */ -class RoverMecanumGuidance : public ModuleParams -{ -public: - /** - * @brief Constructor for RoverMecanumGuidance. - * @param parent The parent ModuleParams object. - */ - RoverMecanumGuidance(ModuleParams *parent); - ~RoverMecanumGuidance() = default; - - /** - * @brief Compute guidance for the vehicle. - * @param yaw The yaw orientation of the vehicle in radians. - * @param nav_state Navigation state of the rover. - */ - void computeGuidance(float yaw, int nav_state); - - void setDesiredYaw(float desired_yaw) { _desired_yaw = desired_yaw; }; - -protected: - /** - * @brief Update the parameters of the module. - */ - void updateParams() override; - -private: - /** - * @brief Update uORB subscriptions - */ - void updateSubscriptions(); - - /** - * @brief Update global/ned waypoint coordinates - */ - void updateWaypoints(); - - // 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_mecanum_guidance_status_pub{ORB_ID(rover_mecanum_guidance_status)}; - uORB::Publication _rover_mecanum_setpoint_pub{ORB_ID(rover_mecanum_setpoint)}; - - // Variables - MapProjection _global_ned_proj_ref{}; // Transform global to ned coordinates. - PurePursuit _pure_pursuit{this}; // Pure pursuit library - bool _mission_finished{false}; - - // 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{}; - Vector2f _next_wp_ned{}; - Vector2d _home_position{}; - float _max_velocity_magnitude{0.f}; - float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad] - float _desired_yaw{0.f}; - - // Parameters - DEFINE_PARAMETERS( - (ParamFloat) _param_rm_max_speed, - (ParamFloat) _param_nav_acc_rad, - (ParamFloat) _param_rm_max_jerk, - (ParamFloat) _param_rm_max_accel, - (ParamFloat) _param_rm_max_yaw_rate, - (ParamFloat) _param_rm_miss_vel_gain - ) -}; diff --git a/src/modules/rover_mecanum/module.yaml b/src/modules/rover_mecanum/module.yaml index 8d70ec90f8..ff58891ea1 100644 --- a/src/modules/rover_mecanum/module.yaml +++ b/src/modules/rover_mecanum/module.yaml @@ -3,47 +3,17 @@ module_name: Rover Mecanum parameters: - group: Rover Mecanum definitions: - RM_WHEEL_TRACK: - description: - short: Wheel track - long: Distance from the center of the right wheels to the center of the left wheels. - type: float - unit: m - min: 0.001 - max: 100 - increment: 0.001 - decimal: 3 - default: 0.5 - - RM_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 - - RM_MAX_YAW_ACCEL: - description: - short: Maximum allowed yaw acceleration for the rover - long: | - This parameter is used to cap desired yaw acceleration. This is used to adjust incoming yaw rate setpoints - to a feasible yaw rate setpoint based on the physical limitation on how fast the yaw rate can change. - This leads to a smooth setpoint trajectory for the closed loop yaw rate controller to track. - Set to -1 to disable. - type: float - unit: deg/s^2 - min: -1 - max: 1000 - increment: 0.01 - decimal: 2 - default: -1 + description: + short: Wheel track + long: Distance from the center of the right wheel to the center of the left wheel. + type: float + unit: m + min: 0 + max: 100 + increment: 0.001 + decimal: 3 + default: 0 RM_MAX_THR_YAW_R: description: @@ -52,7 +22,7 @@ parameters: This parameter is used to calculate the feedforward term of the closed loop yaw rate control. The controller first calculates the required speed difference between the left and right motor to achieve the desired yaw rate. This desired speed difference is then linearly mapped to normalized motor commands. - A good starting point is twice the speed the rover drives at maximum throttle (RM_MAX_THRTL_SPD)). + A good starting point is half the speed the rover drives at maximum throttle (RD_MAX_THR_SPD)). Increase this parameter if the rover turns faster than the setpoint, and decrease if the rover turns slower. type: float unit: m/s @@ -60,149 +30,37 @@ parameters: max: 100 increment: 0.01 decimal: 2 - default: 2 - - RM_YAW_RATE_P: - description: - short: Proportional gain for the closed-loop yaw rate controller. - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 1 - - RM_YAW_RATE_I: - description: - short: Integral gain for the closed-loop yaw rate controller. - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 2 default: 0 - RM_YAW_P: + RM_MISS_SPD_GAIN: description: - short: Proportional gain for closed loop yaw controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 1 - - RM_YAW_I: - description: - short: Integral gain for closed loop yaw controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 0.1 - - RM_MAX_SPEED: - description: - short: Maximum speed the rover is allowed to drive + short: Tuning parameter for the speed reduction during waypoint transition long: | - This parameter is used cap the maximum speed the rover is allowed to drive - and to map stick inputs to desired speeds in position mode. + The waypoint transition speed is calculated as: + Transition_speed = Maximum_speed * (1 - normalized_transition_angle * RM_MISS_VEL_GAIN) + The normalized transition angle is the angle between the line segment from prev-curr waypoint and + curr-next waypoint interpolated from [0, 180] -> [0, 1]. + Higher value -> More speed reduction during waypoint transitions. + Set to -1 to disable any speed reduction during waypoint transition. type: float - unit: m/s - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 7 - - RM_MAX_THR_SPD: - description: - short: Speed the rover drives at maximum throttle - long: | - This parameter is used to calculate the feedforward term of the closed loop speed control which linearly - maps desired speeds to normalized motor commands [-1. 1]. - A good starting point is the observed ground speed when the rover drives at maximum throttle in manual mode. - Increase this parameter if the rover is faster than the setpoint, and decrease if the rover is slower. - type: float - unit: m/s - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 1 - - RM_SPEED_P: - description: - short: Proportional gain for speed controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 3 - default: 1 - - RM_SPEED_I: - description: - short: Integral gain for ground speed controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 3 - default: 0 - - RM_MAX_JERK: - description: - short: Maximum jerk - long: Limit for forwards acc/deceleration change. - type: float - unit: m/s^3 - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 0.5 - - RM_MAX_ACCEL: - description: - short: Maximum acceleration - long: Maximum acceleration is used to limit the acceleration of the rover - type: float - unit: m/s^2 - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 0.5 - - RM_MAX_DECEL: - description: - short: Maximum deceleration - long: | - Maximum decelaration is used to limit the deceleration of the rover. - Set to -1 to disable, causing the rover to decelerate as fast as possible. - Caution: Disabling the deceleration limit also disables the slow down effect in auto modes. - type: float - unit: m/s^2 min: -1 max: 100 increment: 0.01 decimal: 2 default: -1 - RM_MISS_VEL_GAIN: + RM_COURSE_CTL_TH: description: - short: Tuning parameter for the velocity reduction during waypoint transition + short: Threshold to update course control in manual position mode long: | - The waypoint transition speed is calculated as: - Transition_speed = Maximum_speed * (1 - normalized_transition_angle * RM_MISS_VEL_GAIN) - The normalized transition angle is the angle between the line segment from prev-curr WP and curr-next WP - interpolated from [0, 180] -> [0, 1]. - Higher value -> More velocity reduction during cornering. + Threshold for the angle between the active cruise direction and the cruise direction given + by the stick inputs. + This can be understood as a deadzone for the combined stick inputs for forward/backwards + and lateral speed which defines a course direction. type: float - min: 0.05 - max: 100 + unit: rad + min: 0 + max: 3.14 increment: 0.01 decimal: 2 - default: 1 + default: 0.17 diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index 7466077a93..119c8c18e8 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -441,7 +441,6 @@ RoverPositionControl::Run() _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon); pos_ctrl_status.acceptance_radius = turn_distance; - pos_ctrl_status.yaw_acceptance = NAN; pos_ctrl_status.timestamp = hrt_absolute_time(); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 26cc3329da..97ceb6db21 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -278,32 +278,15 @@ void Sensors::diff_pres_poll() vehicle_air_data_s air_data{}; _vehicle_air_data_sub.copy(&air_data); - - float air_temperature_celsius = NAN; - - // assume anything outside of a (generous) operating range of -40C to 125C is invalid - if (PX4_ISFINITE(diff_pres.temperature) && (diff_pres.temperature >= -40.f) && (diff_pres.temperature <= 125.f)) { - - air_temperature_celsius = diff_pres.temperature; - - } else { - // differential pressure temperature invalid, check barometer - if ((air_data.timestamp != 0) && PX4_ISFINITE(air_data.baro_temp_celcius) - && (air_data.baro_temp_celcius >= -40.f) && (air_data.baro_temp_celcius <= 125.f)) { - - // TODO: review PCB_TEMP_ESTIMATE_DEG, ignore for external baro - air_temperature_celsius = air_data.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG; - } - } + const float temperature = air_data.ambient_temperature; // push raw data into validator - float airspeed_input[3] { diff_pres.differential_pressure_pa, air_temperature_celsius, 0.0f }; + float airspeed_input[3] { diff_pres.differential_pressure_pa, 0.0f, 0.0f }; _airspeed_validator.put(diff_pres.timestamp_sample, airspeed_input, diff_pres.error_count, 100); // TODO: real priority? // accumulate average for publication _diff_pres_timestamp_sum += diff_pres.timestamp_sample; _diff_pres_pressure_sum += diff_pres.differential_pressure_pa; - _diff_pres_temperature_sum += air_temperature_celsius; _baro_pressure_sum += air_data.baro_pressure_pa; _diff_pres_count++; @@ -313,12 +296,10 @@ void Sensors::diff_pres_poll() const uint64_t timestamp_sample = _diff_pres_timestamp_sum / _diff_pres_count; const float differential_pressure_pa = _diff_pres_pressure_sum / _diff_pres_count - _parameters.diff_pres_offset_pa; const float baro_pressure_pa = _baro_pressure_sum / _diff_pres_count; - const float temperature = _diff_pres_temperature_sum / _diff_pres_count; // reset _diff_pres_timestamp_sum = 0; _diff_pres_pressure_sum = 0; - _diff_pres_temperature_sum = 0; _baro_pressure_sum = 0; _diff_pres_count = 0; @@ -354,7 +335,6 @@ void Sensors::diff_pres_poll() airspeed.timestamp_sample = timestamp_sample; airspeed.indicated_airspeed_m_s = indicated_airspeed_m_s; airspeed.true_airspeed_m_s = true_airspeed_m_s; - airspeed.air_temperature_celsius = temperature; airspeed.confidence = _airspeed_validator.confidence(hrt_absolute_time()); airspeed.timestamp = hrt_absolute_time(); _airspeed_pub.publish(airspeed); diff --git a/src/modules/sensors/sensors.hpp b/src/modules/sensors/sensors.hpp index 911005249c..3a8bcd50bc 100644 --- a/src/modules/sensors/sensors.hpp +++ b/src/modules/sensors/sensors.hpp @@ -90,11 +90,7 @@ using namespace sensors; using namespace time_literals; -/** - * HACK - true temperature is much less than indicated temperature in baro, - * subtract 5 degrees in an attempt to account for the electrical upheating of the PCB - */ -#define PCB_TEMP_ESTIMATE_DEG 5.0f + class Sensors : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp index 263a56090f..a12634055f 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp @@ -45,6 +45,9 @@ using namespace matrix; using namespace atmosphere; static constexpr uint32_t SENSOR_TIMEOUT{300_ms}; +static constexpr float DEFAULT_TEMPERATURE_CELSIUS = 15.f; +static constexpr float TEMPERATURE_MIN_CELSIUS = -60.f; +static constexpr float TEMPERATURE_MAX_CELSIUS = 60.f; VehicleAirData::VehicleAirData() : ModuleParams(nullptr), @@ -77,21 +80,23 @@ void VehicleAirData::Stop() } } -void VehicleAirData::AirTemperatureUpdate() +float VehicleAirData::AirTemperatureUpdate(const float temperature_baro, TemperatureSource &source, + const hrt_abstime time_now_us) { + // use the temperature from the differential pressure sensor if available + // otherwise use the temperature from the external barometer + // Temperature measurements from internal baros are not used as typically not representative for ambient temperature + float temperature = source == TemperatureSource::EXTERNAL_BARO ? temperature_baro : DEFAULT_TEMPERATURE_CELSIUS; differential_pressure_s differential_pressure; - static constexpr float temperature_min_celsius = -20.f; - static constexpr float temperature_max_celsius = 35.f; - - // update air temperature if data from differential pressure sensor is finite and not exactly 0 - // limit the range to max 35°C to limt the error due to heated up airspeed sensors prior flight - if (_differential_pressure_sub.update(&differential_pressure) && PX4_ISFINITE(differential_pressure.temperature) - && fabsf(differential_pressure.temperature) > FLT_EPSILON) { - - _air_temperature_celsius = math::constrain(differential_pressure.temperature, temperature_min_celsius, - temperature_max_celsius); + if (_differential_pressure_sub.copy(&differential_pressure) + && time_now_us - differential_pressure.timestamp_sample < 1_s + && PX4_ISFINITE(differential_pressure.temperature)) { + temperature = differential_pressure.temperature; + source = TemperatureSource::AIRSPEED; } + + return math::constrain(temperature, TEMPERATURE_MIN_CELSIUS, TEMPERATURE_MAX_CELSIUS); } bool VehicleAirData::ParametersUpdate(bool force) @@ -139,8 +144,6 @@ void VehicleAirData::Run() const bool parameter_update = ParametersUpdate(); - AirTemperatureUpdate(); - estimator_status_flags_s estimator_status_flags; const bool estimator_status_flags_updated = _estimator_status_flags_sub.update(&estimator_status_flags); @@ -272,23 +275,26 @@ void VehicleAirData::Run() if (publish) { const float pressure_pa = _data_sum[instance] / _data_sum_count[instance]; - const float temperature = _temperature_sum[instance] / _data_sum_count[instance]; + const float temperature_baro = _temperature_sum[instance] / _data_sum_count[instance]; + TemperatureSource temperature_source = _calibration[instance].external() ? TemperatureSource::EXTERNAL_BARO : + TemperatureSource::DEFAULT_TEMP; + const float ambient_temperature = AirTemperatureUpdate(temperature_baro, temperature_source, time_now_us); const float pressure_sealevel_pa = _param_sens_baro_qnh.get() * 100.f; const float altitude = getAltitudeFromPressure(pressure_pa, pressure_sealevel_pa); // calculate air density - const float air_density = getDensityFromPressureAndTemp(pressure_pa, temperature); + const float air_density = getDensityFromPressureAndTemp(pressure_pa, ambient_temperature); // populate vehicle_air_data with and publish vehicle_air_data_s out{}; out.timestamp_sample = timestamp_sample; out.baro_device_id = _calibration[instance].device_id(); out.baro_alt_meter = altitude; - out.baro_temp_celcius = temperature; + out.ambient_temperature = ambient_temperature; + out.temperature_source = static_cast(temperature_source); out.baro_pressure_pa = pressure_pa; out.rho = air_density; - out.eas2tas = sqrtf(kAirDensitySeaLevelStandardAtmos / math::max(air_density, FLT_EPSILON)); out.calibration_count = _calibration[instance].calibration_count(); out.timestamp = hrt_absolute_time(); diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp index a0bdb4007d..1e6555fcdd 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp @@ -73,9 +73,15 @@ public: void PrintStatus(); private: + enum TemperatureSource { + DEFAULT_TEMP = 0, + EXTERNAL_BARO = 1, + AIRSPEED = 2, + }; + void Run() override; - void AirTemperatureUpdate(); + float AirTemperatureUpdate(const float temperature_baro, TemperatureSource &source, const hrt_abstime time_now_us); void CheckFailover(const hrt_abstime &time_now_us); bool ParametersUpdate(bool force = false); void UpdateStatus(); @@ -124,8 +130,6 @@ private: int8_t _selected_sensor_sub_index{-1}; - float _air_temperature_celsius{20.f}; // initialize with typical 20degC ambient temperature - bool _last_status_baro_fault{false}; DEFINE_PARAMETERS( diff --git a/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c b/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c index cb5f320bc3..cfd6fb8cf6 100644 --- a/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c +++ b/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c @@ -45,7 +45,7 @@ * @min 0 * @max 1000 * @unit Hz -* @reboot_required true +* @reboot_required false * @group Sensors */ PARAM_DEFINE_FLOAT(IMU_GYRO_NF0_FRQ, 0.0f); @@ -60,7 +60,7 @@ PARAM_DEFINE_FLOAT(IMU_GYRO_NF0_FRQ, 0.0f); * @min 0 * @max 100 * @unit Hz -* @reboot_required true +* @reboot_required false * @group Sensors */ PARAM_DEFINE_FLOAT(IMU_GYRO_NF0_BW, 20.0f); @@ -79,7 +79,7 @@ PARAM_DEFINE_FLOAT(IMU_GYRO_NF0_BW, 20.0f); * @min 0 * @max 1000 * @unit Hz -* @reboot_required true +* @reboot_required false * @group Sensors */ PARAM_DEFINE_FLOAT(IMU_GYRO_NF1_FRQ, 0.0f); @@ -94,7 +94,7 @@ PARAM_DEFINE_FLOAT(IMU_GYRO_NF1_FRQ, 0.0f); * @min 0 * @max 100 * @unit Hz -* @reboot_required true +* @reboot_required false * @group Sensors */ PARAM_DEFINE_FLOAT(IMU_GYRO_NF1_BW, 20.0f); @@ -112,7 +112,7 @@ PARAM_DEFINE_FLOAT(IMU_GYRO_NF1_BW, 20.0f); * @min 0 * @max 1000 * @unit Hz -* @reboot_required true +* @reboot_required false * @group Sensors */ PARAM_DEFINE_FLOAT(IMU_GYRO_CUTOFF, 40.0f); @@ -154,7 +154,7 @@ PARAM_DEFINE_INT32(IMU_GYRO_RATEMAX, 400); * @min 0 * @max 1000 * @unit Hz -* @reboot_required true +* @reboot_required false * @group Sensors */ PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 30.0f); diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp index df90ffdf90..a0ba14bcfd 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp @@ -209,6 +209,19 @@ void VehicleMagnetometer::UpdateMagBiasEstimate() if (_magnetometer_bias_estimate_sub.copy(&mag_bias_est)) { bool parameters_notify = false; + bool external_mag_available = false; + + for (unsigned mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) { + if (_calibration[mag_index].external() + && _calibration[mag_index].enabled() + && mag_bias_est.valid[mag_index] + && mag_bias_est.stable[mag_index]) { + + external_mag_available = true; + break; + } + } + for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) { if (mag_bias_est.valid[mag_index] && (mag_bias_est.timestamp > _last_calibration_update)) { @@ -228,6 +241,11 @@ void VehicleMagnetometer::UpdateMagBiasEstimate() const Vector3f offset = _calibration[mag_index].BiasCorrectedSensorOffset(_calibration_estimator_bias[mag_index]); if (_calibration[mag_index].set_offset(offset)) { + if (external_mag_available && !_calibration[mag_index].external()) { + // automatically disable the internal mags as they should not be used for navigation + _calibration[mag_index].disable(); + } + // save parameters with preferred calibration slot to current sensor index _calibration[mag_index].ParametersSave(mag_index); diff --git a/src/modules/simulation/battery_simulator/BatterySimulator.cpp b/src/modules/simulation/battery_simulator/BatterySimulator.cpp index 45e8fde168..92c22285bc 100644 --- a/src/modules/simulation/battery_simulator/BatterySimulator.cpp +++ b/src/modules/simulation/battery_simulator/BatterySimulator.cpp @@ -36,7 +36,7 @@ BatterySimulator::BatterySimulator() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default), - _battery(1, this, BATTERY_SIMLATOR_SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE) + _battery(1, this, BATTERY_SIMLATOR_SAMPLE_INTERVAL_US, battery_status_s::SOURCE_POWER_MODULE) { } diff --git a/src/modules/simulation/gz_bridge/CMakeLists.txt b/src/modules/simulation/gz_bridge/CMakeLists.txt index e57c602045..5fdc455cc8 100644 --- a/src/modules/simulation/gz_bridge/CMakeLists.txt +++ b/src/modules/simulation/gz_bridge/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2022-2023 PX4 Development Team. All rights reserved. +# Copyright (c) 2025 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,21 +31,42 @@ # ############################################################################ -# Find the gz_Transport library -# Look for GZ Ionic or Harmonic -find_package(gz-transport NAMES gz-transport14 gz-transport13) +if(NOT DEFINED ENV{GZ_DISTRO}) + set(GZ_DISTRO "harmonic" CACHE STRING "Gazebo distribution to use") +else() + set(GZ_DISTRO $ENV{GZ_DISTRO}) +endif() -if(gz-transport_FOUND) +# Define library version combinations for different Gazebo distributions +# https://github.com/gazebo-tooling/gazebodistro/blob/master/collection-harmonic.yaml +if(GZ_DISTRO STREQUAL "harmonic") + set(GZ_CMAKE_VERSION "3") + set(GZ_MSGS_VERSION "10") + set(GZ_TRANSPORT_VERSION "13") + set(GZ_PLUGIN_VERSION "2") + set(GZ_SIM_VERSION "8") + set(GZ_SENSORS_VERSION "8") + message(STATUS "Using Gazebo Harmonic (cmake:${GZ_CMAKE_VERSION}, msgs:${GZ_MSGS_VERSION}, transport:${GZ_TRANSPORT_VERSION})") +elseif(GZ_DISTRO STREQUAL "ionic") + set(GZ_CMAKE_VERSION "4") + set(GZ_MSGS_VERSION "11") + set(GZ_TRANSPORT_VERSION "14") + set(GZ_PLUGIN_VERSION "3") + set(GZ_SIM_VERSION "9") + set(GZ_SENSORS_VERSION "9") + message(STATUS "Using Gazebo Ionic (cmake:${GZ_CMAKE_VERSION}, msgs:${GZ_MSGS_VERSION}, transport:${GZ_TRANSPORT_VERSION})") +else() + message(FATAL_ERROR "Unknown Gazebo distribution: ${GZ_DISTRO}. Valid options are: harmonic or ionic") +endif() - add_compile_options(-frtti -fexceptions) +# Use gz-transport as litmus test for prescence of gz +find_package(gz-transport${GZ_TRANSPORT_VERSION}) - set(GZ_TRANSPORT_VER ${gz-transport_VERSION_MAJOR}) +if (gz-transport${GZ_TRANSPORT_VERSION}_FOUND) - if(GZ_TRANSPORT_VER GREATER_EQUAL 12) - set(GZ_TRANSPORT_LIB gz-transport${GZ_TRANSPORT_VER}::core) - else() - set(GZ_TRANSPORT_LIB ignition-transport${GZ_TRANSPORT_VER}::core) - endif() + find_package(gz-cmake${GZ_CMAKE_VERSION} REQUIRED) + find_package(gz-msgs${GZ_MSGS_VERSION} REQUIRED) + find_package(Protobuf REQUIRED) px4_add_module( MODULE modules__simulation__gz_bridge @@ -66,11 +87,19 @@ if(gz-transport_FOUND) DEPENDS mixer_module px4_work_queue - ${GZ_TRANSPORT_LIB} + gz-transport${GZ_TRANSPORT_VERSION}::core MODULE_CONFIG module.yaml ) + target_include_directories(modules__simulation__gz_bridge + PUBLIC + ${PX4_GZ_MSGS_BINARY_DIR} + ) + + target_include_directories(modules__simulation__gz_bridge PUBLIC px4_gz_msgs) + target_link_libraries(modules__simulation__gz_bridge PUBLIC px4_gz_msgs) + px4_add_git_submodule(TARGET git_gz PATH "${PX4_SOURCE_DIR}/Tools/simulation/gz") include(ExternalProject) ExternalProject_Add(gz @@ -82,59 +111,41 @@ if(gz-transport_FOUND) USES_TERMINAL_CONFIGURE true USES_TERMINAL_BUILD true EXCLUDE_FROM_ALL true - BUILD_ALWAYS 1 ) - set(gz_worlds - default - windy - baylands - lawn - aruco - rover - walls - ) - - # find corresponding airframes - file(GLOB gz_airframes - RELATIVE ${PX4_SOURCE_DIR}/ROMFS/px4fmu_common/init.d-posix/airframes - ${PX4_SOURCE_DIR}/ROMFS/px4fmu_common/init.d-posix/airframes/*_gz_* - ) - - # remove any .post files - foreach(gz_airframe IN LISTS gz_airframes) - if(gz_airframe MATCHES ".post") - list(REMOVE_ITEM gz_airframes ${gz_airframe}) - endif() - endforeach() - list(REMOVE_DUPLICATES gz_airframes) + # Below we setup the build targets for our worlds and models + # Syntax: gz__ + # Example: gz_x500_flow_forest + file(GLOB gz_worlds ${PX4_SOURCE_DIR}/Tools/simulation/gz/worlds/*.sdf) + file(GLOB gz_airframes ${PX4_SOURCE_DIR}/ROMFS/px4fmu_common/init.d-posix/airframes/*_gz_*) foreach(gz_airframe IN LISTS gz_airframes) - set(model_only) - string(REGEX REPLACE ".*_gz_" "" model_only ${gz_airframe}) + set(model_name) + string(REGEX REPLACE ".*_gz_" "" model_name ${gz_airframe}) foreach(world ${gz_worlds}) get_filename_component("world_name" ${world} NAME_WE) if(world_name STREQUAL "default") - add_custom_target(gz_${model_only} - COMMAND ${CMAKE_COMMAND} -E env PX4_SIM_MODEL=gz_${model_only} $ + add_custom_target(gz_${model_name} + COMMAND ${CMAKE_COMMAND} -E env PX4_SIM_MODEL=gz_${model_name} $ WORKING_DIRECTORY ${SITL_WORKING_DIR} USES_TERMINAL - DEPENDS px4 + DEPENDS px4 px4_gz_plugins ) else() - add_custom_target(gz_${model_only}_${world_name} - COMMAND ${CMAKE_COMMAND} -E env PX4_SIM_MODEL=gz_${model_only} PX4_GZ_WORLD=${world_name} $ + add_custom_target(gz_${model_name}_${world_name} + COMMAND ${CMAKE_COMMAND} -E env PX4_SIM_MODEL=gz_${model_name} PX4_GZ_WORLD=${world_name} $ WORKING_DIRECTORY ${SITL_WORKING_DIR} USES_TERMINAL - DEPENDS px4 + DEPENDS px4 px4_gz_plugins ) endif() endforeach() endforeach() - # PX4_GZ_MODELS, PX4_GZ_WORLDS, GZ_SIM_RESOURCE_PATH + + # Setup the environment variables: PX4_GZ_MODELS, PX4_GZ_WORLDS, GZ_SIM_RESOURCE_PATH configure_file(gz_env.sh.in ${PX4_BINARY_DIR}/rootfs/gz_env.sh) endif() diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 23684e76c1..6cf1734da5 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -1,20 +1,20 @@ /**************************************************************************** * - * Copyright (c) 2022-2023 PX4 Development Team. All rights reserved. + * Copyright (c) 2025 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. + * 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. + * 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. + * 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 @@ -43,24 +43,17 @@ #include #include -GZBridge::GZBridge(const char *world, const char *name, const char *model, - const char *pose_str) : +GZBridge::GZBridge(const std::string &world, const std::string &model_name) : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), _world_name(world), - _model_name(name), - _model_sim(model), - _model_pose(pose_str) + _model_name(model_name) { - pthread_mutex_init(&_node_mutex, nullptr); - updateParams(); } GZBridge::~GZBridge() { - // TODO: unsubscribe - for (auto &sub_topic : _node.SubscribedTopics()) { _node.Unsubscribe(sub_topic); } @@ -68,110 +61,6 @@ GZBridge::~GZBridge() int GZBridge::init() { - if (!_model_sim.empty()) { - - // service call to create model - gz::msgs::EntityFactory req{}; - req.set_sdf_filename(_model_sim + "/model.sdf"); - - req.set_name(_model_name); // New name for the entity, overrides the name on the SDF. - - req.set_allow_renaming(false); // allowed to rename the entity in case of overlap with existing entities - - if (!_model_pose.empty()) { - PX4_INFO("Requested Model Position: %s", _model_pose.c_str()); - - std::vector model_pose_v; - - std::stringstream ss(_model_pose); - - while (ss.good()) { - std::string substr; - std::getline(ss, substr, ','); - model_pose_v.push_back(std::stod(substr)); - } - - while (model_pose_v.size() < 6) { - model_pose_v.push_back(0.0); - } - - gz::msgs::Pose *p = req.mutable_pose(); - gz::msgs::Vector3d *position = p->mutable_position(); - position->set_x(model_pose_v[0]); - position->set_y(model_pose_v[1]); - position->set_z(model_pose_v[2]); - - gz::math::Quaterniond q(model_pose_v[3], model_pose_v[4], model_pose_v[5]); - - q.Normalize(); - gz::msgs::Quaternion *orientation = p->mutable_orientation(); - orientation->set_x(q.X()); - orientation->set_y(q.Y()); - orientation->set_z(q.Z()); - orientation->set_w(q.W()); - } - - //world/$WORLD/create service. - gz::msgs::Boolean rep; - bool result; - std::string create_service = "/world/" + _world_name + "/create"; - - bool gz_called = false; - // Check if PX4_GZ_STANDALONE has been set. - char *standalone_val = std::getenv("PX4_GZ_STANDALONE"); - - if ((standalone_val != nullptr) && (std::strcmp(standalone_val, "1") == 0)) { - // Check if Gazebo has been called and if not attempt to reconnect. - while (gz_called == false) { - if (_node.Request(create_service, req, 1000, rep, result)) { - if (!rep.data() || !result) { - PX4_ERR("EntityFactory service call failed"); - return PX4_ERROR; - - } else { - gz_called = true; - } - } - - // If Gazebo has not been called, wait 2 seconds and try again. - else { - PX4_WARN("Service call timed out as Gazebo has not been detected. Retrying..."); - system_usleep(2000000); - } - } - } - - - // 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 (!callEntityFactoryService(create_service, req)) { - return PX4_ERROR; - } - - std::string scene_info_service = "/world/" + _world_name + "/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. Retrying..."); - system_usleep(2000000); - - } else { - scene_created = true; - } - } - - gz::msgs::StringMsg follow_msg{}; - follow_msg.set_data(_model_name); - 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); - } - } - // clock std::string clock_topic = "/world/" + _world_name + "/clock"; @@ -196,8 +85,16 @@ int GZBridge::init() return PX4_ERROR; } + // mag: /world/$WORLD/model/$MODEL/link/base_link/sensor/magnetometer_sensor/magnetometer + std::string mag_topic = "/world/" + _world_name + "/model/" + _model_name + + "/link/base_link/sensor/magnetometer_sensor/magnetometer"; - // IMU: /world/$WORLD/model/$MODEL/link/base_link/sensor/imu_sensor/imu + if (!_node.Subscribe(mag_topic, &GZBridge::magnetometerCallback, this)) { + PX4_ERR("failed to subscribe to %s", mag_topic.c_str()); + return PX4_ERROR; + } + + // odom: /world/$WORLD/model/$MODEL/link/base_link/odometry_with_covariance std::string odometry_topic = "/model/" + _model_name + "/odometry_with_covariance"; if (!_node.Subscribe(odometry_topic, &GZBridge::odometryCallback, this)) { @@ -247,6 +144,14 @@ int GZBridge::init() return PX4_ERROR; } + std::string flow_topic = "/world/" + _world_name + "/model/" + _model_name + + "/link/flow_link/sensor/optical_flow/optical_flow"; + + if (!_node.Subscribe(flow_topic, &GZBridge::opticalFlowCallback, this)) { + PX4_ERR("failed to subscribe to %s", flow_topic.c_str()); + return PX4_ERROR; + } + if (!_mixing_interface_esc.init(_model_name)) { PX4_ERR("failed to init ESC output"); return PX4_ERROR; @@ -271,295 +176,178 @@ int GZBridge::init() return OK; } -int GZBridge::task_spawn(int argc, char *argv[]) +void GZBridge::clockCallback(const gz::msgs::Clock &msg) { - const char *world_name = "default"; - const char *model_name = nullptr; - const char *model_pose = nullptr; - const char *model_sim = nullptr; - const char *px4_instance = nullptr; - std::string model_name_std; - - - bool error_flag = false; - int myoptind = 1; - int ch; - const char *myoptarg = nullptr; - - while ((ch = px4_getopt(argc, argv, "w:m:p:i:n:", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'w': - // world - world_name = myoptarg; - break; - - case 'n': - // model - model_name = myoptarg; - break; - - case 'p': - // pose - model_pose = myoptarg; - break; - - case 'm': - // pose - model_sim = myoptarg; - break; - - case 'i': - // pose - px4_instance = myoptarg; - break; - - case '?': - error_flag = true; - break; - - default: - PX4_WARN("unrecognized flag"); - error_flag = true; - break; - } - } - - if (error_flag) { - return PX4_ERROR; - } - - if (!model_pose) { - model_pose = ""; - } - - if (!model_sim) { - model_sim = ""; - } - - if (!px4_instance) { - if (!model_name) { - model_name = model_sim; - } - - } else if (!model_name) { - model_name_std = std::string(model_sim) + "_" + std::string(px4_instance); - model_name = model_name_std.c_str(); - } - - PX4_INFO("world: %s, model name: %s, simulation model: %s", world_name, model_name, model_sim); - - GZBridge *instance = new GZBridge(world_name, model_name, model_sim, model_pose); - - if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; - - if (instance->init() == PX4_OK) { - -#if defined(ENABLE_LOCKSTEP_SCHEDULER) - // lockstep scheduler wait for initial clock set before returning - int sleep_count_limit = 10000; - - while ((instance->world_time_us() == 0) && sleep_count_limit > 0) { - // wait for first clock message - system_usleep(1000); - sleep_count_limit--; - } - - if (instance->world_time_us() == 0) { - PX4_ERR("timed out waiting for clock message"); - instance->request_stop(); - instance->ScheduleNow(); - - } else { - return PX4_OK; - } - -#else - return PX4_OK; -#endif // ENABLE_LOCKSTEP_SCHEDULER - - //return PX4_OK; - } - - } else { - PX4_ERR("alloc failed"); - } - - delete instance; - _object.store(nullptr); - _task_id = -1; - - return PX4_ERROR; -} - -bool GZBridge::updateClock(const uint64_t tv_sec, const uint64_t tv_nsec) -{ -#if defined(ENABLE_LOCKSTEP_SCHEDULER) + // NOTE: PX4-SITL time needs to stay in sync with gz, so this clock-sync will happen on every callback. struct timespec ts; - ts.tv_sec = tv_sec; - ts.tv_nsec = tv_nsec; - - if (px4_clock_settime(CLOCK_MONOTONIC, &ts) == 0) { - _world_time_us.store(ts_to_abstime(&ts)); - return true; - } - -#endif // ENABLE_LOCKSTEP_SCHEDULER - - return false; + ts.tv_sec = msg.sim().sec(); + ts.tv_nsec = msg.sim().nsec(); + px4_clock_settime(CLOCK_MONOTONIC, &ts); } -void GZBridge::clockCallback(const gz::msgs::Clock &clock) +void GZBridge::opticalFlowCallback(const px4::msgs::OpticalFlow &msg) { - pthread_mutex_lock(&_node_mutex); + sensor_optical_flow_s report = {}; - const uint64_t time_us = (clock.sim().sec() * 1000000) + (clock.sim().nsec() / 1000); + report.timestamp = hrt_absolute_time(); + report.timestamp_sample = msg.time_usec(); + report.pixel_flow[0] = msg.integrated_x(); + report.pixel_flow[1] = msg.integrated_y(); + report.quality = msg.quality(); + report.integration_timespan_us = msg.integration_time_us(); - if (time_us > _world_time_us.load()) { - updateClock(clock.sim().sec(), clock.sim().nsec()); - } + // Static data + device::Device::DeviceId id; + id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; + id.devid_s.bus = 0; + id.devid_s.address = 0; + id.devid_s.devtype = DRV_FLOW_DEVTYPE_SIM; + report.device_id = id.devid; - pthread_mutex_unlock(&_node_mutex); + // values taken from PAW3902 + report.mode = sensor_optical_flow_s::MODE_LOWLIGHT; + report.max_flow_rate = 7.4f; + report.min_ground_distance = 0.f; + report.max_ground_distance = 30.f; + report.error_count = 0; + + // No delta angle + // No distance + // This means that delta angle will come from vehicle gyro + // Distance will come from vehicle distance sensor + + _optical_flow_pub.publish(report); } -void GZBridge::barometerCallback(const gz::msgs::FluidPressure &air_pressure) +void GZBridge::magnetometerCallback(const gz::msgs::Magnetometer &msg) { - if (hrt_absolute_time() == 0) { - return; - } + const uint64_t timestamp = hrt_absolute_time(); - pthread_mutex_lock(&_node_mutex); + device::Device::DeviceId id{}; + id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; + id.devid_s.devtype = DRV_MAG_DEVTYPE_MAGSIM; + id.devid_s.bus = 1; + id.devid_s.address = 3; // TODO: any value other than 3 causes Commander to not use the mag.... wtf - const uint64_t time_us = (air_pressure.header().stamp().sec() * 1000000) - + (air_pressure.header().stamp().nsec() / 1000); + sensor_mag_s report{}; + report.timestamp = timestamp; + report.timestamp_sample = timestamp; + report.device_id = id.devid; + report.temperature = this->_temperature; - // publish - sensor_baro_s sensor_baro{}; - sensor_baro.timestamp_sample = time_us; - sensor_baro.device_id = 6620172; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION - sensor_baro.pressure = air_pressure.pressure(); - sensor_baro.temperature = this->_temperature; - sensor_baro.timestamp = hrt_absolute_time(); - _sensor_baro_pub.publish(sensor_baro); + // FIMEX: once we're on jetty or later + // The magnetometer plugin publishes in units of gauss and in a weird left handed coordinate system + // https://github.com/gazebosim/gz-sim/pull/2460 + report.x = -msg.field_tesla().y(); + report.y = -msg.field_tesla().x(); + report.z = msg.field_tesla().z(); - pthread_mutex_unlock(&_node_mutex); + _sensor_mag_pub.publish(report); +} + +void GZBridge::barometerCallback(const gz::msgs::FluidPressure &msg) +{ + const uint64_t timestamp = hrt_absolute_time(); + + device::Device::DeviceId id{}; + id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; + id.devid_s.devtype = DRV_BARO_DEVTYPE_BAROSIM; + id.devid_s.bus = 1; + id.devid_s.address = 1; + + sensor_baro_s report{}; + report.timestamp = timestamp; + report.timestamp_sample = timestamp; + report.device_id = id.devid; + report.pressure = msg.pressure(); + report.temperature = this->_temperature; + _sensor_baro_pub.publish(report); } -void GZBridge::airspeedCallback(const gz::msgs::AirSpeed &air_speed) +void GZBridge::airspeedCallback(const gz::msgs::AirSpeed &msg) { - if (hrt_absolute_time() == 0) { - return; - } + const uint64_t timestamp = hrt_absolute_time(); - pthread_mutex_lock(&_node_mutex); - - const uint64_t time_us = (air_speed.header().stamp().sec() * 1000000) - + (air_speed.header().stamp().nsec() / 1000); - - double air_speed_value = air_speed.diff_pressure(); + device::Device::DeviceId id{}; + id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; + id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_SIM; + id.devid_s.bus = 1; + id.devid_s.address = 1; differential_pressure_s report{}; - report.timestamp_sample = time_us; - report.device_id = 1377548; // 1377548: DRV_DIFF_PRESS_DEVTYPE_SIM, BUS: 1, ADDR: 5, TYPE: SIMULATION - report.differential_pressure_pa = static_cast(air_speed_value); // hPa to Pa; - report.temperature = static_cast(air_speed.temperature()) + atmosphere::kAbsoluteNullCelsius; // K to C - report.timestamp = hrt_absolute_time();; + report.timestamp = timestamp; + report.timestamp_sample = timestamp; + report.device_id = id.devid; + report.differential_pressure_pa = msg.diff_pressure(); // hPa to Pa; + report.temperature = static_cast(msg.temperature()) + atmosphere::kAbsoluteNullCelsius; // K to C _differential_pressure_pub.publish(report); this->_temperature = report.temperature; - - pthread_mutex_unlock(&_node_mutex); } -void GZBridge::imuCallback(const gz::msgs::IMU &imu) +void GZBridge::imuCallback(const gz::msgs::IMU &msg) { - if (hrt_absolute_time() == 0) { - return; - } - - pthread_mutex_lock(&_node_mutex); - - const uint64_t time_us = (imu.header().stamp().sec() * 1000000) + (imu.header().stamp().nsec() / 1000); - - if (time_us > _world_time_us.load()) { - updateClock(imu.header().stamp().sec(), imu.header().stamp().nsec()); - } + const uint64_t timestamp = hrt_absolute_time(); // FLU -> FRD static const auto q_FLU_to_FRD = gz::math::Quaterniond(0, 1, 0, 0); gz::math::Vector3d accel_b = q_FLU_to_FRD.RotateVector(gz::math::Vector3d( - imu.linear_acceleration().x(), - imu.linear_acceleration().y(), - imu.linear_acceleration().z())); + msg.linear_acceleration().x(), + msg.linear_acceleration().y(), + msg.linear_acceleration().z())); + + device::Device::DeviceId id{}; + id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; + id.devid_s.devtype = DRV_IMU_DEVTYPE_SIM; + id.devid_s.bus = 1; + id.devid_s.address = 1; // publish accel - sensor_accel_s sensor_accel{}; -#if defined(ENABLE_LOCKSTEP_SCHEDULER) - sensor_accel.timestamp_sample = time_us; - sensor_accel.timestamp = time_us; -#else - sensor_accel.timestamp_sample = hrt_absolute_time(); - sensor_accel.timestamp = hrt_absolute_time(); -#endif - sensor_accel.device_id = 1310988; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION - sensor_accel.x = accel_b.X(); - sensor_accel.y = accel_b.Y(); - sensor_accel.z = accel_b.Z(); - sensor_accel.temperature = NAN; - sensor_accel.samples = 1; - _sensor_accel_pub.publish(sensor_accel); + sensor_accel_s accel{}; + accel.timestamp_sample = timestamp; + accel.timestamp = timestamp; + accel.device_id = id.devid; + + accel.x = accel_b.X(); + accel.y = accel_b.Y(); + accel.z = accel_b.Z(); + accel.temperature = NAN; + accel.samples = 1; + _sensor_accel_pub.publish(accel); gz::math::Vector3d gyro_b = q_FLU_to_FRD.RotateVector(gz::math::Vector3d( - imu.angular_velocity().x(), - imu.angular_velocity().y(), - imu.angular_velocity().z())); + msg.angular_velocity().x(), + msg.angular_velocity().y(), + msg.angular_velocity().z())); // publish gyro - sensor_gyro_s sensor_gyro{}; -#if defined(ENABLE_LOCKSTEP_SCHEDULER) - sensor_gyro.timestamp_sample = time_us; - sensor_gyro.timestamp = time_us; -#else - sensor_gyro.timestamp_sample = hrt_absolute_time(); - sensor_gyro.timestamp = hrt_absolute_time(); -#endif - sensor_gyro.device_id = 1310988; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION - sensor_gyro.x = gyro_b.X(); - sensor_gyro.y = gyro_b.Y(); - sensor_gyro.z = gyro_b.Z(); - sensor_gyro.temperature = NAN; - sensor_gyro.samples = 1; - _sensor_gyro_pub.publish(sensor_gyro); - - pthread_mutex_unlock(&_node_mutex); + sensor_gyro_s gyro{}; + gyro.timestamp_sample = timestamp; + gyro.timestamp = timestamp; + gyro.device_id = id.devid; + gyro.x = gyro_b.X(); + gyro.y = gyro_b.Y(); + gyro.z = gyro_b.Z(); + gyro.temperature = NAN; + gyro.samples = 1; + _sensor_gyro_pub.publish(gyro); } -void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose) +void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &msg) { - if (hrt_absolute_time() == 0) { - return; - } + const uint64_t timestamp = hrt_absolute_time(); - pthread_mutex_lock(&_node_mutex); + for (int p = 0; p < msg.pose_size(); p++) { + if (msg.pose(p).name() == _model_name) { - for (int p = 0; p < pose.pose_size(); p++) { - if (pose.pose(p).name() == _model_name) { + const double dt = math::constrain((timestamp - _timestamp_prev) * 1e-6, 0.001, 0.1); + _timestamp_prev = timestamp; - const uint64_t time_us = (pose.header().stamp().sec() * 1000000) + (pose.header().stamp().nsec() / 1000); - - if (time_us > _world_time_us.load()) { - updateClock(pose.header().stamp().sec(), pose.header().stamp().nsec()); - } - - const double dt = math::constrain((time_us - _timestamp_prev) * 1e-6, 0.001, 0.1); - _timestamp_prev = time_us; - - gz::msgs::Vector3d pose_position = pose.pose(p).position(); - gz::msgs::Quaternion pose_orientation = pose.pose(p).orientation(); + gz::msgs::Vector3d pose_position = msg.pose(p).position(); + gz::msgs::Quaternion pose_orientation = msg.pose(p).orientation(); // ground truth gz::math::Quaterniond q_gr = gz::math::Quaterniond( @@ -573,39 +361,27 @@ void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose) // publish attitude groundtruth vehicle_attitude_s vehicle_attitude_groundtruth{}; -#if defined(ENABLE_LOCKSTEP_SCHEDULER) - vehicle_attitude_groundtruth.timestamp_sample = time_us; -#else - vehicle_attitude_groundtruth.timestamp_sample = hrt_absolute_time(); -#endif + vehicle_attitude_groundtruth.timestamp_sample = timestamp; vehicle_attitude_groundtruth.q[0] = q_nb.W(); vehicle_attitude_groundtruth.q[1] = q_nb.X(); vehicle_attitude_groundtruth.q[2] = q_nb.Y(); vehicle_attitude_groundtruth.q[3] = q_nb.Z(); - vehicle_attitude_groundtruth.timestamp = hrt_absolute_time(); + vehicle_attitude_groundtruth.timestamp = timestamp; _attitude_ground_truth_pub.publish(vehicle_attitude_groundtruth); // publish angular velocity groundtruth const matrix::Eulerf euler{matrix::Quatf(vehicle_attitude_groundtruth.q)}; vehicle_angular_velocity_s vehicle_angular_velocity_groundtruth{}; -#if defined(ENABLE_LOCKSTEP_SCHEDULER) - vehicle_angular_velocity_groundtruth.timestamp_sample = time_us; -#else - vehicle_angular_velocity_groundtruth.timestamp_sample = hrt_absolute_time(); -#endif + vehicle_angular_velocity_groundtruth.timestamp_sample = timestamp; const matrix::Vector3f angular_velocity = (euler - _euler_prev) / dt; _euler_prev = euler; angular_velocity.copyTo(vehicle_angular_velocity_groundtruth.xyz); - vehicle_angular_velocity_groundtruth.timestamp = hrt_absolute_time(); + vehicle_angular_velocity_groundtruth.timestamp = timestamp; _angular_velocity_ground_truth_pub.publish(vehicle_angular_velocity_groundtruth); vehicle_local_position_s local_position_groundtruth{}; -#if defined(ENABLE_LOCKSTEP_SCHEDULER) - local_position_groundtruth.timestamp_sample = time_us; -#else - local_position_groundtruth.timestamp_sample = hrt_absolute_time(); -#endif + local_position_groundtruth.timestamp_sample = timestamp; // position ENU -> NED const matrix::Vector3d position{pose_position.y(), pose_position.x(), -pose_position.z()}; const matrix::Vector3d velocity{(position - _position_prev) / dt}; @@ -644,48 +420,29 @@ void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose) local_position_groundtruth.z_global = false; } - local_position_groundtruth.timestamp = hrt_absolute_time(); + local_position_groundtruth.timestamp = timestamp; _lpos_ground_truth_pub.publish(local_position_groundtruth); - - pthread_mutex_unlock(&_node_mutex); return; } } - - pthread_mutex_unlock(&_node_mutex); } -void GZBridge::odometryCallback(const gz::msgs::OdometryWithCovariance &odometry) +void GZBridge::odometryCallback(const gz::msgs::OdometryWithCovariance &msg) { - if (hrt_absolute_time() == 0) { - return; - } + const uint64_t timestamp = hrt_absolute_time(); - pthread_mutex_lock(&_node_mutex); - - const uint64_t time_us = (odometry.header().stamp().sec() * 1000000) + (odometry.header().stamp().nsec() / 1000); - - if (time_us > _world_time_us.load()) { - updateClock(odometry.header().stamp().sec(), odometry.header().stamp().nsec()); - } - - vehicle_odometry_s odom{}; -#if defined(ENABLE_LOCKSTEP_SCHEDULER) - odom.timestamp_sample = time_us; - odom.timestamp = time_us; -#else - odom.timestamp_sample = hrt_absolute_time(); - odom.timestamp = hrt_absolute_time(); -#endif + vehicle_odometry_s report{}; + report.timestamp_sample = timestamp; + report.timestamp = timestamp; // gz odometry position is in ENU frame and needs to be converted to NED - odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED; - odom.position[0] = odometry.pose_with_covariance().pose().position().y(); - odom.position[1] = odometry.pose_with_covariance().pose().position().x(); - odom.position[2] = -odometry.pose_with_covariance().pose().position().z(); + report.pose_frame = vehicle_odometry_s::POSE_FRAME_NED; + report.position[0] = msg.pose_with_covariance().pose().position().y(); + report.position[1] = msg.pose_with_covariance().pose().position().x(); + report.position[2] = -msg.pose_with_covariance().pose().position().z(); // gz odometry orientation is "body FLU->ENU" and needs to be converted in "body FRD->NED" - gz::msgs::Quaternion pose_orientation = odometry.pose_with_covariance().pose().orientation(); + gz::msgs::Quaternion pose_orientation = msg.pose_with_covariance().pose().orientation(); gz::math::Quaterniond q_gr = gz::math::Quaterniond( pose_orientation.w(), pose_orientation.x(), @@ -693,101 +450,214 @@ void GZBridge::odometryCallback(const gz::msgs::OdometryWithCovariance &odometry pose_orientation.z()); gz::math::Quaterniond q_nb; GZBridge::rotateQuaternion(q_nb, q_gr); - odom.q[0] = q_nb.W(); - odom.q[1] = q_nb.X(); - odom.q[2] = q_nb.Y(); - odom.q[3] = q_nb.Z(); + report.q[0] = q_nb.W(); + report.q[1] = q_nb.X(); + report.q[2] = q_nb.Y(); + report.q[3] = q_nb.Z(); // gz odometry linear velocity is in body FLU and needs to be converted in body FRD - odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_BODY_FRD; - odom.velocity[0] = odometry.twist_with_covariance().twist().linear().x(); - odom.velocity[1] = -odometry.twist_with_covariance().twist().linear().y(); - odom.velocity[2] = -odometry.twist_with_covariance().twist().linear().z(); + report.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_BODY_FRD; + report.velocity[0] = msg.twist_with_covariance().twist().linear().x(); + report.velocity[1] = -msg.twist_with_covariance().twist().linear().y(); + report.velocity[2] = -msg.twist_with_covariance().twist().linear().z(); // gz odometry angular velocity is in body FLU and need to be converted in body FRD - odom.angular_velocity[0] = odometry.twist_with_covariance().twist().angular().x(); - odom.angular_velocity[1] = -odometry.twist_with_covariance().twist().angular().y(); - odom.angular_velocity[2] = -odometry.twist_with_covariance().twist().angular().z(); + report.angular_velocity[0] = msg.twist_with_covariance().twist().angular().x(); + report.angular_velocity[1] = -msg.twist_with_covariance().twist().angular().y(); + report.angular_velocity[2] = -msg.twist_with_covariance().twist().angular().z(); // VISION_POSITION_ESTIMATE covariance // pose 6x6 cross-covariance matrix // (states: x, y, z, roll, pitch, yaw). // If unknown, assign NaN value to first element in the array. - odom.position_variance[0] = odometry.pose_with_covariance().covariance().data(7); // Y row 1, col 1 - odom.position_variance[1] = odometry.pose_with_covariance().covariance().data(0); // X row 0, col 0 - odom.position_variance[2] = odometry.pose_with_covariance().covariance().data(14); // Z row 2, col 2 + report.position_variance[0] = msg.pose_with_covariance().covariance().data(7); // Y row 1, col 1 + report.position_variance[1] = msg.pose_with_covariance().covariance().data(0); // X row 0, col 0 + report.position_variance[2] = msg.pose_with_covariance().covariance().data(14); // Z row 2, col 2 - odom.orientation_variance[0] = odometry.pose_with_covariance().covariance().data(21); // R row 3, col 3 - odom.orientation_variance[1] = odometry.pose_with_covariance().covariance().data(28); // P row 4, col 4 - odom.orientation_variance[2] = odometry.pose_with_covariance().covariance().data(35); // Y row 5, col 5 + report.orientation_variance[0] = msg.pose_with_covariance().covariance().data(21); // R row 3, col 3 + report.orientation_variance[1] = msg.pose_with_covariance().covariance().data(28); // P row 4, col 4 + report.orientation_variance[2] = msg.pose_with_covariance().covariance().data(35); // Y row 5, col 5 - odom.velocity_variance[0] = odometry.twist_with_covariance().covariance().data(7); // Y row 1, col 1 - odom.velocity_variance[1] = odometry.twist_with_covariance().covariance().data(0); // X row 0, col 0 - odom.velocity_variance[2] = odometry.twist_with_covariance().covariance().data(14); // Z row 2, col 2 + report.velocity_variance[0] = msg.twist_with_covariance().covariance().data(7); // Y row 1, col 1 + report.velocity_variance[1] = msg.twist_with_covariance().covariance().data(0); // X row 0, col 0 + report.velocity_variance[2] = msg.twist_with_covariance().covariance().data(14); // Z row 2, col 2 - // odom.reset_counter = vpe.reset_counter; - _visual_odometry_pub.publish(odom); - - pthread_mutex_unlock(&_node_mutex); + // report.reset_counter = vpe.reset_counter; + _visual_odometry_pub.publish(report); } -void GZBridge::navSatCallback(const gz::msgs::NavSat &nav_sat) +static float generate_wgn() { - if (hrt_absolute_time() == 0) { - return; + // generate white Gaussian noise sample with std=1 + + // algorithm 1: + // float temp=((float)(rand()+1))/(((float)RAND_MAX+1.0f)); + // return sqrtf(-2.0f*logf(temp))*cosf(2.0f*M_PI_F*rand()/RAND_MAX); + // algorithm 2: from BlockRandGauss.hpp + static float V1, V2, S; + static bool phase = true; + float X; + + if (phase) { + do { + float U1 = (float)rand() / (float)RAND_MAX; + float U2 = (float)rand() / (float)RAND_MAX; + V1 = 2.0f * U1 - 1.0f; + V2 = 2.0f * U2 - 1.0f; + S = V1 * V1 + V2 * V2; + } while (S >= 1.0f || fabsf(S) < 1e-8f); + + X = V1 * float(sqrtf(-2.0f * float(logf(S)) / S)); + + } else { + X = V2 * float(sqrtf(-2.0f * float(logf(S)) / S)); } - pthread_mutex_lock(&_node_mutex); + phase = !phase; + return X; +} - const uint64_t time_us = (nav_sat.header().stamp().sec() * 1000000) + (nav_sat.header().stamp().nsec() / 1000); +void GZBridge::addGpsNoise(double &latitude, double &longitude, double &altitude, + float &vel_north, float &vel_east, float &vel_down) +{ + _gps_pos_noise_n = _pos_markov_time * _gps_pos_noise_n + + _pos_random_walk * generate_wgn() * _pos_noise_amplitude - + 0.02f * _gps_pos_noise_n; - if (time_us > _world_time_us.load()) { - updateClock(nav_sat.header().stamp().sec(), nav_sat.header().stamp().nsec()); - } + _gps_pos_noise_e = _pos_markov_time * _gps_pos_noise_e + + _pos_random_walk * generate_wgn() * _pos_noise_amplitude - + 0.02f * _gps_pos_noise_e; + + _gps_pos_noise_d = _pos_markov_time * _gps_pos_noise_d + + _pos_random_walk * generate_wgn() * _pos_noise_amplitude * 1.5f - + 0.02f * _gps_pos_noise_d; + + latitude += math::degrees((double)_gps_pos_noise_n / CONSTANTS_RADIUS_OF_EARTH); + longitude += math::degrees((double)_gps_pos_noise_e / CONSTANTS_RADIUS_OF_EARTH); + altitude += (double)_gps_pos_noise_d; + + _gps_vel_noise_n = _vel_markov_time * _gps_vel_noise_n + + _vel_noise_density * generate_wgn() * _vel_noise_amplitude; + + _gps_vel_noise_e = _vel_markov_time * _gps_vel_noise_e + + _vel_noise_density * generate_wgn() * _vel_noise_amplitude; + + _gps_vel_noise_d = _vel_markov_time * _gps_vel_noise_d + + _vel_noise_density * generate_wgn() * _vel_noise_amplitude * 1.2f; + + vel_north += _gps_vel_noise_n; + vel_east += _gps_vel_noise_e; + vel_down += _gps_vel_noise_d; +} + +void GZBridge::navSatCallback(const gz::msgs::NavSat &msg) +{ + const uint64_t timestamp = hrt_absolute_time(); // initialize gps position if (!_pos_ref.isInitialized()) { - _pos_ref.initReference(nav_sat.latitude_deg(), nav_sat.longitude_deg(), hrt_absolute_time()); - _alt_ref = nav_sat.altitude(); - - } else { - // publish GPS groundtruth - vehicle_global_position_s global_position_groundtruth{}; -#if defined(ENABLE_LOCKSTEP_SCHEDULER) - global_position_groundtruth.timestamp_sample = time_us; -#else - global_position_groundtruth.timestamp_sample = hrt_absolute_time(); -#endif - global_position_groundtruth.lat = nav_sat.latitude_deg(); - global_position_groundtruth.lon = nav_sat.longitude_deg(); - global_position_groundtruth.alt = nav_sat.altitude(); - _gpos_ground_truth_pub.publish(global_position_groundtruth); - } - - pthread_mutex_unlock(&_node_mutex); -} -void GZBridge::laserScantoLidarSensorCallback(const gz::msgs::LaserScan &scan) -{ - if (hrt_absolute_time() == 0) { + _pos_ref.initReference(msg.latitude_deg(), msg.longitude_deg(), timestamp); + _alt_ref = msg.altitude(); return; } - distance_sensor_s distance_sensor{}; - distance_sensor.timestamp = hrt_absolute_time(); - device::Device::DeviceId id; - id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; - id.devid_s.bus = 0; - id.devid_s.address = 0; - id.devid_s.devtype = DRV_DIST_DEVTYPE_SIM; - distance_sensor.device_id = id.devid; - distance_sensor.min_distance = static_cast(scan.range_min()); - distance_sensor.max_distance = static_cast(scan.range_max()); - distance_sensor.current_distance = static_cast(scan.ranges()[0]); - distance_sensor.variance = 0.0f; - distance_sensor.signal_quality = -1; - distance_sensor.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; + double latitude = msg.latitude_deg(); + double longitude = msg.longitude_deg(); + double altitude = msg.altitude(); + float vel_north = msg.velocity_north(); + float vel_east = msg.velocity_east(); + float vel_down = -msg.velocity_up(); - gz::msgs::Quaternion pose_orientation = scan.world_pose().orientation(); + vehicle_global_position_s gps_truth{}; + + // Publish GPS groundtruth + gps_truth.timestamp = timestamp; + gps_truth.timestamp_sample = timestamp; + gps_truth.lat = latitude; + gps_truth.lon = longitude; + gps_truth.alt = altitude; + _gpos_ground_truth_pub.publish(gps_truth); + + // Apply noise model (based on ublox F9P) + addGpsNoise(latitude, longitude, altitude, vel_north, vel_east, vel_down); + + // Device ID + device::Device::DeviceId id{}; + id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; + id.devid_s.devtype = DRV_GPS_DEVTYPE_SIM; + id.devid_s.bus = 1; + id.devid_s.address = 1; + + sensor_gps_s sensor_gps{}; + + if (_sim_gps_used.get() >= 4) { + // fix + sensor_gps.fix_type = 3; // 3D fix + sensor_gps.s_variance_m_s = 0.4f; + sensor_gps.c_variance_rad = 0.1f; + sensor_gps.eph = 0.9f; + sensor_gps.epv = 1.78f; + sensor_gps.hdop = 0.7f; + sensor_gps.vdop = 1.1f; + + } else { + // no fix + sensor_gps.fix_type = 0; // No fix + sensor_gps.s_variance_m_s = 100.f; + sensor_gps.c_variance_rad = 100.f; + sensor_gps.eph = 100.f; + sensor_gps.epv = 100.f; + sensor_gps.hdop = 100.f; + sensor_gps.vdop = 100.f; + } + + sensor_gps.timestamp = timestamp; + sensor_gps.timestamp_sample = timestamp; + sensor_gps.time_utc_usec = 0; + sensor_gps.device_id = id.devid; + sensor_gps.latitude_deg = latitude; + sensor_gps.longitude_deg = longitude; + sensor_gps.altitude_msl_m = altitude; + sensor_gps.altitude_ellipsoid_m = altitude; + sensor_gps.noise_per_ms = 0; + sensor_gps.jamming_indicator = 0; + sensor_gps.vel_m_s = sqrtf(vel_north * vel_north + vel_east * vel_east); + sensor_gps.vel_n_m_s = vel_north; + sensor_gps.vel_e_m_s = vel_east; + sensor_gps.vel_d_m_s = vel_down; + sensor_gps.cog_rad = atan2(vel_east, vel_north); + sensor_gps.timestamp_time_relative = 0; + sensor_gps.heading = NAN; + sensor_gps.heading_offset = NAN; + sensor_gps.heading_accuracy = 0; + sensor_gps.automatic_gain_control = 0; + sensor_gps.jamming_state = 0; + sensor_gps.spoofing_state = 0; + sensor_gps.vel_ned_valid = true; + sensor_gps.satellites_used = _sim_gps_used.get(); + + _sensor_gps_pub.publish(sensor_gps); +} + +void GZBridge::laserScantoLidarSensorCallback(const gz::msgs::LaserScan &msg) +{ + device::Device::DeviceId id{}; + id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; + id.devid_s.devtype = DRV_DIST_DEVTYPE_SIM; + id.devid_s.bus = 1; + id.devid_s.address = 1; + + distance_sensor_s report{}; + report.timestamp = hrt_absolute_time(); + report.device_id = id.devid; + report.min_distance = static_cast(msg.range_min()); + report.max_distance = static_cast(msg.range_max()); + report.current_distance = static_cast(msg.ranges()[0]); + report.variance = 0.0f; + report.signal_quality = -1; + report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; + + gz::msgs::Quaternion pose_orientation = msg.world_pose().orientation(); gz::math::Quaterniond q_sensor = gz::math::Quaterniond( pose_orientation.w(), pose_orientation.x(), @@ -801,34 +671,34 @@ void GZBridge::laserScantoLidarSensorCallback(const gz::msgs::LaserScan &scan) const gz::math::Quaterniond q_down(0, 1, 0, 0); if (q_sensor.Equal(q_front, 0.03)) { - distance_sensor.orientation = distance_sensor_s::ROTATION_FORWARD_FACING; + report.orientation = distance_sensor_s::ROTATION_FORWARD_FACING; } else if (q_sensor.Equal(q_down, 0.03)) { - distance_sensor.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; + report.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; } else if (q_sensor.Equal(q_left, 0.03)) { - distance_sensor.orientation = distance_sensor_s::ROTATION_LEFT_FACING; + report.orientation = distance_sensor_s::ROTATION_LEFT_FACING; } else { - distance_sensor.orientation = distance_sensor_s::ROTATION_CUSTOM; - distance_sensor.q[0] = q_sensor.W(); - distance_sensor.q[1] = q_sensor.X(); - distance_sensor.q[2] = q_sensor.Y(); - distance_sensor.q[3] = q_sensor.Z(); + report.orientation = distance_sensor_s::ROTATION_CUSTOM; + report.q[0] = q_sensor.W(); + report.q[1] = q_sensor.X(); + report.q[2] = q_sensor.Y(); + report.q[3] = q_sensor.Z(); } - _distance_sensor_pub.publish(distance_sensor); + _distance_sensor_pub.publish(report); } -void GZBridge::laserScanCallback(const gz::msgs::LaserScan &scan) +void GZBridge::laserScanCallback(const gz::msgs::LaserScan &msg) { static constexpr int SECTOR_SIZE_DEG = 5; // PX4 Collision Prevention uses 5 degree sectors - double angle_min_deg = scan.angle_min() * 180 / M_PI; - double angle_step_deg = scan.angle_step() * 180 / M_PI; + double angle_min_deg = msg.angle_min() * 180 / M_PI; + double angle_step_deg = msg.angle_step() * 180 / M_PI; int samples_per_sector = std::round(SECTOR_SIZE_DEG / angle_step_deg); - int number_of_sectors = scan.ranges_size() / samples_per_sector; + int number_of_sectors = msg.ranges_size() / samples_per_sector; std::vector ds_array(number_of_sectors, UINT16_MAX); @@ -841,7 +711,7 @@ void GZBridge::laserScanCallback(const gz::msgs::LaserScan &scan) for (int j = 0; j < samples_per_sector; j++) { - double distance = scan.ranges()[i * samples_per_sector + j]; + double distance = msg.ranges()[i * samples_per_sector + j]; // inf values mean no object if (isinf(distance)) { @@ -854,7 +724,7 @@ void GZBridge::laserScanCallback(const gz::msgs::LaserScan &scan) // If all samples in a sector are inf then it means the sector is clear if (samples_used_in_sector == 0) { - ds_array[i] = scan.range_max(); + ds_array[i] = msg.range_max(); } else { ds_array[i] = sum / samples_used_in_sector; @@ -862,20 +732,20 @@ void GZBridge::laserScanCallback(const gz::msgs::LaserScan &scan) } // Publish to uORB - obstacle_distance_s obs {}; + obstacle_distance_s report {}; // Initialize unknown - for (auto &i : obs.distances) { + for (auto &i : report.distances) { i = UINT16_MAX; } - obs.timestamp = hrt_absolute_time(); - obs.frame = obstacle_distance_s::MAV_FRAME_BODY_FRD; - obs.sensor_type = obstacle_distance_s::MAV_DISTANCE_SENSOR_LASER; - obs.min_distance = static_cast(scan.range_min() * 100.); - obs.max_distance = static_cast(scan.range_max() * 100.); - obs.angle_offset = static_cast(angle_min_deg); - obs.increment = static_cast(SECTOR_SIZE_DEG); + report.timestamp = hrt_absolute_time(); + report.frame = obstacle_distance_s::MAV_FRAME_BODY_FRD; + report.sensor_type = obstacle_distance_s::MAV_DISTANCE_SENSOR_LASER; + report.min_distance = static_cast(msg.range_min() * 100.); + report.max_distance = static_cast(msg.range_max() * 100.); + report.angle_offset = static_cast(angle_min_deg); + report.increment = static_cast(SECTOR_SIZE_DEG); // Map samples in FOV into sectors in ObstacleDistance int index = 0; @@ -885,109 +755,22 @@ void GZBridge::laserScanCallback(const gz::msgs::LaserScan &scan) uint16_t distance_cm = (*i) * 100.; - if (distance_cm >= obs.max_distance) { - obs.distances[index] = obs.max_distance + 1; + if (distance_cm >= report.max_distance) { + report.distances[index] = report.max_distance + 1; - } else if (distance_cm < obs.min_distance) { - obs.distances[index] = 0; + } else if (distance_cm < report.min_distance) { + report.distances[index] = 0; } else { - obs.distances[index] = distance_cm; + report.distances[index] = distance_cm; } index++; } - _obstacle_distance_pub.publish(obs); + _obstacle_distance_pub.publish(report); } -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_WARN("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, 3000, rep, result)) { - if (!result) { - PX4_ERR("Scene Info service call failed."); - return false; - - } else { - return true; - } - - } else { - PX4_WARN("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, 3000, rep, result)) { - if (!rep.data() || !result) { - PX4_ERR("String service call failed"); - return false; - - } - } - - else { - PX4_WARN("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, 3000, rep, result)) { - if (!rep.data() || !result) { - PX4_ERR("String service call failed"); - return false; - - } - } - - else { - PX4_WARN("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 @@ -1020,8 +803,6 @@ void GZBridge::Run() return; } - pthread_mutex_lock(&_node_mutex); - if (_parameter_update_sub.updated()) { parameter_update_s pupdate; _parameter_update_sub.copy(&pupdate); @@ -1035,8 +816,53 @@ void GZBridge::Run() } ScheduleDelayed(10_ms); +} - pthread_mutex_unlock(&_node_mutex); +int GZBridge::task_spawn(int argc, char *argv[]) +{ + std::string world_name; + std::string model_name; + + int myoptind = 1; + int ch; + const char *myoptarg = nullptr; + + while ((ch = px4_getopt(argc, argv, "w:n:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'w': + world_name = myoptarg; + break; + + case 'n': + model_name = myoptarg; + break; + + default: + print_usage(); + return PX4_ERROR; + } + } + + PX4_INFO("world: %s, model: %s", world_name.c_str(), model_name.c_str()); + + GZBridge *instance = new GZBridge(world_name, model_name); + + if (!instance) { + PX4_ERR("alloc failed"); + return PX4_ERROR; + } + + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init() != PX4_OK) { + delete instance; + _object.store(nullptr); + _task_id = -1; + return PX4_ERROR; + } + + return PX4_OK; } int GZBridge::print_status() @@ -1072,11 +898,8 @@ int GZBridge::print_usage(const char *reason) PRINT_MODULE_USAGE_NAME("gz_bridge", "driver"); PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_PARAM_STRING('m', nullptr, nullptr, "Fuel model name", false); - PRINT_MODULE_USAGE_PARAM_STRING('p', nullptr, nullptr, "Model Pose", false); - PRINT_MODULE_USAGE_PARAM_STRING('n', nullptr, nullptr, "Model name", false); - PRINT_MODULE_USAGE_PARAM_STRING('i', nullptr, nullptr, "PX4 instance", false); PRINT_MODULE_USAGE_PARAM_STRING('w', nullptr, nullptr, "World name", true); + PRINT_MODULE_USAGE_PARAM_STRING('n', nullptr, nullptr, "Model name", false); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index 27ef424956..ac096744c2 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2022-2023 PX4 Development Team. All rights reserved. + * Copyright (c) 2025 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 @@ -44,24 +44,28 @@ #include #include #include +#include #include + #include #include #include #include #include #include -#include #include #include +#include +#include +#include +#include +#include +#include #include #include #include #include -#include #include -#include -#include #include #include @@ -75,13 +79,15 @@ #include #include #include +// Custom PX4 proto +#include using namespace time_literals; class GZBridge : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: - GZBridge(const char *world, const char *name, const char *model, const char *pose_str); + GZBridge(const std::string &world, const std::string &model_name); ~GZBridge() override; /** @see ModuleBase */ @@ -98,76 +104,28 @@ public: /** @see ModuleBase::print_status() */ int print_status() override; - uint64_t world_time_us() const { return _world_time_us.load(); } - private: void Run() override; - bool updateClock(const uint64_t tv_sec, const uint64_t tv_nsec); + void clockCallback(const gz::msgs::Clock &msg); + void airspeedCallback(const gz::msgs::AirSpeed &msg); + void barometerCallback(const gz::msgs::FluidPressure &msg); + void imuCallback(const gz::msgs::IMU &msg); + void poseInfoCallback(const gz::msgs::Pose_V &msg); + void odometryCallback(const gz::msgs::OdometryWithCovariance &msg); + void navSatCallback(const gz::msgs::NavSat &msg); + void laserScantoLidarSensorCallback(const gz::msgs::LaserScan &msg); + void laserScanCallback(const gz::msgs::LaserScan &msg); + void opticalFlowCallback(const px4::msgs::OpticalFlow &msg); + void magnetometerCallback(const gz::msgs::Magnetometer &msg); - void clockCallback(const gz::msgs::Clock &clock); - - void airspeedCallback(const gz::msgs::AirSpeed &air_speed); - void barometerCallback(const gz::msgs::FluidPressure &air_pressure); - void imuCallback(const gz::msgs::IMU &imu); - void poseInfoCallback(const gz::msgs::Pose_V &pose); - void odometryCallback(const gz::msgs::OdometryWithCovariance &odometry); - void navSatCallback(const gz::msgs::NavSat &nav_sat); - void laserScantoLidarSensorCallback(const gz::msgs::LaserScan &scan); - 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) - * to FRD_to_NED frames (PX4 convention) - * - * @param q_FRD_to_NED output quaterion in PX4 conventions - * @param q_FLU_to_ENU input quaterion in ROS conventions - */ static void rotateQuaternion(gz::math::Quaterniond &q_FRD_to_NED, const gz::math::Quaterniond q_FLU_to_ENU); - // Subscriptions - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + void addGpsNoise(double &latitude, double &longitude, double &altitude, + float &vel_north, float &vel_east, float &vel_down); + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Publication _distance_sensor_pub{ORB_ID(distance_sensor)}; uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; @@ -176,23 +134,23 @@ private: uORB::Publication _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)}; uORB::Publication _gpos_ground_truth_pub{ORB_ID(vehicle_global_position_groundtruth)}; uORB::Publication _lpos_ground_truth_pub{ORB_ID(vehicle_local_position_groundtruth)}; - uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; + uORB::PublicationMulti _sensor_gps_pub{ORB_ID(sensor_gps)}; + uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; + uORB::PublicationMulti _sensor_accel_pub{ORB_ID(sensor_accel)}; + uORB::PublicationMulti _sensor_gyro_pub{ORB_ID(sensor_gyro)}; + uORB::PublicationMulti _sensor_mag_pub{ORB_ID(sensor_mag)}; + uORB::PublicationMulti _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; + uORB::PublicationMulti _optical_flow_pub{ORB_ID(sensor_optical_flow)}; - uORB::PublicationMulti _sensor_accel_pub{ORB_ID(sensor_accel)}; - uORB::PublicationMulti _sensor_gyro_pub{ORB_ID(sensor_gyro)}; - uORB::PublicationMulti _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; - GZMixingInterfaceESC _mixing_interface_esc{_node, _node_mutex}; - GZMixingInterfaceServo _mixing_interface_servo{_node, _node_mutex}; - GZMixingInterfaceWheel _mixing_interface_wheel{_node, _node_mutex}; - GZGimbal _gimbal{_node, _node_mutex}; + GZMixingInterfaceESC _mixing_interface_esc{_node}; + GZMixingInterfaceServo _mixing_interface_servo{_node}; + GZMixingInterfaceWheel _mixing_interface_wheel{_node}; - px4::atomic _world_time_us{0}; - - pthread_mutex_t _node_mutex; + GZGimbal _gimbal{_node}; MapProjection _pos_ref{}; - double _alt_ref{}; // starting altitude reference + double _alt_ref{}; matrix::Vector3d _position_prev{}; matrix::Vector3d _velocity_prev{}; @@ -201,10 +159,26 @@ private: const std::string _world_name; const std::string _model_name; - const std::string _model_sim; - const std::string _model_pose; float _temperature{288.15}; // 15 degrees gz::transport::Node _node; + + // GPS noise model + float _gps_pos_noise_n = 0.0f; + float _gps_pos_noise_e = 0.0f; + float _gps_pos_noise_d = 0.0f; + float _gps_vel_noise_n = 0.0f; + float _gps_vel_noise_e = 0.0f; + float _gps_vel_noise_d = 0.0f; + const float _pos_noise_amplitude = 0.8f; // Position noise amplitude [m] + const float _pos_random_walk = 0.01f; // Position random walk coefficient + const float _pos_markov_time = 0.95f; // Position Markov process coefficient + const float _vel_noise_amplitude = 0.05f; // Velocity noise amplitude [m/s] + const float _vel_noise_density = 0.2f; // Velocity noise process density + const float _vel_markov_time = 0.85f; // Velocity Markov process coefficient + + DEFINE_PARAMETERS( + (ParamInt) _sim_gps_used + ) }; diff --git a/src/modules/simulation/gz_bridge/GZGimbal.cpp b/src/modules/simulation/gz_bridge/GZGimbal.cpp index 976200020e..2d7ed9e069 100644 --- a/src/modules/simulation/gz_bridge/GZGimbal.cpp +++ b/src/modules/simulation/gz_bridge/GZGimbal.cpp @@ -49,6 +49,8 @@ bool GZGimbal::init(const std::string &world_name, const std::string &model_name return false; } + pthread_mutex_init(&_node_mutex, nullptr); + updateParameters(); ScheduleOnInterval(200_ms); // @5Hz diff --git a/src/modules/simulation/gz_bridge/GZGimbal.hpp b/src/modules/simulation/gz_bridge/GZGimbal.hpp index 662268abd2..897c43150f 100644 --- a/src/modules/simulation/gz_bridge/GZGimbal.hpp +++ b/src/modules/simulation/gz_bridge/GZGimbal.hpp @@ -27,18 +27,17 @@ using namespace time_literals; class GZGimbal : public px4::ScheduledWorkItem, public ModuleParams { public: - GZGimbal(gz::transport::Node &node, pthread_mutex_t &node_mutex) : + GZGimbal(gz::transport::Node &node) : px4::ScheduledWorkItem(MODULE_NAME "-gimbal", px4::wq_configurations::rate_ctrl), ModuleParams(nullptr), - _node(node), - _node_mutex(node_mutex) + _node(node) {} private: friend class GZBridge; gz::transport::Node &_node; - pthread_mutex_t &_node_mutex; + pthread_mutex_t _node_mutex; uORB::Subscription _gimbal_device_set_attitude_sub{ORB_ID(gimbal_device_set_attitude)}; uORB::Subscription _gimbal_controls_sub{ORB_ID(gimbal_controls)}; diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp index c1a88f7bda..82ff4cf3da 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp @@ -55,6 +55,8 @@ bool GZMixingInterfaceESC::init(const std::string &model_name) _esc_status_pub.advertise(); + pthread_mutex_init(&_node_mutex, nullptr); + ScheduleNow(); return true; diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.hpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.hpp index 316ff6195a..d724c5c6f9 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.hpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.hpp @@ -50,10 +50,9 @@ class GZMixingInterfaceESC : public OutputModuleInterface public: static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS; - GZMixingInterfaceESC(gz::transport::Node &node, pthread_mutex_t &node_mutex) : + GZMixingInterfaceESC(gz::transport::Node &node) : OutputModuleInterface(MODULE_NAME "-actuators-esc", px4::wq_configurations::rate_ctrl), - _node(node), - _node_mutex(node_mutex) + _node(node) {} bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], @@ -77,7 +76,7 @@ private: void motorSpeedCallback(const gz::msgs::Actuators &actuators); gz::transport::Node &_node; - pthread_mutex_t &_node_mutex; + pthread_mutex_t _node_mutex; MixingOutput _mixing_output{"SIM_GZ_EC", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp index 2fc656074c..6fd069ec09 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp @@ -121,6 +121,8 @@ bool GZMixingInterfaceServo::init(const std::string &model_name) _angular_range_rad.push_back(max_val - min_val); } + pthread_mutex_init(&_node_mutex, nullptr); + ScheduleNow(); return true; diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp index 180f6cbca2..9980918233 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp @@ -44,10 +44,9 @@ class GZMixingInterfaceServo : public OutputModuleInterface { public: - GZMixingInterfaceServo(gz::transport::Node &node, pthread_mutex_t &node_mutex) : + GZMixingInterfaceServo(gz::transport::Node &node) : OutputModuleInterface(MODULE_NAME "-actuators-servo", px4::wq_configurations::rate_ctrl), - _node(node), - _node_mutex(node_mutex) + _node(node) {} bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], @@ -83,7 +82,7 @@ private: float get_servo_angle_min(const size_t index); gz::transport::Node &_node; - pthread_mutex_t &_node_mutex; + pthread_mutex_t _node_mutex; MixingOutput _mixing_output{"SIM_GZ_SV", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp index 2198bacbaa..cd015892de 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp @@ -51,6 +51,8 @@ bool GZMixingInterfaceWheel::init(const std::string &model_name) return false; } + pthread_mutex_init(&_node_mutex, nullptr); + _wheel_encoders_pub.advertise(); ScheduleNow(); diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.hpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.hpp index 281606dd61..8e14ddf958 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.hpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.hpp @@ -51,10 +51,9 @@ class GZMixingInterfaceWheel : public OutputModuleInterface public: static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS; - GZMixingInterfaceWheel(gz::transport::Node &node, pthread_mutex_t &node_mutex) : + GZMixingInterfaceWheel(gz::transport::Node &node) : OutputModuleInterface(MODULE_NAME "-actuators-wheel", px4::wq_configurations::rate_ctrl), - _node(node), - _node_mutex(node_mutex) + _node(node) {} bool updateOutputs(bool stop_wheels, uint16_t outputs[MAX_ACTUATORS], @@ -78,7 +77,7 @@ private: void wheelSpeedCallback(const gz::msgs::Actuators &actuators); gz::transport::Node &_node; - pthread_mutex_t &_node_mutex; + pthread_mutex_t _node_mutex; MixingOutput _mixing_output{"SIM_GZ_WH", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; diff --git a/src/modules/simulation/gz_bridge/Kconfig b/src/modules/simulation/gz_bridge/Kconfig index 0d4ab457d1..dd34a251b3 100644 --- a/src/modules/simulation/gz_bridge/Kconfig +++ b/src/modules/simulation/gz_bridge/Kconfig @@ -1,6 +1,6 @@ menuconfig MODULES_SIMULATION_GZ_BRIDGE bool "gz_bridge" default n - depends on PLATFORM_POSIX + depends on PLATFORM_POSIX && MODULES_SIMULATION_GZ_MSGS ---help--- Enable support for gz_bridge diff --git a/src/modules/simulation/gz_bridge/gz_env.sh.in b/src/modules/simulation/gz_bridge/gz_env.sh.in index 810bc88948..bff7180b03 100644 --- a/src/modules/simulation/gz_bridge/gz_env.sh.in +++ b/src/modules/simulation/gz_bridge/gz_env.sh.in @@ -1,6 +1,21 @@ #!/usr/bin/env bash +# ----------------------------------------------------------------------- +# Gazebo Environment Configuration +# ----------------------------------------------------------------------- +# GZ_SIM_RESOURCE_PATH: Where Gazebo looks for models and worlds +# GZ_SIM_SYSTEM_PLUGIN_PATH: Where Gazebo looks for plugin libraries +# GZ_SIM_SERVER_CONFIG_PATH: Custom Gazebo server configuration file +# +# See Gazebo docs +# https://gazebosim.org/api/sim/8/resources.html +# https://gazebosim.org/api/sim/8/server_config.html +# ----------------------------------------------------------------------- export PX4_GZ_MODELS=@PX4_SOURCE_DIR@/Tools/simulation/gz/models export PX4_GZ_WORLDS=@PX4_SOURCE_DIR@/Tools/simulation/gz/worlds +export PX4_GZ_PLUGINS=@PX4_BINARY_DIR@/src/modules/simulation/gz_plugins +export PX4_GZ_SERVER_CONFIG=@PX4_SOURCE_DIR@/src/modules/simulation/gz_bridge/server.config export GZ_SIM_RESOURCE_PATH=$GZ_SIM_RESOURCE_PATH:$PX4_GZ_MODELS:$PX4_GZ_WORLDS +export GZ_SIM_SYSTEM_PLUGIN_PATH=$GZ_SIM_SYSTEM_PLUGIN_PATH:$PX4_GZ_PLUGINS +export GZ_SIM_SERVER_CONFIG_PATH=$PX4_GZ_SERVER_CONFIG diff --git a/src/modules/simulation/gz_bridge/server.config b/src/modules/simulation/gz_bridge/server.config new file mode 100644 index 0000000000..93bff3f442 --- /dev/null +++ b/src/modules/simulation/gz_bridge/server.config @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + ogre2 + + + + + diff --git a/src/modules/simulation/gz_msgs/CMakeLists.txt b/src/modules/simulation/gz_msgs/CMakeLists.txt new file mode 100644 index 0000000000..2501811e5e --- /dev/null +++ b/src/modules/simulation/gz_msgs/CMakeLists.txt @@ -0,0 +1,33 @@ +############################################################################ +# +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# +############################################################################ + +# Find required packages +find_package(Protobuf) + +if (Protobuf_FOUND) + # Generate protobuf messages + file(GLOB MSGS_PROTOS "${CMAKE_CURRENT_SOURCE_DIR}/*.proto") + PROTOBUF_GENERATE_CPP(PROTO_SRCS PROTO_HDRS ${MSGS_PROTOS}) + + # Create library + add_library(px4_gz_msgs STATIC + ${PROTO_SRCS} + ${PROTO_HDRS} + ) + + target_include_directories(px4_gz_msgs + PUBLIC + ${CMAKE_CURRENT_BINARY_DIR} + ${Protobuf_INCLUDE_DIRS} + ) + target_link_libraries(px4_gz_msgs PUBLIC ${PROTOBUF_LIBRARIES}) + if (Protobuf_VERSION VERSION_LESS "3.8") + target_compile_options(px4_gz_msgs PRIVATE -Wno-error=float-equal) + endif() + + # Export the binary dir for other modules + set(PX4_GZ_MSGS_BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR} CACHE INTERNAL "") +endif() diff --git a/src/modules/simulation/gz_msgs/Kconfig b/src/modules/simulation/gz_msgs/Kconfig new file mode 100644 index 0000000000..65b2a6453b --- /dev/null +++ b/src/modules/simulation/gz_msgs/Kconfig @@ -0,0 +1,6 @@ +menuconfig MODULES_SIMULATION_GZ_MSGS + bool "gz_msgs" + default n + depends on PLATFORM_POSIX + ---help--- + Enable proto generation for custom PX4 messages diff --git a/src/modules/simulation/gz_msgs/opticalflow.proto b/src/modules/simulation/gz_msgs/opticalflow.proto new file mode 100644 index 0000000000..c03a96b9a8 --- /dev/null +++ b/src/modules/simulation/gz_msgs/opticalflow.proto @@ -0,0 +1,19 @@ +syntax = "proto3"; +package px4.msgs; + +message OpticalFlow { + // Timestamp (microseconds, since system start) + int64 time_usec = 1; + + // Integration time + uint32 integration_time_us = 2; + + // Integrated x-axis flow (rad) + float integrated_x = 3; + + // Integrated y-axis flow (rad) + float integrated_y = 4; + + // Quality of optical flow measurement (0: bad, 255: maximum quality) + float quality = 5; +} diff --git a/src/modules/simulation/gz_plugins/CMakeLists.txt b/src/modules/simulation/gz_plugins/CMakeLists.txt new file mode 100644 index 0000000000..4bb3307108 --- /dev/null +++ b/src/modules/simulation/gz_plugins/CMakeLists.txt @@ -0,0 +1,86 @@ +############################################################################ +# +# Copyright (c) 2025 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. +# +############################################################################ + +project(px4_gz_plugins) + +if(NOT DEFINED ENV{GZ_DISTRO}) + set(GZ_DISTRO "harmonic" CACHE STRING "Gazebo distribution to use") +else() + set(GZ_DISTRO $ENV{GZ_DISTRO}) +endif() + +# Define library version combinations for different Gazebo distributions +# https://github.com/gazebo-tooling/gazebodistro/blob/master/collection-harmonic.yaml +if(GZ_DISTRO STREQUAL "harmonic") + set(GZ_CMAKE_VERSION "3") + set(GZ_MSGS_VERSION "10") + set(GZ_TRANSPORT_VERSION "13") + set(GZ_PLUGIN_VERSION "2") + set(GZ_SIM_VERSION "8") + set(GZ_SENSORS_VERSION "8") + message(STATUS "Using Gazebo Harmonic (cmake:${GZ_CMAKE_VERSION}, msgs:${GZ_MSGS_VERSION}, transport:${GZ_TRANSPORT_VERSION})") +elseif(GZ_DISTRO STREQUAL "ionic") + set(GZ_CMAKE_VERSION "4") + set(GZ_MSGS_VERSION "11") + set(GZ_TRANSPORT_VERSION "14") + set(GZ_PLUGIN_VERSION "3") + set(GZ_SIM_VERSION "9") + set(GZ_SENSORS_VERSION "9") + message(STATUS "Using Gazebo Ionic (cmake:${GZ_CMAKE_VERSION}, msgs:${GZ_MSGS_VERSION}, transport:${GZ_TRANSPORT_VERSION})") +else() + message(FATAL_ERROR "Unknown Gazebo distribution: ${GZ_DISTRO}. Valid options are: harmonic or ionic") +endif() + +# Use gz-transport as litmus test for presence of gz +find_package(gz-transport${GZ_TRANSPORT_VERSION}) + +if (gz-transport${GZ_TRANSPORT_VERSION}_FOUND) + find_package(gz-cmake${GZ_CMAKE_VERSION} REQUIRED) + find_package(gz-msgs${GZ_MSGS_VERSION} REQUIRED) + find_package(Protobuf REQUIRED) + find_package(gz-plugin${GZ_PLUGIN_VERSION} REQUIRED COMPONENTS register) + find_package(gz-sim${GZ_SIM_VERSION} REQUIRED) + find_package(gz-sensors${GZ_SENSORS_VERSION} REQUIRED) + + # Create a flat output directory for all plugin libraries + set(PX4_GZ_PLUGIN_OUTPUT_DIR "${CMAKE_CURRENT_BINARY_DIR}" CACHE PATH "Directory for all Gazebo plugin libraries") + file(MAKE_DIRECTORY ${PX4_GZ_PLUGIN_OUTPUT_DIR}) + set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PX4_GZ_PLUGIN_OUTPUT_DIR}) + + # Add our plugins as subdirectories + add_subdirectory(optical_flow) + add_subdirectory(template_plugin) + + # Add an alias target for each plugin + add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem TemplatePlugin) +endif() diff --git a/src/modules/simulation/gz_plugins/Kconfig b/src/modules/simulation/gz_plugins/Kconfig new file mode 100644 index 0000000000..eae335fbc5 --- /dev/null +++ b/src/modules/simulation/gz_plugins/Kconfig @@ -0,0 +1,6 @@ +menuconfig MODULES_SIMULATION_GZ_PLUGINS + bool "gz_plugins" + default n + depends on PLATFORM_POSIX && MODULES_SIMULATION_GZ_MSGS + ---help--- + Enable support for gz_plugins diff --git a/src/modules/simulation/gz_plugins/optical_flow/CMakeLists.txt b/src/modules/simulation/gz_plugins/optical_flow/CMakeLists.txt new file mode 100644 index 0000000000..2a57ba3c25 --- /dev/null +++ b/src/modules/simulation/gz_plugins/optical_flow/CMakeLists.txt @@ -0,0 +1,65 @@ +############################################################################ +# +# Copyright (c) 2025 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. +# +############################################################################ + +project(OpticalFlowSystem) + +# Include the OpticalFlow external dependency +include(${CMAKE_CURRENT_SOURCE_DIR}/optical_flow.cmake) + +# Find OpenCV +find_package(OpenCV REQUIRED) + +add_library(${PROJECT_NAME} SHARED + OpticalFlowSensor.cpp + OpticalFlowSystem.cpp +) + +target_link_libraries(${PROJECT_NAME} + PUBLIC px4_gz_msgs + PUBLIC gz-sensors${GZ_SENSORS_VERSION}::gz-sensors${GZ_SENSORS_VERSION} + PUBLIC gz-plugin${GZ_PLUGIN_VERSION}::gz-plugin${GZ_PLUGIN_VERSION} + PUBLIC gz-sim${GZ_SIM_VERSION}::gz-sim${GZ_SIM_VERSION} + PUBLIC gz-transport${GZ_TRANSPORT_VERSION}::gz-transport${GZ_TRANSPORT_VERSION} + PUBLIC ${OpenCV_LIBS} + PUBLIC ${OpticalFlow_LIBS} +) + +target_include_directories(${PROJECT_NAME} + PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} + PUBLIC ${CMAKE_CURRENT_BINARY_DIR} + PUBLIC ${OpenCV_INCLUDE_DIRS} + PUBLIC ${OpticalFlow_INCLUDE_DIRS} + PUBLIC px4_gz_msgs +) + +add_dependencies(${PROJECT_NAME} OpticalFlow) diff --git a/src/modules/simulation/gz_plugins/optical_flow/OpticalFlowSensor.cpp b/src/modules/simulation/gz_plugins/optical_flow/OpticalFlowSensor.cpp new file mode 100644 index 0000000000..4c0dac72d7 --- /dev/null +++ b/src/modules/simulation/gz_plugins/optical_flow/OpticalFlowSensor.cpp @@ -0,0 +1,165 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 + +#include "OpticalFlowSensor.hpp" +#include "opticalflow.pb.h" + +using namespace custom; + +bool OpticalFlowSensor::Load(const sdf::Sensor &_sdf) +{ + auto type = gz::sensors::customType(_sdf); + + if ("optical_flow" != type) { + gzerr << "Trying to load [optical_flow] sensor, but got type [" << type << "] instead." << std::endl; + return false; + } + + gz::sensors::Sensor::Load(_sdf); + + std::string output_topic = this->Topic(); + _publisher = _node.Advertise(output_topic); + gzdbg << "Advertising optical flow data on: " << output_topic << std::endl; + + std::string camera_topic = output_topic; + size_t last_segment = camera_topic.rfind("/optical_flow/optical_flow"); + + if (last_segment != std::string::npos) { + camera_topic = camera_topic.substr(0, last_segment) + "/flow_camera/image"; + } + + int image_width = 0; + int image_height = 0; + int update_rate = 0; + float hfov = 0; + + auto sensorElem = _sdf.Element()->GetParent()->GetElement("sensor"); + + while (sensorElem) { + if (sensorElem->Get("name") == "flow_camera") { + auto cameraElem = sensorElem->GetElement("camera"); + update_rate = sensorElem->GetElement("update_rate")->Get(); + hfov = cameraElem->GetElement("horizontal_fov")->Get(); + + auto imageElem = cameraElem->GetElement("image"); + image_width = imageElem->GetElement("width")->Get(); + image_height = imageElem->GetElement("height")->Get(); + break; + } + + sensorElem = sensorElem->GetNextElement("sensor"); + } + + gzdbg << "Camera parameters:" << std::endl + << " image_width: " << image_width << std::endl + << " image_height: " << image_height << std::endl + << " update_rate: " << update_rate << std::endl + << " hfov: " << hfov << std::endl; + + gzdbg << "Subscribing to camera topic for flow: " << camera_topic << std::endl; + + if (!_node.Subscribe(camera_topic, &OpticalFlowSensor::OnImage, this)) { + gzerr << "Failed to subscribe to camera topic: " << camera_topic << std::endl; + return false; + } + + // Assume pinhole camera and 1:1 aspect ratio + float focal_length = (image_width / 2.0f) / tan(hfov / 2.0f); + _optical_flow = std::make_shared(focal_length, focal_length, + update_rate, image_width, image_height); + + return true; +} + +void OpticalFlowSensor::OnImage(const gz::msgs::Image &image_msg) +{ + if (image_msg.width() == 0 || image_msg.height() == 0) { + gzerr << "Invalid image dimensions" << std::endl; + return; + } + + if (image_msg.pixel_format_type() == gz::msgs::PixelFormatType::RGB_INT8) { + cv::Mat temp(image_msg.height(), image_msg.width(), CV_8UC3); + std::memcpy(temp.data, image_msg.data().c_str(), image_msg.data().size()); + cv::cvtColor(temp, _last_image_gray, cv::COLOR_RGB2GRAY); + + } else if (image_msg.pixel_format_type() == gz::msgs::PixelFormatType::L_INT8) { + std::memcpy(_last_image_gray.data, image_msg.data().c_str(), image_msg.data().size()); + + } else { + gzerr << "Unsupported image format" << std::endl; + return; + } + + uint32_t current_timestamp = (image_msg.header().stamp().sec() * 1000000ULL + + image_msg.header().stamp().nsec() / 1000ULL) & 0xFFFFFFFF; + + if (_last_image_timestamp != 0) { + _integration_time_us = (current_timestamp - _last_image_timestamp) & 0xFFFFFFFF; + } + + _last_image_timestamp = current_timestamp; + _new_image_available = true; +} + +bool OpticalFlowSensor::Update(const std::chrono::steady_clock::duration &_now) +{ + if (!_new_image_available) { + return true; + } + + px4::msgs::OpticalFlow msg; + msg.set_time_usec(_last_image_timestamp); + + float flow_x = 0.f; + float flow_y = 0.f; + + int quality = _optical_flow->calcFlow(_last_image_gray.data, _last_image_timestamp, + _integration_time_us, flow_x, flow_y); + + msg.set_integrated_x(flow_x); + msg.set_integrated_y(flow_y); + msg.set_integration_time_us(_integration_time_us); + msg.set_quality(quality); + + if (!_publisher.Publish(msg)) { + gzwarn << "Failed to publish optical flow message" << std::endl; + } + + _new_image_available = false; + return true; +} diff --git a/src/modules/simulation/gz_plugins/optical_flow/OpticalFlowSensor.hpp b/src/modules/simulation/gz_plugins/optical_flow/OpticalFlowSensor.hpp new file mode 100644 index 0000000000..497fd79276 --- /dev/null +++ b/src/modules/simulation/gz_plugins/optical_flow/OpticalFlowSensor.hpp @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 "flow_opencv.hpp" + +namespace custom +{ +class OpticalFlowSensor : public gz::sensors::Sensor +{ +public: + virtual bool Load(const sdf::Sensor &_sdf) override; + virtual bool Update(const std::chrono::steady_clock::duration &_now) override; + +private: + void OnImage(const gz::msgs::Image &_msg); + + gz::transport::Node _node; + gz::transport::Node::Publisher _publisher; + + // Flow + std::shared_ptr _optical_flow {nullptr}; + int _integration_time_us; + + // Camera + double _horizontal_fov {0.0}; + double _vertical_fov {0.0}; + + cv::Mat _last_image_gray; + uint32_t _last_image_timestamp {0}; + bool _new_image_available {false}; +}; + +} // end namespace custom diff --git a/src/modules/simulation/gz_plugins/optical_flow/OpticalFlowSystem.cpp b/src/modules/simulation/gz_plugins/optical_flow/OpticalFlowSystem.cpp new file mode 100644 index 0000000000..3459f8a386 --- /dev/null +++ b/src/modules/simulation/gz_plugins/optical_flow/OpticalFlowSystem.cpp @@ -0,0 +1,125 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "OpticalFlowSensor.hpp" +#include "OpticalFlowSystem.hpp" + +using namespace custom; + +void OpticalFlowSystem::PreUpdate(const gz::sim::UpdateInfo &, gz::sim::EntityComponentManager &_ecm) +{ + // Register each new custom sensor + _ecm.EachNew( + [&](const gz::sim::Entity & _entity, + const gz::sim::components::CustomSensor * _custom, + const gz::sim::components::ParentEntity * _parent)->bool { + auto sensorScopedName = gz::sim::removeParentScope(gz::sim::scopedName(_entity, _ecm, "::", false), "::"); + + sdf::Sensor data = _custom->Data(); + data.SetName(sensorScopedName); + + if (data.Topic().empty()) + { + std::string topic = scopedName(_entity, _ecm) + "/optical_flow"; + data.SetTopic(topic); + } + + gz::sensors::SensorFactory sensorFactory; + auto sensor = sensorFactory.CreateSensor(data); + + if (sensor == nullptr) + { + gzerr << "Failed to create optical flow sensor [" << sensorScopedName << "]" << std::endl; + return false; + } + + auto parentName = _ecm.Component(_parent->Data())->Data(); + + sensor->SetParent(parentName); + + _ecm.CreateComponent(_entity, gz::sim::components::SensorTopic(sensor->Topic())); + + this->entitySensorMap.insert(std::make_pair(_entity, std::move(sensor))); + + gzdbg << "OpticalFlowSystem PreUpdate" << std::endl; + + return true; + }); +} + +void OpticalFlowSystem::PostUpdate(const gz::sim::UpdateInfo &_info, const gz::sim::EntityComponentManager &_ecm) +{ + if (!_info.paused) { + for (auto &[entity, sensor] : this->entitySensorMap) { + sensor->Update(_info.simTime); + } + } + + this->RemoveSensorEntities(_ecm); +} + +void OpticalFlowSystem::RemoveSensorEntities(const gz::sim::EntityComponentManager &_ecm) +{ + _ecm.EachRemoved( + [&](const gz::sim::Entity & _entity, + const gz::sim::components::CustomSensor *)->bool { + if (this->entitySensorMap.erase(_entity) == 0) + { + gzerr << "Internal error, missing optical flow sensor for entity [" + << _entity << "]" << std::endl; + } + return true; + }); +} + +GZ_ADD_PLUGIN(OpticalFlowSystem, gz::sim::System, + OpticalFlowSystem::ISystemPreUpdate, + OpticalFlowSystem::ISystemPostUpdate + ) + +GZ_ADD_PLUGIN_ALIAS(OpticalFlowSystem, "custom::OpticalFlowSystem") diff --git a/src/modules/simulation/gz_plugins/optical_flow/OpticalFlowSystem.hpp b/src/modules/simulation/gz_plugins/optical_flow/OpticalFlowSystem.hpp new file mode 100644 index 0000000000..eecaaa2f6f --- /dev/null +++ b/src/modules/simulation/gz_plugins/optical_flow/OpticalFlowSystem.hpp @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 + +namespace custom +{ +class OpticalFlowSystem: + public gz::sim::System, + public gz::sim::ISystemPreUpdate, + public gz::sim::ISystemPostUpdate +{ +public: + void PreUpdate(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) final; + + void PostUpdate(const gz::sim::UpdateInfo &_info, + const gz::sim::EntityComponentManager &_ecm) final; + +private: + void RemoveSensorEntities(const gz::sim::EntityComponentManager &_ecm); + + std::unordered_map> entitySensorMap; +}; +} // end namespace custom diff --git a/src/modules/simulation/gz_plugins/optical_flow/optical_flow.cmake b/src/modules/simulation/gz_plugins/optical_flow/optical_flow.cmake new file mode 100644 index 0000000000..8ac1c1f649 --- /dev/null +++ b/src/modules/simulation/gz_plugins/optical_flow/optical_flow.cmake @@ -0,0 +1,53 @@ +############################################################################ +# +# Copyright (c) 2025 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(ExternalProject) +find_package(OpenCV REQUIRED) + +if(NOT TARGET OpticalFlow) + ExternalProject_Add(OpticalFlow + GIT_REPOSITORY https://github.com/PX4/PX4-OpticalFlow.git + GIT_TAG master + PREFIX ${CMAKE_BINARY_DIR}/OpticalFlow + INSTALL_DIR ${CMAKE_BINARY_DIR}/OpticalFlow/install + CMAKE_ARGS -DCMAKE_INSTALL_PREFIX= + BUILD_BYPRODUCTS ${CMAKE_BINARY_DIR}/OpticalFlow/install/lib/libOpticalFlow.so + UPDATE_DISCONNECTED ON + BUILD_ALWAYS OFF + STEP_TARGETS build + ) + + ExternalProject_Get_Property(OpticalFlow install_dir) + set(OpticalFlow_INCLUDE_DIRS ${install_dir}/include CACHE INTERNAL "") + set(OpticalFlow_LIBS ${install_dir}/lib/libOpticalFlow.so CACHE INTERNAL "") +endif() diff --git a/src/modules/simulation/gz_plugins/template_plugin/CMakeLists.txt b/src/modules/simulation/gz_plugins/template_plugin/CMakeLists.txt new file mode 100644 index 0000000000..f5e2935ef6 --- /dev/null +++ b/src/modules/simulation/gz_plugins/template_plugin/CMakeLists.txt @@ -0,0 +1,68 @@ +############################################################################ +# +# Copyright (c) 2025 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. +# +############################################################################ + +# Template for a new plugin project +# Replace TemplatePlugin with your plugin name +project(TemplatePlugin) + +# Add external dependencies if needed +# include(${CMAKE_CURRENT_SOURCE_DIR}/dependency.cmake) + +# Find required packages +# find_package(PackageName REQUIRED) + +add_library(${PROJECT_NAME} SHARED + # Add your source files here + TemplateSystem.cpp +) + +target_link_libraries(${PROJECT_NAME} + PUBLIC px4_gz_msgs + PUBLIC gz-sensors${GZ_SENSORS_VERSION}::gz-sensors${GZ_SENSORS_VERSION} + PUBLIC gz-plugin${GZ_PLUGIN_VERSION}::gz-plugin${GZ_PLUGIN_VERSION} + PUBLIC gz-sim${GZ_SIM_VERSION}::gz-sim${GZ_SIM_VERSION} + PUBLIC gz-transport${GZ_TRANSPORT_VERSION}::gz-transport${GZ_TRANSPORT_VERSION} + # Add other dependencies as needed + # PUBLIC ${OtherLib_LIBS} +) + +target_include_directories(${PROJECT_NAME} + PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} + PUBLIC ${CMAKE_CURRENT_BINARY_DIR} + PUBLIC px4_gz_msgs + # Add other include directories as needed + # PUBLIC ${OtherLib_INCLUDE_DIRS} +) + +# Add dependencies if needed +# add_dependencies(${PROJECT_NAME} ExternalDependency) diff --git a/src/modules/simulation/gz_plugins/template_plugin/README.md b/src/modules/simulation/gz_plugins/template_plugin/README.md new file mode 100644 index 0000000000..dda5fbcbf5 --- /dev/null +++ b/src/modules/simulation/gz_plugins/template_plugin/README.md @@ -0,0 +1,30 @@ +# Template Gazebo Plugin + +This is a template for creating new Gazebo plugins for PX4. Follow these steps to create your own plugin: + +1. Copy this directory and rename it to your plugin name +2. Update the project name in CMakeLists.txt +3. Rename and implement the TemplateSystem.hpp/cpp files +4. Add your plugin to the top-level CMakeLists.txt in the gz_plugins directory: + ```cmake + add_subdirectory(your_plugin_directory) + ``` +5. Add your plugin's target to the `px4_gz_plugins` target dependencies in the top-level CMakeLists.txt: + ```cmake + add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem YourPluginSystem) + ``` +6. Update the server.config file to load your plugin: + ```xml + + ``` + +## Plugin Structure + +This template follows the standard Gazebo plugin structure: + +- `TemplateSystem.hpp/cpp`: The main plugin system class that is loaded by Gazebo +- CMakeLists.txt: Build configuration for this plugin + +## Testing Your Plugin + +After building, you can test your plugin by adding it to the server.config file and running a simulation. diff --git a/src/modules/simulation/gz_plugins/template_plugin/TemplateSystem.cpp b/src/modules/simulation/gz_plugins/template_plugin/TemplateSystem.cpp new file mode 100644 index 0000000000..ce07329224 --- /dev/null +++ b/src/modules/simulation/gz_plugins/template_plugin/TemplateSystem.cpp @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 "TemplateSystem.hpp" + +#include + +using namespace custom; + +// Register the plugin +GZ_ADD_PLUGIN( + TemplateSystem, + gz::sim::System, + TemplateSystem::ISystemPreUpdate, + TemplateSystem::ISystemPostUpdate +) + +void TemplateSystem::PreUpdate(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) +{ + // Implement pre-update logic here +} + +void TemplateSystem::PostUpdate(const gz::sim::UpdateInfo &_info, + const gz::sim::EntityComponentManager &_ecm) +{ + // Implement post-update logic here +} diff --git a/src/modules/simulation/gz_plugins/template_plugin/TemplateSystem.hpp b/src/modules/simulation/gz_plugins/template_plugin/TemplateSystem.hpp new file mode 100644 index 0000000000..840dec1c33 --- /dev/null +++ b/src/modules/simulation/gz_plugins/template_plugin/TemplateSystem.hpp @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 + +namespace custom +{ +class TemplateSystem: + public gz::sim::System, + public gz::sim::ISystemPreUpdate, + public gz::sim::ISystemPostUpdate +{ +public: + void PreUpdate(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) final; + + void PostUpdate(const gz::sim::UpdateInfo &_info, + const gz::sim::EntityComponentManager &_ecm) final; + +private: + // Add your private member variables and methods here + gz::transport::Node _node; +}; +} // end namespace custom diff --git a/src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp b/src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp index 7ee92ad750..18c4adbf1d 100644 --- a/src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp +++ b/src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp @@ -78,7 +78,7 @@ private: uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude_groundtruth)}; uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position_groundtruth)}; - PX4Magnetometer _px4_mag{197388, ROTATION_NONE}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 1, ADDR: 1, TYPE: SIMULATION + PX4Magnetometer _px4_mag{197388, ROTATION_NONE}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 1, ADDR: 3, TYPE: SIMULATION bool _mag_earth_available{false}; diff --git a/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake b/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake index b67fe546cc..258e18d8b2 100644 --- a/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake +++ b/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake @@ -80,7 +80,6 @@ if(gazebo_FOUND) iris_dual_gps iris_foggy_lidar iris_irlock - iris_obs_avoid iris_depth_camera iris_downward_depth_camera iris_opt_flow diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index 07c24fe642..2ebf936823 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -556,7 +556,6 @@ void Sih::send_airspeed(const hrt_abstime &time_now_us) // regardless of vehicle type, body frame, etc this holds as long as wind=0 airspeed.true_airspeed_m_s = fmaxf(0.1f, _v_E.norm() + generate_wgn() * 0.2f); airspeed.indicated_airspeed_m_s = airspeed.true_airspeed_m_s * sqrtf(_wing_l.get_rho() / RHO); - airspeed.air_temperature_celsius = NAN; airspeed.confidence = 0.7f; airspeed.timestamp = hrt_absolute_time(); _airspeed_pub.publish(airspeed); diff --git a/src/modules/uxrce_dds_client/dds_topics.yaml b/src/modules/uxrce_dds_client/dds_topics.yaml index 211e75fd05..bbd0d8c631 100644 --- a/src/modules/uxrce_dds_client/dds_topics.yaml +++ b/src/modules/uxrce_dds_client/dds_topics.yaml @@ -71,9 +71,6 @@ publications: - topic: /fmu/out/vehicle_status type: px4_msgs::msg::VehicleStatus - - topic: /fmu/out/vehicle_trajectory_waypoint_desired - type: px4_msgs::msg::VehicleTrajectoryWaypoint - - topic: /fmu/out/airspeed_validated type: px4_msgs::msg::AirspeedValidated @@ -145,12 +142,6 @@ subscriptions: - topic: /fmu/in/vehicle_command_mode_executor type: px4_msgs::msg::VehicleCommand - - topic: /fmu/in/vehicle_trajectory_bezier - type: px4_msgs::msg::VehicleTrajectoryBezier - - - topic: /fmu/in/vehicle_trajectory_waypoint - type: px4_msgs::msg::VehicleTrajectoryWaypoint - - topic: /fmu/in/vehicle_thrust_setpoint type: px4_msgs::msg::VehicleThrustSetpoint diff --git a/src/modules/uxrce_dds_client/module.yaml b/src/modules/uxrce_dds_client/module.yaml index 4e5db21160..773475f1f5 100644 --- a/src/modules/uxrce_dds_client/module.yaml +++ b/src/modules/uxrce_dds_client/module.yaml @@ -105,3 +105,27 @@ parameters: category: System reboot_required: true default: 0 + + UXRCE_DDS_TX_TO: + description: + short: TX rate timeout configuration + long: | + Specifies after how many seconds without sending data the DDS connection is reestablished. + A value less than one disables the TX rate timeout. + type: int32 + category: System + reboot_required: true + default: 3 + unit: s + + UXRCE_DDS_RX_TO: + description: + short: RX rate timeout configuration + long: | + Specifies after how many seconds without receiving data the DDS connection is reestablished. + A value less than one disables the RX rate timeout. + type: int32 + category: System + reboot_required: true + default: -1 + unit: s diff --git a/src/modules/uxrce_dds_client/uxrce_dds_client.cpp b/src/modules/uxrce_dds_client/uxrce_dds_client.cpp index 339bedd7a9..b550912791 100644 --- a/src/modules/uxrce_dds_client/uxrce_dds_client.cpp +++ b/src/modules/uxrce_dds_client/uxrce_dds_client.cpp @@ -209,7 +209,7 @@ void UxrceddsClient::deinit() _comm = nullptr; } -bool UxrceddsClient::setup_session(uxrSession *session) +bool UxrceddsClient::setupSession(uxrSession *session) { _participant_config = static_cast(_param_uxrce_dds_ptcfg.get()); _synchronize_timestamps = (_param_uxrce_dds_synct.get() > 0); @@ -379,7 +379,7 @@ bool UxrceddsClient::setup_session(uxrSession *session) return true; } -void UxrceddsClient::delete_session(uxrSession *session) +void UxrceddsClient::deleteSession(uxrSession *session) { delete_repliers(); @@ -472,6 +472,20 @@ static void fillMessageFormatResponse(const message_format_request_s &message_fo message_format_response.timestamp = hrt_absolute_time(); } +void UxrceddsClient::calculateTxRxRate() +{ + const hrt_abstime now = hrt_absolute_time(); + + if (now - _last_status_update > 1_s) { + float dt = (now - _last_status_update) / 1e6f; + _last_payload_tx_rate = (_subs->num_payload_sent - _last_num_payload_sent) / dt; + _last_payload_rx_rate = (_pubs->num_payload_received - _last_num_payload_received) / dt; + _last_num_payload_sent = _subs->num_payload_sent; + _last_num_payload_received = _pubs->num_payload_received; + _last_status_update = now; + } +} + void UxrceddsClient::handleMessageFormatRequest() { message_format_request_s message_format_request; @@ -483,6 +497,87 @@ void UxrceddsClient::handleMessageFormatRequest() } } +void UxrceddsClient::checkConnectivity(uxrSession *session) +{ + // Reset TX zero counter, when data is sent + if (_last_payload_tx_rate > 0) { + _num_tx_rate_zero = 0; + } + + // Reset RX zero counter, when data is received + if (_last_payload_rx_rate > 0) { + _num_rx_rate_zero = 0; + } + + const hrt_abstime now = hrt_absolute_time(); + + // Start ping and tx/rx rate monitoring, unless we're actively sending & receiving payloads successfully + if ((_last_payload_tx_rate > 0) && (_last_payload_rx_rate > 0)) { + _connected = true; + _num_pings_missed = 0; + _last_ping = now; + + } else { + if (hrt_elapsed_time(&_last_ping) > 1_s) { + // Check payload tx rate + if (_last_payload_tx_rate == 0) { + _num_tx_rate_zero++; + } + + // Check payload rx rate + if (_last_payload_rx_rate == 0) { + _num_rx_rate_zero++; + } + + // Check ping + _last_ping = now; + + if (_had_ping_reply) { + _num_pings_missed = 0; + + } else { + ++_num_pings_missed; + } + + int timeout_ms = 1'000; // 1 second + uint8_t attempts = 1; + uxr_ping_agent_session(session, timeout_ms, attempts); + + _had_ping_reply = false; + } + + if (_num_pings_missed >= 3) { + PX4_ERR("No ping response, disconnecting"); + _connected = false; + } + + int32_t tx_timeout = _param_uxrce_dds_tx_to.get(); + int32_t rx_timeout = _param_uxrce_dds_rx_to.get(); + + if (tx_timeout > 0 && _num_tx_rate_zero >= tx_timeout) { + PX4_ERR("Payload TX rate zero for too long, disconnecting"); + _connected = false; + } + + if (rx_timeout > 0 && _num_rx_rate_zero >= rx_timeout) { + PX4_ERR("Payload RX rate zero for too long, disconnecting"); + _connected = false; + } + } +} + +void UxrceddsClient::resetConnectivityCounters() +{ + _last_status_update = hrt_absolute_time(); + _last_ping = hrt_absolute_time(); + _had_ping_reply = false; + _num_pings_missed = 0; + _last_num_payload_sent = 0; + _last_num_payload_received = 0; + _num_tx_rate_zero = 0; + _num_rx_rate_zero = 0; +} + void UxrceddsClient::syncSystemClock(uxrSession *session) { struct timespec ts = {}; @@ -535,8 +630,8 @@ void UxrceddsClient::run() continue; } - if (!setup_session(&session)) { - delete_session(&session); + if (!setupSession(&session)) { + deleteSession(&session); px4_usleep(1'000'000); PX4_ERR("session setup failed, will retry now"); continue; @@ -552,13 +647,8 @@ void UxrceddsClient::run() } hrt_abstime last_sync_session = 0; - hrt_abstime last_status_update = hrt_absolute_time(); - hrt_abstime last_ping = hrt_absolute_time(); - int num_pings_missed = 0; - bool had_ping_reply = false; - uint32_t last_num_payload_sent{}; - uint32_t last_num_payload_received{}; int poll_error_counter = 0; + resetConnectivityCounters(); _subs->init(); _subs_initialized = true; @@ -629,55 +719,19 @@ void UxrceddsClient::run() // Check for a ping response /* PONG_IN_SESSION_STATUS */ if (session.on_pong_flag == 1) { - had_ping_reply = true; + _had_ping_reply = true; } - const hrt_abstime now = hrt_absolute_time(); + // Calculate the payload tx/rx rate for connectivity monitoring + calculateTxRxRate(); - if (now - last_status_update > 1_s) { - float dt = (now - last_status_update) / 1e6f; - _last_payload_tx_rate = (_subs->num_payload_sent - last_num_payload_sent) / dt; - _last_payload_rx_rate = (_pubs->num_payload_received - last_num_payload_received) / dt; - last_num_payload_sent = _subs->num_payload_sent; - last_num_payload_received = _pubs->num_payload_received; - last_status_update = now; - } - - // Handle ping, unless we're actively sending & receiving payloads successfully - if ((_last_payload_tx_rate > 0) && (_last_payload_rx_rate > 0)) { - _connected = true; - num_pings_missed = 0; - last_ping = now; - - } else { - if (hrt_elapsed_time(&last_ping) > 1_s) { - last_ping = now; - - if (had_ping_reply) { - num_pings_missed = 0; - - } else { - ++num_pings_missed; - } - - int timeout_ms = 1'000; // 1 second - uint8_t attempts = 1; - uxr_ping_agent_session(&session, timeout_ms, attempts); - - had_ping_reply = false; - } - - if (num_pings_missed >= 3) { - PX4_INFO("No ping response, disconnecting"); - _connected = false; - } - } + // Check if there is still connectivity with the agent + checkConnectivity(&session); perf_end(_loop_perf); - } - delete_session(&session); + deleteSession(&session); } } diff --git a/src/modules/uxrce_dds_client/uxrce_dds_client.h b/src/modules/uxrce_dds_client/uxrce_dds_client.h index 2a75d73e4a..823e242f9d 100644 --- a/src/modules/uxrce_dds_client/uxrce_dds_client.h +++ b/src/modules/uxrce_dds_client/uxrce_dds_client.h @@ -120,13 +120,17 @@ private: bool init(); void deinit(); - bool setup_session(uxrSession *session); - void delete_session(uxrSession *session); + bool setupSession(uxrSession *session); + void deleteSession(uxrSession *session); bool setBaudrate(int fd, unsigned baud); void handleMessageFormatRequest(); + void calculateTxRxRate(); + void checkConnectivity(uxrSession *session); + void resetConnectivityCounters(); + uORB::Publication _message_format_response_pub{ORB_ID(message_format_response)}; uORB::Subscription _message_format_request_sub{ORB_ID(message_format_request)}; @@ -179,6 +183,14 @@ private: uxrCommunication *_comm{nullptr}; int _fd{-1}; + hrt_abstime _last_status_update; + hrt_abstime _last_ping; + bool _had_ping_reply{false}; + int _num_pings_missed{0}; + int32_t _num_tx_rate_zero{0}; + int32_t _num_rx_rate_zero{0}; + uint32_t _last_num_payload_sent{0}; + uint32_t _last_num_payload_received{0}; int _last_payload_tx_rate{}; ///< in B/s int _last_payload_rx_rate{}; ///< in B/s @@ -197,6 +209,8 @@ private: (ParamInt) _param_uxrce_key, (ParamInt) _param_uxrce_dds_ptcfg, (ParamInt) _param_uxrce_dds_syncc, - (ParamInt) _param_uxrce_dds_synct + (ParamInt) _param_uxrce_dds_synct, + (ParamInt) _param_uxrce_dds_tx_to, + (ParamInt) _param_uxrce_dds_rx_to ) }; diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 5422a01898..3681c2b17c 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -172,11 +172,6 @@ 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) { @@ -186,7 +181,6 @@ void Standard::update_transition_state() } memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); - 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) @@ -198,6 +192,16 @@ void Standard::update_transition_state() _v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0]; } + + 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(); + + if (_v_control_mode->flag_control_climb_rate_enabled) { + roll_body = Eulerf(Quatf(_fw_virtual_att_sp->q_d)).phi(); + } + if (_vtol_mode == vtol_mode::TRANSITION_TO_FW) { if (_param_vt_psher_slew.get() <= FLT_EPSILON) { // just set the final target throttle value diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 9a05cf1034..7fb52b4688 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -203,11 +203,6 @@ 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) { @@ -217,7 +212,6 @@ void Tiltrotor::update_transition_state() } memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); - roll_body = Eulerf(Quatf(_fw_virtual_att_sp->q_d)).phi(); _thrust_transition = -_mc_virtual_att_sp->thrust_body[2]; } else { @@ -231,6 +225,15 @@ void Tiltrotor::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(); + + if (_v_control_mode->flag_control_climb_rate_enabled) { + roll_body = Eulerf(Quatf(_fw_virtual_att_sp->q_d)).phi(); + } + if (_vtol_mode == vtol_mode::TRANSITION_FRONT_P1) { // for the first part of the transition all rotors are enabled diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index e7ec2d3e01..a4355cfcbc 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -100,8 +100,6 @@ void VtolType::parameters_update() void VtolType::update_mc_state() { - resetAccelToPitchPitchIntegrator(); - // copy virtual attitude setpoint to real attitude setpoint memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); @@ -113,7 +111,6 @@ void VtolType::update_mc_state() void VtolType::update_fw_state() { - resetAccelToPitchPitchIntegrator(); _last_thr_in_fw_mode = _vehicle_thrust_setpoint_virtual_fw->xyz[0]; // copy virtual attitude setpoint to real attitude setpoint diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 737746a78b..592779ecfd 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -313,8 +313,6 @@ protected: float _quadchute_ref_alt{NAN}; // altitude (AMSL) reference to compute quad-chute altitude loss condition - float _accel_to_pitch_integ = 0; - bool _quadchute_command_treated{false}; bool isFrontTransitionCompleted(); @@ -364,7 +362,6 @@ protected: private: hrt_abstime _throttle_blend_start_ts{0}; // time at which we start blending between transition throttle and fixed wing throttle - void resetAccelToPitchPitchIntegrator() { _accel_to_pitch_integ = 0.f; } bool shouldBlendThrottleAfterFrontTransition() { return _throttle_blend_start_ts != 0; }; void stopBlendingThrottleAfterFrontTransition() { _throttle_blend_start_ts = 0; } diff --git a/src/modules/zenoh/Kconfig b/src/modules/zenoh/Kconfig index c3c042df13..3cce22f026 100644 --- a/src/modules/zenoh/Kconfig +++ b/src/modules/zenoh/Kconfig @@ -46,9 +46,6 @@ if MODULES_ZENOH select ZENOH_PUBSUB_VEHICLE_ODOMETRY select ZENOH_PUBSUB_VEHICLE_RATES_SETPOINT select ZENOH_PUBSUB_VEHICLE_COMMAND - select ZENOH_PUBSUB_VEHICLE_TRAJECTORY_BEZIER - select ZENOH_PUBSUB_VEHICLE_TRAJECTORY_WAYPOINT - config ZENOH_PUBSUB_ALL bool "All" diff --git a/src/modules/zenoh/Kconfig.topics b/src/modules/zenoh/Kconfig.topics index 585831cd2f..ac96b8586f 100644 --- a/src/modules/zenoh/Kconfig.topics +++ b/src/modules/zenoh/Kconfig.topics @@ -741,10 +741,6 @@ menu "Zenoh publishers/subscribers" bool "timesync_status" default n - config ZENOH_PUBSUB_TRAJECTORY_BEZIER - bool "trajectory_bezier" - default n - config ZENOH_PUBSUB_TRAJECTORY_SETPOINT bool "trajectory_setpoint" default n @@ -881,14 +877,6 @@ menu "Zenoh publishers/subscribers" bool "vehicle_torque_setpoint" default n - config ZENOH_PUBSUB_VEHICLE_TRAJECTORY_BEZIER - bool "vehicle_trajectory_bezier" - default n - - config ZENOH_PUBSUB_VEHICLE_TRAJECTORY_WAYPOINT - bool "vehicle_trajectory_waypoint" - default n - config ZENOH_PUBSUB_VELOCITY_LIMITS bool "velocity_limits" default n @@ -1099,7 +1087,6 @@ config ZENOH_PUBSUB_ALL_SELECTION select ZENOH_PUBSUB_TELEMETRY_STATUS select ZENOH_PUBSUB_TILTROTOR_EXTRA_CONTROLS select ZENOH_PUBSUB_TIMESYNC_STATUS - select ZENOH_PUBSUB_TRAJECTORY_BEZIER select ZENOH_PUBSUB_TRAJECTORY_SETPOINT select ZENOH_PUBSUB_TRAJECTORY_WAYPOINT select ZENOH_PUBSUB_TRANSPONDER_REPORT @@ -1134,8 +1121,6 @@ config ZENOH_PUBSUB_ALL_SELECTION select ZENOH_PUBSUB_VEHICLE_STATUS select ZENOH_PUBSUB_VEHICLE_THRUST_SETPOINT select ZENOH_PUBSUB_VEHICLE_TORQUE_SETPOINT - select ZENOH_PUBSUB_VEHICLE_TRAJECTORY_BEZIER - select ZENOH_PUBSUB_VEHICLE_TRAJECTORY_WAYPOINT select ZENOH_PUBSUB_VELOCITY_LIMITS select ZENOH_PUBSUB_VTOL_VEHICLE_STATUS select ZENOH_PUBSUB_WHEEL_ENCODERS diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/CMakeLists.txt b/src/systemcmds/mft_cfg/CMakeLists.txt similarity index 87% rename from src/modules/rover_differential/RoverDifferentialGuidance/CMakeLists.txt rename to src/systemcmds/mft_cfg/CMakeLists.txt index 81f350d484..ca1a3b5544 100644 --- a/src/modules/rover_differential/RoverDifferentialGuidance/CMakeLists.txt +++ b/src/systemcmds/mft_cfg/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2023-2024 PX4 Development Team. All rights reserved. +# Copyright (c) 2025 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,9 +30,10 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ - -px4_add_library(RoverDifferentialGuidance - RoverDifferentialGuidance.cpp -) - -target_include_directories(RoverDifferentialGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +px4_add_module( + MODULE systemcmds__mft_cfg + MAIN mft_cfg + COMPILE_FLAGS + SRCS + mft_cfg.cpp + ) diff --git a/src/systemcmds/mft_cfg/Kconfig b/src/systemcmds/mft_cfg/Kconfig new file mode 100644 index 0000000000..85091c6ff3 --- /dev/null +++ b/src/systemcmds/mft_cfg/Kconfig @@ -0,0 +1,5 @@ +menuconfig SYSTEMCMDS_MFT_CFG + bool "mft_cfg" + default n + ---help--- + Enable support for mft_cfg diff --git a/src/systemcmds/mft_cfg/mft_cfg.cpp b/src/systemcmds/mft_cfg/mft_cfg.cpp new file mode 100644 index 0000000000..c87c4e6bdc --- /dev/null +++ b/src/systemcmds/mft_cfg/mft_cfg.cpp @@ -0,0 +1,239 @@ +/**************************************************************************** +* +* Copyright (c) 2025 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 mft_cfg.c +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +static void usage(const char *reason) +{ + if (reason != nullptr) { + printf("%s\n\n", reason); + } + + PRINT_MODULE_DESCRIPTION("Tool to set and get manifest configuration"); + + PRINT_MODULE_USAGE_NAME("mft_cfg", "command"); + PRINT_MODULE_USAGE_COMMAND_DESCR("get", "get manifest configuration"); + PRINT_MODULE_USAGE_COMMAND_DESCR("set", "set manifest configuration"); + PRINT_MODULE_USAGE_COMMAND_DESCR("reset", "reset manifest configuration"); + PRINT_MODULE_USAGE_ARG("hwver|hwrev", "Select type: MTD_MTF_VER|MTD_MTF_REV", false); + PRINT_MODULE_USAGE_PARAM_INT('i', 0x10, 0x10, 0xFFFF, + "argument to set extended hardware id (id == version for , id == revision for )", false); + +} + + +static int print_extended_id(const char *type) +{ + mtd_mft_v0_t mtd_mft; + mtd_mft.version.id = MTD_MFT_v0; + mtd_mft.hw_extended_id = -1; + + const char *path = nullptr; + int ret_val = px4_mtd_query(type, NULL, &path); + + if (ret_val != PX4_OK) { + PX4_ERR("Can't get mtd query (%s, %i)", type, ret_val); + + } else { + + ret_val = board_get_eeprom_hw_info(path, (mtd_mft_t *)&mtd_mft); + + if (ret_val == PX4_OK) { + PX4_INFO("%s, hw_extended_id = %#x", type, mtd_mft.hw_extended_id); + + } else { + + if (ret_val == -EPROTO) { + PX4_ERR("Manifest data may not exist for %s", path); + + } else { + PX4_ERR("Can't read hw_extended_id from EEPROM (%s, %i)", type, ret_val); + } + } + } + + return ret_val; +} + +extern "C" __EXPORT int mft_cfg_main(int argc, char *argv[]) +{ + if ((argc == 2) && (!strcmp(argv[1], "get"))) { + + char type_ver[] = "MTD_MFT_VER"; + char type_rev[] = "MTD_MFT_REV"; + + print_extended_id(type_ver); + print_extended_id(type_rev); + + return 0; + + } else if (argc >= 3) { + + int ret_val = -1; + const char *path = nullptr; + bool arg_exist = false; + + for (int i = 2; i < argc; ++i) { + if (strcmp("hwver", argv[i]) == 0) { + ret_val = px4_mtd_query("MTD_MFT_VER", NULL, &path); + arg_exist = true; + break; + } + + if (strcmp("hwrev", argv[i]) == 0) { + ret_val = px4_mtd_query("MTD_MFT_REV", NULL, &path); + arg_exist = true; + break; + } + } + + if (!arg_exist) { + PX4_ERR("Missing or arguments'"); + return 1; + } + + if (ret_val != PX4_OK) { + PX4_ERR("Can't get mtd query (%i)", ret_val); + return 1; + } + + mtd_mft_v0_t mtd_mft; + mtd_mft.version.id = MTD_MFT_v0; + mtd_mft.hw_extended_id = -1; + + if (!strcmp(argv[1], "set")) { + if (argc == 5) { + + const char *myoptarg = NULL; + int ch = 0; + int myoptind = 1; + int hw_extended_id = -1; + + while ((ch = px4_getopt(argc, argv, "i:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'i': + hw_extended_id = strtol(myoptarg, NULL, 0); + break; + + default: + PX4_ERR("To set id use '-i x'"); + break; + } + } + + if (hw_extended_id != -1) { + + mtd_mft.hw_extended_id = (uint16_t)hw_extended_id; + + ret_val = board_set_eeprom_hw_info(path, (mtd_mft_t *)&mtd_mft); + + if (ret_val != PX4_OK) { + PX4_ERR("Can't write to EEPROM (%i)", ret_val); + + } else { + board_get_eeprom_hw_info(path, (mtd_mft_t *)&mtd_mft); + PX4_INFO("New hw_extended_id = %#x", mtd_mft.hw_extended_id); + } + } + + } else { + PX4_ERR("Not enough arguments, try 'mft_cfg set hwver -i x'"); + return 1; + } + + return 0; + } + + if (!strcmp(argv[1], "get")) { + + ret_val = board_get_eeprom_hw_info(path, (mtd_mft_t *)&mtd_mft); + + if (ret_val == PX4_OK) { + PX4_INFO("hw_extended_id = %#x", mtd_mft.hw_extended_id); + + } else { + + if (ret_val == -EPROTO) { + PX4_ERR("Manifest data may not exist for %s", path); + + } else { + PX4_ERR("Can't read from EEPROM (%s, %i)", path, ret_val); + } + + return 1; + } + + return 0; + } + + if (!strcmp(argv[1], "reset")) { + + uint8_t buffer[64]; + memset(buffer, 0xFF, sizeof(buffer)); + + int fd = open(path, O_WRONLY); + + if (fd == -1) { + PX4_ERR("Failed to open partition %s", path); + return 1; + } + + while (write(fd, buffer, sizeof(buffer)) == sizeof(buffer)) {} + + PX4_INFO("Reset for %s completed. To remove manifest data from RAM, a reboot is required.", path); + close(fd); + + return 0; + } + + } else { + usage("Error, not enough parameters."); + return 1; + } + + return 0; +} diff --git a/src/systemcmds/tests/CMakeLists.txt b/src/systemcmds/tests/CMakeLists.txt index 72d99c6708..2beab87943 100644 --- a/src/systemcmds/tests/CMakeLists.txt +++ b/src/systemcmds/tests/CMakeLists.txt @@ -33,7 +33,6 @@ set(srcs test_atomic_bitset.cpp - test_bezierQuad.cpp test_bitset.cpp test_bson.cpp test_dataman.cpp diff --git a/src/systemcmds/tests/test_bezierQuad.cpp b/src/systemcmds/tests/test_bezierQuad.cpp deleted file mode 100644 index 9fc5f7f987..0000000000 --- a/src/systemcmds/tests/test_bezierQuad.cpp +++ /dev/null @@ -1,278 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2018-2019 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 test_bezierQuad.cpp - * Test for Bezier curve computation. - */ - -#include -#include -#include -#include - -#include "../../lib/bezier/BezierQuad.hpp" - -class BezierQuadTest : public UnitTest -{ -public: - virtual bool run_tests(); - -private: - - bool _get_states_from_time(); - bool _get_arc_length(); - bool _set_bez_from_vel(); - - float random(float min, float max); - -}; - - -bool BezierQuadTest::run_tests() -{ - ut_run_test(_get_states_from_time); - ut_run_test(_get_arc_length); - ut_run_test(_set_bez_from_vel); - - return (_tests_failed == 0); -} - -bool BezierQuadTest::_get_states_from_time() -{ - // symmetric around 0 - matrix::Vector3f pt0(-0.5f, 0.0f, 0.0f); - matrix::Vector3f ctrl(0.0f, 0.5f, 0.0f); - matrix::Vector3f pt1(0.5f, 0.0f, 0.0f); - - // create bezier with default t = [0,1] - bezier::BezierQuad_f bz(pt0, ctrl, pt1); - - matrix::Vector3f pos, vel, acc; - float precision = 0.00001; - - // states at time = 0 - bz.getStates(pos, vel, acc, 0.0f); - - ut_compare_float("pos[0] not equal pt0[0]", pos(0), pt0(0), precision); - ut_compare_float("pos[1] not equal pt0[1]", pos(1), pt0(1), precision); - ut_compare_float("pos[2] not equal pt0[2]", pos(2), pt0(2), precision); - - ut_compare_float("slope not equal 1", vel(0), 1.0f, precision); - ut_compare_float("slope not equal 1", vel(1), 1.0f, precision); - ut_compare_float("slope not equal 0", vel(2), 0.0f, precision); - - ut_compare_float("acc not equal 0", acc(0), 0.0f, precision); - ut_compare_float("acc not equal 1", acc(1), -2.0f, precision); - ut_compare_float("acc not equal 0", acc(2), 0.0f, precision); - - // states at time = 1 - bz.getStates(pos, vel, acc, 1.0f); - - ut_compare_float("pos[0] not equal pt1[0]", pos(0), pt1(0), precision); - ut_compare_float("pos[1] not equal pt1[1]", pos(1), pt1(1), precision); - ut_compare_float("pos[2] not equal pt1[2]", pos(2), pt1(2), precision); - - ut_compare_float("slope not equal 1", vel(0), 1.0f, precision); - ut_compare_float("slope not equal -1", vel(1), -1.0f, precision); - ut_compare_float("slope not equal 0", vel(2), 0.0f, precision); - - ut_compare_float("acc not equal 0", acc(0), 0.0f, precision); - ut_compare_float("acc not equal 1", acc(1), -2.0f, precision); - ut_compare_float("acc not equal 0", acc(2), 0.0f, precision); - - // states at time = 0.5 - bz.getStates(pos, vel, acc, 0.50f); - - // pos must be equal to ctrl(0) and lower than ctrl(1) - ut_compare_float("pos[0] not equal ctrl[0]", pos(0), ctrl(0), precision); - ut_assert_true(pos(1) < ctrl(1)); - - ut_compare_float("slope not equal 1", vel(0), 1.0f, precision); - ut_compare_float("slope not equal -1", vel(1), 0.0f, precision); - ut_compare_float("slope not equal 0", vel(2), 0.0f, precision); - - ut_compare_float("acc not equal 0", acc(0), 0.0f, precision); - ut_compare_float("acc not equal -2", acc(1), -2.0f, precision); - ut_compare_float("acc not equal 0", acc(2), 0.0f, precision); - - // acceleration - pt0 = matrix::Vector3f(0.0f, 0.0f, 0.0f); - ctrl = matrix::Vector3f(0.0f, 0.0f, 0.0f); - pt1 = matrix::Vector3f(1.0f, 0.0f, 0.0f); - - // create bezier with default t = [0,1] - bz.setBezier(pt0, ctrl, pt1, 1.0f); - - // states at time = 0.0 - bz.getStates(pos, vel, acc, 0.0f); - - ut_compare_float("pos[0] not equal pt0[0]", pos(0), pt0(0), precision); - ut_compare_float("pos[1] not equal pt0[1]", pos(1), pt0(1), precision); - ut_compare_float("pos[2] not equal pt0[2]", pos(2), pt0(2), precision); - - ut_compare_float("slope not equal 0", vel(0), 0.0f, precision); - ut_compare_float("slope not equal 0", vel(1), 0.0f, precision); - ut_compare_float("slope not equal 0", vel(2), 0.0f, precision); - - ut_compare_float("acc not equal 2", acc(0), 2.0f, precision); - ut_compare_float("acc not equal 0", acc(1), 0.0f, precision); - ut_compare_float("acc not equal 0", acc(2), 0.0f, precision); - - // states at time = 1.0 - bz.getStates(pos, vel, acc, 1.0f); - - ut_compare_float("pos[0] not equal pt1[0]", pos(0), pt1(0), precision); - ut_compare_float("pos[1] not equal pt1[1]", pos(1), pt1(1), precision); - ut_compare_float("pos[2] not equal pt1[2]", pos(2), pt1(2), precision); - - ut_compare_float("slope not equal 2", vel(0), 2.0f, precision); - ut_compare_float("slope not equal 0", vel(1), 0.0f, precision); - ut_compare_float("slope not equal 0", vel(2), 0.0f, precision); - - ut_compare_float("acc not equal 2", acc(0), 2.0f, precision); - ut_compare_float("acc not equal 0", acc(1), 0.0f, precision); - ut_compare_float("acc not equal 0", acc(2), 0.0f, precision); - - // states at time = 0.5 - bz.getStates(pos, vel, acc, 0.5f); - - ut_compare_float("slope not equal 1", vel(0), 1.0f, precision); - ut_compare_float("slope not equal 0", vel(1), 0.0f, precision); - ut_compare_float("slope not equal 0", vel(2), 0.0f, precision); - - ut_compare_float("acc not equal 2", acc(0), 2.0f, precision); - ut_compare_float("acc not equal 0", acc(1), 0.0f, precision); - ut_compare_float("acc not equal 0", acc(2), 0.0f, precision); - - return true; - -} - -bool BezierQuadTest::_get_arc_length() -{ - // create random numbers - srand(0); // choose a constant to make it deterministic - - float min = -50.f; - float max = 50.f; - float resolution = 0.1f; - - matrix::Vector3f pt0, pt1, ctrl; - float duration, arc_length, triangle_length, straigth_length; - float T = 100.0f; - - // loop trough different control points 100x and check if arc_length is in the expected range - for (int i = 0; i < 100 ; i++) { - // random bezier point - pt0 = matrix::Vector3f(random(min, max), random(min, max), random(min, max)); - pt1 = matrix::Vector3f(random(min, max), random(min, max), random(min, max)); - ctrl = matrix::Vector3f(random(min, max), random(min, max), random(min, max)); - - // use for each test a new duration - duration = random(0.0f, T); - - // create bezier - bezier::BezierQuad_f bz(pt0, ctrl, pt1, duration); - - // compute arc length, triangle length and straigh length - arc_length = bz.getArcLength(resolution); - triangle_length = (ctrl - pt0).length() + (pt1 - ctrl).length(); - straigth_length = (pt1 - pt0).length(); - - // we also compute length from going point to point and add segment - float time_increment = duration / T; - float t = 0.0f + time_increment; - matrix::Vector3f p0 = pt0; - float sum_segments = 0.0f; - - for (int s = 0; s < (int)T; s++) { - matrix::Vector3f nextpt = bz.getPoint(t); - sum_segments = (nextpt - p0).length() + sum_segments; - p0 = bz.getPoint(t); - t = t + time_increment; - } - - // test comparisons - ut_assert_true((triangle_length >= arc_length) && (arc_length >= straigth_length) - && (fabsf(arc_length - sum_segments) < 1.f)); - } - - - return true; -} - -bool BezierQuadTest::_set_bez_from_vel() -{ - // create random numbers - srand(100); // choose a constant to make it deterministic - - float low = -50.0f; - float max = 50.0f; - float precision = 0.001f; - - for (int i = 0; i < 20; i++) { - - // set velocity - matrix::Vector3f ctrl(random(low, max), random(low, max), random(low, max)); - matrix::Vector3f vel0(random(low, max), random(low, max), random(low, max)); - matrix::Vector3f vel1(random(low, max), random(low, max), random(low, max)); - float duration = random(0.0f, 100.0f); - - bezier::BezierQuad_f bz;; - bz.setBezFromVel(ctrl, vel0, vel1, duration); - - // get velocity back - matrix::Vector3f v0 = bz.getVelocity(0.0f); - matrix::Vector3f v1 = bz.getVelocity(duration); - ut_compare_float("", vel0(0), v0(0), precision); - ut_compare_float("", vel1(0), v1(0), precision); - - ut_compare_float("", vel0(1), v0(1), precision); - ut_compare_float("", vel1(1), v1(1), precision); - - ut_compare_float("", vel0(2), v0(2), precision); - ut_compare_float("", vel1(2), v1(2), precision); - } - - return true; -} - -float BezierQuadTest::random(float min, float max) -{ - float s = rand() / (float)RAND_MAX; - return (min + s * (max - min)); - -} - -ut_declare_test_c(test_bezierQuad, BezierQuadTest) diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index af25fd074e..3d04ad31ef 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -78,7 +78,6 @@ const struct { #endif /* __PX4_NUTTX */ {"atomic_bitset", test_atomic_bitset, 0}, - {"bezier", test_bezierQuad, 0}, {"bitset", test_bitset, 0}, {"bson", test_bson, 0}, {"dataman", test_dataman, OPT_NOJIGTEST | OPT_NOALLTEST}, diff --git a/src/systemcmds/tests/tests_main.h b/src/systemcmds/tests/tests_main.h index 0e5a14743c..c42bce07fc 100644 --- a/src/systemcmds/tests/tests_main.h +++ b/src/systemcmds/tests/tests_main.h @@ -44,7 +44,6 @@ __BEGIN_DECLS extern int test_atomic_bitset(int argc, char *argv[]); -extern int test_bezierQuad(int argc, char *argv[]); extern int test_bitset(int argc, char *argv[]); extern int test_bson(int argc, char *argv[]); extern int test_dataman(int argc, char *argv[]); diff --git a/test/mavros_posix_test_avoidance.test b/test/mavros_posix_test_avoidance.test deleted file mode 100644 index 5c5adeaa4d..0000000000 --- a/test/mavros_posix_test_avoidance.test +++ /dev/null @@ -1,52 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - $(arg pointcloud_topics) - - - - diff --git a/test/rostest_avoidance_run.sh b/test/rostest_avoidance_run.sh deleted file mode 100755 index 2fc2be7eb6..0000000000 --- a/test/rostest_avoidance_run.sh +++ /dev/null @@ -1,20 +0,0 @@ -#! /bin/bash - -DIR=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd ) -PX4_SRC_DIR=${DIR}/.. - -source /opt/ros/${ROS_DISTRO:-kinetic}/setup.bash -mkdir -p ${PX4_SRC_DIR}/catkin_ws/src -cd ${PX4_SRC_DIR}/catkin_ws/ -git clone -b 0.3.1 --single-branch --depth 1 https://github.com/PX4/avoidance.git src/avoidance - -catkin init -catkin build local_planner safe_landing_planner --cmake-args -DCMAKE_BUILD_TYPE=Release - -source ${PX4_SRC_DIR}/catkin_ws/devel/setup.bash -source /usr/share/gazebo/setup.sh - -export CATKIN_SETUP_UTIL_ARGS=--extend -export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:${PX4_SRC_DIR}/catkin_ws/src/avoidance/avoidance/sim/models - -source $DIR/rostest_px4_run.sh "$@"