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

- ecl in PX4/Firmware (a300d32523e24df3f366a0d564b764261e1c1909): https://github.com/PX4/PX4-ECL/commit/a7b8afe420f438554ad90bcba0f1f4872325e75b
    - ecl current upstream: https://github.com/PX4/PX4-ECL/commit/29243ac5cbb5d27ac71744e88afcd786df6f748d
    - Changes: https://github.com/PX4/PX4-ECL/compare/a7b8afe420f438554ad90bcba0f1f4872325e75b...29243ac5cbb5d27ac71744e88afcd786df6f748d

    29243ac 2021-05-05 bresch - yaw_reset: reduce minimum vector length to compute yaw error
aad4840 2021-05-02 Kabir Mohammed - EKF: increase allowed difference between flow and gyro ODRs

.ci/Jenkinsfile-compile

@@ -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()

.github/workflows/checks.yml

@@ -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

.github/workflows/compile_nuttx.yml

@@ -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: [

.github/workflows/compile_nuttx_cannode.yml

@@ -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: [

.github/workflows/container.yml

@@ -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 }}

.github/workflows/metadata.yml

@@ -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:

Dockerfile

@@ -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"]

Jenkinsfile

@@ -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')

Tools/docker_run.sh

@@ -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"

Tools/setup/Makefile

@@ -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

Tools/setup/entrypoint.sh

@@ -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

Tools/setup/requirements.txt

@@ -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

Tools/setup/ubuntu.sh

@@ -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 \

src/drivers/dshot/DShot.cpp

@@ -530,11 +530,27 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 
 	} else {
 		for (int i = 0; i < (int)num_outputs; i++) {
-			if (outputs[i] == DSHOT_DISARM_VALUE) {
+
+			uint16_t output = outputs[i];
+
+			// DShot 3D splits the throttle ranges in two.
+			// This is in terms of DShot values, code below is in terms of actuator_output
+			// Direction 1) 48 is the slowest, 1047 is the fastest.
+			// Direction 2) 1049 is the slowest, 2047 is the fastest.
+			if (_param_dshot_3d_enable.get()) {
+				if (output >= _param_dshot_3d_dead_l.get() && output <= _param_dshot_3d_dead_h.get()) {
+					output = DSHOT_DISARM_VALUE;
+
+				} else if (output < 1000 && output > 0) { //Todo: allow actuator 0 or dshot 48 to be used
+					output = 999 - output;
+				}
+			}
+
+			if (output == DSHOT_DISARM_VALUE) {
 				up_dshot_motor_command(i, DShot_cmd_motor_stop, i == requested_telemetry_index);
 
 			} else {
-				up_dshot_motor_data_set(i, math::min(outputs[i], static_cast<uint16_t>(DSHOT_MAX_THROTTLE)),
+				up_dshot_motor_data_set(i, math::min(output, static_cast<uint16_t>(DSHOT_MAX_THROTTLE)),
 							i == requested_telemetry_index);
 			}
 		}

src/drivers/dshot/DShot.h

@@ -231,6 +231,9 @@ private:
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::DSHOT_CONFIG>)   _param_dshot_config,
 		(ParamFloat<px4::params::DSHOT_MIN>)    _param_dshot_min,
+		(ParamBool<px4::params::DSHOT_3D_ENABLE>) _param_dshot_3d_enable,
+		(ParamInt<px4::params::DSHOT_3D_DEAD_H>) _param_dshot_3d_dead_h,
+		(ParamInt<px4::params::DSHOT_3D_DEAD_L>) _param_dshot_3d_dead_l,
 		(ParamInt<px4::params::MOT_POLE_COUNT>) _param_mot_pole_count
 	)
 };

src/drivers/dshot/module.yaml

@@ -14,7 +14,7 @@ parameters:
                 long: |
                     This enables/disables DShot. The different modes define different
                     speeds, for example DShot150 = 150kb/s. Not all ESCs support all modes.
-                    
+
                     Note: this enables DShot on the FMU outputs. For boards with an IO it is the
                     AUX outputs.
             type: enum
@@ -40,6 +40,37 @@ parameters:
             decimal: 2
             increment: 0.01
             default: 0.055
+        DSHOT_3D_ENABLE:
+            description:
+                short: Allows for 3d mode when using DShot and suitable mixer
+                long: |
+                    WARNING: ESC must be configured for 3D mode, and DSHOT_MIN set to 0.
+                    This splits the throttle ranges in two.
+                    Direction 1) 48 is the slowest, 1047 is the fastest.
+                    Direction 2) 1049 is the slowest, 2047 is the fastest.
+                    When mixer outputs 1000 or value inside DSHOT 3D deadband, DShot 0 is sent.
+            type: boolean
+            default: 0
+        DSHOT_3D_DEAD_H:
+            description:
+                short: DSHOT 3D deadband high
+                long: |
+                    When the actuator_output is between DSHOT_3D_DEAD_L and DSHOT_3D_DEAD_H, motor will not spin.
+                    This value is with respect to the mixer_module range (0-1999), not the DSHOT values.
+            type: int32
+            min: 1000
+            max: 1999
+            default: 1000
+        DSHOT_3D_DEAD_L:
+            description:
+                short: DSHOT 3D deadband low
+                long: |
+                    When the actuator_output is between DSHOT_3D_DEAD_L and DSHOT_3D_DEAD_H, motor will not spin.
+                    This value is with respect to the mixer_module range (0-1999), not the DSHOT values.
+            type: int32
+            min: 0
+            max: 1000
+            default: 1000
         MOT_POLE_COUNT: # only used by dshot so far, so keep it under the dshot group
             description:
                 short: Number of magnetic poles of the motors
