forked from Archive/PX4-Autopilot
UAVCAN v1 GPS demo with MAVCAN autoconfigure
This commit is contained in:
Peter van der Perk
Lorenz Meier
parent
5f8ad65c40
commit
8242968b2b
|
|
@ -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()
|
||||
|
|
|
|||
|
|
@ -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" ],
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
|
|
|||
|
|
@ -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: [
|
||||
|
|
|
|||
|
|
@ -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: [
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -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: [
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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')
|
||||
|
|
|
|||
|
|
@ -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'";
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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"
|
||||
|
|
@ -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
|
||||
)
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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"
|
||||
|
|
@ -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
|
||||
)
|
||||
Binary file not shown.
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -47,6 +47,7 @@
|
|||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/sensor_gps.h>
|
||||
|
||||
#include "o1heap/o1heap.h"
|
||||
|
||||
|
|
@ -67,25 +68,26 @@
|
|||
#include <uavcan/_register/Access_1_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 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_s> _battery_status_pub{ORB_ID(battery_status)};
|
||||
uORB::Publication<sensor_gps_s> _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};
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
Subproject commit d874225ea4b5a79a4639bcfcb9096ccd943138a9
|
||||
Subproject commit 81526eb6e27095701736dfb6e74842fe07a59f4f
|
||||
|
|
@ -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 <canard_dsdl.h>
|
||||
{{T.full_namespace | open_namespace}}
|
||||
|
||||
{%- block object -%}{%- endblock -%}
|
||||
|
||||
{{T.full_namespace | close_namespace}}
|
||||
#endif // {{T.full_name | c.macrofy}}
|
||||
|
|
@ -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 -%}
|
||||
|
|
@ -1,5 +0,0 @@
|
|||
{% extends "Header.j2" %}
|
||||
{%- block object -%}
|
||||
{% set composite_type = T -%}
|
||||
{% include '_composite_type.j2' %}
|
||||
{% endblock -%}
|
||||
|
|
@ -1,5 +0,0 @@
|
|||
{% extends "Header.j2" %}
|
||||
{%- block object -%}
|
||||
{% set composite_type = T -%}
|
||||
{% include '_composite_type.j2' %}
|
||||
{% endblock -%}
|
||||
|
|
@ -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 }}
|
||||
|
|
@ -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
|
||||
)
|
||||
|
|
@ -0,0 +1,448 @@
|
|||
/****************************************************************************
|
||||
* examples/canard/canard_main.c
|
||||
*
|
||||
* Copyright (C) 2016 ETH Zuerich. All rights reserved.
|
||||
* Author: Matthias Renner <rennerm@ethz.ch>
|
||||
*
|
||||
* 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 <canard.h>
|
||||
#include <canard_dsdl.h>
|
||||
|
||||
#include <sched.h>
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <time.h>
|
||||
#include <string.h>
|
||||
#include <assert.h>
|
||||
#include <errno.h>
|
||||
|
||||
#include <net/if.h>
|
||||
#include <sys/time.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/socket.h>
|
||||
|
||||
#include <poll.h>
|
||||
|
||||
#include <nuttx/can.h>
|
||||
#include <netpacket/can.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
|
|
@ -0,0 +1 @@
|
|||
Subproject commit cde670347425023480a1417fcd603b27c8eb06c1
|
||||
|
|
@ -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
|
||||
|
|
@ -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 <crc64.h>
|
||||
|
||||
#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);
|
||||
}
|
||||
|
|
@ -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 <canard.h>
|
||||
|
||||
#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_
|
||||
|
|
@ -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 <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
/****************************************************************************
|
||||
* 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;
|
||||
}
|
||||
|
|
@ -0,0 +1,49 @@
|
|||
|
||||
#ifndef UAVCAN_REGISTER_INTERFACE_H_
|
||||
#define UAVCAN_REGISTER_INTERFACE_H_
|
||||
|
||||
#include <canard.h>
|
||||
|
||||
#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_
|
||||
|
|
@ -0,0 +1,6 @@
|
|||
#ifndef MAVCAN_TIME_H_
|
||||
#define MAVCAN_TIME_H_
|
||||
|
||||
uint64_t getMonotonicTimestampUSec(void);
|
||||
|
||||
#endif //MAVCAN_TIME_H_
|
||||
|
|
@ -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 <pavel.kirienko@zubax.com>
|
||||
|
||||
#include "o1heap.h"
|
||||
#include <assert.h>
|
||||
|
||||
// ---------------------------------------- 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;
|
||||
}
|
||||
|
|
@ -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 <pavel.kirienko@zubax.com>
|
||||
//
|
||||
// READ THE DOCUMENTATION IN README.md.
|
||||
|
||||
#ifndef O1HEAP_H_INCLUDED
|
||||
#define O1HEAP_H_INCLUDED
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stddef.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#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
|
||||
|
|
@ -0,0 +1 @@
|
|||
Subproject commit 7f5489e6e916cc8b13db0582dcf9930e225594b9
|
||||
|
|
@ -0,0 +1,171 @@
|
|||
#include "socketcan.h"
|
||||
|
||||
#include <net/if.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <string.h>
|
||||
|
||||
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;
|
||||
}
|
||||
|
|
@ -0,0 +1,69 @@
|
|||
|
||||
#ifndef SOCKETCAN_H_INCLUDED
|
||||
#define SOCKETCAN_H_INCLUDED
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stddef.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include <sys/time.h>
|
||||
#include <sys/socket.h>
|
||||
|
||||
#include <nuttx/can.h>
|
||||
#include <netpacket/can.h>
|
||||
|
||||
#include <canard.h>
|
||||
|
||||
|
||||
#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
|
||||
|
|
@ -0,0 +1,107 @@
|
|||
/*
|
||||
* uorb_converter.c
|
||||
*
|
||||
* Created on: Apr 10, 2020
|
||||
* Author: hovergames
|
||||
*/
|
||||
|
||||
#include <string.h>
|
||||
|
||||
#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");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -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 <px4_platform_common/posix.h>
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/sensor_gps.h>
|
||||
|
||||
/* canard */
|
||||
#include <canard.h>
|
||||
#include <canard_dsdl.h>
|
||||
|
||||
/* 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_ */
|
||||
Loading…
Reference in New Issue