Use Rangefinder

A user configurable delay for the internal `vehicle_local_position` seems confusing in my eyes. It's a different timeout for fixed-wing and multirotor which might have made sense earlier but not really anymore since the topic is constantly published by the estimator and not expected to time out on either vehicle type and the parameter description is also misleading because it's outdated.

.ci/Jenkinsfile-compile

@@ -1,233 +0,0 @@
-#!/usr/bin/env groovy
-
-pipeline {
-  agent none
-  stages {
-
-    stage('Build') {
-      steps {
-        script {
-          def build_nodes = [:]
-          def docker_images = [
-            armhf: "px4io/px4-dev-armhf:2023-06-26",
-            arm64: "px4io/px4-dev-aarch64:2022-08-12",
-            base: "px4io/px4-dev-ros2-foxy:2022-08-12",
-            nuttx: "px4io/px4-dev-nuttx-focal:2022-08-12",
-          ]
-
-          def armhf_builds = [
-            target: ["beaglebone_blue_default", "emlid_navio2_default", "px4_raspberrypi_default", "scumaker_pilotpi_default"],
-            image: docker_images.armhf,
-            archive: false
-          ]
-
-          def arm64_builds = [
-            target: ["scumaker_pilotpi_arm64"],
-            image: docker_images.arm64,
-            archive: false
-          ]
-
-          def base_builds = [
-            target: ["px4_sitl_default"],
-            image: docker_images.base,
-            archive: false
-          ]
-
-          def nuttx_builds_archive = [
-            target: [
-                     "3dr_ctrl-zero-h7-oem-revg_default",
-                     "airmind_mindpx-v2_default",
-                     "ark_can-flow_canbootloader",
-                     "ark_can-flow_default",
-                     "ark_can-gps_canbootloader",
-                     "ark_can-gps_default",
-                     "ark_can-rtk-gps_canbootloader",
-                     "ark_can-rtk-gps_default",
-                     "ark_cannode_canbootloader",
-                     "ark_cannode_default",
-                     "ark_fmu-v6x_bootloader",
-                     "ark_fmu-v6x_default",
-                     "ark_fpv_bootloader",
-                     "ark_fpv_default",
-                     "ark_pi6x_bootloader",
-                     "ark_pi6x_default",
-                     "atl_mantis-edu_default",
-                     "av_x-v1_default",
-                     "bitcraze_crazyflie21_default",
-                     "bitcraze_crazyflie_default",
-                     "cuav_7-nano_default",
-                     "cuav_can-gps-v1_canbootloader",
-                     "cuav_can-gps-v1_default",
-                     "cuav_nora_default",
-                     "cuav_x7pro_default",
-                     "cubepilot_cubeorange_default",
-                     "cubepilot_cubeorangeplus_default",
-                     "cubepilot_cubeyellow_default",
-                     "diatone_mamba-f405-mk2_default",
-                     "flywoo_gn-f405_default",
-                     "freefly_can-rtk-gps_canbootloader",
-                     "freefly_can-rtk-gps_default",
-                     "holybro_can-gps-v1_canbootloader",
-                     "holybro_can-gps-v1_default",
-                     "holybro_durandal-v1_default",
-                     "holybro_kakutef7_default",
-                     "holybro_kakuteh7_default",
-                     "holybro_kakuteh7mini_default",
-                     "holybro_kakuteh7v2_default",
-                     "holybro_pix32v5_default",
-                     "matek_gnss-m9n-f4_canbootloader",
-                     "matek_gnss-m9n-f4_default",
-                     "matek_h743-mini_default",
-                     "matek_h743-slim_default",
-                     "matek_h743_default",
-                     "micoair_h743_default",
-                     "micoair_h743-aio_default",
-                     "modalai_fc-v1_default",
-                     "modalai_fc-v2_default",
-                     "mro_ctrl-zero-classic_default",
-                     "mro_ctrl-zero-f7-oem_default",
-                     "mro_ctrl-zero-f7_default",
-                     "mro_ctrl-zero-h7-oem_default",
-                     "mro_ctrl-zero-h7_default",
-                     "mro_pixracerpro_default",
-                     "mro_x21-777_default",
-                     "mro_x21_default",
-                     "nxp_fmuk66-e_default",
-                     "nxp_fmuk66-e_socketcan",
-                     "nxp_fmuk66-v3_default",
-                     "nxp_fmuk66-v3_socketcan",
-                     "nxp_mr-canhubk3_default",
-                     "nxp_mr-canhubk3_fmu",
-                     "nxp_ucans32k146_canbootloader",
-                     "nxp_ucans32k146_default",
-                     "nxp_tropic-community_default",
-                     "omnibus_f4sd_default",
-                     "px4_fmu-v2_default",
-                     "px4_fmu-v2_fixedwing",
-                     "px4_fmu-v2_lto",
-                     "px4_fmu-v2_multicopter",
-                     "px4_fmu-v2_rover",
-                     "px4_fmu-v3_default",
-                     "px4_fmu-v4_default",
-                     "px4_fmu-v4pro_default",
-                     "px4_fmu-v5_cyphal",
-                     "px4_fmu-v5_debug",
-                     "px4_fmu-v5_default",
-                     "px4_fmu-v5_lto",
-                     "px4_fmu-v5_rover",
-                     "px4_fmu-v5_stackcheck",
-                     "px4_fmu-v5_uavcanv0periph",
-                     "px4_fmu-v5x_default",
-                     "px4_fmu-v5x_rover",
-                     "px4_fmu-v6c_default",
-                     "px4_fmu-v6c_rover",
-                     "px4_fmu-v6u_default",
-                     "px4_fmu-v6u_rover",
-                     "px4_fmu-v6x_default",
-                     "px4_fmu-v6x_rover",
-                     "px4_fmu-v6xrt_bootloader",
-                     "px4_fmu-v6xrt_default",
-                     "px4_fmu-v6xrt_rover",
-                     "px4_io-v2_default",
-                     "raspberrypi_pico_default",
-                     "siyi_n7_default",
-                     "sky-drones_smartap-airlink_default",
-                     "spracing_h7extreme_default",
-                     "thepeach_k1_default",
-                     "thepeach_r1_default",
-                     "uvify_core_default",
-                     "x-mav_ap-h743v2_default",
-                     "zeroone_x6_bootloader",
-                     "zeroone_x6_default",
-            ],
-            image: docker_images.nuttx,
-            archive: true
-          ]
-
-          def docker_builds = [
-            armhf_builds, base_builds, nuttx_builds_archive
-          ]
-
-          for (def build_type = 0; build_type < docker_builds.size(); build_type++) {
-            for (def build_target = 0; build_target < docker_builds[build_type].target.size(); build_target++) {
-              build_nodes.put(docker_builds[build_type].target[build_target],
-                createBuildNode(docker_builds[build_type].archive, docker_builds[build_type].image, docker_builds[build_type].target[build_target])
-                )
-            }
-          }
-
-        parallel build_nodes
-
-        } // script
-      } // steps
-    } // stage Build
-
-    // TODO: actually upload artifacts to S3
-    // stage('S3 Upload') {
-    //   agent {
-    //     docker { image 'px4io/px4-dev-base-focal:2021-09-08' }
-    //   }
-    //   options {
-    //         skipDefaultCheckout()
-    //   }
-    //   when {
-    //     anyOf {
-    //       branch 'master'
-    //       branch 'beta'
-    //       branch 'stable'
-    //       branch 'pr-jenkins' // for testing
-    //     }
-    //   }
-    //   steps {
-    //     sh 'echo "uploading to S3"'
-    //   }
-    // }
-
-  } // stages
-  environment {
-    CCACHE_DIR = '/tmp/ccache'
-    CI = true
-  }
-  options {
-    buildDiscarder(logRotator(numToKeepStr: '5', artifactDaysToKeepStr: '14'))
-    timeout(time: 120, unit: 'MINUTES')
-  }
-}
-
-def createBuildNode(Boolean archive, String docker_image, String target) {
-  return {
-
-    bypass_entrypoint = ''
-
-    node {
-      docker.withRegistry('https://registry.hub.docker.com', 'docker_hub_dagar') {
-        docker.image(docker_image).inside('-e CCACHE_BASEDIR=${WORKSPACE} -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' + bypass_entrypoint) {
-          stage(target) {
-            try {
-              sh('export')
-              checkout(scm)
-              sh('make distclean; git clean -ff -x -d .')
-              sh('git fetch --tags')
-              sh('ccache -s')
-              sh('make ' + target)
-              sh('ccache -s')
-              sh('make sizes')
-              if (archive) {
-                archiveArtifacts(allowEmptyArchive: false, artifacts: 'build/*/*.px4, build/*/*.elf, build/*/*.bin', fingerprint: true, onlyIfSuccessful: true)
-              }
-              sh('make ' + target + ' package')
-              archiveArtifacts(allowEmptyArchive: true, artifacts: 'build/*/*.tar.bz2', fingerprint: true, onlyIfSuccessful: true)
-              archiveArtifacts(allowEmptyArchive: true, artifacts: 'build/*/*.deb', fingerprint: true, onlyIfSuccessful: true)
-            }
-            catch (exc) {
-              throw (exc)
-            }
-            finally {
-              sh('make distclean; git clean -ff -x -d .')
-            }
-          }
-        }
-      }
-    }
-  }
-}

.ci/Jenkinsfile-hardware

@@ -1,904 +0,0 @@
-#!/usr/bin/env groovy
-
-pipeline {
-  agent none
-  stages {
-    stage('Hardware Test') {
-
-      parallel {
-
-        stage("cubepilot_cubeorange_test") {
-          stages {
-            stage("build cubepilot_cubeorange_test") {
-              agent {
-                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2022-08-12'
-                  args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
-                }
-              }
-              steps {
-                checkoutSCM()
-                sh 'make cubepilot_cubeorange_bootloader'
-                sh 'make cubepilot_cubeorange_test'
-                sh 'ccache -s'
-                stash includes: 'build/*/*.elf, platforms/nuttx/Debug/*, platforms/nuttx/NuttX/nuttx/tools/nuttx-gdbinit, Tools/HIL/*, build/cubepilot_cubeorange_test/etc/init.d/airframes/*', name: 'cubepilot_cubeorange_test'
-              }
-              post {
-                always {
-                  sh 'make distclean; git clean -ff -x -d .'
-                }
-              }
-            } // stage build
-            stage("hardware") {
-              agent {
-                label 'cubepilot_cubeorange'
-              }
-              stages {
-                stage("flash") {
-                  steps {
-                    sh 'export'
-                    sh 'find /dev/serial'
-                    unstash 'cubepilot_cubeorange_test'
-                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/cubepilot_cubeorange_bootloader/cubepilot_cubeorange_bootloader.elf'
-                    // flash board and watch bootup
-                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/cubepilot_cubeorange_test/cubepilot_cubeorange_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600'
-                    resetBoard()
-                  }
-                }
-                stage("tests") {
-                  steps {
-                    // run tests
-                    runTests()
-
-                    // load all airframes
-                    // sh("./Tools/HIL/test_airframes.sh `find /dev/serial -name *usb-*` `cd build/cubepilot_cubeorange_test/etc/init.d/airframes/; find . -regex '.*/[0-9].*' -exec basename {} \\; | cut -d '_' -f 1` || true") // test loading all airframes\
-                  }
-                }
-                stage("status") {
-                  steps {
-                    // configure
-                    resetParameters()
-                    sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "IMU_GYRO_RATEMAX" --value "2000"'
-                    sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_AUTOSTART" --value "13000"'   // generic vtol standard
-                    sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_BL_UPDATE" --value "1"'       // update bootloader
-                    sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply
-
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger on"' // run logger
-                    checkStatus()
-                    quickCalibrate()
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "px4io status"'
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger off"' // stop logger
-                  }
-                }
-                stage("print topics") {
-                  steps {
-                    printTopics()
-                  }
-                }
-              }
-              post {
-                always {
-                  sh 'cat /tmp/pyserial_spy_file.txt || true'
-                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/cubepilot_cubeorange_test/cubepilot_cubeorange_test.elf || true'
-                }
-              }
-            } // stage test
-          }
-        }
-
-        stage("cuav_x7pro_test") {
-          stages {
-            stage("build cuav_x7pro_test") {
-              agent {
-                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2022-08-12'
-                  args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
-                }
-              }
-              steps {
-                checkoutSCM()
-                sh 'make cuav_x7pro_bootloader'
-                sh 'make cuav_x7pro_test'
-                sh 'ccache -s'
-                stash includes: 'build/*/*.elf, platforms/nuttx/Debug/*, platforms/nuttx/NuttX/nuttx/tools/nuttx-gdbinit, Tools/HIL/*', name: 'cuav_x7pro_test'
-              }
-              post {
-                always {
-                  sh 'make distclean; git clean -ff -x -d .'
-                }
-              }
-            } // stage build
-            stage("hardware") {
-              agent {
-                label 'cuav_x7pro'
-              }
-              stages {
-                stage("flash") {
-                  steps {
-                    sh 'export'
-                    sh 'find /dev/serial'
-                    unstash 'cuav_x7pro_test'
-                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/cuav_x7pro_bootloader/cuav_x7pro_bootloader.elf'
-                    // flash board and watch bootup
-                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/cuav_x7pro_test/cuav_x7pro_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600'
-                    resetBoard()
-                  }
-                }
-                stage("tests") {
-                  steps {
-                    runTests()
-                  }
-                }
-                stage("status") {
-                  steps {
-                    // configure
-                    resetParameters()
-                    sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "IMU_GYRO_RATEMAX" --value "2000"'
-                    sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_AUTOSTART" --value "13000"'   // generic vtol standard
-                    sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_BL_UPDATE" --value "1"'       // update bootloader
-                    sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply
-
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger on"' // run logger
-                    checkStatus()
-                    quickCalibrate()
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger off"' // stop logger
-                  }
-                }
-                stage("print topics") {
-                  steps {
-                    printTopics()
-                  }
-                }
-              }
-              post {
-                always {
-                  sh 'cat /tmp/pyserial_spy_file.txt || true'
-                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/cuav_x7pro_test/cuav_x7pro_test.elf || true'
-                }
-              }
-            } // stage test
-          }
-        }
-
-        stage("px4_fmu-v4_test") {
-          stages {
-            stage("build px4_fmu-v4_test") {
-              agent {
-                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2022-08-12'
-                  args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
-                }
-              }
-              steps {
-                checkoutSCM()
-                sh 'make px4_fmu-v4_test'
-                sh 'make px4_fmu-v4_test bootloader_elf'
-                sh 'ccache -s'
-                stash includes: 'build/*/*.elf, platforms/nuttx/Debug/*, platforms/nuttx/NuttX/nuttx/tools/nuttx-gdbinit, Tools/HIL/*', name: 'px4_fmu-v4_test'
-              }
-              post {
-                always {
-                  sh 'make distclean; git clean -ff -x -d .'
-                }
-              }
-            } // stage build
-            stage("hardware") {
-              agent {
-                label 'px4_fmu-v4'
-              }
-              stages {
-                stage("flash") {
-                  steps {
-                    sh 'export'
-                    sh 'find /dev/serial'
-                    unstash 'px4_fmu-v4_test'
-                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v4_test/px4_fmu-v4_bootloader.elf'
-                    // flash board and watch bootup
-                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v4_test/px4_fmu-v4_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600'
-                    resetBoard()
-                  }
-                }
-                stage("tests") {
-                  steps {
-                    runTests()
-                  }
-                }
-                stage("status") {
-                  steps {
-                    // configure
-                    resetParameters()
-                    sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_AUTOSTART" --value "4001"'    // generic quadcopter
-                    sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_BL_UPDATE" --value "1"'       // update bootloader
-                    sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply
-
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger on"' // run logger
-                    checkStatus()
-                    quickCalibrate()
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger off"' // stop logger
-                  }
-                }
-                stage("print topics") {
-                  steps {
-                    printTopics()
-                  }
-                }
-              }
-              post {
-                always {
-                  sh 'cat /tmp/pyserial_spy_file.txt || true'
-                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v4_test/px4_fmu-v4_test.elf || true'
-                }
-              }
-            } // stage test
-          }
-        }
-
-        stage("px4_fmu-v4pro_test") {
-          stages {
-            stage("build px4_fmu-v4pro_test") {
-              agent {
-                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2022-08-12'
-                  args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
-                }
-              }
-              steps {
-                checkoutSCM()
-                sh 'make px4_fmu-v4pro_test'
-                sh 'make px4_fmu-v4pro_test bootloader_elf'
-                sh 'ccache -s'
-                stash includes: 'build/*/*.elf, platforms/nuttx/Debug/*, platforms/nuttx/NuttX/nuttx/tools/nuttx-gdbinit, Tools/HIL/*', name: 'px4_fmu-v4pro_test'
-              }
-              post {
-                always {
-                  sh 'make distclean; git clean -ff -x -d .'
-                }
-              }
-            } // stage build
-            stage("hardware") {
-              agent {
-                label 'px4_fmu-v4pro'
-              }
-              stages {
-                stage("flash") {
-                  steps {
-                    sh 'export'
-                    sh 'find /dev/serial'
-                    unstash 'px4_fmu-v4pro_test'
-                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v4pro_test/px4_fmu-v4pro_bootloader.elf'
-                    // flash board and watch bootup
-                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v4pro_test/px4_fmu-v4pro_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600'
-                    resetBoard()
-                  }
-                }
-                stage("tests") {
-                  steps {
-                    runTests()
-                  }
-                }
-                stage("status") {
-                  steps {
-                    // configure
-                    resetParameters()
-                    sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_AUTOSTART" --value "13000"'   // generic vtol standard
-                    sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_BL_UPDATE" --value "1"'       // update bootloader
-                    sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply
-
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger on"' // run logger
-                    checkStatus()
-                    quickCalibrate()
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "px4io status"'
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger off"' // stop logger
-                  }
-                }
-                stage("print topics") {
-                  steps {
-                    printTopics()
-                  }
-                }
-              }
-              post {
-                always {
-                  sh 'cat /tmp/pyserial_spy_file.txt || true'
-                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v4pro_test/px4_fmu-v4pro_test.elf || true'
-                }
-              }
-            } // stage test
-          }
-        }
-
-        stage("px4_fmu-v5_debug") {
-          stages {
-            stage("build px4_fmu-v5_debug") {
-              agent {
-                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2022-08-12'
-                  args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
-                }
-              }
-              steps {
-                checkoutSCM()
-                sh 'make px4_fmu-v5_debug'
-                sh 'make px4_fmu-v5_debug bootloader_elf'
-                sh 'ccache -s'
-                stash includes: 'build/*/*.elf, platforms/nuttx/Debug/*, platforms/nuttx/NuttX/nuttx/tools/nuttx-gdbinit, Tools/HIL/*', name: 'px4_fmu-v5_debug'
-              }
-              post {
-                always {
-                  sh 'make distclean; git clean -ff -x -d .'
-                }
-              }
-            } // stage build
-            stage("hardware") {
-              agent {
-                label 'px4_fmu-v5'
-              }
-              stages {
-                stage("flash") {
-                  steps {
-                    sh 'export'
-                    sh 'find /dev/serial'
-                    unstash 'px4_fmu-v5_debug'
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 0" || true'
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set IMU_GYRO_RATEMAX 200" || true' // limit cpu usage
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_0_CONFIG 0" || true'       // limit cpu usage
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_1_CONFIG 0" || true'       // limit cpu usage
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param save" || true'
-                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v5_debug/px4_fmu-v5_bootloader.elf'
-                    // flash board and watch bootup
-                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v5_debug/px4_fmu-v5_debug.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600 || true'
-                    resetBoard()
-                  }
-                }
-                stage("tests") {
-                  steps {
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "top once"'
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb top -1"'
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "work_queue status"'
-
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_bench"'
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_bench -v"'
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_bench -u -v" || true'
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_stress"'
-
-                    // test dataman
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "tests dataman" --ignore-stdout-errors'
-
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "tests file" || true'
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb_tests"'
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb_tests latency_test" || true'
-                  }
-                }
-                stage("status") {
-                  steps {
-                    // configure
-                    resetParameters()
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 4001" || true'   // generic quadcopter
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set IMU_GYRO_RATEMAX 200" || true' // limit cpu usage
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_0_CONFIG 0" || true'       // limit cpu usage
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_1_CONFIG 0" || true'       // limit cpu usage
-                    sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply
-                    checkStatus()
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "px4io status" || true'
-                  }
-                }
-                stage("print topics") {
-                  steps {
-                    printTopics()
-                  }
-                }
-              }
-              post {
-                always {
-                  sh 'cat /tmp/pyserial_spy_file.txt || true'
-                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v5_debug/px4_fmu-v5_debug.elf || true'
-                }
-              }
-            } // stage test
-          }
-        }
-
-        stage("px4_fmu-v5_stackcheck") {
-          stages {
-            stage("build px4_fmu-v5_stackcheck") {
-              agent {
-                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2022-08-12'
-                  args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
-                }
-              }
-              steps {
-                checkoutSCM()
-                sh 'make px4_fmu-v5_stackcheck'
-                sh 'make px4_fmu-v5_stackcheck bootloader_elf'
-                sh 'ccache -s'
-                stash includes: 'build/*/*.elf, platforms/nuttx/Debug/*, platforms/nuttx/NuttX/nuttx/tools/nuttx-gdbinit, Tools/HIL/*', name: 'px4_fmu-v5_stackcheck'
-              }
-              post {
-                always {
-                  sh 'make distclean; git clean -ff -x -d .'
-                }
-              }
-            } // stage build
-            stage("hardware") {
-              agent {
-                label 'px4_fmu-v5'
-              }
-              stages {
-                stage("flash") {
-                  steps {
-                    sh 'export'
-                    sh 'find /dev/serial'
-                    unstash 'px4_fmu-v5_stackcheck'
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 0" || true'
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set IMU_GYRO_RATEMAX 200" || true' // limit cpu usage
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_0_CONFIG 0" || true'       // limit cpu usage
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_1_CONFIG 0" || true'       // limit cpu usage
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param save" || true'
-                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v5_stackcheck/px4_fmu-v5_bootloader.elf'
-                    // flash board and watch bootup
-                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v5_stackcheck/px4_fmu-v5_stackcheck.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600'
-                    resetBoard()
-                  }
-                }
-                stage("tests") {
-                  steps {
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "top once"'
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "work_queue status"'
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb_tests"'
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb_tests latency_test" || true'
-
-                    // test dataman
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "tests dataman" --ignore-stdout-errors'
-                  }
-                }
-                stage("status") {
-                  steps {
-                    // configure
-                    resetParameters()
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 4001" || true'   // generic quadcopter
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set IMU_GYRO_RATEMAX 200" || true' // limit cpu usage
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_0_CONFIG 0" || true'       // limit cpu usage
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_1_CONFIG 0" || true'       // limit cpu usage
-                    sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply
-                    checkStatus()
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "px4io status" || true'
-                  }
-                }
-                stage("print topics") {
-                  steps {
-                    printTopics()
-                  }
-                }
-              }
-              post {
-                always {
-                  sh 'cat /tmp/pyserial_spy_file.txt || true'
-                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v5_stackcheck/px4_fmu-v5_stackcheck.elf || true'
-                }
-              }
-            } // stage test
-          }
-        }
-
-        stage("px4_fmu-v5_test") {
-          stages {
-            stage("build px4_fmu-v5_test") {
-              agent {
-                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2022-08-12'
-                  args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
-                }
-              }
-              steps {
-                checkoutSCM()
-                sh 'make px4_fmu-v5_test'
-                sh 'make px4_fmu-v5_test bootloader_elf'
-                sh 'ccache -s'
-                stash includes: 'build/*/*.elf, platforms/nuttx/Debug/*, platforms/nuttx/NuttX/nuttx/tools/nuttx-gdbinit, Tools/HIL/*', name: 'px4_fmu-v5_test'
-              }
-              post {
-                always {
-                  sh 'make distclean; git clean -ff -x -d .'
-                }
-              }
-            } // stage build
-            stage("hardware") {
-              agent {
-                label 'px4_fmu-v5'
-              }
-              stages {
-                stage("flash") {
-                  steps {
-                    sh 'export'
-                    sh 'find /dev/serial'
-                    unstash 'px4_fmu-v5_test'
-                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v5_test/px4_fmu-v5_bootloader.elf'
-                    // flash board and watch bootup
-                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v5_test/px4_fmu-v5_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600'
-                    resetBoard()
-                  }
-                }
-                stage("tests") {
-                  steps {
-                    runTests()
-                  }
-                }
-                stage("status") {
-                  steps {
-                    // configure
-                    resetParameters()
-                    sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_AUTOSTART" --value "4001"'    // generic quadcopter
-                    sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_BL_UPDATE" --value "1"'       // update bootloader
-                    sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply
-
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger on"' // run logger
-                    checkStatus()
-                    quickCalibrate()
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "px4io status" || true'
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger off"' // stop logger
-                  }
-                }
-                stage("print topics") {
-                  steps {
-                    printTopics()
-                  }
-                }
-              }
-              post {
-                always {
-                  sh 'cat /tmp/pyserial_spy_file.txt || true'
-                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v5_test/px4_fmu-v5_test.elf || true'
-                }
-              }
-            } // stage test
-          }
-        }
-
-        stage("nxp_fmuk66-v3_test") {
-          stages {
-            stage("build nxp_fmuk66-v3_test") {
-              agent {
-                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2022-08-12'
-                  args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
-                }
-              }
-              steps {
-                checkoutSCM()
-                sh 'make nxp_fmuk66-v3_test'
-                //sh 'make nxp_fmuk66-v3_test bootloader_elf'
-                sh 'ccache -s'
-                stash includes: 'build/*/*.elf, platforms/nuttx/Debug/*, platforms/nuttx/NuttX/nuttx/tools/nuttx-gdbinit, Tools/HIL/*', name: 'nxp_fmuk66-v3_test'
-              }
-              post {
-                always {
-                  sh 'make distclean; git clean -ff -x -d .'
-                }
-              }
-            } // stage build
-            stage("hardware") {
-              agent {
-                label 'nxp_fmuk66-v3'
-              }
-              stages {
-                stage("flash") {
-                  steps {
-                    sh 'export'
-                    sh 'find /dev/serial'
-                    unstash 'nxp_fmuk66-v3_test'
-                    //sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/nxp_fmuk66-v3_test/nxp_fmuk66-v3_bootloader.elf'
-                    // flash board and watch bootup
-                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/nxp_fmuk66-v3_test/nxp_fmuk66-v3_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600'
-                    resetBoard()
-                  }
-                }
-                stage("tests") {
-                  steps {
-                    runTests()
-                  }
-                }
-                stage("status") {
-                  steps {
-                    // configure
-                    resetParameters()
-                    sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "IMU_GYRO_RATEMAX" --value "400"'
-                    sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_AUTOSTART" --value "4001"'    // generic quadcopter
-                    sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_BL_UPDATE" --value "1"'       // update bootloader
-                    sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply
-
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger on"' // run logger
-                    checkStatus()
-                    quickCalibrate()
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger off"' // stop logger
-                  }
-                }
-                stage("print topics") {
-                  steps {
-                    printTopics()
-                  }
-                }
-              }
-              post {
-                always {
-                  sh 'cat /tmp/pyserial_spy_file.txt || true'
-                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/nxp_fmuk66-v3_test/nxp_fmuk66-v3_test.elf || true'
-                }
-              }
-            } // stage test
-          }
-        }
-
-      } // parallel
-    } // stage Hardware Test
-  } // stages
-  environment {
-    CCACHE_DIR = '/tmp/ccache'
-    CCACHE_NOHASHDIR = 1
-    CI = true
-  }
-  options {
-    buildDiscarder(logRotator(numToKeepStr: '30', artifactDaysToKeepStr: '60'))
-    timeout(time: 180, unit: 'MINUTES')
-    skipDefaultCheckout()
-  }
-}
-
-void checkoutSCM() {
-  retry(3) {
-    checkout scm
-    sh 'export'
-    sh 'make distclean; git clean -ff -x -d .'
-    sh 'git fetch --tags'
-    sh 'ccache -z'
-  }
-}
-
-void quickCalibrate() {
-
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CAL_*"' // parameters before
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sensors status"'
-
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "gyro_calibration status || true"'
-
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate accel quick; sleep 1"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CAL_ACC*"'
-
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate gyro; sleep 2"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CAL_GYRO*"'
-
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate level; sleep 2"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show SENS*"'
-
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate mag quick; sleep 1"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CAL_MAG*"'
-
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate baro; sleep 5"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CAL_BARO*"'
-
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CAL_*"' // parameters after
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sensors status"'
-}
-
-void checkStatus() {
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param save"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param status"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show SYS*"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "bsondump /fs/mtd_params"'
-
-  // status commands
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/fs/blocks"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/fs/mount"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/fs/usage"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/meminfo"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/uptime"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander check" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander status"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "dataman status"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "df -h"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "df"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ekf2 status"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "free"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "gps status"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener cpuload; top once; listener cpuload"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger status"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /bin"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /dev"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /etc"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /fs"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /fs/microsd"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /obj"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /proc"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /proc/fs"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mavlink status streams" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mavlink status" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mount"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mtd status"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "bsondump /fs/mtd_params"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param status"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "perf latency"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "perf"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ps"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "pwm_out status"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sensors status"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "top once"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uavcan status" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb status" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb top -1 -a" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ver all"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "work_queue status"'
-}
-
-void resetParameters() {
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param reset_all"'
-  sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "CBRK_BUZZER" --value "782097"'
-  sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SDLOG_DIRS_MAX" --value "1"'
-}
-
-void runTests() {
-
-  // test loading a range of airframes
-  sh './Tools/HIL/test_airframes.sh `find /dev/serial -name *usb-*` 2100 3000 4001 6001 8001'
-
-  resetParameters()
-
-  sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "IMU_GYRO_CAL_EN" --value "0" || true' // disable during testing
-  sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "IMU_GYRO_FFT_EN" --value "0" || true' // disable during testing
-  sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SENS_IMU_AUTOCAL" --value "0" || true' // disable during testing
-  sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SENS_MAG_AUTOCAL" --value "0" || true' // disable during testing
-
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param save"'
-  sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply
-
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "top once"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "work_queue status"'
-
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sensors stop"' // ignore irrelevant sensor timeouts during test
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ostest"'
-  sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot after ostest
-
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sensors stop"' // ignore irrelevant sensor timeouts during test
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander_tests" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "controllib_test"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "lightware_laser_test"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mavlink_tests" || true' // TODO
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb_tests"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb_tests latency_test" || true'
-
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "tests file" || true'
-
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "bsondump /fs/mtd_params"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mtd readtest"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "bsondump /fs/mtd_params"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mtd rwtest"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "bsondump /fs/mtd_params"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mtd erase"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "bsondump /fs/mtd_params" || true' // expected to fail after erase
-
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_bench"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_bench -v"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_bench -u -v"'
-
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_stress"'
-
-  // tests (stop modules first)
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander stop"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mavlink stop-all"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "navigator stop"'
-  sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-*`'
-
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sensors stop"' // ignore irrelevant sensor timeouts during microbenchmarks
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "microbench all"'
-
-  //sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "calib_udelay"'
-}
-
-void printTopics() {
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /obj"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb status"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb top -1 -a" || true'
-
-  // these are for casually inspecting the system, output failure doesn't matter
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener actuator_armed" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener actuator_controls_0" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener actuator_controls_1" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener actuator_controls_2" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener actuator_outputs" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener adc_report" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener airspeed_validated" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener airspeed_wind" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener battery_status" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener commander_state" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener cpuload" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener distance_sensor" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener ekf2_timestamps" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener esc_status" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_aid_src_fake_pos" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_aid_src_gnss_pos" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_aid_src_gnss_vel" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_attitude" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_baro_bias" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_event_flags" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_global_position" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_gps_status" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_innovation_test_ratios" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_innovation_variances" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_innovations" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_local_position" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_odometry" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_optical_flow_vel" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_selector_status" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_sensor_bias" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_states" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_status" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_status_flags" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_wind" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener event" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener heater_status" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener input_rc" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener led_control" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener log_message" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener logger_status" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener manual_control_setpoint" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener mavlink_log" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener mission" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener multirotor_motor_limits" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener optical_flow" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener parameter_update" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener position_controller_landing_status" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener position_setpoint_triplet" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener radio_status" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener rate_ctrl_status" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener safety" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_accel" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_accel_fifo" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_baro" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_combined" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gyro" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gyro_fft" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gyro_fifo" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_mag" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_preflight_mag" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_selection" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensors_status_imu" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener system_power" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener task_stack_info" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener telemetry_status" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener trajectory_setpoint" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener tune_control" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_acceleration" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_air_data" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_angular_velocity" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_attitude" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_attitude_setpoint" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_command" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_command_ack" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_control_mode" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_global_position" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_imu" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_imu_status" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_land_detected" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_local_position" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_local_position_setpoint" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_magnetometer" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_odometry" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_rates_setpoint" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_status" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener failsafe_flags" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vtol_vehicle_status" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener yaw_estimator_status" || true'
-}
-
-void resetBoard() {
-  resetParameters()
-
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 0" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set IMU_GYRO_RATEMAX 200" || true' // limit cpu usage
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_0_CONFIG 0" || true'       // limit cpu usage
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_1_CONFIG 0" || true'       // limit cpu usage
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SDLOG_MODE -1" || true'        // limit cpu usage
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "echo > /fs/microsd/.format" || true'
-  sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply
-}

.github/workflows/build_all_targets.yml

@@ -6,11 +6,6 @@
 name: Build all targets
 
 on:
-  workflow_dispatch:
-    inputs:
-      tag:
-        required: true
-        description: release version
   push:
     tags:
       - 'v*'
@@ -51,26 +46,15 @@ jobs:
       - id: set-timestamp
         run: echo "::set-output name=timestamp::$(date +"%Y%m%d%H%M%S")"
 
-      # This job is also triggered with versioned tags
-      #   Creating a and pushing a tag starting with "v" just as "v1.0.0"
-      #   will trigger this workflow and when all builds are done create a Github Release
-      #   then it will upload all binaries built as assets
-      # Additionally, we can also trigger this step manually
-      #   From the Github Actions tab for this repository:
-      #   https://github.com/PX4/PX4-Autopilot/actions/workflows/build_all_targets.yml
-      #   You can now click a "Run Workflow" button that will prompt you for a tag name
-      #   This tag name has to match an existing tag otherwise the new release will be detached
-      #   Note: Only developers with "write" permission to the repository can use this feature
-      - id: set-tag
-        if: startsWith(github.ref, 'refs/tags/v') || github.event_name == 'workflow_dispatch'
-        run: echo "::set-output name=tagname::${{ github.event_name == 'workflow_dispatch' && inputs.tag || github.ref_name }}"
-
       - id: set-branch
         run: echo "::set-output name=branchname::${GITHUB_HEAD_REF:-${GITHUB_REF#refs/heads/}}"
 
       - name: Debug Matrix Output
         if: runner.debug == '1'
-        run: echo "$(./Tools/ci/generate_board_targets_json.py --group --verbose)"
+        run: |
+          echo "${{ steps.set-timestamp.outputs.timestamp }}"
+          echo "${{ steps.set-branch.outputs.branchname }}"
+          echo "$(./Tools/ci/generate_board_targets_json.py --group --verbose)"
 
   setup:
     name: Build Group [${{ matrix.group }}]
@@ -159,20 +143,16 @@ jobs:
     # runs-on: ubuntu-latest
     runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}",spot=false]
     needs: [setup, group_targets]
-    if: startsWith(github.ref, 'refs/tags/v') || github.event_name == 'workflow_dispatch'
+    if: startsWith(github.ref, 'refs/tags/v')
     steps:
       - name: Download Artifacts
         uses: actions/download-artifact@v4
-
-      - name: Arrange Binaries
-        run: |
-          mkdir artifacts
-          cp **/**/*.px4 artifacts/
+        with:
+          path: artifacts/
+          merge-multiple: true
 
       - name: Upload Binaries to Release
         uses: softprops/action-gh-release@v2
         with:
-          name: ${{ needs.group_targets.outputs.tagname }}
-          tag_name: ${{ needs.group_targets.outputs.tagname }}
           draft: true
           files: artifacts/*.px4

.github/workflows/dev_container.yml

@@ -25,6 +25,18 @@ jobs:
           submodules: false
           fetch-depth: 0
 
+      - name: Set PX4 Tag
+        id: px4-tag
+        run: |
+          echo "tag=$(git describe --tags --match 'v[0-9]*')" >> $GITHUB_OUTPUT
+
+      - name: Login to Docker Hub
+        uses: docker/login-action@v3
+        if: github.event_name != 'pull_request'
+        with:
+          username: ${{ secrets.DOCKERHUB_USERNAME }}
+          password: ${{ secrets.DOCKERHUB_TOKEN }}
+
       - name: Login to GitHub Container Registry
         uses: docker/login-action@v3
         with:
@@ -38,6 +50,15 @@ jobs:
         with:
           images: |
             ghcr.io/PX4/px4-dev
+            ${{ (github.event_name != 'pull_request') && 'px4io/px4-dev' || '' }}
+          tags: |
+            type=semver,pattern={{version}}
+            type=semver,pattern={{major}}.{{minor}}
+            type=semver,pattern={{major}}
+            type=ref,event=branch,value=${{ steps.px4-tag.outputs.tag }},priority=700
+            type=ref,event=branch,suffix=-{{date 'YYYY-MM-DD'}},priority=600
+            type=ref,event=branch,suffix=,priority=500
+            type=ref,event=pr
 
       - name: Set up Docker Buildx
         uses: docker/setup-buildx-action@v3
@@ -81,6 +102,6 @@ jobs:
           platforms: |
             linux/amd64
           provenance: mode=max
-          push: ${{ github.event_name == 'push' }}
+          push: true
           cache-from: type=s3,blobs_prefix=cache/${{ github.repository }}/,manifests_prefix=cache/${{ github.repository }}/,region=${{ env.RUNS_ON_AWS_REGION }},bucket=${{ env.RUNS_ON_S3_BUCKET_CACHE }}
           cache-to: type=s3,blobs_prefix=cache/${{ github.repository }}/,manifests_prefix=cache/${{ github.repository }}/,region=${{ env.RUNS_ON_AWS_REGION }},bucket=${{ env.RUNS_ON_S3_BUCKET_CACHE }},mode=max

.github/workflows/flash_analysis.yml

@@ -10,8 +10,8 @@ on:
 
 jobs:
   analyze_flash:
-    name: FLASH usage analysis
-    runs-on: ubuntu-latest
+    name: Analyzing ${{ matrix.target }}
+    runs-on: [runs-on,runner=8cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",spot=false]
     container:
       image: px4io/px4-dev-nuttx-focal
     strategy:
@@ -29,7 +29,7 @@ jobs:
       - name: Git ownership workaround
         run: git config --system --add safe.directory '*'
 
-      - name: Build
+      - name: Build Target
         run: make ${{ matrix.target }}
 
       - name: Store the ELF with the change
@@ -73,36 +73,46 @@ jobs:
           echo "$EOF" >> $GITHUB_OUTPUT
 
   post_pr_comment:
-    name: Post PR comment
-    runs-on: ubuntu-latest
+    name: Publish Results
+    runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",spot=false]
     needs: [analyze_flash]
+    if: ${{ github.event.pull_request }}
     steps:
-      - name: If it's a PR add a comment with the bloaty output
-        if: ${{ github.event.pull_request }}
-        uses: actions/github-script@v6
+      - name: Find Comment
+        uses: peter-evans/find-comment@v3
+        id: fc
         with:
-          script: |
-            const comment = [
-               '## FLASH Analysis',
-               '<details>',
-               '<summary>px4_fmu-v5x</summary>',
-               '',
-               '```',
-               `${{ needs.analyze_flash.outputs.px4_fmu-v5x }}`,
-               '```',
-               '</details>',
-               '',
-               '<details>',
-               '<summary>px4_fmu-v6x</summary>',
-               '',
-               '```',
-               `${{ needs.analyze_flash.outputs.px4_fmu-v6x }}`,
-               '```',
-               '</details>'
-            ]
-            github.rest.issues.createComment({
-              issue_number: context.issue.number,
-              owner: context.repo.owner,
-              repo: context.repo.repo,
-              body: comment.join('\n')
-            })
+          issue-number: ${{ github.event.pull_request.number }}
+          comment-author: 'github-actions[bot]'
+          body-includes: FLASH Analysis
+
+      - name: Set Build Time
+        id: bt
+        run: |
+          echo "timestamp=$(date +'%Y-%m-%dT%H:%M:%S')" >> $GITHUB_OUTPUT
+
+      - name: Create or update comment
+        uses: peter-evans/create-or-update-comment@v4
+        with:
+          comment-id: ${{ steps.fc.outputs.comment-id }}
+          issue-number: ${{ github.event.pull_request.number }}
+          body: |
+            ## FLASH Analysis
+            <details>
+              <summary>px4_fmu-v5x</summary>
+
+              ```
+              ${{ needs.analyze_flash.outputs.px4_fmu-v5x }}
+              ```
+            </details>
+
+            <details>
+              <summary>px4_fmu-v6x</summary>
+
+              ```
+              ${{ needs.analyze_flash.outputs.px4_fmu-v6x }}
+              ```
+            </details>
+
+            **Updated: _${{ steps.bt.outputs.timestamp }}_**
+          edit-mode: replace

