From 8242968b2b1b9530b208bd7345516717474b0677 Mon Sep 17 00:00:00 2001 From: Peter van der Perk Date: Tue, 22 Dec 2020 10:21:22 +0100 Subject: [PATCH] UAVCAN v1 GPS demo with MAVCAN autoconfigure --- .ci/Jenkinsfile-compile | 10 +- .devcontainer/devcontainer.json | 2 +- .github/workflows/checks.yml | 2 +- .github/workflows/clang-tidy.yml | 2 +- .github/workflows/compile_linux.yml | 2 +- .github/workflows/compile_linux_arm64.yml | 2 +- .github/workflows/compile_nuttx.yml | 4 +- .github/workflows/compile_nuttx_cannode.yml | 2 +- .github/workflows/mavros_avoidance_tests.yml | 2 +- .github/workflows/mavros_mission_tests.yml | 2 +- .github/workflows/mavros_offboard_tests.yml | 2 +- .github/workflows/metadata.yml | 14 +- .github/workflows/sitl_tests.yml | 2 +- .gitmodules | 18 +- Jenkinsfile | 24 +- Tools/docker_run.sh | 10 +- boards/nxp/ucans32k146/default.cmake | 5 +- boards/px4/fmu-v4/default.cmake | 3 +- boards/px4/fmu-v4/nuttx-config/nsh/defconfig | 3 - .../fmu-v4/nuttx-config/uavcanv1/defconfig | 239 +++++++++ boards/px4/fmu-v4/uavcanv1.cmake | 137 +++++ boards/px4/fmu-v5/default.cmake | 3 +- boards/px4/fmu-v5/nuttx-config/nsh/defconfig | 6 - .../fmu-v5/nuttx-config/uavcanv1/defconfig | 250 +++++++++ boards/px4/fmu-v5/uavcanv1.cmake | 140 +++++ src/drivers/uavcan_v1/.Uavcan.cpp.kate-swp | Bin 65 -> 0 bytes src/drivers/uavcan_v1/CMakeLists.txt | 6 - src/drivers/uavcan_v1/CanardSocketCAN.cpp | 6 +- src/drivers/uavcan_v1/Uavcan.cpp | 343 +++++++----- src/drivers/uavcan_v1/Uavcan.hpp | 51 +- .../uavcan_v1/public_regulated_data_types | 2 +- src/drivers/uavcan_v1/templates/Header.j2 | 32 -- .../uavcan_v1/templates/ServiceType.j2 | 10 - .../uavcan_v1/templates/StructureType.j2 | 5 - src/drivers/uavcan_v1/templates/UnionType.j2 | 5 - .../uavcan_v1/templates/_composite_type.j2 | 115 ---- .../uavcannode_gps_demo/CMakeLists.txt | 82 +++ src/drivers/uavcannode_gps_demo/canard_main.c | 448 ++++++++++++++++ src/drivers/uavcannode_gps_demo/libcanard | 1 + .../uavcannode_gps_demo/libcancl/README.md | 12 + .../uavcannode_gps_demo/libcancl/pnp.c | 264 ++++++++++ .../uavcannode_gps_demo/libcancl/pnp.h | 74 +++ .../libcancl/registerinterface.c | 279 ++++++++++ .../libcancl/registerinterface.h | 49 ++ .../uavcannode_gps_demo/libcancl/time.h | 6 + src/drivers/uavcannode_gps_demo/o1heap.c | 498 ++++++++++++++++++ src/drivers/uavcannode_gps_demo/o1heap.h | 142 +++++ .../public_regulated_data_types | 1 + src/drivers/uavcannode_gps_demo/socketcan.c | 171 ++++++ src/drivers/uavcannode_gps_demo/socketcan.h | 69 +++ .../uavcannode_gps_demo/uorb_converter.c | 107 ++++ .../uavcannode_gps_demo/uorb_converter.h | 34 ++ 52 files changed, 3294 insertions(+), 404 deletions(-) create mode 100644 boards/px4/fmu-v4/nuttx-config/uavcanv1/defconfig create mode 100644 boards/px4/fmu-v4/uavcanv1.cmake create mode 100644 boards/px4/fmu-v5/nuttx-config/uavcanv1/defconfig create mode 100644 boards/px4/fmu-v5/uavcanv1.cmake delete mode 100644 src/drivers/uavcan_v1/.Uavcan.cpp.kate-swp delete mode 100644 src/drivers/uavcan_v1/templates/Header.j2 delete mode 100644 src/drivers/uavcan_v1/templates/ServiceType.j2 delete mode 100644 src/drivers/uavcan_v1/templates/StructureType.j2 delete mode 100644 src/drivers/uavcan_v1/templates/UnionType.j2 delete mode 100644 src/drivers/uavcan_v1/templates/_composite_type.j2 create mode 100644 src/drivers/uavcannode_gps_demo/CMakeLists.txt create mode 100644 src/drivers/uavcannode_gps_demo/canard_main.c create mode 160000 src/drivers/uavcannode_gps_demo/libcanard create mode 100644 src/drivers/uavcannode_gps_demo/libcancl/README.md create mode 100644 src/drivers/uavcannode_gps_demo/libcancl/pnp.c create mode 100644 src/drivers/uavcannode_gps_demo/libcancl/pnp.h create mode 100644 src/drivers/uavcannode_gps_demo/libcancl/registerinterface.c create mode 100644 src/drivers/uavcannode_gps_demo/libcancl/registerinterface.h create mode 100644 src/drivers/uavcannode_gps_demo/libcancl/time.h create mode 100644 src/drivers/uavcannode_gps_demo/o1heap.c create mode 100644 src/drivers/uavcannode_gps_demo/o1heap.h create mode 160000 src/drivers/uavcannode_gps_demo/public_regulated_data_types create mode 100644 src/drivers/uavcannode_gps_demo/socketcan.c create mode 100644 src/drivers/uavcannode_gps_demo/socketcan.h create mode 100644 src/drivers/uavcannode_gps_demo/uorb_converter.c create mode 100644 src/drivers/uavcannode_gps_demo/uorb_converter.h diff --git a/.ci/Jenkinsfile-compile b/.ci/Jenkinsfile-compile index b2fa883eef..8199a99d68 100644 --- a/.ci/Jenkinsfile-compile +++ b/.ci/Jenkinsfile-compile @@ -9,10 +9,10 @@ pipeline { script { def build_nodes = [:] def docker_images = [ - armhf: "px4io/px4-dev-armhf:2020-11-18", - arm64: "px4io/px4-dev-aarch64:2020-11-18", - base: "px4io/px4-dev-base-bionic:2020-11-18", - nuttx: "px4io/px4-dev-nuttx-focal:2020-11-18", + armhf: "px4io/px4-dev-armhf:2021-02-04", + arm64: "px4io/px4-dev-aarch64:2021-02-04", + base: "px4io/px4-dev-base-bionic:2021-02-04", + nuttx: "px4io/px4-dev-nuttx-focal:2021-02-04", snapdragon: "lorenzmeier/px4-dev-snapdragon:2020-04-01" ] @@ -128,7 +128,7 @@ pipeline { // TODO: actually upload artifacts to S3 // stage('S3 Upload') { // agent { - // docker { image 'px4io/px4-dev-base-focal:2020-11-18' } + // docker { image 'px4io/px4-dev-base-focal:2021-02-04' } // } // options { // skipDefaultCheckout() diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 9d9ac33721..eb2dc80d1f 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -2,7 +2,7 @@ // https://github.com/microsoft/vscode-dev-containers/tree/v0.134.0/containers/cpp { "name": "px4-dev-nuttx", - "image": "px4io/px4-dev-nuttx-focal:2020-11-18", + "image": "px4io/px4-dev-nuttx-focal:2021-02-04", "runArgs": [ "--cap-add=SYS_PTRACE", "--security-opt", "seccomp=unconfined" ], diff --git a/.github/workflows/checks.yml b/.github/workflows/checks.yml index 61c6dd46b1..c4b0e2a328 100644 --- a/.github/workflows/checks.yml +++ b/.github/workflows/checks.yml @@ -28,7 +28,7 @@ jobs: "parameters_metadata", ] container: - image: px4io/px4-dev-nuttx-focal:2020-11-18 + image: px4io/px4-dev-nuttx-focal:2021-02-04 options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined steps: - uses: actions/checkout@v1 diff --git a/.github/workflows/clang-tidy.yml b/.github/workflows/clang-tidy.yml index c9c2fd69b2..eb12f605b5 100644 --- a/.github/workflows/clang-tidy.yml +++ b/.github/workflows/clang-tidy.yml @@ -11,7 +11,7 @@ on: jobs: build: runs-on: ubuntu-latest - container: px4io/px4-dev-clang:2020-11-18 + container: px4io/px4-dev-clang:2021-02-04 steps: - uses: actions/checkout@v1 with: diff --git a/.github/workflows/compile_linux.yml b/.github/workflows/compile_linux.yml index ec1b40abad..3ce87d237d 100644 --- a/.github/workflows/compile_linux.yml +++ b/.github/workflows/compile_linux.yml @@ -11,7 +11,7 @@ on: jobs: build: runs-on: ubuntu-latest - container: px4io/px4-dev-armhf:2020-11-18 + container: px4io/px4-dev-armhf:2021-02-04 strategy: matrix: config: [ diff --git a/.github/workflows/compile_linux_arm64.yml b/.github/workflows/compile_linux_arm64.yml index 2de6ab1246..2f16acffef 100644 --- a/.github/workflows/compile_linux_arm64.yml +++ b/.github/workflows/compile_linux_arm64.yml @@ -11,7 +11,7 @@ on: jobs: build: runs-on: ubuntu-latest - container: px4io/px4-dev-aarch64:2020-11-18 + container: px4io/px4-dev-aarch64:2021-02-04 strategy: matrix: config: [ diff --git a/.github/workflows/compile_nuttx.yml b/.github/workflows/compile_nuttx.yml index 8b6ebc57d5..0569cd13ed 100644 --- a/.github/workflows/compile_nuttx.yml +++ b/.github/workflows/compile_nuttx.yml @@ -11,7 +11,7 @@ on: jobs: build: runs-on: ubuntu-latest - container: px4io/px4-dev-nuttx-focal:2020-11-18 + container: px4io/px4-dev-nuttx-focal:2021-02-04 strategy: matrix: config: [ @@ -59,6 +59,7 @@ jobs: px4_fmu-v4_ctrlalloc, px4_fmu-v4_default, px4_fmu-v4_optimized, + px4_fmu-v4_uavcanv1, px4_fmu-v4pro_default, px4_fmu-v5_ctrlalloc, px4_fmu-v5_default, @@ -68,6 +69,7 @@ jobs: px4_fmu-v5_rover, px4_fmu-v5_rtps, px4_fmu-v5_stackcheck, + px4_fmu-v5_uavcanv1, px4_fmu-v5x_base_phy_DP83848C, px4_fmu-v5x_default, px4_fmu-v6x_default, diff --git a/.github/workflows/compile_nuttx_cannode.yml b/.github/workflows/compile_nuttx_cannode.yml index 4694352121..cdc14ab4ac 100644 --- a/.github/workflows/compile_nuttx_cannode.yml +++ b/.github/workflows/compile_nuttx_cannode.yml @@ -11,7 +11,7 @@ on: jobs: build: runs-on: ubuntu-latest - container: px4io/px4-dev-nuttx-focal:2020-11-18 + container: px4io/px4-dev-nuttx-focal:2021-02-04 strategy: matrix: config: [ diff --git a/.github/workflows/mavros_avoidance_tests.yml b/.github/workflows/mavros_avoidance_tests.yml index 52d62b761e..24dd53629f 100644 --- a/.github/workflows/mavros_avoidance_tests.yml +++ b/.github/workflows/mavros_avoidance_tests.yml @@ -19,7 +19,7 @@ jobs: - {test_file: "mavros_posix_test_safe_landing.test", vehicle: "iris_obs_avoid", mission: "MC_safe_landing", build_type: "RelWithDebInfo"} container: - image: px4io/px4-dev-ros-melodic:2020-11-18 + image: px4io/px4-dev-ros-melodic:2021-02-04 options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined steps: - uses: actions/checkout@v1 diff --git a/.github/workflows/mavros_mission_tests.yml b/.github/workflows/mavros_mission_tests.yml index 4d6d25c89b..6463e17d12 100644 --- a/.github/workflows/mavros_mission_tests.yml +++ b/.github/workflows/mavros_mission_tests.yml @@ -25,7 +25,7 @@ jobs: #- {vehicle: "tiltrotor", mission: "VTOL_mission_1", build_type: "RelWithDebInfo"} container: - image: px4io/px4-dev-ros-melodic:2020-11-18 + image: px4io/px4-dev-ros-melodic:2021-02-04 options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined steps: - uses: actions/checkout@v1 diff --git a/.github/workflows/mavros_offboard_tests.yml b/.github/workflows/mavros_offboard_tests.yml index 2839c0e5b7..ed3832ca65 100644 --- a/.github/workflows/mavros_offboard_tests.yml +++ b/.github/workflows/mavros_offboard_tests.yml @@ -20,7 +20,7 @@ jobs: #- {test_file: "mavros_posix_tests_offboard_rpyrt_ctl.test", vehicle: "iris", build_type: "RelWithDebInfo"} container: - image: px4io/px4-dev-ros-melodic:2020-11-18 + image: px4io/px4-dev-ros-melodic:2021-02-04 options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined steps: - uses: actions/checkout@v1 diff --git a/.github/workflows/metadata.yml b/.github/workflows/metadata.yml index 3f81697f8b..d73eff4ae0 100644 --- a/.github/workflows/metadata.yml +++ b/.github/workflows/metadata.yml @@ -12,7 +12,7 @@ jobs: airframe: runs-on: ubuntu-latest - container: px4io/px4-dev-base-focal:2020-11-18 + container: px4io/px4-dev-base-focal:2021-02-04 steps: - uses: actions/checkout@v1 with: @@ -27,7 +27,7 @@ jobs: module: runs-on: ubuntu-latest - container: px4io/px4-dev-base-focal:2020-11-18 + container: px4io/px4-dev-base-focal:2021-02-04 steps: - uses: actions/checkout@v1 with: @@ -42,7 +42,7 @@ jobs: parameter: runs-on: ubuntu-latest - container: px4io/px4-dev-base-focal:2020-11-18 + container: px4io/px4-dev-base-focal:2021-02-04 steps: - uses: actions/checkout@v1 with: @@ -57,7 +57,7 @@ jobs: uorb_graph: runs-on: ubuntu-latest - container: px4io/px4-dev-nuttx-focal:2020-11-18 + container: px4io/px4-dev-nuttx-focal:2021-02-04 steps: - uses: actions/checkout@v1 with: @@ -72,7 +72,7 @@ jobs: micrortps_agent: runs-on: ubuntu-latest - container: px4io/px4-dev-base-focal:2020-11-18 + container: px4io/px4-dev-base-focal:2021-02-04 steps: - uses: actions/checkout@v1 with: @@ -86,7 +86,7 @@ jobs: ROS_msgs: runs-on: ubuntu-latest - container: px4io/px4-dev-base-focal:2020-11-18 + container: px4io/px4-dev-base-focal:2021-02-04 steps: - uses: actions/checkout@v1 with: @@ -99,7 +99,7 @@ jobs: ROS2_bridge: runs-on: ubuntu-latest - container: px4io/px4-dev-base-focal:2020-11-18 + container: px4io/px4-dev-base-focal:2021-02-04 steps: - uses: actions/checkout@v1 with: diff --git a/.github/workflows/sitl_tests.yml b/.github/workflows/sitl_tests.yml index f8739146c6..328c20b06a 100644 --- a/.github/workflows/sitl_tests.yml +++ b/.github/workflows/sitl_tests.yml @@ -20,7 +20,7 @@ jobs: - {latitude: "29.660316", longitude: "-82.316658", altitude: "30", build_type: "RelWithDebInfo", model: "tailsitter" } # Florida - {latitude: "47.397742", longitude: "8.545594", altitude: "488", build_type: "Coverage", model: "standard_vtol" } # Zurich container: - image: px4io/px4-dev-simulation-focal:2020-11-18 + image: px4io/px4-dev-simulation-focal:2021-02-04 options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined steps: - uses: actions/checkout@v1 diff --git a/.gitmodules b/.gitmodules index ea0f86aabb..8338c4fa2d 100644 --- a/.gitmodules +++ b/.gitmodules @@ -30,12 +30,6 @@ path = src/drivers/gps/devices url = https://github.com/PX4/PX4-GPSDrivers.git branch = master -[submodule "src/drivers/uavcan_v1/libcanard"] - path = src/drivers/uavcan_v1/libcanard - url = https://github.com/UAVCAN/libcanard.git -[submodule "src/drivers/uavcan_v1/public_regulated_data_types"] - path = src/drivers/uavcan_v1/public_regulated_data_types - url = https://github.com/UAVCAN/public_regulated_data_types.git [submodule "src/modules/micrortps_bridge/micro-CDR"] path = src/modules/micrortps_bridge/micro-CDR url = https://github.com/PX4/Micro-CDR.git @@ -60,3 +54,15 @@ [submodule "src/examples/gyro_fft/CMSIS_5"] path = src/examples/gyro_fft/CMSIS_5 url = https://github.com/ARM-software/CMSIS_5.git +[submodule "src/drivers/uavcan_v1/libcanard"] + path = src/drivers/uavcan_v1/libcanard + url = https://github.com/UAVCAN/libcanard.git +[submodule "src/drivers/uavcan_v1/public_regulated_data_types"] + path = src/drivers/uavcan_v1/public_regulated_data_types + url = https://github.com/UAVCAN/public_regulated_data_types.git +[submodule "src/drivers/uavcannode_gps_demo/public_regulated_data_types"] + path = src/drivers/uavcannode_gps_demo/public_regulated_data_types + url = https://github.com/UAVCAN/public_regulated_data_types +[submodule "src/drivers/uavcannode_gps_demo/libcanard"] + path = src/drivers/uavcannode_gps_demo/libcanard + url = https://github.com/UAVCAN/libcanard diff --git a/Jenkinsfile b/Jenkinsfile index 20b8b6b51d..8b1c5525e8 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -15,7 +15,7 @@ pipeline { // stage('Catkin build on ROS workspace') { // agent { // docker { - // image 'px4io/px4-dev-ros-melodic:2020-11-18' + // image 'px4io/px4-dev-ros-melodic:2021-02-04' // args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw -e HOME=$WORKSPACE' // } // } @@ -56,7 +56,7 @@ pipeline { stage('Colcon build on ROS2 workspace') { agent { docker { - image 'px4io/px4-dev-ros2-foxy:2020-11-18' + image 'px4io/px4-dev-ros2-foxy:2021-02-04' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw -e HOME=$WORKSPACE' } } @@ -87,7 +87,7 @@ pipeline { stage('Airframe') { agent { - docker { image 'px4io/px4-dev-base-focal:2020-11-18' } + docker { image 'px4io/px4-dev-base-focal:2021-02-04' } } steps { sh 'make distclean' @@ -106,7 +106,7 @@ pipeline { stage('Parameter') { agent { - docker { image 'px4io/px4-dev-base-focal:2020-11-18' } + docker { image 'px4io/px4-dev-base-focal:2021-02-04' } } steps { sh 'make distclean' @@ -125,7 +125,7 @@ pipeline { stage('Module') { agent { - docker { image 'px4io/px4-dev-base-focal:2020-11-18' } + docker { image 'px4io/px4-dev-base-focal:2021-02-04' } } steps { sh 'make distclean' @@ -145,7 +145,7 @@ pipeline { stage('uORB graphs') { agent { docker { - image 'px4io/px4-dev-nuttx-focal:2020-11-18' + image 'px4io/px4-dev-nuttx-focal:2021-02-04' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } @@ -174,7 +174,7 @@ pipeline { stage('Userguide') { agent { - docker { image 'px4io/px4-dev-base-focal:2020-11-18' } + docker { image 'px4io/px4-dev-base-focal:2021-02-04' } } steps { sh('export') @@ -204,7 +204,7 @@ pipeline { stage('QGroundControl') { agent { - docker { image 'px4io/px4-dev-base-focal:2020-11-18' } + docker { image 'px4io/px4-dev-base-focal:2021-02-04' } } steps { sh('export') @@ -232,7 +232,7 @@ pipeline { stage('microRTPS agent') { agent { - docker { image 'px4io/px4-dev-base-focal:2020-11-18' } + docker { image 'px4io/px4-dev-base-focal:2021-02-04' } } steps { sh('export') @@ -262,7 +262,7 @@ pipeline { stage('PX4 ROS msgs') { agent { - docker { image 'px4io/px4-dev-base-focal:2020-11-18' } + docker { image 'px4io/px4-dev-base-focal:2021-02-04' } } steps { sh('export') @@ -291,7 +291,7 @@ pipeline { stage('PX4 ROS2 bridge') { agent { - docker { image 'px4io/px4-dev-base-focal:2020-11-18' } + docker { image 'px4io/px4-dev-base-focal:2021-02-04' } } steps { sh('export') @@ -334,7 +334,7 @@ pipeline { stage('S3') { agent { - docker { image 'px4io/px4-dev-base-focal:2020-11-18' } + docker { image 'px4io/px4-dev-base-focal:2021-02-04' } } steps { sh('export') diff --git a/Tools/docker_run.sh b/Tools/docker_run.sh index b6e245d920..ea6dc52aeb 100755 --- a/Tools/docker_run.sh +++ b/Tools/docker_run.sh @@ -4,10 +4,10 @@ if [ -z ${PX4_DOCKER_REPO+x} ]; then echo "guessing PX4_DOCKER_REPO based on input"; if [[ $@ =~ .*px4_fmu.* ]]; then # nuttx-px4fmu-v{1,2,3,4,5} - PX4_DOCKER_REPO="px4io/px4-dev-nuttx-focal:2020-11-18" + PX4_DOCKER_REPO="px4io/px4-dev-nuttx-focal:2021-02-04" elif [[ $@ =~ .*ocpoc.* ]] || [[ $@ =~ .*navio2.* ]] || [[ $@ =~ .*raspberry.* ]] || [[ $@ =~ .*beaglebone.* ]] || [[ $@ =~ .*pilotpi.default ]]; then # aerotenna_ocpoc_default, beaglebone_blue_default, emlid_navio2_default, px4_raspberrypi_default, scumaker_pilotpi_default - PX4_DOCKER_REPO="px4io/px4-dev-armhf:2020-11-18" + PX4_DOCKER_REPO="px4io/px4-dev-armhf:2021-02-04" elif [[ $@ =~ .*pilotpi.arm64 ]]; then # scumaker_pilotpi_arm64 PX4_DOCKER_REPO="px4io/px4-dev-aarch64:latest" @@ -16,13 +16,13 @@ if [ -z ${PX4_DOCKER_REPO+x} ]; then PX4_DOCKER_REPO="lorenzmeier/px4-dev-snapdragon:2020-04-01" elif [[ $@ =~ .*ocpoc.* ]] || [[ $@ =~ .*navio2.* ]] || [[ $@ =~ .*raspberry.* ]] || [[ $@ =~ .*bebop.* ]]; then # aerotenna_ocpoc_default, posix_rpi_cross, posix_bebop_default - PX4_DOCKER_REPO="px4io/px4-dev-armhf:2020-11-18" + PX4_DOCKER_REPO="px4io/px4-dev-armhf:2021-02-04" elif [[ $@ =~ .*clang.* ]] || [[ $@ =~ .*scan-build.* ]]; then # clang tools - PX4_DOCKER_REPO="px4io/px4-dev-clang:2020-11-18" + PX4_DOCKER_REPO="px4io/px4-dev-clang:2021-02-04" elif [[ $@ =~ .*tests* ]]; then # run all tests with simulation - PX4_DOCKER_REPO="px4io/px4-dev-simulation-bionic:2020-11-18" + PX4_DOCKER_REPO="px4io/px4-dev-simulation-bionic:2021-02-04" fi else echo "PX4_DOCKER_REPO is set to '$PX4_DOCKER_REPO'"; diff --git a/boards/nxp/ucans32k146/default.cmake b/boards/nxp/ucans32k146/default.cmake index 32f50735ae..a432db26e4 100644 --- a/boards/nxp/ucans32k146/default.cmake +++ b/boards/nxp/ucans32k146/default.cmake @@ -28,6 +28,8 @@ px4_add_board( CONSTRAINED_MEMORY ROMFSROOT cannode UAVCAN_INTERFACES 2 + SERIAL_PORTS + GPS1:/dev/ttyS1 DRIVERS #adc/board_adc #barometer # all available barometer drivers @@ -45,7 +47,8 @@ px4_add_board( #safety_button #tone_alarm #uavcannode # TODO: CAN driver needed - uavcan_v1 + #uavcan_v1 + uavcannode_gps_demo MODULES #ekf2 #load_mon diff --git a/boards/px4/fmu-v4/default.cmake b/boards/px4/fmu-v4/default.cmake index 2e0d5531ae..3d657b650b 100644 --- a/boards/px4/fmu-v4/default.cmake +++ b/boards/px4/fmu-v4/default.cmake @@ -58,8 +58,7 @@ px4_add_board( telemetry # all available telemetry drivers test_ppm tone_alarm - #uavcan - uavcan_v1 + uavcan MODULES airspeed_selector attitude_estimator_q diff --git a/boards/px4/fmu-v4/nuttx-config/nsh/defconfig b/boards/px4/fmu-v4/nuttx-config/nsh/defconfig index c7f276eeb7..d135e4dd34 100644 --- a/boards/px4/fmu-v4/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v4/nuttx-config/nsh/defconfig @@ -40,7 +40,6 @@ CONFIG_BOARD_LOOPSPERMSEC=16717 CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y -CONFIG_CAN_EXTID=y CONFIG_CDCACM=y CONFIG_CDCACM_PRODUCTID=0x0012 CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v4.x" @@ -151,8 +150,6 @@ CONFIG_STM32_ADC1=y CONFIG_STM32_BBSRAM=y CONFIG_STM32_BBSRAM_FILES=5 CONFIG_STM32_BKPSRAM=y -CONFIG_STM32_CAN1=y -CONFIG_STM32_CAN1_BAUD=1000000 CONFIG_STM32_CCMDATARAM=y CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y CONFIG_STM32_DMA1=y diff --git a/boards/px4/fmu-v4/nuttx-config/uavcanv1/defconfig b/boards/px4/fmu-v4/nuttx-config/uavcanv1/defconfig new file mode 100644 index 0000000000..9695e004b2 --- /dev/null +++ b/boards/px4/fmu-v4/nuttx-config/uavcanv1/defconfig @@ -0,0 +1,239 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +# CONFIG_DISABLE_ENVIRON is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_MMCSD_HAVE_CARDDETECT is not set +# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set +# CONFIG_MMCSD_MMCSUPPORT is not set +# CONFIG_MMCSD_SPI is not set +# CONFIG_NSH_DISABLEBG is not set +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLE_DF is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_TIME is not set +# CONFIG_STM32_CCMEXCLUDE is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARCH_CHIP_STM32=y +CONFIG_ARCH_CHIP_STM32F427V=y +CONFIG_ARCH_INTERRUPTSTACK=512 +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARMV7M_LAZYFPU=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_CRASHDUMP=y +CONFIG_BOARD_LOOPSPERMSEC=16717 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_C99_BOOL8=y +CONFIG_CAN_EXTID=y +CONFIG_CDCACM=y +CONFIG_CDCACM_PRODUCTID=0x0012 +CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v4.x" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=2000 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_VENDORSTR="3D Robotics" +CONFIG_CLOCK_MONOTONIC=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DEV_FIFO_SIZE=0 +CONFIG_DEV_PIPE_MAXSIZE=1024 +CONFIG_DEV_PIPE_SIZE=70 +CONFIG_DISABLE_MQUEUE=y +CONFIG_FAT_DMAMEMORY=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_LFN_ALIAS_HASH=y +CONFIG_FDCLONE_STDIO=y +CONFIG_FS_BINFS=y +CONFIG_FS_CROMFS=y +CONFIG_FS_FAT=y +CONFIG_FS_FATTIME=y +CONFIG_FS_PROCFS=y +CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y +CONFIG_FS_PROCFS_REGISTER=y +CONFIG_FS_ROMFS=y +CONFIG_GRAN=y +CONFIG_GRAN_INTR=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_I2C=y +CONFIG_I2C_RESET=y +CONFIG_IDLETHREAD_STACKSIZE=750 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MMCSD_SDIO=y +CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y +CONFIG_MM_REGIONS=2 +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=y +CONFIG_MTD_RAMTRON=y +CONFIG_NAME_MAX=40 +CONFIG_NFILE_DESCRIPTORS=12 +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_ARCHROMFS=y +CONFIG_NSH_ARGCAT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CMDPARMS=y +CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_DISABLE_IFCONFIG=y +CONFIG_NSH_DISABLE_IFUPDOWN=y +CONFIG_NSH_DISABLE_MB=y +CONFIG_NSH_DISABLE_MH=y +CONFIG_NSH_DISABLE_TELNETD=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=15 +CONFIG_NSH_NESTDEPTH=8 +CONFIG_NSH_QUOTE=y +CONFIG_NSH_ROMFSETC=y +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_VARS=y +CONFIG_PIPES=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTHREAD_MUTEX_ROBUST=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_SETSPEED=y +CONFIG_RAMTRON_WRITEWAIT=y +CONFIG_RAM_SIZE=262144 +CONFIG_RAM_START=0x20000000 +CONFIG_RAW_BINARY=y +CONFIG_RTC_DATETIME=y +CONFIG_SCHED_ATEXIT=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SDIO_BLOCKSETUP=y +CONFIG_SEM_NNESTPRIO=8 +CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y +CONFIG_SERIAL_TERMIOS=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_SIG_SIGWORK=4 +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STM32_ADC1=y +CONFIG_STM32_BBSRAM=y +CONFIG_STM32_BBSRAM_FILES=5 +CONFIG_STM32_BKPSRAM=y +CONFIG_STM32_CAN1=y +CONFIG_STM32_CAN1_BAUD=1000000 +CONFIG_STM32_CCMDATARAM=y +CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y +CONFIG_STM32_DMA1=y +CONFIG_STM32_DMA2=y +CONFIG_STM32_FLASH_CONFIG_I=y +CONFIG_STM32_FLOWCONTROL_BROKEN=y +CONFIG_STM32_I2C1=y +CONFIG_STM32_I2CTIMEOMS=10 +CONFIG_STM32_I2CTIMEOTICKS=10 +CONFIG_STM32_JTAG_SW_ENABLE=y +CONFIG_STM32_OTGFS=y +CONFIG_STM32_PWR=y +CONFIG_STM32_RTC=y +CONFIG_STM32_RTC_HSECLOCK=y +CONFIG_STM32_RTC_MAGIC=0xfacefeee +CONFIG_STM32_RTC_MAGIC_REG=1 +CONFIG_STM32_RTC_MAGIC_TIME_SET=0xfacefeef +CONFIG_STM32_SAVE_CRASHDUMP=y +CONFIG_STM32_SDIO=y +CONFIG_STM32_SDIO_CARD=y +CONFIG_STM32_SERIALBRK_BSDCOMPAT=y +CONFIG_STM32_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32_SPI1=y +CONFIG_STM32_SPI1_DMA=y +CONFIG_STM32_SPI1_DMA_BUFFER=1024 +CONFIG_STM32_SPI2=y +CONFIG_STM32_SPI4=y +CONFIG_STM32_SPI_DMA=y +CONFIG_STM32_SPI_DMATHRESHOLD=8 +CONFIG_STM32_TIM10=y +CONFIG_STM32_TIM11=y +CONFIG_STM32_TIM8=y +CONFIG_STM32_TIM9=y +CONFIG_STM32_UART4=y +CONFIG_STM32_UART7=y +CONFIG_STM32_UART8=y +CONFIG_STM32_USART1=y +CONFIG_STM32_USART2=y +CONFIG_STM32_USART3=y +CONFIG_STM32_USART6=y +CONFIG_STM32_USART_BREAKS=y +CONFIG_STM32_USART_SINGLEWIRE=y +CONFIG_STM32_WWDG=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_UART4_BAUD=57600 +CONFIG_UART4_RXBUFSIZE=300 +CONFIG_UART4_RXDMA=y +CONFIG_UART4_TXBUFSIZE=300 +CONFIG_UART7_BAUD=57600 +CONFIG_UART7_RXBUFSIZE=300 +CONFIG_UART7_RXDMA=y +CONFIG_UART7_SERIAL_CONSOLE=y +CONFIG_UART7_TXBUFSIZE=300 +CONFIG_UART8_BAUD=57600 +CONFIG_UART8_RXBUFSIZE=300 +CONFIG_UART8_TXBUFSIZE=300 +CONFIG_USART1_RXBUFSIZE=600 +CONFIG_USART1_RXDMA=y +CONFIG_USART1_TXBUFSIZE=2500 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_IFLOWCONTROL=y +CONFIG_USART2_OFLOWCONTROL=y +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_RXDMA=y +CONFIG_USART2_TXBUFSIZE=1100 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_IFLOWCONTROL=y +CONFIG_USART3_OFLOWCONTROL=y +CONFIG_USART3_RXBUFSIZE=1200 +CONFIG_USART3_RXDMA=y +CONFIG_USART3_TXBUFSIZE=900 +CONFIG_USART6_BAUD=57600 +CONFIG_USART6_RXBUFSIZE=300 +CONFIG_USART6_RXDMA=y +CONFIG_USART6_TXBUFSIZE=300 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_USERMAIN_STACKSIZE=2944 +CONFIG_USER_ENTRYPOINT="nsh_main" diff --git a/boards/px4/fmu-v4/uavcanv1.cmake b/boards/px4/fmu-v4/uavcanv1.cmake new file mode 100644 index 0000000000..9dfdd33164 --- /dev/null +++ b/boards/px4/fmu-v4/uavcanv1.cmake @@ -0,0 +1,137 @@ + +px4_add_board( + PLATFORM nuttx + VENDOR px4 + MODEL fmu-v4 + LABEL uavcanv1 + TOOLCHAIN arm-none-eabi + ARCHITECTURE cortex-m4 + ROMFSROOT px4fmu_common + TESTING + UAVCAN_INTERFACES 1 + SERIAL_PORTS + GPS1:/dev/ttyS3 + TEL1:/dev/ttyS1 + TEL2:/dev/ttyS2 + WIFI:/dev/ttyS0 + + DRIVERS + adc/board_adc + adc/ads1115 + barometer # all available barometer drivers + batt_smbus + camera_capture + camera_trigger + differential_pressure # all available differential pressure drivers + distance_sensor # all available distance sensor drivers + dshot + gps + heater + #imu # all available imu drivers + imu/adis16448 + imu/adis16477 + imu/adis16497 + imu/invensense/icm20602 + imu/invensense/icm20608g + imu/invensense/icm40609d + imu/invensense/mpu6500 + imu/invensense/mpu9250 + irlock + lights/blinkm + lights/rgbled + lights/rgbled_ncp5623c + magnetometer # all available magnetometer drivers + mkblctrl + optical_flow # all available optical flow drivers + #osd + pca9685 + pca9685_pwm_out + #protocol_splitter + pwm_input + pwm_out_sim + pwm_out + rc_input + roboclaw + safety_button + tap_esc + telemetry # all available telemetry drivers + test_ppm + tone_alarm + #uavcan + uavcan_v1 + MODULES + airspeed_selector + attitude_estimator_q + battery_status + camera_feedback + commander + dataman + ekf2 + esc_battery + events + flight_mode_manager + fw_att_control + fw_pos_control_l1 + land_detector + landing_target_estimator + load_mon + local_position_estimator + logger + mavlink + mc_att_control + mc_hover_thrust_estimator + mc_pos_control + mc_rate_control + #micrortps_bridge + navigator + rc_update + rover_pos_control + sensors + sih + temperature_compensation + uuv_att_control + vmount + vtol_att_control + SYSTEMCMDS + bl_update + #dmesg + dumpfile + esc_calib + gpio + hardfault_log + i2cdetect + led_control + mft + mixer + motor_ramp + motor_test + mtd + nshterm + param + perf + pwm + reboot + reflect + sd_bench + system_time + tests # tests and test runner + top + topic_listener + tune_control + usb_connected + ver + work_queue + EXAMPLES + fake_gyro + fake_magnetometer + fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control + gyro_fft + hello + hwtest # Hardware test + #matlab_csv_serial + px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html + px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html + rover_steering_control # Rover example app + uuv_example_app + work_item + ) diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake index e860c3f90b..8c9f8519be 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/boards/px4/fmu-v5/default.cmake @@ -60,8 +60,7 @@ px4_add_board( telemetry # all available telemetry drivers test_ppm tone_alarm - #uavcan # legacy v0 - uavcan_v1 + uavcan MODULES airspeed_selector attitude_estimator_q diff --git a/boards/px4/fmu-v5/nuttx-config/nsh/defconfig b/boards/px4/fmu-v5/nuttx-config/nsh/defconfig index 9670132e73..202dcbbff1 100644 --- a/boards/px4/fmu-v5/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v5/nuttx-config/nsh/defconfig @@ -44,8 +44,6 @@ CONFIG_BOARD_LOOPSPERMSEC=22114 CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y -CONFIG_CAN=y -CONFIG_CAN_EXTID=y CONFIG_CDCACM=y CONFIG_CDCACM_PRODUCTID=0x0032 CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v5.x" @@ -158,10 +156,6 @@ CONFIG_STM32F7_ADC1=y CONFIG_STM32F7_BBSRAM=y CONFIG_STM32F7_BBSRAM_FILES=5 CONFIG_STM32F7_BKPSRAM=y -CONFIG_STM32F7_CAN1=y -CONFIG_STM32F7_CAN1_BAUD=1000000 -CONFIG_STM32F7_CAN_TSEG1=7 -CONFIG_STM32F7_CAN_TSEG2=1 CONFIG_STM32F7_DMA1=y CONFIG_STM32F7_DMA2=y CONFIG_STM32F7_DMACAPABLE=y diff --git a/boards/px4/fmu-v5/nuttx-config/uavcanv1/defconfig b/boards/px4/fmu-v5/nuttx-config/uavcanv1/defconfig new file mode 100644 index 0000000000..a16fa32699 --- /dev/null +++ b/boards/px4/fmu-v5/nuttx-config/uavcanv1/defconfig @@ -0,0 +1,250 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +# CONFIG_DISABLE_ENVIRON is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_FS_PROCFS_EXCLUDE_ENVIRON is not set +# CONFIG_MMCSD_HAVE_CARDDETECT is not set +# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set +# CONFIG_MMCSD_MMCSUPPORT is not set +# CONFIG_MMCSD_SPI is not set +# CONFIG_NSH_DISABLEBG is not set +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLE_DF is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_TIME is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32f7" +CONFIG_ARCH_CHIP_STM32F765II=y +CONFIG_ARCH_CHIP_STM32F7=y +CONFIG_ARCH_INTERRUPTSTACK=512 +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARMV7M_BASEPRI_WAR=y +CONFIG_ARMV7M_DCACHE=y +CONFIG_ARMV7M_DTCM=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_LAZYFPU=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_CRASHDUMP=y +CONFIG_BOARD_LOOPSPERMSEC=22114 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_C99_BOOL8=y +CONFIG_CAN=y +CONFIG_CAN_EXTID=y +CONFIG_CDCACM=y +CONFIG_CDCACM_PRODUCTID=0x0032 +CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v5.x" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_VENDORSTR="3D Robotics" +CONFIG_CLOCK_MONOTONIC=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DEV_FIFO_SIZE=0 +CONFIG_DEV_PIPE_MAXSIZE=1024 +CONFIG_DEV_PIPE_SIZE=70 +CONFIG_DISABLE_MQUEUE=y +CONFIG_FAT_DMAMEMORY=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_LFN_ALIAS_HASH=y +CONFIG_FDCLONE_STDIO=y +CONFIG_FS_BINFS=y +CONFIG_FS_CROMFS=y +CONFIG_FS_FAT=y +CONFIG_FS_FATTIME=y +CONFIG_FS_PROCFS=y +CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y +CONFIG_FS_PROCFS_REGISTER=y +CONFIG_FS_ROMFS=y +CONFIG_GRAN=y +CONFIG_GRAN_INTR=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_I2C=y +CONFIG_I2C_RESET=y +CONFIG_IDLETHREAD_STACKSIZE=750 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_MAX_TASKS=64 +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MMCSD_SDIO=y +CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y +CONFIG_MM_REGIONS=3 +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=y +CONFIG_MTD_RAMTRON=y +CONFIG_NAME_MAX=40 +CONFIG_NFILE_DESCRIPTORS=12 +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_ARCHROMFS=y +CONFIG_NSH_ARGCAT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CMDPARMS=y +CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_DISABLE_IFCONFIG=y +CONFIG_NSH_DISABLE_IFUPDOWN=y +CONFIG_NSH_DISABLE_MB=y +CONFIG_NSH_DISABLE_MH=y +CONFIG_NSH_DISABLE_TELNETD=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=15 +CONFIG_NSH_NESTDEPTH=8 +CONFIG_NSH_QUOTE=y +CONFIG_NSH_ROMFSETC=y +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_VARS=y +CONFIG_PIPES=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTHREAD_MUTEX_ROBUST=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_SETSPEED=y +CONFIG_RAMTRON_WRITEWAIT=y +CONFIG_RAM_SIZE=245760 +CONFIG_RAM_START=0x20010000 +CONFIG_RAW_BINARY=y +CONFIG_RTC_DATETIME=y +CONFIG_SCHED_ATEXIT=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SDMMC1_SDIO_MODE=y +CONFIG_SDMMC1_SDIO_PULLUP=y +CONFIG_SEM_NNESTPRIO=8 +CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y +CONFIG_SERIAL_TERMIOS=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_SIG_SIGWORK=4 +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=256 +CONFIG_STM32F7_ADC1=y +CONFIG_STM32F7_BBSRAM=y +CONFIG_STM32F7_BBSRAM_FILES=5 +CONFIG_STM32F7_BKPSRAM=y +CONFIG_STM32F7_CAN1=y +CONFIG_STM32F7_CAN1_BAUD=1000000 +CONFIG_STM32F7_CAN_TSEG1=7 +CONFIG_STM32F7_CAN_TSEG2=1 +CONFIG_STM32F7_DMA1=y +CONFIG_STM32F7_DMA2=y +CONFIG_STM32F7_DMACAPABLE=y +CONFIG_STM32F7_FLOWCONTROL_BROKEN=y +CONFIG_STM32F7_I2C1=y +CONFIG_STM32F7_I2C2=y +CONFIG_STM32F7_I2C3=y +CONFIG_STM32F7_I2C4=y +CONFIG_STM32F7_I2C_DYNTIMEO=y +CONFIG_STM32F7_I2C_DYNTIMEO_STARTSTOP=10 +CONFIG_STM32F7_OTGFS=y +CONFIG_STM32F7_PROGMEM=y +CONFIG_STM32F7_PWR=y +CONFIG_STM32F7_RTC=y +CONFIG_STM32F7_RTC_HSECLOCK=y +CONFIG_STM32F7_RTC_MAGIC_REG=1 +CONFIG_STM32F7_SAVE_CRASHDUMP=y +CONFIG_STM32F7_SDMMC1=y +CONFIG_STM32F7_SDMMC_DMA=y +CONFIG_STM32F7_SERIALBRK_BSDCOMPAT=y +CONFIG_STM32F7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32F7_SPI1=y +CONFIG_STM32F7_SPI1_DMA=y +CONFIG_STM32F7_SPI1_DMA_BUFFER=1024 +CONFIG_STM32F7_SPI2=y +CONFIG_STM32F7_SPI4=y +CONFIG_STM32F7_SPI5=y +CONFIG_STM32F7_SPI6=y +CONFIG_STM32F7_SPI_DMA=y +CONFIG_STM32F7_SPI_DMATHRESHOLD=8 +CONFIG_STM32F7_TIM10=y +CONFIG_STM32F7_TIM11=y +CONFIG_STM32F7_UART4=y +CONFIG_STM32F7_UART7=y +CONFIG_STM32F7_UART8=y +CONFIG_STM32F7_USART1=y +CONFIG_STM32F7_USART2=y +CONFIG_STM32F7_USART3=y +CONFIG_STM32F7_USART6=y +CONFIG_STM32F7_USART_BREAKS=y +CONFIG_STM32F7_USART_INVERT=y +CONFIG_STM32F7_USART_SINGLEWIRE=y +CONFIG_STM32F7_USART_SWAP=y +CONFIG_STM32F7_WWDG=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_UART4_BAUD=57600 +CONFIG_UART4_RXBUFSIZE=600 +CONFIG_UART4_RXDMA=y +CONFIG_UART4_TXBUFSIZE=1500 +CONFIG_UART7_BAUD=57600 +CONFIG_UART7_RXBUFSIZE=600 +CONFIG_UART7_SERIAL_CONSOLE=y +CONFIG_UART7_TXBUFSIZE=1500 +CONFIG_UART8_BAUD=57600 +CONFIG_UART8_RXBUFSIZE=600 +CONFIG_UART8_RXDMA=y +CONFIG_UART8_TXBUFSIZE=1500 +CONFIG_USART1_BAUD=57600 +CONFIG_USART1_RXBUFSIZE=600 +CONFIG_USART1_TXBUFSIZE=1500 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_IFLOWCONTROL=y +CONFIG_USART2_OFLOWCONTROL=y +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_RXDMA=y +CONFIG_USART2_TXBUFSIZE=1500 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_IFLOWCONTROL=y +CONFIG_USART3_OFLOWCONTROL=y +CONFIG_USART3_RXBUFSIZE=600 +CONFIG_USART3_RXDMA=y +CONFIG_USART3_TXBUFSIZE=3000 +CONFIG_USART3_TXDMA=y +CONFIG_USART6_BAUD=57600 +CONFIG_USART6_RXBUFSIZE=600 +CONFIG_USART6_RXDMA=y +CONFIG_USART6_TXBUFSIZE=1500 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_USERMAIN_STACKSIZE=2944 +CONFIG_USER_ENTRYPOINT="nsh_main" diff --git a/boards/px4/fmu-v5/uavcanv1.cmake b/boards/px4/fmu-v5/uavcanv1.cmake new file mode 100644 index 0000000000..f209bfd0b3 --- /dev/null +++ b/boards/px4/fmu-v5/uavcanv1.cmake @@ -0,0 +1,140 @@ + +px4_add_board( + PLATFORM nuttx + VENDOR px4 + MODEL fmu-v5 + LABEL uavcanv1 + TOOLCHAIN arm-none-eabi + ARCHITECTURE cortex-m7 + ROMFSROOT px4fmu_common + IO px4_io-v2_default + TESTING + UAVCAN_INTERFACES 2 + SERIAL_PORTS + GPS1:/dev/ttyS0 + TEL1:/dev/ttyS1 + TEL2:/dev/ttyS2 + TEL4:/dev/ttyS3 + DRIVERS + adc/board_adc + adc/ads1115 + barometer # all available barometer drivers + batt_smbus + camera_capture + camera_trigger + differential_pressure # all available differential pressure drivers + distance_sensor # all available distance sensor drivers + dshot + gps + #heater + #imu # all available imu drivers + imu/adis16448 + imu/adis16477 + imu/adis16497 + imu/bosch/bmi055 + imu/invensense/icm20602 + imu/invensense/icm20689 + #imu/mpu6000 # legacy icm20602/icm20689 driver + irlock + lights/blinkm + lights/rgbled + lights/rgbled_ncp5623c + lights/rgbled_pwm + magnetometer # all available magnetometer drivers + mkblctrl + optical_flow # all available optical flow drivers + #osd + pca9685 + pca9685_pwm_out + power_monitor/ina226 + #protocol_splitter + pwm_input + pwm_out_sim + pwm_out + px4io + rc_input + roboclaw + rpm + safety_button + tap_esc + telemetry # all available telemetry drivers + test_ppm + tone_alarm + #uavcan # legacy v0 + uavcan_v1 + MODULES + airspeed_selector + attitude_estimator_q + battery_status + camera_feedback + commander + dataman + ekf2 + esc_battery + events + flight_mode_manager + fw_att_control + fw_pos_control_l1 + land_detector + landing_target_estimator + load_mon + local_position_estimator + logger + mavlink + mc_att_control + mc_hover_thrust_estimator + mc_pos_control + mc_rate_control + #micrortps_bridge + navigator + rc_update + rover_pos_control + sensors + sih + temperature_compensation + uuv_att_control + vmount + vtol_att_control + SYSTEMCMDS + bl_update + dmesg + dumpfile + esc_calib + gpio + hardfault_log + i2cdetect + led_control + mft + mixer + motor_ramp + motor_test + mtd + nshterm + param + perf + pwm + reboot + reflect + sd_bench + system_time + tests # tests and test runner + top + topic_listener + tune_control + usb_connected + ver + work_queue + EXAMPLES + fake_gyro + fake_magnetometer + fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control + gyro_fft + hello + hwtest # Hardware test + #matlab_csv_serial + px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html + px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html + rover_steering_control # Rover example app + uuv_example_app + work_item + ) diff --git a/src/drivers/uavcan_v1/.Uavcan.cpp.kate-swp b/src/drivers/uavcan_v1/.Uavcan.cpp.kate-swp deleted file mode 100644 index a383caf56a50ca80a357ae13bb1d482a32acf76a..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 65 zcmZQzU=Z?7EJ;-eE>A2_aLdd|RWQ;sU|?Vn5py=Y&Qe{jXyj-5WV>!a;r_Q*rGq^g P7#PigvLL{s00gc8DM}Bf diff --git a/src/drivers/uavcan_v1/CMakeLists.txt b/src/drivers/uavcan_v1/CMakeLists.txt index 9e1f3b74a5..4b52d74358 100644 --- a/src/drivers/uavcan_v1/CMakeLists.txt +++ b/src/drivers/uavcan_v1/CMakeLists.txt @@ -45,10 +45,6 @@ else() message(FATAL_ERROR "UAVCAN Nunavut nnvg not found") endif() -add_definitions( - -DCANARD_DSDL_CONFIG_LITTLE_ENDIAN=1 -) - set(SRCS) if(${PX4_PLATFORM} MATCHES "nuttx") if(CONFIG_NET_CAN) @@ -80,8 +76,6 @@ px4_add_module( ${SRCS} o1heap/o1heap.c o1heap/o1heap.h - ${LIBCANARD_DIR}/libcanard/canard_dsdl.c - ${LIBCANARD_DIR}/libcanard/canard_dsdl.h ${LIBCANARD_DIR}/libcanard/canard.c ${LIBCANARD_DIR}/libcanard/canard.h DEPENDS diff --git a/src/drivers/uavcan_v1/CanardSocketCAN.cpp b/src/drivers/uavcan_v1/CanardSocketCAN.cpp index 1d47e1f3cf..c26ad2a9ce 100644 --- a/src/drivers/uavcan_v1/CanardSocketCAN.cpp +++ b/src/drivers/uavcan_v1/CanardSocketCAN.cpp @@ -46,9 +46,9 @@ int CanardSocketCAN::init() struct sockaddr_can addr; struct ifreq ifr; - //FIXME HOTFIX to make this code compile - bool can_fd = 0; - + //FIXME HOTFIX to make this code compile + bool can_fd = 0; + _can_fd = can_fd; /* open socket */ diff --git a/src/drivers/uavcan_v1/Uavcan.cpp b/src/drivers/uavcan_v1/Uavcan.cpp index 164331d704..d097fd49c7 100644 --- a/src/drivers/uavcan_v1/Uavcan.cpp +++ b/src/drivers/uavcan_v1/Uavcan.cpp @@ -155,37 +155,45 @@ void UavcanNode::Run() uavcan_node_Heartbeat_1_0_EXTENT_BYTES_, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, &_heartbeat_subscription); - - // Subscribe to messages uavcan.node.NodeIDAllocationData_1_0 for PNP V1 - canardRxSubscribe(&_canard_instance, + + // Subscribe to messages uavcan.node.NodeIDAllocationData_1_0 for PNP V1 + canardRxSubscribe(&_canard_instance, CanardTransferKindMessage, uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_, uavcan_pnp_NodeIDAllocationData_1_0_EXTENT_BYTES_, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, &_pnp_v1_subscription); - - canardRxSubscribe(&_canard_instance, - CanardTransferKindResponse, - uavcan_register_Access_1_0_FIXED_PORT_ID_, - uavcan_register_Access_Response_1_0_EXTENT_BYTES_, - CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, - &_register_access_subscription); - canardRxSubscribe(&_canard_instance, - CanardTransferKindResponse, - uavcan_register_List_1_0_FIXED_PORT_ID_, - uavcan_register_List_Response_1_0_EXTENT_BYTES_, - CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, - &_register_list_subscription); - + canardRxSubscribe(&_canard_instance, + CanardTransferKindResponse, + uavcan_register_Access_1_0_FIXED_PORT_ID_, + uavcan_register_Access_Response_1_0_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_register_access_subscription); + + canardRxSubscribe(&_canard_instance, + CanardTransferKindResponse, + uavcan_register_List_1_0_FIXED_PORT_ID_, + uavcan_register_List_Response_1_0_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_register_list_subscription); - canardRxSubscribe(&_canard_instance, - CanardTransferKindMessage, - test_port_id, - reg_drone_srv_battery_Status_0_1_EXTENT_BYTES_, - CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, - &_drone_srv_battery_subscription); + + canardRxSubscribe(&_canard_instance, + CanardTransferKindMessage, + bms_port_id, + reg_drone_srv_battery_Status_0_1_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_drone_srv_battery_subscription); + + canardRxSubscribe(&_canard_instance, //Temporory GPS message DSDL not defined yet + CanardTransferKindMessage, + gps_port_id, + sizeof(struct sensor_gps_s), + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_drone_srv_gps_subscription); + _initialized = true; } @@ -213,39 +221,39 @@ void UavcanNode::Run() - if (hrt_elapsed_time(&_uavcan_pnp_nodeidallocation_last) >= 1_s && - _node_register_setup != CANARD_NODE_ID_UNSET && - _node_register_request_index == _node_register_last_received_index+1){ - - PX4_INFO("NodeID %i request register %i", _node_register_setup, _node_register_request_index); - - uavcan_register_List_Request_1_0 msg; - msg.index = _node_register_request_index; - - uint8_t request_payload_buffer[uavcan_register_List_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; - - CanardTransfer request = { - .timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited. - .priority = CanardPriorityNominal, - .transfer_kind = CanardTransferKindRequest, - .port_id = uavcan_register_List_1_0_FIXED_PORT_ID_, // This is the subject-ID. - .remote_node_id = _node_register_setup, - .transfer_id = _uavcan_register_list_request_transfer_id, - .payload_size = uavcan_register_List_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, - .payload = &request_payload_buffer, - }; + if (hrt_elapsed_time(&_uavcan_pnp_nodeidallocation_last) >= 1_s && + _node_register_setup != CANARD_NODE_ID_UNSET && + _node_register_request_index == _node_register_last_received_index + 1) { - int32_t result = uavcan_register_List_Request_1_0_serialize_(&msg, request_payload_buffer, &request.payload_size); + PX4_INFO("NodeID %i request register %i", _node_register_setup, _node_register_request_index); + + uavcan_register_List_Request_1_0 msg; + msg.index = _node_register_request_index; + + uint8_t request_payload_buffer[uavcan_register_List_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; + + CanardTransfer request = { + .timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited. + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindRequest, + .port_id = uavcan_register_List_1_0_FIXED_PORT_ID_, // This is the subject-ID. + .remote_node_id = _node_register_setup, + .transfer_id = _uavcan_register_list_request_transfer_id, + .payload_size = uavcan_register_List_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, + .payload = &request_payload_buffer, + }; + + int32_t result = uavcan_register_List_Request_1_0_serialize_(&msg, request_payload_buffer, &request.payload_size); + + if (result == 0) { + // set the data ready in the buffer and chop if needed + ++_uavcan_register_list_request_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. + result = canardTxPush(&_canard_instance, &request); + + ++_node_register_request_index; + } + } - if(result == 0){ - // set the data ready in the buffer and chop if needed - ++_uavcan_register_list_request_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. - result = canardTxPush(&_canard_instance, &request); - - ++_node_register_request_index; - } - } - // send uavcan::node::Heartbeat_1_0 @ 1 Hz if (hrt_elapsed_time(&_uavcan_node_heartbeat_last) >= 1_s) { @@ -266,7 +274,7 @@ void UavcanNode::Run() .payload = &_uavcan_node_heartbeat_buffer, }; - uavcan_node_Heartbeat_1_0_serialize_(&heartbeat, _uavcan_node_heartbeat_buffer, &transfer.payload_size); + uavcan_node_Heartbeat_1_0_serialize_(&heartbeat, _uavcan_node_heartbeat_buffer, &transfer.payload_size); int32_t result = canardTxPush(&_canard_instance, &transfer); @@ -330,109 +338,152 @@ void UavcanNode::Run() // A transfer has been received, process it. PX4_INFO("received Port ID: %d", receive.port_id); - if (receive.port_id == uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_) { - uavcan_pnp_NodeIDAllocationData_1_0 msg; - - size_t pnp_in_size_bits = receive.payload_size; - uavcan_pnp_NodeIDAllocationData_1_0_deserialize_(&msg, (const uint8_t*)receive.payload, &pnp_in_size_bits); - - //TODO internal database with unique id to node ip mappings now we give an hardcoded ID back - - msg.allocated_node_id.count = 1; - msg.allocated_node_id.elements[0].value = 15; // HACK hardcoded ID - - _uavcan_pnp_nodeidallocation_last = hrt_absolute_time(); - _node_register_request_index = 0; - _node_register_last_received_index = -1; - _node_register_setup = msg.allocated_node_id.elements[0].value; // This nodeID has to be configured - - PX4_INFO("Received NodeID allocation request assigning %i", msg.allocated_node_id.elements[0].value); - - uint8_t node_id_alloc_payload_buffer[uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; - - CanardTransfer transfer = { - .timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited. - .priority = CanardPriorityNominal, - .transfer_kind = CanardTransferKindMessage, - .port_id = uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_, // This is the subject-ID. - .remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET. - .transfer_id = _uavcan_pnp_nodeidallocation_v1_transfer_id, - .payload_size = uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, - .payload = &node_id_alloc_payload_buffer, - }; + if (receive.port_id == uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_) { + uavcan_pnp_NodeIDAllocationData_1_0 msg; - result = uavcan_pnp_NodeIDAllocationData_1_0_serialize_(&msg, node_id_alloc_payload_buffer, &transfer.payload_size); + size_t pnp_in_size_bits = receive.payload_size; + uavcan_pnp_NodeIDAllocationData_1_0_deserialize_(&msg, (const uint8_t *)receive.payload, &pnp_in_size_bits); + + //TODO internal database with unique id to node ip mappings now we give an hardcoded ID back + + msg.allocated_node_id.count = 1; + msg.allocated_node_id.elements[0].value = 15; // HACK hardcoded ID + + _uavcan_pnp_nodeidallocation_last = hrt_absolute_time(); + _node_register_request_index = 0; + _node_register_last_received_index = -1; + _node_register_setup = msg.allocated_node_id.elements[0].value; // This nodeID has to be configured + + PX4_INFO("Received NodeID allocation request assigning %i", msg.allocated_node_id.elements[0].value); + + uint8_t node_id_alloc_payload_buffer[uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; + + CanardTransfer transfer = { + .timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited. + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindMessage, + .port_id = uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_, // This is the subject-ID. + .remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET. + .transfer_id = _uavcan_pnp_nodeidallocation_v1_transfer_id, + .payload_size = uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, + .payload = &node_id_alloc_payload_buffer, + }; + + result = uavcan_pnp_NodeIDAllocationData_1_0_serialize_(&msg, node_id_alloc_payload_buffer, &transfer.payload_size); + + if (result == 0) { + // set the data ready in the buffer and chop if needed + ++_uavcan_pnp_nodeidallocation_v1_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. + result = canardTxPush(&_canard_instance, &transfer); + } - if(result == 0) { - // set the data ready in the buffer and chop if needed - ++_uavcan_pnp_nodeidallocation_v1_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. - result = canardTxPush(&_canard_instance, &transfer); - } } if (receive.port_id == uavcan_register_List_1_0_FIXED_PORT_ID_) { - uavcan_register_List_Response_1_0 msg; - - size_t register_in_size_bits = receive.payload_size; - uavcan_register_List_Response_1_0_deserialize_(&msg, (const uint8_t*)receive.payload, ®ister_in_size_bits); - - - - - - if(strncmp((char*)msg.name.name.elements, "uavcan.pub.battery_status.id", msg.name.name.count) == 0) { //Battery status publisher - _node_register_setup = CANARD_NODE_ID_UNSET; - PX4_INFO("NodeID %i battery_status publisher set PortID to %i", receive.remote_node_id, test_port_id); - _node_register_last_received_index++; - - uavcan_register_Access_Request_1_0 request_msg; - memcpy(&request_msg.name, &msg.name, sizeof(uavcan_register_Name_1_0)); - - uavcan_register_Value_1_0_select_natural16_(&request_msg.value); - request_msg.value.natural16.value.count = 1; - request_msg.value.natural16.value.elements[0] = test_port_id; - - - uint8_t request_payload_buffer[uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; - - CanardTransfer transfer = { - .timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited. - .priority = CanardPriorityNominal, - .transfer_kind = CanardTransferKindRequest, - .port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_, // This is the subject-ID. - .remote_node_id = receive.remote_node_id, // Messages cannot be unicast, so use UNSET. - .transfer_id = _uavcan_register_access_request_transfer_id, - .payload_size = uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, - .payload = &request_payload_buffer, - }; - result = uavcan_register_Access_Request_1_0_serialize_(&request_msg, request_payload_buffer, &transfer.payload_size); + uavcan_register_List_Response_1_0 msg; - if(result == 0) { - // set the data ready in the buffer and chop if needed - ++_uavcan_register_access_request_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. - result = canardTxPush(&_canard_instance, &transfer); - } - } - - - } else if (receive.port_id == test_port_id) { + size_t register_in_size_bits = receive.payload_size; + uavcan_register_List_Response_1_0_deserialize_(&msg, (const uint8_t *)receive.payload, ®ister_in_size_bits); + + + if (strncmp((char *)msg.name.name.elements, "uavcan.pub.gnss_uorb.id", + msg.name.name.count) == 0) { //Demo GPS status publisher + _node_register_setup = CANARD_NODE_ID_UNSET; + PX4_INFO("NodeID %i GPS publisher set PortID to %i", receive.remote_node_id, gps_port_id); + _node_register_last_received_index++; + + uavcan_register_Access_Request_1_0 request_msg; + memcpy(&request_msg.name, &msg.name, sizeof(uavcan_register_Name_1_0)); + + uavcan_register_Value_1_0_select_natural16_(&request_msg.value); + request_msg.value.natural16.value.count = 1; + request_msg.value.natural16.value.elements[0] = gps_port_id; + + + uint8_t request_payload_buffer[uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; + + CanardTransfer transfer = { + .timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited. + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindRequest, + .port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_, // This is the subject-ID. + .remote_node_id = receive.remote_node_id, // Messages cannot be unicast, so use UNSET. + .transfer_id = _uavcan_register_access_request_transfer_id, + .payload_size = uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, + .payload = &request_payload_buffer, + }; + + result = uavcan_register_Access_Request_1_0_serialize_(&request_msg, request_payload_buffer, &transfer.payload_size); + + if (result == 0) { + // set the data ready in the buffer and chop if needed + ++_uavcan_register_access_request_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. + result = canardTxPush(&_canard_instance, &transfer); + } + } + + if (strncmp((char *)msg.name.name.elements, "uavcan.pub.battery_status.id", + msg.name.name.count) == 0) { //Battery status publisher + _node_register_setup = CANARD_NODE_ID_UNSET; + PX4_INFO("NodeID %i battery_status publisher set PortID to %i", receive.remote_node_id, bms_port_id); + _node_register_last_received_index++; + + uavcan_register_Access_Request_1_0 request_msg; + memcpy(&request_msg.name, &msg.name, sizeof(uavcan_register_Name_1_0)); + + uavcan_register_Value_1_0_select_natural16_(&request_msg.value); + request_msg.value.natural16.value.count = 1; + request_msg.value.natural16.value.elements[0] = bms_port_id; + + + uint8_t request_payload_buffer[uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; + + CanardTransfer transfer = { + .timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited. + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindRequest, + .port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_, // This is the subject-ID. + .remote_node_id = receive.remote_node_id, // Messages cannot be unicast, so use UNSET. + .transfer_id = _uavcan_register_access_request_transfer_id, + .payload_size = uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, + .payload = &request_payload_buffer, + }; + + result = uavcan_register_Access_Request_1_0_serialize_(&request_msg, request_payload_buffer, &transfer.payload_size); + + if (result == 0) { + // set the data ready in the buffer and chop if needed + ++_uavcan_register_access_request_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. + result = canardTxPush(&_canard_instance, &transfer); + } + } + + + } else if (receive.port_id == bms_port_id) { PX4_INFO("NodeID %i Battery Status msg", receive.remote_node_id); - //TODO deserialize - - /* + //TODO deserialize + /* - battery_status_s battery_status{}; - battery_status.id = bms_status.battery_id; - battery_status.voltage_v = bms_status.voltage; - //battery_status.remaining = bms_status.remaining_capacity; - battery_status.timestamp = hrt_absolute_time(); - _battery_status_pub.publish(battery_status);*/ + battery_status_s battery_status{}; + battery_status.id = bms_status.battery_id; + battery_status.voltage_v = bms_status.voltage; + //battery_status.remaining = bms_status.remaining_capacity; + battery_status.timestamp = hrt_absolute_time(); + _battery_status_pub.publish(battery_status);*/ + + } else if (receive.port_id == gps_port_id) { + PX4_INFO("NodeID %i GPS sensor msg", receive.remote_node_id); + + sensor_gps_s *gps_msg = (sensor_gps_s *)receive.payload; + + _sensor_gps_pub.publish(*gps_msg); } // Deallocate the dynamic memory afterwards. _canard_instance.memory_free(&_canard_instance, (void *)receive.payload); + } else { - PX4_INFO("RX canard %d\r\n", result); - } + //PX4_INFO("RX canard %d\r\n", result); + } } perf_end(_cycle_perf); diff --git a/src/drivers/uavcan_v1/Uavcan.hpp b/src/drivers/uavcan_v1/Uavcan.hpp index cb18578de9..f5132043ba 100644 --- a/src/drivers/uavcan_v1/Uavcan.hpp +++ b/src/drivers/uavcan_v1/Uavcan.hpp @@ -47,6 +47,7 @@ #include #include #include +#include #include "o1heap/o1heap.h" @@ -67,25 +68,26 @@ #include #define PNP1_PORT_ID uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_ -#define PNP1_PAYLOAD_SIZE uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_ +#define PNP1_PAYLOAD_SIZE uavcan_pnp_NodeIDAllocationData_1_0_EXTENT_BYTES_ #define PNP2_PORT_ID uavcan_pnp_NodeIDAllocationData_2_0_FIXED_PORT_ID_ -#define PNP2_PAYLOAD_SIZE uavcan_pnp_NodeIDAllocationData_2_0_SERIALIZATION_BUFFER_SIZE_BYTES_ +#define PNP2_PAYLOAD_SIZE uavcan_pnp_NodeIDAllocationData_2_0_EXTENT_BYTES_ #include "CanardInterface.hpp" class UavcanNode : public ModuleParams, public px4::ScheduledWorkItem { /* - * This memory is reserved for uavcan to use as over flow for message - * Coming from multiple sources that my not be considered at development - * time. - * - * The call to getNumFreeBlocks will tell how many blocks there are - * free -and multiply it times getBlockSize to get the number of bytes - * - */ + * This memory is allocated for the 01Heap allocator used by + * libcanard to store incoming/outcoming data + * Current size of 8192 bytes is arbitrary, should be optimized further + * when more nodes and messages are on the CAN bus + */ static constexpr unsigned HeapSize = 8192; + /* + * Base interval, has to be complemented with events from the CAN driver + * and uORB topics sending data, to decrease response time. + */ static constexpr unsigned ScheduleIntervalMs = 10; public: @@ -123,6 +125,7 @@ private: CanardRxSubscription _heartbeat_subscription; CanardRxSubscription _pnp_v1_subscription; CanardRxSubscription _drone_srv_battery_subscription; + CanardRxSubscription _drone_srv_gps_subscription; CanardRxSubscription _register_access_subscription; CanardRxSubscription _register_list_subscription; @@ -130,6 +133,7 @@ private: uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Publication _battery_status_pub{ORB_ID(battery_status)}; + uORB::Publication _sensor_gps_pub{ORB_ID(sensor_gps)}; perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")}; perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")}; @@ -138,20 +142,25 @@ private: uint8_t _uavcan_node_heartbeat_buffer[uavcan_node_Heartbeat_1_0_EXTENT_BYTES_]; hrt_abstime _uavcan_node_heartbeat_last{0}; CanardTransferID _uavcan_node_heartbeat_transfer_id{0}; - - const uint16_t test_port_id = 1234; - + + /* Temporary hardcoded port IDs used by the register interface + * for demo purposes untill we have nice interface (QGC or latter) + * to configure the nodes + */ + const uint16_t bms_port_id = 1234; + const uint16_t gps_port_id = 1235; + CanardTransferID _uavcan_pnp_nodeidallocation_v1_transfer_id{0}; - hrt_abstime _uavcan_pnp_nodeidallocation_last{0}; - + hrt_abstime _uavcan_pnp_nodeidallocation_last{0}; + CanardTransferID _uavcan_register_list_request_transfer_id{0}; CanardTransferID _uavcan_register_access_request_transfer_id{0}; - //Register interface NodeID TODO MVP right have to make a queue - uint8_t _node_register_setup = CANARD_NODE_ID_UNSET; - int32_t _node_register_request_index = 0; - int32_t _node_register_last_received_index = -1; - - // regulated::drone::sensor::BMSStatus_1_0 + //Register interface NodeID TODO MVP right have to make a queue + uint8_t _node_register_setup = CANARD_NODE_ID_UNSET; + int32_t _node_register_request_index = 0; + int32_t _node_register_last_received_index = -1; + + // regulated::drone::sensor::BMSStatus_1_0 uint8_t _regulated_drone_sensor_bmsstatus_buffer[reg_drone_srv_battery_Status_0_1_EXTENT_BYTES_]; hrt_abstime _regulated_drone_sensor_bmsstatus_last{0}; CanardTransferID _regulated_drone_sensor_bmsstatus_transfer_id{0}; diff --git a/src/drivers/uavcan_v1/public_regulated_data_types b/src/drivers/uavcan_v1/public_regulated_data_types index d874225ea4..81526eb6e2 160000 --- a/src/drivers/uavcan_v1/public_regulated_data_types +++ b/src/drivers/uavcan_v1/public_regulated_data_types @@ -1 +1 @@ -Subproject commit d874225ea4b5a79a4639bcfcb9096ccd943138a9 +Subproject commit 81526eb6e27095701736dfb6e74842fe07a59f4f diff --git a/src/drivers/uavcan_v1/templates/Header.j2 b/src/drivers/uavcan_v1/templates/Header.j2 deleted file mode 100644 index f1474317fa..0000000000 --- a/src/drivers/uavcan_v1/templates/Header.j2 +++ /dev/null @@ -1,32 +0,0 @@ -/* - * - * UAVCAN data structure definition. - * - * AUTOGENERATED, DO NOT EDIT. - * - * Source File: - * {{ T.source_file_path }} - * - * Template: - * {{ self._TemplateReference__context.name }} - * - * Generated at: {{ now_utc }} UTC - * Is deprecated: {{ T.deprecated and 'yes' or 'no' }} - * Fixed port ID: {{ T.fixed_port_id }} - * Full name: {{ T.full_name }} - * Version: {{ T.version.major }}.{{ T.version.minor }} - * - */ - -#ifndef {{T.full_name | c.macrofy}} -#define {{T.full_name | c.macrofy}} -{% for n in T | includes -%} -#include {{ n }} -{% endfor %} -#include -{{T.full_namespace | open_namespace}} - -{%- block object -%}{%- endblock -%} - -{{T.full_namespace | close_namespace}} -#endif // {{T.full_name | c.macrofy}} diff --git a/src/drivers/uavcan_v1/templates/ServiceType.j2 b/src/drivers/uavcan_v1/templates/ServiceType.j2 deleted file mode 100644 index 6f4d9a4f44..0000000000 --- a/src/drivers/uavcan_v1/templates/ServiceType.j2 +++ /dev/null @@ -1,10 +0,0 @@ -{% extends "Header.j2" %} -{%- block object -%} -{{ T | definition_begin }} -{ -{% set composite_type = T.request_type -%} -{% include '_composite_type.j2' %} -{% set composite_type = T.response_type -%} -{% include '_composite_type.j2' %} -}{{ T | definition_end }} -{% endblock -%} \ No newline at end of file diff --git a/src/drivers/uavcan_v1/templates/StructureType.j2 b/src/drivers/uavcan_v1/templates/StructureType.j2 deleted file mode 100644 index 87137b3291..0000000000 --- a/src/drivers/uavcan_v1/templates/StructureType.j2 +++ /dev/null @@ -1,5 +0,0 @@ -{% extends "Header.j2" %} -{%- block object -%} -{% set composite_type = T -%} -{% include '_composite_type.j2' %} -{% endblock -%} diff --git a/src/drivers/uavcan_v1/templates/UnionType.j2 b/src/drivers/uavcan_v1/templates/UnionType.j2 deleted file mode 100644 index d763941e58..0000000000 --- a/src/drivers/uavcan_v1/templates/UnionType.j2 +++ /dev/null @@ -1,5 +0,0 @@ -{% extends "Header.j2" %} -{%- block object -%} -{% set composite_type = T -%} -{% include '_composite_type.j2' %} -{% endblock -%} \ No newline at end of file diff --git a/src/drivers/uavcan_v1/templates/_composite_type.j2 b/src/drivers/uavcan_v1/templates/_composite_type.j2 deleted file mode 100644 index 07dec0846d..0000000000 --- a/src/drivers/uavcan_v1/templates/_composite_type.j2 +++ /dev/null @@ -1,115 +0,0 @@ -{{ composite_type | definition_begin }} -{ -{%- if T.fixed_port_id is not None %} - static constexpr CanardPortID PORT_ID = {{T.fixed_port_id}}; -{%- endif %} - -{%- set total_size = namespace(value=0) -%} -{%- for field in composite_type.fields -%} - {% set total_size.value = total_size.value + field.data_type.bit_length_set|max %} -{%- endfor %} - static constexpr size_t SIZE = {{total_size.value}}; - - static constexpr auto getDataTypeFullName() { return "{{T}}"; } - -{% for constant in composite_type.constants %} - static constexpr {{ constant.data_type | declaration }} {{ constant.name | id }} = {{ constant.value.native_value.numerator }} / {{ constant.value.native_value.denominator }}; -{%- endfor -%} - -{% if composite_type is UnionType %} -#error "TODO: UnionType -{% else %} - -{% for field in composite_type.fields -%} - {%- if field is not padding %} - {%- if field.data_type is FloatType %} - {%- if field.data_type.bit_length_set|max <= 32 %} - float {{ field | id }}{NAN}; - {%- else %} - double {{ field | id }}{NAN}; - {%- endif %} - {%- elif field.data_type is BooleanType %} - bool {{ field | id }}{false}; - {%- else %} - {{ field.data_type | declaration }} {{ field | id }}{}; - {%- endif -%} - {%- endif -%} -{%- endfor %} -{% endif %} - - void serializeToBuffer(uint8_t* const buffer, const size_t starting_bit = 0) - { -{%- set bit_offset = namespace(value=0) -%} -{%- for field in composite_type.fields -%} - {%- if field is not padding %} - {%- if field.data_type is SerializableType %} - {%- if field.data_type is IntegerType %} - canardDSDLSetUxx(buffer, starting_bit + {{ bit_offset.value }}, {{ field.name }}, {{ field.data_type.bit_length_set | max }}); - {%- elif field.data_type is BooleanType %} - canardDSDLSetBit(buffer, starting_bit + {{ field.data_type.bit_length_set | max }}, {{ field.name }}); - {%- elif field.data_type is FloatType %} - {%- if field.data_type.bit_length_set == 16 %} - canardDSDLSetF16(buffer, starting_bit + {{ field.data_type.bit_length_set | max }}, {{ field.name }}); - {%- elif field.data_type.bit_length_set == 32 %} - canardDSDLSetF32(buffer, starting_bit + {{ field.data_type.bit_length_set | max }}, {{ field.name }}); - {%- elif field.data_type.bit_length_set == 64 %} - canardDSDLSetF64(buffer, starting_bit + {{ field.data_type.bit_length_set | max }}, {{ field.name }}); - {%- endif %} - {%- else %} - {{ field.name }}.serializeToBuffer(buffer, starting_bit + {{ bit_offset.value }}); - {%- endif %} - {%- endif -%} - {%- endif -%} - {% set bit_offset.value = bit_offset.value + field.data_type.bit_length_set|max %} -{%- endfor %} - } - - static {{T | full_reference_name}} deserializeFromBuffer(const uint8_t* const buffer, const size_t buf_size, const size_t starting_bit = 0) - { - {{T | full_reference_name}} msg; -{% set bit_offset = namespace(value=0) %} -{%- for field in composite_type.fields -%} - {%- if field is not padding %} - {%- if field.data_type is SerializableType %} - {%- if field.data_type is UnsignedIntegerType %} - {%- if field.data_type.bit_length_set|max<= 8 %} - msg.{{ field.name }} = canardDSDLGetU8(buffer, buf_size, starting_bit + {{ bit_offset.value }}, {{ field.data_type.bit_length_set | max }}); - {%- elif field.data_type.bit_length_set|max <= 16 %} - msg.{{ field.name }} = canardDSDLGetU16(buffer, buf_size, starting_bit + {{ bit_offset.value }}, {{ field.data_type.bit_length_set | max }}); - {%- elif field.data_type.bit_length_set|max <= 32 %} - msg.{{ field.name }} = canardDSDLGetU32(buffer, buf_size, starting_bit + {{ bit_offset.value }}, {{ field.data_type.bit_length_set | max }}); - {%- elif field.data_type.bit_length_set|max <= 64 %} - msg.{{ field.name }} = canardDSDLGetU64(buffer, buf_size, starting_bit + {{ bit_offset.value }}, {{ field.data_type.bit_length_set | max }}); - {%- endif %} - {%- elif field.data_type is SignedIntegerType %} - {%- if field.data_type.bit_length_set|max<= 8 %} - msg.{{ field.name }} = canardDSDLGetI8(buffer, buf_size, starting_bit + {{ bit_offset.value }}, {{ field.data_type.bit_length_set | max }}); - {%- elif field.data_type.bit_length_set|max <= 16 %} - msg.{{ field.name }} = canardDSDLGetI16(buffer, buf_size, starting_bit + {{ bit_offset.value }}, {{ field.data_type.bit_length_set | max }}); - {%- elif field.data_type.bit_length_set|max <= 32 %} - msg.{{ field.name }} = canardDSDLGetI32(buffer, buf_size, starting_bit + {{ bit_offset.value }}, {{ field.data_type.bit_length_set | max }}); - {%- elif field.data_type.bit_length_set|max <= 64 %} - msg.{{ field.name }} = canardDSDLGetI64(buffer, buf_size, starting_bit + {{ bit_offset.value }}, {{ field.data_type.bit_length_set | max }}); - {%- endif %} - {%- elif field.data_type is BooleanType %} - msg.{{ field.name }} = canardDSDLGetBit(buffer, buf_size, starting_bit + {{ bit_offset.value }}); - {%- elif field.data_type is FloatType %} - {%- if field.data_type.bit_length_set == 16 %} - msg.{{ field.name }} = canardDSDLGetF16(buffer, buf_size, starting_bit + {{ bit_offset.value }}); - {%- elif field.data_type.bit_length_set == 32 %} - msg.{{ field.name }} = canardDSDLGetF32(buffer, buf_size, starting_bit + {{ bit_offset.value }}); - {%- elif field.data_type.bit_length_set == 64 %} - msg.{{ field.name }} = canardDSDLGetF64(buffer, buf_size, starting_bit + {{ bit_offset.value }}); - {%- endif %} - {%- else %} - msg.{{ field.name }} = {{ field.data_type | declaration }}::deserializeFromBuffer(buffer, buf_size, starting_bit + {{ bit_offset.value }}); - {%- endif %} - {%- endif -%} - {%- endif -%} - {% set bit_offset.value = bit_offset.value + field.data_type.bit_length_set|max %} -{%- endfor %} - - return msg; - } - -}{{ composite_type | definition_end }} diff --git a/src/drivers/uavcannode_gps_demo/CMakeLists.txt b/src/drivers/uavcannode_gps_demo/CMakeLists.txt new file mode 100644 index 0000000000..fd3ae1b56a --- /dev/null +++ b/src/drivers/uavcannode_gps_demo/CMakeLists.txt @@ -0,0 +1,82 @@ +############################################################################ +# +# 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. +# +############################################################################ + +set(LIBCANARD_DIR ${CMAKE_CURRENT_SOURCE_DIR}/libcanard) +set(DSDL_DIR ${CMAKE_CURRENT_SOURCE_DIR}/public_regulated_data_types) + +px4_add_git_submodule(TARGET git_libcanard PATH ${LIBCANARD_DIR}) +px4_add_git_submodule(TARGET git_public_regulated_data_types PATH ${DSDL_DIR}) + +find_program(NNVG_PATH nnvg) +if(NNVG_PATH) + execute_process(COMMAND ${NNVG_PATH} --outdir ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated --target-language c -I ${DSDL_DIR}/uavcan ${DSDL_DIR}/reg) + execute_process(COMMAND ${NNVG_PATH} --outdir ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated --target-language c ${DSDL_DIR}/uavcan) + +else() + message(FATAL_ERROR "UAVCAN Nunavut nnvg not found") +endif() + +add_definitions( + -DCONFIG_EXAMPLES_LIBCANARDV1_DEV="can0" + -DCONFIG_EXAMPLES_LIBCANARDV1_NODE_MEM_POOL_SIZE=8192 + -DCONFIG_EXAMPLES_LIBCANARDV1_DAEMON_STACK_SIZE=5000 + -DCONFIG_EXAMPLES_LIBCANARDV1_DAEMON_PRIORITY=100 + -DCANARD_DSDL_CONFIG_LITTLE_ENDIAN=1 +) + +px4_add_module( + MODULE drivers__uavcannode-gps-demo + MAIN uavcannode_gps_demo + COMPILE_FLAGS + -Wno-error + -DUINT32_C\(x\)=__UINT32_C\(x\) + INCLUDES + ${LIBCANARD_DIR}/libcanard/ + ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated + SRCS + canard_main.c + socketcan.c + socketcan.h + uorb_converter.c + uorb_converter.h + o1heap.c + o1heap.h + libcancl/pnp.c + libcancl/registerinterface.c + ${LIBCANARD_DIR}/libcanard/canard.c + ${LIBCANARD_DIR}/libcanard/canard.h + DEPENDS + git_libcanard + git_public_regulated_data_types + version + ) diff --git a/src/drivers/uavcannode_gps_demo/canard_main.c b/src/drivers/uavcannode_gps_demo/canard_main.c new file mode 100644 index 0000000000..a7a26ae64d --- /dev/null +++ b/src/drivers/uavcannode_gps_demo/canard_main.c @@ -0,0 +1,448 @@ +/**************************************************************************** + * examples/canard/canard_main.c + * + * Copyright (C) 2016 ETH Zuerich. All rights reserved. + * Author: Matthias Renner + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include +#include + +#include "socketcan.h" +#include "o1heap.h" +#include "uorb_converter.h" + +#include "libcancl/pnp.h" +#include "libcancl/registerinterface.h" +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* Arena for memory allocation, used by the library */ +#define O1_HEAP_SIZE CONFIG_EXAMPLES_LIBCANARDV1_NODE_MEM_POOL_SIZE +#define UNIQUE_ID_LENGTH_BYTES 16 + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +O1HeapInstance *my_allocator; +static uint8_t uavcan_heap[O1_HEAP_SIZE] +__attribute__((aligned(O1HEAP_ALIGNMENT))); + +static uint8_t unique_id[UNIQUE_ID_LENGTH_BYTES] = { //FIXME real HW ID + 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x01 +}; + +/* Node status variables */ + +static bool g_canard_daemon_started; + +static int16_t gps_uorb_port_id = -1; +static int16_t gps_fix_port_id = -1; +static int16_t gps_aux_port_id = -1; + +struct pollfd fd; + +/**************************************************************************** + * private functions + ****************************************************************************/ + + +//TODO move this to a seperate file probably + +int32_t set_gps_uorb_port_id(uavcan_register_Value_1_0 *value) +{ + if (uavcan_register_Value_1_0_is_natural16_(value) && value->natural16.value.count == 1) { // Natural 16 + //TODO check validity + printf("Master: set uORB portID to %i\n", value->natural16.value.elements[0]); + gps_uorb_port_id = value->natural16.value.elements[0]; + return 0; + } + + return -UAVCAN_REGISTER_ERROR_SERIALIZATION; +} + +uavcan_register_Value_1_0 get_gps_uorb_port_id() +{ + void *dataReturn; + uavcan_register_Value_1_0 value; + + value.natural16.value.elements[0] = gps_uorb_port_id; + value.natural16.value.count = 1; + value._tag_ = 10; // TODO does nunavut generate ENUM/defines for this?? + return value; +} + +int32_t set_gps_fix_port_id(uavcan_register_Value_1_0 *value) +{ + if (uavcan_register_Value_1_0_is_natural16_(value) && value->natural16.value.count == 1) { // Natural 16 + //TODO check validity + printf("Master: set FIX portID to %i\n", value->natural16.value.elements[0]); + gps_fix_port_id = value->natural16.value.elements[0]; + return 0; + } + + return -UAVCAN_REGISTER_ERROR_SERIALIZATION; +} + +uavcan_register_Value_1_0 get_gps_fix_port_id() +{ + void *dataReturn; + uavcan_register_Value_1_0 value; + + value.natural16.value.elements[0] = gps_fix_port_id; + value.natural16.value.count = 1; + value._tag_ = 10; // TODO does nunavut generate ENUM/defines for this?? + return value; +} + +int32_t set_gps_aux_port_id(uavcan_register_Value_1_0 *value) +{ + if (uavcan_register_Value_1_0_is_natural16_(value) && value->natural16.value.count == 1) { // Natural 16 + //TODO check validity + printf("Master: set AUX portID to %i\n", value->natural16.value.elements[0]); + gps_fix_port_id = value->natural16.value.elements[0]; + return 0; + } + + return -UAVCAN_REGISTER_ERROR_SERIALIZATION; +} + +uavcan_register_Value_1_0 get_gps_aux_port_id() +{ + void *dataReturn; + uavcan_register_Value_1_0 value; + + value.natural16.value.elements[0] = gps_aux_port_id; + value.natural16.value.count = 1; + value._tag_ = 10; // TODO does nunavut generate ENUM/defines for this?? + return value; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: memAllocate + * + * Description: + * + ****************************************************************************/ +static void *memAllocate(CanardInstance *const ins, const size_t amount) +{ + (void) ins; + return o1heapAllocate(my_allocator, amount); +} + +/**************************************************************************** + * Name: memFree + * + * Description: + * + ****************************************************************************/ + +static void memFree(CanardInstance *const ins, void *const pointer) +{ + (void) ins; + o1heapFree(my_allocator, pointer); +} + +/**************************************************************************** + * Name: getMonotonicTimestampUSec + * + * Description: + * + ****************************************************************************/ + + +uint64_t getMonotonicTimestampUSec(void) +{ + int ret; + struct timespec ts; + + memset(&ts, 0, sizeof(ts)); + + ret = clock_gettime(CLOCK_MONOTONIC, &ts); + + if (ret != 0) { + PX4_ERR("clock error %i", ret); + abort(); + } + + return ts.tv_sec * 1000000ULL + ts.tv_nsec / 1000ULL; +} + +/**************************************************************************** + * Name: processTxRxOnce + * + * Description: + * Transmits all frames from the TX queue, receives up to one frame. + * + ****************************************************************************/ + +void processTxRxOnce(CanardInstance *ins, CanardSocketInstance *sock_ins, int timeout_msec) +{ + int32_t result; + + /* Transmitting */ + + + for (const CanardFrame *txf = NULL; (txf = canardTxPeek(ins)) != NULL;) { // Look at the top of the TX queue. + if (txf->timestamp_usec > getMonotonicTimestampUSec()) { // Check if the frame has timed out. + if (socketcanTransmit(sock_ins, txf) == 0) { // Send the frame. Redundant interfaces may be used here. + break; // If the driver is busy, break and retry later. + } + + } else { + printf("Timeout??\n"); + } + + canardTxPop(ins); // Remove the frame from the queue after it's transmitted. + ins->memory_free(ins, (CanardFrame *)txf); // Deallocate the dynamic memory afterwards. + } + + + /* Poll receive */ + if (poll(&fd, 1, timeout_msec) <= 0) { + return; + } + + /* Receiving */ + CanardFrame received_frame; + + socketcanReceive(sock_ins, &received_frame); + + CanardTransfer receive; + result = canardRxAccept(ins, + &received_frame, // The CAN frame received from the bus. + 0, // If the transport is not redundant, use 0. + &receive); + + if (result < 0) { + // An error has occurred: either an argument is invalid or we've ran out of memory. + // It is possible to statically prove that an out-of-memory will never occur for a given application if + // the heap is sized correctly; for background, refer to the Robson's Proof and the documentation for O1Heap. + // Reception of an invalid frame is NOT an error. + fprintf(stderr, "Receive error %d\n", result); + + } else if (result == 1) { + printf("Receive portId %i\n", receive.port_id); + + if (receive.port_id == PNPGetPortID(ins)) { + PNPProcess(ins, &receive); + + } else { + uavcan_register_interface_process(ins, &receive); + } + + ins->memory_free(ins, (void *)receive.payload); // Deallocate the dynamic memory afterwards. + + } else { + // printf("RX canard %d\r\n", result); + // Nothing to do. + // The received frame is either invalid or it's a non-last frame of a multi-frame transfer. + // Reception of an invalid frame is NOT reported as an error because it is not an error. + } + +} + +/**************************************************************************** + * Name: canard_daemon + * + * Description: + * + ****************************************************************************/ + +static int canard_daemon(int argc, char *argv[]) +{ + int errval = 0; + int can_fd = 0; + + if (argc > 2) { + for (int args = 2; args < argc; args++) { + if (!strcmp(argv[args], "canfd")) { + can_fd = 1; + } + } + } + + my_allocator = o1heapInit(&uavcan_heap, O1_HEAP_SIZE, NULL, NULL); + + if (my_allocator == NULL) { + printf("o1heapInit failed with size %d\n", O1_HEAP_SIZE); + errval = 2; + goto errout_with_dev; + } + + CanardInstance ins = canardInit(&memAllocate, &memFree); + + if (can_fd) { + ins.mtu_bytes = CANARD_MTU_CAN_FD; + + } else { + ins.mtu_bytes = CANARD_MTU_CAN_CLASSIC; + } + + /* Open the CAN device for reading */ + CanardSocketInstance sock_ins; + socketcanOpen(&sock_ins, CONFIG_EXAMPLES_LIBCANARDV1_DEV, can_fd); + + /* setup poll fd */ + fd.fd = sock_ins.s; + fd.events = POLLIN; + + if (sock_ins.s < 0) { + printf("canard_daemon: ERROR: open %s failed: %d\n", + CONFIG_EXAMPLES_LIBCANARDV1_DEV, errno); + errval = 2; + goto errout_with_dev; + } + + /* libcancl functions */ + + /* Dynamic NodeId */ + + /* Init UAVCAN register interfaces */ + uavcan_node_GetInfo_Response_1_0 node_information; // TODO ADD INFO + uavcan_register_interface_init(&ins, &node_information); + uavcan_register_interface_add_entry("gnss_uorb", set_gps_uorb_port_id, get_gps_uorb_port_id); + uavcan_register_interface_add_entry("gnss_fix", set_gps_fix_port_id, get_gps_fix_port_id); + uavcan_register_interface_add_entry("gnss_aux", set_gps_aux_port_id, get_gps_aux_port_id); + + initPNPAllocatee(&ins, unique_id); + + uint32_t random_no; + random_no = ((float)rand() / RAND_MAX) * (1000000); + + uint64_t next_alloc_req = getMonotonicTimestampUSec() + random_no; + + while (ins.node_id == CANARD_NODE_ID_UNSET) { + // process the TX and RX buffer + processTxRxOnce(&ins, &sock_ins, 10); //10Ms + + const uint64_t ts = getMonotonicTimestampUSec(); + + if (ts >= next_alloc_req) { + next_alloc_req += ((float)rand() / RAND_MAX) * (1000000); + int32_t result = PNPAllocRequest(&ins); + + if (result) { + ins.node_id = PNPGetNodeID(); + } + } + } + + printf("canard_daemon: canard initialized\n"); + printf("start node (ID: %d MTU: %d)\n", ins.node_id, + ins.mtu_bytes); + + /* Initialize uORB publishers & subscribers */ + uorbConverterInit(&ins, &gps_uorb_port_id, &gps_fix_port_id, &gps_aux_port_id); + + g_canard_daemon_started = true; + uint64_t next_1hz_service_at = getMonotonicTimestampUSec(); + + for (;;) { + processTxRxOnce(&ins, &sock_ins, 10); + + uorbProcessSub(10); + + } + +errout_with_dev: + + g_canard_daemon_started = false; + printf("canard_daemon: Terminating!\n"); + fflush(stdout); + return errval; +} + + +/**************************************************************************** + * Name: canard_main + * + * Description: + * + ****************************************************************************/ + +int uavcannode_gps_demo_main(int argc, FAR char *argv[]) +{ + int ret; + + printf("canard_main: Starting canard_daemon\n"); + + if (g_canard_daemon_started) { + printf("canard_main: receive and send task already running\n"); + return EXIT_SUCCESS; + } + + ret = task_create("canard_daemon", CONFIG_EXAMPLES_LIBCANARDV1_DAEMON_PRIORITY, + CONFIG_EXAMPLES_LIBCANARDV1_DAEMON_STACK_SIZE, canard_daemon, + argv); + + if (ret < 0) { + int errcode = errno; + printf("canard_main: ERROR: Failed to start canard_daemon: %d\n", + errcode); + return EXIT_FAILURE; + } + + printf("canard_main: canard_daemon started\n"); + return EXIT_SUCCESS; +} diff --git a/src/drivers/uavcannode_gps_demo/libcanard b/src/drivers/uavcannode_gps_demo/libcanard new file mode 160000 index 0000000000..cde6703474 --- /dev/null +++ b/src/drivers/uavcannode_gps_demo/libcanard @@ -0,0 +1 @@ +Subproject commit cde670347425023480a1417fcd603b27c8eb06c1 diff --git a/src/drivers/uavcannode_gps_demo/libcancl/README.md b/src/drivers/uavcannode_gps_demo/libcancl/README.md new file mode 100644 index 0000000000..1d27ba821c --- /dev/null +++ b/src/drivers/uavcannode_gps_demo/libcancl/README.md @@ -0,0 +1,12 @@ +# libcancl + +Libcanard client library, that implements common client functionality for use with UAVCAN/CAN +Current features are + +- UAVCAN PNP client +- UAVCAN Register interface +- UAVCAN GetInfo responder + +# TODO's +- UAVCAN ExecuteCommand implementation +- UAVCAN diagnostic abstraction/helper diff --git a/src/drivers/uavcannode_gps_demo/libcancl/pnp.c b/src/drivers/uavcannode_gps_demo/libcancl/pnp.c new file mode 100644 index 0000000000..4ae8c7a2b2 --- /dev/null +++ b/src/drivers/uavcannode_gps_demo/libcancl/pnp.c @@ -0,0 +1,264 @@ +/**************************************************************************** + * nxp_bms/BMS_v1/src/UAVCAN/pnp.c + * + * BSD 3-Clause License + * + * Copyright 2020 NXP + * + * 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 of the copyright holder 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 HOLDER 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 "pnp.h" + +// Use NuttX crc64 function TODO fallback header for other platforms +#include + +#include "uavcan/node/ID_1_0.h" +#include "uavcan/pnp/NodeIDAllocationData_1_0.h" +#include "uavcan/pnp/NodeIDAllocationData_2_0.h" + +#define PNP1_PORT_ID uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_ +#define PNP1_PAYLOAD_SIZE uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_ +#define PNP2_PORT_ID uavcan_pnp_NodeIDAllocationData_2_0_FIXED_PORT_ID_ +#define PNP2_PAYLOAD_SIZE uavcan_pnp_NodeIDAllocationData_2_0_SERIALIZATION_BUFFER_SIZE_BYTES_ + +#define PNP_UNIQUE_ID_SIZE 16 // 128 bit unique id + +#define PNP_NODE_ID_NO_PREFERENCE CANARD_NODE_ID_MAX - 2 // No preference -> highest possible node ID, note: the two highest node-ID values are reserved for network maintenance tools + +/**************************************************************************** + * private data + ****************************************************************************/ + +CanardRxSubscription allocSub; + +// 128 bit unique id +uint8_t local_unique_id[PNP_UNIQUE_ID_SIZE]; + +const CanardNodeID preffered_node_id = PNP_NODE_ID_NO_PREFERENCE; + +CanardNodeID node_id = CANARD_NODE_ID_UNSET; + +CanardTransferID node_id_alloc_transfer_id = 0; + +/**************************************************************************** + * private Functions declerations + ****************************************************************************/ + +uint64_t makePseudoUniqueId(uint8_t *pUniqueID) +{ + // NuttX CRC64/WE implementation + return crc64(pUniqueID, PNP_UNIQUE_ID_SIZE); +} + + +/**************************************************************************** + * public functions + ****************************************************************************/ + +/* Rule A. On initialization: + * 1. The allocatee subscribes to this message. + * 2. The allocatee starts the Request Timer with a random interval [0, 1) sec of Trequest. + */ + +uint32_t initPNPAllocatee(CanardInstance *ins, uint8_t *unique_id) +{ + // Store unique_id locally + memcpy(&local_unique_id[0], &unique_id[0], sizeof(local_unique_id)); + + // Create RX Subscriber so we can listen to NodeIDAllocationData msgs + (void) canardRxSubscribe(ins, // Subscribe to messages uavcan.node.Heartbeat. + CanardTransferKindMessage, + (ins->mtu_bytes == CANARD_MTU_CAN_FD ? PNP2_PORT_ID : PNP1_PORT_ID), // The fixed Subject-ID + (ins->mtu_bytes == CANARD_MTU_CAN_FD ? PNP2_PAYLOAD_SIZE : PNP1_PAYLOAD_SIZE), + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &allocSub); + + // Callee should start a timer with a random interval between 0 and 1 sec +} + +/* Rule B. On expiration of the Request Timer (started as per rules A, B, or C): + * 1. Request Timer restarts with a random interval [0, 1) sec of Trequest (chosen anew). + * 2. The allocatee broadcasts an allocation request message, where the fields are populated as follows: + * unique_id_hash - a 48-bit hash of the unique-ID of the allocatee. + * allocated_node_id - empty (not populated). + */ +int32_t PNPAllocRequest(CanardInstance *ins) +{ + int32_t result; + CanardMicrosecond transmission_deadline = getMonotonicTimestampUSec() + 1000 * 10; + + // Callee should restart timer + + // Allocation already done, nothing to do TODO maybe stop subscribing + if (node_id != CANARD_NODE_ID_UNSET) { + return 1; + } + + if (ins->mtu_bytes == CANARD_MTU_CAN_FD) { + // NodeIDAllocationData message + uavcan_pnp_NodeIDAllocationData_2_0 node_id_alloc_msg; + uint8_t node_id_alloc_payload_buffer[PNP2_PAYLOAD_SIZE]; + + memcpy(node_id_alloc_msg.unique_id, local_unique_id, sizeof(node_id_alloc_msg.unique_id)); + node_id_alloc_msg.node_id.value = preffered_node_id; + + CanardTransfer transfer = { + .timestamp_usec = transmission_deadline, // Zero if transmission deadline is not limited. + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindMessage, + .port_id = PNP2_PORT_ID, // This is the subject-ID. + .remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET. + .transfer_id = node_id_alloc_transfer_id, + .payload_size = PNP2_PAYLOAD_SIZE, + .payload = &node_id_alloc_payload_buffer, + }; + + result = uavcan_pnp_NodeIDAllocationData_2_0_serialize_(&node_id_alloc_msg, &node_id_alloc_payload_buffer, + &transfer.payload_size); + + if (result == 0) { + // set the data ready in the buffer and chop if needed + ++node_id_alloc_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. + result = canardTxPush(ins, &transfer); + } + + } else { + // NodeIDAllocationData message + uavcan_pnp_NodeIDAllocationData_1_0 node_id_alloc_msg; + uavcan_pnp_NodeIDAllocationData_1_0_initialize_(&node_id_alloc_msg); + uint8_t node_id_alloc_payload_buffer[PNP1_PAYLOAD_SIZE]; + + node_id_alloc_msg.unique_id_hash = (makePseudoUniqueId(local_unique_id) & 0xFFFFFFFFFFFF); + + CanardTransfer transfer = { + .timestamp_usec = transmission_deadline, // Zero if transmission deadline is not limited. + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindMessage, + .port_id = PNP1_PORT_ID, // This is the subject-ID. + .remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET. + .transfer_id = node_id_alloc_transfer_id, + .payload_size = PNP1_PAYLOAD_SIZE, + .payload = &node_id_alloc_payload_buffer, + }; + + result = uavcan_pnp_NodeIDAllocationData_1_0_serialize_(&node_id_alloc_msg, &node_id_alloc_payload_buffer, + &transfer.payload_size); + + if (result == 0) { + // set the data ready in the buffer and chop if needed + ++node_id_alloc_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. + result = canardTxPush(ins, &transfer); + } + } + + + if (result < 0) { + // An error has occurred: either an argument is invalid or we've ran out of memory. + // It is possible to statically prove that an out-of-memory will never occur for a given application if the + // heap is sized correctly; for background, refer to the Robson's Proof and the documentation for O1Heap. + return result; + } + + return 0; +} + +/* Rule C. On any allocation message, even if other rules also match: + * 1. Request Timer restarts with a random interval of Trequest (chosen anew). + * + * Rule D. On an allocation message WHERE (source node-ID is non-anonymous, i.e., regular allocation response) + * AND (the field unique_id_hash matches the allocatee's 48-bit unique-ID hash) + * AND (the field allocated_node_id is populated): + * 1. Request Timer stops. + * 2. The allocatee initializes its node-ID with the received value. + * 3. The allocatee terminates its subscription to allocation messages. + * 4. Exit. + */ +int32_t PNPProcess(CanardInstance *ins, CanardTransfer *transfer) +{ + + // Allocation already done, nothing to do + if (node_id != CANARD_NODE_ID_UNSET) { + return 1; + } + + if (transfer->remote_node_id == CANARD_NODE_ID_UNSET) { // Another request, ignore. + return 0; + } + + int32_t allocated = CANARD_NODE_ID_UNSET; + + if (ins->mtu_bytes == CANARD_MTU_CAN_FD) { + uavcan_pnp_NodeIDAllocationData_2_0 msg; + + size_t pnp_in_size_bits = transfer->payload_size; + uavcan_pnp_NodeIDAllocationData_2_0_deserialize_(&msg, transfer->payload, &pnp_in_size_bits); + + if (memcmp(msg.unique_id, local_unique_id, sizeof(msg.unique_id)) == 0) { + allocated = msg.node_id.value; + } + + } else { + uavcan_pnp_NodeIDAllocationData_1_0 msg; + + size_t pnp_in_size_bits = transfer->payload_size; + uavcan_pnp_NodeIDAllocationData_1_0_deserialize_(&msg, transfer->payload, &pnp_in_size_bits); + + if (msg.allocated_node_id.count > 0) { + if (msg.unique_id_hash == (makePseudoUniqueId(local_unique_id) & 0xFFFFFFFFFFFF)) { + allocated = msg.allocated_node_id.elements[0].value; + } + } + } + + if (allocated == CANARD_NODE_ID_UNSET) { + return -1; // UID mismatch. + } + + if (allocated <= 0 || allocated >= CANARD_NODE_ID_MAX) + // Allocated node-ID ignored because it exceeds max_node_id + { + return -1; + } + + // Plug-and-play allocation done: got node-ID %s from server %s', allocated, meta.source_node_id) + node_id = allocated; + + return 1; +} + + +CanardNodeID PNPGetNodeID() +{ + return node_id; +} + + +CanardPortID PNPGetPortID(CanardInstance *ins) +{ + return (ins->mtu_bytes == CANARD_MTU_CAN_FD ? PNP2_PORT_ID : PNP1_PORT_ID); +} diff --git a/src/drivers/uavcannode_gps_demo/libcancl/pnp.h b/src/drivers/uavcannode_gps_demo/libcancl/pnp.h new file mode 100644 index 0000000000..5847837a47 --- /dev/null +++ b/src/drivers/uavcannode_gps_demo/libcancl/pnp.h @@ -0,0 +1,74 @@ +/**************************************************************************** + * nxp_bms/BMS_v1/inc/UAVCAN/pnp.h + * + * BSD 3-Clause License + * + * Copyright 2020 NXP + * + * 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 of the copyright holder 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 HOLDER 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. + * + ** ################################################################### + ** Filename : pnp.h + ** Project : SmartBattery_RDDRONE_BMS772 + ** Processor : S32K144 + ** Version : 1.00 + ** Date : 2020-10-29 + ** Abstract : + ** pnp module. + ** + ** ###################################################################*/ +/*! + ** @file pnp.h + ** + ** @version 01.00 + ** + ** @brief + ** pnp module. this module implements the UAVCAN plug and play protocol + ** + */ + +#ifndef UAVCAN_PNP_H_ +#define UAVCAN_PNP_H_ + +#define NUNAVUT_ASSERT +#include + +#include "time.h" + + +uint32_t initPNPAllocatee(CanardInstance *ins, uint8_t *unique_id); + +int32_t PNPAllocRequest(CanardInstance *ins); + +int32_t PNPProcess(CanardInstance *ins, CanardTransfer *transfer); + +CanardNodeID PNPGetNodeID(); + +const CanardPortID PNPGetPortID(); + + + +#endif //UAVCAN_PNP_H_ diff --git a/src/drivers/uavcannode_gps_demo/libcancl/registerinterface.c b/src/drivers/uavcannode_gps_demo/libcancl/registerinterface.c new file mode 100644 index 0000000000..299e8dc6fd --- /dev/null +++ b/src/drivers/uavcannode_gps_demo/libcancl/registerinterface.c @@ -0,0 +1,279 @@ + +#include "registerinterface.h" + +#include "uavcan/_register/Access_1_0.h" +#include "uavcan/_register/List_1_0.h" +#include "uavcan/_register/Name_1_0.h" +#include "uavcan/_register/Value_1_0.h" + +#include +#include + +/**************************************************************************** + * private data + ****************************************************************************/ + +uavcan_node_GetInfo_Response_1_0 *node_info; +CanardTransferID getinfo_response_transfer_id = 0; +CanardTransferID register_access_response_transfer_id = 0; +CanardTransferID register_list_response_transfer_id = 0; + +CanardRxSubscription getinfo_subscription; +CanardRxSubscription register_access_subscription; +CanardRxSubscription register_list_subscription; + +//TODO register list and data +uavcan_register_interface_entry register_list[UAVCAN_REGISTER_COUNT]; +uint32_t register_list_size = 0; + +/**************************************************************************** + * public functions + ****************************************************************************/ + +int32_t uavcan_register_interface_init(CanardInstance *ins, uavcan_node_GetInfo_Response_1_0 *info) +{ + node_info = info; //TODO think about retention, copy isntead? + + (void) canardRxSubscribe(ins, + CanardTransferKindRequest, + uavcan_node_GetInfo_1_0_FIXED_PORT_ID_, + uavcan_node_GetInfo_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &getinfo_subscription); + + (void) canardRxSubscribe(ins, + CanardTransferKindRequest, + uavcan_register_Access_1_0_FIXED_PORT_ID_, + uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + ®ister_access_subscription); + + (void) canardRxSubscribe(ins, + CanardTransferKindRequest, + uavcan_register_List_1_0_FIXED_PORT_ID_, + uavcan_register_List_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + ®ister_list_subscription); +} + +int32_t uavcan_register_interface_add_entry(const char *name, register_access_set_callback cb_set, + register_access_get_callback cb_get) +{ + if (register_list_size < UAVCAN_REGISTER_COUNT) { + register_list[register_list_size].name = name; + register_list[register_list_size].cb_set = cb_set; + register_list[register_list_size].cb_get = cb_get; + register_list_size++; + return 0; + + } else { + return -UAVCAN_REGISTER_ERROR_OUT_OF_MEMORY; // register list full + } + +} + +// Handler for all PortID registration related messages +int32_t uavcan_register_interface_process(CanardInstance *ins, CanardTransfer *transfer) +{ + if (transfer->port_id == uavcan_node_GetInfo_1_0_FIXED_PORT_ID_) { + return uavcan_register_interface_get_info_response(ins, transfer); + + } else if (transfer->port_id == uavcan_register_Access_1_0_FIXED_PORT_ID_) { + return uavcan_register_interface_access_response(ins, transfer); + + } else if (transfer->port_id == uavcan_register_List_1_0_FIXED_PORT_ID_) { + return uavcan_register_interface_list_response(ins, transfer); + } + + return 0; // Nothing to do +} + +// Handler for node.GetInfo which yields a response +int32_t uavcan_register_interface_get_info_response(CanardInstance *ins, CanardTransfer *request) +{ + uavcan_node_GetInfo_Request_1_0 msg; + + size_t in_size_bits; + + if (uavcan_node_GetInfo_Request_1_0_deserialize_(&msg, request->payload, request->payload_size) < 0) { + //Error deserialize failed + return -UAVCAN_REGISTER_ERROR_SERIALIZATION; + } + + //Note so technically the uavcan_node_GetInfo_Request_1_0 is an empty message not sure if the code above is required + + // Setup node.GetInfo response + + uint8_t response_payload_buffer[uavcan_node_GetInfo_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; + + CanardMicrosecond transmission_deadline = getMonotonicTimestampUSec() + 1000 * 100; + + CanardTransfer response = { + .timestamp_usec = transmission_deadline, // Zero if transmission deadline is not limited. + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindResponse, + .port_id = uavcan_node_GetInfo_1_0_FIXED_PORT_ID_, // This is the subject-ID. + .remote_node_id = request->remote_node_id, // Send back to request Node + .transfer_id = getinfo_response_transfer_id, + .payload_size = uavcan_node_GetInfo_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, + .payload = &response_payload_buffer, + }; + + int32_t result = uavcan_node_GetInfo_Response_1_0_serialize_(node_info, &response_payload_buffer, + &response.payload_size); + + if (result == 0) { + // set the data ready in the buffer and chop if needed + ++getinfo_response_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. + result = canardTxPush(ins, &response); + } + + if (result < 0) { + // An error has occurred: either an argument is invalid or we've ran out of memory. + // It is possible to statically prove that an out-of-memory will never occur for a given application if the + // heap is sized correctly; for background, refer to the Robson's Proof and the documentation for O1Heap. + return -UAVCAN_REGISTER_ERROR_SERIALIZATION; + } + + return 1; + +} + +// Handler for register access interface +int32_t uavcan_register_interface_access_response(CanardInstance *ins, CanardTransfer *request) +{ + + int index; + { + uavcan_register_Access_Request_1_0 msg; + + if (uavcan_register_Access_Request_1_0_deserialize_(&msg, request->payload, &request->payload_size) < 0) { + //Error deserialize failed + return -UAVCAN_REGISTER_ERROR_SERIALIZATION; + } + + { + char register_string[255]; + int reg_str_len; + + for (index = 0; index < register_list_size; index++) { + reg_str_len = sprintf(register_string, "uavcan.pub.%s.id", + register_list[index].name); //TODO more option then pub (sub rate etc) + + if (strncmp(msg.name.name.elements, register_string, reg_str_len) == 0) { + if (msg.value._tag_ != 0) { // Value has been set thus we call set handler + if (register_list[index].cb_set(&msg.value) != 0) { + // TODO error ocurred check doc for correct response + } + } + + break; // We're done exit loop + } + } + } + } + + uavcan_register_Access_Response_1_0 response_msg; + uavcan_register_Access_Response_1_0_initialize_(&response_msg); + + if (index < register_list_size) { // Index is available + response_msg.value = register_list[index].cb_get(); + + } else { // Index not found return empty response + uavcan_register_Value_1_0_initialize_(&response_msg.value); + uavcan_register_Value_1_0_select_empty_(&response_msg.value); + } + + uint8_t response_payload_buffer[uavcan_register_Access_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; + + CanardMicrosecond transmission_deadline = getMonotonicTimestampUSec() + 1000 * 100; + + CanardTransfer response = { + .timestamp_usec = transmission_deadline, // Zero if transmission deadline is not limited. + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindResponse, + .port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_, // This is the subject-ID. + .remote_node_id = request->remote_node_id, // Send back to request Node + .transfer_id = register_list_response_transfer_id, + .payload_size = uavcan_register_Access_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, + .payload = &response_payload_buffer, + }; + + int32_t result = uavcan_register_Access_Response_1_0_serialize_(&response_msg, &response_payload_buffer, + &response.payload_size); + + if (result == 0) { + // set the data ready in the buffer and chop if needed + ++register_access_response_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. + result = canardTxPush(ins, &response); + } + + if (result < 0) { + // An error has occurred: either an argument is invalid or we've ran out of memory. + // It is possible to statically prove that an out-of-memory will never occur for a given application if the + // heap is sized correctly; for background, refer to the Robson's Proof and the documentation for O1Heap. + return -UAVCAN_REGISTER_ERROR_SERIALIZATION; + } + + return 1; +} + +// Handler for register list interface +int32_t uavcan_register_interface_list_response(CanardInstance *ins, CanardTransfer *request) +{ + uavcan_register_List_Request_1_0 msg; + + if (uavcan_register_List_Request_1_0_deserialize_(&msg, request->payload, &request->payload_size) < 0) { + //Error deserialize failed + return -UAVCAN_REGISTER_ERROR_SERIALIZATION; + } + + //Setup register response + + uint8_t response_payload_buffer[uavcan_register_List_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; //TODO we know already how big our response is, don't overallocate. + + CanardMicrosecond transmission_deadline = getMonotonicTimestampUSec() + 1000 * 100; + + uavcan_register_List_Response_1_0 response_msg; + + // Reponse magic start + + if (msg.index <= register_list_size) { + response_msg.name.name.count = sprintf(response_msg.name.name.elements, + "uavcan.pub.%s.id", + register_list[msg.index].name); + } + + //TODO more option then pub (sub rate + + // Response magic end + + CanardTransfer response = { + .timestamp_usec = transmission_deadline, // Zero if transmission deadline is not limited. + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindResponse, + .port_id = uavcan_register_List_1_0_FIXED_PORT_ID_, // This is the subject-ID. + .remote_node_id = request->remote_node_id, // Send back to request Node + .transfer_id = register_list_response_transfer_id, + .payload_size = uavcan_register_List_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, //See prev TODO + .payload = &response_payload_buffer, + }; + + int32_t result = uavcan_register_List_Response_1_0_serialize_(&response_msg, &response_payload_buffer, + &response.payload_size); + + if (result == 0) { + // set the data ready in the buffer and chop if needed + ++register_list_response_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. + result = canardTxPush(ins, &response); + } + + if (result < 0) { + // An error has occurred: either an argument is invalid or we've ran out of memory. + // It is possible to statically prove that an out-of-memory will never occur for a given application if the + // heap is sized correctly; for background, refer to the Robson's Proof and the documentation for O1Heap. + return -UAVCAN_REGISTER_ERROR_SERIALIZATION; + } + + return 1; +} diff --git a/src/drivers/uavcannode_gps_demo/libcancl/registerinterface.h b/src/drivers/uavcannode_gps_demo/libcancl/registerinterface.h new file mode 100644 index 0000000000..91ffcc63da --- /dev/null +++ b/src/drivers/uavcannode_gps_demo/libcancl/registerinterface.h @@ -0,0 +1,49 @@ + +#ifndef UAVCAN_REGISTER_INTERFACE_H_ +#define UAVCAN_REGISTER_INTERFACE_H_ + +#include + +#define NUNAVUT_ASSERT +#include "uavcan/node/GetInfo_1_0.h" +#include "uavcan/_register/Value_1_0.h" + +#include "time.h" + +//No of pre allocated register entries +#ifndef UAVCAN_REGISTER_COUNT +# define UAVCAN_REGISTER_COUNT 5 +#endif + +#define UAVCAN_REGISTER_ERROR_SERIALIZATION 1 +#define UAVCAN_REGISTER_ERROR_OUT_OF_MEMORY 2 + +typedef int32_t (*register_access_set_callback)(uavcan_register_Value_1_0 *value); +typedef uavcan_register_Value_1_0(*register_access_get_callback)(void); + +typedef struct { + /// uavcan.register.Name.1.0 name + const char *name; + register_access_set_callback cb_set; + register_access_get_callback cb_get; +} uavcan_register_interface_entry; + + +int32_t uavcan_register_interface_init(CanardInstance *ins, uavcan_node_GetInfo_Response_1_0 *info); + +int32_t uavcan_register_interface_add_entry(const char *name, register_access_set_callback cb_set, + register_access_get_callback cb_get); + +// Handler for all PortID registration related messages +int32_t uavcan_register_interface_process(CanardInstance *ins, CanardTransfer *transfer); + +// Handler for node.GetInfo which yields a response +int32_t uavcan_register_interface_get_info_response(CanardInstance *ins, CanardTransfer *request); + +// Handler for register access interface +int32_t uavcan_register_interface_access_response(CanardInstance *ins, CanardTransfer *request); + +// Handler for register list interface +int32_t uavcan_register_interface_list_response(CanardInstance *ins, CanardTransfer *request); + +#endif //UAVCAN_REGISTER_INTERFACE_H_ diff --git a/src/drivers/uavcannode_gps_demo/libcancl/time.h b/src/drivers/uavcannode_gps_demo/libcancl/time.h new file mode 100644 index 0000000000..e25202f8ba --- /dev/null +++ b/src/drivers/uavcannode_gps_demo/libcancl/time.h @@ -0,0 +1,6 @@ +#ifndef MAVCAN_TIME_H_ +#define MAVCAN_TIME_H_ + +uint64_t getMonotonicTimestampUSec(void); + +#endif //MAVCAN_TIME_H_ diff --git a/src/drivers/uavcannode_gps_demo/o1heap.c b/src/drivers/uavcannode_gps_demo/o1heap.c new file mode 100644 index 0000000000..0766745f09 --- /dev/null +++ b/src/drivers/uavcannode_gps_demo/o1heap.c @@ -0,0 +1,498 @@ +// Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated +// documentation files (the "Software"), to deal in the Software without restriction, including without limitation +// the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, +// and to permit persons to whom the Software is furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all copies or substantial portions +// of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE +// WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS +// OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +// OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. +// +// Copyright (c) 2020 Pavel Kirienko +// Authors: Pavel Kirienko + +#include "o1heap.h" +#include + +// ---------------------------------------- BUILD CONFIGURATION OPTIONS ---------------------------------------- + +/// The assertion macro defaults to the standard assert(). +/// It can be overridden to manually suppress assertion checks or use a different error handling policy. +#ifndef O1HEAP_ASSERT +// Intentional violation of MISRA: the assertion check macro cannot be replaced with a function definition. +# define O1HEAP_ASSERT(x) assert(x) // NOSONAR +#endif + +/// Branch probability annotations are used to improve the worst case execution time (WCET). They are entirely optional. +/// A stock implementation is provided for some well-known compilers; for other compilers it defaults to nothing. +/// If you are using a different compiler, consider overriding this value. +#ifndef O1HEAP_LIKELY +# if defined(__GNUC__) || defined(__clang__) || defined(__CC_ARM) +// Intentional violation of MISRA: branch hinting macro cannot be replaced with a function definition. +# define O1HEAP_LIKELY(x) __builtin_expect((x), 1) // NOSONAR +# else +# define O1HEAP_LIKELY(x) x +# endif +#endif + +/// This option is used for testing only. Do not use in production. +#if defined(O1HEAP_EXPOSE_INTERNALS) && O1HEAP_EXPOSE_INTERNALS +# define O1HEAP_PRIVATE +#else +# define O1HEAP_PRIVATE static inline +#endif + +// ---------------------------------------- INTERNAL DEFINITIONS ---------------------------------------- + +#if !defined(__STDC_VERSION__) || (__STDC_VERSION__ < 199901L) +# error "Unsupported language: ISO C99 or a newer version is required." +#endif + +#if __STDC_VERSION__ < 201112L +// Intentional violation of MISRA: static assertion macro cannot be replaced with a function definition. +# define static_assert(x, ...) typedef char _static_assert_gl(_static_assertion_, __LINE__)[(x) ? 1 : -1] // NOSONAR +# define _static_assert_gl(a, b) _static_assert_gl_impl(a, b) // NOSONAR +// Intentional violation of MISRA: the paste operator ## cannot be avoided in this context. +# define _static_assert_gl_impl(a, b) a##b // NOSONAR +#endif + +/// The overhead is at most O1HEAP_ALIGNMENT bytes large, +/// then follows the user data which shall keep the next fragment aligned. +#define FRAGMENT_SIZE_MIN (O1HEAP_ALIGNMENT * 2U) + +/// This is risky, handle with care: if the allocation amount plus per-fragment overhead exceeds 2**(b-1), +/// where b is the pointer bit width, then ceil(log2(amount)) yields b; then 2**b causes an integer overflow. +/// To avoid this, we put a hard limit on fragment size (which is amount + per-fragment overhead): 2**(b-1) +#define FRAGMENT_SIZE_MAX ((SIZE_MAX >> 1U) + 1U) + +/// Normally we should subtract log2(FRAGMENT_SIZE_MIN) but log2 is bulky to compute using the preprocessor only. +/// We will certainly end up with unused bins this way, but it is cheap to ignore. +#define NUM_BINS_MAX (sizeof(size_t) * 8U) + +static_assert((O1HEAP_ALIGNMENT & (O1HEAP_ALIGNMENT - 1U)) == 0U, "Not a power of 2"); +static_assert((FRAGMENT_SIZE_MIN & (FRAGMENT_SIZE_MIN - 1U)) == 0U, "Not a power of 2"); +static_assert((FRAGMENT_SIZE_MAX & (FRAGMENT_SIZE_MAX - 1U)) == 0U, "Not a power of 2"); + +typedef struct Fragment Fragment; + +typedef struct FragmentHeader { + Fragment *next; + Fragment *prev; + size_t size; + bool used; +} FragmentHeader; +static_assert(sizeof(FragmentHeader) <= O1HEAP_ALIGNMENT, "Memory layout error"); + +struct Fragment { + FragmentHeader header; + // Everything past the header may spill over into the allocatable space. The header survives across alloc/free. + Fragment *next_free; // Next free fragment in the bin; NULL in the last one. + Fragment *prev_free; // Same but points back; NULL in the first one. +}; +static_assert(sizeof(Fragment) <= FRAGMENT_SIZE_MIN, "Memory layout error"); + +struct O1HeapInstance { + Fragment *bins[NUM_BINS_MAX]; ///< Smallest fragments are in the bin at index 0. + size_t nonempty_bin_mask; ///< Bit 1 represents a non-empty bin; bin at index 0 is for the smallest fragments. + + O1HeapHook critical_section_enter; + O1HeapHook critical_section_leave; + + O1HeapDiagnostics diagnostics; +}; + +/// The amount of space allocated for the heap instance. +/// Its size is padded up to O1HEAP_ALIGNMENT to ensure correct alignment of the allocation arena that follows. +#define INSTANCE_SIZE_PADDED ((sizeof(O1HeapInstance) + O1HEAP_ALIGNMENT - 1U) & ~(O1HEAP_ALIGNMENT - 1U)) + +static_assert(INSTANCE_SIZE_PADDED >= sizeof(O1HeapInstance), "Invalid instance footprint computation"); +static_assert((INSTANCE_SIZE_PADDED % O1HEAP_ALIGNMENT) == 0U, "Invalid instance footprint computation"); + +/// True if the argument is an integer power of two or zero. +O1HEAP_PRIVATE bool isPowerOf2(const size_t x); +O1HEAP_PRIVATE bool isPowerOf2(const size_t x) +{ + return (x & (x - 1U)) == 0U; +} + +/// Special case: if the argument is zero, returns zero. +O1HEAP_PRIVATE uint8_t log2Floor(const size_t x); +O1HEAP_PRIVATE uint8_t log2Floor(const size_t x) +{ + size_t tmp = x; + uint8_t y = 0; + + // This is currently the only exception to the statement "routines contain neither loops nor recursion". + // It is unclear if there is a better way to compute the binary logarithm than this. + while (tmp > 1U) { + tmp >>= 1U; + y++; + } + + return y; +} + +/// Special case: if the argument is zero, returns zero. +O1HEAP_PRIVATE uint8_t log2Ceil(const size_t x); +O1HEAP_PRIVATE uint8_t log2Ceil(const size_t x) +{ + return (uint8_t)(log2Floor(x) + (isPowerOf2(x) ? 0U : 1U)); +} + +/// Raise 2 into the specified power. +/// You might be tempted to do something like (1U << power). WRONG! We humans are prone to forgetting things. +/// If you forget to cast your 1U to size_t or ULL, you may end up with undefined behavior. +O1HEAP_PRIVATE size_t pow2(const uint8_t power); +O1HEAP_PRIVATE size_t pow2(const uint8_t power) +{ + return ((size_t) 1U) << power; +} + +O1HEAP_PRIVATE void invoke(const O1HeapHook hook); +O1HEAP_PRIVATE void invoke(const O1HeapHook hook) +{ + if (hook != NULL) { + hook(); + } +} + +/// Links two fragments so that their next/prev pointers point to each other; left goes before right. +O1HEAP_PRIVATE void interlink(Fragment *const left, Fragment *const right); +O1HEAP_PRIVATE void interlink(Fragment *const left, Fragment *const right) +{ + if (O1HEAP_LIKELY(left != NULL)) { + left->header.next = right; + } + + if (O1HEAP_LIKELY(right != NULL)) { + right->header.prev = left; + } +} + +/// Adds a new block into the appropriate bin and updates the lookup mask. +O1HEAP_PRIVATE void rebin(O1HeapInstance *const handle, Fragment *const fragment); +O1HEAP_PRIVATE void rebin(O1HeapInstance *const handle, Fragment *const fragment) +{ + O1HEAP_ASSERT(handle != NULL); + O1HEAP_ASSERT(fragment != NULL); + O1HEAP_ASSERT(fragment->header.size >= FRAGMENT_SIZE_MIN); + O1HEAP_ASSERT((fragment->header.size % FRAGMENT_SIZE_MIN) == 0U); + const uint8_t idx = log2Floor(fragment->header.size / FRAGMENT_SIZE_MIN); // Round DOWN when inserting. + O1HEAP_ASSERT(idx < NUM_BINS_MAX); + // Add the new fragment to the beginning of the bin list. + // I.e., each allocation will be returning the least-recently-used fragment -- good for caching. + fragment->next_free = handle->bins[idx]; + fragment->prev_free = NULL; + + if (O1HEAP_LIKELY(handle->bins[idx] != NULL)) { + handle->bins[idx]->prev_free = fragment; + } + + handle->bins[idx] = fragment; + handle->nonempty_bin_mask |= pow2(idx); +} + +/// Removes the specified block from its bin. +O1HEAP_PRIVATE void unbin(O1HeapInstance *const handle, const Fragment *const fragment); +O1HEAP_PRIVATE void unbin(O1HeapInstance *const handle, const Fragment *const fragment) +{ + O1HEAP_ASSERT(handle != NULL); + O1HEAP_ASSERT(fragment != NULL); + O1HEAP_ASSERT(fragment->header.size >= FRAGMENT_SIZE_MIN); + O1HEAP_ASSERT((fragment->header.size % FRAGMENT_SIZE_MIN) == 0U); + const uint8_t idx = log2Floor(fragment->header.size / FRAGMENT_SIZE_MIN); // Round DOWN when removing. + O1HEAP_ASSERT(idx < NUM_BINS_MAX); + + // Remove the bin from the free fragment list. + if (O1HEAP_LIKELY(fragment->next_free != NULL)) { + fragment->next_free->prev_free = fragment->prev_free; + } + + if (O1HEAP_LIKELY(fragment->prev_free != NULL)) { + fragment->prev_free->next_free = fragment->next_free; + } + + // Update the bin header. + if (O1HEAP_LIKELY(handle->bins[idx] == fragment)) { + O1HEAP_ASSERT(fragment->prev_free == NULL); + handle->bins[idx] = fragment->next_free; + + if (O1HEAP_LIKELY(handle->bins[idx] == NULL)) { + handle->nonempty_bin_mask &= ~pow2(idx); + } + } +} + +// ---------------------------------------- PUBLIC API IMPLEMENTATION ---------------------------------------- + +O1HeapInstance *o1heapInit(void *const base, + const size_t size, + const O1HeapHook critical_section_enter, + const O1HeapHook critical_section_leave) +{ + O1HeapInstance *out = NULL; + + if ((base != NULL) && ((((size_t) base) % O1HEAP_ALIGNMENT) == 0U) && + (size >= (INSTANCE_SIZE_PADDED + FRAGMENT_SIZE_MIN))) { + // Allocate the core heap metadata structure in the beginning of the arena. + O1HEAP_ASSERT(((size_t) base) % sizeof(O1HeapInstance *) == 0U); + out = (O1HeapInstance *) base; + out->nonempty_bin_mask = 0U; + out->critical_section_enter = critical_section_enter; + out->critical_section_leave = critical_section_leave; + + for (size_t i = 0; i < NUM_BINS_MAX; i++) { + out->bins[i] = NULL; + } + + // Limit and align the capacity. + size_t capacity = size - INSTANCE_SIZE_PADDED; + + if (capacity > FRAGMENT_SIZE_MAX) { + capacity = FRAGMENT_SIZE_MAX; + } + + while ((capacity % FRAGMENT_SIZE_MIN) != 0) { + O1HEAP_ASSERT(capacity > 0U); + capacity--; + } + + O1HEAP_ASSERT((capacity % FRAGMENT_SIZE_MIN) == 0); + O1HEAP_ASSERT((capacity >= FRAGMENT_SIZE_MIN) && (capacity <= FRAGMENT_SIZE_MAX)); + + // Initialize the root fragment. + Fragment *const frag = (Fragment *)(void *)(((uint8_t *) base) + INSTANCE_SIZE_PADDED); + O1HEAP_ASSERT((((size_t) frag) % O1HEAP_ALIGNMENT) == 0U); + frag->header.next = NULL; + frag->header.prev = NULL; + frag->header.size = capacity; + frag->header.used = false; + frag->next_free = NULL; + frag->prev_free = NULL; + rebin(out, frag); + O1HEAP_ASSERT(out->nonempty_bin_mask != 0U); + + // Initialize the diagnostics. + out->diagnostics.capacity = capacity; + out->diagnostics.allocated = 0U; + out->diagnostics.peak_allocated = 0U; + out->diagnostics.peak_request_size = 0U; + out->diagnostics.oom_count = 0U; + } + + return out; +} + +void *o1heapAllocate(O1HeapInstance *const handle, const size_t amount) +{ + O1HEAP_ASSERT(handle != NULL); + O1HEAP_ASSERT(handle->diagnostics.capacity <= FRAGMENT_SIZE_MAX); + void *out = NULL; + + // If the amount approaches approx. SIZE_MAX/2, an undetected integer overflow may occur. + // To avoid that, we do not attempt allocation if the amount exceeds the hard limit. + // We perform multiple redundant checks to account for a possible unaccounted overflow. + if (O1HEAP_LIKELY((amount > 0U) && (amount <= (handle->diagnostics.capacity - O1HEAP_ALIGNMENT)))) { + // Add the header size and align the allocation size to the power of 2. + // See "Timing-Predictable Memory Allocation In Hard Real-Time Systems", Herter, page 27. + const size_t fragment_size = pow2(log2Ceil(amount + O1HEAP_ALIGNMENT)); + O1HEAP_ASSERT(fragment_size <= FRAGMENT_SIZE_MAX); + O1HEAP_ASSERT(fragment_size >= FRAGMENT_SIZE_MIN); + O1HEAP_ASSERT(fragment_size >= amount + O1HEAP_ALIGNMENT); + O1HEAP_ASSERT(isPowerOf2(fragment_size)); + + const uint8_t optimal_bin_index = log2Ceil(fragment_size / FRAGMENT_SIZE_MIN); // Use CEIL when fetching. + O1HEAP_ASSERT(optimal_bin_index < NUM_BINS_MAX); + const size_t candidate_bin_mask = ~(pow2(optimal_bin_index) - 1U); + + invoke(handle->critical_section_enter); + + // Find the smallest non-empty bin we can use. + const size_t suitable_bins = handle->nonempty_bin_mask & candidate_bin_mask; + const size_t smallest_bin_mask = suitable_bins & ~(suitable_bins - 1U); // Clear all bits but the lowest. + + if (O1HEAP_LIKELY(smallest_bin_mask != 0)) { + O1HEAP_ASSERT(isPowerOf2(smallest_bin_mask)); + const uint8_t bin_index = log2Floor(smallest_bin_mask); + O1HEAP_ASSERT(bin_index >= optimal_bin_index); + O1HEAP_ASSERT(bin_index < NUM_BINS_MAX); + + // The bin we found shall not be empty, otherwise it's a state divergence (memory corruption?). + Fragment *const frag = handle->bins[bin_index]; + O1HEAP_ASSERT(frag != NULL); + O1HEAP_ASSERT(frag->header.size >= fragment_size); + O1HEAP_ASSERT((frag->header.size % FRAGMENT_SIZE_MIN) == 0U); + O1HEAP_ASSERT(!frag->header.used); + unbin(handle, frag); + + // Split the fragment if it is too large. + const size_t leftover = frag->header.size - fragment_size; + frag->header.size = fragment_size; + O1HEAP_ASSERT(leftover < handle->diagnostics.capacity); // Overflow check. + O1HEAP_ASSERT(leftover % FRAGMENT_SIZE_MIN == 0U); // Alignment check. + + if (O1HEAP_LIKELY(leftover >= FRAGMENT_SIZE_MIN)) { + Fragment *const new_frag = (Fragment *)(void *)(((uint8_t *) frag) + fragment_size); + O1HEAP_ASSERT(((size_t) new_frag) % O1HEAP_ALIGNMENT == 0U); + new_frag->header.size = leftover; + new_frag->header.used = false; + interlink(new_frag, frag->header.next); + interlink(frag, new_frag); + rebin(handle, new_frag); + } + + // Update the diagnostics. + O1HEAP_ASSERT((handle->diagnostics.allocated % FRAGMENT_SIZE_MIN) == 0U); + handle->diagnostics.allocated += fragment_size; + O1HEAP_ASSERT(handle->diagnostics.allocated <= handle->diagnostics.capacity); + + if (O1HEAP_LIKELY(handle->diagnostics.peak_allocated < handle->diagnostics.allocated)) { + handle->diagnostics.peak_allocated = handle->diagnostics.allocated; + } + + // Finalize the fragment we just allocated. + O1HEAP_ASSERT(frag->header.size >= amount + O1HEAP_ALIGNMENT); + frag->header.used = true; + + out = ((uint8_t *) frag) + O1HEAP_ALIGNMENT; + } + + } else { + invoke(handle->critical_section_enter); + } + + // Update the diagnostics. + if (O1HEAP_LIKELY(handle->diagnostics.peak_request_size < amount)) { + handle->diagnostics.peak_request_size = amount; + } + + if (O1HEAP_LIKELY((out == NULL) && (amount > 0U))) { + handle->diagnostics.oom_count++; + } + + invoke(handle->critical_section_leave); + return out; +} + +void o1heapFree(O1HeapInstance *const handle, void *const pointer) +{ + O1HEAP_ASSERT(handle != NULL); + O1HEAP_ASSERT(handle->diagnostics.capacity <= FRAGMENT_SIZE_MAX); + + if (O1HEAP_LIKELY(pointer != NULL)) { // NULL pointer is a no-op. + Fragment *const frag = (Fragment *)(void *)(((uint8_t *) pointer) - O1HEAP_ALIGNMENT); + + // Check for heap corruption in debug builds. + O1HEAP_ASSERT(((size_t) frag) % sizeof(Fragment *) == 0U); + O1HEAP_ASSERT(((size_t) frag) >= (((size_t) handle) + INSTANCE_SIZE_PADDED)); + O1HEAP_ASSERT(((size_t) frag) <= + (((size_t) handle) + INSTANCE_SIZE_PADDED + handle->diagnostics.capacity - FRAGMENT_SIZE_MIN)); + O1HEAP_ASSERT(frag->header.used); // Catch double-free + O1HEAP_ASSERT(((size_t) frag->header.next) % sizeof(Fragment *) == 0U); + O1HEAP_ASSERT(((size_t) frag->header.prev) % sizeof(Fragment *) == 0U); + O1HEAP_ASSERT(frag->header.size >= FRAGMENT_SIZE_MIN); + O1HEAP_ASSERT(frag->header.size <= handle->diagnostics.capacity); + O1HEAP_ASSERT((frag->header.size % FRAGMENT_SIZE_MIN) == 0U); + + invoke(handle->critical_section_enter); + + // Even if we're going to drop the fragment later, mark it free anyway to prevent double-free. + frag->header.used = false; + + // Update the diagnostics. It must be done before merging because it invalidates the fragment size information. + O1HEAP_ASSERT(handle->diagnostics.allocated >= frag->header.size); // Heap corruption check. + handle->diagnostics.allocated -= frag->header.size; + + // Merge with siblings and insert the returned fragment into the appropriate bin and update metadata. + Fragment *const prev = frag->header.prev; + Fragment *const next = frag->header.next; + const bool join_left = (prev != NULL) && (!prev->header.used); + const bool join_right = (next != NULL) && (!next->header.used); + + if (join_left && join_right) { // [ prev ][ this ][ next ] => [ ------- prev ------- ] + unbin(handle, prev); + unbin(handle, next); + prev->header.size += frag->header.size + next->header.size; + frag->header.size = 0; // Invalidate the dropped fragment headers to prevent double-free. + next->header.size = 0; + O1HEAP_ASSERT((prev->header.size % FRAGMENT_SIZE_MIN) == 0U); + interlink(prev, next->header.next); + rebin(handle, prev); + + } else if (join_left) { // [ prev ][ this ][ next ] => [ --- prev --- ][ next ] + unbin(handle, prev); + prev->header.size += frag->header.size; + frag->header.size = 0; + O1HEAP_ASSERT((prev->header.size % FRAGMENT_SIZE_MIN) == 0U); + interlink(prev, next); + rebin(handle, prev); + + } else if (join_right) { // [ prev ][ this ][ next ] => [ prev ][ --- this --- ] + unbin(handle, next); + frag->header.size += next->header.size; + next->header.size = 0; + O1HEAP_ASSERT((frag->header.size % FRAGMENT_SIZE_MIN) == 0U); + interlink(frag, next->header.next); + rebin(handle, frag); + + } else { + rebin(handle, frag); + } + + invoke(handle->critical_section_leave); + } +} + +bool o1heapDoInvariantsHold(const O1HeapInstance *const handle) +{ + O1HEAP_ASSERT(handle != NULL); + bool valid = true; + + invoke(handle->critical_section_enter); + + // Check the bin mask consistency. + for (size_t i = 0; i < NUM_BINS_MAX; i++) { // Dear compiler, feel free to unroll this loop. + const bool mask_bit_set = (handle->nonempty_bin_mask & pow2((uint8_t) i)) != 0U; + const bool bin_nonempty = handle->bins[i] != NULL; + valid = valid && (mask_bit_set == bin_nonempty); + } + + // Create a local copy of the diagnostics struct to check later and release the critical section early. + const O1HeapDiagnostics diag = handle->diagnostics; + + invoke(handle->critical_section_leave); + + // Capacity check. + valid = valid && (diag.capacity <= FRAGMENT_SIZE_MAX) && (diag.capacity >= FRAGMENT_SIZE_MIN) && + ((diag.capacity % FRAGMENT_SIZE_MIN) == 0U); + + // Allocation info check. + valid = valid && (diag.allocated <= diag.capacity) && ((diag.allocated % FRAGMENT_SIZE_MIN) == 0U) && + (diag.peak_allocated <= diag.capacity) && (diag.peak_allocated >= diag.allocated) && + ((diag.peak_allocated % FRAGMENT_SIZE_MIN) == 0U); + + // Peak request check + valid = valid && ((diag.peak_request_size < diag.capacity) || (diag.oom_count > 0U)); + + if (diag.peak_request_size == 0U) { + valid = valid && (diag.peak_allocated == 0U) && (diag.allocated == 0U) && (diag.oom_count == 0U); + + } else { + valid = valid && // Overflow on summation is possible but safe to ignore. + (((diag.peak_request_size + O1HEAP_ALIGNMENT) <= diag.peak_allocated) || (diag.oom_count > 0U)); + } + + return valid; +} + +O1HeapDiagnostics o1heapGetDiagnostics(const O1HeapInstance *const handle) +{ + O1HEAP_ASSERT(handle != NULL); + invoke(handle->critical_section_enter); + const O1HeapDiagnostics out = handle->diagnostics; + invoke(handle->critical_section_leave); + return out; +} diff --git a/src/drivers/uavcannode_gps_demo/o1heap.h b/src/drivers/uavcannode_gps_demo/o1heap.h new file mode 100644 index 0000000000..4f095495a0 --- /dev/null +++ b/src/drivers/uavcannode_gps_demo/o1heap.h @@ -0,0 +1,142 @@ +// Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated +// documentation files (the "Software"), to deal in the Software without restriction, including without limitation +// the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, +// and to permit persons to whom the Software is furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all copies or substantial portions +// of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE +// WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS +// OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +// OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. +// +// Copyright (c) 2020 Pavel Kirienko +// Authors: Pavel Kirienko +// +// READ THE DOCUMENTATION IN README.md. + +#ifndef O1HEAP_H_INCLUDED +#define O1HEAP_H_INCLUDED + +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/// The semantic version number of this distribution. +#define O1HEAP_VERSION_MAJOR 1 + +/// The guaranteed alignment depends on the platform pointer width. +#define O1HEAP_ALIGNMENT (sizeof(void*) * 4U) + +/// The definition is private, so the user code can only operate on pointers. This is done to enforce encapsulation. +typedef struct O1HeapInstance O1HeapInstance; + +/// A hook function invoked by the allocator. NULL hooks are silently not invoked (not an error). +typedef void (*O1HeapHook)(void); + +/// Runtime diagnostic information. This information can be used to facilitate runtime self-testing, +/// as required by certain safety-critical development guidelines. +/// If assertion checks are not disabled, the library will perform automatic runtime self-diagnostics that trigger +/// an assertion failure if a heap corruption is detected. +/// Health checks and validation can be done with @ref o1heapDoInvariantsHold(). +typedef struct { + /// The total amount of memory available for serving allocation requests (heap size). + /// The maximum allocation size is (capacity - O1HEAP_ALIGNMENT). + /// This parameter does not include the overhead used up by @ref O1HeapInstance and arena alignment. + /// This parameter is constant. + size_t capacity; + + /// The amount of memory that is currently allocated, including the per-fragment overhead and size alignment. + /// For example, if the application requested a fragment of size 1 byte, the value reported here may be 32 bytes. + size_t allocated; + + /// The maximum value of 'allocated' seen since initialization. This parameter is never decreased. + size_t peak_allocated; + + /// The largest amount of memory that the allocator has attempted to allocate (perhaps unsuccessfully) + /// since initialization (not including the rounding and the allocator's own per-fragment overhead, + /// so the total is larger). This parameter is never decreased. The initial value is zero. + size_t peak_request_size; + + /// The number of times an allocation request could not be completed due to the lack of memory or + /// excessive fragmentation. OOM stands for "out of memory". This parameter is never decreased. + uint64_t oom_count; +} O1HeapDiagnostics; + +/// The arena base pointer shall be aligned at @ref O1HEAP_ALIGNMENT, otherwise NULL is returned. +/// +/// The total heap capacity cannot exceed approx. (SIZE_MAX/2). If the arena size allows for a larger heap, +/// the excess will be silently truncated away (no error). This is not a realistic use case because a typical +/// application is unlikely to be able to dedicate that much of the address space for the heap. +/// +/// The critical section enter/leave callbacks will be invoked when the allocator performs an atomic transaction. +/// There is at most one atomic transaction per allocation/deallocation. +/// Either or both of the callbacks may be NULL if locking is not needed (i.e., the heap is not shared). +/// It is guaranteed that a critical section will never be entered recursively. +/// It is guaranteed that 'enter' is invoked the same number of times as 'leave', unless either of them are NULL. +/// It is guaranteed that 'enter' is invoked before 'leave', unless either of them are NULL. +/// The callbacks are never invoked from the initialization function itself. +/// +/// The function initializes a new heap instance allocated in the provided arena, taking some of its space for its +/// own needs (normally about 40..600 bytes depending on the architecture, but this parameter is not characterized). +/// A pointer to the newly initialized instance is returned. +/// +/// If the provided space is insufficient, NULL is returned. +/// +/// An initialized instance does not hold any resources. Therefore, if the instance is no longer needed, +/// it can be discarded without any de-initialization procedures. +/// +/// The time complexity is unspecified. +O1HeapInstance *o1heapInit(void *const base, + const size_t size, + const O1HeapHook critical_section_enter, + const O1HeapHook critical_section_leave); + +/// The semantics follows malloc() with additional guarantees the full list of which is provided below. +/// +/// If the allocation request is served successfully, a pointer to the newly allocated memory fragment is returned. +/// The returned pointer is guaranteed to be aligned at @ref O1HEAP_ALIGNMENT. +/// +/// If the allocation request cannot be served due to the lack of memory or its excessive fragmentation, +/// a NULL pointer is returned. +/// +/// The function is executed in constant time (unless the critical section management hooks are used and are not +/// constant-time). The allocated memory is NOT zero-filled (because zero-filling is a variable-complexity operation). +/// +/// The function may invoke critical_section_enter and critical_section_leave at most once each (NULL hooks ignored). +void *o1heapAllocate(O1HeapInstance *const handle, const size_t amount); + +/// The semantics follows free() with additional guarantees the full list of which is provided below. +/// +/// If the pointer does not point to a previously allocated block and is not NULL, the behavior is undefined. +/// Builds where assertion checks are enabled may trigger an assertion failure for some invalid inputs. +/// +/// The function is executed in constant time (unless the critical section management hooks are used and are not +/// constant-time). +/// +/// The function may invoke critical_section_enter and critical_section_leave at most once each (NULL hooks ignored). +void o1heapFree(O1HeapInstance *const handle, void *const pointer); + +/// Performs a basic sanity check on the heap. +/// This function can be used as a weak but fast method of heap corruption detection. +/// It invokes critical_section_enter once (unless NULL) and then critical_section_leave once (unless NULL). +/// If the handle pointer is NULL, the behavior is undefined. +/// The time complexity is constant. +/// The return value is truth if the heap looks valid, falsity otherwise. +bool o1heapDoInvariantsHold(const O1HeapInstance *const handle); + +/// Samples and returns a copy of the diagnostic information, see @ref O1HeapDiagnostics. +/// This function merely copies the structure from an internal storage, so it is fast to return. +/// It invokes critical_section_enter once (unless NULL) and then critical_section_leave once (unless NULL). +/// If the handle pointer is NULL, the behavior is undefined. +O1HeapDiagnostics o1heapGetDiagnostics(const O1HeapInstance *const handle); + +#ifdef __cplusplus +} +#endif +#endif // O1HEAP_H_INCLUDED diff --git a/src/drivers/uavcannode_gps_demo/public_regulated_data_types b/src/drivers/uavcannode_gps_demo/public_regulated_data_types new file mode 160000 index 0000000000..7f5489e6e9 --- /dev/null +++ b/src/drivers/uavcannode_gps_demo/public_regulated_data_types @@ -0,0 +1 @@ +Subproject commit 7f5489e6e916cc8b13db0582dcf9930e225594b9 diff --git a/src/drivers/uavcannode_gps_demo/socketcan.c b/src/drivers/uavcannode_gps_demo/socketcan.c new file mode 100644 index 0000000000..0176155387 --- /dev/null +++ b/src/drivers/uavcannode_gps_demo/socketcan.c @@ -0,0 +1,171 @@ +#include "socketcan.h" + +#include +#include +#include + +int16_t socketcanOpen(CanardSocketInstance *ins, const char *const can_iface_name, const bool can_fd) +{ + struct sockaddr_can addr; + struct ifreq ifr; + + ins->can_fd = can_fd; + + /* open socket */ + if ((ins->s = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) { + perror("socket"); + return -1; + } + + strncpy(ifr.ifr_name, can_iface_name, IFNAMSIZ - 1); + ifr.ifr_name[IFNAMSIZ - 1] = '\0'; + ifr.ifr_ifindex = if_nametoindex(ifr.ifr_name); + + if (!ifr.ifr_ifindex) { + perror("if_nametoindex"); + return -1; + } + + memset(&addr, 0, sizeof(addr)); + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + + const int on = 1; + /* RX Timestamping */ + + if (setsockopt(ins->s, SOL_SOCKET, SO_TIMESTAMP, &on, sizeof(on)) < 0) { + perror("SO_TIMESTAMP is disabled"); + return -1; + } + + /* NuttX Feature: Enable TX deadline when sending CAN frames + * When a deadline occurs the driver will remove the CAN frame + */ + + if (setsockopt(ins->s, SOL_CAN_RAW, CAN_RAW_TX_DEADLINE, &on, sizeof(on)) < 0) { + perror("CAN_RAW_TX_DEADLINE is disabled"); + return -1; + } + + if (can_fd) { + if (setsockopt(ins->s, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &on, sizeof(on)) < 0) { + perror("no CAN FD support"); + return -1; + } + } + + if (bind(ins->s, (struct sockaddr *)&addr, sizeof(addr)) < 0) { + perror("bind"); + return -1; + } + + // Setup TX msg + ins->send_iov.iov_base = &ins->send_frame; + + if (ins->can_fd) { + ins->send_iov.iov_len = sizeof(struct canfd_frame); + + } else { + ins->send_iov.iov_len = sizeof(struct can_frame); + } + + memset(&ins->send_control, 0x00, sizeof(ins->send_control)); + + ins->send_msg.msg_iov = &ins->send_iov; + ins->send_msg.msg_iovlen = 1; + ins->send_msg.msg_control = &ins->send_control; + ins->send_msg.msg_controllen = sizeof(ins->send_control); + + ins->send_cmsg = CMSG_FIRSTHDR(&ins->send_msg); + ins->send_cmsg->cmsg_level = SOL_CAN_RAW; + ins->send_cmsg->cmsg_type = CAN_RAW_TX_DEADLINE; + ins->send_cmsg->cmsg_len = sizeof(struct timeval); + ins->send_tv = (struct timeval *)CMSG_DATA(&ins->send_cmsg); + + // Setup RX msg + ins->recv_iov.iov_base = &ins->recv_frame; + + if (can_fd) { + ins->recv_iov.iov_len = sizeof(struct canfd_frame); + + } else { + ins->recv_iov.iov_len = sizeof(struct can_frame); + } + + memset(&ins->recv_control, 0x00, sizeof(ins->recv_control)); + + ins->recv_msg.msg_iov = &ins->recv_iov; + ins->recv_msg.msg_iovlen = 1; + ins->recv_msg.msg_control = &ins->recv_control; + ins->recv_msg.msg_controllen = sizeof(ins->recv_control); + ins->recv_cmsg = CMSG_FIRSTHDR(&ins->recv_msg); + + return 0; +} + +int16_t socketcanTransmit(CanardSocketInstance *ins, const CanardFrame *txf) +{ + /* Copy CanardFrame to can_frame/canfd_frame */ + + if (ins->can_fd) { + ins->send_frame.can_id = txf->extended_can_id; + ins->send_frame.can_id |= CAN_EFF_FLAG; + ins->send_frame.len = txf->payload_size; + memcpy(&ins->send_frame.data, txf->payload, txf->payload_size); + + } else { + struct can_frame *frame = (struct can_frame *)&ins->send_frame; + frame->can_id = txf->extended_can_id; + frame->can_id |= CAN_EFF_FLAG; + frame->can_dlc = txf->payload_size; + memcpy(&frame->data, txf->payload, txf->payload_size); + } + + /* Set CAN_RAW_TX_DEADLINE timestamp */ + + ins->send_tv->tv_usec = txf->timestamp_usec % 1000000ULL; + ins->send_tv->tv_sec = (txf->timestamp_usec - ins->send_tv->tv_usec) / 1000000ULL; + + return sendmsg(ins->s, &ins->send_msg, 0); +} + +int16_t socketcanReceive(CanardSocketInstance *ins, CanardFrame *rxf) +{ + int32_t result = recvmsg(ins->s, &ins->recv_msg, 0); + + if (result < 0) { + return result; + } + + /* Copy CAN frame to CanardFrame */ + + if (ins->can_fd) { + struct canfd_frame *recv_frame = (struct canfd_frame *)&ins->recv_frame; + rxf->extended_can_id = recv_frame->can_id & CAN_EFF_MASK; + rxf->payload_size = recv_frame->len; + rxf->payload = &recv_frame->data; + + } else { + struct can_frame *recv_frame = (struct can_frame *)&ins->recv_frame; + rxf->extended_can_id = recv_frame->can_id & CAN_EFF_MASK; + rxf->payload_size = recv_frame->can_dlc; + rxf->payload = &recv_frame->data; //FIXME either copy or clearly state the pointer reference + } + + /* Read SO_TIMESTAMP value */ + + if (ins->recv_cmsg->cmsg_level == SOL_SOCKET && ins->recv_cmsg->cmsg_type == SO_TIMESTAMP) { + struct timeval *tv = (struct timeval *)CMSG_DATA(ins->recv_cmsg); + rxf->timestamp_usec = tv->tv_sec * 1000000ULL + tv->tv_usec; + } + + return result; +} + + +/* TODO implement corresponding IOCTL */ + +int16_t socketcanConfigureFilter(const fd_t fd, const size_t num_filters, const struct can_filter *filters) +{ + return -1; +} diff --git a/src/drivers/uavcannode_gps_demo/socketcan.h b/src/drivers/uavcannode_gps_demo/socketcan.h new file mode 100644 index 0000000000..64db4323bb --- /dev/null +++ b/src/drivers/uavcannode_gps_demo/socketcan.h @@ -0,0 +1,69 @@ + +#ifndef SOCKETCAN_H_INCLUDED +#define SOCKETCAN_H_INCLUDED + +#include +#include +#include + +#include +#include + +#include +#include + +#include + + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct CanardSocketInstance CanardSocketInstance; +typedef int fd_t; + +struct CanardSocketInstance { + fd_t s; + bool can_fd; + + //// Send msg structure + struct iovec send_iov; + struct canfd_frame send_frame; + struct msghdr send_msg; + struct cmsghdr *send_cmsg; + struct timeval *send_tv; /* TX deadline timestamp */ + uint8_t send_control[sizeof(struct cmsghdr) + sizeof(struct timeval)]; + + //// Receive msg structure + struct iovec recv_iov; + struct canfd_frame recv_frame; + struct msghdr recv_msg; + struct cmsghdr *recv_cmsg; + uint8_t recv_control[sizeof(struct cmsghdr) + sizeof(struct timeval)]; +}; + +/// Creates a SocketCAN socket for corresponding iface can_iface_name +/// Also sets up the message structures required for socketcanTransmit & socketcanReceive +/// can_fd determines to use CAN FD frame when is 1, and classical CAN frame when is 0 +/// The return value is 0 on succes and -1 on error +int16_t socketcanOpen(CanardSocketInstance *ins, const char *const can_iface_name, const bool can_fd); + +/// Send a CanardFrame to the CanardSocketInstance socket +/// This function is blocking +/// The return value is number of bytes transferred, negative value on error. +int16_t socketcanTransmit(CanardSocketInstance *ins, const CanardFrame *txf); + +/// Receive a CanardFrame from the CanardSocketInstance socket +/// This function is blocking +/// The return value is number of bytes received, negative value on error. +int16_t socketcanReceive(CanardSocketInstance *ins, CanardFrame *rxf); + +// TODO implement ioctl for CAN filter +int16_t socketcanConfigureFilter(const fd_t fd, const size_t num_filters, const struct can_filter *filters); + + +#ifdef __cplusplus +} +#endif + +#endif //SOCKETCAN_H_INCLUDED diff --git a/src/drivers/uavcannode_gps_demo/uorb_converter.c b/src/drivers/uavcannode_gps_demo/uorb_converter.c new file mode 100644 index 0000000000..bcf06537bc --- /dev/null +++ b/src/drivers/uavcannode_gps_demo/uorb_converter.c @@ -0,0 +1,107 @@ +/* + * uorb_converter.c + * + * Created on: Apr 10, 2020 + * Author: hovergames + */ + +#include + +#include "uorb_converter.h" + + +/**************************************************************************** + * Private Data + ****************************************************************************/ +int uorb_sub_fd; +px4_pollfd_struct_t fds[1]; +int error_counter; +CanardInstance *canard_ins; +int raw_uorb_message_transfer_id; +int fix_message_transfer_id; +int aux_message_transfer_id; +int16_t *gps_raw_uorb_port_id; +int16_t *gps_fix_port_id; +int16_t *gps_aux_port_id; + + +void uorbConverterInit(CanardInstance *ins, int16_t *raw_uorb_port_id, int16_t *fix_port_id, int16_t *aux_port_id) +{ + canard_ins = ins; + + gps_raw_uorb_port_id = raw_uorb_port_id; + gps_fix_port_id = fix_port_id; + gps_aux_port_id = aux_port_id; + + /* subscribe to the uORB topic */ + uorb_sub_fd = orb_subscribe(ORB_ID(sensor_gps)); + + orb_set_interval(uorb_sub_fd, 200); + + /* one could wait for multiple topics with this technique, just using one here */ + fds[0].fd = uorb_sub_fd; + fds[0].events = POLLIN; + + error_counter = 0; + raw_uorb_message_transfer_id = 0; + fix_message_transfer_id = 0; + aux_message_transfer_id = 0; +} + +void uorbProcessSub(int timeout_msec) +{ + /* wait for subscriber update of 1 file descriptor for timeout_msec ms */ + int poll_ret = px4_poll(fds, 1, timeout_msec); + + /* handle the poll result */ + if (poll_ret == 0) { + /* this means none of our providers is giving us data */ + } else if (poll_ret < 0) { + /* this is seriously bad - should be an emergency */ + if (error_counter < 10 || error_counter % 50 == 0) { + /* use a counter to prevent flooding (and slowing us down) */ + PX4_ERR("ERROR return value from poll(): %d", poll_ret); + } + + error_counter++; + + } else { + if (fds[0].revents & POLLIN) { + /* obtained data for the first file descriptor */ + struct sensor_gps_s raw; + /* copy sensors raw data into local buffer */ + orb_copy(ORB_ID(sensor_gps), uorb_sub_fd, &raw); + + CanardMicrosecond transmission_deadline = getMonotonicTimestampUSec() + (uint64_t)(1000ULL * 100ULL); + + // raw uORB sensor_gps message + if (*gps_raw_uorb_port_id != -1) { + + CanardTransfer transfer = { + .timestamp_usec = transmission_deadline, // Zero if transmission deadline is not limited. + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindMessage, + .port_id = *gps_raw_uorb_port_id, // This is the subject-ID. + .remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET. + .transfer_id = raw_uorb_message_transfer_id, + .payload_size = sizeof(struct sensor_gps_s), + .payload = &raw, + }; + + ++raw_uorb_message_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. + int32_t result = canardTxPush(canard_ins, &transfer); + + if (result < 0) { + // An error has occurred: either an argument is invalid or we've ran out of memory. + // It is possible to statically prove that an out-of-memory will never occur for a given application if the + // heap is sized correctly; for background, refer to the Robson's Proof and the documentation for O1Heap. + PX4_ERR("canardTxPush error %d", result); + } + } + + + PX4_INFO("Recv from uORB"); + } + } +} + diff --git a/src/drivers/uavcannode_gps_demo/uorb_converter.h b/src/drivers/uavcannode_gps_demo/uorb_converter.h new file mode 100644 index 0000000000..b246ca84e1 --- /dev/null +++ b/src/drivers/uavcannode_gps_demo/uorb_converter.h @@ -0,0 +1,34 @@ +/* + * uorb_converter.h + * + * Created on: Apr 10, 2020 + * Author: hovergames + */ + +#ifndef SRC_DRIVERS_UAVCANNODE_V1_UORB_CONVERTER_H_ +#define SRC_DRIVERS_UAVCANNODE_V1_UORB_CONVERTER_H_ + + +/* uORB */ +#include +#include +#include + +/* canard */ +#include +#include + +/* monotonic timestamp */ +#include "libcancl/time.h" + + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +void uorbConverterInit(CanardInstance *ins, int16_t *raw_uorb_port_id, int16_t *fix_port_id, int16_t *aux_port_id); + +void uorbProcessSub(int timeout_msec); + + +#endif /* SRC_DRIVERS_UAVCANNODE_V1_UORB_CONVERTER_H_ */