WIP: in tree Dockerfile and github actions push to github registry

2026-05-27 14:20:05 +08:00 · 2021-05-06 15:08:03 -04:00
44 changed files with 378 additions and 541 deletions
@@ -130,7 +130,7 @@ pipeline {
    // TODO: actually upload artifacts to S3
    // stage('S3 Upload') {
    //   agent {
-    //     docker { image 'px4io/px4-dev-base-focal:2021-04-29' }
+    //     docker { image 'ghcr.io/px4/px4-dev:2021-05-06' }
    //   }
    //   options {
    //         skipDefaultCheckout()
@@ -28,7 +28,7 @@ jobs:
          "parameters_metadata",
        ]
    container:
-      image: px4io/px4-dev-nuttx-focal:2021-04-29
+      image: ghcr.io/px4/px4-dev:2021-05-06
      options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
    steps:
    - uses: actions/checkout@v1
@@ -4,6 +4,7 @@ on:
  push:
    branches:
    - 'master'
+    - 'github-actions'
  pull_request:
    branches:
    - '*'
@@ -11,7 +12,7 @@ on:
 jobs:
  build:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-nuttx-focal:2021-04-29
+    container: ghcr.io/px4/px4-dev:2021-05-06
    strategy:
      matrix:
        config: [
@@ -11,7 +11,7 @@ on:
 jobs:
  build:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-nuttx-focal:2021-04-29
+    container: ghcr.io/px4/px4-dev:2021-05-06
    strategy:
      matrix:
        config: [
@@ -0,0 +1,31 @@
+name: Build and publish Docker image
+
+on:
+  push:
+    branches:
+    - 'master'
+    - 'github-actions'
+
+jobs:
+  docker:
+    runs-on: ubuntu-latest
+    steps:
+      - name: Login to GitHub Container Registry
+        uses: docker/login-action@v1
+        with:
+          registry: ghcr.io
+          username: ${{ github.repository_owner }}
+          password: ${{ secrets.GITHUB_TOKEN }}
+
+      - name: Build and push
+        uses: docker/build-push-action@v2
+        with:
+          context: .
+          platforms: linux/amd64
+          push: true
+          tags: |
+            px4/px4-dev:latest
+            ghcr.io/px4/px4-dev:latest
+
+      - name: Image digest
+        run: echo ${{ steps.docker_build.outputs.digest }}
@@ -11,7 +11,7 @@ jobs:

  airframe:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-04-29
+    container: ghcr.io/px4/px4-dev:2021-05-06
    steps:
    - uses: actions/checkout@v1
      with:
@@ -26,7 +26,7 @@ jobs:

  module:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-04-29
+    container: ghcr.io/px4/px4-dev:2021-05-06
    steps:
    - uses: actions/checkout@v1
      with:
@@ -41,7 +41,7 @@ jobs:

  parameter:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-04-29
+    container: ghcr.io/px4/px4-dev:2021-05-06
    steps:
    - uses: actions/checkout@v1
      with:
@@ -65,7 +65,7 @@ jobs:

  uorb_graph:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-nuttx-focal:2021-04-29
+    container: ghcr.io/px4/px4-dev:2021-05-06
    steps:
    - uses: actions/checkout@v1
      with:
@@ -80,7 +80,7 @@ jobs:

  micrortps_agent:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-04-29
+    container: ghcr.io/px4/px4-dev:2021-05-06
    steps:
    - uses: actions/checkout@v1
      with:
@@ -94,7 +94,7 @@ jobs:

  ROS_msgs:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-04-29
+    container: ghcr.io/px4/px4-dev:2021-05-06
    steps:
    - uses: actions/checkout@v1
      with:
@@ -107,7 +107,7 @@ jobs:

  ROS2_bridge:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-04-29
+    container: ghcr.io/px4/px4-dev:2021-05-06
    steps:
    - uses: actions/checkout@v1
      with:
@@ -0,0 +1,29 @@
+#
+# PX4 development environment
+#
+
+FROM ubuntu:20.04
+LABEL maintainer="Daniel Agar <daniel@agar.ca>"
+
+COPY Tools/setup/ubuntu.sh /tmp/ubuntu.sh
+COPY Tools/setup/requirements.txt /tmp/requirements.txt
+RUN DEBIAN_FRONTEND=noninteractive /tmp/ubuntu.sh --no-sim-tools \
+	&& apt-get -y autoremove \
+	&& apt-get clean autoclean \
+	&& rm -rf /var/lib/apt/lists/{apt,dpkg,cache,log} /tmp/* /var/tmp/*
+
+# create user with id 1001 (jenkins docker workflow default)
+RUN useradd --shell /bin/bash -u 1001 -c "" -m user && usermod -a -G dialout user
+
+ENV CCACHE_UMASK=000
+ENV PATH="/usr/lib/ccache:$PATH"
+
+# SITL UDP PORTS
+EXPOSE 14556/udp
+EXPOSE 14557/udp
+
+# create and start as LOCAL_USER_ID
+COPY Tools/setup/entrypoint.sh /usr/local/bin/entrypoint.sh
+ENTRYPOINT ["/usr/local/bin/entrypoint.sh"]
+
+CMD ["/bin/bash"]
@@ -85,7 +85,7 @@ pipeline {

        stage('Airframe') {
          agent {
-            docker { image 'px4io/px4-dev-base-focal:2021-04-29' }
+            docker { image 'ghcr.io/px4/px4-dev:2021-05-06' }
          }
          steps {
            sh 'make distclean'
@@ -105,7 +105,7 @@ pipeline {

        stage('Parameter') {
          agent {
-            docker { image 'px4io/px4-dev-base-focal:2021-04-29' }
+            docker { image 'ghcr.io/px4/px4-dev:2021-05-06' }
          }
          steps {
            sh 'make distclean'
@@ -125,7 +125,7 @@ pipeline {

        stage('Module') {
          agent {
-            docker { image 'px4io/px4-dev-base-focal:2021-04-29' }
+            docker { image 'ghcr.io/px4/px4-dev:2021-05-06' }
          }
          steps {
            sh 'make distclean'
@@ -176,7 +176,7 @@ pipeline {

        stage('Userguide') {
          agent {
-            docker { image 'px4io/px4-dev-base-focal:2021-04-29' }
+            docker { image 'ghcr.io/px4/px4-dev:2021-05-06' }
          }
          steps {
            sh('export')
@@ -206,7 +206,7 @@ pipeline {

        stage('QGroundControl') {
          agent {
-            docker { image 'px4io/px4-dev-base-focal:2021-04-29' }
+            docker { image 'ghcr.io/px4/px4-dev:2021-05-06' }
          }
          steps {
            sh('export')
@@ -234,7 +234,7 @@ pipeline {

        stage('microRTPS agent') {
          agent {
-            docker { image 'px4io/px4-dev-base-focal:2021-04-29' }
+            docker { image 'ghcr.io/px4/px4-dev:2021-05-06' }
          }
          steps {
            sh('export')
@@ -264,7 +264,7 @@ pipeline {

        stage('PX4 ROS msgs') {
          agent {
-            docker { image 'px4io/px4-dev-base-focal:2021-04-29' }
+            docker { image 'ghcr.io/px4/px4-dev:2021-05-06' }
          }
          steps {
            sh('export')
@@ -293,7 +293,7 @@ pipeline {

        stage('PX4 ROS2 bridge') {
          agent {
-            docker { image 'px4io/px4-dev-base-focal:2021-04-29' }
+            docker { image 'ghcr.io/px4/px4-dev:2021-05-06' }
          }
          steps {
            sh('export')
@@ -336,7 +336,7 @@ pipeline {

        stage('S3') {
          agent {
-            docker { image 'px4io/px4-dev-base-focal:2021-04-29' }
+            docker { image 'ghcr.io/px4/px4-dev:2021-05-06' }
          }
          steps {
            sh('export')
@@ -468,27 +468,26 @@ validate_module_configs:
 .PHONY: clean submodulesclean submodulesupdate gazeboclean distclean

 clean:
-	@find "$(SRC_DIR)/build" -mindepth 1 -maxdepth 1 -type d -exec sh -c "echo {}; cmake --build {} -- clean || rm -rf {}" \; # use generated build system to clean, wipe build directory if it fails
-	@git submodule foreach git clean -dX --force # some submodules generate build artifacts in source
+	@rm -rf "$(SRC_DIR)"/build
+	@git submodule foreach git clean -df

 submodulesclean:
 	@git submodule foreach --quiet --recursive git clean -ff -x -d
 	@git submodule update --quiet --init --recursive --force || true
 	@git submodule sync --recursive
-	@git submodule update --init --recursive --force --jobs 4
+	@git submodule update --init --recursive --force

 submodulesupdate:
-	@git submodule update --quiet --init --recursive --jobs 4 || true
+	@git submodule update --quiet --init --recursive || true
 	@git submodule sync --recursive
-	@git submodule update --init --recursive --jobs 4
+	@git submodule update --init --recursive

 gazeboclean:
 	@rm -rf ~/.gazebo/*

 distclean: gazeboclean
-	@git submodule deinit --force $(SRC_DIR)
-	@rm -rf "$(SRC_DIR)/build"
-	@git clean --force -X "$(SRC_DIR)/msg/" "$(SRC_DIR)/platforms/" "$(SRC_DIR)/posix-configs/" "$(SRC_DIR)/ROMFS/" "$(SRC_DIR)/src/" "$(SRC_DIR)/test/" "$(SRC_DIR)/Tools/"
+	@git submodule deinit -f .
+	@git clean -ff -x -d -e ".cproject" -e ".idea" -e ".project" -e ".settings" -e ".vscode"

 # Help / Error
 # --------------------------------------------------------------------
@@ -2,10 +2,7 @@

 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:2021-04-29"
-	elif [[ $@ =~ .*navio2.* ]] || [[ $@ =~ .*raspberry.* ]] || [[ $@ =~ .*beaglebone.* ]] || [[ $@ =~ .*pilotpi.default ]]; then
+	if [[ $@ =~ .*navio2.* ]] || [[ $@ =~ .*raspberry.* ]] || [[ $@ =~ .*beaglebone.* ]] || [[ $@ =~ .*pilotpi.default ]]; then
 		# beaglebone_blue_default, emlid_navio2_default, px4_raspberrypi_default, scumaker_pilotpi_default
 		PX4_DOCKER_REPO="px4io/px4-dev-armhf:2021-02-04"
 	elif [[ $@ =~ .*pilotpi.arm64 ]]; then
@@ -30,7 +27,7 @@ fi

 # otherwise default to nuttx
 if [ -z ${PX4_DOCKER_REPO+x} ]; then
-	PX4_DOCKER_REPO="px4io/px4-dev-nuttx-focal:2021-04-29"
+	PX4_DOCKER_REPO="ghcr.io/px4/px4-dev:2021-05-06"
 fi

 # docker hygiene
@@ -67,4 +64,5 @@ docker run -it --rm -w "${SRC_DIR}" \
 	--publish 14556:14556/udp \
 	--volume=${CCACHE_DIR}:${CCACHE_DIR}:rw \
 	--volume=${SRC_DIR}:${SRC_DIR}:rw \
+	-v /tmp/.X11-unix:/tmp/.X11-unix -e DISPLAY=$DISPLAY -h $HOSTNAME
 	${PX4_DOCKER_REPO} /bin/bash -c "$1 $2 $3"
@@ -0,0 +1,11 @@
+.PHONY: docker_build docker_push all
+
+DATE := $(shell date +%Y-%m-%d)
+
+docker_build:
+	docker build -f Dockerfile -t px4-dev:${DATE} .
+
+docker_push: build
+        docker push px4io/px4-dev:${DATE}
+
+all: docker_build
@@ -0,0 +1,27 @@
+#!/bin/bash
+
+# Start virtual X server in the background
+# - DISPLAY default is :99, set in dockerfile
+# - Users can override with `-e DISPLAY=` in `docker run` command to avoid
+#   running Xvfb and attach their screen
+if [[ -x "$(command -v Xvfb)" && "$DISPLAY" == ":99" ]]; then
+	echo "Starting Xvfb"
+	Xvfb :99 -screen 0 1600x1200x24+32 &
+fi
+
+# Check if the ROS_DISTRO is passed and use it
+# to source the ROS environment
+if [ -n "${ROS_DISTRO}" ]; then
+	source "/opt/ros/$ROS_DISTRO/setup.bash"
+fi
+
+# Use the LOCAL_USER_ID if passed in at runtime
+if [ -n "${LOCAL_USER_ID}" ]; then
+	echo "Starting with UID : $LOCAL_USER_ID"
+	# modify existing user's id
+	usermod -u $LOCAL_USER_ID user
+	# run as user
+	exec gosu user "$@"
+else
+	exec "$@"
+fi
@@ -1,24 +1,26 @@
 argcomplete
-argparse>=1.2
+argparse
 cerberus
 coverage
 empy>=3.3
 jinja2>=2.8
+jsonschema
+kconfiglib
 matplotlib>=3.0.*
 numpy>=1.13
 nunavut>=1.1.0
 packaging
-pandas>=0.21
 pkgconfig
 psutil
 pygments
-wheel>=0.31.1
 pymavlink
 pyros-genmsg
 pyserial>=3.0
 pyulog>=0.5.0
 pyyaml
 requests
+serial
 setuptools>=39.2.0
 six>=1.12.0
 toml>=0.9
+wheel>=0.31.1
@@ -31,10 +31,9 @@ done
 # detect if running in docker
 if [ -f /.dockerenv ]; then
 	echo "Running within docker, installing initial dependencies";
-	apt-get --quiet -y update && DEBIAN_FRONTEND=noninteractive apt-get --quiet -y install \
+	apt-get -qq update && DEBIAN_FRONTEND=noninteractive apt-get -q -y --no-install-recommends install \
 		ca-certificates \
-		gnupg \
-		lsb-core \
+		gosu \
 		sudo \
 		wget \
 		;
@@ -53,7 +52,7 @@ fi

 # check ubuntu version
 # otherwise warn and point to docker?
-UBUNTU_RELEASE="`lsb_release -rs`"
+UBUNTU_RELEASE=$(cat /etc/os-release | grep VERSION_ID | cut -d "\"" -f 2)

 if [[ "${UBUNTU_RELEASE}" == "14.04" ]]; then
 	echo "Ubuntu 14.04 is no longer supported"
@@ -63,16 +62,18 @@ elif [[ "${UBUNTU_RELEASE}" == "16.04" ]]; then
 	exit 1
 elif [[ "${UBUNTU_RELEASE}" == "18.04" ]]; then
 	echo "Ubuntu 18.04"
+
 elif [[ "${UBUNTU_RELEASE}" == "20.04" ]]; then
 	echo "Ubuntu 20.04"
+
 fi


 echo
 echo "Installing PX4 general dependencies"

-sudo apt-get update -y --quiet
-sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
+sudo apt-get -qq update
+sudo DEBIAN_FRONTEND=noninteractive apt-get -q -y --no-install-recommends install \
 	astyle \
 	build-essential \
 	ccache \
@@ -93,20 +94,23 @@ sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends i
 	python3-wheel \
 	rsync \
 	shellcheck \
-	unzip \
-	zip \
 	;

 if [[ "${UBUNTU_RELEASE}" == "16.04" ]]; then
 	echo "Installing Ubuntu 16.04 PX4-compatible ccache version"
-	wget -O /tmp/ccache_3.4.1-1_amd64.deb http://launchpadlibrarian.net/356662933/ccache_3.4.1-1_amd64.deb
+	wget -q -O /tmp/ccache_3.4.1-1_amd64.deb http://launchpadlibrarian.net/356662933/ccache_3.4.1-1_amd64.deb
 	sudo dpkg -i /tmp/ccache_3.4.1-1_amd64.deb
 fi

 # Python3 dependencies
 echo
 echo "Installing PX4 Python3 dependencies"
-python3 -m pip install --user -r ${DIR}/requirements.txt
+if [ -f /.dockerenv ]; then
+	# system wide for docker
+	python3 -m pip install -r ${DIR}/requirements.txt
+else
+	python3 -m pip install --user --quiet -r ${DIR}/requirements.txt
+fi

 # NuttX toolchain (arm-none-eabi-gcc)
 if [[ $INSTALL_NUTTX == "true" ]]; then
@@ -114,33 +118,13 @@ if [[ $INSTALL_NUTTX == "true" ]]; then
 	echo
 	echo "Installing NuttX dependencies"

-	sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
-		automake \
-		binutils-dev \
-		bison \
-		build-essential \
-		flex \
+	sudo DEBIAN_FRONTEND=noninteractive apt-get -q -y --no-install-recommends install \
 		g++-multilib \
 		gcc-multilib \
 		gdb-multiarch \
 		genromfs \
-		gettext \
-		gperf \
-		kconfig-frontends \
-		libelf-dev \
-		libexpat-dev \
-		libgmp-dev \
-		libisl-dev \
-		libmpc-dev \
-		libmpfr-dev \
-		libncurses5-dev \
-		libncursesw5-dev \
-		libtool \
 		pkg-config \
 		screen \
-		texinfo \
-		u-boot-tools \
-		util-linux \
 		vim-common \
 		;

@@ -164,7 +148,7 @@ if [[ $INSTALL_NUTTX == "true" ]]; then

 	else
 		echo "Installing arm-none-eabi-gcc-${NUTTX_GCC_VERSION}";
-		wget -O /tmp/gcc-arm-none-eabi-${NUTTX_GCC_VERSION}-linux.tar.bz2 https://armkeil.blob.core.windows.net/developer/Files/downloads/gnu-rm/${NUTTX_GCC_VERSION_SHORT}/gcc-arm-none-eabi-${NUTTX_GCC_VERSION}-${INSTALL_ARCH}-linux.tar.bz2 && \
+		wget -q -O /tmp/gcc-arm-none-eabi-${NUTTX_GCC_VERSION}-linux.tar.bz2 https://armkeil.blob.core.windows.net/developer/Files/downloads/gnu-rm/${NUTTX_GCC_VERSION_SHORT}/gcc-arm-none-eabi-${NUTTX_GCC_VERSION}-${INSTALL_ARCH}-linux.tar.bz2 && \
 			sudo tar -jxf /tmp/gcc-arm-none-eabi-${NUTTX_GCC_VERSION}-linux.tar.bz2 -C /opt/;

 		# add arm-none-eabi-gcc to user's PATH
@@ -185,37 +169,36 @@ if [[ $INSTALL_SIM == "true" ]]; then
 	echo "Installing PX4 simulation dependencies"

 	# General simulation dependencies
-	sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
+	sudo DEBIAN_FRONTEND=noninteractive apt-get -q -y --no-install-recommends install \
 		bc \
 		;

 	if [[ "${UBUNTU_RELEASE}" == "18.04" ]]; then
-		java_version=11
 		gazebo_version=9
+		MAVSDK_VERSION=0.39.0
+		wget -q "https://github.com/mavlink/MAVSDK/releases/download/v${MAVSDK_VERSION}/mavsdk_{MAVSDK_VERSION})_ubuntu18.04_amd64.deb" -O /tmp/mavsdk.deb && sudo dpkg -i /tmp/mavsdk.deb
 	elif [[ "${UBUNTU_RELEASE}" == "20.04" ]]; then
-		java_version=14
 		gazebo_version=11
+		MAVSDK_VERSION=0.39.0
+		wget -q "https://github.com/mavlink/MAVSDK/releases/download/v{MAVSDK_VERSION}/mavsdk_{MAVSDK_VERSION}_ubuntu20.04_amd64.deb" -O /tmp/mavsdk.deb && sudo dpkg -i /tmp/mavsdk.deb
 	else
-		java_version=14
 		gazebo_version=11
 	fi
+
 	# Java (jmavsim or fastrtps)
-	sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
+	sudo DEBIAN_FRONTEND=noninteractive apt-get -q -y --no-install-recommends install \
 		ant \
-		openjdk-$java_version-jre \
-		openjdk-$java_version-jdk \
+		default-jre-headless \
+		default-jdk-headless \
 		libvecmath-java \
 		;

-	# Set Java 11 as default
-	sudo update-alternatives --set java $(update-alternatives --list java | grep "java-$java_version")
-
 	# Gazebo
 	sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
-	wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
+	wget -q http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
 	# Update list, since new gazebo-stable.list has been added
-	sudo apt-get update -y --quiet
-	sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
+	sudo apt-get update -qq
+	sudo DEBIAN_FRONTEND=noninteractive apt-get -q -y --no-install-recommends install \
 		dmidecode \
 		gazebo$gazebo_version \
 		gstreamer1.0-plugins-bad \
@@ -63,7 +63,7 @@ CONFIG_HAVE_CXXINITIALIZE=y
 CONFIG_I2C_RESET=y
 CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_IOB_NBUFFERS=48
-CONFIG_IOB_NCHAINS=18
+CONFIG_IOB_NCHAINS=16
 CONFIG_KINETIS_ADC0=y
 CONFIG_KINETIS_ADC1=y
 CONFIG_KINETIS_CRC=y
@@ -82,7 +82,7 @@ CONFIG_I2C_RESET=y
 CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
-CONFIG_LIBC_STRERROR=n
+CONFIG_LIBC_STRERROR=y
 CONFIG_LIBC_STRERROR_SHORT=y
 CONFIG_MEMSET_64BIT=y
 CONFIG_MEMSET_OPTSPEED=y
@@ -126,7 +126,7 @@ int16_t CanardNuttXCDev::receive(CanardFrame *received_frame)
 	// Any recieved CAN messages will cause the poll statement to unblock and run
 	// This way CAN read runs with minimal latency.
 	// Note that multiple messages may be received in a short time, so this will try to read any availible in a loop
-	::poll(&fds, 1, 0);
+	::poll(&fds, 1, 10);

 	// Only execute this part if can0 is changed.
 	if (fds.revents & POLLIN) {
@@ -162,12 +162,12 @@ int16_t CanardSocketCAN::transmit(const CanardFrame &txf, int timeout_ms)
 	_send_tv->tv_usec = txf.timestamp_usec % 1000000ULL;
 	_send_tv->tv_sec = (txf.timestamp_usec - _send_tv->tv_usec) / 1000000ULL;

-	return sendmsg(_fd, &_send_msg, 0);
+	return sendmsg(_fd, &_send_msg, 1000);
 }

 int16_t CanardSocketCAN::receive(CanardFrame *rxf)
 {
-	int32_t result = recvmsg(_fd, &_recv_msg, MSG_DONTWAIT);
+	int32_t result = recvmsg(_fd, &_recv_msg, 1000);

 	if (result < 0) {
 		return result;
@@ -39,20 +39,16 @@
 * @author Peter van der Perk <peter.vanderperk@nxp.com>
 */

-#define RETRY_COUNT 10
-
 #include "NodeManager.hpp"

 bool NodeManager::HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg)
 {
 	if (msg.allocated_node_id.count == 0) {
-		uint32_t i;
-
 		msg.allocated_node_id.count = 1;
 		msg.allocated_node_id.elements[0].value = CANARD_NODE_ID_UNSET;

 		/* Search for an available NodeID to assign */
-		for (i = 1; i < sizeof(nodeid_registry) / sizeof(nodeid_registry[0]); i++) {
+		for (uint32_t i = 1; i < sizeof(nodeid_registry) / sizeof(nodeid_registry[0]); i++) {
 			if (i == _canard_instance.node_id) {
 				continue; // Don't give our NodeID to a node

@@ -67,10 +63,6 @@ bool NodeManager::HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg)
 			}
 		}

-		nodeid_registry[i].register_setup = false; // Re-instantiate register setup
-		nodeid_registry[i].register_index = 0;
-		nodeid_registry[i].retry_count = 0;
-
 		if (msg.allocated_node_id.elements[0].value != CANARD_NODE_ID_UNSET) {

 			PX4_INFO("Received NodeID allocation request assigning %i", msg.allocated_node_id.elements[0].value);
@@ -128,15 +120,17 @@ void NodeManager::HandleListResponse(CanardNodeID node_id, uavcan_register_List_
 			if (nodeid_registry[i].node_id == node_id) {
 				nodeid_registry[i].register_index++; // Increment index counter for next update()
 				nodeid_registry[i].register_setup = false;
-				nodeid_registry[i].retry_count = 0;
 			}
 		}

-		if (_access_request.setPortId(node_id, msg.name)) {
-			PX4_INFO("Set portID succesfull");

-		} else {
-			PX4_INFO("Register not found %.*s", msg.name.name.count, msg.name.name.elements);
+		if (strncmp((char *)msg.name.name.elements, gps_uorb_register_name,
+			    msg.name.name.count) == 0) {
+			_access_request.setPortId(node_id, gps_uorb_register_name, 1235); //TODO configurable and combine with ParamManager.
+
+		} else if (strncmp((char *)msg.name.name.elements, bms_status_register_name,
+				   msg.name.name.count) == 0) { //Battery status publisher
+			_access_request.setPortId(node_id, bms_status_register_name, 1234); //TODO configurable and combine with ParamManager.
 		}
 	}
 }
@@ -148,11 +142,7 @@ void NodeManager::update()
 			if (nodeid_registry[i].node_id != 0 && nodeid_registry[i].register_setup == false) {
 				//Setting up registers
 				_list_request.request(nodeid_registry[i].node_id, nodeid_registry[i].register_index);
-				nodeid_registry[i].retry_count++;
-
-				if (nodeid_registry[i].retry_count > RETRY_COUNT) {
-					nodeid_registry[i].register_setup = true; // Don't update anymore
-				}
+				nodeid_registry[i].register_setup = true;
 			}
 		}

@@ -63,14 +63,12 @@ typedef struct {
 	uint8_t   unique_id[16];
 	bool      register_setup;
 	uint16_t  register_index;
-	uint16_t  retry_count;
 } UavcanNodeEntry;

 class NodeManager
 {
 public:
-	NodeManager(CanardInstance &ins, UavcanParamManager &pmgr) : _canard_instance(ins), _access_request(ins, pmgr),
-		_list_request(ins) { };
+	NodeManager(CanardInstance &ins) : _canard_instance(ins), _access_request(ins), _list_request(ins) { };

 	bool HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg);
 	bool HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_2_0 &msg);
@@ -91,4 +89,8 @@ private:
 	bool nodeRegisterSetup = 0;

 	hrt_abstime _register_request_last{0};
+
+	//TODO work this out
+	const char *gps_uorb_register_name = "uavcan.pub.gnss_uorb.id";
+	const char *bms_status_register_name = "uavcan.pub.battery_status.id";
 };