README.md

@@ -2,7 +2,7 @@
 
 [![Releases](https://img.shields.io/github/release/PX4/PX4-Autopilot.svg)](https://github.com/PX4/PX4-Autopilot/releases) [![DOI](https://zenodo.org/badge/22634/PX4/PX4-Autopilot.svg)](https://zenodo.org/badge/latestdoi/22634/PX4/PX4-Autopilot)
 
-[![Nuttx Targets](https://github.com/PX4/PX4-Autopilot/workflows/Nuttx%20Targets/badge.svg)](https://github.com/PX4/PX4-Autopilot/actions?query=workflow%3A%22Nuttx+Targets%22?branch=master) [![SITL Tests](https://github.com/PX4/PX4-Autopilot/workflows/SITL%20Tests/badge.svg?branch=master)](https://github.com/PX4/PX4-Autopilot/actions?query=workflow%3A%22SITL+Tests%22)
+[![Build Targets](https://github.com/PX4/PX4-Autopilot/actions/workflows/build_all_targets.yml/badge.svg?branch=main)](https://github.com/PX4/PX4-Autopilot/actions/workflows/build_all_targets.yml) [![SITL Tests](https://github.com/PX4/PX4-Autopilot/workflows/SITL%20Tests/badge.svg?branch=master)](https://github.com/PX4/PX4-Autopilot/actions?query=workflow%3A%22SITL+Tests%22)
 
 [![Discord Shield](https://discordapp.com/api/guilds/1022170275984457759/widget.png?style=shield)](https://discord.gg/dronecode)
 

ROMFS/px4fmu_common/init.d-posix/airframes/10040_sihsim_quadx

@@ -38,4 +38,6 @@ param set-default PWM_MAIN_FUNC2 102
 param set-default PWM_MAIN_FUNC3 103
 param set-default PWM_MAIN_FUNC4 104
 
+param set-default EKF2_GPS_DELAY 0
+
 param set SIH_VEHICLE_TYPE 0

ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane

@@ -45,7 +45,9 @@ param set-default CA_SV_CS1_TRQ_P 1
 param set-default CA_SV_CS1_TYPE 3
 param set-default CA_SV_CS2_TRQ_Y 1
 param set-default CA_SV_CS2_TYPE 4
-param set-default PWM_MAIN_FUNC3 201
-param set-default PWM_MAIN_FUNC4 202
-param set-default PWM_MAIN_FUNC5 203
-param set-default PWM_MAIN_FUNC6 101
+param set-default PWM_MAIN_FUNC1 201
+param set-default PWM_MAIN_FUNC2 202
+param set-default PWM_MAIN_FUNC3 203
+param set-default PWM_MAIN_FUNC4 101
+
+param set-default EKF2_GPS_DELAY 0

ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert

@@ -11,6 +11,7 @@
 PX4_SIMULATOR=${PX4_SIMULATOR:=sihsim}
 PX4_SIM_MODEL=${PX4_SIM_MODEL:=xvert}
 
+param set-default EKF2_GPS_DELAY 0
 param set-default EKF2_FUSE_BETA 0 # side slip fusion is currently not supported for tailsitters
 
 param set-default SENS_EN_GPSSIM 1

ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision

@@ -75,7 +75,6 @@ param set-default MPC_Z_VEL_P_ACC 5
 param set-default MPC_Z_VEL_I_ACC 3
 param set-default MPC_LAND_ALT1 3
 param set-default MPC_LAND_ALT2 1
-param set-default MPC_POS_MODE 3
 param set-default CP_GO_NO_DATA 1
 
 # Navigator Parameters

ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover

@@ -26,7 +26,6 @@ param set-default RD_MAX_THR_SPD 2.15
 param set-default RD_SPEED_P 0.1
 param set-default RD_SPEED_I 0.01
 param set-default RD_MAX_YAW_RATE 180
-param set-default RD_MISS_SPD_DEF 2
 param set-default RD_TRANS_DRV_TRN 0.349066
 param set-default RD_TRANS_TRN_DRV 0.174533
 param set-default RD_MAX_YAW_ACCEL 1000

ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower

@@ -29,7 +29,6 @@ param set-default RD_MAX_SPEED 8
 param set-default RD_YAW_P 5
 param set-default RD_YAW_I 0.1
 param set-default RD_MAX_YAW_RATE 30
-param set-default RD_MISS_SPD_DEF 8
 param set-default RD_TRANS_DRV_TRN 0.349066
 param set-default RD_TRANS_TRN_DRV 0.174533
 

ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum

@@ -13,18 +13,18 @@ param set-default SIM_GZ_EN 1 # Gazebo bridge
 
 # Rover parameters
 param set-default RM_WHEEL_TRACK 0.3
-param set-default RM_MAN_YAW_SCALE 0.1
-param set-default RM_YAW_RATE_I 0
-param set-default RM_YAW_RATE_P 0.01
+param set-default RM_YAW_RATE_I 0.1
+param set-default RM_YAW_RATE_P 0.1
 param set-default RM_MAX_ACCEL 3
+param set-default RM_MAX_DECEL 5
 param set-default RM_MAX_JERK 5
-param set-default RM_MAX_SPEED 4
-param set-default RM_MAX_THR_SPD 7
-param set-default RM_MAX_THR_YAW_R 7.5
+param set-default RM_MAX_SPEED 2
+param set-default RM_MAX_THR_SPD 2.2
+param set-default RM_MAX_THR_YAW_R 1.2
 param set-default RM_YAW_P 5
 param set-default RM_YAW_I 0.1
-param set-default RM_MAX_YAW_RATE 180
-param set-default RM_MISS_SPD_DEF 3
+param set-default RM_MAX_YAW_RATE 120
+param set-default RM_MAX_YAW_ACCEL 240
 param set-default RM_MISS_VEL_GAIN 1
 param set-default RM_SPEED_I 0.01
 param set-default RM_SPEED_P 0.1
@@ -42,23 +42,23 @@ param set-default SENS_EN_ARSPDSIM 0
 
 # Actuator mapping
 param set-default SIM_GZ_WH_FUNC1 102 # right wheel front
-param set-default SIM_GZ_WH_MIN1 0
-param set-default SIM_GZ_WH_MAX1 200
+param set-default SIM_GZ_WH_MIN1 70
+param set-default SIM_GZ_WH_MAX1 130
 param set-default SIM_GZ_WH_DIS1 100
 
 param set-default SIM_GZ_WH_FUNC2 101 # left wheel front
-param set-default SIM_GZ_WH_MIN2 0
-param set-default SIM_GZ_WH_MAX2 200
+param set-default SIM_GZ_WH_MIN2 70
+param set-default SIM_GZ_WH_MAX2 130
 param set-default SIM_GZ_WH_DIS2 100
 
 param set-default SIM_GZ_WH_FUNC3 104 # right wheel back
-param set-default SIM_GZ_WH_MIN3 0
-param set-default SIM_GZ_WH_MAX3 200
+param set-default SIM_GZ_WH_MIN3 70
+param set-default SIM_GZ_WH_MAX3 130
 param set-default SIM_GZ_WH_DIS3 100
 
 param set-default SIM_GZ_WH_FUNC4 103 # left wheel back
-param set-default SIM_GZ_WH_MIN4 0
-param set-default SIM_GZ_WH_MAX4 200
+param set-default SIM_GZ_WH_MIN4 70
+param set-default SIM_GZ_WH_MAX4 130
 param set-default SIM_GZ_WH_DIS4 100
 
 param set-default SIM_GZ_WH_REV 10

ROMFS/px4fmu_common/init.d-posix/airframes/4018_gz_quadtailsitter

@@ -0,0 +1,97 @@
+#!/bin/sh
+#
+# @name Quadrotor + Tailsitter
+#
+# @type VTOL Quad Tailsitter
+#
+
+. ${R}etc/init.d/rc.vtol_defaults
+
+PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
+PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=quadtailsitter}
+
+param set-default SIM_GZ_EN 1 # Gazebo bridge
+
+param set-default SENS_EN_GPSSIM 1
+param set-default SENS_EN_BAROSIM 0
+param set-default SENS_EN_MAGSIM 1
+param set-default SENS_EN_ARSPDSIM 0
+
+
+param set-default MAV_TYPE 20
+
+param set-default CA_AIRFRAME 4
+
+param set-default CA_ROTOR_COUNT 4
+param set-default CA_ROTOR0_PX 0.15
+param set-default CA_ROTOR0_PY 0.23
+param set-default CA_ROTOR0_KM 0.05
+param set-default CA_ROTOR1_PX -0.15
+param set-default CA_ROTOR1_PY -0.23
+param set-default CA_ROTOR1_KM 0.05
+param set-default CA_ROTOR2_PX 0.15
+param set-default CA_ROTOR2_PY -0.23
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -0.15
+param set-default CA_ROTOR3_PY 0.23
+param set-default CA_ROTOR3_KM -0.05
+
+param set-default CA_SV_CS_COUNT 0
+
+param set-default SIM_GZ_EC_FUNC1 101
+param set-default SIM_GZ_EC_FUNC2 102
+param set-default SIM_GZ_EC_FUNC3 103
+param set-default SIM_GZ_EC_FUNC4 104
+
+param set-default SIM_GZ_EC_MIN1 10
+param set-default SIM_GZ_EC_MIN2 10
+param set-default SIM_GZ_EC_MIN3 10
+param set-default SIM_GZ_EC_MIN4 10
+
+param set-default SIM_GZ_EC_MAX1 1500
+param set-default SIM_GZ_EC_MAX2 1500
+param set-default SIM_GZ_EC_MAX3 1500
+param set-default SIM_GZ_EC_MAX4 1500
+
+param set-default FD_FAIL_R 70
+
+param set-default FW_P_TC 0.6
+
+param set-default FW_PR_I 0.3
+param set-default FW_PR_P 0.5
+param set-default FW_PSP_OFF 2
+param set-default FW_RR_FF 0.1
+param set-default FW_RR_I 0.1
+param set-default FW_RR_P 0.2
+param set-default FW_YR_FF 0 # make yaw rate controller very weak, only keep default P
+param set-default FW_YR_I 0
+param set-default FW_THR_TRIM 0.35
+param set-default FW_THR_MAX 0.8
+param set-default FW_THR_MIN 0.05
+param set-default FW_T_CLMB_MAX 6
+param set-default FW_T_HRATE_FF 0.5
+param set-default FW_T_SINK_MAX 3
+param set-default FW_T_SINK_MIN 1.6
+param set-default FW_AIRSPD_STALL 10
+param set-default FW_AIRSPD_MIN 14
+param set-default FW_AIRSPD_TRIM 18
+param set-default FW_AIRSPD_MAX 22
+
+param set-default MC_AIRMODE 2
+param set-default MAN_ARM_GESTURE 0 # required for yaw airmode
+param set-default MC_ROLL_P 3
+param set-default MC_PITCH_P 3
+param set-default MC_ROLLRATE_P 0.3
+param set-default MC_PITCHRATE_P 0.3
+
+param set-default VT_ARSP_TRANS 15
+param set-default VT_B_TRANS_DUR 5
+param set-default VT_FW_DIFTHR_EN 7
+param set-default VT_FW_DIFTHR_S_Y 1
+param set-default VT_F_TRANS_DUR 1.5
+param set-default VT_TYPE 0
+
+param set-default WV_EN 0
+
+param set-default EKF2_FUSE_BETA 0

ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt

@@ -89,6 +89,7 @@ px4_add_romfs_files(
 	4015_gz_r1_rover_mecanum
 	4016_gz_x500_lidar_down
 	4017_gz_x500_lidar_front
+	4018_gz_quadtailsitter
 
 	6011_gazebo-classic_typhoon_h480
 	6011_gazebo-classic_typhoon_h480.post

ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil

@@ -18,22 +18,19 @@ param set UAVCAN_ENABLE 0
 param set-default CA_AIRFRAME 1
 param set-default CA_ROTOR_COUNT 1
 param set-default CA_ROTOR0_PX 0.3
-param set-default CA_SV_CS_COUNT 4
-param set-default CA_SV_CS0_TRQ_R 0.5
-param set-default CA_SV_CS0_TYPE 2
+param set-default CA_SV_CS_COUNT 3
+param set-default CA_SV_CS0_TRQ_R -0.5
+param set-default CA_SV_CS0_TYPE 1
 param set-default CA_SV_CS1_TRQ_P 1
 param set-default CA_SV_CS1_TYPE 3
 param set-default CA_SV_CS2_TRQ_Y 1
 param set-default CA_SV_CS2_TYPE 4
-param set-default CA_SV_CS3_TYPE 10
 
-param set-default HIL_ACT_REV 2
-param set-default HIL_ACT_FUNC1 201
-param set-default HIL_ACT_FUNC2 202
-param set-default HIL_ACT_FUNC3 203
-param set-default HIL_ACT_FUNC4 101
-param set-default HIL_ACT_FUNC5 204
-param set-default HIL_ACT_FUNC6 400
+param set HIL_ACT_REV 1
+param set HIL_ACT_FUNC1 201
+param set HIL_ACT_FUNC2 202
+param set HIL_ACT_FUNC3 203
+param set HIL_ACT_FUNC4 101
 
 # set SYS_HITL to 2 to start the SIH and avoid sensors startup
 param set-default SYS_HITL 2

ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision

@@ -67,7 +67,6 @@ param set-default MPC_Z_VEL_P_ACC 5
 param set-default MPC_Z_VEL_I_ACC 3
 param set-default MPC_LAND_ALT1 3
 param set-default MPC_LAND_ALT2 1
-param set-default MPC_POS_MODE 3
 param set-default CP_GO_NO_DATA 1
 
 # Navigator Parameters

ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5

@@ -68,7 +68,6 @@ param set-default MPC_Z_VEL_P_ACC 5
 param set-default MPC_Z_VEL_I_ACC 3
 param set-default MPC_LAND_ALT1 3
 param set-default MPC_LAND_ALT2 1
-param set-default MPC_POS_MODE 3
 param set-default CP_GO_NO_DATA 1
 
 # Navigator Parameters

ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover

@@ -31,7 +31,6 @@ param set-default RD_MAX_THR_SPD 1.9
 param set-default RD_MAX_THR_YAW_R 0.7
 param set-default RD_MAX_YAW_ACCEL 600
 param set-default RD_MAX_YAW_RATE 250
-param set-default RD_MISS_SPD_DEF 1.5
 param set-default RD_SPEED_P 0.1
 param set-default RD_SPEED_I 0.01
 param set-default RD_TRANS_DRV_TRN 0.785398

ROMFS/px4fmu_common/init.d/rc.fw_defaults

@@ -13,8 +13,6 @@ param set-default MAV_TYPE 1
 #
 # Default parameters for fixed wing UAVs.
 #
-param set-default COM_POS_FS_DELAY 5
-
 # there is a 2.5 factor applied on the _FS thresholds if for invalidation
 param set-default COM_POS_FS_EPH 50
 param set-default COM_VEL_FS_EVH 3

ROMFS/px4fmu_common/init.d/rc.sensors

@@ -211,6 +211,13 @@ then
 	spl06 -X -a 0x77 start
 fi
 
+# SPA06 sensor external I2C
+if param compare -s SENS_EN_SPA06 1
+then
+	spa06 -X start
+	spa06 -X -a 0x77 start
+fi
+
 # PCF8583 counter (RPM sensor)
 if param compare -s SENS_EN_PCF8583 1
 then

Tools/astyle/check_code_style_all.sh

@@ -43,7 +43,7 @@ fi
 
 # install git pre-commit hook
 HOOK_FILE="$DIR/../../.git/hooks/pre-commit"
-if [ ! -f $HOOK_FILE ] && [ "$CI" != "true" ]; then
+if [ ! -f $HOOK_FILE ] && [ "$CI" != "true" ] && [ $- == *i* ]; then
 	echo ""
 	echo -e "\033[31mNinja tip: add a git pre-commit hook to automatically check code style\033[0m"
 	echo -e "Would you like to install one now? (\033[94mcp ./Tools/astyle/pre-commit .git/hooks/pre-commit\033[0m): [y/\033[1mN\033[0m]"

Tools/px4airframes/markdownout.py

@@ -137,7 +137,7 @@ div.frame_variant td, div.frame_variant th {
                             #print(output_name,value, attribstrs[0].strip(),attribstrs[1].strip())
                     outputs += '</ul>'
                     if has_outputs:
-                        outputs_entry = '<p><b>Specific Outputs:</b>' + outputs + '</p>'
+                        outputs_entry = '<br><b>Specific Outputs:</b>' + outputs
                     else:
                         outputs_entry = ''
 

Tools/setup/ubuntu.sh

@@ -64,6 +64,7 @@ sudo apt-get update -y --quiet
 sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
 	astyle \
 	build-essential \
+	ccache \
 	cmake \
 	cppcheck \
 	file \

boards/emlid/navio2/arm64.px4board

@@ -0,0 +1 @@
+CONFIG_BOARD_TOOLCHAIN="aarch64-linux-gnu"

boards/holybro/kakuteh7/init/rc.board_sensors

@@ -11,4 +11,7 @@ then
 	icm42688p -R 6 -s start
 fi
 
-bmp280 -X start
+if ! bmp280 -X start
+then
+	spa06 -X start
+fi

boards/holybro/kakuteh7mini/init/rc.board_sensors

@@ -12,4 +12,7 @@ then
   fi
 fi
 
-bmp280 -X start
+if ! bmp280 -X start
+then
+	spa06 -X start
+fi

boards/holybro/kakuteh7v2/init/rc.board_sensors

@@ -9,4 +9,7 @@ then
 	icm42688p -R 0 -s start
 fi
 
-bmp280 -X start
+if ! bmp280 -X start
+then
+	spa06 -X start
+fi

boards/micoair/h743-aio/default.px4board

@@ -20,10 +20,10 @@ CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_COMMON_RC=y
 CONFIG_DRIVERS_RC_INPUT=y
 CONFIG_DRIVERS_TAP_ESC=y
+CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
-CONFIG_COMMON_TELEMETRY=y
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_COMMANDER=y

boards/mro/x21-777/default.px4board

@@ -67,7 +67,7 @@ CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
-CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
+CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=n
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y

boards/px4/fmu-v6u/default.px4board

@@ -68,7 +68,7 @@ CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
-CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
+CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=n
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UXRCE_DDS_CLIENT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y

msg/EstimatorAidSource2d.msg

@@ -7,7 +7,7 @@ uint32 device_id
 
 uint64 time_last_fuse
 
-float32[2] observation
+float64[2] observation
 float32[2] observation_variance
 
 float32[2] innovation

msg/RoverMecanumSetpoint.msg

@@ -5,7 +5,7 @@ float32 forward_speed_setpoint_normalized # [-1, 1] Desired normalized forward s
 float32 lateral_speed_setpoint            # [m/s] Desired lateral speed
 float32 lateral_speed_setpoint_normalized # [-1, 1] Desired normalized lateral speed
 float32 yaw_rate_setpoint                 # [rad/s] Desired yaw rate
-float32 yaw_rate_setpoint_normalized      # [-1, 1] Desired normalized yaw rate
+float32 speed_diff_setpoint_normalized    # [-1, 1] Normalized speed difference between the left and right wheels
 float32 yaw_setpoint 	                  # [rad] Desired yaw (heading)
 
 # TOPICS rover_mecanum_setpoint

msg/RoverMecanumStatus.msg

@@ -1,13 +1,17 @@
 uint64 timestamp # time since system start (microseconds)
 
-float32 measured_forward_speed        # [m/s] Measured speed in body x direction. Positiv: forwards, Negativ: backwards
-float32 measured_lateral_speed        # [m/s] Measured speed in body y direction. Positiv: right, Negativ: left
-float32 adjusted_yaw_rate_setpoint    # [rad/s] Yaw rate setpoint output of the closed loop yaw controller
-float32 measured_yaw_rate  	      # [rad/s] Measured yaw rate
-float32 measured_yaw        	      # [rad] Measured yaw
-float32 pid_yaw_rate_integral  	      # Integral of the PID for the closed loop yaw rate controller
-float32 pid_yaw_integral       	      # Integral of the PID for the closed loop yaw controller
-float32 pid_forward_throttle_integral # Integral of the PID for the closed loop forward speed controller
-float32 pid_lateral_throttle_integral # Integral of the PID for the closed loop lateral speed controller
+float32 measured_forward_speed          # [m/s] Measured speed in body x direction. Positiv: forwards, Negativ: backwards
+float32 adjusted_forward_speed_setpoint # [m/s] Speed setpoint after applying slew rate
+float32 measured_lateral_speed          # [m/s] Measured speed in body y direction. Positiv: right, Negativ: left
+float32 adjusted_lateral_speed_setpoint # [m/s] Speed setpoint after applying slew rate
+float32 measured_yaw_rate  	        # [rad/s] Measured yaw rate
+float32 clyaw_yaw_rate_setpoint         # [rad/s] Yaw rate setpoint output by the closed loop yaw controller
+float32 adjusted_yaw_rate_setpoint      # [rad/s] Yaw rate setpoint from the closed loop yaw controller
+float32 measured_yaw        	        # [rad] Measured yaw
+float32 adjusted_yaw_setpoint           # [rad] Yaw setpoint after applying slew rate
+float32 pid_yaw_rate_integral  	        # Integral of the PID for the closed loop yaw rate controller
+float32 pid_yaw_integral       	        # Integral of the PID for the closed loop yaw controller
+float32 pid_forward_throttle_integral   # Integral of the PID for the closed loop forward speed controller
+float32 pid_lateral_throttle_integral   # Integral of the PID for the closed loop lateral speed controller
 
 # TOPICS rover_mecanum_status

platforms/common/spi.cpp

@@ -87,7 +87,12 @@ const px4_spi_bus_t *px4_spi_buses{nullptr};
 
 int px4_find_spi_bus(uint32_t devid)
 {
+// px4_spi_buses is only NULL on certain targets depending on defines
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Waddress"
+
 	for (int i = 0; ((px4_spi_bus_t *) px4_spi_buses) != nullptr && i < SPI_BUS_MAX_BUS_ITEMS; ++i) {
+#pragma GCC diagnostic pop
 		const px4_spi_bus_t &bus_data = px4_spi_buses[i];
 
 		if (bus_data.bus == -1) {

src/drivers/barometer/Kconfig

@@ -11,6 +11,7 @@ menu "barometer"
         select DRIVERS_BAROMETER_MS5611
         select DRIVERS_BAROMETER_MAIERTEK_MPC2520
         select DRIVERS_BAROMETER_GOERTEK_SPL06
+        select DRIVERS_BAROMETER_GOERTEK_SPA06
         select DRIVERS_BAROMETER_INVENSENSE_ICP101XX
         select DRIVERS_BAROMETER_INVENSENSE_ICP201XX
         ---help---

src/drivers/barometer/goertek/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2022-2024 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
@@ -32,3 +32,4 @@
 ############################################################################
 
 add_subdirectory(spl06)
+add_subdirectory(spa06)

src/drivers/barometer/goertek/spa06/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2024 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
@@ -31,9 +31,15 @@
 #
 ############################################################################
 
-px4_add_library(FlightTaskManualPositionSmoothVel
-	FlightTaskManualPositionSmoothVel.cpp
-)
-
-target_link_libraries(FlightTaskManualPositionSmoothVel PUBLIC FlightTaskManualPosition FlightTaskUtility)
-target_include_directories(FlightTaskManualPositionSmoothVel PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+px4_add_module(
+	MODULE drivers__barometer__spa06
+	MAIN spa06
+	SRCS
+		SPA06.cpp
+		SPA06.hpp
+		SPA06_I2C.cpp
+		SPA06_SPI.cpp
+		spa06_main.cpp
+	DEPENDS
+		px4_work_queue
+	)

src/drivers/barometer/goertek/spa06/Kconfig

@@ -0,0 +1,5 @@
+menuconfig DRIVERS_BAROMETER_GOERTEK_SPA06
+	bool "spa06"
+	default n
+	---help---
+		Enable support for spa06

src/drivers/barometer/goertek/spa06/SPA06.cpp

@@ -0,0 +1,259 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 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.
+ *
+ ****************************************************************************/
+
+#include "SPA06.hpp"
+
+SPA06::SPA06(const I2CSPIDriverConfig &config, spa06::ISPA06 *interface) :
+	I2CSPIDriver(config),
+	_interface(interface),
+	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": sample")),
+	_measure_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": measure")),
+	_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms errors"))
+{
+}
+
+SPA06::~SPA06()
+{
+	// free perf counters
+	perf_free(_sample_perf);
+	perf_free(_measure_perf);
+	perf_free(_comms_errors);
+
+	delete _interface;
+}
+
+/*
+float
+SPA06::scale_factor(int oversampling_rate)
+{
+	float k;
+
+	switch (oversampling_rate) {
+	case 1:
+		k = 524288.0f;
+		break;
+
+	case 2:
+		k = 1572864.0f;
+		break;
+
+	case 4:
+		k = 3670016.0f;
+		break;
+
+	case 8:
+		k = 7864320.0f;
+		break;
+
+	case 16:
+		k = 253952.0f;
+		break;
+
+	case 32:
+		k = 516096.0f;
+		break;
+
+	case 64:
+		k = 1040384.0f;
+		break;
+
+	case 128:
+		k = 2088960.0f;
+		break;
+
+	default:
+		k = 0;
+		break;
+	}
+
+	return k;
+}
+*/
+
+int
+SPA06::calibrate()
+{
+	uint8_t buf[21];
+
+	_interface->read(SPA06_ADDR_CAL, buf, sizeof(buf));
+
+	_cal.c0 = (uint16_t)(buf[0]) << 4 | (uint16_t)(buf[1]) >> 4;
+	// If value is negative, we need to fill the missing bits.
+	_cal.c0 = (_cal.c0 & 1 << 11) ? (0xf000 | _cal.c0) : _cal.c0;
+
+	_cal.c1 = (uint16_t)(buf[1] & 0x0F) << 8 | buf[2];
+	_cal.c1 = (_cal.c1 & 1 << 11) ? (0xf000 | _cal.c1) : _cal.c1;
+
+	_cal.c00 = (uint32_t)(buf[3]) << 12 | (uint32_t)(buf[4]) << 4 | (buf[5]) >> 4;
+	_cal.c00 = (_cal.c00 & 1 << 19) ? (0xfff00000 | _cal.c00) : _cal.c00;
+
+	_cal.c10 = (uint32_t)(buf[5] & 0x0F) << 16 | (uint32_t)(buf[6]) << 8 | buf[7];
+	_cal.c10 = (_cal.c10 & 1 << 19) ? (0xfff00000 | _cal.c10) : _cal.c10;
+
+	_cal.c01 = (uint16_t)(buf[8]) << 8 | buf[9];
+
+	_cal.c11 = (uint16_t)(buf[10]) << 8 | buf[11];
+
+	_cal.c20 = (uint16_t)(buf[12]) << 8 | buf[13];
+
+	_cal.c21 = (uint16_t)(buf[14]) << 8 | buf[15];
+
+	_cal.c30 = (uint16_t)(buf[16]) << 8 | buf[17];
+
+	_cal.c31 = (uint16_t)(buf[18]) << 4 | (uint16_t)(buf[19] & 0xF0) >> 4;
+	_cal.c31 = (_cal.c31 & 1 << 11) ? (0xf000 | _cal.c31) : _cal.c31;
+
+	_cal.c40 = (uint16_t)(buf[19] & 0x0F) << 8 | buf[20];
+	_cal.c40 = (_cal.c40 & 1 << 11) ? (0xf000 | _cal.c40) : _cal.c40;
+
+	PX4_DEBUG("c0:%d\nc1:%d\nc00:%ld\nc10:%ld\nc01:%d\nc11:%d\nc20:%d\nc21:%d\nc30:%d\nc31:%d\nc40:%d\n",
+		  _cal.c0, _cal.c1,
+		  _cal.c00, _cal.c10,
+		  _cal.c01, _cal.c11, _cal.c20, _cal.c21, _cal.c30, _cal.c31, _cal.c40);
+
+	return OK;
+}
+int
+SPA06::init()
+{
+	int8_t tries = 5;
+	// reset sensor
+	_interface->set_reg(SPA06_VALUE_RESET, SPA06_ADDR_RESET);
+	usleep(10000);
+
+	// check id
+	if (_interface->get_reg(SPA06_ADDR_ID) != SPA06_VALUE_ID) {
+		PX4_DEBUG("id of your baro is not: 0x%02x", SPA06_VALUE_ID);
+		return -EIO;
+	}
+
+	while (tries--) {
+		uint8_t meas_cfg = _interface->get_reg(SPA06_ADDR_MEAS_CFG);
+
+		if (meas_cfg & (1 << 7) && meas_cfg & (1 << 6)) {
+			break;
+		}
+
+		usleep(10000);
+	}
+
+	if (tries < 0) {
+		PX4_DEBUG("spa06 sensor or coef not ready");
+		return -EIO;
+	}
+
+	// get calibration and pre process them
+	calibrate();
+
+	// set config, recommended settings
+	_interface->set_reg(_curr_prs_cfg, SPA06_ADDR_PRS_CFG);
+	kp = 253952.0f; // refer to scale_factor()
+	_interface->set_reg(_curr_tmp_cfg, SPA06_ADDR_TMP_CFG);
+	kt = 524288.0f;
+
+	// Enable FIFO
+	_interface->set_reg(1 << 2, SPA06_ADDR_CFG_REG);
+	// Continuous pressure and temperature mesasurement.
+	_interface->set_reg(7, SPA06_ADDR_MEAS_CFG);
+
+	Start();
+
+	return OK;
+}
+
+void
+SPA06::Start()
+{
+	// schedule a cycle to start things
+	ScheduleNow();
+}
+
+void
+SPA06::RunImpl()
+{
+	collect();
+
+	ScheduleDelayed(_measure_interval);
+}
+int
+SPA06::collect()
+{
+	perf_begin(_sample_perf);
+
+	// this should be fairly close to the end of the conversion, so the best approximation of the time
+	const hrt_abstime timestamp_sample = hrt_absolute_time();
+
+	if (_interface->read(SPA06_ADDR_DATA, (uint8_t *)&_data, sizeof(_data)) != OK) {
+		perf_count(_comms_errors);
+		perf_cancel(_sample_perf);
+		return -EIO;
+	}
+
+	int32_t temp_raw = (uint32_t)_data.t_msb << 16 | (uint32_t)_data.t_lsb << 8 | (uint32_t)_data.t_xlsb;
+	temp_raw = (temp_raw & 1 << 23) ? (0xff000000 | temp_raw) : temp_raw;
+
+	int32_t press_raw = (uint32_t)_data.p_msb << 16 | (uint32_t) _data.p_lsb << 8 | (uint32_t) _data.p_xlsb;
+	press_raw = (press_raw & 1 << 23) ? (0xff000000 | press_raw) : press_raw;
+
+	// calculate
+	float ftsc = (float)temp_raw / kt;
+	float fpsc = (float)press_raw / kp;
+	float qua2 = (float)_cal.c10 + fpsc * ((float)_cal.c20 + fpsc * ((float)_cal.c30) + fpsc * (float)_cal.c40);
+	float qua3 = ftsc * fpsc * ((float)_cal.c11 + fpsc * ((float)_cal.c21) + fpsc * (float)_cal.c31);
+
+	float fp = (float)_cal.c00 + fpsc * qua2 + ftsc * (float)_cal.c01 + qua3;
+	float temperature = (float)_cal.c0 * 0.5f + (float)_cal.c1 * ftsc;
+
+
+	sensor_baro_s sensor_baro{};
+	sensor_baro.timestamp_sample = timestamp_sample;
+	sensor_baro.device_id = _interface->get_device_id();
+	sensor_baro.pressure = fp;
+	sensor_baro.temperature = temperature;
+	sensor_baro.error_count = perf_event_count(_comms_errors);
+	sensor_baro.timestamp = hrt_absolute_time();
+	_sensor_baro_pub.publish(sensor_baro);
+
+	perf_end(_sample_perf);
+
+	return OK;
+}
+
+void
+SPA06::print_status()
+{
+	I2CSPIDriverBase::print_status();
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_measure_perf);
+	perf_print_counter(_comms_errors);
+}

src/drivers/barometer/goertek/spa06/SPA06.hpp

@@ -0,0 +1,117 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "spa06.h"
+
+#include <drivers/drv_hrt.h>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/i2c_spi_buses.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <lib/perf/perf_counter.h>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/sensor_baro.h>
+
+class SPA06 : public I2CSPIDriver<SPA06>
+{
+public:
+	SPA06(const I2CSPIDriverConfig &config, spa06::ISPA06 *interface);
+	virtual ~SPA06();
+
+	static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance);
+	static void print_usage();
+
+	int			init();
+	void			print_status();
+
+	void			RunImpl();
+private:
+	void			Start();
+	// float scale_factor(int oversampling_rate);
+
+	int			collect(); //get results and publish
+	int			calibrate();
+
+	uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
+
+	spa06::ISPA06		*_interface;
+	spa06::data_s		_data;
+	spa06::calibration_s	_cal{};
+
+	// set config, recommended settings
+	//
+	// oversampling rate  : single | 2       | 4       | 8       | 16     | 32     | 64      | 128
+	// scale factor(KP/KT): 524288 | 1572864 | 3670016 | 7864320 | 253952 | 516096 | 1040384 | 2088960
+
+	// configuration of pressure measurement rate (PM_RATE) and resolution (PM_PRC)
+	//
+	// PM_RATE[7:4]    : 0 | 1 | 2 | 3 | 4  | 5  | 6  | 7   | 8     | 9    | 10   | 11   | 12 | 13 | 14  | 15
+	// measurement rate: 1 | 2 | 4 | 8 | 16 | 32 | 64 | 128 | 25/16 | 25/8 | 25/4 | 25/2 | 25 | 50 | 100 | 200
+	// note: applicable for measurements in background mode only
+	//
+	// PM_PRC[3:0]         : 0      | 1   | 2   | 3    | 4    | 5    | 6     | 7
+	// oversampling (times): single | 2   | 4   | 8    | 16   | 32   | 64    | 128
+	// measurement time(ms): 3.6    | 5.2 | 8.4 | 14.8 | 27.6 | 53.2 | 104.4 | 206.8
+	// precision(PaRMS)    : 5.0    |     | 2.5 |      | 1.2  | 0.9  | 0.5   |
+	// note: use in combination with a bit shift when the oversampling rate is > 8 times. see CFG_REG(0x19) register
+	//
+	// -> 32 measurements per second, 16 oversampling
+	static constexpr uint8_t	_curr_prs_cfg{5 << 4 | 4};
+
+	// configuration of temperature measurment rate (TMP_RATE) and resolution (TMP_PRC)
+	//
+	// temperature measurement: internal sensor (in ASIC) | external sensor (in pressure sensor MEMS element)
+	// PM_RATE[7:4]    : 0 | 1 | 2 | 3 | 4  | 5  | 6  | 7   | 8     | 9    | 10   | 11   | 12 | 13 | 14  | 15
+	// measurement rate: 1 | 2 | 4 | 8 | 16 | 32 | 64 | 128 | 25/16 | 25/8 | 25/4 | 25/2 | 25 | 50 | 100 | 200
+	// note: applicable for measurements in background mode only
+	//
+	// TMP_PRC[2:0]        : 0      | 1 | 2 | 3 | 4  | 5  | 6  | 7
+	// oversampling (times): single | 2 | 4 | 8 | 16 | 32 | 64 | 128
+	// note: single(default) measurement time 3.6ms, other settings are optional, and may not be relevant
+	// note: use in combination with a bit shift when the oversampling rate is > 8 times. see CFG_REG(0x19) register
+
+	// -> 32 measurements per second, single oversampling
+	static constexpr uint8_t	_curr_tmp_cfg{5 << 4 | 0};
+
+	bool			_collect_phase{false};
+	float kp;
+	float kt;
+
+	perf_counter_t		_sample_perf;
+	perf_counter_t		_measure_perf;
+	perf_counter_t		_comms_errors;
+
+	static constexpr uint32_t _sample_rate{32};
+	static constexpr uint32_t _measure_interval{1000000 / _sample_rate};
+};

src/drivers/barometer/goertek/spa06/SPA06_I2C.cpp

@@ -0,0 +1,102 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 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 SPA06_I2C.cpp
+ *
+ * SPI interface for Goertek SPA06
+ */
+
+#include "spa06.h"
+
+#include <px4_platform_common/px4_config.h>
+#include <drivers/device/i2c.h>
+
+#if defined(CONFIG_I2C)
+
+class SPA06_I2C: public device::I2C, public spa06::ISPA06
+{
+public:
+	SPA06_I2C(uint8_t bus, uint32_t device, int bus_frequency);
+	virtual ~SPA06_I2C() override = default;
+
+	int init() override { return I2C::init(); }
+
+	uint8_t	get_reg(uint8_t addr) override;
+	int	set_reg(uint8_t value, uint8_t addr) override;
+
+	int read(uint8_t addr, uint8_t *buf, uint8_t len) override;
+	//spa06::data_s		*get_data(uint8_t addr) override;
+	//spa06::calibration_s	*get_calibration(uint8_t addr) override;
+
+	uint32_t get_device_id() const override { return device::I2C::get_device_id(); }
+
+	uint8_t get_device_address() const override { return device::I2C::get_device_address(); }
+private:
+	spa06::calibration_s	_cal{};
+	spa06::data_s		_data{};
+};
+
+spa06::ISPA06 *spa06_i2c_interface(uint8_t busnum, uint32_t device, int bus_frequency)
+{
+	return new SPA06_I2C(busnum, device, bus_frequency);
+}
+
+SPA06_I2C::SPA06_I2C(uint8_t bus, uint32_t device, int bus_frequency) :
+	I2C(DRV_BARO_DEVTYPE_SPA06, MODULE_NAME, bus, device, bus_frequency)
+{
+}
+
+uint8_t
+SPA06_I2C::get_reg(uint8_t addr)
+{
+	uint8_t cmd[2] = { (uint8_t)(addr), 0};
+	transfer(&cmd[0], 1, &cmd[1], 1);
+
+	return cmd[1];
+}
+
+int
+SPA06_I2C::set_reg(uint8_t value, uint8_t addr)
+{
+	uint8_t cmd[2] = { (uint8_t)(addr), value};
+	return transfer(cmd, sizeof(cmd), nullptr, 0);
+}
+
+int
+SPA06_I2C::read(uint8_t addr, uint8_t *buf, uint8_t len)
+{
+	return transfer(&addr, 1, buf, len);
+}
+
+#endif // CONFIG_I2C

src/drivers/barometer/goertek/spa06/SPA06_SPI.cpp

@@ -0,0 +1,104 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 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 SPA06_SPI.cpp
+ *
+ * SPI interface for Goertek SPA06
+ */
+
+#include "spa06.h"
+
+#include <px4_platform_common/px4_config.h>
+#include <drivers/device/spi.h>
+
+#if defined(CONFIG_SPI)
+
+/* SPI protocol address bits */
+#define DIR_READ			(1<<7)  //for set
+#define DIR_WRITE			~(1<<7) //for clear
+
+class SPA06_SPI: public device::SPI, public spa06::ISPA06
+{
+public:
+	SPA06_SPI(uint8_t bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode);
+	virtual ~SPA06_SPI() override = default;
+
+	int init() override { return SPI::init(); }
+
+	uint8_t	get_reg(uint8_t addr) override;
+	int	set_reg(uint8_t value, uint8_t addr) override;
+
+	int read(uint8_t addr, uint8_t *buf, uint8_t len) override;
+
+	uint32_t get_device_id() const override { return device::SPI::get_device_id(); }
+
+	uint8_t get_device_address() const override { return device::SPI::get_device_address(); }
+};
+
+spa06::ISPA06 *
+spa06_spi_interface(uint8_t busnum, uint32_t device, int bus_frequency, spi_mode_e spi_mode)
+{
+	return new SPA06_SPI(busnum, device, bus_frequency, spi_mode);
+}
+
+SPA06_SPI::SPA06_SPI(uint8_t bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode) :
+	SPI(DRV_BARO_DEVTYPE_SPA06, MODULE_NAME, bus, device, spi_mode, bus_frequency)
+{
+}
+
+uint8_t
+SPA06_SPI::get_reg(uint8_t addr)
+{
+	uint8_t cmd[2] = { (uint8_t)(addr | DIR_READ), 0}; // set MSB bit
+	transfer(&cmd[0], &cmd[0], 2);
+
+	return cmd[1];
+}
+
+int
+SPA06_SPI::set_reg(uint8_t value, uint8_t addr)
+{
+	uint8_t cmd[2] = { (uint8_t)(addr & DIR_WRITE), value}; // clear MSB bit
+	return transfer(&cmd[0], nullptr, 2);
+}
+
+int
+SPA06_SPI::read(uint8_t addr, uint8_t *buf, uint8_t len)
+{
+	uint8_t tx_buf[len + 1] = {(uint8_t)(addr | DIR_READ)}; // GCC support VLA, let's use it
+
+	return transfer(tx_buf, buf, len);
+}
+
+#endif // CONFIG_SPI

src/drivers/barometer/goertek/spa06/parameters.c

@@ -0,0 +1,41 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 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.
+ *
+ ****************************************************************************/
+
+/**
+ * Goertek SPA06 Barometer (external I2C)
+ *
+ * @reboot_required true
+ * @group Sensors
+ * @boolean
+  */
+PARAM_DEFINE_INT32(SENS_EN_SPA06, 0);

src/drivers/barometer/goertek/spa06/spa06.h

@@ -0,0 +1,107 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 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 spa06.h
+ *
+ * Shared defines for the SPA06 driver.
+ */
+#pragma once
+
+#include <px4_platform_common/i2c_spi_buses.h>
+
+#define SPA06_ADDR_ID	0x0d
+#define SPA06_ADDR_RESET 0x0c   // set to reset
+#define SPA06_ADDR_CAL 0x10
+#define SPA06_ADDR_PRS_CFG  0x06
+#define SPA06_ADDR_TMP_CFG  0x07
+#define SPA06_ADDR_MEAS_CFG	0x08
+#define SPA06_ADDR_CFG_REG	0x09
+#define SPA06_ADDR_DATA 0x00
+
+
+#define SPA06_VALUE_RESET 9
+#define SPA06_VALUE_ID    0x11
+
+namespace spa06
+{
+
+#pragma pack(push,1)
+struct calibration_s {
+	int16_t c0, c1;
+	int32_t c00, c10;
+	int16_t c01, c11, c20, c21, c30, c31, c40;
+};
+
+struct data_s {
+	uint8_t p_msb;
+	uint8_t p_lsb;
+	uint8_t p_xlsb;
+
+	uint8_t t_msb;
+	uint8_t t_lsb;
+	uint8_t t_xlsb;
+};
+#pragma pack(pop)
+
+class ISPA06
+{
+public:
+	virtual ~ISPA06() = default;
+
+	virtual int init() = 0;
+
+	// read reg value
+	virtual uint8_t get_reg(uint8_t addr) = 0;
+
+	// write reg value
+	virtual int set_reg(uint8_t value, uint8_t addr) = 0;
+
+	// bulk read of data into buffer, return same pointer
+	virtual int read(uint8_t addr, uint8_t *buf, uint8_t len) = 0;
+	// bulk read of calibration data into buffer, return same pointer
+
+	virtual uint32_t get_device_id() const = 0;
+
+	virtual uint8_t get_device_address() const = 0;
+};
+
+} // namespace spa06
+
+
+#if defined(CONFIG_SPI)
+extern spa06::ISPA06 *spa06_spi_interface(uint8_t busnum, uint32_t device, int bus_frequency, spi_mode_e spi_mode);
+#endif // CONFIG_SPI
+#if defined(CONFIG_I2C)
+extern spa06::ISPA06 *spa06_i2c_interface(uint8_t busnum, uint32_t device, int bus_frequency);
+#endif // CONFIG_I2C

src/drivers/barometer/goertek/spa06/spa06_main.cpp

@@ -0,0 +1,141 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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.
+ *
+ ****************************************************************************/
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/module.h>
+
+#include "SPA06.hpp"
+
+#include <drivers/drv_sensor.h>
+
+extern "C" { __EXPORT int spa06_main(int argc, char *argv[]); }
+
+void
+SPA06::print_usage()
+{
+	PRINT_MODULE_USAGE_NAME("spa06", "driver");
+	PRINT_MODULE_USAGE_SUBCATEGORY("baro");
+	PRINT_MODULE_USAGE_COMMAND("start");
+#if defined(CONFIG_I2C)
+	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true);
+	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x76);
+#else
+	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
+#endif
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+}
+
+I2CSPIDriverBase *SPA06::instantiate(const I2CSPIDriverConfig &config, int runtime_instance)
+{
+	spa06::ISPA06 *interface = nullptr;
+
+#if defined(CONFIG_I2C)
+
+	if (config.bus_type == BOARD_I2C_BUS) {
+		interface = spa06_i2c_interface(config.bus, config.i2c_address, config.bus_frequency);
+
+	}
+
+#endif // CONFIG_I2C
+#if defined(CONFIG_SPI)
+
+	if (config.bus_type == BOARD_SPI_BUS) {
+		interface = spa06_spi_interface(config.bus, config.spi_devid, config.bus_frequency, config.spi_mode);
+	}
+
+#endif // CONFIG_SPI
+
+	if (interface == nullptr) {
+		PX4_ERR("failed creating interface for bus %i", config.bus);
+		return nullptr;
+	}
+
+	if (interface->init() != OK) {
+		delete interface;
+		PX4_DEBUG("no device on bus %i", config.bus);
+		return nullptr;
+	}
+
+	SPA06 *dev = new SPA06(config, interface);
+
+	if (dev == nullptr) {
+		delete interface;
+		return nullptr;
+	}
+
+	if (OK != dev->init()) {
+		delete dev;
+		return nullptr;
+	}
+
+	return dev;
+}
+
+int
+spa06_main(int argc, char *argv[])
+{
+	using ThisDriver = SPA06;
+	BusCLIArguments cli{true, true};
+#if defined(CONFIG_I2C)
+	cli.i2c_address = 0x76;
+	cli.default_i2c_frequency = 100 * 1000;
+#endif // CONFIG_I2C
+#if defined(CONFIG_SPI)
+	cli.default_spi_frequency = 10 * 1000 * 1000;
+#endif // CONFIG_SPI
+
+	const char *verb = cli.parseDefaultArguments(argc, argv);
+
+	if (!verb) {
+		ThisDriver::print_usage();
+		return -1;
+	}
+
+	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_BARO_DEVTYPE_SPA06);
+
+	if (!strcmp(verb, "start")) {
+		return ThisDriver::module_start(cli, iterator);
+	}
+
+	if (!strcmp(verb, "stop")) {
+		return ThisDriver::module_stop(iterator);
+	}
+
+	if (!strcmp(verb, "status")) {
+		return ThisDriver::module_status(iterator);
+	}
+
+	ThisDriver::print_usage();
+	return -1;
+}

src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp

@@ -32,24 +32,24 @@
  ****************************************************************************/
 
 #include "lightware_sf45_serial.hpp"
-#include "sf45_commands.h"
 
 #include <inttypes.h>
 #include <fcntl.h>
 #include <termios.h>
 #include <lib/crc/crc.h>
+#include <lib/mathlib/mathlib.h>
 
 #include <float.h>
-#include <mathlib/mathlib.h>
-#include <matrix/matrix/math.hpp>
+
+using namespace time_literals;
 
 /* Configuration Constants */
-#define SF45_MAX_PAYLOAD 256
 #define SF45_SCALE_FACTOR 0.01f
+using namespace matrix;
 
-SF45LaserSerial::SF45LaserSerial(const char *port, uint8_t rotation) :
+SF45LaserSerial::SF45LaserSerial(const char *port) :
 	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)),
-	_px4_rangefinder(0, rotation),
+	_px4_rangefinder(0, distance_sensor_s::ROTATION_CUSTOM),
 	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
 	_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err"))
 {
@@ -68,16 +68,9 @@ SF45LaserSerial::SF45LaserSerial(const char *port, uint8_t rotation) :
 		device_id.devid_s.bus = bus_num;
 	}
 
-	_num_retries = 2;
 	_px4_rangefinder.set_device_id(device_id.devid);
 	_px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_LIGHTWARE_LASER);
-
-	// populate obstacle map members
-	_obstacle_map_msg.frame = obstacle_distance_s::MAV_FRAME_BODY_FRD;
-	_obstacle_map_msg.increment = 5;
-	_obstacle_map_msg.angle_offset = 2.5;
-	_obstacle_map_msg.min_distance = UINT16_MAX;
-	_obstacle_map_msg.max_distance = 5000;
+	_px4_rangefinder.set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_LASER);
 
 }
 