@@ -51,4 +82,3 @@ parameters:
                     Typical motors for 5 inch props have 14 poles.
             type: int32
             default: 14
-

src/drivers/optical_flow/paw3902/PAW3902.cpp

@@ -84,8 +84,6 @@ int PAW3902::init()
 		return PX4_ERROR;
 	}
 
-	Reset();
-
 	// force to low light mode (1) initially
 	ChangeMode(Mode::LowLight, true);
 
@@ -150,24 +148,6 @@ bool PAW3902::DataReadyInterruptDisable()
 	return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
 }
 
-bool PAW3902::Reset()
-{
-	DataReadyInterruptDisable();
-
-	// Power on reset
-	RegisterWrite(Register::Power_Up_Reset, 0x5A);
-	usleep(1000);
-
-	// Read from registers 0x02, 0x03, 0x04, 0x05 and 0x06 one time regardless of the motion state
-	RegisterRead(Register::Motion);
-	RegisterRead(Register::Delta_X_L);
-	RegisterRead(Register::Delta_X_H);
-	RegisterRead(Register::Delta_Y_L);
-	RegisterRead(Register::Delta_Y_H);
-
-	return true;
-}
-
 void PAW3902::exit_and_cleanup()
 {
 	DataReadyInterruptDisable();
@@ -183,6 +163,7 @@ bool PAW3902::ChangeMode(Mode newMode, bool force)
 
 		// Issue a soft reset
 		RegisterWrite(Register::Power_Up_Reset, 0x5A);
+		px4_usleep(1000);
 
 		uint32_t interval_us = 0;
 
@@ -206,6 +187,10 @@ bool PAW3902::ChangeMode(Mode newMode, bool force)
 			break;
 		}
 
+		EnableLed();
+
+		_discard_reading = 3;
+
 		if (DataReadyInterruptConfigure()) {
 			// backup schedule as a watchdog timeout
 			ScheduleDelayed(500_ms);
@@ -214,17 +199,6 @@ bool PAW3902::ChangeMode(Mode newMode, bool force)
 			ScheduleOnInterval(interval_us);
 		}
 
-		// Discard the first three motion data.
-		for (int i = 0; i < 3; i++) {
-			RegisterRead(Register::Motion);
-			RegisterRead(Register::Delta_X_L);
-			RegisterRead(Register::Delta_X_H);
-			RegisterRead(Register::Delta_Y_L);
-			RegisterRead(Register::Delta_Y_H);
-		}
-
-		EnableLed();
-
 		_mode = newMode;
 	}
 
@@ -232,11 +206,6 @@ bool PAW3902::ChangeMode(Mode newMode, bool force)
 	_low_to_superlow_counter = 0;
 	_low_to_bright_counter = 0;
 	_superlow_to_low_counter = 0;
-	// Approximate Resolution = (Register Value + 1) * (50 / 8450) ≈ 0.6% of data point in Figure 19
-	// The maximum register value is 0xA8. The minimum register value is 0.
-	uint8_t resolution = RegisterRead(Register::Resolution);
-	PX4_DEBUG("Resolution: %X", resolution);
-	PX4_DEBUG("Resolution is approx: %.3f", (double)((resolution + 1.0f) * (50.0f / 8450.0f)));
 
 	return true;
 }
@@ -582,9 +551,9 @@ void PAW3902::ModeSuperLowLight()
 void PAW3902::EnableLed()
 {
 	// Enable LED_N controls
-	RegisterWriteVerified(0x7F, 0x14);
-	RegisterWriteVerified(0x6F, 0x1c);
-	RegisterWriteVerified(0x7F, 0x00);
+	RegisterWrite(0x7F, 0x14);
+	RegisterWrite(0x6F, 0x1c);
+	RegisterWrite(0x7F, 0x00);
 }
 
 uint8_t PAW3902::RegisterRead(uint8_t reg, int retries)
@@ -663,11 +632,17 @@ void PAW3902::RunImpl()
 	// update for next iteration
 	_previous_collect_timestamp = timestamp_sample;
 
+	if (_discard_reading > 0) {
+		_discard_reading--;
+		ResetAccumulatedData();
+		return;
+	}
+
 	// check SQUAL & Shutter values
 	// To suppress false motion reports, discard Delta X and Delta Y values if the SQUAL and Shutter values meet the condition
-	// Bright Mode,			SQUAL < 0x19, Shutter ≥ 0x1FF0
-	// Low Light Mode,		SQUAL < 0x46, Shutter ≥ 0x1FF0
-	// Super Low Light Mode,	SQUAL < 0x55, Shutter ≥ 0x0BC0
+	// Bright Mode,          SQUAL < 0x19, Shutter ≥ 0x1FF0
+	// Low Light Mode,       SQUAL < 0x46, Shutter ≥ 0x1FF0
+	// Super Low Light Mode, SQUAL < 0x55, Shutter ≥ 0x0BC0
 	const uint16_t shutter = (buf.data.Shutter_Upper << 8) | buf.data.Shutter_Lower;
 
 	bool data_valid = true;
@@ -677,7 +652,6 @@ void PAW3902::RunImpl()
 		if ((buf.data.SQUAL < 0x19) && (shutter >= 0x1FF0)) {
 			// false motion report, discarding
 			perf_count(_false_motion_perf);
-			ResetAccumulatedData();
 			data_valid = false;
 		}
 
@@ -699,7 +673,6 @@ void PAW3902::RunImpl()
 		if ((buf.data.SQUAL < 0x46) && (shutter >= 0x1FF0)) {
 			// false motion report, discarding
 			perf_count(_false_motion_perf);
-			ResetAccumulatedData();
 			data_valid = false;
 		}
 