@@ -40,7 +40,6 @@
 */

 #include "ParamManager.hpp"
-#include <px4_platform_common/defines.h>

 bool UavcanParamManager::GetParamByName(const char *param_name, uavcan_register_Value_1_0 &value)
 {
@@ -57,16 +56,8 @@ bool UavcanParamManager::GetParamByName(const char *param_name, uavcan_register_
 			case PARAM_TYPE_INT32: {
 					int32_t out_val {};
 					param_get(param_handle, &out_val);
-
-					if (uavcan_register_Value_1_0_is_natural16_(&value)) { //FIXME param rewrite
-						value.natural16.value.elements[0] = (uint16_t)out_val;
-						uavcan_register_Value_1_0_select_natural16_(&value);
-
-					} else {
-						value.integer32.value.elements[0] = out_val;
-						uavcan_register_Value_1_0_select_integer32_(&value);
-					}
-
+					value.integer32.value.elements[0] = out_val;
+					uavcan_register_Value_1_0_select_integer32_(&value);
 					break;
 				}

@@ -100,23 +91,15 @@ bool UavcanParamManager::GetParamByName(const uavcan_register_Name_1_0 &name, ua
 			switch (param_type(param_handle)) {
 			case PARAM_TYPE_INT32: {
 					int32_t out_val {};
-					param_get(param_handle, &out_val);
-
-					if (uavcan_register_Value_1_0_is_natural16_(&value)) { //FIXME param rewrite
-						value.natural16.value.elements[0] = (uint16_t)out_val;
-						uavcan_register_Value_1_0_select_natural16_(&value);
-
-					} else {
-						value.integer32.value.elements[0] = out_val;
-						uavcan_register_Value_1_0_select_integer32_(&value);
-					}
-
+					param_set(param_handle, &out_val);
+					value.integer32.value.elements[0] = out_val;
+					uavcan_register_Value_1_0_select_integer32_(&value);
 					break;
 				}

 			case PARAM_TYPE_FLOAT: {
 					float out_val {};
-					param_get(param_handle, &out_val);
+					param_set(param_handle, &out_val);
 					value.real32.value.elements[0] = out_val;
 					uavcan_register_Value_1_0_select_real32_(&value);
 					break;
@@ -63,7 +63,7 @@ public:

 private:

-	const UavcanParamBinder _uavcan_params[11] {
+	const UavcanParamBinder _uavcan_params[10] {
 		{"uavcan.pub.esc.0.id",              "UCAN1_ESC_PUB"},
 		{"uavcan.pub.servo.0.id",            "UCAN1_SERVO_PUB"},
 		{"uavcan.pub.gps.0.id",              "UCAN1_GPS_PUB"},
@@ -74,7 +74,6 @@ private:
 		{"uavcan.sub.battery_status.0.id",     "UCAN1_BMS_BS_PID"},
 		{"uavcan.sub.battery_parameters.0.id", "UCAN1_BMS_BP_PID"},
 		{"uavcan.sub.legacy_bms.0.id",         "UCAN1_LG_BMS_PID"},
-		{"uavcan.sub.uorb.sensor_gps.0.id",    "UCAN1_UORB_GPS"},
 		//{"uavcan.sub.bms.0.id",   "UCAN1_BMS0_PID"}, //FIXME instancing
 		//{"uavcan.sub.bms.1.id",   "UCAN1_BMS1_PID"},
 	};
@@ -1,84 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file sensor_gps.hpp
- *
-* Defines uORB over UAVCANv1 sensor_gps publisher
- *
- * @author Peter van der Perk <peter.vanderperk@nxp.com>
- */
-
-#pragma once
-
-#include <uORB/topics/sensor_gps.h>
-
-#include "../Publisher.hpp"
-
-class UORB_over_UAVCAN_sensor_gps_Publisher : public UavcanPublisher
-{
-public:
-	UORB_over_UAVCAN_sensor_gps_Publisher(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) :
-		UavcanPublisher(ins, pmgr, "sensor_gps", instance)
-	{};
-
-	// Update the uORB Subscription and broadcast a UAVCAN message
-	virtual void update() override
-	{
-		// Not sure if actuator_armed is a good indication of readiness but seems close to it
-		if (_sensor_gps_sub.updated() && _port_id != CANARD_PORT_ID_UNSET && _port_id != 0) {
-			sensor_gps_s gps_msg {};
-			_sensor_gps_sub.update(&gps_msg);
-
-			CanardTransfer transfer = {
-				.timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
-				.priority       = CanardPriorityNominal,
-				.transfer_kind  = CanardTransferKindMessage,
-				.port_id        = _port_id, // This is the subject-ID.
-				.remote_node_id = CANARD_NODE_ID_UNSET,
-				.transfer_id    = _transfer_id,
-				.payload_size   = sizeof(struct sensor_gps_s),
-				.payload        = &gps_msg,
-			};
-
-			if (result == 0) {
-				// set the data ready in the buffer and chop if needed
-				++_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
-				result = canardTxPush(&_canard_instance, &transfer);
-			}
-		}
-	};
-
-private:
-	uORB::Subscription _sensor_gps_sub{ORB_ID(sensor_gps)};
-};
@@ -52,55 +52,45 @@
 class UavcanAccessServiceRequest
 {
 public:
-	UavcanAccessServiceRequest(CanardInstance &ins, UavcanParamManager &pmgr) :
-		_canard_instance(ins), _param_manager(pmgr) { };
+	UavcanAccessServiceRequest(CanardInstance &ins) :
+		_canard_instance(ins) { };

-	bool setPortId(CanardNodeID node_id, uavcan_register_Name_1_0 &name)
+	void setPortId(CanardNodeID node_id, const char *register_name, uint16_t port_id)
 	{
 		int result {0};

 		uavcan_register_Access_Request_1_0 request_msg;
+		strncpy((char *)&request_msg.name.name.elements[0], register_name, sizeof(uavcan_register_Name_1_0));
+		request_msg.name.name.count = strlen(register_name);
+
+		uavcan_register_Value_1_0_select_natural16_(&request_msg.value);
 		request_msg.value.natural16.value.count = 1;
-		uavcan_register_Value_1_0_select_natural16_(&request_msg.value); // Set to natural16 so that ParamManager casts type
+		request_msg.value.natural16.value.elements[0] = port_id;

-		//FIXME ParamManager only has notion of being either sub/pub have to find a portable way to address trhis
-		name.name.elements[7] = 's'; //HACK Change pub into sub
+		uint8_t request_payload_buffer[uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];

-		if (_param_manager.GetParamByName(name, request_msg.value)) {
-			name.name.elements[7] = 'p'; //HACK Change sub into pub
-			memcpy(&request_msg.name, &name, sizeof(request_msg.name));
+		CanardTransfer transfer = {
+			.timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
+			.priority       = CanardPriorityNominal,
+			.transfer_kind  = CanardTransferKindRequest,
+			.port_id        = uavcan_register_Access_1_0_FIXED_PORT_ID_,                // This is the subject-ID.
+			.remote_node_id = node_id,       // Messages cannot be unicast, so use UNSET.
+			.transfer_id    = access_request_transfer_id,
+			.payload_size   = uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
+			.payload        = &request_payload_buffer,
+		};

-			uint8_t request_payload_buffer[uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
+		result = uavcan_register_Access_Request_1_0_serialize_(&request_msg, request_payload_buffer, &transfer.payload_size);

-			CanardTransfer transfer = {
-				.timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
-				.priority       = CanardPriorityNominal,
-				.transfer_kind  = CanardTransferKindRequest,
-				.port_id        = uavcan_register_Access_1_0_FIXED_PORT_ID_,                // This is the subject-ID.
-				.remote_node_id = node_id,       // Messages cannot be unicast, so use UNSET.
-				.transfer_id    = 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
-				++access_request_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
-				result = canardTxPush(&_canard_instance, &transfer);
-			}
-
-			return result > 0;
-
-		} else {
-			return false;
+		if (result == 0) {
+			// set the data ready in the buffer and chop if needed
+			++access_request_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
+			result = canardTxPush(&_canard_instance, &transfer);
 		}
 	};

 private:
 	CanardInstance &_canard_instance;
 	CanardTransferID access_request_transfer_id = 0;
-	UavcanParamManager &_param_manager;

 };
@@ -68,27 +68,25 @@ public:

 			// Set _port_id from _uavcan_param
 			uavcan_register_Value_1_0 value;
+			_param_manager.GetParamByName(uavcan_param, value);
+			int32_t new_id = value.integer32.value.elements[0];

-			if (_param_manager.GetParamByName(uavcan_param, value)) {
-				int32_t new_id = value.integer32.value.elements[0];
+			/* FIXME how about partial subscribing */
+			if (curSubj->_canard_sub._port_id != new_id) {
+				if (new_id == CANARD_PORT_ID_UNSET) {
+					// Cancel subscription
+					unsubscribe();

-				/* FIXME how about partial subscribing */
-				if (curSubj->_canard_sub._port_id != new_id) {
-					if (new_id == CANARD_PORT_ID_UNSET) {
-						// Cancel subscription
+				} else {
+					if (curSubj->_canard_sub._port_id != CANARD_PORT_ID_UNSET) {
+						// Already active; unsubscribe first
 						unsubscribe();
-
-					} else {
-						if (curSubj->_canard_sub._port_id != CANARD_PORT_ID_UNSET) {
-							// Already active; unsubscribe first
-							unsubscribe();
-						}
-
-						// Subscribe on the new port ID
-						curSubj->_canard_sub._port_id = (CanardPortID)new_id;
-						PX4_INFO("Subscribing %s.%d on port %d", curSubj->_subject_name, _instance, curSubj->_canard_sub._port_id);
-						subscribe();
 					}
+
+					// Subscribe on the new port ID
+					curSubj->_canard_sub._port_id = (CanardPortID)new_id;
+					PX4_INFO("Subscribing %s.%d on port %d", curSubj->_subject_name, _instance, curSubj->_canard_sub._port_id);
+					subscribe();
 				}
 			}

@@ -1,83 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file sensor_gps.hpp
- *
- * Defines uORB over UAVCANv1 sensor_gps subscriber
- *
- * @author Peter van der Perk <peter.vanderperk@nxp.com>
- */
-
-#pragma once
-
-#include <uORB/topics/sensor_gps.h>
-#include <uORB/PublicationMulti.hpp>
-
-#include "../DynamicPortSubscriber.hpp"
-
-class UORB_over_UAVCAN_sensor_gps_Subscriber : public UavcanDynamicPortSubscriber
-{
-public:
-	UORB_over_UAVCAN_sensor_gps_Subscriber(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) :
-		UavcanDynamicPortSubscriber(ins, pmgr, "uorb.sensor_gps", instance) { };
-
-	void subscribe() override
-	{
-		// Subscribe to messages uORB sensor_gps payload over UAVCAN
-		canardRxSubscribe(&_canard_instance,
-				  CanardTransferKindMessage,
-				  _subj_sub._canard_sub._port_id,
-				  sizeof(struct sensor_gps_s),
-				  CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC * 10000,
-				  &_subj_sub._canard_sub);
-	};
-
-	void callback(const CanardTransfer &receive) override
-	{
-		//PX4_INFO("uORB sensor_gps Callback");
-
-		if (receive.payload_size == sizeof(struct sensor_gps_s)) {
-			sensor_gps_s *gps_msg = (sensor_gps_s *)receive.payload;
-			_sensor_gps_pub.publish(*gps_msg);
-
-		} else {
-			PX4_ERR("uORB over UAVCAN %s playload size mismatch got %d expected %d",
-				_subj_sub._subject_name, receive.payload_size, sizeof(struct sensor_gps_s));
-		}
-	};
-
-private:
-	uORB::PublicationMulti<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
-
-};
@@ -56,7 +56,8 @@ static void memFree(CanardInstance *const ins, void *const pointer) { o1heapFree
 UavcanNode::UavcanNode(CanardInterface *interface, uint32_t node_id) :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::uavcan),
-	_can_interface(interface)
+	_can_interface(interface)//,
+	// _subscribers{&_gps0_sub, &_gps1_sub, &_bms0_sub, &_bms1_sub}
 {
 	pthread_mutex_init(&_node_mutex, nullptr);

@@ -167,6 +168,13 @@ void UavcanNode::init()
 					  CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
 					  &_heartbeat_subscription);

+			canardRxSubscribe(&_canard_instance, // uORB over UAVCAN GPS message
+					  CanardTransferKindMessage,
+					  gps_port_id,
+					  sizeof(struct sensor_gps_s),
+					  CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
+					  &_drone_srv_gps_subscription);
+
 			for (auto &subscriber : _subscribers) {
 				subscriber->subscribe();
 			}
@@ -230,69 +238,7 @@ void UavcanNode::Run()

 	_node_manager.update();

-	transmit();
-
-	/* Process received messages */
-
-	uint8_t data[64] {};
-	CanardFrame received_frame{};
-	received_frame.payload = &data;
-
-	while (_can_interface->receive(&received_frame) > 0) {
-		CanardTransfer receive{};
-		int32_t result = canardRxAccept(&_canard_instance, &received_frame, 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.
-			PX4_ERR("Receive error %d\n", result);
-
-		} else if (result == 1) {
-			// A transfer has been received, process it.
-			// PX4_INFO("received Port ID: %d", receive.port_id);
-
-			if (receive.port_id > 0) {
-				// If not a fixed port ID, check any subscribers which may have registered it
-				for (auto &subscriber : _subscribers) {
-					if (subscriber->hasPortID(receive.port_id)) {
-						subscriber->callback(receive);
-					}
-				}
-
-				for (auto &subscriber : _dynsubscribers) {
-					if (subscriber->hasPortID(receive.port_id)) {
-						subscriber->callback(receive);
-					}
-				}
-			}
-
-			// Deallocate the dynamic memory afterwards.
-			_canard_instance.memory_free(&_canard_instance, (void *)receive.payload);
-
-		} else {
-			//PX4_INFO("RX canard %d", result);
-		}
-	}
-
-	// Pop canardTx queue to send out responses to requets
-	transmit();
-
-	perf_end(_cycle_perf);
-
-	if (_task_should_exit.load()) {
-		_can_interface->close();
-
-		ScheduleClear();
-		_instance = nullptr;
-	}
-
-	pthread_mutex_unlock(&_node_mutex);
-}
-
-void UavcanNode::transmit()
-{
+	// Transmitting
 	// Look at the top of the TX queue.
 	for (const CanardFrame *txf = nullptr; (txf = canardTxPeek(&_canard_instance)) != nullptr;) {
 		// Attempt transmission only if the frame is not yet timed out while waiting in the TX queue.
@@ -322,6 +268,67 @@ void UavcanNode::transmit()
 			}
 		}
 	}
+
+	/* Process received messages */
+
+	uint8_t data[64] {};
+	CanardFrame received_frame{};
+	received_frame.payload = &data;
+
+	/* FIXME this flawed we've to go through the whole loop to get the next frame in the buffer  */
+
+	if (_can_interface->receive(&received_frame) > 0) {
+
+		CanardTransfer receive{};
+		int32_t result = canardRxAccept(&_canard_instance, &received_frame, 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.
+			PX4_ERR("Receive error %d\n", result);
+
+		} else if (result == 1) {
+			// A transfer has been received, process it.
+			// PX4_INFO("received Port ID: %d", receive.port_id);
+
+			if (receive.port_id == gps_port_id) {
+				result = handleUORBSensorGPS(receive);
+
+			} else if (receive.port_id > 0) {
+				// If not a fixed port ID, check any subscribers which may have registered it
+				for (auto &subscriber : _subscribers) {
+					if (subscriber->hasPortID(receive.port_id)) {
+						subscriber->callback(receive);
+					}
+				}
+
+				for (auto &subscriber : _dynsubscribers) {
+					if (subscriber->hasPortID(receive.port_id)) {
+						subscriber->callback(receive);
+					}
+				}
+			}
+
+			// Deallocate the dynamic memory afterwards.
+			_canard_instance.memory_free(&_canard_instance, (void *)receive.payload);
+
+		} else {
+			//PX4_INFO("RX canard %d\r\n", result);
+		}
+	}
+
+	perf_end(_cycle_perf);
+
+	if (_task_should_exit.load()) {
+		_can_interface->close();
+
+		ScheduleClear();
+		_instance = nullptr;
+	}
+
+	pthread_mutex_unlock(&_node_mutex);
 }

 void UavcanNode::print_info()
@@ -331,13 +338,6 @@ void UavcanNode::print_info()
 	perf_print_counter(_cycle_perf);
 	perf_print_counter(_interval_perf);

-	O1HeapDiagnostics heap_diagnostics = o1heapGetDiagnostics(uavcan_allocator);
-
-	PX4_INFO("Heap status %zu/%zu Peak alloc %zu Peak req %zu OOM count %" PRIu64,
-		 heap_diagnostics.allocated, heap_diagnostics.capacity,
-		 heap_diagnostics.peak_allocated, heap_diagnostics.peak_request_size,
-		 heap_diagnostics.oom_count);
-
 	for (auto &publisher : _publishers) {
 		publisher->printInfo();
 	}
@@ -346,10 +346,6 @@ void UavcanNode::print_info()
 		subscriber->printInfo();
 	}

-	for (auto &subscriber : _dynsubscribers) {
-		subscriber->printInfo();
-	}
-
 	_mixing_output.printInfo();
 	_esc_controller.printInfo();

@@ -446,6 +442,16 @@ void UavcanNode::sendHeartbeat()
 	}
 }

+int UavcanNode::handleUORBSensorGPS(const CanardTransfer &receive)
+{
+	PX4_INFO("NodeID %i GPS sensor msg", receive.remote_node_id);
+
+	sensor_gps_s *gps_msg = (sensor_gps_s *)receive.payload;
+
+	return _sensor_gps_pub.publish(*gps_msg) ? 0 : -1;
+}
+
+
 bool UavcanMixingInterface::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
 		unsigned num_control_groups_updated)
 {
@@ -78,9 +78,6 @@
 #include "Subscribers/NodeIDAllocationData.hpp"
 #include "Subscribers/LegacyBatteryInfo.hpp"

-// uORB over UAVCAN subscribers
-#include "Subscribers/uORB/sensor_gps.hpp"
-
 #include "ServiceClients/GetInfo.hpp"
 #include "ServiceClients/Access.hpp"

@@ -165,11 +162,14 @@ private:
 	void Run() override;
 	void fill_node_info();

-	void transmit();
-
 	// Sends a heartbeat at 1s intervals
 	void sendHeartbeat();

+	int handlePnpNodeIDAllocationData(const CanardTransfer &receive);
+	int handleRegisterList(const CanardTransfer &receive);
+	int handleRegisterAccess(const CanardTransfer &receive);
+	int handleUORBSensorGPS(const CanardTransfer &receive);
+
 	void *_uavcan_heap{nullptr};

 	CanardInterface *const _can_interface;
@@ -185,9 +185,14 @@ private:
 	pthread_mutex_t _node_mutex;

 	CanardRxSubscription _heartbeat_subscription;
+	CanardRxSubscription _drone_srv_gps_subscription;

+	uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
 	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")};

@@ -196,6 +201,15 @@ private:
 	hrt_abstime _uavcan_node_heartbeat_last{0};
 	CanardTransferID _uavcan_node_heartbeat_transfer_id{0};

+	/* 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 gps_port_id = 1235;
+
+	CanardTransferID _uavcan_register_list_request_transfer_id{0};
+	CanardTransferID _uavcan_register_access_request_transfer_id{0};
+
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::UAVCAN_V1_ENABLE>) _param_uavcan_v1_enable,
 		(ParamInt<px4::params::UAVCAN_V1_ID>) _param_uavcan_v1_id,
@@ -206,7 +220,7 @@ private:

 	UavcanParamManager _param_manager;

-	NodeManager _node_manager {_canard_instance, _param_manager};
+	NodeManager _node_manager {_canard_instance};

 	UavcanGnssPublisher _gps_pub {_canard_instance, _param_manager};

@@ -224,8 +238,6 @@ private:
 	UavcanLegacyBatteryInfoSubscriber  _legacybms_sub {_canard_instance, _param_manager, 0};
 	UavcanNodeIDAllocationDataSubscriber _nodeid_sub {_canard_instance, _node_manager};

-	UORB_over_UAVCAN_sensor_gps_Subscriber _sensor_gps_sub {_canard_instance, _param_manager, 0};
-
 	UavcanGetInfoResponse _getinfo_rsp {_canard_instance};
 	UavcanAccessResponse  _access_rsp {_canard_instance, _param_manager};

@@ -233,7 +245,7 @@ private:
 	UavcanListServiceReply   _list_service {_canard_instance, _node_manager};

 	// Subscriber objects: Any object used to bridge a UAVCAN message to a uORB message
-	UavcanDynamicPortSubscriber *_dynsubscribers[6] {&_gps0_sub, &_gps1_sub, &_bms0_sub, &_esc_sub, &_legacybms_sub, &_sensor_gps_sub}; /// TODO: turn into List<UavcanSubscription*>
+	UavcanDynamicPortSubscriber *_dynsubscribers[5] {&_gps0_sub, &_gps1_sub, &_bms0_sub, &_esc_sub, &_legacybms_sub}; /// TODO: turn into List<UavcanSubscription*>
 	UavcanBaseSubscriber *_subscribers[5] {&_nodeid_sub, &_getinfo_rsp, &_access_rsp, &_access_service, &_list_service}; /// TODO: turn into List<UavcanSubscription*>

 	UavcanMixingInterface _mixing_output {_node_mutex, _esc_controller};
@@ -103,6 +103,3 @@ PARAM_DEFINE_INT32(UCAN1_LG_BMS_PID, 0);
 PARAM_DEFINE_INT32(UCAN1_ESC_PUB, 0);
 PARAM_DEFINE_INT32(UCAN1_GPS_PUB, 0);
 PARAM_DEFINE_INT32(UCAN1_SERVO_PUB, 0);
-
-// uORB over UAVCAN
-PARAM_DEFINE_INT32(UCAN1_UORB_GPS, 0);
@@ -381,7 +381,7 @@ static int canard_daemon(int argc, char *argv[])
 	/* 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("uorb.sensor_gps.0", set_gps_uorb_port_id, get_gps_uorb_port_id);
+	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);

@@ -238,13 +238,10 @@ int32_t uavcan_register_interface_list_response(CanardInstance *ins, CanardTrans

 	// Reponse magic start

-	if (msg.index < register_list_size) {
+	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);
-
-	} else {
-		response_msg.name.name.count = 0;
 	}

 	//TODO more option then pub (sub rate
@@ -65,12 +65,7 @@ bool PreFlightCheck::accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_s

 		device_id = accel.get().device_id;

-		if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
-			calibration_valid = true;
-
-		} else {
-			calibration_valid = (calibration::FindCalibrationIndex("ACC", device_id) >= 0);
-		}
+		calibration_valid = (calibration::FindCalibrationIndex("ACC", device_id) >= 0);

 		if (!calibration_valid) {
 			if (report_fail) {
@@ -64,12 +64,7 @@ bool PreFlightCheck::gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &

 		device_id = gyro.get().device_id;

-		if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
-			calibration_valid = true;
-
-		} else {
-			calibration_valid = (calibration::FindCalibrationIndex("GYRO", device_id) >= 0);
-		}
+		calibration_valid = (calibration::FindCalibrationIndex("GYRO", device_id) >= 0);

 		if (!calibration_valid) {
 			if (report_fail) {
@@ -66,12 +66,7 @@ bool PreFlightCheck::magnetometerCheck(orb_advert_t *mavlink_log_pub, vehicle_st

 		device_id = magnetometer.get().device_id;

-		if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
-			calibration_valid = true;
-
-		} else {
-			calibration_valid = (calibration::FindCalibrationIndex("MAG", device_id) >= 0);
-		}
+		calibration_valid = (calibration::FindCalibrationIndex("MAG", device_id) >= 0);

 		if (!calibration_valid) {
 			if (report_fail) {
@@ -63,11 +63,6 @@ bool PreFlightCheck::powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_sta
 		return true;
 	}

-	if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
-		// Ignore power check in HITL.
-		return true;
-	}
-
 	uORB::SubscriptionData<system_power_s> system_power_sub{ORB_ID(system_power)};
 	system_power_sub.update();
 	const system_power_s &system_power = system_power_sub.get();
@@ -199,7 +199,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
 	/* wait 500 ms to ensure parameter propagated through the system */
 	px4_usleep(500 * 1000);

-	calibration_log_critical(mavlink_log_pub, "[cal] Blow into front of pitot without touching");
+	calibration_log_critical(mavlink_log_pub, "[cal] Blow across front of pitot without touching");

 	calibration_counter = 0;

@@ -467,18 +467,6 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta

 calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_mask)
 {
-	// We should not try to subscribe if the topic doesn't actually exist and can be counted.
-	const unsigned orb_mag_count = orb_group_count(ORB_ID(sensor_mag));
-
-	// Warn that we will not calibrate more than MAX_GYROS gyroscopes
-	if (orb_mag_count > MAX_MAGS) {
-		calibration_log_critical(mavlink_log_pub, "Detected %u mags, but will calibrate only %u", orb_mag_count, MAX_MAGS);
-
-	} else if (orb_mag_count < 1) {
-		calibration_log_critical(mavlink_log_pub, "No mags found");
-		return calibrate_return_error;
-	}
-
 	calibrate_return result = calibrate_return_ok;

 	mag_worker_data_t worker_data{};
@@ -1742,8 +1742,7 @@ int EKF2::task_spawn(int argc, char *argv[])

 		while ((multi_instances_allocated < multi_instances)
 		       && (vehicle_status_sub.get().arming_state != vehicle_status_s::ARMING_STATE_ARMED)
-		       && ((hrt_elapsed_time(&time_started) < 30_s)
-			   || (vehicle_status_sub.get().hil_state == vehicle_status_s::HIL_STATE_ON))) {
+		       && (hrt_elapsed_time(&time_started) < 30_s)) {

 			vehicle_status_sub.update();

@@ -315,11 +315,6 @@ bool EKF2Selector::UpdateErrorScores()
 				if (error_delta > 0 || error_delta < -threshold) {
 					_instance[i].relative_test_ratio += error_delta;
 					_instance[i].relative_test_ratio = constrain(_instance[i].relative_test_ratio, -_rel_err_score_lim, _rel_err_score_lim);
-
-					if ((error_delta < -threshold) && (_instance[i].relative_test_ratio < 1.f)) {
-						// increase status publication rate if there's movement towards a potential instance change
-						_selector_status_publish = true;
-					}
 				}
 			}
 		}
@@ -655,6 +650,7 @@ void EKF2Selector::Run()
 	}

 	if (updated) {
+
 		const uint8_t available_instances_prev = _available_instances;
 		const uint8_t selected_instance_prev = _selected_instance;
 		const uint32_t instance_changed_count_prev = _instance_changed_count;
@@ -723,7 +719,32 @@ void EKF2Selector::Run()
 		    || (last_instance_change_prev != _last_instance_change)
 		    || _accel_fault_detected || _gyro_fault_detected) {

-			PublishEstimatorSelectorStatus();
+			estimator_selector_status_s selector_status{};
+			selector_status.primary_instance = _selected_instance;
+			selector_status.instances_available = _available_instances;
+			selector_status.instance_changed_count = _instance_changed_count;
+			selector_status.last_instance_change = _last_instance_change;
+			selector_status.accel_device_id = _instance[_selected_instance].accel_device_id;
+			selector_status.baro_device_id = _instance[_selected_instance].baro_device_id;
+			selector_status.gyro_device_id = _instance[_selected_instance].gyro_device_id;
+			selector_status.mag_device_id = _instance[_selected_instance].mag_device_id;
+			selector_status.gyro_fault_detected = _gyro_fault_detected;
+			selector_status.accel_fault_detected = _accel_fault_detected;
+
+			for (int i = 0; i < EKF2_MAX_INSTANCES; i++) {
+				selector_status.combined_test_ratio[i] = _instance[i].combined_test_ratio;
+				selector_status.relative_test_ratio[i] = _instance[i].relative_test_ratio;
+				selector_status.healthy[i] = _instance[i].healthy;
+			}
+
+			for (int i = 0; i < IMU_STATUS_SIZE; i++) {
+				selector_status.accumulated_gyro_error[i] = _accumulated_gyro_error[i];
+				selector_status.accumulated_accel_error[i] = _accumulated_accel_error[i];
+			}
+
+			selector_status.timestamp = hrt_absolute_time();
+			_estimator_selector_status_pub.publish(selector_status);
+			_last_status_publish = selector_status.timestamp;
 			_selector_status_publish = false;
 		}
 	}
@@ -736,36 +757,6 @@ void EKF2Selector::Run()
 	PublishWindEstimate();
 }

-void EKF2Selector::PublishEstimatorSelectorStatus()
-{
-	estimator_selector_status_s selector_status{};
-	selector_status.primary_instance = _selected_instance;
-	selector_status.instances_available = _available_instances;
-	selector_status.instance_changed_count = _instance_changed_count;
-	selector_status.last_instance_change = _last_instance_change;
-	selector_status.accel_device_id = _instance[_selected_instance].accel_device_id;
-	selector_status.baro_device_id = _instance[_selected_instance].baro_device_id;
-	selector_status.gyro_device_id = _instance[_selected_instance].gyro_device_id;
-	selector_status.mag_device_id = _instance[_selected_instance].mag_device_id;
-	selector_status.gyro_fault_detected = _gyro_fault_detected;
-	selector_status.accel_fault_detected = _accel_fault_detected;
-
-	for (int i = 0; i < EKF2_MAX_INSTANCES; i++) {
-		selector_status.combined_test_ratio[i] = _instance[i].combined_test_ratio;
-		selector_status.relative_test_ratio[i] = _instance[i].relative_test_ratio;
-		selector_status.healthy[i] = _instance[i].healthy;
-	}
-
-	for (int i = 0; i < IMU_STATUS_SIZE; i++) {
-		selector_status.accumulated_gyro_error[i] = _accumulated_gyro_error[i];
-		selector_status.accumulated_accel_error[i] = _accumulated_accel_error[i];
-	}
-
-	selector_status.timestamp = hrt_absolute_time();
-	_estimator_selector_status_pub.publish(selector_status);
-	_last_status_publish = selector_status.timestamp;
-}
-
 void EKF2Selector::PrintStatus()
 {
 	PX4_INFO("available instances: %d", _available_instances);
@@ -78,7 +78,6 @@ private:
 	static constexpr uint64_t FILTER_UPDATE_PERIOD{10_ms};

 	void Run() override;
-	void PublishEstimatorSelectorStatus();
 	void PublishVehicleAttitude();
 	void PublishVehicleLocalPosition();
 	void PublishVehicleGlobalPosition();
@@ -141,26 +141,19 @@ FixedwingPositionControl::parameters_update()

 	landing_status_publish();

-	int check_ret = PX4_OK;
-
 	// sanity check parameters
-	if (_param_fw_airspd_max.get() < _param_fw_airspd_min.get()) {
-		mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Airspeed max smaller than min");
-		check_ret = PX4_ERROR;
+	if ((_param_fw_airspd_max.get() < _param_fw_airspd_min.get()) ||
+	    (_param_fw_airspd_max.get() < 5.0f) ||
+	    (_param_fw_airspd_min.get() > 100.0f) ||
+	    (_param_fw_airspd_trim.get() < _param_fw_airspd_min.get()) ||
+	    (_param_fw_airspd_trim.get() > _param_fw_airspd_max.get())) {
+
+		mavlink_log_critical(&_mavlink_log_pub, "Airspeed parameters invalid");
+
+		return PX4_ERROR;
 	}

-	if (_param_fw_airspd_max.get() < 5.0f || _param_fw_airspd_min.get() > 100.0f) {
-		mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Airspeed max < 5 m/s or min > 100 m/s");
-		check_ret = PX4_ERROR;
-	}
-
-	if (_param_fw_airspd_trim.get() < _param_fw_airspd_min.get() ||
-	    _param_fw_airspd_trim.get() > _param_fw_airspd_max.get()) {
-		mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Airspeed cruise out of min or max bounds");
-		check_ret = PX4_ERROR;
-	}
-
-	return check_ret;
+	return PX4_OK;
 }

 void
@@ -461,12 +461,11 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 10.0f);
 PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 20.0f);

 /**
- * Trim/ Cruise Airspeed (CAS)
+ * Cruise Airspeed (CAS)
 *
- * This is the default cruise airspeed setpoint (calibrated airspeed) used by the system in assisted
- * and autonomous control modes if no other airspeed setpoint is given.
- * It is also used for control surface effectiveness scaling.
- * (scaling = FW_AIRSPD_TRIM / calibrated_airspeed).
+ * The trim CAS (calibrated airspeed) of the vehicle. If an airspeed controller is active,
+ * this is the default airspeed setpoint that the controller will try to achieve if
+ * no other airspeed setpoint sources are present (e.g. through non-centered RC sticks).
 *
 * @unit m/s
 * @min 0.5