@@ -97,47 +90,41 @@ int SF45LaserSerial::init()
 	param_get(param_find("SF45_YAW_CFG"), &_yaw_cfg);
 
 	/* SF45/B (50M) */
+	_px4_rangefinder.set_orientation(distance_sensor_s::ROTATION_CUSTOM);
 	_px4_rangefinder.set_min_distance(0.2f);
 	_px4_rangefinder.set_max_distance(50.0f);
 	_interval = 10000;
 
 	start();
-
 	return PX4_OK;
 }
 
 int SF45LaserSerial::measure()
 {
-
-	int rate = (int)_update_rate;
-	_data_output = 0x101; // raw distance + yaw readings
+	int32_t rate = (int32_t)_update_rate;
+	_data_output = 0x101; // raw distance (first return) + yaw readings
 	_stream_data = 5; // enable constant streaming
 
-	// send some packets so the sensor starts scanning
+	// send packets to the sensor depending on the state
 	switch (_sensor_state) {
 
-	// sensor should now respond
 	case STATE_UNINIT:
-		while (_num_retries--) {
-			sf45_send(SF_PRODUCT_NAME, false, &_product_name[0], 0);
-			_sensor_state = STATE_UNINIT;
-		}
-
-		_sensor_state = STATE_SEND_PRODUCT_NAME;
+		// Used to probe if the sensor is alive
+		sf45_send(SF_PRODUCT_NAME, false, &_product_name[0], 0);
 		break;
 
-	case STATE_SEND_PRODUCT_NAME:
+	case STATE_ACK_PRODUCT_NAME:
 		// Update rate default to 50 readings/s
 		sf45_send(SF_UPDATE_RATE, true, &rate, sizeof(uint8_t));
-		_sensor_state = STATE_SEND_UPDATE_RATE;
 		break;
 
-	case STATE_SEND_UPDATE_RATE:
+	case STATE_ACK_UPDATE_RATE:
+		// Configure the data that the sensor shall output
 		sf45_send(SF_DISTANCE_OUTPUT, true, &_data_output, sizeof(_data_output));
-		_sensor_state = STATE_SEND_DISTANCE_DATA;
 		break;
 
-	case STATE_SEND_DISTANCE_DATA:
+	case STATE_ACK_DISTANCE_OUTPUT:
+		// Configure the sensor to automatically output data at the configured update rate
 		sf45_send(SF_STREAM, true, &_stream_data, sizeof(_stream_data));
 		_sensor_state = STATE_SEND_STREAM;
 		break;
@@ -151,129 +138,90 @@ int SF45LaserSerial::measure()
 
 int SF45LaserSerial::collect()
 {
-	perf_begin(_sample_perf);
-
-	/* clear buffer if last read was too long ago */
-	int ret;
-	/* the buffer for read chars is buflen minus null termination */
-	uint8_t readbuf[SF45_MAX_PAYLOAD];
-
 	float distance_m = -1.0f;
 
-	/* read from the sensor (uart buffer) */
-	const hrt_abstime timestamp_sample = hrt_absolute_time();
+	if (_sensor_state == STATE_UNINIT) {
 
+		perf_begin(_sample_perf);
+		const int payload_length = 22;
 
+		_crc_valid = false;
+		sf45_get_and_handle_request(payload_length, SF_PRODUCT_NAME);
 
-	if (_sensor_state == STATE_SEND_PRODUCT_NAME) {
-
-		ret = ::read(_fd, &readbuf[0], 22);
-
-		if (ret < 0) {
-			PX4_ERR("ERROR (ack from sending product name cmd): %d", ret);
-			perf_count(_comms_errors);
+		if (_crc_valid) {
+			_sensor_state = STATE_ACK_PRODUCT_NAME;
 			perf_end(_sample_perf);
-			return ret;
+			return PX4_OK;
 		}
 
-		sf45_request_handle(ret, readbuf);
-		ScheduleDelayed(_interval * 3);
+		return -EAGAIN;
 
-	} else if (_sensor_state == STATE_SEND_UPDATE_RATE) {
+	} else if (_sensor_state == STATE_ACK_PRODUCT_NAME) {
 
-		ret = ::read(_fd, &readbuf[0], 7);
+		perf_begin(_sample_perf);
+		const int payload_length = 7;
 
-		if (ret < 0) {
-			PX4_ERR("ERROR (ack from sending update rate cmd): %d", ret);
-			perf_count(_comms_errors);
+		_crc_valid = false;
+		sf45_get_and_handle_request(payload_length, SF_UPDATE_RATE);
+
+		if (_crc_valid) {
+			_sensor_state = STATE_ACK_UPDATE_RATE;
 			perf_end(_sample_perf);
-			return ret;
+			return PX4_OK;
 		}
 
-		if (readbuf[3] == SF_UPDATE_RATE) {
-			sf45_request_handle(ret, readbuf);
-			ScheduleDelayed(_interval * 3);
-		}
+		return -EAGAIN;
 
-	} else if (_sensor_state == STATE_SEND_DISTANCE_DATA) {
+	} else if (_sensor_state == STATE_ACK_UPDATE_RATE) {
 
-		ret = ::read(_fd, &readbuf[0], 8);
+		perf_begin(_sample_perf);
+		const int payload_length = 10;
 
-		if (ret < 0) {
-			PX4_ERR("ERROR (ack from sending distance data cmd): %d", ret);
-			perf_count(_comms_errors);
+		_crc_valid = false;
+		sf45_get_and_handle_request(payload_length, SF_DISTANCE_OUTPUT);
+
+		if (_crc_valid) {
+			_sensor_state = STATE_ACK_DISTANCE_OUTPUT;
 			perf_end(_sample_perf);
-			return ret;
+			return PX4_OK;
 		}
 
-		if (readbuf[3] == SF_DISTANCE_OUTPUT) {
-			sf45_request_handle(ret, readbuf);
-			ScheduleDelayed(_interval * 3);
-		}
-
-		// Stream data from sensor
+		return -EAGAIN;
 
 	} else {
 
-		ret = ::read(_fd, &readbuf[0], 10);
+		// Stream data from sensor
+		perf_begin(_sample_perf);
+		const int payload_length = 10;
 
-		if (ret < 0) {
-			PX4_ERR("ERROR (ack from streaming distance data): %d", ret);
-			perf_count(_comms_errors);
+		_crc_valid = false;
+		sf45_get_and_handle_request(payload_length, SF_DISTANCE_DATA_CM);
+
+		if (_crc_valid) {
+			sf45_process_replies(distance_m);
+			PX4_DEBUG("val (float): %8.4f, valid: %s", (double)distance_m, ((_crc_valid) ? "OK" : "NO"));
 			perf_end(_sample_perf);
-			return ret;
+			return PX4_OK;
 		}
 
-		uint8_t flags_payload = (readbuf[1] >> 6) | (readbuf[2] << 2);
-
-		// Process the incoming distance data
-		if (readbuf[3] == SF_DISTANCE_DATA_CM && flags_payload == 5) {
-
-			for (uint8_t i = 0; i < ret; ++i) {
-				sf45_request_handle(ret, readbuf);
-			}
-
-			if (_init_complete) {
-				sf45_process_replies(&distance_m);
-			} // end if
-
-		} else {
-
-			ret = ::read(_fd, &readbuf[0], 10);
-
-			if (ret < 0) {
-				PX4_ERR("ERROR (unknown sensor data): %d", ret);
-				perf_count(_comms_errors);
-				perf_end(_sample_perf);
-				return ret;
-			}
-		}
-	}
-
-	if (_consecutive_fail_count > 35 && !_sensor_ready) {
-		PX4_ERR("Restarting the state machine");
-		return PX4_ERROR;
-	}
-
-	_last_read = hrt_absolute_time();
-
-	if (!_crc_valid) {
 		return -EAGAIN;
 	}
-
-	PX4_DEBUG("val (float): %8.4f, raw: %s, valid: %s", (double)distance_m, _linebuf, ((_crc_valid) ? "OK" : "NO"));
-	_px4_rangefinder.update(timestamp_sample, distance_m);
-
-	perf_end(_sample_perf);
-
-	return PX4_OK;
 }
 
 void SF45LaserSerial::start()
 {
-	/* reset the report ring and state machine */
+	/* reset the sensor state */
+	_sensor_state = STATE_UNINIT;
+
+	/* reset the report ring */
 	_collect_phase = false;
 
+	/* reset the UART receive buffer size */
+	_linebuf_size = 0;
+
+	/* reset the fail counter */
+	_last_received_time = hrt_absolute_time();
+
 	/* schedule a cycle to start things */
 	ScheduleNow();
 }
@@ -288,8 +236,7 @@ void SF45LaserSerial::Run()
 	/* fds initialized? */
 	if (_fd < 0) {
 		/* open fd: non-blocking read mode*/
-
-		_fd = ::open(_port, O_RDWR | O_NOCTTY);
+		_fd = ::open(_port, O_RDWR | O_NOCTTY | O_NONBLOCK);
 
 		if (_fd < 0) {
 			PX4_ERR("serial open failed (%i)", errno);
@@ -303,34 +250,11 @@ void SF45LaserSerial::Run()
 		/* fill the struct for the new configuration */
 		tcgetattr(_fd, &uart_config);
 
-		uart_config.c_cflag = (uart_config.c_cflag & ~CSIZE) | CS8;
+		/* clear ONLCR flag (which appends a CR for every LF) */
+		uart_config.c_oflag &= ~ONLCR;
 
-		uart_config.c_cflag |= (CLOCAL | CREAD);
-
-		// no parity, 1 stop bit, flow control disabled
-		uart_config.c_cflag &= ~(PARENB | PARODD);
-
-		uart_config.c_cflag |= 0;
-
-		uart_config.c_cflag &= ~CSTOPB;
-
-		uart_config.c_cflag &= ~CRTSCTS;
-
-		uart_config.c_iflag &= ~IGNBRK;
-
-		uart_config.c_iflag &= ~ICRNL;
-
-		uart_config.c_iflag &= ~(IXON | IXOFF | IXANY);
-
-		// echo and echo NL off, canonical mode off (raw mode)
-		// extended input processing off, signal chars off
-		uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
-
-		uart_config.c_oflag = 0;
-
-		uart_config.c_cc[VMIN] = 0;
-
-		uart_config.c_cc[VTIME] = 1;
+		/* no parity, one stop bit */
+		uart_config.c_cflag &= ~(CSTOPB | PARENB);
 
 		unsigned speed = B921600;
 
@@ -346,51 +270,37 @@ void SF45LaserSerial::Run()
 		if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) {
 			PX4_ERR("baud %d ATTR", termios_state);
 		}
-
 	}
 
 	if (_collect_phase) {
-
-		/* perform collection */
-		int collect_ret = collect();
-
-
-		if (collect_ret == -EAGAIN) {
-			/* reschedule to grab the missing bits, time to transmit 8 bytes @ 9600 bps */
-			ScheduleDelayed(1042 * 8);
-
-			return;
-		}
-
-		if (OK != collect_ret) {
-			//  Too many packet errors in init, restart the consecutive fail count
-			_consecutive_fail_count = 0;
-
-			/* restart the measurement state machine */
+		if (hrt_absolute_time() - _last_received_time >= 1_s) {
 			start();
 			return;
-
-		} else {
-			/* apparently success */
-			_consecutive_fail_count = 0;
 		}
 
-		/* next phase is measurement */
-		_collect_phase = false;
+		/* perform collection */
+		if (collect() != PX4_OK && errno != EAGAIN) {
+			PX4_DEBUG("collect error");
+		}
+
+		if (_sensor_state != STATE_SEND_STREAM) {
+			/* next phase is measurement */
+			_collect_phase = false;
+		}
+
+	} else {
+		/* measurement phase */
+
+		if (measure() != PX4_OK) {
+			PX4_DEBUG("measure error");
+		}
+
+		/* next phase is collection */
+		_collect_phase = true;
 	}
 
-	/* measurement phase */
-
-	if (OK != measure()) {
-		PX4_DEBUG("measure error");
-	}
-
-	/* next phase is collection */
-	_collect_phase = true;
-
 	/* schedule a fresh cycle call when the measurement is done */
 	ScheduleDelayed(_interval);
-
 }
 
 void SF45LaserSerial::print_info()
@@ -399,9 +309,8 @@ void SF45LaserSerial::print_info()
 	perf_print_counter(_comms_errors);
 }
 
-void SF45LaserSerial::sf45_request_handle(int return_val, uint8_t *input_buf)
+void SF45LaserSerial::sf45_get_and_handle_request(const int payload_length, const SF_SERIAL_CMD msg_id)
 {
-
 	// SF45 protocol
 	// Start byte is 0xAA and is the start of packet
 	// Payload length sanity check (0-1023) bytes
@@ -410,172 +319,178 @@ void SF45LaserSerial::sf45_request_handle(int return_val, uint8_t *input_buf)
 	// ID byte precedes the data in the payload
 	// CRC comprised of 16-bit checksum (not included in checksum calc.)
 
-	uint16_t recv_crc = 0;
-	bool restart_flag = false;
+	int ret;
+	size_t max_read = sizeof(_linebuf) - _linebuf_size;
+	ret = ::read(_fd, &_linebuf[_linebuf_size], max_read);
 
-	while (restart_flag != true) {
+	if (ret < 0 && errno != EAGAIN) {
+		PX4_ERR("ERROR (ack from streaming distance data): %d", ret);
+		_linebuf_size = 0;
+		perf_count(_comms_errors);
+		perf_end(_sample_perf);
+		return;
+	}
 
-		switch (_parsed_state) {
-		case 0: {
-				if (input_buf[0] == 0xAA) {
-					// start of frame is valid, continue
-					_sop_valid = true;
-					_calc_crc = sf45_format_crc(_calc_crc, _start_of_frame);
-					_parsed_state = 1;
-					break;
+	if (ret > 0) {
+		_last_received_time = hrt_absolute_time();
+		_linebuf_size += ret;
+	}
 
-				} else {
-					_sop_valid = false;
-					_crc_valid = false;
-					_parsed_state = 0;
-					restart_flag = true;
-					_calc_crc = 0;
-					perf_count(_comms_errors);
-					perf_end(_sample_perf);
-					PX4_DEBUG("Start of packet not valid: %d", _sensor_state);
-					_consecutive_fail_count++;
-					break;
-				} // end else
-			} // end case 0
+	// Not enough data to parse a complete packet. Gather more data in the next cycle.
+	if (_linebuf_size < payload_length) {
+		return;
+	}
 
-		case 1: {
-				rx_field.flags_lo = input_buf[1];
-				_calc_crc = sf45_format_crc(_calc_crc, rx_field.flags_lo);
-				_parsed_state = 2;
-				break;
-			}
+	int index = 0;
 
-		case 2: {
-				rx_field.flags_hi = input_buf[2];
-				rx_field.data_len = (rx_field.flags_hi << 2) | (rx_field.flags_lo >> 6);
-				_calc_crc = sf45_format_crc(_calc_crc, rx_field.flags_hi);
+	while (index <= _linebuf_size - payload_length && _crc_valid == false) {
+		bool restart_flag = false;
 
-				// Check payload length against known max value
-				if (rx_field.data_len > 17) {
-					_parsed_state = 0;
-					restart_flag = true;
-					_calc_crc = 0;
-					perf_count(_comms_errors);
-					perf_end(_sample_perf);
-					PX4_DEBUG("Payload length error: %d", _sensor_state);
-					_consecutive_fail_count++;
-					break;
-
-				} else {
-					_parsed_state = 3;
-					break;
-				}
-			}
-
-		case 3: {
-
-				rx_field.msg_id = input_buf[3];
-
-				if (rx_field.msg_id == SF_PRODUCT_NAME || rx_field.msg_id == SF_UPDATE_RATE || rx_field.msg_id == SF_DISTANCE_OUTPUT
-				    || rx_field.msg_id == SF_STREAM || rx_field.msg_id == SF_DISTANCE_DATA_CM) {
-
-					if (rx_field.msg_id == SF_DISTANCE_DATA_CM && rx_field.data_len > 1) {
-						_sensor_ready = true;
+		while (restart_flag != true) {
+			switch (_parsed_state) {
+			case 0: {
+					if (_linebuf[index] == 0xAA) {
+						// start of frame is valid, continue
+						_sop_valid = true;
+						_calc_crc = sf45_format_crc(_calc_crc, _start_of_frame);
+						_parsed_state = 1;
+						break;
 
 					} else {
-						_sensor_ready = false;
+						_sop_valid = false;
+						_crc_valid = false;
+						_parsed_state = 0;
+						restart_flag = true;
+						_calc_crc = 0;
+						PX4_DEBUG("Start of packet not valid: %d", _sensor_state);
+						break;
+					} // end else
+				} // end case 0
+
+			case 1: {
+					rx_field.flags_lo = _linebuf[index + 1];
+					_calc_crc = sf45_format_crc(_calc_crc, rx_field.flags_lo);
+					_parsed_state = 2;
+					break;
+				}
+
+			case 2: {
+					rx_field.flags_hi = _linebuf[index + 2];
+					rx_field.data_len = (rx_field.flags_hi << 2) | (rx_field.flags_lo >> 6);
+					_calc_crc = sf45_format_crc(_calc_crc, rx_field.flags_hi);
+
+					// Check payload length against known max value
+					if (rx_field.data_len > 17) {
+						_parsed_state = 0;
+						restart_flag = true;
+						_calc_crc = 0;
+						PX4_DEBUG("Payload length error: %d", _sensor_state);
+						break;
+
+					} else {
+						_parsed_state = 3;
+						break;
+					}
+				}
+
+			case 3: {
+					rx_field.msg_id = _linebuf[index + 3];
+
+					if (rx_field.msg_id == msg_id) {
+						_calc_crc = sf45_format_crc(_calc_crc, rx_field.msg_id);
+						_parsed_state = 4;
+						break;
 					}
 
-					_calc_crc = sf45_format_crc(_calc_crc, rx_field.msg_id);
-
-					_parsed_state = 4;
-					break;
+					// Ignore message ID's that aren't searched
+					else {
+						_parsed_state = 0;
+						_calc_crc = 0;
+						restart_flag = true;
+						PX4_DEBUG("Non needed message ID: %d", _sensor_state);
+						break;
+					}
 				}
 
-				// Ignore message ID's that aren't defined
-				else {
-					_parsed_state = 0;
-					_calc_crc = 0;
-					restart_flag = true;
-					perf_count(_comms_errors);
-					perf_end(_sample_perf);
-					_consecutive_fail_count++;
-					PX4_DEBUG("Unknown message ID: %d", _sensor_state);
-					break;
+			// Data
+			case 4: {
+					// Process commands with  & w/out data bytes
+					if (rx_field.data_len > 1) {
+						for (uint8_t i = 4; i < 3 + rx_field.data_len; ++i) {
 
-				}
-			}
+							rx_field.data[_data_bytes_recv] = _linebuf[index + i];
+							_calc_crc = sf45_format_crc(_calc_crc, rx_field.data[_data_bytes_recv]);
+							_data_bytes_recv = _data_bytes_recv + 1;
 
-		// Data
-		case 4: {
-				// Process commands with  & w/out data bytes
-				if (rx_field.data_len > 1) {
-					for (uint8_t i = 4; i < 3 + rx_field.data_len; ++i) {
+						}  // end for
+					} //end if
 
-						rx_field.data[_data_bytes_recv] = input_buf[i];
-						_calc_crc = sf45_format_crc(_calc_crc, rx_field.data[_data_bytes_recv]);
-						_data_bytes_recv = _data_bytes_recv + 1;
+					else {
 
-					}  // end for
-				} //end if
-
-				else {
+						_parsed_state = 5;
+						_data_bytes_recv = 0;
+						_calc_crc = sf45_format_crc(_calc_crc, _data_bytes_recv);
+					}
 
 					_parsed_state = 5;
 					_data_bytes_recv = 0;
-					_calc_crc = sf45_format_crc(_calc_crc, _data_bytes_recv);
-
+					break;
 				}
 
-				_parsed_state = 5;
-				_data_bytes_recv = 0;
-				break;
-			}
+			// CRC low byte
+			case 5: {
+					rx_field.crc[0] = _linebuf[index + 3 + rx_field.data_len];
+					_parsed_state = 6;
+					break;
+				}
 
-		// CRC low byte
-		case 5: {
-				rx_field.crc[0] = input_buf[3 + rx_field.data_len];
-				_parsed_state = 6;
-				break;
-			}
+			// CRC high byte
+			case 6: {
+					rx_field.crc[1] = _linebuf[index + 4 + rx_field.data_len];
+					uint16_t recv_crc = (rx_field.crc[1] << 8) | rx_field.crc[0];
 
-		// CRC high byte
-		case 6: {
-				rx_field.crc[1] = input_buf[4 + rx_field.data_len];
-				recv_crc = (rx_field.crc[1] << 8) | rx_field.crc[0];
-
-				// Check the received crc bytes from the sf45 against our own CRC calcuation
-				// If it matches, we can check if sensor ready
-				// Only if crc match is valid and sensor ready (transmitting distance data) do we flag _init_complete
-				if (recv_crc == _calc_crc) {
-					_crc_valid = true;
-
-					// Sensor is ready if we read msg ID 44: SF_DISTANCE_DATA_CM
-					if (_sensor_ready) {
-						_init_complete = true;
+					// Check the received crc bytes from the sf45 against our own CRC calcuation
+					// If it matches, we can check if sensor ready
+					// Only if crc match is valid and sensor ready (transmitting distance data) do we flag _init_complete
+					if (recv_crc == _calc_crc) {
+						_crc_valid = true;
+						_parsed_state = 0;
+						_calc_crc = 0;
+						restart_flag = true;
+						break;
 
 					} else {
-						_init_complete = false;
+
+						_crc_valid = false;
+						_parsed_state = 0;
+						_calc_crc = 0;
+						restart_flag = true;
+						perf_count(_comms_errors);
+						PX4_DEBUG("CRC mismatch: %d", _sensor_state);
+						break;
 					}
-
-					_parsed_state = 0;
-					_calc_crc = 0;
-					restart_flag = true;
-					break;
-
-				} else {
-
-					_crc_valid = false;
-					_init_complete = false;
-					_parsed_state = 0;
-					_calc_crc = 0;
-					restart_flag = true;
-					perf_count(_comms_errors);
-					perf_end(_sample_perf);
-					PX4_DEBUG("CRC mismatch: %d", _sensor_state);
-					break;
 				}
-			}
-		} // end switch
-	} //end while
+			} // end switch
+		} //end while
+
+		index++;
+	}
+
+	// If we parsed successfully, remove the parsed part from the buffer if it is still large enough
+	if (_crc_valid && index + payload_length < _linebuf_size) {
+		unsigned next_after_index = index + payload_length;
+		memmove(&_linebuf[0], &_linebuf[next_after_index], _linebuf_size - next_after_index);
+		_linebuf_size -= next_after_index;
+	}
+
+	// The buffer is filled. Either we can't keep up with the stream and/or it contains only invalid data. Reset to try again.
+	if ((unsigned)_linebuf_size >= sizeof(_linebuf)) {
+		_linebuf_size = 0;
+		perf_count(_comms_errors);
+	}
 }
 
-void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int *data, uint8_t data_len)
+void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int32_t *data, uint8_t data_len)
 {
 	uint16_t crc_val = 0;
 	uint8_t packet_buff[SF45_MAX_PAYLOAD];
@@ -612,7 +527,7 @@ void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int *data, uint8_t d
 	if (msg_id == SF_DISTANCE_OUTPUT) {
 		uint8_t data_convert = data[0] & 0x00FF;
 		// write data bytes to the output buffer
-		packet_buff[data_inc] =  data_convert;
+		packet_buff[data_inc] = data_convert;
 		// Add data bytes to crc add function
 		crc_val = sf45_format_crc(crc_val, data_convert);
 		data_inc = data_inc + 1;
@@ -650,12 +565,11 @@ void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int *data, uint8_t d
 		// Add data bytes to crc add function
 		crc_val = sf45_format_crc(crc_val, data[0]);
 		data_inc = data_inc + 1;
-
 	}
 
 	else {
 		// Product Name
-		PX4_INFO("INFO: Product name");
+		PX4_DEBUG("DEBUG: Product name");
 	}
 
 	uint8_t crc_lo = crc_val & 0xFF;
@@ -683,12 +597,11 @@ void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int *data, uint8_t d
 	}
 }
 
-void SF45LaserSerial::sf45_process_replies(float *distance_m)
+void SF45LaserSerial::sf45_process_replies(float &distance_m)
 {
 	switch (rx_field.msg_id) {
 	case SF_DISTANCE_DATA_CM: {
 
-			uint16_t obstacle_dist_cm = 0;
 			const float raw_distance = (rx_field.data[0] << 0) | (rx_field.data[1] << 8);
 			int16_t raw_yaw = ((rx_field.data[2] << 0) | (rx_field.data[3] << 8));
 			int16_t scaled_yaw = 0;
@@ -696,11 +609,10 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m)
 			// The sensor scans from 0 to -160, so extract negative angle from int16 and represent as if a float
 			if (raw_yaw > 32000) {
 				raw_yaw = raw_yaw - 65535;
-
 			}
 
 			// The sensor is facing downward, so the sensor is flipped about it's x-axis -inverse of each yaw angle
-			if (_orient_cfg == 1) {
+			if (_orient_cfg == ROTATION_DOWNWARD_FACING) {
 				raw_yaw = raw_yaw * -1;
 			}
 
@@ -708,10 +620,10 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m)
 			scaled_yaw = raw_yaw * SF45_SCALE_FACTOR;
 
 			switch (_yaw_cfg) {
-			case 0:
+			case ROTATION_FORWARD_FACING:
 				break;
 
-			case 1:
+			case ROTATION_BACKWARD_FACING:
 				if (scaled_yaw > 180) {
 					scaled_yaw = scaled_yaw - 180;
 
@@ -721,11 +633,11 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m)
 
 				break;
 
-			case 2:
+			case ROTATION_RIGHT_FACING:
 				scaled_yaw = scaled_yaw + 90; // rotation facing right
 				break;
 
-			case 3:
+			case ROTATION_LEFT_FACING:
 				scaled_yaw = scaled_yaw - 90; // rotation facing left
 				break;
 
@@ -733,27 +645,21 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m)
 				break;
 			}
 
-			// Convert to meters for rangefinder update
-			*distance_m = raw_distance * SF45_SCALE_FACTOR;
-			obstacle_dist_cm = (uint16_t)raw_distance;
 
-			uint8_t current_bin = sf45_convert_angle(scaled_yaw);
+			if (raw_distance < 65436) { // Discard invalid readings
+				// Convert to meters for rangefinder update
+				distance_m = raw_distance * SF45_SCALE_FACTOR;
 
-			// If we have moved to a new bin
+				Quatf quaternion(Eulerf{0, 0, sf45_wrap_360(scaled_yaw)*M_DEG_TO_RAD_F});
+				float q[4];
 
-			if (current_bin != _previous_bin) {
-
-				// update the current bin to the distance sensor reading
-				// readings in cm
-				_obstacle_map_msg.distances[current_bin] = obstacle_dist_cm;
-				_obstacle_map_msg.timestamp = hrt_absolute_time();
+				for (int i = 0; i < 4; i++) {
+					q[i] = quaternion(i);
+				}
 
+				_px4_rangefinder.update(hrt_absolute_time(), distance_m, -1, q);
 			}
 
-			_previous_bin = current_bin;
-
-			_obstacle_distance_pub.publish(_obstacle_map_msg);
-
 			break;
 		}
 
@@ -763,16 +669,6 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m)
 	}
 }
 
-uint8_t SF45LaserSerial::sf45_convert_angle(const int16_t yaw)
-{
-
-	uint8_t mapped_sector = 0;
-	float adjusted_yaw = sf45_wrap_360(yaw - _obstacle_map_msg.angle_offset);
-	mapped_sector = round(adjusted_yaw / _obstacle_map_msg.increment);
-
-	return mapped_sector;
-}
-
 float SF45LaserSerial::sf45_wrap_360(float f)
 {
 	return matrix::wrap(f, 0.f, 360.f);

src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp

@@ -56,33 +56,37 @@
 
 enum SF_SERIAL_STATE {
 	STATE_UNINIT = 0,
-	STATE_SEND_PRODUCT_NAME = 1,
-	STATE_SEND_UPDATE_RATE = 2,
-	STATE_SEND_DISTANCE_DATA = 3,
+	STATE_ACK_PRODUCT_NAME = 1,
+	STATE_ACK_UPDATE_RATE = 2,
+	STATE_ACK_DISTANCE_OUTPUT = 3,
 	STATE_SEND_STREAM = 4,
 };
 
 
+enum SensorOrientation {	  // Direction the sensor faces from MAV_SENSOR_ORIENTATION enum
+	ROTATION_FORWARD_FACING = 0,  // MAV_SENSOR_ROTATION_NONE
+	ROTATION_RIGHT_FACING = 2,    // MAV_SENSOR_ROTATION_YAW_90
+	ROTATION_BACKWARD_FACING = 4, // MAV_SENSOR_ROTATION_YAW_180
+	ROTATION_LEFT_FACING = 6,     // MAV_SENSOR_ROTATION_YAW_270
+	ROTATION_UPWARD_FACING = 24,  // MAV_SENSOR_ROTATION_PITCH_90
+	ROTATION_DOWNWARD_FACING = 25,// MAV_SENSOR_ROTATION_PITCH_270
+};
+using namespace time_literals;
 class SF45LaserSerial : public px4::ScheduledWorkItem
 {
 public:
-	SF45LaserSerial(const char *port, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
+	SF45LaserSerial(const char *port);
 	~SF45LaserSerial() override;
 
-	int 			        init();
+	int				init();
 	void				print_info();
-	void                            sf45_request_handle(int val, uint8_t *value);
-	void                            sf45_send(uint8_t msg_id, bool r_w, int *data, uint8_t data_len);
-	uint16_t                        sf45_format_crc(uint16_t crc, uint8_t data_value);
-	void                            sf45_process_replies(float *data);
-	uint8_t                         sf45_convert_angle(const int16_t yaw);
-	float                           sf45_wrap_360(float f);
-protected:
-	obstacle_distance_s                       _obstacle_map_msg{};
-	uORB::Publication<obstacle_distance_s>		_obstacle_distance_pub{ORB_ID(obstacle_distance)};	/**< obstacle_distance publication */
+	void				sf45_get_and_handle_request(const int payload_length, const SF_SERIAL_CMD msg_id);
+	void				sf45_send(uint8_t msg_id, bool r_w, int32_t *data, uint8_t data_len);
+	uint16_t			sf45_format_crc(uint16_t crc, uint8_t data_value);
+	void				sf45_process_replies(float &data);
+	float				sf45_wrap_360(float f);
 
 private:
-
 	void				start();
 	void				stop();
 	void				Run() override;
@@ -91,41 +95,34 @@ private:
 	bool                            _crc_valid{false};
 	PX4Rangefinder                  _px4_rangefinder;
 
+
 	char 				_port[20] {};
-	int	        _interval{10000};
+	int				_interval{2000};
 	bool				_collect_phase{false};
 	int 				_fd{-1};
-	int         _linebuf[256] {};
-	unsigned		_linebuf_index{0};
-	hrt_abstime _last_read{0};
+	uint8_t				_linebuf[SF45_MAX_PAYLOAD] {};
+	int				_linebuf_size{0};
 
 	// SF45/B uses a binary protocol to include header,flags
 	// message ID, payload, and checksum
-	bool                            _is_sf45{false};
-	bool                            _init_complete{false};
-	bool                            _sensor_ready{false};
-	uint8_t                         _sensor_state{0};
-	int                             _baud_rate{0};
-	int                             _product_name[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
-	int                             _stream_data{0};
-	int32_t                         _update_rate{1};
-	int                             _data_output{0};
-	const uint8_t                   _start_of_frame{0xAA};
-	uint16_t                        _data_bytes_recv{0};
-	uint8_t                         _parsed_state{0};
-	bool                            _sop_valid{false};
-	uint16_t                        _calc_crc{0};
-	uint8_t                         _num_retries{0};
-	int32_t                         _yaw_cfg{0};
-	int32_t                         _orient_cfg{0};
-	int32_t                         _collision_constraint{0};
-	uint16_t                        _previous_bin{0};
+	bool				_is_sf45{false};
+	SF_SERIAL_STATE			_sensor_state{STATE_UNINIT};
+	int				_baud_rate{0};
+	int32_t				_product_name[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+	int32_t				_stream_data{0};
+	int32_t				_update_rate{0};
+	int32_t				_data_output{0};
+	const uint8_t			_start_of_frame{0xAA};
+	uint16_t			_data_bytes_recv{0};
+	uint8_t				_parsed_state{0};
+	bool				_sop_valid{false};
+	uint16_t			_calc_crc{0};
+	int32_t				_yaw_cfg{0};
+	int32_t				_orient_cfg{0};
 
 	// end of SF45/B data members
 
-	unsigned			                  _consecutive_fail_count;
-
-	perf_counter_t			            _sample_perf;
-	perf_counter_t			            _comms_errors;
-
+	hrt_abstime			_last_received_time{0};
+	perf_counter_t			_sample_perf;
+	perf_counter_t			_comms_errors;
 };

src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial_main.cpp

@@ -41,7 +41,7 @@ namespace lightware_sf45
 
 SF45LaserSerial *g_dev{nullptr};
 
-static int start(const char *port, uint8_t rotation)
+static int start(const char *port)
 {
 	if (g_dev != nullptr) {
 		PX4_WARN("already started");
@@ -54,7 +54,7 @@ static int start(const char *port, uint8_t rotation)
 	}
 
 	/* create the driver */
-	g_dev = new SF45LaserSerial(port, rotation);
+	g_dev = new SF45LaserSerial(port);
 
 	if (g_dev == nullptr) {
 		return -1;
@@ -102,7 +102,7 @@ static int usage()
 
 Serial bus driver for the Lightware SF45/b Laser rangefinder.
 
-Setup/usage information: https://docs.px4.io/master/en/sensor/sfxx_lidar.html
+Setup/usage information: https://docs.px4.io/main/en/sensor/rangefinders.html
 
 ### Examples
 
@@ -116,7 +116,6 @@ $ lightware_sf45_serial stop
 	PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor");
 	PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start driver");
 	PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false);
-	PRINT_MODULE_USAGE_PARAM_INT('R', 25, 0, 25, "Sensor rotation - downward facing by default", false);
 	PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop driver");
 	return PX4_OK;
 }
@@ -125,18 +124,13 @@ $ lightware_sf45_serial stop
 
 extern "C" __EXPORT int lightware_sf45_serial_main(int argc, char *argv[])
 {
-	uint8_t rotation = distance_sensor_s::ROTATION_FORWARD_FACING;
 	const char *device_path = nullptr;
 	int ch;
 	int myoptind = 1;
 	const char *myoptarg = nullptr;
 
-	while ((ch = px4_getopt(argc, argv, "R:d:", &myoptind, &myoptarg)) != EOF) {
+	while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
-		case 'R':
-			rotation = (uint8_t)atoi(myoptarg);
-			break;
-
 		case 'd':
 			device_path = myoptarg;
 			break;
@@ -153,7 +147,7 @@ extern "C" __EXPORT int lightware_sf45_serial_main(int argc, char *argv[])
 	}
 
 	if (!strcmp(argv[myoptind], "start")) {
-		return lightware_sf45::start(device_path, rotation);
+		return lightware_sf45::start(device_path);
 
 	} else if (!strcmp(argv[myoptind], "stop")) {
 		return lightware_sf45::stop();

src/drivers/distance_sensor/lightware_sf45_serial/module.yaml

@@ -32,7 +32,7 @@ parameters:
                   12: 5000hz
               reboot_required: true
               num_instances: 1
-              default: 1
+              default: 5
 
           SF45_ORIENT_CFG:
               description:
@@ -41,11 +41,11 @@ parameters:
                       The SF45 mounted facing upward or downward on the frame
               type: enum
               values:
-                0: Rotation upward
-                1: Rotation downward
+                24: Rotation upward
+                25: Rotation downward
               reboot_required: true
               num_instances: 1
-              default: 0
+              default: 24
 
           SF45_YAW_CFG:
               description:
@@ -55,9 +55,9 @@ parameters:
               type: enum
               values:
                 0: Rotation forward
-                1: Rotation backward
                 2: Rotation right
-                3: Rotation left
+                4: Rotation backward
+                6: Rotation left
               reboot_required: true
               num_instances: 1
               default: 0

src/drivers/drv_sensor.h

@@ -247,6 +247,7 @@
 
 #define DRV_DIFF_PRESS_DEVTYPE_AUAV	0xE6
 #define DRV_BARO_DEVTYPE_AUAV		0xE7
+#define DRV_BARO_DEVTYPE_SPA06          0xE8
 
 #define DRV_DEVTYPE_UNUSED		0xff
 

src/lib/CMakeLists.txt

@@ -54,6 +54,7 @@ add_subdirectory(geo EXCLUDE_FROM_ALL)
 add_subdirectory(heatshrink EXCLUDE_FROM_ALL)
 add_subdirectory(hysteresis EXCLUDE_FROM_ALL)
 add_subdirectory(l1 EXCLUDE_FROM_ALL)
+add_subdirectory(lat_lon_alt EXCLUDE_FROM_ALL)
 add_subdirectory(led EXCLUDE_FROM_ALL)
 add_subdirectory(matrix EXCLUDE_FROM_ALL)
 add_subdirectory(mathlib EXCLUDE_FROM_ALL)

src/lib/collision_prevention/CollisionPrevention.cpp

@@ -41,51 +41,19 @@
 #include <px4_platform_common/events.h>
 
 using namespace matrix;
-using namespace time_literals;
-
-namespace
-{
-static constexpr int INTERNAL_MAP_INCREMENT_DEG = 10; //cannot be lower than 5 degrees, should divide 360 evenly
-static constexpr int INTERNAL_MAP_USED_BINS = 360 / INTERNAL_MAP_INCREMENT_DEG;
-
-static float wrap_360(float f)
-{
-	return wrap(f, 0.f, 360.f);
-}
-
-static int wrap_bin(int i)
-{
-	i = i % INTERNAL_MAP_USED_BINS;
-
-	while (i < 0) {
-		i += INTERNAL_MAP_USED_BINS;
-	}
-
-	return i;
-}
-
-} // namespace
 
 CollisionPrevention::CollisionPrevention(ModuleParams *parent) :
 	ModuleParams(parent)
 {
-	static_assert(INTERNAL_MAP_INCREMENT_DEG >= 5, "INTERNAL_MAP_INCREMENT_DEG needs to be at least 5");
-	static_assert(360 % INTERNAL_MAP_INCREMENT_DEG == 0, "INTERNAL_MAP_INCREMENT_DEG should divide 360 evenly");
+	static_assert(BIN_SIZE >= 5, "BIN_SIZE must be at least 5");
+	static_assert(360 % BIN_SIZE == 0, "BIN_SIZE must divide 360 evenly");
 
 	// initialize internal obstacle map
-	_obstacle_map_body_frame.timestamp = getTime();
 	_obstacle_map_body_frame.frame = obstacle_distance_s::MAV_FRAME_BODY_FRD;
-	_obstacle_map_body_frame.increment = INTERNAL_MAP_INCREMENT_DEG;
+	_obstacle_map_body_frame.increment = BIN_SIZE;
 	_obstacle_map_body_frame.min_distance = UINT16_MAX;
-	_obstacle_map_body_frame.max_distance = 0;
-	_obstacle_map_body_frame.angle_offset = 0.f;
-	uint32_t internal_bins = sizeof(_obstacle_map_body_frame.distances) / sizeof(_obstacle_map_body_frame.distances[0]);
-	uint64_t current_time = getTime();
 
-	for (uint32_t i = 0 ; i < internal_bins; i++) {
-		_data_timestamps[i] = current_time;
-		_data_maxranges[i] = 0;
-		_data_fov[i] = 0;
+	for (uint32_t i = 0 ; i < BIN_COUNT; i++) {
 		_obstacle_map_body_frame.distances[i] = UINT16_MAX;
 	}
 }
@@ -112,48 +80,234 @@ bool CollisionPrevention::is_active()
 	return activated;
 }
 
-void
-CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle, const matrix::Quatf &vehicle_attitude)
+void CollisionPrevention::modifySetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel)
 {
-	int msg_index = 0;
-	float vehicle_orientation_deg = math::degrees(Eulerf(vehicle_attitude).psi());
-	float increment_factor = 1.f / obstacle.increment;
+	if (_vehicle_attitude_sub.updated()) {
+		vehicle_attitude_s vehicle_attitude;
+
+		if (_vehicle_attitude_sub.copy(&vehicle_attitude)) {
+			_vehicle_attitude = Quatf(vehicle_attitude.q);
+			_vehicle_yaw = Eulerf(_vehicle_attitude).psi();
+		}
+	}
+
+	//calculate movement constraints based on range data
+	const Vector2f original_setpoint = setpoint_accel;
+	_updateObstacleMap();
+	_updateObstacleData();
+	_calculateConstrainedSetpoint(setpoint_accel, setpoint_vel);
+
+	// publish constraints
+	collision_constraints_s	constraints{};
+	original_setpoint.copyTo(constraints.original_setpoint);
+	setpoint_accel.copyTo(constraints.adapted_setpoint);
+	constraints.timestamp = getTime();
+	_constraints_pub.publish(constraints);
+}
+
+void CollisionPrevention::_updateObstacleMap()
+{
+	// add distance sensor data
+	for (auto &dist_sens_sub : _distance_sensor_subs) {
+		distance_sensor_s distance_sensor;
+
+		if (dist_sens_sub.update(&distance_sensor)) {
+			// consider only instances with valid data and orientations useful for collision prevention
+			if ((getElapsedTime(&distance_sensor.timestamp) < RANGE_STREAM_TIMEOUT_US) &&
+			    (distance_sensor.orientation != distance_sensor_s::ROTATION_DOWNWARD_FACING) &&
+			    (distance_sensor.orientation != distance_sensor_s::ROTATION_UPWARD_FACING)) {
+
+				// update message description
+				_obstacle_map_body_frame.timestamp = math::max(_obstacle_map_body_frame.timestamp, distance_sensor.timestamp);
+				_obstacle_map_body_frame.max_distance = math::max(_obstacle_map_body_frame.max_distance,
+									(uint16_t)(distance_sensor.max_distance * 100.0f));
+				_obstacle_map_body_frame.min_distance = math::min(_obstacle_map_body_frame.min_distance,
+									(uint16_t)(distance_sensor.min_distance * 100.0f));
+
+				_addDistanceSensorData(distance_sensor, _vehicle_attitude);
+			}
+		}
+	}
+
+	// add obstacle distance data
+	if (_sub_obstacle_distance.update()) {
+		const obstacle_distance_s &obstacle_distance = _sub_obstacle_distance.get();
+
+		// Update map with obstacle data if the data is not stale
+		if (getElapsedTime(&obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US && obstacle_distance.increment > 0.f) {
+			//update message description
+			_obstacle_map_body_frame.timestamp = math::max(_obstacle_map_body_frame.timestamp, obstacle_distance.timestamp);
+			_obstacle_map_body_frame.max_distance = math::max(_obstacle_map_body_frame.max_distance,
+								obstacle_distance.max_distance);
+			_obstacle_map_body_frame.min_distance = math::min(_obstacle_map_body_frame.min_distance,
+								obstacle_distance.min_distance);
+			_addObstacleSensorData(obstacle_distance, _vehicle_yaw);
+		}
+	}
+
+	// publish fused obtacle distance message with data from offboard obstacle_distance and distance sensor
+	_obstacle_distance_fused_pub.publish(_obstacle_map_body_frame);
+}
+
+void CollisionPrevention::_updateObstacleData()
+{
+	_obstacle_data_present = false;
+	_closest_dist = UINT16_MAX;
+	_closest_dist_dir.setZero();
+
+	for (int i = 0; i < BIN_COUNT; i++) {
+		// if the data is stale, reset the bin
+		if (getTime() - _data_timestamps[i] > RANGE_STREAM_TIMEOUT_US) {
+			_obstacle_map_body_frame.distances[i] = UINT16_MAX;
+		}
+
+		float angle = wrap_2pi(_vehicle_yaw + math::radians((float)i * BIN_SIZE +
+				       _obstacle_map_body_frame.angle_offset));
+		const Vector2f bin_direction = {cosf(angle), sinf(angle)};
+		const uint16_t bin_distance = _obstacle_map_body_frame.distances[i];
+
+		// check if there is avaliable data and the data of the map is not stale
+		if (bin_distance < UINT16_MAX
+		    && (getTime() - _obstacle_map_body_frame.timestamp) < RANGE_STREAM_TIMEOUT_US) {
+			_obstacle_data_present = true;
+		}
+
+		if (bin_distance * 0.01f < _closest_dist) {
+			_closest_dist = bin_distance * 0.01f;
+			_closest_dist_dir = bin_direction;
+		}
+	}
+}
+
+void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel)
+{
+	const float setpoint_length = setpoint_accel.norm();
+	_min_dist_to_keep = math::max(_obstacle_map_body_frame.min_distance / 100.0f, _param_cp_dist.get());
+
+	const hrt_abstime now = getTime();
+
+	const bool is_stick_deflected = setpoint_length > 0.001f;
+
+	if (_obstacle_data_present && is_stick_deflected) {
+
+		_transformSetpoint(setpoint_accel);
+
+		float vel_comp_accel = INFINITY;
+		Vector2f vel_comp_accel_dir{};
+
+		_getVelocityCompensationAcceleration(_vehicle_yaw, setpoint_vel, now,
+						     vel_comp_accel, vel_comp_accel_dir);
+
+		Vector2f constr_accel_setpoint{};
+
+		if (_checkSetpointDirectionFeasability()) {
+			constr_accel_setpoint = _constrainAccelerationSetpoint(setpoint_length);
+		}
+
+		setpoint_accel = constr_accel_setpoint + vel_comp_accel * vel_comp_accel_dir;
+
+	} else if (!_obstacle_data_present) {
+		// allow no movement
+		setpoint_accel.setZero();
+
+		// if distance data is stale, switch to Loiter
+		if (getElapsedTime(&_last_timeout_warning) > 1_s && getElapsedTime(&_time_activated) > 1_s) {
+			if ((now - _obstacle_map_body_frame.timestamp) > TIMEOUT_HOLD_US &&
+			    getElapsedTime(&_time_activated) > TIMEOUT_HOLD_US) {
+				_publishVehicleCmdDoLoiter();
+			}
+
+			PX4_WARN("No obstacle data, not moving...");
+			_last_timeout_warning = now;
+		}
+	}
+}
+
+// TODO this gives false output if the offset is not a multiple of the resolution. to be fixed...
+void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle, const float vehicle_yaw)
+{
+
+	float vehicle_orientation_deg = math::degrees(vehicle_yaw);
+
 
 	if (obstacle.frame == obstacle.MAV_FRAME_GLOBAL || obstacle.frame == obstacle.MAV_FRAME_LOCAL_NED) {
 		// Obstacle message arrives in local_origin frame (north aligned)
 		// corresponding data index (convert to world frame and shift by msg offset)
-		for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) {
-			float bin_angle_deg = (float)i * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset;
-			msg_index = ceil(wrap_360(vehicle_orientation_deg + bin_angle_deg - obstacle.angle_offset) * increment_factor);
+		for (int i = 0; i < BIN_COUNT; i++) {
+			for (int j = 0; (j < 360 / obstacle.increment) && (j < BIN_COUNT); j++) {
+				float bin_lower_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset
+								  - (float)_obstacle_map_body_frame.increment / 2.f);
+				float bin_upper_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset
+								  + (float)_obstacle_map_body_frame.increment / 2.f);
+				float msg_lower_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset - vehicle_orientation_deg -
+								  obstacle.increment / 2.f);
+				float msg_upper_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset - vehicle_orientation_deg +
+								  obstacle.increment / 2.f);
 
-			//add all data points inside to FOV
-			if (obstacle.distances[msg_index] != UINT16_MAX) {
-				if (_enterData(i, obstacle.max_distance * 0.01f, obstacle.distances[msg_index] * 0.01f)) {
-					_obstacle_map_body_frame.distances[i] = obstacle.distances[msg_index];
-					_data_timestamps[i] = _obstacle_map_body_frame.timestamp;
-					_data_maxranges[i] = obstacle.max_distance;
-					_data_fov[i] = 1;
+				// if a bin stretches over the 0/360 degree line, adjust the angles
+				if (bin_lower_angle > bin_upper_angle) {
+					bin_lower_angle -= 360;
 				}
+
+				if (msg_lower_angle > msg_upper_angle) {
+					msg_lower_angle -= 360;
+				}
+
+				// Check for overlaps.
+				if ((msg_lower_angle > bin_lower_angle && msg_lower_angle < bin_upper_angle) ||
+				    (msg_upper_angle > bin_lower_angle && msg_upper_angle < bin_upper_angle) ||
+				    (msg_lower_angle <= bin_lower_angle && msg_upper_angle >= bin_upper_angle) ||
+				    (msg_lower_angle >= bin_lower_angle && msg_upper_angle <= bin_upper_angle)) {
+					if (obstacle.distances[j] != UINT16_MAX) {
+						if (_enterData(i, obstacle.max_distance * 0.01f, obstacle.distances[j] * 0.01f)) {
+							_obstacle_map_body_frame.distances[i] = obstacle.distances[j];
+							_data_timestamps[i] = _obstacle_map_body_frame.timestamp;
+							_data_maxranges[i] = obstacle.max_distance;
+							_data_fov[i] = 1;
+						}
+					}
+				}
+
 			}
 		}
 
 	} else if (obstacle.frame == obstacle.MAV_FRAME_BODY_FRD) {
 		// Obstacle message arrives in body frame (front aligned)
 		// corresponding data index (shift by msg offset)
-		for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) {
-			float bin_angle_deg = (float)i * INTERNAL_MAP_INCREMENT_DEG +
-					      _obstacle_map_body_frame.angle_offset;
-			msg_index = ceil(wrap_360(bin_angle_deg - obstacle.angle_offset) * increment_factor);
+		for (int i = 0; i < BIN_COUNT; i++) {
+			for (int j = 0; j < 360 / obstacle.increment; j++) {
+				float bin_lower_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset
+								  - (float)_obstacle_map_body_frame.increment / 2.f);
+				float bin_upper_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset
+								  + (float)_obstacle_map_body_frame.increment / 2.f);
+				float msg_lower_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset - obstacle.increment / 2.f);
+				float msg_upper_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset + obstacle.increment / 2.f);
 
-			//add all data points inside to FOV
-			if (obstacle.distances[msg_index] != UINT16_MAX) {
-
-				if (_enterData(i, obstacle.max_distance * 0.01f, obstacle.distances[msg_index] * 0.01f)) {
-					_obstacle_map_body_frame.distances[i] = obstacle.distances[msg_index];
-					_data_timestamps[i] = _obstacle_map_body_frame.timestamp;
-					_data_maxranges[i] = obstacle.max_distance;
-					_data_fov[i] = 1;
+				// if a bin stretches over the 0/360 degree line, adjust the angles
+				if (bin_lower_angle > bin_upper_angle) {
+					bin_lower_angle -= 360;
 				}
+
+				if (msg_lower_angle > msg_upper_angle) {
+					msg_lower_angle -= 360;
+				}
+
+				// Check for overlaps.
+				if ((msg_lower_angle > bin_lower_angle && msg_lower_angle < bin_upper_angle) ||
+				    (msg_upper_angle > bin_lower_angle && msg_upper_angle < bin_upper_angle) ||
+				    (msg_lower_angle <= bin_lower_angle && msg_upper_angle >= bin_upper_angle) ||
+				    (msg_lower_angle >= bin_lower_angle && msg_upper_angle <= bin_upper_angle)) {
+					if (obstacle.distances[j] != UINT16_MAX) {
+
+						if (_enterData(i, obstacle.max_distance * 0.01f, obstacle.distances[j] * 0.01f)) {
+							_obstacle_map_body_frame.distances[i] = obstacle.distances[j];
+							_data_timestamps[i] = _obstacle_map_body_frame.timestamp;
+							_data_maxranges[i] = obstacle.max_distance;
+							_data_fov[i] = 1;
+						}
+					}
+				}
+
 			}
 		}
 
@@ -197,80 +351,60 @@ CollisionPrevention::_enterData(int map_index, float sensor_range, float sensor_
 	return false;
 }
 
-void
-CollisionPrevention::_updateObstacleMap()
+bool
+CollisionPrevention::_checkSetpointDirectionFeasability()
 {
-	_sub_vehicle_attitude.update();
+	bool setpoint_feasible = true;
 
-	// add distance sensor data
-	for (auto &dist_sens_sub : _distance_sensor_subs) {
-		distance_sensor_s distance_sensor;
+	for (int i = 0; i < BIN_COUNT; i++) {
+		// check if our setpoint is either pointing in a direction where data exists, or if not, wether we are allowed to go where there is no data
+		if ((_obstacle_map_body_frame.distances[i] == UINT16_MAX && i == _setpoint_index) && (!_param_cp_go_no_data.get()
+				|| (_param_cp_go_no_data.get() && _data_fov[i]))) {
+			setpoint_feasible =  false;
 
-		if (dist_sens_sub.update(&distance_sensor)) {
-			// consider only instances with valid data and orientations useful for collision prevention
-			if ((getElapsedTime(&distance_sensor.timestamp) < RANGE_STREAM_TIMEOUT_US) &&
-			    (distance_sensor.orientation != distance_sensor_s::ROTATION_DOWNWARD_FACING) &&
-			    (distance_sensor.orientation != distance_sensor_s::ROTATION_UPWARD_FACING)) {
-
-				// update message description
-				_obstacle_map_body_frame.timestamp = math::max(_obstacle_map_body_frame.timestamp, distance_sensor.timestamp);
-				_obstacle_map_body_frame.max_distance = math::max(_obstacle_map_body_frame.max_distance,
-									(uint16_t)(distance_sensor.max_distance * 100.0f));
-				_obstacle_map_body_frame.min_distance = math::min(_obstacle_map_body_frame.min_distance,
-									(uint16_t)(distance_sensor.min_distance * 100.0f));
-
-				_addDistanceSensorData(distance_sensor, Quatf(_sub_vehicle_attitude.get().q));
-			}
 		}
 	}
 
-	// add obstacle distance data
-	if (_sub_obstacle_distance.update()) {
-		const obstacle_distance_s &obstacle_distance = _sub_obstacle_distance.get();
-
-		// Update map with obstacle data if the data is not stale
-		if (getElapsedTime(&obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US && obstacle_distance.increment > 0.f) {
-			//update message description
-			_obstacle_map_body_frame.timestamp = math::max(_obstacle_map_body_frame.timestamp, obstacle_distance.timestamp);
-			_obstacle_map_body_frame.max_distance = math::max(_obstacle_map_body_frame.max_distance,
-								obstacle_distance.max_distance);
-			_obstacle_map_body_frame.min_distance = math::min(_obstacle_map_body_frame.min_distance,
-								obstacle_distance.min_distance);
-			_addObstacleSensorData(obstacle_distance, Quatf(_sub_vehicle_attitude.get().q));
-		}
-	}
-
-	// publish fused obtacle distance message with data from offboard obstacle_distance and distance sensor
-	_obstacle_distance_pub.publish(_obstacle_map_body_frame);
+	return setpoint_feasible;
 }
 
 void
-CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, const matrix::Quatf &vehicle_attitude)
+CollisionPrevention::_transformSetpoint(const Vector2f &setpoint)
+{
+	const float sp_angle_body_frame = atan2f(setpoint(1), setpoint(0)) - _vehicle_yaw;
+	const float sp_angle_with_offset_deg = _wrap_360(math::degrees(sp_angle_body_frame) -
+					       _obstacle_map_body_frame.angle_offset);
+	_setpoint_index = floor(sp_angle_with_offset_deg / BIN_SIZE);
+	// change setpoint direction slightly (max by _param_cp_guide_ang degrees) to help guide through narrow gaps
+	_setpoint_dir = setpoint.unit_or_zero();
+	_adaptSetpointDirection(_setpoint_dir, _setpoint_index, _vehicle_yaw);
+}
+
+void
+CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, const Quatf &vehicle_attitude)
 {
 	// clamp at maximum sensor range
 	float distance_reading = math::min(distance_sensor.current_distance, distance_sensor.max_distance);
 
-	// discard values below min range
-	if ((distance_reading > distance_sensor.min_distance)) {
+	// negative values indicate out of range but valid measurements.
+	if (fabsf(distance_sensor.current_distance - -1.f) < FLT_EPSILON && distance_sensor.signal_quality == 0) {
+		distance_reading = distance_sensor.max_distance;
+	}
 
+	// discard values below min range
+	if (distance_reading > distance_sensor.min_distance) {
 		float sensor_yaw_body_rad = _sensorOrientationToYawOffset(distance_sensor, _obstacle_map_body_frame.angle_offset);
 		float sensor_yaw_body_deg = math::degrees(wrap_2pi(sensor_yaw_body_rad));
 
 		// calculate the field of view boundary bin indices
-		int lower_bound = (int)floor((sensor_yaw_body_deg  - math::degrees(distance_sensor.h_fov / 2.0f)) /
-					     INTERNAL_MAP_INCREMENT_DEG);
-		int upper_bound = (int)floor((sensor_yaw_body_deg  + math::degrees(distance_sensor.h_fov / 2.0f)) /
-					     INTERNAL_MAP_INCREMENT_DEG);
+		int lower_bound = (int)round((sensor_yaw_body_deg  - math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE);
+		int upper_bound = (int)round((sensor_yaw_body_deg  + math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE);
 
-		// floor values above zero, ceil values below zero
-		if (lower_bound < 0) { lower_bound++; }
-
-		if (upper_bound < 0) { upper_bound++; }
 
 		// rotate vehicle attitude into the sensor body frame
-		matrix::Quatf attitude_sensor_frame = vehicle_attitude;
+		Quatf attitude_sensor_frame = vehicle_attitude;
 		attitude_sensor_frame.rotate(Vector3f(0.f, 0.f, sensor_yaw_body_rad));
-		float sensor_dist_scale = cosf(Eulerf(attitude_sensor_frame).theta());
+		float sensor_dist_scale = cosf(Eulerf(attitude_sensor_frame).theta()); // verify
 
 		if (distance_reading < distance_sensor.max_distance) {
 			distance_reading = distance_reading * sensor_dist_scale;
@@ -279,7 +413,7 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor,
 		uint16_t sensor_range = static_cast<uint16_t>(100.0f * distance_sensor.max_distance + 0.5f); // convert to cm
 
 		for (int bin = lower_bound; bin <= upper_bound; ++bin) {
-			int wrapped_bin = wrap_bin(bin);
+			int wrapped_bin = _wrap_bin(bin);
 
 			if (_enterData(wrapped_bin, distance_sensor.max_distance, distance_reading)) {
 				_obstacle_map_body_frame.distances[wrapped_bin] = static_cast<uint16_t>(100.0f * distance_reading + 0.5f);
@@ -294,8 +428,7 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor,
 void
 CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad)
 {
-	const float col_prev_d = _param_cp_dist.get();
-	const int guidance_bins = floor(_param_cp_guide_ang.get() / INTERNAL_MAP_INCREMENT_DEG);
+	const int guidance_bins = floor(_param_cp_guide_ang.get() / BIN_SIZE);
 	const int sp_index_original = setpoint_index;
 	float best_cost = 9999.f;
 	int new_sp_index = setpoint_index;
@@ -307,19 +440,19 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi
 		float mean_dist = 0;
 
 		for (int j = i - filter_size; j <= i + filter_size; j++) {
-			int bin = wrap_bin(j);
+			int bin = _wrap_bin(j);
 
 			if (_obstacle_map_body_frame.distances[bin] == UINT16_MAX) {
-				mean_dist += col_prev_d * 100.f;
+				mean_dist += _param_cp_dist.get() * 100.f;
 
 			} else {
 				mean_dist += _obstacle_map_body_frame.distances[bin];
 			}
 		}
 
-		const int bin = wrap_bin(i);
+		const int bin = _wrap_bin(i);
 		mean_dist = mean_dist / (2.f * filter_size + 1.f);
-		const float deviation_cost = col_prev_d * 50.f * abs(i - sp_index_original);
+		const float deviation_cost = _param_cp_dist.get() * 50.f * abs(i - sp_index_original);
 		const float bin_cost = deviation_cost - mean_dist - _obstacle_map_body_frame.distances[bin];
 
 		if (bin_cost < best_cost && _obstacle_map_body_frame.distances[bin] != UINT16_MAX) {
@@ -330,7 +463,7 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi
 
 	//only change setpoint direction if it was moved to a different bin
 	if (new_sp_index != setpoint_index) {
-		float angle = math::radians((float)new_sp_index * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset);
+		float angle = math::radians((float)new_sp_index * BIN_SIZE + _obstacle_map_body_frame.angle_offset);
 		angle = wrap_2pi(vehicle_yaw_angle_rad + angle);
 		setpoint_dir = {cosf(angle), sinf(angle)};
 		setpoint_index = new_sp_index;
@@ -340,7 +473,7 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi
 float
 CollisionPrevention::_sensorOrientationToYawOffset(const distance_sensor_s &distance_sensor, float angle_offset) const
 {
-	float offset = angle_offset > 0.0f ? math::radians(angle_offset) : 0.0f;
+	float offset = math::max(math::radians(angle_offset), 0.f);
 
 	switch (distance_sensor.orientation) {
 	case distance_sensor_s::ROTATION_YAW_0:
@@ -376,180 +509,147 @@ CollisionPrevention::_sensorOrientationToYawOffset(const distance_sensor_s &dist
 		break;
 
 	case distance_sensor_s::ROTATION_CUSTOM:
-		offset = matrix::Eulerf(matrix::Quatf(distance_sensor.q)).psi();
+		offset = Eulerf(Quatf(distance_sensor.q)).psi();
 		break;
 	}
 
 	return offset;
 }
 
-void
-CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vector2f &curr_pos,
-		const Vector2f &curr_vel)
+float CollisionPrevention::_getObstacleDistance(const Vector2f &direction)
 {
-	_updateObstacleMap();
-
-	// read parameters
-	const float col_prev_d = _param_cp_dist.get();
-	const float col_prev_dly = _param_cp_delay.get();
-	const bool move_no_data = _param_cp_go_nodata.get();
-	const float xy_p = _param_mpc_xy_p.get();
-	const float max_jerk = _param_mpc_jerk_max.get();
-	const float max_accel = _param_mpc_acc_hor.get();
-	const matrix::Quatf attitude = Quatf(_sub_vehicle_attitude.get().q);
-	const float vehicle_yaw_angle_rad = Eulerf(attitude).psi();
-
-	const float setpoint_length = setpoint.norm();
-
-	const hrt_abstime constrain_time = getTime();
-	int num_fov_bins = 0;
-
-	if ((constrain_time - _obstacle_map_body_frame.timestamp) < RANGE_STREAM_TIMEOUT_US) {
-		if (setpoint_length > 0.001f) {
-
-			Vector2f setpoint_dir = setpoint / setpoint_length;
-			float vel_max = setpoint_length;
-			const float min_dist_to_keep = math::max(_obstacle_map_body_frame.min_distance / 100.0f, col_prev_d);
-
-			const float sp_angle_body_frame = atan2f(setpoint_dir(1), setpoint_dir(0)) - vehicle_yaw_angle_rad;
-			const float sp_angle_with_offset_deg = wrap_360(math::degrees(sp_angle_body_frame) -
-							       _obstacle_map_body_frame.angle_offset);
-			int sp_index = floor(sp_angle_with_offset_deg / INTERNAL_MAP_INCREMENT_DEG);
-
-			// change setpoint direction slightly (max by _param_cp_guide_ang degrees) to help guide through narrow gaps
-			_adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw_angle_rad);
-
-			// limit speed for safe flight
-			for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { // disregard unused bins at the end of the message
-
-				// delete stale values
-				const hrt_abstime data_age = constrain_time - _data_timestamps[i];
-
-				if (data_age > RANGE_STREAM_TIMEOUT_US) {
-					_obstacle_map_body_frame.distances[i] = UINT16_MAX;
-				}
-
-				const float distance = _obstacle_map_body_frame.distances[i] * 0.01f; // convert to meters
-				const float max_range = _data_maxranges[i] * 0.01f; // convert to meters
-				float angle = math::radians((float)i * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset);
-
-				// convert from body to local frame in the range [0, 2*pi]
-				angle = wrap_2pi(vehicle_yaw_angle_rad + angle);
-
-				// get direction of current bin
-				const Vector2f bin_direction = {cosf(angle), sinf(angle)};
-
-				//count number of bins in the field of valid_new
-				if (_obstacle_map_body_frame.distances[i] < UINT16_MAX) {
-					num_fov_bins ++;
-				}
-
-				if (_obstacle_map_body_frame.distances[i] > _obstacle_map_body_frame.min_distance
-				    && _obstacle_map_body_frame.distances[i] < UINT16_MAX) {
-
-					if (setpoint_dir.dot(bin_direction) > 0) {
-						// calculate max allowed velocity with a P-controller (same gain as in the position controller)
-						const float curr_vel_parallel = math::max(0.f, curr_vel.dot(bin_direction));
-						float delay_distance = curr_vel_parallel * col_prev_dly;
-
-						if (distance < max_range) {
-							delay_distance += curr_vel_parallel * (data_age * 1e-6f);
-						}
-
-						const float stop_distance = math::max(0.f, distance - min_dist_to_keep - delay_distance);
-						const float vel_max_posctrl = xy_p * stop_distance;
-
-						const float vel_max_smooth = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_accel, stop_distance, 0.f);
-						const float projection = bin_direction.dot(setpoint_dir);
-						float vel_max_bin = vel_max;
-
-						if (projection > 0.01f) {
-							vel_max_bin = math::min(vel_max_posctrl, vel_max_smooth) / projection;
-						}
-
-						// constrain the velocity
-						if (vel_max_bin >= 0) {
-							vel_max = math::min(vel_max, vel_max_bin);
-						}
-					}
-
-				} else if (_obstacle_map_body_frame.distances[i] == UINT16_MAX && i == sp_index) {
-					if (!move_no_data || (move_no_data && _data_fov[i])) {
-						vel_max = 0.f;
-					}
-				}
-			}
-
-			//if the sensor field of view is zero, never allow to move (even if move_no_data=1)
-			if (num_fov_bins == 0) {
-				vel_max = 0.f;
-			}
-
-			setpoint = setpoint_dir * vel_max;
-		}
-
-	} else {
-		//allow no movement
-		float vel_max = 0.f;
-		setpoint = setpoint * vel_max;
-
-		// if distance data is stale, switch to Loiter
-		if (getElapsedTime(&_last_timeout_warning) > 1_s && getElapsedTime(&_time_activated) > 1_s) {
-
-			if ((constrain_time - _obstacle_map_body_frame.timestamp) > TIMEOUT_HOLD_US
-			    && getElapsedTime(&_time_activated) > TIMEOUT_HOLD_US) {
-				_publishVehicleCmdDoLoiter();
-			}
-
-			_last_timeout_warning = getTime();
-		}
-
+	float obstacle_distance = 0.f;
+	const float direction_norm = direction.norm();
 
+	if (direction_norm > FLT_EPSILON) {
+		Vector2f dir = direction / direction_norm;
+		const float sp_angle_body_frame = atan2f(dir(1), dir(0)) - _vehicle_yaw;
+		const float sp_angle_with_offset_deg =
+			_wrap_360(math::degrees(sp_angle_body_frame) - _obstacle_map_body_frame.angle_offset);
+		int dir_index = floor(sp_angle_with_offset_deg / BIN_SIZE);
+		dir_index = math::constrain(dir_index, 0, BIN_COUNT - 1);
+		obstacle_distance = _obstacle_map_body_frame.distances[dir_index] * 0.01f;
 	}
+
+	return obstacle_distance;
 }
 
-void
-CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max_speed, const Vector2f &curr_pos,
-				    const Vector2f &curr_vel)
+Vector2f
+CollisionPrevention::_constrainAccelerationSetpoint(const float &setpoint_length)
 {
-	//calculate movement constraints based on range data
-	Vector2f new_setpoint = original_setpoint;
-	_calculateConstrainedSetpoint(new_setpoint, curr_pos, curr_vel);
+	Vector2f new_setpoint{};
+	const Vector2f normal_component = _closest_dist_dir * (_setpoint_dir.dot(_closest_dist_dir));
+	const Vector2f tangential_component = _setpoint_dir - normal_component;
 
-	//warn user if collision prevention starts to interfere
-	bool currently_interfering = (new_setpoint(0) < original_setpoint(0) - 0.05f * max_speed
-				      || new_setpoint(0) > original_setpoint(0) + 0.05f * max_speed
-				      || new_setpoint(1) < original_setpoint(1) - 0.05f * max_speed
-				      || new_setpoint(1) > original_setpoint(1) + 0.05f * max_speed);
+	const float normal_scale = _getScale(_closest_dist);
 
-	_interfering = currently_interfering;
 
-	// publish constraints
-	collision_constraints_s	constraints{};
-	constraints.timestamp = getTime();
-	original_setpoint.copyTo(constraints.original_setpoint);
-	new_setpoint.copyTo(constraints.adapted_setpoint);
-	_constraints_pub.publish(constraints);
+	const float closest_dist_tangential = _getObstacleDistance(tangential_component);
+	const float tangential_scale = _getScale(closest_dist_tangential);
 
-	original_setpoint = new_setpoint;
+
+	// only scale accelerations towards the obstacle
+	if (_closest_dist_dir.dot(_setpoint_dir) > 0) {
+		new_setpoint = (tangential_component * tangential_scale + normal_component * normal_scale) * setpoint_length;
+
+	} else {
+		new_setpoint = _setpoint_dir * setpoint_length;
+	}
+
+	return new_setpoint;
+}
+
+float
+CollisionPrevention::_getScale(const float &reference_distance)
+{
+	float scale = (reference_distance - _min_dist_to_keep);
+	const float scale_distance = math::max(_min_dist_to_keep, _param_mpc_vel_manual.get() / _param_mpc_xy_p.get());
+
+	// if scale is positive, square it and scale it with the scale_distance
+	scale = scale > 0 ? powf(scale / scale_distance, 2) : scale;
+	scale = math::min(scale, 1.0f);
+	return scale;
+}
+
+void CollisionPrevention::_getVelocityCompensationAcceleration(const float vehicle_yaw_angle_rad,
+		const Vector2f &setpoint_vel,
+		const hrt_abstime now, float &vel_comp_accel, Vector2f &vel_comp_accel_dir)
+{
+	for (int i = 0; i < BIN_COUNT; i++) {
+		const float max_range = _data_maxranges[i] * 0.01f;
+
+		// get the vector pointing into the direction of current bin
+		float bin_angle = wrap_2pi(vehicle_yaw_angle_rad
+					   + math::radians((float)i * BIN_SIZE + _obstacle_map_body_frame.angle_offset));
+
+		const Vector2f bin_direction = { cosf(bin_angle), sinf(bin_angle) };
+		float bin_distance = _obstacle_map_body_frame.distances[i];
+
+		// only consider bins which are between min and max values
+		if (bin_distance > _obstacle_map_body_frame.min_distance && bin_distance < UINT16_MAX) {
+			const float distance = bin_distance * 0.01f;
+
+			// Assume current velocity is sufficiently close to the setpoint velocity, this breaks down if flying high
+			// acceleration maneuvers
+			const float curr_vel_parallel = math::max(0.f, setpoint_vel.dot(bin_direction));
+			float delay_distance = curr_vel_parallel * _param_cp_delay.get();
+
+			const hrt_abstime data_age = now - _data_timestamps[i];
+
+			if (distance < max_range) {
+				delay_distance += curr_vel_parallel * (data_age * 1e-6f);
+			}
+
+			const float stop_distance = distance - _min_dist_to_keep - delay_distance;
+
+			float curr_acc_vel_constraint;
+
+			if (stop_distance >= 0.f) {
+				const float max_vel = math::trajectory::computeMaxSpeedFromDistance(_param_mpc_jerk_max.get(),
+						      _param_mpc_acc_hor.get(), stop_distance, 0.f);
+				curr_acc_vel_constraint = _param_mpc_xy_vel_p_acc.get() * math::min(max_vel - curr_vel_parallel, 0.f);
+
+			} else {
+				curr_acc_vel_constraint = -1.f * _param_mpc_xy_vel_p_acc.get() * curr_vel_parallel;
+			}
+
+			if (curr_acc_vel_constraint < vel_comp_accel) {
+				vel_comp_accel = curr_acc_vel_constraint;
+				vel_comp_accel_dir = bin_direction;
+			}
+		}
+	}
 }
 
 void CollisionPrevention::_publishVehicleCmdDoLoiter()
 {
 	vehicle_command_s command{};
-	command.timestamp = getTime();
 	command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
-	command.param1 = (float)1; // base mode
-	command.param3 = (float)0; // sub mode
+	command.param1 = 1.f; // base mode VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED
+	command.param2 = (float)PX4_CUSTOM_MAIN_MODE_AUTO;
+	command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
 	command.target_system = 1;
 	command.target_component = 1;
 	command.source_system = 1;
 	command.source_component = 1;
 	command.confirmation = false;
 	command.from_external = false;
-	command.param2 = (float)PX4_CUSTOM_MAIN_MODE_AUTO;
-	command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
-
-	// publish the vehicle command
+	command.timestamp = getTime();
 	_vehicle_command_pub.publish(command);
 }
+
+float CollisionPrevention::_wrap_360(const float f)
+{
+	return wrap(f, 0.f, 360.f);
+}
+
+int CollisionPrevention::_wrap_bin(int i)
+{
+	i = i % BIN_COUNT;
+
+	while (i < 0) {
+		i += BIN_COUNT;
+	}
+
+	return i;
+}

src/lib/collision_prevention/CollisionPrevention.hpp

@@ -34,6 +34,7 @@
 /**
  * @file CollisionPrevention.hpp
  * @author Tanja Baumann <tanja@auterion.com>
+ * @author Claudio Chies <claudio@chies.com>
  *
  * CollisionPrevention controller.
  *
@@ -74,21 +75,29 @@ public:
 
 	/**
 	 * Computes collision free setpoints
-	 * @param original_setpoint, setpoint before collision prevention intervention
-	 * @param max_speed, maximum xy speed
-	 * @param curr_pos, current vehicle position
-	 * @param curr_vel, current vehicle velocity
+	 * @param setpoint_accel setpoint purely based on sticks, to be modified
+	 * @param setpoint_vel current velocity setpoint as information to be able to stop in time, does not get changed
 	 */
-	void modifySetpoint(matrix::Vector2f &original_setpoint, const float max_speed,
-			    const matrix::Vector2f &curr_pos, const matrix::Vector2f &curr_vel);
+	void modifySetpoint(matrix::Vector2f &setpoint_accel, const matrix::Vector2f &setpoint_vel);
+
+	static constexpr int BIN_COUNT =
+		sizeof(obstacle_distance_s::distances) / sizeof(obstacle_distance_s::distances[0]); // 72
+	static constexpr int BIN_SIZE = 360 / BIN_COUNT; // cannot be lower than 5 degrees, should divide 360 evenly
 
 protected:
+	/** Aggregates the sensor data into an internal obstacle map in body frame */
+	void _updateObstacleMap();
 
-	obstacle_distance_s _obstacle_map_body_frame {};
-	bool _data_fov[sizeof(_obstacle_map_body_frame.distances) / sizeof(_obstacle_map_body_frame.distances[0])];
-	uint64_t _data_timestamps[sizeof(_obstacle_map_body_frame.distances) / sizeof(_obstacle_map_body_frame.distances[0])];
-	uint16_t _data_maxranges[sizeof(_obstacle_map_body_frame.distances) / sizeof(
-										    _obstacle_map_body_frame.distances[0])]; /**< in cm */
+	/** Updates the obstacle data based on stale data and calculates values from the map */
+	void _updateObstacleData();
+
+	/** Calculate the constrained setpoint considering the current obstacle distances, acceleration setpoint and velocity setpoint */
+	void _calculateConstrainedSetpoint(matrix::Vector2f &setpoint_accel, const matrix::Vector2f &setpoint_vel);
+
+	obstacle_distance_s _obstacle_map_body_frame{};
+	bool _data_fov[BIN_COUNT] {};
+	uint64_t _data_timestamps[BIN_COUNT] {};
+	uint16_t _data_maxranges[BIN_COUNT] {}; /**< in cm */
 
 	void _addDistanceSensorData(distance_sensor_s &distance_sensor, const matrix::Quatf &vehicle_attitude);
 
@@ -96,7 +105,7 @@ protected:
 	 * Updates obstacle distance message with measurement from offboard
 	 * @param obstacle, obstacle_distance message to be updated
 	 */
-	void _addObstacleSensorData(const obstacle_distance_s &obstacle, const matrix::Quatf &vehicle_attitude);
+	void _addObstacleSensorData(const obstacle_distance_s &obstacle, const float vehicle_yaw);
 
 	/**
 	 * Computes an adaption to the setpoint direction to guide towards free space
@@ -106,6 +115,34 @@ protected:
 	 */
 	void _adaptSetpointDirection(matrix::Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad);
 
+	/**
+	 * Constrain the acceleration setpoint based on the distance to the obstacle
+	 * The Scaling of the acceleration setpoint is linear below the min_dist_to_keep and quadratic until the scale_distance above
+	 *           +1          ________ _ _
+	 * ┌─┐      │           //
+	 * │X│      │          //
+	 * │X│      │         //
+	 * │X│      │       ///
+	 * │X│      │     //
+	 * │X│      │/////
+	 * │X│──────┼─────────────┬─────────────
+	 * │X│     /│             scale_distance
+	 * │X│    / │
+	 * │X│   /  │
+	 * │X│  /   │
+	 * │X│ /    │
+	 * └─┘/     │
+	 *           -1
+	 */
+	matrix::Vector2f _constrainAccelerationSetpoint(const float &setpoint_length);
+
+	void _getVelocityCompensationAcceleration(const float vehicle_yaw_angle_rad, const matrix::Vector2f &setpoint_vel,
+			const hrt_abstime now, float &vel_comp_accel, matrix::Vector2f &vel_comp_accel_dir);
+
+	float _getObstacleDistance(const matrix::Vector2f &direction);
+
+	float _getScale(const float &reference_distance);
+
 	/**
 	 * Determines whether a new sensor measurement is used
 	 * @param map_index, index of the bin in the internal map the measurement belongs in
@@ -114,25 +151,39 @@ protected:
 	 */
 	bool _enterData(int map_index, float sensor_range, float sensor_reading);
 
+	bool _checkSetpointDirectionFeasability();
+
+	void _transformSetpoint(const matrix::Vector2f &setpoint);
+
 
 	//Timing functions. Necessary to mock time in the tests
 	virtual hrt_abstime getTime();
 	virtual hrt_abstime getElapsedTime(const hrt_abstime *ptr);
 
-
 private:
-
-	bool _interfering{false};		/**< states if the collision prevention interferes with the user input */
+	bool _data_stale{true}; 		/**< states if the data is stale */
 	bool _was_active{false};		/**< states if the collision prevention interferes with the user input */
+	bool _obstacle_data_present{false};	/**< states if obstacle data is present */
+
+	int _setpoint_index{};			/**< index of the setpoint*/
+	matrix::Vector2f _setpoint_dir{};		/**< direction of the setpoint*/
+
+	float _closest_dist{};			/**< closest distance to an obstacle  */
+	matrix::Vector2f _closest_dist_dir{NAN, NAN};	/**< direction of the closest obstacle  */
+
+	float _min_dist_to_keep{};
 
 	orb_advert_t _mavlink_log_pub{nullptr};	 	/**< Mavlink log uORB handle */
 
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+	matrix::Quatf _vehicle_attitude{};
+	float _vehicle_yaw{0.f};
+
 	uORB::Publication<collision_constraints_s>	_constraints_pub{ORB_ID(collision_constraints)};		/**< constraints publication */
-	uORB::Publication<obstacle_distance_s>		_obstacle_distance_pub{ORB_ID(obstacle_distance_fused)};	/**< obstacle_distance publication */
+	uORB::Publication<obstacle_distance_s>		_obstacle_distance_fused_pub{ORB_ID(obstacle_distance_fused)};	/**< obstacle_distance publication */
 	uORB::Publication<vehicle_command_s>	_vehicle_command_pub{ORB_ID(vehicle_command)};			/**< vehicle command do publication */
 
 	uORB::SubscriptionData<obstacle_distance_s> _sub_obstacle_distance{ORB_ID(obstacle_distance)}; /**< obstacle distances received form a range sensor */
-	uORB::SubscriptionData<vehicle_attitude_s> _sub_vehicle_attitude{ORB_ID(vehicle_attitude)};
 	uORB::SubscriptionMultiArray<distance_sensor_s> _distance_sensor_subs{ORB_ID::distance_sensor};
 
 	static constexpr uint64_t RANGE_STREAM_TIMEOUT_US{500_ms};
@@ -142,13 +193,15 @@ private:
 	hrt_abstime	_time_activated{0};
 
 	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::CP_DIST>) _param_cp_dist, /**< collision prevention keep minimum distance */
-		(ParamFloat<px4::params::CP_DELAY>) _param_cp_delay, /**< delay of the range measurement data*/
-		(ParamFloat<px4::params::CP_GUIDE_ANG>) _param_cp_guide_ang, /**< collision prevention change setpoint angle */
-		(ParamBool<px4::params::CP_GO_NO_DATA>) _param_cp_go_nodata, /**< movement allowed where no data*/
-		(ParamFloat<px4::params::MPC_XY_P>) _param_mpc_xy_p, /**< p gain from position controller*/
-		(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max, /**< vehicle maximum jerk*/
-		(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor /**< vehicle maximum horizontal acceleration*/
+		(ParamFloat<px4::params::CP_DIST>) _param_cp_dist, 		/**< collision prevention keep minimum distance */
+		(ParamFloat<px4::params::CP_DELAY>) _param_cp_delay, 		/**< delay of the range measurement data*/
+		(ParamFloat<px4::params::CP_GUIDE_ANG>) _param_cp_guide_ang, 	/**< collision prevention change setpoint angle */
+		(ParamBool<px4::params::CP_GO_NO_DATA>) _param_cp_go_no_data, 	/**< movement allowed where no data*/
+		(ParamFloat<px4::params::MPC_XY_P>) _param_mpc_xy_p, 		/**< p gain from position controller*/
+		(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max, 	/**< vehicle maximum jerk*/
+		(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor, 	/**< vehicle maximum horizontal acceleration*/
+		(ParamFloat<px4::params::MPC_XY_VEL_P_ACC>) _param_mpc_xy_vel_p_acc, /**< p gain from velocity controller*/
+		(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual   /**< maximum velocity in manual flight mode*/
 	)
 
 	/**
@@ -180,14 +233,11 @@ private:
 	 */
 	void _publishObstacleDistance(obstacle_distance_s &obstacle);
 
-	/**
-	 * Aggregates the sensor data into a internal obstacle map in body frame
-	 */
-	void _updateObstacleMap();
-
 	/**
 	 * Publishes vehicle command.
 	 */
 	void _publishVehicleCmdDoLoiter();
 
+	static float _wrap_360(const float f);
+	static int _wrap_bin(int i);
 };

src/lib/drivers/rangefinder/PX4Rangefinder.cpp

@@ -83,3 +83,32 @@ void PX4Rangefinder::update(const hrt_abstime &timestamp_sample, const float dis
 
 	_distance_sensor_pub.update();
 }
+
+void PX4Rangefinder::update(const hrt_abstime &timestamp_sample, const float distance, const int8_t quality,
+			    const float q[4])
+{
+
+	if (_distance_sensor_pub.get().orientation != distance_sensor_s::ROTATION_CUSTOM) {
+		PX4_ERR("Orientation not set to ROTATION_CUSTOM");
+		return;
+	}
+
+	distance_sensor_s &report = _distance_sensor_pub.get();
+
+	report.timestamp = timestamp_sample;
+	report.current_distance = distance;
+	report.signal_quality = quality;
+
+	// if quality is unavailable (-1) set to 0 if distance is outside bounds
+	if (quality < 0) {
+		if ((distance < report.min_distance) || (distance > report.max_distance)) {
+			report.signal_quality = 0;
+		}
+	}
+
+	for (int i = 0; i < 4; i++) {
+		report.q[i] = q[i];
+	}
+
+	_distance_sensor_pub.update();
+}

src/lib/drivers/rangefinder/PX4Rangefinder.hpp

@@ -63,6 +63,7 @@ public:
 	void set_mode(const uint8_t mode) { _distance_sensor_pub.get().mode = mode; }
 
 	void update(const hrt_abstime &timestamp_sample, const float distance, const int8_t quality = -1);
+	void update(const hrt_abstime &timestamp_sample, const float distance, const int8_t quality, const float q[4]);
 
 	int get_instance() { return _distance_sensor_pub.get_instance(); };
 

src/lib/lat_lon_alt/CMakeLists.txt

@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2024 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.
+#
+############################################################################
+
+add_library(lat_lon_alt
+	lat_lon_alt.cpp
+	lat_lon_alt.hpp
+)
+
+add_dependencies(lat_lon_alt prebuild_targets)
+target_include_directories(lat_lon_alt PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+target_compile_options(lat_lon_alt PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+
+px4_add_unit_gtest(SRC test_lat_lon_alt.cpp LINKLIBS lat_lon_alt geo)

src/lib/lat_lon_alt/lat_lon_alt.cpp

@@ -0,0 +1,152 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 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.
+ *
+ ****************************************************************************/
+
+#include "lat_lon_alt.hpp"
+
+using matrix::Vector3f;
+using matrix::Vector3d;
+using matrix::Vector2d;
+
+LatLonAlt LatLonAlt::fromEcef(const Vector3d &p_ecef)
+{
+	// Convert position using Borkowski closed-form exact solution
+	// P. D. Groves, "Principles of GNSS, inertial, and multisensor integrated navigation systems, 2nd edition (appendix C)
+	const double k1 = sqrt(1 - Wgs84::eccentricity2) * std::abs(p_ecef(2));
+	const double k2 = Wgs84::eccentricity2 * Wgs84::equatorial_radius;
+	const double beta = sqrt(p_ecef(0) * p_ecef(0) + p_ecef(1) * p_ecef(1));
+	const double E = (k1 - k2) / beta;
+	const double F = (k1 + k2) / beta;
+
+	const double P = 4.0 / 3.0 * (E * F + 1);
+	const double Q = 2 * (E * E - F * F);
+	const double D = P * P * P + Q * Q;
+	const double V = pow(sqrt(D) - Q, 1.0 / 3.0) - pow(sqrt(D) + Q, 1.0 / 3.0);
+	const double G = 0.5 * (sqrt(E * E + V) + E);
+	const double T = sqrt(G * G + (F - V * G) / (2 * G - E)) - G;
+
+	const double lon = atan2(p_ecef(1), p_ecef(0));
+	const double lat = matrix::sign(p_ecef(2)) * atan((1.0 - T * T) / (2.0 * T * sqrt(1.0 - Wgs84::eccentricity2)));
+	const double alt = (beta - Wgs84::equatorial_radius * T) * cos(lat) +
+			   (p_ecef(2) - matrix::sign(p_ecef(2)) * Wgs84::equatorial_radius * sqrt(1.0 - Wgs84::eccentricity2)) * sin(lat);
+
+	LatLonAlt lla;
+	lla.setLatLonRad(lat, lon);
+	lla.setAltitude(static_cast<float>(alt));
+	return lla;
+}
+
+Vector3d LatLonAlt::toEcef() const
+{
+	const double cos_lat = cos(_latitude_rad);
+	const double sin_lat = sin(_latitude_rad);
+	const double cos_lon = cos(_longitude_rad);
+	const double sin_lon = sin(_longitude_rad);
+
+	const double r_e = Wgs84::equatorial_radius / sqrt(1.0 - std::pow(Wgs84::eccentricity * sin_lat, 2.0));
+	const double r_total = r_e + static_cast<double>(_altitude);
+
+	return Vector3d(r_total * cos_lat * cos_lon,
+			r_total * cos_lat * sin_lon,
+			((1.0 - Wgs84::eccentricity2) * r_e + static_cast<double>(_altitude)) * sin_lat);
+}
+
+Vector3f LatLonAlt::computeAngularRateNavFrame(const Vector3f &v_ned) const
+{
+	double r_n;
+	double r_e;
+	computeRadiiOfCurvature(_latitude_rad, r_n, r_e);
+	return Vector3f(
+		       v_ned(1) / (static_cast<float>(r_e) + _altitude),
+		       -v_ned(0) / (static_cast<float>(r_n) + _altitude),
+		       -v_ned(1) * tanf(_latitude_rad) / (static_cast<float>(r_e) + _altitude));
+}
+
+Vector2d LatLonAlt::deltaLatLonToDeltaXY(const double latitude, const float altitude)
+{
+	double r_n;
+	double r_e;
+	computeRadiiOfCurvature(latitude, r_n, r_e);
+	const double dn_dlat = r_n + static_cast<double>(altitude);
+	const double de_dlon = (r_e + static_cast<double>(altitude)) * cos(latitude);
+
+	return Vector2d(dn_dlat, de_dlon);
+}
+
+void LatLonAlt::computeRadiiOfCurvature(const double latitude, double &meridian_radius_of_curvature,
+					double &transverse_radius_of_curvature)
+{
+	const double tmp = 1.0 - pow(Wgs84::eccentricity * sin(latitude), 2);
+	const double sqrt_tmp = std::sqrt(tmp);
+	meridian_radius_of_curvature = Wgs84::meridian_radius_of_curvature_numerator / (tmp * tmp * sqrt_tmp);
+	transverse_radius_of_curvature = Wgs84::equatorial_radius / sqrt_tmp;
+}
+
+LatLonAlt LatLonAlt::operator+(const matrix::Vector3f &delta_pos) const
+{
+	const matrix::Vector2d d_lat_lon_to_d_xy = deltaLatLonToDeltaXY(latitude_rad(), altitude());
+	const double latitude_rad = matrix::wrap_pi(_latitude_rad + static_cast<double>(delta_pos(0)) / d_lat_lon_to_d_xy(0));
+	const double longitude_rad = matrix::wrap_pi(_longitude_rad + static_cast<double>(delta_pos(1)) / d_lat_lon_to_d_xy(1));
+	const float altitude = _altitude - delta_pos(2);
+
+	LatLonAlt lla_new;
+	lla_new.setLatLonRad(latitude_rad, longitude_rad);
+	lla_new.setAltitude(altitude);
+	return lla_new;
+}
+
+void LatLonAlt::operator+=(const matrix::Vector3f &delta_pos)
+{
+	matrix::Vector2d d_lat_lon_to_d_xy = deltaLatLonToDeltaXY(_latitude_rad, _altitude);
+	_latitude_rad = matrix::wrap_pi(_latitude_rad + static_cast<double>(delta_pos(0)) / d_lat_lon_to_d_xy(0));
+	_longitude_rad = matrix::wrap_pi(_longitude_rad + static_cast<double>(delta_pos(1)) / d_lat_lon_to_d_xy(1));
+	_altitude -= delta_pos(2);
+}
+
+void LatLonAlt::operator+=(const matrix::Vector2f &delta_pos)
+{
+	matrix::Vector2d d_lat_lon_to_d_xy = deltaLatLonToDeltaXY(_latitude_rad, _altitude);
+	_latitude_rad = matrix::wrap_pi(_latitude_rad + static_cast<double>(delta_pos(0)) / d_lat_lon_to_d_xy(0));
+	_longitude_rad = matrix::wrap_pi(_longitude_rad + static_cast<double>(delta_pos(1)) / d_lat_lon_to_d_xy(1));
+}
+
+matrix::Vector3f LatLonAlt::operator-(const LatLonAlt &lla) const
+{
+	const double delta_lat = matrix::wrap_pi(_latitude_rad - lla.latitude_rad());
+	const double delta_lon = matrix::wrap_pi(_longitude_rad - lla.longitude_rad());
+	const float delta_alt = _altitude - lla.altitude();
+
+	const matrix::Vector2d d_lat_lon_to_d_xy = deltaLatLonToDeltaXY(_latitude_rad, _altitude);
+	return matrix::Vector3f(static_cast<float>(delta_lat * d_lat_lon_to_d_xy(0)),
+				static_cast<float>(delta_lon * d_lat_lon_to_d_xy(1)),
+				-delta_alt);
+}

src/lib/lat_lon_alt/lat_lon_alt.hpp

@@ -0,0 +1,119 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "mathlib/math/Limits.hpp"
+#include <matrix/math.hpp>
+
+class LatLonAlt
+{
+public:
+	LatLonAlt() = default;
+	LatLonAlt(const LatLonAlt &lla)
+	{
+		_latitude_rad = lla.latitude_rad();
+		_longitude_rad = lla.longitude_rad();
+		_altitude = lla.altitude();
+	}
+
+	LatLonAlt(const double latitude_deg, const double longitude_deg, const float altitude_m)
+	{
+		_latitude_rad = math::radians(latitude_deg);
+		_longitude_rad = math::radians(longitude_deg);
+		_altitude = altitude_m;
+	}
+
+
+	static LatLonAlt fromEcef(const matrix::Vector3d &p_ecef);
+	matrix::Vector3d toEcef() const;
+
+	void setZero() { _latitude_rad = 0.0; _longitude_rad = 0.0; _altitude = 0.f; }
+
+	double latitude_deg() const { return math::degrees(latitude_rad()); }
+	double longitude_deg() const { return math::degrees(longitude_rad()); }
+
+	const double &latitude_rad() const { return _latitude_rad; }
+	const double &longitude_rad() const { return _longitude_rad; }
+	float altitude() const { return _altitude; }
+
+	void setLatitudeDeg(const double &latitude_deg) { _latitude_rad = math::radians(latitude_deg); }
+	void setLongitudeDeg(const double &longitude_deg) { _longitude_rad = math::radians(longitude_deg); }
+	void setAltitude(const float altitude) { _altitude = altitude; }
+
+	void setLatLon(const LatLonAlt &lla) { _latitude_rad = lla.latitude_rad(); _longitude_rad = lla.longitude_rad(); }
+	void setLatLonDeg(const double latitude, const double longitude) { _latitude_rad = math::radians(latitude); _longitude_rad = math::radians(longitude); }
+	void setLatLonRad(const double latitude, const double longitude) { _latitude_rad = latitude; _longitude_rad = longitude; }
+
+	void print() const { printf("latitude = %f (deg), longitude = %f (deg), altitude = %f (m)\n", _latitude_rad, _longitude_rad, (double)_altitude); }
+
+	/*
+	 * The plus and minus operators below use approximations and should only be used when the Cartesian component is small
+	 */
+	LatLonAlt operator+(const matrix::Vector3f &delta_pos) const;
+	void operator+=(const matrix::Vector3f &delta_pos);
+	void operator+=(const matrix::Vector2f &delta_pos);
+	matrix::Vector3f operator-(const LatLonAlt &lla) const;
+
+	void operator=(const LatLonAlt &lla)
+	{
+		_latitude_rad = lla.latitude_rad();
+		_longitude_rad = lla.longitude_rad();
+		_altitude = lla.altitude();
+	}
+
+	/*
+	 * Compute the angular rate of the local navigation frame at the current latitude and height
+	 * with respect to an inertial frame and resolved in the navigation frame
+	 */
+	matrix::Vector3f computeAngularRateNavFrame(const matrix::Vector3f &v_ned) const;
+
+	struct Wgs84 {
+		static constexpr double equatorial_radius = 6378137.0;
+		static constexpr double eccentricity = 0.0818191908425;
+		static constexpr double eccentricity2 = eccentricity * eccentricity;
+		static constexpr double meridian_radius_of_curvature_numerator = equatorial_radius * (1.0 - eccentricity2);
+		static constexpr double gravity_equator = 9.7803253359;
+	};
+
+private:
+	// Convert between curvilinear and cartesian errors
+	static matrix::Vector2d deltaLatLonToDeltaXY(const double latitude, const float altitude);
+
+	static void computeRadiiOfCurvature(const double latitude, double &meridian_radius_of_curvature,
+					    double &transverse_radius_of_curvature);
+
+	double _latitude_rad{0.0};
+	double _longitude_rad{0.0};
+	float _altitude{0.0};
+};

src/lib/lat_lon_alt/test_lat_lon_alt.cpp

@@ -0,0 +1,121 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2024 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.
+ *
+ ****************************************************************************/
+
+#include <gtest/gtest.h>
+#include <matrix/math.hpp>
+#include <lib/geo/geo.h>
+
+#include "lat_lon_alt.hpp"
+
+using namespace matrix;
+using math::radians;
+using math::degrees;
+
+TEST(TestLatLonAlt, init)
+{
+	LatLonAlt lla(5.7, -2.3, 420);
+	ASSERT_FLOAT_EQ(lla.latitude_deg(), 5.7);
+	ASSERT_FLOAT_EQ(lla.longitude_deg(), -2.3);
+	ASSERT_EQ(lla.altitude(), 420);
+}
+
+TEST(TestLatLonAlt, set)
+{
+	LatLonAlt lla(0.0, 0.0, 0);
+	ASSERT_EQ(lla.latitude_rad(), 0.0);
+	ASSERT_EQ(lla.longitude_rad(), 0.0);
+	ASSERT_EQ(lla.altitude(), 0);
+
+	lla.setLatLonRad(0.1, -0.5);
+	lla.setAltitude(420);
+	ASSERT_EQ(lla.latitude_rad(), 0.1);
+	ASSERT_EQ(lla.longitude_rad(), -0.5);
+	ASSERT_EQ(lla.altitude(), 420);
+}
+
+TEST(TestLatLonAlt, copy)
+{
+	LatLonAlt lla(-0.8, -0.1, 500);
+
+	LatLonAlt lla_copy = lla;
+	ASSERT_EQ(lla_copy.latitude_deg(), -0.8);
+	ASSERT_EQ(lla_copy.longitude_deg(), -0.1);
+	ASSERT_EQ(lla_copy.altitude(), 500);
+}
+
+TEST(TestLatLonAlt, addDeltaPos)
+{
+	MapProjection pos_ref(60.0, 5.0);
+	LatLonAlt lla(pos_ref.getProjectionReferenceLat(), pos_ref.getProjectionReferenceLon(), 400.f);
+
+	Vector3f delta_pos(5.f, -2.f, 3.f);
+	lla += delta_pos;
+
+	double lat_new, lon_new;
+	pos_ref.reproject(delta_pos(0), delta_pos(1), lat_new, lon_new);
+
+	EXPECT_NEAR(lla.latitude_deg(), lat_new, 1e-6);
+	EXPECT_NEAR(lla.longitude_deg(), lon_new, 1e-6);
+	EXPECT_EQ(lla.altitude(), 397.f);
+}
+
+TEST(TestLatLonAlt, subLatLonAlt)
+{
+	MapProjection pos_ref(60.0, 5.0);
+	LatLonAlt lla(pos_ref.getProjectionReferenceLat(), pos_ref.getProjectionReferenceLon(), 0.f);
+
+	const Vector3f delta_pos_true(1.f, -2.f, 3.f);
+
+	double lat_new, lon_new;
+	pos_ref.reproject(delta_pos_true(0), delta_pos_true(1), lat_new, lon_new);
+	LatLonAlt lla_new(lat_new, lon_new, -3.f);
+	Vector3f delta_pos = lla_new - lla;
+
+	EXPECT_NEAR(delta_pos(0), delta_pos_true(0), 1e-2);
+	EXPECT_NEAR(delta_pos(1), delta_pos_true(1), 1e-2);
+	EXPECT_EQ(delta_pos(2), delta_pos_true(2));
+}
+
+TEST(TestLatLonAlt, fromAndToECEF)
+{
+	for (double lat = -M_PI; lat < M_PI; lat += M_PI / 4.0) {
+		for (double lon = -M_PI; lon < M_PI; lon += M_PI / 4.0) {
+			for (float alt = -500.f; alt < 8000.f; alt += 500.f) {
+				LatLonAlt lla(lat, lon, alt);
+
+				LatLonAlt res = LatLonAlt::fromEcef(lla.toEcef());
+				EXPECT_TRUE(!(lla - res).longerThan(10e-6f)) << "lat: " << lat << ", lon: " << lon << ", alt: " << alt;
+			}
+		}
+	}
+}

src/lib/matrix/matrix/Matrix.hpp

@@ -55,6 +55,16 @@ public:
 		}
 	}
 
+	template<typename S>
+	Matrix(const Matrix<S, M, N> &aa)
+	{
+		for (size_t i = 0; i < M; i++) {
+			for (size_t j = 0; j < N; j++) {
+				_data[i][j] = static_cast<Type>(aa(i, j));
+			}
+		}
+	}
+
 	template<size_t P, size_t Q>
 	Matrix(const Slice<Type, M, N, P, Q> &in_slice)
 	{

src/lib/mixer_module/mixer_module.hpp

@@ -170,8 +170,6 @@ public:
 	void setAllMinValues(uint16_t value);
 	void setAllMaxValues(uint16_t value);
 
-	uint16_t &reverseOutputMask() { return _reverse_output_mask; }
-	uint16_t &failsafeValue(int index) { return _failsafe_value[index]; }
 	/** Disarmed values: disarmedValue < minValue needs to hold */
 	uint16_t &disarmedValue(int index) { return _disarmed_value[index]; }
 	uint16_t &minValue(int index) { return _min_value[index]; }

src/lib/motion_planning/PositionSmoothing.cpp

@@ -106,16 +106,32 @@ float PositionSmoothing::_getMaxXYSpeed(const Vector3f(&waypoints)[3]) const
 
 float PositionSmoothing::_getMaxZSpeed(const Vector3f(&waypoints)[3]) const
 {
+	const Vector3f &start_position = {_trajectory[0].getCurrentPosition(),
+					  _trajectory[1].getCurrentPosition(),
+					  _trajectory[2].getCurrentPosition()
+					 };
+	const Vector3f &target = waypoints[1];
+	const Vector3f &next_target = waypoints[2];
 
-	const auto &target = waypoints[1];
+	const Vector2f start_position_xy_z = {start_position.xy().norm(), start_position(2)};
+	const Vector2f target_xy_z = {target.xy().norm(), target(2)};
+	const Vector2f next_target_xy_z = {next_target.xy().norm(), next_target(2)};
 
-	Vector3f pos_traj(_trajectory[0].getCurrentPosition(),
-			  _trajectory[1].getCurrentPosition(),
-			  _trajectory[2].getCurrentPosition());
+	float arrival_z_speed = 0.0f;
+	const bool target_next_different = fabsf(target(2) - next_target(2)) > 0.001f;
 
-	const float distance_start_target = fabs(target(2) - pos_traj(2));
-	const float arrival_z_speed = 0.f;
+	if (target_next_different) {
+		const float alpha = acosf(Vector2f((target_xy_z - start_position_xy_z)).unit_or_zero().dot(
+						  Vector2f((target_xy_z - next_target_xy_z)).unit_or_zero()));
 
+		const float safe_alpha = math::constrain(alpha, 0.f, M_PI_F - FLT_EPSILON);
+		float accel_tmp = _trajectory[2].getMaxAccel();
+		float max_speed_in_turn = math::trajectory::computeMaxSpeedInWaypoint(safe_alpha, accel_tmp,
+					  _vertical_acceptance_radius);
+		arrival_z_speed = math::min(max_speed_in_turn, _trajectory[2].getMaxVel());
+	}
+
+	const float distance_start_target = fabs(target(2) - start_position(2));
 	float max_speed = math::min(_trajectory[2].getMaxVel(), math::trajectory::computeMaxSpeedFromDistance(
 					    _trajectory[2].getMaxJerk(), _trajectory[2].getMaxAccel(),
 					    distance_start_target, arrival_z_speed));

src/lib/motion_planning/PositionSmoothingTest.cpp

@@ -151,14 +151,17 @@ TEST_F(PositionSmoothingTest, reachesTargetVelocityIntegration)
 
 TEST_F(PositionSmoothingTest, reachesTargetInitialVelocity)
 {
-	const int N_ITER = 2000;
+	const int N_ITER = 20000;
 	const float DELTA_T = 0.02f;
 	const Vector3f INITIAL_POSITION{0.f, 0.f, 0.f};
 	const Vector3f TARGET{12.f, 17.f, 8.f};
 	const Vector3f NEXT_TARGET{8.f, 12.f, 80.f};
 
+	const float XY_ACC_RAD = 10.f;
+	const float Z_ACC_RAD = 0.8f;
 
-	Vector3f waypoints[3] = {INITIAL_POSITION, TARGET, NEXT_TARGET};
+
+	Vector3f waypoints[3] = {INITIAL_POSITION, TARGET, TARGET};
 	Vector3f ff_velocity{1.f, 0.1f, 0.3f};
 
 	Vector3f position{0.f, 0.f, 0.f};
@@ -180,12 +183,13 @@ TEST_F(PositionSmoothingTest, reachesTargetInitialVelocity)
 		ff_velocity = {0.f, 0.f, 0.f};
 		expectDynamicsLimitsRespected(out);
 
-		if (position == TARGET) {
+		if (Vector2f(position.xy() - TARGET.xy()).norm() < XY_ACC_RAD && fabsf(position(2) - TARGET(2)) < Z_ACC_RAD) {
 			printf("Converged in %d iterations\n", iteration);
 			break;
 		}
 	}
 
-	EXPECT_EQ(TARGET, position);
+	EXPECT_LT(Vector2f(position.xy() - TARGET.xy()).norm(), XY_ACC_RAD);
+	EXPECT_LT(fabsf(position(2) - TARGET(2)), Z_ACC_RAD);
 	EXPECT_LT(iteration, N_ITER) << "Took too long to converge\n";
 }

src/lib/motion_planning/TrajectoryConstraints.hpp

@@ -74,15 +74,11 @@ inline float computeStartXYSpeedFromWaypoints(const Vector3f &start_position, co
 
 	const bool target_next_different = distance_target_next  > 0.001f;
 	const bool waypoint_overlap = distance_target_next < config.xy_accept_rad;
-	const bool has_reached_altitude = fabsf(target(2) - start_position(2)) < config.z_accept_rad;
-	const bool altitude_stays_same = fabsf(next_target(2) - target(2)) < config.z_accept_rad;
 
 	float speed_at_target = 0.0f;
 
 	if (target_next_different &&
-	    !waypoint_overlap &&
-	    has_reached_altitude &&
-	    altitude_stays_same
+	    !waypoint_overlap
 	   ) {
 		const float alpha = acosf(Vector2f((target - start_position).xy()).unit_or_zero().dot(
 						  Vector2f((target - next_target).xy()).unit_or_zero()));
@@ -108,15 +104,15 @@ inline float computeStartXYSpeedFromWaypoints(const Vector3f &start_position, co
  *
  * @return the maximum speed at waypoint[0] which allows it to follow the trajectory while respecting the dynamic limits
  */
-template <size_t N>
+template <int N>
 float computeXYSpeedFromWaypoints(const Vector3f waypoints[N], const VehicleDynamicLimits &config)
 {
 	static_assert(N >= 2, "Need at least 2 points to compute speed");
 
 	float max_speed = 0.f;
 
-	for (size_t j = 0; j < N - 1; j++) {
-		size_t i = N - 2 - j;
+	// go backwards through the waypoints
+	for (int i = (N - 2); i >= 0; i--) {
 		max_speed = computeStartXYSpeedFromWaypoints(waypoints[i],
 				waypoints[i + 1],
 				waypoints[min(i + 2, N - 1)],

src/lib/pid/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2018-2024 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
@@ -31,4 +31,10 @@
 #
 ############################################################################
 
-px4_add_library(pid pid.cpp)
+px4_add_library(PID
+	PID.cpp
+	PID.hpp
+)
+target_include_directories(PID PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+px4_add_unit_gtest(SRC PIDTest.cpp LINKLIBS PID)

src/lib/pid/PID.cpp

@@ -0,0 +1,75 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022-2024 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.
+ *
+ ****************************************************************************/
+
+#include "PID.hpp"
+#include "lib/mathlib/math/Functions.hpp"
+
+void PID::setGains(const float P, const float I, const float D)
+{
+	_gain_proportional = P;
+	_gain_integral = I;
+	_gain_derivative = D;
+}
+
+float PID::update(const float feedback, const float dt, const bool update_integral)
+{
+	const float error = _setpoint - feedback;
+	const float output = (_gain_proportional * error) + _integral + (_gain_derivative * updateDerivative(feedback, dt));
+
+	if (update_integral) {
+		updateIntegral(error, dt);
+	}
+
+	_last_feedback = feedback;
+	return math::constrain(output, -_limit_output, _limit_output);
+}
+
+void PID::updateIntegral(float error, const float dt)
+{
+	const float integral_new = _integral + _gain_integral * error * dt;
+
+	if (std::isfinite(integral_new)) {
+		_integral = math::constrain(integral_new, -_limit_integral, _limit_integral);
+	}
+}
+
+float PID::updateDerivative(float feedback, const float dt)
+{
+	float feedback_change = 0.f;
+
+	if ((dt > FLT_EPSILON) && std::isfinite(_last_feedback)) {
+		feedback_change = (feedback - _last_feedback) / dt;
+	}
+
+	return feedback_change;
+}

src/lib/pid/PID.hpp

@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <cmath>
+
+class PID
+{
+public:
+	PID() = default;
+	virtual ~PID() = default;
+	void setOutputLimit(const float limit) { _limit_output = limit; }
+	void setIntegralLimit(const float limit) { _limit_integral = limit; }
+	void setGains(const float P, const float I, const float D);
+	void setSetpoint(const float setpoint) { _setpoint = setpoint; }
+	float update(const float feedback, const float dt, const bool update_integral = true);
+	float getIntegral() { return _integral; }
+	void resetIntegral() { _integral = 0.f; };
+	void resetDerivative() { _last_feedback = NAN; };
+private:
+	void updateIntegral(float error, const float dt);
+	float updateDerivative(float feedback, const float dt);
+
+	float _setpoint{0.f}; ///< current setpoint to track
+	float _integral{0.f}; ///< integral state
+	float _last_feedback{NAN};
+
+	// Gains, Limits
+	float _gain_proportional{0.f};
+	float _gain_integral{0.f};
+	float _gain_derivative{0.f};
+	float _limit_integral{0.f};
+	float _limit_output{0.f};
+};

src/lib/pid/PIDTest.cpp

@@ -0,0 +1,130 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2022 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.
+ *
+ ****************************************************************************/
+
+#include <gtest/gtest.h>
+#include <PID.hpp>
+
+TEST(PIDTest, AllZeroCase)
+{
+	PID pid;
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), 0.f);
+}
+
+TEST(PIDTest, OutputLimit)
+{
+	PID pid;
+	pid.setOutputLimit(.01f);
+	pid.setGains(.1f, 0.f, 0.f);
+	pid.setSetpoint(1.f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), .01f);
+	EXPECT_FLOAT_EQ(pid.update(.9f, 0.f, false), .01f);
+	EXPECT_NEAR(pid.update(.95f, 0.f, false), .005f, 1e-6f);
+	EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, false), 0.f);
+	EXPECT_NEAR(pid.update(1.05f, 0.f, false), -.005f, 1e-6f);
+	EXPECT_FLOAT_EQ(pid.update(1.1f, 0.f, false), -.01f);
+	EXPECT_FLOAT_EQ(pid.update(1.15f, 0.f, false), -.01f);
+	EXPECT_FLOAT_EQ(pid.update(2.f, 0.f, false), -.01f);
+}
+
+TEST(PIDTest, ProportinalOnly)
+{
+	PID pid;
+	pid.setOutputLimit(1.f);
+	pid.setGains(.1f, 0.f, 0.f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), 0.f);
+	pid.setSetpoint(1.f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), .1f);
+	EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, false), 0.f);
+
+	float plant = 0.f;
+	float output = 10000.f;
+	int i; // need function scope to check how many steps
+
+	for (i = 1000; i > 0; i--) {
+		const float output_new = pid.update(plant, 0.f, false);
+		plant += output_new;
+
+		// expect the output to get smaller with each iteration
+		if (output_new >= output) {
+			break;
+		}
+
+		output = output_new;
+	}
+
+	EXPECT_FLOAT_EQ(plant, 1.f);
+	EXPECT_GT(i, 0); // it shouldn't have taken longer than an iteration timeout to converge
+}
+
+TEST(PIDTest, InteralOpenLoop)
+{
+	PID pid;
+	pid.setOutputLimit(1.f);
+	pid.setGains(0.f, .1f, 0.f);
+	pid.setIntegralLimit(.05f);
+	pid.setSetpoint(1.f);
+
+	// Zero error
+	EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, true), 0.f);
+	EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, true), 0.f);
+	EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, true), 0.f);
+
+	// Open loop ramp up
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), 0.f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .01f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .02f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .03f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .04f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f);
+
+	// Open loop ramp down
+	pid.setSetpoint(-1.f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .04f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .03f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .02f);
+	EXPECT_NEAR(pid.update(0.f, 0.1f, true), .01f, 1e-6f);
+	EXPECT_NEAR(pid.update(0.f, 0.1f, true), 0.f, 1e-6f);
+	EXPECT_NEAR(pid.update(0.f, 0.1f, true), -.01f, 1e-6f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.02f);
+	EXPECT_NEAR(pid.update(0.f, 0.1f, true), -.03f, 1e-6f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.04f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.05f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.05f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.05f);
+	pid.resetIntegral();
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), 0.f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.01f);
+}

src/lib/pid/pid.cpp

@@ -1,185 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
- *   Author: Laurens Mackay <mackayl@student.ethz.ch>
- *           Tobias Naegeli <naegelit@student.ethz.ch>
- *           Martin Rutschmann <rutmarti@student.ethz.ch>
- *           Anton Babushkin <anton.babushkin@me.com>
- *           Julian Oes <joes@student.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name 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 pid.cpp
- *
- * Implementation of generic PID controller.
- *
- * @author Laurens Mackay <mackayl@student.ethz.ch>
- * @author Tobias Naegeli <naegelit@student.ethz.ch>
- * @author Martin Rutschmann <rutmarti@student.ethz.ch>
- * @author Anton Babushkin <anton.babushkin@me.com>
- * @author Julian Oes <joes@student.ethz.ch>
- */
-
-#include "pid.h"
-#include <math.h>
-#include <px4_platform_common/defines.h>
-
-#define SIGMA 0.000001f
-
-__EXPORT void pid_init(PID_t *pid, pid_mode_t mode, float dt_min)
-{
-	pid->mode = mode;
-	pid->dt_min = dt_min;
-	pid->kp = 0.0f;
-	pid->ki = 0.0f;
-	pid->kd = 0.0f;
-	pid->integral = 0.0f;
-	pid->integral_limit = 0.0f;
-	pid->output_limit = 0.0f;
-	pid->error_previous = 0.0f;
-	pid->last_output = 0.0f;
-}
-
-__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit)
-{
-	int ret = 0;
-
-	if (PX4_ISFINITE(kp)) {
-		pid->kp = kp;
-
-	} else {
-		ret = 1;
-	}
-
-	if (PX4_ISFINITE(ki)) {
-		pid->ki = ki;
-
-	} else {
-		ret = 1;
-	}
-
-	if (PX4_ISFINITE(kd)) {
-		pid->kd = kd;
-
-	} else {
-		ret = 1;
-	}
-
-	if (PX4_ISFINITE(integral_limit)) {
-		pid->integral_limit = integral_limit;
-
-	}  else {
-		ret = 1;
-	}
-
-	if (PX4_ISFINITE(output_limit)) {
-		pid->output_limit = output_limit;
-
-	}  else {
-		ret = 1;
-	}
-
-	return ret;
-}
-
-__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt)
-{
-	if (!PX4_ISFINITE(sp) || !PX4_ISFINITE(val) || !PX4_ISFINITE(val_dot) || !PX4_ISFINITE(dt)) {
-		return pid->last_output;
-	}
-
-	float i, d;
-
-	/* current error value */
-	float error = sp - val;
-
-	/* current error derivative */
-	if (pid->mode == PID_MODE_DERIVATIV_CALC) {
-		d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min);
-		pid->error_previous = error;
-
-	} else if (pid->mode == PID_MODE_DERIVATIV_CALC_NO_SP) {
-		d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min);
-		pid->error_previous = -val;
-
-	} else if (pid->mode == PID_MODE_DERIVATIV_SET) {
-		d = -val_dot;
-
-	} else {
-		d = 0.0f;
-	}
-
-	if (!PX4_ISFINITE(d)) {
-		d = 0.0f;
-	}
-
-	/* calculate PD output */
-	float output = (error * pid->kp) + (d * pid->kd);
-
-	if (pid->ki > SIGMA) {
-		// Calculate the error integral and check for saturation
-		i = pid->integral + (error * dt);
-
-		/* check for saturation */
-		if (PX4_ISFINITE(i)) {
-			if ((pid->output_limit < SIGMA || (fabsf(output + (i * pid->ki)) <= pid->output_limit)) &&
-			    fabsf(i) <= pid->integral_limit) {
-				/* not saturated, use new integral value */
-				pid->integral = i;
-			}
-		}
-
-		/* add I component to output */
-		output += pid->integral * pid->ki;
-	}
-
-	/* limit output */
-	if (PX4_ISFINITE(output)) {
-		if (pid->output_limit > SIGMA) {
-			if (output > pid->output_limit) {
-				output = pid->output_limit;
-
-			} else if (output < -pid->output_limit) {
-				output = -pid->output_limit;
-			}
-		}
-
-		pid->last_output = output;
-	}
-
-	return pid->last_output;
-}
-
-
-__EXPORT void pid_reset_integral(PID_t *pid)
-{
-	pid->integral = 0.0f;
-}

src/lib/pid/pid.h

@@ -1,91 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
- *   Author: Laurens Mackay <mackayl@student.ethz.ch>
- *           Tobias Naegeli <naegelit@student.ethz.ch>
- *           Martin Rutschmann <rutmarti@student.ethz.ch>
- *           Anton Babushkin <anton.babushkin@me.com>
- *           Julian Oes <joes@student.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name 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 pid.h
- *
- * Definition of generic PID controller.
- *
- * @author Laurens Mackay <mackayl@student.ethz.ch>
- * @author Tobias Naegeli <naegelit@student.ethz.ch>
- * @author Martin Rutschmann <rutmarti@student.ethz.ch>
- * @author Anton Babushkin <anton.babushkin@me.com>
- * @author Julian Oes <joes@student.ethz.ch>
- */
-
-#ifndef PID_H_
-#define PID_H_
-
-#include <stdint.h>
-
-__BEGIN_DECLS
-
-typedef enum PID_MODE {
-	/* Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID) */
-	PID_MODE_DERIVATIV_NONE = 0,
-	/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error,
-	 * val_dot in pid_calculate() will be ignored */
-	PID_MODE_DERIVATIV_CALC,
-	/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value,
-	 * setpoint derivative will be ignored, val_dot in pid_calculate() will be ignored */
-	PID_MODE_DERIVATIV_CALC_NO_SP,
-	/* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */
-	PID_MODE_DERIVATIV_SET
-} pid_mode_t;
-
-typedef struct {
-	pid_mode_t mode;
-	float dt_min;
-	float kp;
-	float ki;
-	float kd;
-	float integral;
-	float integral_limit;
-	float output_limit;
-	float error_previous;
-	float last_output;
-} PID_t;
-
-__EXPORT void pid_init(PID_t *pid, pid_mode_t mode, float dt_min);
-__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit);
-__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt);
-__EXPORT void pid_reset_integral(PID_t *pid);
-
-__END_DECLS
-
-#endif /* PID_H_ */

src/modules/commander/Commander.cpp

@@ -716,6 +716,8 @@ Commander::Commander() :
 	}
 
 	updateParameters();
+
+	_failsafe.setOnNotifyUserCallback(&Commander::onFailsafeNotifyUserTrampoline, this);
 }
 
 Commander::~Commander()
@@ -2799,8 +2801,8 @@ void Commander::dataLinkCheck()
 					_open_drone_id_system_lost = false;
 
 					if (_datalink_last_heartbeat_open_drone_id_system != 0) {
-						mavlink_log_info(&_mavlink_log_pub, "OpenDroneID system regained\t");
-						events::send(events::ID("commander_open_drone_id_regained"), events::Log::Info, "OpenDroneID system regained");
+						mavlink_log_info(&_mavlink_log_pub, "Remote ID system regained\t");
+						events::send(events::ID("commander_open_drone_id_regained"), events::Log::Info, "Remote ID system regained");
 					}
 				}
 
