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

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

boards/px4/fmu-v2/nuttx-config/nsh/defconfig

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

src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp

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

src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp

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

src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp

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

src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp

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

src/modules/commander/airspeed_calibration.cpp

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

src/modules/ekf2/EKF2.cpp

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

src/modules/ekf2/EKF2Selector.cpp

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

src/modules/ekf2/EKF2Selector.hpp

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