@@ -732,11 +705,14 @@ void PAW3902::RunImpl()
 		if ((buf.data.SQUAL < 0x55) && (shutter >= 0x0BC0)) {
 			// false motion report, discarding
 			perf_count(_false_motion_perf);
-			ResetAccumulatedData();
 			data_valid = false;
 		}
 
-		if (shutter < 0x03E8) {
+		if (shutter < 0x01F4) {
+			// should not operate with Shutter < 0x01F4 in Mode 2
+			ChangeMode(Mode::LowLight);
+
+		} else if (shutter < 0x03E8) {
 			// SuperLowLight -> LowLight
 			_superlow_to_low_counter++;
 

src/drivers/optical_flow/paw3902/PAW3902.hpp

@@ -90,8 +90,6 @@ private:
 	void RegisterWrite(uint8_t reg, uint8_t data);
 	bool RegisterWriteVerified(uint8_t reg, uint8_t data, int retries = 5);
 
-	bool Reset();
-
 	void EnableLed();
 
 	void ModeBright();
@@ -124,6 +122,8 @@ private:
 
 	matrix::Dcmf	_rotation;
 
+	int             _discard_reading{3};
+
 	int		_flow_sum_x{0};
 	int		_flow_sum_y{0};
 

src/modules/ekf2/EKF2Selector.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -37,7 +37,6 @@ using namespace time_literals;
 using matrix::Quatf;
 using matrix::Vector2f;
 using math::constrain;
-using math::max;
 using math::radians;
 
 EKF2Selector::EKF2Selector() :
@@ -118,12 +117,6 @@ bool EKF2Selector::SelectInstance(uint8_t ekf_instance)
 			_instance[i].relative_test_ratio = 0;
 		}
 
-		// publish new data immediately with resets
-		PublishVehicleAttitude(true);
-		PublishVehicleLocalPosition(true);
-		PublishVehicleGlobalPosition(true);
-		PublishWindEstimate(true);
-
 		return true;
 	}
 
@@ -330,104 +323,152 @@ bool EKF2Selector::UpdateErrorScores()
 	return (primary_updated || updated);
 }
 