@@ -2861,11 +2863,11 @@ void Commander::dataLinkCheck()
 		_status_changed = true;
 	}
 
-	// OpenDroneID system
+	// Remote ID system
 	if ((hrt_elapsed_time(&_datalink_last_heartbeat_open_drone_id_system) > 3_s)
 	    && !_open_drone_id_system_lost) {
-		mavlink_log_critical(&_mavlink_log_pub, "OpenDroneID system lost");
-		events::send(events::ID("commander_open_drone_id_lost"), events::Log::Critical, "OpenDroneID system lost");
+		mavlink_log_critical(&_mavlink_log_pub, "Remote ID system lost");
+		events::send(events::ID("commander_remote_id_lost"), events::Log::Critical, "Remote ID system lost");
 		_vehicle_status.open_drone_id_system_present = false;
 		_vehicle_status.open_drone_id_system_healthy = false;
 		_open_drone_id_system_lost = true;
@@ -3000,6 +3002,20 @@ void Commander::send_parachute_command()
 	set_tune_override(tune_control_s::TUNE_ID_PARACHUTE_RELEASE);
 }
 
+void Commander::onFailsafeNotifyUserTrampoline(void *arg)
+{
+	Commander *commander = static_cast<Commander *>(arg);
+	commander->onFailsafeNotifyUser();
+}
+
+void Commander::onFailsafeNotifyUser()
+{
+	// If we are about to inform about a failsafe, we need to ensure any pending health report is sent out first,
+	// as the failsafe message might reference that. This is only needed in case the report is currently rate-limited,
+	// i.e. it had a recent previous change already.
+	_health_and_arming_checks.reportIfUnreportedDifferences();
+}
+
 int Commander::print_usage(const char *reason)
 {
 	if (reason) {

src/modules/commander/Commander.hpp

@@ -200,6 +200,9 @@ private:
 
 	void modeManagementUpdate();
 
+	static void onFailsafeNotifyUserTrampoline(void *arg);
+	void onFailsafeNotifyUser();
+
 	enum class PrearmedMode {
 		DISABLED = 0,
 		SAFETY_BUTTON = 1,

src/modules/commander/HealthAndArmingChecks/Common.cpp

@@ -219,7 +219,7 @@ bool Report::finalize()
 	return _results_changed;
 }
 
-bool Report::report(bool is_armed, bool force)
+bool Report::report(bool force)
 {
 	const hrt_abstime now = hrt_absolute_time();
 	const bool has_difference = _had_unreported_difference || _results_changed;
@@ -317,3 +317,12 @@ bool Report::report(bool is_armed, bool force)
 			       current_results.health.error, current_results.health.warning);
 	return true;
 }
+
+bool Report::reportIfUnreportedDifferences()
+{
+	if (_had_unreported_difference) {
+		return report(true);
+	}
+
+	return false;
+}

src/modules/commander/HealthAndArmingChecks/Common.hpp

@@ -337,7 +337,12 @@ private:
 	 */
 	bool finalize();
 
-	bool report(bool is_armed, bool force);
+	bool report(bool force);
+
+	/**
+	 * Send out any unreported changes if there are any
+	 */
+	bool reportIfUnreportedDifferences();
 
 	const hrt_abstime _min_reporting_interval;
 

src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp

@@ -67,7 +67,7 @@ bool HealthAndArmingChecks::update(bool force_reporting, bool is_arming_request)
 	}
 
 	const bool results_changed = _reporter.finalize();
-	const bool reported = _reporter.report(_context.isArmed(), force_reporting);
+	const bool reported = _reporter.report(force_reporting);
 
 	if (reported) {
 
@@ -89,7 +89,7 @@ bool HealthAndArmingChecks::update(bool force_reporting, bool is_arming_request)
 		}
 
 		_reporter.finalize();
-		_reporter.report(_context.isArmed(), false);
+		_reporter.report(false);
 		_reporter._mavlink_log_pub = nullptr;
 		// LEGACY end
 
@@ -120,3 +120,8 @@ void HealthAndArmingChecks::updateParams()
 		_checks[i]->updateParams();
 	}
 }
+
+bool HealthAndArmingChecks::reportIfUnreportedDifferences()
+{
+	return _reporter.reportIfUnreportedDifferences();
+}

src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp

@@ -88,6 +88,8 @@ public:
 	 */
 	bool update(bool force_reporting = false, bool is_arming_request = false);
 
+	bool reportIfUnreportedDifferences();
+
 	/**
 	 * Whether arming is possible for a given navigation mode
 	 */

src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecksTest.cpp

@@ -69,7 +69,7 @@ TEST_F(ReporterTest, basic_no_checks)
 
 	reporter.reset();
 	reporter.finalize();
-	reporter.report(false, false);
+	reporter.report(false);
 
 	ASSERT_TRUE(reporter.canArm(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION));
 	ASSERT_EQ((uint8_t)reporter.armingCheckResults().can_arm, 0xff);