-void EKF2Selector::PublishVehicleAttitude(bool reset)
+void EKF2Selector::PublishVehicleAttitude()
 {
+	// selected estimator_attitude -> vehicle_attitude
 	vehicle_attitude_s attitude;
 
-	if (_instance[_selected_instance].estimator_attitude_sub.copy(&attitude)) {
-		if (reset) {
-			// on reset compute deltas from last published data
-			++_quat_reset_counter;
+	if (_instance[_selected_instance].estimator_attitude_sub.update(&attitude)) {
+		bool instance_change = false;
 
-			_delta_q_reset = (Quatf(attitude.q) * Quatf(_attitude_last.q).inversed()).normalized();
-
-			// ensure monotonically increasing timestamp_sample through reset
-			attitude.timestamp_sample = max(attitude.timestamp_sample, _attitude_last.timestamp_sample);
-
-		} else {
-			// otherwise propogate deltas from estimator data while maintaining the overall reset counts
-			if (attitude.quat_reset_counter > _attitude_last.quat_reset_counter) {
-				++_quat_reset_counter;
-				_delta_q_reset = Quatf{attitude.delta_q_reset};
-			}
+		if (_instance[_selected_instance].estimator_attitude_sub.get_instance() != _attitude_instance_prev) {
+			_attitude_instance_prev = _instance[_selected_instance].estimator_attitude_sub.get_instance();
+			instance_change = true;
 		}
 
-		// save last primary estimator_attitude
+		if (_attitude_last.timestamp != 0) {
+			if (!instance_change && (attitude.quat_reset_counter == _attitude_last.quat_reset_counter + 1)) {
+				// propogate deltas from estimator data while maintaining the overall reset counts
+				++_quat_reset_counter;
+				_delta_q_reset = Quatf{attitude.delta_q_reset};
+
+			} else if (instance_change || (attitude.quat_reset_counter != _attitude_last.quat_reset_counter)) {
+				// on reset compute deltas from last published data
+				++_quat_reset_counter;
+				_delta_q_reset = (Quatf(attitude.q) * Quatf(_attitude_last.q).inversed()).normalized();
+			}
+
+		} else {
+			_quat_reset_counter = attitude.quat_reset_counter;
+			_delta_q_reset = Quatf{attitude.delta_q_reset};
+		}
+
+		bool publish = true;
+
+		// ensure monotonically increasing timestamp_sample through reset, don't publish
+		//  estimator's attitude for system (vehicle_attitude) if it's stale
+		if ((attitude.timestamp_sample <= _attitude_last.timestamp_sample)
+		    || (attitude.timestamp_sample < _instance[_selected_instance].timestamp_sample_last)) {
+
+			publish = false;
+		}
+
+		// save last primary estimator_attitude as published with original resets
 		_attitude_last = attitude;
 
-		// republish with total reset count and current timestamp
-		attitude.quat_reset_counter = _quat_reset_counter;
-		_delta_q_reset.copyTo(attitude.delta_q_reset);
+		if (publish) {
+			// republish with total reset count and current timestamp
+			attitude.quat_reset_counter = _quat_reset_counter;
+			_delta_q_reset.copyTo(attitude.delta_q_reset);
 
-		attitude.timestamp = hrt_absolute_time();
-		_vehicle_attitude_pub.publish(attitude);
-
-		_instance[_selected_instance].timestamp_sample_last = attitude.timestamp_sample;
+			attitude.timestamp = hrt_absolute_time();
+			_vehicle_attitude_pub.publish(attitude);
+		}
 	}
 }
 
-void EKF2Selector::PublishVehicleLocalPosition(bool reset)
+void EKF2Selector::PublishVehicleLocalPosition()
 {
-	// vehicle_local_position
+	// selected estimator_local_position -> vehicle_local_position
 	vehicle_local_position_s local_position;
 
-	if (_instance[_selected_instance].estimator_local_position_sub.copy(&local_position)) {
-		if (reset) {
-			// on reset compute deltas from last published data
-			++_xy_reset_counter;
-			++_z_reset_counter;
-			++_vxy_reset_counter;
-			++_vz_reset_counter;
-			++_heading_reset_counter;
+	if (_instance[_selected_instance].estimator_local_position_sub.update(&local_position)) {
+		bool instance_change = false;
 
-			_delta_xy_reset = Vector2f{local_position.x, local_position.y} - Vector2f{_local_position_last.x, _local_position_last.y};
-			_delta_z_reset = local_position.z - _local_position_last.z;
-			_delta_vxy_reset = Vector2f{local_position.vx, local_position.vy} - Vector2f{_local_position_last.vx, _local_position_last.vy};
-			_delta_vz_reset = local_position.vz - _local_position_last.vz;
-			_delta_heading_reset = matrix::wrap_2pi(local_position.heading - _local_position_last.heading);
-
-			// ensure monotonically increasing timestamp_sample through reset
-			local_position.timestamp_sample = max(local_position.timestamp_sample, _local_position_last.timestamp_sample);
-
-		} else {
-			// otherwise propogate deltas from estimator data while maintaining the overall reset counts
+		if (_instance[_selected_instance].estimator_local_position_sub.get_instance() != _local_position_instance_prev) {
+			_local_position_instance_prev = _instance[_selected_instance].estimator_local_position_sub.get_instance();
+			instance_change = true;
+		}
 
+		if (_local_position_last.timestamp != 0) {
 			// XY reset
-			if (local_position.xy_reset_counter > _local_position_last.xy_reset_counter) {
+			if (!instance_change && (local_position.xy_reset_counter == _local_position_last.xy_reset_counter + 1)) {
 				++_xy_reset_counter;
 				_delta_xy_reset = Vector2f{local_position.delta_xy};
+
+			} else if (instance_change || (local_position.xy_reset_counter != _local_position_last.xy_reset_counter)) {
+				++_xy_reset_counter;
+				_delta_xy_reset = Vector2f{local_position.x, local_position.y} - Vector2f{_local_position_last.x, _local_position_last.y};
 			}
 
 			// Z reset
-			if (local_position.z_reset_counter > _local_position_last.z_reset_counter) {
+			if (!instance_change && (local_position.z_reset_counter == _local_position_last.z_reset_counter + 1)) {
 				++_z_reset_counter;
 				_delta_z_reset = local_position.delta_z;
+
+			} else if (instance_change || (local_position.z_reset_counter != _local_position_last.z_reset_counter)) {
+				++_z_reset_counter;
+				_delta_z_reset = local_position.z - _local_position_last.z;
 			}
 
 			// VXY reset
-			if (local_position.vxy_reset_counter > _local_position_last.vxy_reset_counter) {
+			if (!instance_change && (local_position.vxy_reset_counter == _local_position_last.vxy_reset_counter + 1)) {
 				++_vxy_reset_counter;
 				_delta_vxy_reset = Vector2f{local_position.delta_vxy};
+
+			} else if (instance_change || (local_position.vxy_reset_counter != _local_position_last.vxy_reset_counter)) {
+				++_vxy_reset_counter;
+				_delta_vxy_reset = Vector2f{local_position.vx, local_position.vy} - Vector2f{_local_position_last.vx, _local_position_last.vy};
 			}
 
 			// VZ reset
-			if (local_position.vz_reset_counter > _local_position_last.vz_reset_counter) {
+			if (!instance_change && (local_position.vz_reset_counter == _local_position_last.vz_reset_counter + 1)) {
 				++_vz_reset_counter;
 				_delta_vz_reset = local_position.delta_vz;
+
+			} else if (instance_change || (local_position.vz_reset_counter != _local_position_last.vz_reset_counter)) {
+				++_vz_reset_counter;
+				_delta_vz_reset = local_position.vz - _local_position_last.vz;
 			}
 
 			// heading reset
-			if (local_position.heading_reset_counter > _local_position_last.heading_reset_counter) {
+			if (!instance_change && (local_position.heading_reset_counter == _local_position_last.heading_reset_counter + 1)) {
 				++_heading_reset_counter;
 				_delta_heading_reset = local_position.delta_heading;
+
+			} else if (instance_change || (local_position.heading_reset_counter != _local_position_last.heading_reset_counter)) {
+				++_heading_reset_counter;
+				_delta_heading_reset = matrix::wrap_pi(local_position.heading - _local_position_last.heading);
 			}
+
+		} else {
+			_xy_reset_counter = local_position.xy_reset_counter;
+			_z_reset_counter = local_position.z_reset_counter;
+			_vxy_reset_counter = local_position.vxy_reset_counter;
+			_vz_reset_counter = local_position.vz_reset_counter;
+			_heading_reset_counter = local_position.heading_reset_counter;
+
+			_delta_xy_reset = Vector2f{local_position.delta_xy};
+			_delta_z_reset = local_position.delta_z;
+			_delta_vxy_reset = Vector2f{local_position.delta_vxy};
+			_delta_vz_reset = local_position.delta_vz;
+			_delta_heading_reset = local_position.delta_heading;
 		}
 
-		// save last primary estimator_local_position
+		bool publish = true;
+
+		// ensure monotonically increasing timestamp_sample through reset, don't publish
+		//  estimator's local position for system (vehicle_local_position) if it's stale
+		if ((local_position.timestamp_sample <= _local_position_last.timestamp_sample)
+		    || (local_position.timestamp_sample < _instance[_selected_instance].timestamp_sample_last)) {
+
+			publish = false;
+		}
+
+		// save last primary estimator_local_position as published with original resets
 		_local_position_last = local_position;
 
-		// publish estimator's local position for system (vehicle_local_position) unless it's stale
-		if (local_position.timestamp_sample >= _instance[_selected_instance].timestamp_sample_last) {
+		if (publish) {
 			// republish with total reset count and current timestamp
 			local_position.xy_reset_counter = _xy_reset_counter;
 			local_position.z_reset_counter = _z_reset_counter;
@@ -447,47 +488,92 @@ void EKF2Selector::PublishVehicleLocalPosition(bool reset)
 	}
 }
 
-void EKF2Selector::PublishVehicleGlobalPosition(bool reset)
+void EKF2Selector::PublishVehicleOdometry()
 {
+	// selected estimator_odometry -> vehicle_odometry
+	vehicle_odometry_s odometry;
+
+	if (_instance[_selected_instance].estimator_odometry_sub.update(&odometry)) {
+		bool publish = true;
+
+		// ensure monotonically increasing timestamp_sample through reset, don't publish
+		//  estimator's odometry for system (vehicle_odometry) if it's stale
+		if ((odometry.timestamp_sample <= _odometry_last.timestamp_sample)
+		    || (odometry.timestamp_sample < _instance[_selected_instance].timestamp_sample_last)) {
+
+			publish = false;
+		}
+
+		// save last primary estimator_odometry
+		_odometry_last = odometry;
+
+		if (publish) {
+			odometry.timestamp = hrt_absolute_time();
+			_vehicle_odometry_pub.publish(odometry);
+		}
+	}
+}
+
+void EKF2Selector::PublishVehicleGlobalPosition()
+{
+	// selected estimator_global_position -> vehicle_global_position
 	vehicle_global_position_s global_position;
 
-	if (_instance[_selected_instance].estimator_global_position_sub.copy(&global_position)) {
-		if (reset) {
-			// on reset compute deltas from last published data
-			++_lat_lon_reset_counter;
-			++_alt_reset_counter;
+	if (_instance[_selected_instance].estimator_global_position_sub.update(&global_position)) {
+		bool instance_change = false;
 
-			_delta_lat_reset = global_position.lat - _global_position_last.lat;
-			_delta_lon_reset = global_position.lon - _global_position_last.lon;
-			_delta_alt_reset = global_position.delta_alt - _global_position_last.delta_alt;
-
-			// ensure monotonically increasing timestamp_sample through reset
-			global_position.timestamp_sample = max(global_position.timestamp_sample, _global_position_last.timestamp_sample);
-
-		} else {
-			// otherwise propogate deltas from estimator data while maintaining the overall reset counts
+		if (_instance[_selected_instance].estimator_global_position_sub.get_instance() != _global_position_instance_prev) {
+			_global_position_instance_prev = _instance[_selected_instance].estimator_global_position_sub.get_instance();
+			instance_change = true;
+		}
 
+		if (_global_position_last.timestamp != 0) {
 			// lat/lon reset
-			if (global_position.lat_lon_reset_counter > _global_position_last.lat_lon_reset_counter) {
+			if (!instance_change && (global_position.lat_lon_reset_counter == _global_position_last.lat_lon_reset_counter + 1)) {
 				++_lat_lon_reset_counter;
 
 				// TODO: delta latitude/longitude
-				//_delta_lat_reset = global_position.delta_lat;
-				//_delta_lon_reset = global_position.delta_lon;
+				_delta_lat_reset = global_position.lat - _global_position_last.lat;
+				_delta_lon_reset = global_position.lon - _global_position_last.lon;
+
+			} else if (instance_change || (global_position.lat_lon_reset_counter != _global_position_last.lat_lon_reset_counter)) {
+				++_lat_lon_reset_counter;
+
+				_delta_lat_reset = global_position.lat - _global_position_last.lat;
+				_delta_lon_reset = global_position.lon - _global_position_last.lon;
 			}
 
 			// alt reset
-			if (global_position.alt_reset_counter > _global_position_last.alt_reset_counter) {
+			if (!instance_change && (global_position.alt_reset_counter == _global_position_last.alt_reset_counter + 1)) {
 				++_alt_reset_counter;
 				_delta_alt_reset = global_position.delta_alt;
+
+			} else if (instance_change || (global_position.alt_reset_counter != _global_position_last.alt_reset_counter)) {
+				++_alt_reset_counter;
+				_delta_alt_reset = global_position.delta_alt - _global_position_last.delta_alt;
 			}
+
+		} else {
+			_lat_lon_reset_counter = global_position.lat_lon_reset_counter;
+			_alt_reset_counter = global_position.alt_reset_counter;
+
+			_delta_alt_reset = global_position.delta_alt;
 		}
 
-		// save last primary estimator_global_position
+		bool publish = true;
+
+		// ensure monotonically increasing timestamp_sample through reset, don't publish
+		//  estimator's global position for system (vehicle_global_position) if it's stale
+		if ((global_position.timestamp_sample <= _global_position_last.timestamp_sample)
+		    || (global_position.timestamp_sample < _instance[_selected_instance].timestamp_sample_last)) {
+
+			publish = false;
+		}
+
+		// save last primary estimator_global_position as published with original resets
 		_global_position_last = global_position;
 
-		// publish estimator's global position for system (vehicle_global_position) unless it's stale
-		if (global_position.timestamp_sample >= _instance[_selected_instance].timestamp_sample_last) {
+		if (publish) {
 			// republish with total reset count and current timestamp
 			global_position.lat_lon_reset_counter = _lat_lon_reset_counter;
 			global_position.alt_reset_counter = _alt_reset_counter;
@@ -499,22 +585,31 @@ void EKF2Selector::PublishVehicleGlobalPosition(bool reset)
 	}
 }
 
-void EKF2Selector::PublishWindEstimate(bool reset)
+void EKF2Selector::PublishWindEstimate()
 {
+	// selected estimator_wind -> wind
 	wind_s wind;
 
-	if (_instance[_selected_instance].estimator_wind_sub.copy(&wind)) {
-		if (reset) {
-			// ensure monotonically increasing timestamp_sample through reset
-			wind.timestamp_sample = max(wind.timestamp_sample, _wind_last.timestamp_sample);
+	if (_instance[_selected_instance].estimator_wind_sub.update(&wind)) {
+		bool publish = true;
+
+		// ensure monotonically increasing timestamp_sample through reset, don't publish
+		//  estimator's wind for system (wind) if it's stale
+		if ((wind.timestamp_sample <= _wind_last.timestamp_sample)
+		    || (wind.timestamp_sample < _instance[_selected_instance].timestamp_sample_last)) {
+
+			publish = false;
 		}
 
 		// save last primary wind
 		_wind_last = wind;
 
-		// republish with current timestamp
-		wind.timestamp = hrt_absolute_time();
-		_wind_pub.publish(wind);
+		// publish estimator's wind for system unless it's stale
+		if (publish) {
+			// republish with current timestamp
+			wind.timestamp = hrt_absolute_time();
+			_wind_pub.publish(wind);
+		}
 	}
 }
 
@@ -655,38 +750,11 @@ void EKF2Selector::Run()
 	}
 
 	// republish selected estimator data for system
-
-	// selected estimator_attitude -> vehicle_attitude
-	if (_instance[_selected_instance].estimator_attitude_sub.updated()) {
-		PublishVehicleAttitude();
-	}
-
-	// selected estimator_local_position -> vehicle_local_position
-	if (_instance[_selected_instance].estimator_local_position_sub.updated()) {
-		PublishVehicleLocalPosition();
-	}
-
-	// selected estimator_global_position -> vehicle_global_position
-	if (_instance[_selected_instance].estimator_global_position_sub.updated()) {
-		PublishVehicleGlobalPosition();
-	}
-
-	// selected estimator_wind -> wind
-	if (_instance[_selected_instance].estimator_wind_sub.updated()) {
-		PublishWindEstimate();
-	}
-
-	// selected estimator_odometry -> vehicle_odometry
-	if (_instance[_selected_instance].estimator_odometry_sub.updated()) {
-		vehicle_odometry_s vehicle_odometry;
-
-		if (_instance[_selected_instance].estimator_odometry_sub.update(&vehicle_odometry)) {
-			if (vehicle_odometry.timestamp_sample >= _instance[_selected_instance].timestamp_sample_last) {
-				vehicle_odometry.timestamp = hrt_absolute_time();
-				_vehicle_odometry_pub.publish(vehicle_odometry);
-			}
-		}
-	}
+	PublishVehicleAttitude();
+	PublishVehicleLocalPosition();
+	PublishVehicleGlobalPosition();
+	PublishVehicleOdometry();
+	PublishWindEstimate();
 }
 
 void EKF2Selector::PrintStatus()

src/modules/ekf2/EKF2Selector.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -78,10 +78,11 @@ private:
 	static constexpr uint64_t FILTER_UPDATE_PERIOD{10_ms};
 
 	void Run() override;
-	void PublishVehicleAttitude(bool reset = false);
-	void PublishVehicleLocalPosition(bool reset = false);
-	void PublishVehicleGlobalPosition(bool reset = false);
-	void PublishWindEstimate(bool reset = false);
+	void PublishVehicleAttitude();
+	void PublishVehicleLocalPosition();
+	void PublishVehicleGlobalPosition();
+	void PublishVehicleOdometry();
+	void PublishWindEstimate();
 	bool SelectInstance(uint8_t instance);
 
 	// Update the error scores for all available instances
@@ -191,6 +192,9 @@ private:
 	uint8_t _vz_reset_counter{0};
 	uint8_t _heading_reset_counter{0};
 
+	// vehicle_odometry
+	vehicle_odometry_s _odometry_last{};
+
 	// vehicle_global_position: reset counters
 	vehicle_global_position_s _global_position_last{};
 	double _delta_lat_reset{0};
@@ -202,6 +206,10 @@ private:
 	// wind estimate
 	wind_s _wind_last{};
 
+	uint8_t _attitude_instance_prev{INVALID_INSTANCE};
+	uint8_t _local_position_instance_prev{INVALID_INSTANCE};
+	uint8_t _global_position_instance_prev{INVALID_INSTANCE};
+
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 	uORB::Subscription _sensors_status_imu{ORB_ID(sensors_status_imu)};
 

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp

@@ -148,23 +148,22 @@ void VehicleAngularVelocity::ResetFilters(float new_scale)
 {
 	if ((_filter_sample_rate_hz > 0) && PX4_ISFINITE(_filter_sample_rate_hz)) {
 
-		const Vector3f angular_velocity_raw{GetResetAngularVelocity(_angular_velocity, new_scale)};
-		const Vector3f angular_velocity_raw_prev{GetResetAngularVelocity(_angular_velocity_prev, new_scale)};
-		const Vector3f angular_acceleration_raw{GetResetAngularAcceleration(new_scale)};
+		const Vector3f angular_velocity{GetResetAngularVelocity(new_scale)};
+		const Vector3f angular_acceleration{GetResetAngularAcceleration(new_scale)};
 
 		for (int axis = 0; axis < 3; axis++) {
 			// angular velocity low pass
 			_lp_filter_velocity[axis].set_cutoff_frequency(_filter_sample_rate_hz, _param_imu_gyro_cutoff.get());
-			_lp_filter_velocity[axis].reset(angular_velocity_raw(axis));
+			_lp_filter_velocity[axis].reset(angular_velocity(axis));
 
 			// angular velocity notch
 			_notch_filter_velocity[axis].setParameters(_filter_sample_rate_hz, _param_imu_gyro_nf_freq.get(),
 					_param_imu_gyro_nf_bw.get());
-			_notch_filter_velocity[axis].reset(angular_velocity_raw(axis));
+			_notch_filter_velocity[axis].reset(angular_velocity(axis));
 
 			// angular acceleration low pass
 			_lp_filter_acceleration[axis].set_cutoff_frequency(_filter_sample_rate_hz, _param_imu_dgyro_cutoff.get());
-			_lp_filter_acceleration[axis].reset(angular_acceleration_raw(axis));
+			_lp_filter_acceleration[axis].reset(angular_acceleration(axis));
 		}
 
 		// dynamic notch filters, first disable, then force update (if available)
@@ -174,8 +173,7 @@ void VehicleAngularVelocity::ResetFilters(float new_scale)
 		UpdateDynamicNotchEscRpm(new_scale, true);
 		UpdateDynamicNotchFFT(new_scale, true);
 
-		_angular_velocity_raw_prev = angular_velocity_raw;
-		_angular_velocity_raw_prev_prev = angular_velocity_raw_prev;
+		_angular_velocity_prev = angular_velocity;
 		_last_scale = new_scale;
 
 		_reset_filters = false;
@@ -352,18 +350,18 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
 	}
 }
 
-Vector3f VehicleAngularVelocity::GetResetAngularVelocity(const Vector3f &angular_velocity, float new_scale) const
+Vector3f VehicleAngularVelocity::GetResetAngularVelocity(float new_scale) const
 {
 	if ((_last_publish != 0) && (new_scale > 0.f)) {
 		// angular velocity filtering is performed on raw unscaled data
 		//  start with last valid vehicle body frame angular velocity and compute equivalent raw data (for current sensor selection)
-		Vector3f angular_velocity_raw{_calibration.Uncorrect(angular_velocity + _bias) / new_scale};
+		Vector3f angular_velocity{_calibration.Uncorrect(_angular_velocity + _bias) / new_scale};
 
-		if (PX4_ISFINITE(angular_velocity_raw(0))
-		    && PX4_ISFINITE(angular_velocity_raw(1))
-		    && PX4_ISFINITE(angular_velocity_raw(2))) {
+		if (PX4_ISFINITE(angular_velocity(0))
+		    && PX4_ISFINITE(angular_velocity(1))
+		    && PX4_ISFINITE(angular_velocity(2))) {
 
-			return angular_velocity_raw;
+			return angular_velocity;
 		}
 	}
 
@@ -375,13 +373,13 @@ Vector3f VehicleAngularVelocity::GetResetAngularAcceleration(float new_scale) co
 	if ((_last_publish != 0) && (new_scale > 0.f)) {
 		// angular acceleration filtering is performed on unscaled angular velocity data
 		//  start with last valid vehicle body frame angular acceleration and compute equivalent raw data (for current sensor selection)
-		Vector3f angular_acceleration_raw{_calibration.rotation().I() *_angular_acceleration / new_scale};
+		Vector3f angular_acceleration{_calibration.rotation().I() *_angular_acceleration / new_scale};
 
-		if (PX4_ISFINITE(angular_acceleration_raw(0))
-		    && PX4_ISFINITE(angular_acceleration_raw(1))
-		    && PX4_ISFINITE(angular_acceleration_raw(2))) {
+		if (PX4_ISFINITE(angular_acceleration(0))
+		    && PX4_ISFINITE(angular_acceleration(1))
+		    && PX4_ISFINITE(angular_acceleration(2))) {
 
-			return angular_acceleration_raw;
+			return angular_acceleration;
 		}
 	}
 
@@ -454,7 +452,7 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(float new_scale, bool forc
 
 							// only reset if there's sufficient change (> 1%)
 							if (force || (change_percent > 0.01f)) {
-								const Vector3f reset_angular_velocity{GetResetAngularVelocity(_angular_velocity, new_scale)};
+								const Vector3f reset_angular_velocity{GetResetAngularVelocity(new_scale)};
 
 								for (int axis = 0; axis < 3; axis++) {
 									auto &dnf = _dynamic_notch_filter_esc_rpm[i][harmonic][axis];
@@ -513,7 +511,7 @@ void VehicleAngularVelocity::UpdateDynamicNotchFFT(float new_scale, bool force)
 
 							// only reset if there's sufficient change
 							if (peak_diff_abs > sensor_gyro_fft.resolution_hz) {
-								dnf.reset(GetResetAngularVelocity(_angular_velocity, new_scale)(axis));
+								dnf.reset(GetResetAngularVelocity(new_scale)(axis));
 							}
 
 							perf_count(_dynamic_notch_filter_fft_update_perf);
@@ -585,6 +583,20 @@ float VehicleAngularVelocity::FilterAngularVelocity(int axis, float data[], int
 	return data[N - 1];
 }
 
+float VehicleAngularVelocity::FilterAngularAcceleration(int axis, float dt_s, float data[], int N)
+{
+	// angular acceleration: Differentiate & apply specific angular acceleration (D-term) low-pass (IMU_DGYRO_CUTOFF)
+	float angular_acceleration_filtered = 0.f;
+
+	for (int n = 0; n < N; n++) {
+		const float angular_acceleration = (data[n] - _angular_velocity_prev(axis)) / dt_s;
+		angular_acceleration_filtered = _lp_filter_acceleration[axis].apply(angular_acceleration);
+		_angular_velocity_prev(axis) = data[n];
+	}
+
+	return angular_acceleration_filtered;
+}
+
 void VehicleAngularVelocity::Run()
 {
 	// backup schedule
@@ -644,18 +656,7 @@ void VehicleAngularVelocity::Run()
 
 					// save last filtered sample
 					angular_velocity_unscaled(axis) = FilterAngularVelocity(axis, data, N);
-
-					// angular acceleration: Differentiate & apply specific angular acceleration (D-term) low-pass (IMU_DGYRO_CUTOFF)
-					for (int n = 0; n < N; n++) {
-						// Backward finite difference (1st derivative 3 coffecients [1/2, -2, 3/2])
-						const float angular_acceleration = (+ 0.5f * _angular_velocity_raw_prev_prev(axis)
-										    - 2.0f * _angular_velocity_raw_prev(axis)
-										    + 1.5f * data[n]) / dt_s;
-						_angular_velocity_raw_prev_prev(axis) = _angular_velocity_raw_prev(axis);
-						_angular_velocity_raw_prev(axis) = data[n];
-
-						angular_acceleration_unscaled(axis) = _lp_filter_acceleration[axis].apply(angular_acceleration);
-					}
+					angular_acceleration_unscaled(axis) = FilterAngularAcceleration(axis, dt_s, data, N);
 				}
 
 				// Publish
@@ -698,11 +699,7 @@ void VehicleAngularVelocity::Run()
 
 				// save last filtered sample
 				angular_velocity(axis) = FilterAngularVelocity(axis, data);
-
-				angular_acceleration(axis) = _lp_filter_acceleration[axis].apply((raw_data_array[axis]
-							     - _angular_velocity_raw_prev(axis)) / dt_s);
-				_angular_velocity_raw_prev_prev(axis) = _angular_velocity_raw_prev(axis);
-				_angular_velocity_raw_prev(axis) = raw_data_array[axis];
+				angular_acceleration(axis) = FilterAngularAcceleration(axis, dt_s, data);
 			}
 
 			// Publish
@@ -715,7 +712,6 @@ void VehicleAngularVelocity::CalibrateAndPublish(bool publish, const hrt_abstime
 		const Vector3f &angular_velocity_unscaled,  const Vector3f &angular_acceleration_unscaled, float scale)
 {
 	// Angular velocity: rotate sensor frame to board, scale raw data to SI, apply calibration, and remove in-run estimated bias
-	_angular_velocity_prev = _angular_velocity;
 	_angular_velocity = _calibration.Correct(angular_velocity_unscaled * scale) - _bias;
 
 	// Angular acceleration: rotate sensor frame to board, scale raw data to SI, apply any additional configured rotation

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp

@@ -79,6 +79,7 @@ private:
 				 const matrix::Vector3f &angular_acceleration, float scale = 1.f);
 
 	float FilterAngularVelocity(int axis, float data[], int N = 1);
+	float FilterAngularAcceleration(int axis, float dt_s, float data[], int N = 1);
 
 	void DisableDynamicNotchEscRpm();
 	void DisableDynamicNotchFFT();
@@ -92,7 +93,7 @@ private:
 	bool UpdateSampleRate();
 
 	// scaled appropriately for current FIFO mode
-	matrix::Vector3f GetResetAngularVelocity(const matrix::Vector3f &angular_velocity, float new_scale = 1.f) const;
+	matrix::Vector3f GetResetAngularVelocity(float new_scale = 1.f) const;
 	matrix::Vector3f GetResetAngularAcceleration(float new_scale = 1.f) const;
 
 	static constexpr int MAX_SENSOR_COUNT = 4;
@@ -118,11 +119,9 @@ private:
 	matrix::Vector3f _bias{};
 
 	matrix::Vector3f _angular_velocity{};
-	matrix::Vector3f _angular_velocity_prev{};
 	matrix::Vector3f _angular_acceleration{};
 
-	matrix::Vector3f _angular_velocity_raw_prev{};
-	matrix::Vector3f _angular_velocity_raw_prev_prev{};
+	matrix::Vector3f _angular_velocity_prev{};
 	hrt_abstime _timestamp_sample_last{0};
 
 	hrt_abstime _publish_interval_min_us{0};

src/modules/vtol_att_control/vtol_type.cpp

@@ -332,7 +332,7 @@ bool VtolType::set_idle_fw()
 
 	for (int i = 0; i < num_outputs_max; i++) {
 		if (is_channel_set(i, generate_bitmap_from_channel_numbers(_params->vtol_motor_id))) {
-			pwm_values.values[i] = PWM_MOTOR_OFF;
+			pwm_values.values[i] = PWM_DEFAULT_MIN;
 
 		} else {
 			pwm_values.values[i] = _min_mc_pwm_values.values[i];