@@ -92,7 +92,7 @@ TEST_F(ReporterTest, basic_fail_all_modes)
 		reporter.armingCheckFailure(NavModes::All, health_component_t::remote_control,
 					    events::ID("arming_test_basic_fail_all_modes_fail1"), events::Log::Info, "");
 		reporter.finalize();
-		reporter.report(false, false);
+		reporter.report(false);
 
 		ASSERT_FALSE(reporter.canArm(nav_state));
 		ASSERT_TRUE(reporter.canRun(nav_state));
@@ -113,7 +113,7 @@ TEST_F(ReporterTest, arming_checks_mode_category)
 			       events::ID("arming_test_arming_checks_mode_category_fail2"), events::Log::Info, "");
 	reporter.setIsPresent(health_component_t::battery);
 	reporter.finalize();
-	reporter.report(false, false);
+	reporter.report(false);
 
 	ASSERT_TRUE(reporter.canArm(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION));
 	ASSERT_TRUE(reporter.canRun(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION));
@@ -138,7 +138,7 @@ TEST_F(ReporterTest, arming_checks_mode_category2)
 	reporter.healthFailure(NavModes::Mission, health_component_t::remote_control,
 			       events::ID("arming_test_arming_checks_mode_category2_fail1"), events::Log::Warning, "");
 	reporter.finalize();
-	reporter.report(false, false);
+	reporter.report(false);
 
 	ASSERT_FALSE(reporter.canArm(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION));
 
@@ -178,7 +178,7 @@ TEST_F(ReporterTest, reporting)
 			}
 
 			reporter.finalize();
-			reporter.report(false, false);
+			reporter.report(false);
 			ASSERT_FALSE(reporter.canArm(vehicle_status_s::NAVIGATION_STATE_POSCTL));
 
 			if (i == 0) {
@@ -219,7 +219,7 @@ TEST_F(ReporterTest, reporting)
 			}
 
 			reporter.finalize();
-			reporter.report(false, false);
+			reporter.report(false);
 			ASSERT_FALSE(reporter.canArm(vehicle_status_s::NAVIGATION_STATE_POSCTL));
 
 			if (i == 0) {
@@ -265,7 +265,7 @@ TEST_F(ReporterTest, reporting_multiple)
 		reporter.armingCheckFailure<uint8_t>(NavModes::All, health_component_t::remote_control,
 						     events::ID("arming_test_reporting_multiple_fail3"), events::Log::Warning, "", 55);
 		reporter.finalize();
-		reporter.report(false, false);
+		reporter.report(false);
 		ASSERT_FALSE(reporter.canArm(vehicle_status_s::NAVIGATION_STATE_POSCTL));
 
 		if (i == 0) {

src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp

@@ -77,7 +77,7 @@ void EscChecks::checkAndReport(const Context &context, Report &reporter)
 
 	esc_status_s esc_status;
 
-	if (_esc_status_sub.copy(&esc_status) && now - esc_status.timestamp < esc_telemetry_timeout) {
+	if (_esc_status_sub.copy(&esc_status) && (now < esc_status.timestamp + esc_telemetry_timeout)) {
 
 		checkEscStatus(context, reporter, esc_status);
 		reporter.setIsPresent(health_component_t::motors_escs);

src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp

@@ -813,7 +813,7 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
 
 
 	// altitude
-	failsafe_flags.local_altitude_invalid = !lpos.z_valid || (now > lpos.timestamp + (_param_com_pos_fs_delay.get() * 1_s));
+	failsafe_flags.local_altitude_invalid = !lpos.z_valid || (now > lpos.timestamp + 1_s);
 
 
 	// attitude
@@ -864,7 +864,7 @@ bool EstimatorChecks::checkPosVelValidity(const hrt_abstime &now, const bool dat
 		const bool was_valid) const
 {
 	bool valid = was_valid;
-	const bool data_stale = (now > data_timestamp_us + _param_com_pos_fs_delay.get() * 1_s) || (data_timestamp_us == 0);
+	const bool data_stale = (now > data_timestamp_us + 1_s) || (data_timestamp_us == 0);
 	const float req_accuracy = (was_valid ? required_accuracy * 2.5f : required_accuracy);
 	const bool level_check_pass = data_valid && !data_stale && (data_accuracy < req_accuracy);
 

src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp

@@ -118,7 +118,6 @@ private:
 					(ParamBool<px4::params::SYS_HAS_GPS>) _param_sys_has_gps,
 					(ParamFloat<px4::params::COM_POS_FS_EPH>) _param_com_pos_fs_eph,
 					(ParamFloat<px4::params::COM_VEL_FS_EVH>) _param_com_vel_fs_evh,
-					(ParamInt<px4::params::COM_POS_FS_DELAY>) _param_com_pos_fs_delay,
 					(ParamFloat<px4::params::COM_POS_LOW_EPH>) _param_com_low_eph,
 					(ParamInt<px4::params::COM_POS_LOW_ACT>) _param_com_pos_low_act
 				       )

src/modules/commander/commander_params.c

@@ -512,19 +512,6 @@ PARAM_DEFINE_INT32(COM_ARM_AUTH_MET, 0);
  */
 PARAM_DEFINE_FLOAT(COM_ARM_AUTH_TO, 1);
 
-/**
- * Loss of position failsafe activation delay.
- *
- * This sets number of seconds that the position checks need to be failed before the failsafe will activate.
- * The default value has been optimised for rotary wing applications. For fixed wing applications, a larger value between 5 and 10 should be used.
- *
- * @unit s
- * @group Commander
- * @min 1
- * @max 100
- */
-PARAM_DEFINE_INT32(COM_POS_FS_DELAY, 1);
-
 /**
  * Horizontal position error threshold.
  *

src/modules/commander/failsafe/framework.cpp

@@ -170,6 +170,10 @@ void FailsafeBase::removeActions(ClearCondition condition)
 
 void FailsafeBase::notifyUser(uint8_t user_intended_mode, Action action, Action delayed_action, Cause cause)
 {
+	if (_on_notify_user_cb) {
+		_on_notify_user_cb(_on_notify_user_arg);
+	}
+
 	int delay_s = (_current_delay + 500_ms) / 1_s;
 	PX4_DEBUG("User notification: failsafe triggered (action=%i, delayed_action=%i, cause=%i, delay=%is)", (int)action,
 		  (int)delayed_action, (int)cause, delay_s);

src/modules/commander/failsafe/framework.h

@@ -165,6 +165,17 @@ public:
 	bool getDeferFailsafes() const { return _defer_failsafes; }
 	bool failsafeDeferred() const { return _failsafe_defer_started != 0; }
 
+	using UserCallback = void(*)(void *);
+
+	/**
+	 * Register a callback that is called before notifying the user.
+	 */
+	void setOnNotifyUserCallback(UserCallback callback, void *arg)
+	{
+		_on_notify_user_cb = callback;
+		_on_notify_user_arg = arg;
+	}
+
 protected:
 	enum class UserTakeoverAllowed {
 		Always, ///< allow takeover (immediately)
@@ -278,6 +289,9 @@ private:
 
 	orb_advert_t _mavlink_log_pub{nullptr};
 
+	UserCallback _on_notify_user_cb{nullptr};
+	void *_on_notify_user_arg{nullptr};
+
 	DEFINE_PARAMETERS_CUSTOM_PARENT(ModuleParams,
 					(ParamFloat<px4::params::COM_FAIL_ACT_T>) 	_param_com_fail_act_t
 				       );

src/modules/ekf2/CMakeLists.txt

@@ -277,6 +277,7 @@ px4_add_module(
 		world_magnetic_model
 
 		${EKF_LIBS}
+		lat_lon_alt
 		bias_estimator
 		output_predictor
 	UNITY_BUILD

src/modules/ekf2/EKF/CMakeLists.txt

@@ -154,6 +154,7 @@ target_link_libraries(ecl_EKF
 	PRIVATE
 		bias_estimator
 		geo
+		lat_lon_alt
 		output_predictor
 		world_magnetic_model
 		${EKF_LIBS}

src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp

@@ -74,24 +74,21 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
 		}
 
 		estimator_aid_source2d_s aid_src{};
-		Vector2f position;
+		const LatLonAlt position(sample.latitude, sample.longitude, sample.altitude_amsl);
+		const Vector2f innovation = (ekf.getLatLonAlt() - position).xy(); // altitude measurements are not used
 
-		if (ekf.global_origin_valid()) {
-			position = ekf.global_origin().project(sample.latitude, sample.longitude);
-			//const float hgt = ekf.getEkfGlobalOriginAltitude() - (float)sample.altitude;
-			// relax the upper observation noise limit which prevents bad measurements perturbing the position estimate
-			float pos_noise = math::max(sample.eph, _param_ekf2_agp_noise.get(), 0.01f);
-			const float pos_var = sq(pos_noise);
-			const Vector2f pos_obs_var(pos_var, pos_var);
+		// relax the upper observation noise limit which prevents bad measurements perturbing the position estimate
+		float pos_noise = math::max(sample.eph, _param_ekf2_agp_noise.get(), 0.01f);
+		const float pos_var = sq(pos_noise);
+		const Vector2f pos_obs_var(pos_var, pos_var);
 
-			ekf.updateAidSourceStatus(aid_src,
-						  sample.time_us,                                    // sample timestamp
-						  position,                                          // observation
-						  pos_obs_var,                                       // observation variance
-						  Vector2f(ekf.state().pos) - position,              // innovation
-						  Vector2f(ekf.getPositionVariance()) + pos_obs_var, // innovation variance
-						  math::max(_param_ekf2_agp_gate.get(), 1.f));       // innovation gate
-		}
+		ekf.updateAidSourceStatus(aid_src,
+					  sample.time_us,                                      // sample timestamp
+					  matrix::Vector2d(sample.latitude, sample.longitude), // observation
+					  pos_obs_var,                                         // observation variance
+					  innovation,                                          // innovation
+					  Vector2f(ekf.getPositionVariance()) + pos_obs_var,   // innovation variance
+					  math::max(_param_ekf2_agp_gate.get(), 1.f));         // innovation gate
 
 		const bool starting_conditions = PX4_ISFINITE(sample.latitude) && PX4_ISFINITE(sample.longitude)
 						 && ekf.control_status_flags().yaw_align;
@@ -113,8 +110,8 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
 
 				} else {
 					// Try to initialize using measurement
-					if (ekf.setEkfGlobalOriginFromCurrentPos(sample.latitude, sample.longitude, sample.altitude_amsl, sample.eph,
-							sample.epv)) {
+					if (ekf.resetGlobalPositionTo(sample.latitude, sample.longitude, sample.altitude_amsl, pos_var,
+								      sq(sample.epv))) {
 						ekf.enableControlStatusAuxGpos();
 						_reset_counters.lat_lon = sample.lat_lon_reset_counter;
 						_state = State::active;
@@ -131,7 +128,7 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
 				if (isTimedOut(aid_src.time_last_fuse, imu_delayed.time_us, ekf._params.no_aid_timeout_max)
 				    || (_reset_counters.lat_lon != sample.lat_lon_reset_counter)) {
 
-					ekf.resetHorizontalPositionTo(Vector2f(aid_src.observation), Vector2f(aid_src.observation_variance));
+					ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance));
 
 					ekf.resetAidSourceStatusZeroInnovation(aid_src);
 

src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp

@@ -73,7 +73,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
 
 			if (_baro_counter <= _obs_buffer_length) {
 				// Initialize the pressure offset (included in the baro bias)
-				bias_est.setBias(_state.pos(2) + _baro_lpf.getState());
+				bias_est.setBias(-_gpos.altitude() + _baro_lpf.getState());
 			}
 		}
 
@@ -106,7 +106,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
 		if (measurement_valid) {
 			bias_est.setMaxStateNoise(sqrtf(measurement_var));
 			bias_est.setProcessNoiseSpectralDensity(_params.baro_bias_nsd);
-			bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
+			bias_est.fuseBias(measurement - _gpos.altitude(), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
 		}
 
 		// determine if we should use height aiding
@@ -131,8 +131,8 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
 					ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME);
 
 					_information_events.flags.reset_hgt_to_baro = true;
-					resetVerticalPositionTo(-(_baro_lpf.getState() - bias_est.getBias()), measurement_var);
-					bias_est.setBias(_state.pos(2) + _baro_lpf.getState());
+					resetAltitudeTo(_baro_lpf.getState() - bias_est.getBias(), measurement_var);
+					bias_est.setBias(-_gpos.altitude() + _baro_lpf.getState());
 
 					// reset vertical velocity if no valid sources available
 					if (!isVerticalVelocityAidingActive()) {
@@ -163,12 +163,12 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
 					_height_sensor_ref = HeightSensor::BARO;
 
 					_information_events.flags.reset_hgt_to_baro = true;
-					resetVerticalPositionTo(-(_baro_lpf.getState() - bias_est.getBias()), measurement_var);
-					bias_est.setBias(_state.pos(2) + _baro_lpf.getState());
+					initialiseAltitudeTo(measurement, measurement_var);
+					bias_est.reset();
 
 				} else {
 					ECL_INFO("starting %s height fusion", HGT_SRC_NAME);
-					bias_est.setBias(_state.pos(2) + _baro_lpf.getState());
+					bias_est.setBias(-_gpos.altitude() + _baro_lpf.getState());
 				}
 
 				aid_src.time_last_fuse = imu_sample.time_us;

src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp

@@ -99,7 +99,7 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp
 	if (measurement_valid && quality_sufficient) {
 		bias_est.setMaxStateNoise(sqrtf(measurement_var));
 		bias_est.setProcessNoiseSpectralDensity(_params.ev_hgt_bias_nsd);
-		bias_est.fuseBias(measurement - _state.pos(2), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
+		bias_est.fuseBias(measurement + _gpos.altitude(), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
 	}
 
 	const bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VPOS))
@@ -117,11 +117,11 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp
 
 					if (_height_sensor_ref == HeightSensor::EV) {
 						_information_events.flags.reset_hgt_to_ev = true;
-						resetVerticalPositionTo(measurement, measurement_var);
+						resetAltitudeTo(-measurement, measurement_var);
 						bias_est.reset();
 
 					} else {
-						bias_est.setBias(-_state.pos(2) + measurement);
+						bias_est.setBias(_gpos.altitude() + measurement);
 					}
 
 					aid_src.time_last_fuse = _time_delayed_us;
@@ -146,8 +146,8 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp
 				// All height sources are failing
 				ECL_WARN("%s fusion reset required, all height sources failing", AID_SRC_NAME);
 				_information_events.flags.reset_hgt_to_ev = true;
-				resetVerticalPositionTo(measurement - bias_est.getBias(), measurement_var);
-				bias_est.setBias(-_state.pos(2) + measurement);
+				resetAltitudeTo(-measurement - bias_est.getBias(), measurement_var);
+				bias_est.setBias(_gpos.altitude() + measurement);
 
 				aid_src.time_last_fuse = _time_delayed_us;
 
@@ -170,14 +170,14 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp
 			if (_params.height_sensor_ref == static_cast<int32_t>(HeightSensor::EV)) {
 				ECL_INFO("starting %s fusion, resetting state", AID_SRC_NAME);
 				_information_events.flags.reset_hgt_to_ev = true;
-				resetVerticalPositionTo(measurement, measurement_var);
+				resetAltitudeTo(-measurement, measurement_var);
 
 				_height_sensor_ref = HeightSensor::EV;
 				bias_est.reset();
 
 			} else {
 				ECL_INFO("starting %s fusion", AID_SRC_NAME);
-				bias_est.setBias(-_state.pos(2) + measurement);
+				bias_est.setBias(_gpos.altitude() + measurement);
 			}
 
 			aid_src.time_last_fuse = _time_delayed_us;

src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp

@@ -137,6 +137,8 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
 
 #endif // CONFIG_EKF2_GNSS
 
+	const Vector2f position_estimate = getLocalHorizontalPosition();
+
 	const Vector2f measurement{pos(0), pos(1)};
 
 	const Vector2f measurement_var{
@@ -150,7 +152,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
 	if (!bias_fusion_was_active && _ev_pos_b_est.fusionActive()) {
 		if (quality_sufficient) {
 			// reset the bias estimator
-			_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
+			_ev_pos_b_est.setBias(-position_estimate + measurement);
 
 		} else if (isOtherSourceOfHorizontalAidingThan(_control_status.flags.ev_pos)) {
 			// otherwise stop EV position, when quality is good again it will restart with reset bias
@@ -165,7 +167,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
 			      ev_sample.time_us,                                      // sample timestamp
 			      position,                                               // observation
 			      pos_obs_var,                                            // observation variance
-			      Vector2f(_state.pos) - position,                        // innovation
+			      position_estimate - position,                           // innovation
 			      Vector2f(getStateVariance<State::pos>()) + pos_obs_var, // innovation variance
 			      math::max(_params.ev_pos_innov_gate, 1.f));             // innovation gate
 
@@ -174,7 +176,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
 	if (measurement_valid && quality_sufficient) {
 		_ev_pos_b_est.setMaxStateNoise(Vector2f(sqrtf(measurement_var(0)), sqrtf(measurement_var(1))));
 		_ev_pos_b_est.setProcessNoiseSpectralDensity(_params.ev_hgt_bias_nsd); // TODO
-		_ev_pos_b_est.fuseBias(measurement - Vector2f(_state.pos.xy()),
+		_ev_pos_b_est.fuseBias(measurement - position_estimate,
 				       measurement_var + Vector2f(getStateVariance<State::pos>()));
 	}
 
@@ -213,7 +215,7 @@ void Ekf::startEvPosFusion(const Vector2f &measurement, const Vector2f &measurem
 	// TODO:  (_params.position_sensor_ref == PositionSensor::EV)
 	if (_control_status.flags.gps) {
 		ECL_INFO("starting %s fusion", EV_AID_SRC_NAME);
-		_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
+		_ev_pos_b_est.setBias(-getLocalHorizontalPosition() + measurement);
 		_ev_pos_b_est.setFusionActive();
 
 	} else {
@@ -245,7 +247,7 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure
 				_ev_pos_b_est.reset();
 
 			} else {
-				_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
+				_ev_pos_b_est.setBias(-getLocalHorizontalPosition() + measurement);
 			}
 
 			aid_src.time_last_fuse = _time_delayed_us;
@@ -275,14 +277,14 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure
 
 			if (_control_status.flags.gps && !pos_xy_fusion_failing) {
 				// reset EV position bias
-				_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
+				_ev_pos_b_est.setBias(-Vector2f(getLocalHorizontalPosition()) + measurement);
 
 			} else {
 				_information_events.flags.reset_pos_to_vision = true;
 
 				if (_control_status.flags.gps) {
 					resetHorizontalPositionTo(measurement - _ev_pos_b_est.getBias(), measurement_var + _ev_pos_b_est.getBiasVar());
-					_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
+					_ev_pos_b_est.setBias(-getLocalHorizontalPosition() + measurement);
 
 				} else {
 					resetHorizontalPositionTo(measurement, measurement_var);

src/modules/ekf2/EKF/aid_sources/fake_height_control.cpp

@@ -51,7 +51,7 @@ void Ekf::controlFakeHgtFusion()
 		const float obs_var = sq(_params.pos_noaid_noise);
 		const float innov_gate = 3.f;
 
-		updateVerticalPositionAidStatus(aid_src, _time_delayed_us, _last_known_pos(2), obs_var, innov_gate);
+		updateVerticalPositionAidStatus(aid_src, _time_delayed_us, -_last_known_gpos.altitude(), obs_var, innov_gate);
 
 		const bool continuing_conditions_passing = !isVerticalAidingActive();
 		const bool starting_conditions_passing = continuing_conditions_passing
@@ -98,7 +98,7 @@ void Ekf::controlFakeHgtFusion()
 void Ekf::resetFakeHgtFusion()
 {
 	ECL_INFO("reset fake height fusion");
-	_last_known_pos(2) = _state.pos(2);
+	_last_known_gpos.setAltitude(_gpos.altitude());
 
 	resetVerticalVelocityToZero();
 	resetHeightToLastKnown();
@@ -109,8 +109,8 @@ void Ekf::resetFakeHgtFusion()
 void Ekf::resetHeightToLastKnown()
 {
 	_information_events.flags.reset_pos_to_last_known = true;
-	ECL_INFO("reset height to last known (%.3f)", (double)_last_known_pos(2));
-	resetVerticalPositionTo(_last_known_pos(2), sq(_params.pos_noaid_noise));
+	ECL_INFO("reset height to last known (%.3f)", (double)_last_known_gpos.altitude());
+	resetAltitudeTo(_last_known_gpos.altitude(), sq(_params.pos_noaid_noise));
 }
 
 void Ekf::stopFakeHgtFusion()

src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp

@@ -63,17 +63,17 @@ void Ekf::controlFakePosFusion()
 			obs_var(0) = obs_var(1) = sq(0.5f);
 		}
 
-		const Vector2f position(_last_known_pos);
+		const Vector2f innovation = (_gpos - _last_known_gpos).xy();
 
 		const float innov_gate = 3.f;
 
 		updateAidSourceStatus(aid_src,
 				      _time_delayed_us,
-				      position,                                           // observation
-				      obs_var,                                            // observation variance
-				      Vector2f(_state.pos) - position,                    // innovation
-				      Vector2f(getStateVariance<State::pos>()) + obs_var, // innovation variance
-				      innov_gate);                                        // innovation gate
+				      Vector2f(_gpos.latitude_deg(), _gpos.longitude_deg()), // observation
+				      obs_var,                                               // observation variance
+				      innovation,                       // innovation
+				      Vector2f(getStateVariance<State::pos>()) + obs_var,    // innovation variance
+				      innov_gate);                                           // innovation gate
 
 		const bool enable_valid_fake_pos = _control_status.flags.constant_pos || _control_status.flags.vehicle_at_rest;
 		const bool enable_fake_pos = !enable_valid_fake_pos
@@ -95,7 +95,7 @@ void Ekf::controlFakePosFusion()
 void Ekf::resetFakePosFusion()
 {
 	ECL_INFO("reset fake position fusion");
-	_last_known_pos.xy() = _state.pos.xy();
+	_last_known_gpos.setLatLon(_gpos);
 
 	resetHorizontalPositionToLastKnown();
 	resetHorizontalVelocityToZero();

src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp

@@ -62,9 +62,9 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
 
 		const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
 		const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
-		const float gnss_alt = _gps_sample_delayed.alt + pos_offset_earth(2);
+		const float gnss_alt = gps_sample.alt + pos_offset_earth(2);
 
-		const float measurement = gnss_alt - getEkfGlobalOriginAltitude();
+		const float measurement = gnss_alt;
 		const float measurement_var = sq(noise);
 
 		const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var);
@@ -81,13 +81,13 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
 		if (measurement_valid) {
 			bias_est.setMaxStateNoise(sqrtf(measurement_var));
 			bias_est.setProcessNoiseSpectralDensity(_params.gps_hgt_bias_nsd);
-			bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
+			bias_est.fuseBias(measurement - _gpos.altitude(), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
 		}
 
 		// determine if we should use height aiding
 		const bool continuing_conditions_passing = (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::VPOS))
 				&& measurement_valid
-				&& _pos_ref.isInitialized()
+				&& _local_origin_lat_lon.isInitialized()
 				&& _gps_checks_passed;
 
 		const bool starting_conditions_passing = continuing_conditions_passing
@@ -105,8 +105,8 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
 					ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME);
 
 					_information_events.flags.reset_hgt_to_gps = true;
-					resetVerticalPositionTo(aid_src.observation, measurement_var);
-					bias_est.setBias(_state.pos(2) + measurement);
+					resetAltitudeTo(measurement, measurement_var);
+					bias_est.setBias(-_gpos.altitude() + measurement);
 
 					aid_src.time_last_fuse = _time_delayed_us;
 
@@ -128,13 +128,13 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
 					_height_sensor_ref = HeightSensor::GNSS;
 
 					_information_events.flags.reset_hgt_to_gps = true;
-					resetVerticalPositionTo(-measurement, measurement_var);
-					_gpos_origin_epv = 0.f; // The uncertainty of the global origin is now contained in the local position uncertainty
+
+					initialiseAltitudeTo(measurement, measurement_var);
 					bias_est.reset();
 
 				} else {
 					ECL_INFO("starting %s height fusion", HGT_SRC_NAME);
-					bias_est.setBias(_state.pos(2) + measurement);
+					bias_est.setBias(-_gpos.altitude() + measurement);
 				}
 
 				aid_src.time_last_fuse = _time_delayed_us;

src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp

@@ -55,32 +55,6 @@
 #define MASK_GPS_VSPD    (1<<8)
 #define MASK_GPS_SPOOFED (1<<9)
 
-void Ekf::collect_gps(const gnssSample &gps)
-{
-	if (_filter_initialised && !_pos_ref.isInitialized() && _gps_checks_passed) {
-		// If we have good GPS data set the origin's WGS-84 position to the last gps fix
-		setLatLonOriginFromCurrentPos(gps.lat, gps.lon, gps.hacc);
-
-		// Take the current GPS height and subtract the filter height above origin to estimate the GPS height of the origin
-		if (!PX4_ISFINITE(_gps_alt_ref)) {
-			setAltOriginFromCurrentPos(gps.alt, gps.vacc);
-		}
-
-		_information_events.flags.gps_checks_passed = true;
-
-		ECL_INFO("GPS origin set to lat=%.6f, lon=%.6f",
-			 _pos_ref.getProjectionReferenceLat(), _pos_ref.getProjectionReferenceLon());
-
-	} else {
-		// a rough 2D fix is sufficient to lookup earth spin rate
-		const bool gps_rough_2d_fix = (gps.fix_type >= 2) && (gps.hacc < 1000);
-
-		if (gps_rough_2d_fix && (_gps_checks_passed || !_pos_ref.isInitialized())) {
-			_earth_rate_NED = calcEarthRateNED((float)math::radians(gps.lat));
-		}
-	}
-}
-
 bool Ekf::runGnssChecks(const gnssSample &gps)
 {
 	_gps_check_fail_status.flags.spoofed = gps.spoofed;

src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp

@@ -67,11 +67,13 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 		    && isTimedOut(_last_gps_fail_us, max((uint64_t)1e6, (uint64_t)_min_gps_health_time_us / 10))) {
 			if (isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us)) {
 				// First time checks are passing, latching.
+				if (!_gps_checks_passed) {
+					_information_events.flags.gps_checks_passed = true;
+				}
+
 				_gps_checks_passed = true;
 			}
 
-			collect_gps(gnss_sample);
-
 		} else {
 			// Skip this sample
 			_gps_data_ready = false;
@@ -82,10 +84,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 			}
 		}
 
-		if (_pos_ref.isInitialized()) {
-			updateGnssPos(gnss_sample, _aid_src_gnss_pos);
-		}
-
+		updateGnssPos(gnss_sample, _aid_src_gnss_pos);
 		updateGnssVel(imu_delayed, gnss_sample, _aid_src_gnss_vel);
 
 	} else if (_control_status.flags.gps) {
@@ -108,9 +107,9 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 
 		const bool continuing_conditions_passing = (gnss_vel_enabled || gnss_pos_enabled)
 				&& _control_status.flags.tilt_align
-				&& _control_status.flags.yaw_align
-				&& _pos_ref.isInitialized();
+				&& _control_status.flags.yaw_align;
 		const bool starting_conditions_passing = continuing_conditions_passing && _gps_checks_passed;
+		const bool gpos_init_conditions_passing = gnss_pos_enabled && _gps_checks_passed;
 
 		if (_control_status.flags.gps) {
 			if (continuing_conditions_passing) {
@@ -174,6 +173,9 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 				}
 
 				_control_status.flags.gps = true;
+
+			} else if (gpos_init_conditions_passing && !_local_origin_lat_lon.isInitialized()) {
+				resetHorizontalPositionToGnss(_aid_src_gnss_pos);
 			}
 		}
 	}
@@ -221,8 +223,10 @@ void Ekf::updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s
 {
 	// correct position and height for offset relative to IMU
 	const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
-	const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
-	const Vector2f position = _pos_ref.project(gnss_sample.lat, gnss_sample.lon) - pos_offset_earth.xy();
+	const Vector3f pos_offset_earth = Vector3f(_R_to_earth * pos_offset_body);
+	const LatLonAlt measurement(gnss_sample.lat, gnss_sample.lon, gnss_sample.alt);
+	const LatLonAlt measurement_corrected = measurement + (-pos_offset_earth);
+	const Vector2f innovation = (_gpos - measurement_corrected).xy();
 
 	// relax the upper observation noise limit which prevents bad GPS perturbing the position estimate
 	float pos_noise = math::max(gnss_sample.hacc, _params.gps_pos_noise);
@@ -237,12 +241,13 @@ void Ekf::updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s
 
 	const float pos_var = math::max(sq(pos_noise), sq(0.01f));
 	const Vector2f pos_obs_var(pos_var, pos_var);
+	const matrix::Vector2d observation(measurement_corrected.latitude_deg(), measurement_corrected.longitude_deg());
 
 	updateAidSourceStatus(aid_src,
 			      gnss_sample.time_us,                                    // sample timestamp
-			      position,                                               // observation
+			      observation,                                            // observation
 			      pos_obs_var,                                            // observation variance
-			      Vector2f(_state.pos) - position,                        // innovation
+			      innovation,                                             // innovation
 			      Vector2f(getStateVariance<State::pos>()) + pos_obs_var, // innovation variance
 			      math::max(_params.gps_pos_innov_gate, 1.f));            // innovation gate
 }
@@ -322,8 +327,9 @@ void Ekf::resetVelocityToGnss(estimator_aid_source3d_s &aid_src)
 void Ekf::resetHorizontalPositionToGnss(estimator_aid_source2d_s &aid_src)
 {
 	_information_events.flags.reset_pos_to_gps = true;
-	resetHorizontalPositionTo(Vector2f(aid_src.observation), Vector2f(aid_src.observation_variance));
-	_gpos_origin_eph = 0.f; // The uncertainty of the global origin is now contained in the local position uncertainty
+	resetLatLonTo(aid_src.observation[0], aid_src.observation[1],
+		      aid_src.observation_variance[0] +
+		      aid_src.observation_variance[1]);
 
 	resetAidSourceStatusZeroInnovation(aid_src);
 }

src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp

@@ -90,12 +90,7 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 			if (global_origin_valid()
 			    && (origin_newer_than_last_mag || (isLocalHorizontalPositionValid() && isTimedOut(_wmm_mag_time_last_checked, 10e6)))
 			   ) {
-				// position of local NED origin in GPS / WGS84 frame
-				double latitude_deg;
-				double longitude_deg;
-				global_origin().reproject(_state.pos(0), _state.pos(1), latitude_deg, longitude_deg);
-
-				if (updateWorldMagneticModel(latitude_deg, longitude_deg)) {
+				if (updateWorldMagneticModel(_gpos.latitude_deg(), _gpos.longitude_deg())) {
 					wmm_updated = true;
 				}
 
@@ -368,7 +363,7 @@ bool Ekf::checkHaglYawResetReq() const
 		// Check if height has increased sufficiently to be away from ground magnetic anomalies
 		// and request a yaw reset if not already requested.
 		static constexpr float mag_anomalies_max_hagl = 1.5f;
-		const bool above_mag_anomalies = (getTerrainVPos() - _state.pos(2)) > mag_anomalies_max_hagl;
+		const bool above_mag_anomalies = (getTerrainVPos() + _gpos.altitude()) > mag_anomalies_max_hagl;
 		return above_mag_anomalies;
 	}
 

src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp

@@ -230,12 +230,6 @@ void Ekf::resetFlowFusion(const flowSample &flow_sample)
 	const float flow_vel_var = sq(predictFlowRange()) * calcOptFlowMeasVar(flow_sample);
 	resetHorizontalVelocityTo(getFilteredFlowVelNE(), flow_vel_var);
 
-	// reset position, estimate is relative to initial position in this mode, so we start with zero error
-	if (!_control_status.flags.in_air) {
-		ECL_INFO("reset position to zero");
-		resetHorizontalPositionTo(Vector2f(0.f, 0.f), 0.f);
-	}
-
 	resetAidSourceStatusZeroInnovation(_aid_src_optical_flow);
 
 	_innov_check_fail_status.flags.reject_optflow_X = false;
@@ -247,7 +241,7 @@ void Ekf::resetTerrainToFlow()
 	ECL_INFO("reset hagl to flow");
 
 	// TODO: use the flow data
-	const float new_terrain = fmaxf(0.0f, _state.pos(2));
+	const float new_terrain = -_gpos.altitude() + _params.rng_gnd_clearance;
 	const float delta_terrain = new_terrain - _state.terrain;
 	_state.terrain = new_terrain;
 	P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, 100.f);